专利名称：基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法
技术领域：
本发明公开了一种基于量子行为粒子群算法的轮式移动机器人轨迹跟踪 方法。
背景技术：
轮式移动机器人(Wheeled Mobile Robots System,简称WMRS)不仅在
工业和服务行业得到了广泛关注，而且因其在控制理论方而遇到的挑战而引 起了大批研究者的注意。轮式移动机器人系统由于受到滚动约束的限制(无纵 向或侧向的滑动)，属于一类典型的非完整约束系机械统。利用非完整约束条 件和非完整运动规划的原理，研究开发新型的具有良好的目标轨迹跟踪和控 制性能轮式移动机器人系统，在机器人领域中成为一个新的研究热点。移动 机器人的轨迹跟踪是个非线性问题，而且由于机器人所跟踪的参考轨迹与时 间因素直接紧密相关，致使对这问题的研究非常困难，但也正是这些因素吸 引了许多专家学者投身其中。对于轨迹跟踪而言，平滑逼近及收敛速度是两 个重要的因素。现在的大多数基于运动学模刑的控制律是建立在一定研究基 础上的，但现有的控制律在平滑性和收敛速度上还存在着些不足，使得设计 的控制律在应用到机器人的实际导航环境中存在着定的难度。
对于非完整约束移动机器人的轨迹跟踪控制问题的研究存在着一些方 法，如利用误差模型的小扰动线性化思想来进行反馈镇定控制器的设计，这 种方法只能得到局部稳定性；利用动态反馈线性化的思想，并将其应用于移 动机器人的轨迹跟踪控制，也得到了局部的指数跟踪收敛；利用Lyapunov 直接法和积分反演技术对移动机器人的轨迹跟踪问题进行了研究，对满足给 定条件的参考模型实现了全局指数跟踪。上述方法主要基于运动学模型对非 完整移动机器人的轨迹跟踪控制进行研究，而动力学模型是系统最本质的模 型，也更复杂，存在诸如摩擦力、质量和转动惯量等不确定项，为系统综合 带来了很大困难。
发明内容
本发明的目的是克服现有技术中存在的不足，提供一种可以解决非线性 与非完整性给轨迹跟踪过程带来的困难、大大地提高了跟踪精度和速度的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法。
按照本发明提供的技术方案，所述基于量子行为粒子群算法的轮式移动 机器人轨迹跟踪方法包含如下步骤
步骤一首先在机器人运动的二维平面上，建立两个独立的平面坐标系， 从而来共同标识出轮式移动机器人某个时刻的空间位姿与朝向；
步骤二在步骤一建立的坐标系基础上，使用Euler-Lagrange方程建立 轮式移动机器人的运动学模型；
步骤三将步骤二得到的运动学方程利用反演设计方法分解成不超过系 统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制 量，直到完成整个控制律的设计；
步骤四根据步骤三得到的子系统建立参考位姿与实际位姿的误差函数；
步骤五利用量子行为粒子群算法求解步骤四得到的误差函数，直到符 合跟踪控制的要求。
上述步骤一即建立坐标系由于机器人运动本身具有方向性，所以在机 器人运动的二维平面上，需要用两个独立的平面坐标系来共同标识出机器人 某时刻的空间位置与朝向，在机器人学中统称为位姿，也就是把一个在空间 中占有一定体积的非完整移动机器人系统抽象为一个参考点，用参考点的坐 标来代表非完整移动机器人平台；
上述步骤二即建立运动学模型设[;C为轮式移动机器人(WMR)质心 的笛卡儿坐标，0为其前进方向与x轴夹角，v、 w分别为WMR的平移速度 和旋转速度。由于从动轮在运动中仅仅起支撑作用，在运动学模型中的影响 可忽略不计。不考虑外界干扰及建模误差带来的影响，具有非完整约束的 WMR模型可以用经典Euler-Lagrange方程表示出；
上述步骤三即简化运动学模型利用反演设计方法降低系统的阶数，主 要方法是将复杂的非线性系统分解成不超过系统阶数的子系统，然后为每个 子系统分别设计Lyapimov函数和中间虚拟控制量， 一直"后退"到整个系统， 直到完成整个控制律的设计，使整个闭环系统满足期望的动静态性能指标；
上述步骤四即确定目标函数轨迹跟踪问题实际上就是根据移动机器人 位姿误差，通过移动机器人的速度跟踪控制器调节自身的跟踪位姿，减少位
姿误差，达到跟踪理想轨迹的目的。设wMR实际位姿为i^^k,凡,《;r，实
际速度为h，H^r，参考位姿i^h,^,^J，参考速度h,^F， 4和A之间的 位姿误差矢量为尸￡ =k,h,《F。WMR的轨迹跟踪问题就是对任意初始位姿和速度误差，寻求有界输入"=[^,^]、使得具有实际位姿的WMR能够跟踪
由参考位姿^和速度控制量K-h,^r所描述的参考模型，并使得
=0;
上述步骤五即求解参数利用量子行为粒子群算法求解步骤四得到的误
差函数，直到符合跟踪控制的要求，即在解空间初始化一组粒子，计算粒子
的目标函数值，然后粒子通过量子行为粒子群算法的搜索策略，寻找个体最 优位姿与全局最优位姿经过若干次的迭代以后完成寻优过程，最后输出全局
最优解，即为最优的控制参数。
本发明与已有技术相比，设计过程简单，系统灵活，鲁棒性强，跟踪速 度快。因此，本发明解决了非线性与非完整性给轨迹跟踪过程带来的困难， 大大地提高了跟踪精度和速度。
图1是本发明的一个非完整的移动机器人示意图
图2是本发明的移动机器人位姿误差示意图。 图3是本发明的非完整移动机器人的控制原理图。 图4是用经典粒子群算法(PSO)得到的机器人轨迹跟踪。 图5是用经典粒子群算法(PS0)得到的X方向、Y方向和角度误差。 图6是用经典粒子群算法(PSO)得到的控制输入。 图7是是用量子行为粒子群算法(QPSO)得到的机器人轨迹跟踪。 图8是用量子行为粒子群算法(QPSO)得到的X方向、Y方向和角度误差。 图9是用量子行为粒子群算法(QPSO)得到的控制输入。 图10是用量子行为粒子群算法(QPSO)进行机器人轨迹跟踪的流程图。
具体实施例方式
为了更好的理解本发明的技术方案，以下对本发明的实施方式做进一步 的介绍。
1.轮式移动机器人运动坐标系的建立
非完整移动机器人的机械结构由车体、两个驱动轮和一个随动轮组成， 随动轮仅在运动过程中起支撑作用，如图1所示。
由于机器人运动本身具有方向性，所以在机器人运动的二维平面上，需 要用两个独立的平面坐标系来共同标识出机器人某时刻的空间位姿与朝向， 在机器人学中统称为位姿，用向量g来表示，这也就是说把一个在空间中占有一定体积的非完整移动机器人系统抽象为一个参考点，用参考点的坐标来 代表非完整移动机器人平台。X-y是以二维平面中某一点为原点的全局坐标 系，A-^是固定于非完整移动机器人本身的横轴正方向始终与机器人朝向 相一致的局部坐标系。为了简化机器人的数学模型，通常把机器人的质心平 移到驱动轴中心，局部坐标系的原点就是驱动轴中心，(x，力为移动机器人质
心的笛卡儿坐标，即机器人的位姿。e为其前进方向与x轴夹角，即机器人 的朝向，也称为方向角，v和^分别为移动机器人的平移速度和旋转速度。々 和^分别是移动机器人左轮和右轮的线速度。i 为非完整机器人的驱动轮半 径，Z为非完整移动机器人两个驱动轮轮心之间的距离。这样，非完整移动
机器人的位姿《就可以表示为[x,少":r。
假设WMR的运动是一种无侧滑、纯滚动运动，则在WMR运动过程中， ^点的速度"力与位姿角e之间应满足约束关系
<formula>formula see original document page 7</formula> (1)
式(1 )表示WMR的瞬时运动约束在2自由度方向，其物理意义是WMR 在两驱动轮轴线上的速度始终为0， WMR瞬时速度的方向始终与WMR的朝 向保持一致。显然此约束条件是不可积的，因此式(O就是WMR的非完整 约束，非完整约束条件表明机器人只能在垂直于轮轴的方向上运动。非完整 约束要求WMR的运动除了满足起始点、终点和障碍物等基本要求外，还必 须满足方向或者轨迹形状的特殊要求，即有纯几何方法计算得到的适用于 完整系统的轨迹，并不一定适用与非完整系统。
2.轮式移动机器人运动学模型的建立
图1所示的移动机器人为轮式移动机器人，[x，>f为WMR质心的笛卡儿 坐标，e为其前进方向与x轴夹角，v、 《分别为WMR的平移速度和旋转速 度，从动轮在运动中仅仅起支撑作用，在运动学模型中的影响可忽略不计。
不考虑外界干扰及建模误差带来的影响，具有非完整约束的WMR模型用 经典Euler-Lagrange方程表示为
<formula>formula see original document page 7</formula> (2)
<formula>formula see original document page 7</formula> (3)
式(2)、 (3)中，^/T为"维广义坐标，M(^"""为系统对称正定惯性 矩阵，r(《,々W为向心力与哥氏力项，G(《)为重力项，对于平面上的WMR， 其G(g)为0， r为r维广义力(控制输入矩阵)，B(W为输入变换矩阵，义为 Lagrange乘子(约束力矢量)，是与约束力相关的矩阵。对于式(3)中的^《)，如果令S(《)为J(《)零空间矩阵的一组基，即J(g).S(g) = 0，则必然 存在控制输入向量"，使得式(2)化为^S(《)."。
令m为WMR的整体质量，/为车轮的转动惯量，、为驱动力矩，^为转 动力矩，则式(1)可以转化为
<formula>formula see original document page 8</formula>(5)
3.利用反演设计方法分解运动学模型
反演设计方法的基本思想是将复杂的非线性系统分解成不超过系统阶数 的子系统，然后为每个子系统分别设计Lyapimov函数和中间虚拟控制量，一 直"后退"到整个系统，直到完成整个控制律的设计，使整个闭环系统满足 期望的动静态性能指标。
假设被控对象为<formula>formula see original document page 8</formula>
其中，600 0。基本反演控制方法设计步骤如下: 步骤1: 定义位姿误差
Zi — X, — Z 一
其中&为指令信号。则
(7)
(8)
8定义虚拟控制量
<formula>formula see original document page 9</formula>
其中c、 >0。
定义 <formula>formula see original document page 9</formula>
定义Lyapunov函数
<formula>formula see original document page 9</formula>
则<formula>formula see original document page 9</formula>
将式(ll)代入式(12)得
<formula>formula see original document page 9</formula>
如果&=0，则^S0。为此，需要进行下一步设计。 步骤2:
定义Lyapunov函数
<formula>formula see original document page 9</formula>
由<formula>formula see original document page 9</formula>
则<formula>formula see original document page 9</formula>
为使&^0，设计控制器为
<formula>formula see original document page 9</formula>
其中。>0。则
<formula>formula see original document page 9</formula>
即当A—o和^—o时，有^> — 0。通过控制律的设计，使得系统满足了 Lyapunov稳定性理论条件，从而保证系统具有全局意义下的渐进稳定性。 根据反演设计方法，轨迹跟踪控制器的控制律设计为
<formula>formula see original document page 9</formula> (19)
其中，&， *2， ^为正常数。
利用Lyapunov函数设计稳定控制律，构造Lyapunov函数为
<formula>formula see original document page 9</formula> (20)对式(20)求导后将轨迹跟踪误差的微分方程代入:
f = A:xA + 、;Ve久+ sin《'么
=允i乂e cos Pe — v ++ ^;^(V/; sin ^e — + sin ^e — 0) =走i乂e1^ cos Pe — A^x^v + A^^eVw sin ^e + sin— sin :^XeV^ cos ^e — A:!XeVw cos ^《一^xeA:3xe + ^；^v^ sin ^e + sin
一sin&^ — sin —sin^A^sin^ =一 A:,x。A^x。 一 sin S。A:， sin ^。
(21)
= -仏;<:，-)t2 sin《2 SO
显然r是一个弱Lyapunov函数(P不严格小于零)， 一个弱Lyapunov函 数虽然不能像一个严格Lyapunov函数一样可以保证系统的全局稳定性，但可 以用来设计一个全局稳定的控制律。因此，采用式(21)表示的控制律能够 使WMR系统渐进收敛于稳定平衡点尸￡ = 0 。
4.由分解模型建立求解目标
设WMR实际位姿为尸c^^凡Af ，实际速度为h,^r，参考位姿 尸《=[^,厶，^『，参考速度[^,^r，尸c和a之间的位姿误差矢量为
& =k,^A『。如图2所示，WMR的轨迹跟踪问题就是对任意初始位姿和速 度误差，寻求有界输入t/^Vc,^r，使得具有实际位姿的WMR能够跟踪由&
和速度控制量"《=[ ，>^所描述的参考模型，并使得i—h,y^r
由图2所示的几何关系，^可描述为<formula>formula see original document page 10</formula>
(22)
对式(22)求导可得到轨迹跟踪误差的微分方程<formula>formula see original document page 10</formula>(23)
轨迹跟踪问题实际上就是根据移动机器人位姿误差，通过移动机器人的 速度跟踪控制器调节自身的跟踪位姿，减少位姿误差，达到跟踪理想轨迹的 目的。图3为控制系统结构示意图，根据WMR的位姿误差及其参考输入产 生控制输入c/，然后通过WMR自身的速度跟踪控制器生成实际的^最终控 制WMR的位姿。
5.利用QPSO算法对求解目标进行优化QPSO算法与其他进化类算法相类似，具有进化和群体智能的特点。在 QPSO算法中，每个候选解称为'粒子'，若干个候选解就构成了群体。每个粒 子没有重量和体积，通过目标函数确定它的适应值。每个粒子在解空间中运 动，粒子通过追随自身的个体极值与群体的极值来动态的调整自己的位姿信 息。
QPSO算法的描述如下
假设算法的搜索空间为D维，粒子群的规模为^，每个粒子包含下列信
息
uA，^…;c,。)粒子的当前位姿；
C 粒子/的当前最优位姿，也可记为; &W;
K^，^,…^):粒子群的全局最优位姿，也可记为^W。 每个粒子都按照以下的进化公式来更新自己的位姿信息
"^加O^l^W:(^l:^(0…,41:4(0) (24)
yv ,=1w yv ,.=1
&(0 二 一 & (0 + (H) 尸^(0 ，^ =謂" (25)
x,rf (" 1) = & (0 ± " (0 - & (0|. ln(l / w) ， W = ra"c/ (26)
其中J是当前的迭代次数，w&W称为平均最优位姿，它是所有粒子自 身最优位姿的中心点；&为^与/^构成的超矩形中的一个随机点；参数"称 为压縮-扩张因子，可以用来控制粒子的收敛速度，采用如下的取值方式，
a 二 (1.0 — 0.5) x 0/ 0.5 (27)
其中，f是当前迭代次数，M4^77Ei 算法的最大迭代次数。
在式(19)控制律方程中，有三个正常数A、 &、 *3没有确定的数值，
由前述分析可知控制律中参数设置不同在很大程度上影响着移动机器人跟 踪控制的效果。因此，需要根据跟踪控制的目标设置合适的&、 it2、、数值，
使得移动机器人跟踪达到较好的品质。而传统的控制器参数设置方法一般为 试凑法，根据经验通过不断的带入不同的参数值来观察移动机器人跟踪的效 果，直到符合跟踪控制的要求。由前面章节可知，群体智能算法在目标问题 不提供全局模型的前提下，能够快速的搜寻到适合问题的解。因此，这里使用群体智能算法搜索合适的跟踪控制器的参数^、 ^、"的值。
移动机器人轨迹跟踪的目标是使得机器人能够从实际的初始位姿快速 的和跟踪轨迹保持一致，即尽可能的在条件允许的情况下使得^， h小，并
且由于实际应用中，移动机器人的线速度v不可能有非常大的值，因此在仿 真中我们设置群体智能算法适应度函数为
<formula>formula see original document page 12</formula> (28)
其中，、是理想速度和实际速度的差值，此问题即为一个最小化优化问题。
QPSO算法完成轮式移动机器人轨迹跟踪参数优化的步骤 步骤l:初始化算法参数，包括粒子数，问题维数，初始化空间及搜索空 间，粒子的初始位置，初始最优值等；
步骤2:由式(24)计算群体的平均最优位置m&W(0;
步骤3:由式(25)计算随机位置/a汁l);
步骤4:由式(26)计算粒子的新位置;c^+l);
步骤5:由式(7)计算粒子新位置的适应度y^"^(x,(汁l));
步骤6:更新粒子的当前最优位置，即如果
y^"e^o,(什i ))〈y^"e^(/^ew,0))，则1 1)，否贝U ，
步骤7:更新群体的最优位置，艮P:如果/ /"e^(p/)e"0+l))〈/ meM(gZ^ (X))， 贝lJgZ eWO十1 )=/^^y ,0+1);
步骤8:循环步骤2 7，直至满足一定的结束条件，然后输出群体的全局 最优位置g&W，即为控制器的最优参数。
在本发明的实例验证中，采用的模型如下
跟踪线速度和角速度均为匀速运动的圆轨迹，圆的轨迹方程为
<formula>formula see original document page 12</formula>
位姿误差初始值为[3.2 -l 0]、 实例条件如下分别采用经典粒子群算法(PSO)和量子行为粒子群算法(QPSO)对实例进 行了仿真。对于PSO算法，惯性系数w采用线性递减的方式取值，起始值为 0.9，终止值为0.4;对于QPSO算法，控制参数"采用线性递减方式取值， 起始值为1.0，终止值为0.5。每个算法的进化次数为50次，分别独立运行 30轮。表1中给出了两种算法仿真的目标函数值，及其对应的控制参数&、 、、^的值。图4、 5、 6是PSO算法优化后的控制器跟踪圆的性能;图7、 8、 9是QPSO算法优化后的控制器跟踪圆的性能。从表1中可以看出，QPSO 算法优化得到的目标函数值更优；并且从图4_9的性能图可以看到，用QPSO 算法优化的控制器跟踪圆的性能比PSO算法的更好。
表l使用两种算法求解实例l的函数值及系数
算法、目标函数值
PSO3.18353.05252細91415.6
QPSO5.23392,89036,1656901.5权利要求
1、一种基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征在于该方法包含如下步骤步骤一首先在机器人运动的二维平面上，建立两个独立的平面坐标系，从而来共同标识出轮式移动机器人某个时刻的空间位姿与朝向；步骤二在步骤一建立的坐标系基础上，使用Euler-Lagrange方程建立轮式移动机器人的运动学模型；步骤三将步骤二得到的运动学方程利用反演设计方法分解成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量，直到完成整个控制律的设计；步骤四根据步骤三得到的子系统建立参考位姿与实际位姿的误差函数；步骤五利用量子行为粒子群算法求解步骤四得到的误差函数，直到符合跟踪控制的要求。
2、 如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是步骤一中需要用两个独立的平面坐标系来共同标识出机器人某时刻的空间位置与朝向，在机器人学中统称为位姿，也就是把一个 在空间中占有一定体积的非完整移动机器人系统抽象为一个参考点，用参考 点的坐标来代表非完整移动机器人平台。
3、 如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹 跟踪方法，其特征是步骤二中设k;f为轮式移动机器人质心的笛卡儿坐标， e为其前进方向与义轴夹角，v、 《分别为WMR的平移速度和旋转速度，由 于从动轮在运动中仅仅起支撑作用，在运动学模型中的影响可忽略不计，不 考虑外界干扰及建模误差带来的影响，将具有非完整约束的WMR模型用经 典Euler-Lagrange方程表示出。
4、 如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹 跟踪方法，其特征是步骤三中应用反演设计方法将复杂的非线性系统分解 成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapimov函数和中 间虚拟控制量， 一直后退到整个系统，直到完成整个控制律的设计，使整个 闭环系统满足期望的动静态性能指标。
5、 如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是步骤四中对任意初始位姿和速度误差，寻求有界输入 t/ = k，wJ，使得具有实际位姿的WMR能够跟踪由参考位姿A和速度控制量^ ^^,^r所描述的参考模型，并使得yik心Ai
6、如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹 跟踪方法，其特征是步骤五以步骤四中得到的误差函数作为求解的目标， 应用量子行为粒子群算法求解得一组控制参数，具体为在解空间初始化一 组粒子，计算粒子的目标函数值，然后粒子通过量子行为粒子群算法的搜索 策略，寻找个体最优位姿与全局最优位姿经过若干次的迭代以后完成寻优过 程，最后输出全局最优解，即为最优的控制参数。
全文摘要
本发明公开了一种基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法。该方法首先在运动范围中设定坐标系，建立轮式机器人的运动学模型，利用反演设计方法分解运动学模型，然后通过路径规划生成的轨迹得到参考位姿与实际位姿的误差传递方程，以该方程作为目标函数，利用量子行为粒子群优化算法以较快的速度和较强的全局求解能力求得控制器的参数，而后通过该组参数设计控制器从而保证机器人可以稳定、快速的收敛于目标轨迹，即在有限的时间内使得目标函数值靠近或者小于某个允许的值。本发明解决了轮式移动机器人轨迹跟踪存在的高度非线性和非完整性问题，可以快速的实现轮式移动机器人轨迹跟踪，提高了移动机器人的跟踪控制效果。
文档编号G05D1/02GK101436073SQ20081024445
公开日2009年5月20日 申请日期2008年12月3日 优先权日2008年12月3日
发明者丁彦蕊, 吴小俊, 奚茂龙, 俊 孙, 伟 方, 柴志雷, 磊 陈, 陈秀宏, 须文波 申请人:江南大学