CN101436073A - 基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法 - Google Patents

Abstract

本发明公开了一种基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法。该方法首先在运动范围中设定坐标系，建立轮式机器人的运动学模型，利用反演设计方法分解运动学模型，然后通过路径规划生成的轨迹得到参考位姿与实际位姿的误差传递方程，以该方程作为目标函数，利用量子行为粒子群优化算法以较快的速度和较强的全局求解能力求得控制器的参数，而后通过该组参数设计控制器从而保证机器人可以稳定、快速的收敛于目标轨迹，即在有限的时间内使得目标函数值靠近或者小于某个允许的值。本发明解决了轮式移动机器人轨迹跟踪存在的高度非线性和非完整性问题，可以快速的实现轮式移动机器人轨迹跟踪，提高了移动机器人的跟踪控制效果。

Description

基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法

技术领域

本发明公开了一种基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法。

背景技术

轮式移动机器人(Wheeled Mobile Robots System，简称WMRS)不仅在工业和服务行业得到了广泛关注，而且因其在控制理论方而遇到的挑战而引起了大批研究者的注意。轮式移动机器人系统由于受到滚动约束的限制(无纵向或侧向的滑动)，属于一类典型的非完整约束系机械统。利用非完整约束条件和非完整运动规划的原理，研究开发新型的具有良好的目标轨迹跟踪和控制性能轮式移动机器人系统，在机器人领域中成为一个新的研究热点。移动机器人的轨迹跟踪是个非线性问题，而且由于机器人所跟踪的参考轨迹与时间因素直接紧密相关，致使对这问题的研究非常困难，但也正是这些因素吸引了许多专家学者投身其中。对于轨迹跟踪而言，平滑逼近及收敛速度是两个重要的因素。现在的大多数基于运动学模刑的控制律是建立在一定研究基础上的，但现有的控制律在平滑性和收敛速度上还存在着些不足，使得设计的控制律在应用到机器人的实际导航环境中存在着定的难度。

对于非完整约束移动机器人的轨迹跟踪控制问题的研究存在着一些方法，如利用误差模型的小扰动线性化思想来进行反馈镇定控制器的设计，这种方法只能得到局部稳定性；利用动态反馈线性化的思想，并将其应用于移动机器人的轨迹跟踪控制，也得到了局部的指数跟踪收敛；利用Lyapunov直接法和积分反演技术对移动机器人的轨迹跟踪问题进行了研究，对满足给定条件的参考模型实现了全局指数跟踪。上述方法主要基于运动学模型对非完整移动机器人的轨迹跟踪控制进行研究，而动力学模型是系统最本质的模型，也更复杂，存在诸如摩擦力、质量和转动惯量等不确定项，为系统综合带来了很大困难。

发明内容

本发明的目的是克服现有技术中存在的不足，提供一种可以解决非线性与非完整性给轨迹跟踪过程带来的困难、大大地提高了跟踪精度和速度的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法。

按照本发明提供的技术方案，所述基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法包含如下步骤：

步骤一：首先在机器人运动的二维平面上，建立两个独立的平面坐标系，从而来共同标识出轮式移动机器人某个时刻的空间位姿与朝向；

步骤二：在步骤一建立的坐标系基础上，使用Euler-Lagrange方程建立轮式移动机器人的运动学模型；

步骤三：将步骤二得到的运动学方程利用反演设计方法分解成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量，直到完成整个控制律的设计；

步骤四：根据步骤三得到的子系统建立参考位姿与实际位姿的误差函数；

步骤五：利用量子行为粒子群算法求解步骤四得到的误差函数，直到符合跟踪控制的要求。

上述步骤一即建立坐标系：由于机器人运动本身具有方向性，所以在机器人运动的二维平面上，需要用两个独立的平面坐标系来共同标识出机器人某时刻的空间位置与朝向，在机器人学中统称为位姿，也就是把一个在空间中占有一定体积的非完整移动机器人系统抽象为一个参考点，用参考点的坐标来代表非完整移动机器人平台；

上述步骤二即建立运动学模型：设[x，y]^T为轮式移动机器人(WMR)质心的笛卡儿坐标，θ为其前进方向与X轴夹角，v、ω分别为WMR的平移速度和旋转速度。由于从动轮在运动中仅仅起支撑作用，在运动学模型中的影响可忽略不计。不考虑外界干扰及建模误差带来的影响，具有非完整约束的WMR模型可以用经典Euler-Lagrange方程表示出；

上述步骤三即简化运动学模型：利用反演设计方法降低系统的阶数，主要方法是将复杂的非线性系统分解成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量，一直“后退”到整个系统，直到完成整个控制律的设计，使整个闭环系统满足期望的动静态性能指标；

上述步骤四即确定目标函数：轨迹跟踪问题实际上就是根据移动机器人位姿误差，通过移动机器人的速度跟踪控制器调节自身的跟踪位姿，减少位姿误差，达到跟踪理想轨迹的目的。设WMR实际位姿为P_C＝[x_c，y_c，θ_c]^T，实际速度为[v_c，w_c]^T，参考位姿P_R＝[x_R，y_R，θ_R]^T，参考速度[v_R，w_R]^T，P_C和P_R之间的位姿误差矢量为P_E＝[x_e，y_e，θ_e]^T。WMR的轨迹跟踪问题就是对任意初始位姿和速度误差，寻求有界输入U＝[v_c，w_c]^T，使得具有实际位姿的WMR能够跟踪由参考位姿P_R和速度控制量U_R＝[v_R，w_R]^T所描述的参考模型，并使得 $\underset{t\→\∞}{\lim }\left|\right|{[x_e,y_e,{\mathrm{\θ}}_e]}^T\left|\right|=0;$

上述步骤五即求解参数：利用量子行为粒子群算法求解步骤四得到的误差函数，直到符合跟踪控制的要求，即在解空间初始化一组粒子，计算粒子的目标函数值，然后粒子通过量子行为粒子群算法的搜索策略，寻找个体最优位姿与全局最优位姿经过若干次的迭代以后完成寻优过程，最后输出全局最优解，即为最优的控制参数。

本发明与已有技术相比，设计过程简单，系统灵活，鲁棒性强，跟踪速度快。因此，本发明解决了非线性与非完整性给轨迹跟踪过程带来的困难，大大地提高了跟踪精度和速度。

附图说明

图1是本发明的一个非完整的移动机器人示意图

图2是本发明的移动机器人位姿误差示意图。

图3是本发明的非完整移动机器人的控制原理图。

图4是用经典粒子群算法(PSO)得到的机器人轨迹跟踪。

图5是用经典粒子群算法(PSO)得到的X方向、Y方向和角度误差。

图6是用经典粒子群算法(PSO)得到的控制输入。

图7是是用量子行为粒子群算法(QPSO)得到的机器人轨迹跟踪。

图8是用量子行为粒子群算法(QPSO)得到的X方向、Y方向和角度误差。

图9是用量子行为粒子群算法(QPSO)得到的控制输入。

图10是用量子行为粒子群算法(QPSO)进行机器人轨迹跟踪的流程图。

具体实施方式

为了更好的理解本发明的技术方案，以下对本发明的实施方式做进一步的介绍。

1.轮式移动机器人运动坐标系的建立

非完整移动机器人的机械结构由车体、两个驱动轮和一个随动轮组成，随动轮仅在运动过程中起支撑作用，如图1所示。

由于机器人运动本身具有方向性，所以在机器人运动的二维平面上，需要用两个独立的平面坐标系来共同标识出机器人某时刻的空间位姿与朝向，在机器人学中统称为位姿，用向量q来表示，这也就是说把一个在空间中占有一定体积的非完整移动机器人系统抽象为一个参考点，用参考点的坐标来代表非完整移动机器人平台。X-Y是以二维平面中某一点为原点的全局坐标系，X₁-Y₁是固定于非完整移动机器人本身的横轴正方向始终与机器人朝向相一致的局部坐标系。为了简化机器人的数学模型，通常把机器人的质心平移到驱动轴中心，局部坐标系的原点就是驱动轴中心，(x，y)为移动机器人质心的笛卡儿坐标，即机器人的位姿。θ为其前进方向与X轴夹角，即机器人的朝向，也称为方向角，v和ω分别为移动机器人的平移速度和旋转速度。v_L和v_R分别是移动机器人左轮和右轮的线速度。R为非完整机器人的驱动轮半径，L为非完整移动机器人两个驱动轮轮心之间的距离。这样，非完整移动机器人的位姿q就可以表示为[x，y，θ]^T。

假设WMR的运动是一种无侧滑、纯滚动运动，则在WMR运动过程中，O₁点的速度

与位姿角θ之间应满足约束关系

$\stackrel{\·}{x}\·\sin \mathrm{\θ}-\stackrel{\·}{y}\·\cos \mathrm{\θ}=0---\left(1\right)$

式(1)表示WMR的瞬时运动约束在2自由度方向，其物理意义是WMR在两驱动轮轴线上的速度始终为0，WMR瞬时速度的方向始终与WMR的朝向保持一致。显然此约束条件是不可积的，因此式(1)就是WMR的非完整约束，非完整约束条件表明机器人只能在垂直于轮轴的方向上运动。非完整约束要求WMR的运动除了满足起始点、终点和障碍物等基本要求外，还必须满足方向或者轨迹形状的特殊要求，即：有纯几何方法计算得到的适用于完整系统的轨迹，并不一定适用与非完整系统。

2.轮式移动机器人运动学模型的建立

图1所示的移动机器人为轮式移动机器人，[x，y]^T为WMR质心的笛卡儿坐标，θ为其前进方向与X轴夹角，v、ω分别为WMR的平移速度和旋转速度，从动轮在运动中仅仅起支撑作用，在运动学模型中的影响可忽略不计。

不考虑外界干扰及建模误差带来的影响，具有非完整约束的WMR模型用经典Euler-Lagrange方程表示为：

$M\left(q\right)\stackrel{\·\·}{q}+V(q,\stackrel{\·}{q})\stackrel{\·}{q}+G\left(q\right)=B\left(q\right)\mathrm{\τ}+A^T\left(q\right)\mathrm{\λ}---\left(2\right)$

$A\left(q\right)\stackrel{\·}{q}=0---\left(3\right)$

式(2)、(3)中，q∈Rⁿ为n维广义坐标，M(q)∈R^n×n为系统对称正定惯性矩阵，

为向心力与哥氏力项，G(q)为重力项，对于平面上的WMR，其G(q)为0，τ为r维广义力(控制输入矩阵)，B(q)为输入变换矩阵，λ为Lagrange乘子(约束力矢量)，A(q)是与约束力相关的矩阵。对于式(3)中的A(q)，如果令S(q)为A(q)零空间矩阵的一组基，即：A(q)·S(q)＝0，则必然存在控制输入向量U，使得式(2)化为 $\stackrel{\·}{q}=S\left(q\right)\·U$ 。

令m为WMR的整体质量，I为车轮的转动惯量，τ₁为驱动力矩，τ₂为转动力矩，则式(1)可以转化为：

$\left[\begin{array}{ccc}m& 0& 0\\ 0& m& 0\\ 0& 0& m\end{array}\right]\·\left[\begin{array}{c}\stackrel{\·\·}{x}\\ \stackrel{\·\·}{y}\\ \stackrel{\·\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{c}\sin \mathrm{\θ}\\ -\cos \mathrm{\θ}\\ 0\end{array}\right]\·\mathrm{\λ}+\left[\begin{array}{cc}\cos \mathrm{\θ}& 0\\ \sin \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\·\left[\begin{array}{c}{\mathrm{\τ}}_1\\ {\mathrm{\τ}}_2\end{array}\right]---\left(4\right)$

其中， $\left\{\begin{array}{c}\stackrel{\·}{x}=v\cos \mathrm{\θ}\\ \stackrel{\·}{y}=v\sin \mathrm{\θ}\\ \stackrel{\·}{\mathrm{\θ}}=\mathrm{\ω}\\ m\stackrel{\·}{v}={\mathrm{\τ}}_1\\ I\stackrel{\·}{\mathrm{\ω}}={\mathrm{\τ}}_2\end{array}\right.$

图1所示的WMR受到式(4)的非完整约束，可求得A(q)＝[sin θ，-cos θ，0]^T，那么可求得S(q)＝[cos θ，sin θ，0；0，0，1]^T。如果选择输入向量U＝[v，ω]^T，则WMR的运动学模型为

$\left[\begin{array}{c}\stackrel{\·}{x}\\ \stackrel{\·}{y}\\ \stackrel{\·}{\mathrm{\θ}}\end{array}\right]=\left[\begin{array}{cc}\sin \mathrm{\θ}& 0\\ \cos \mathrm{\θ}& 0\\ 0& 1\end{array}\right]\left[\begin{array}{c}v\\ w\end{array}\right]---\left(5\right)$

3.利用反演设计方法分解运动学模型

反演设计方法的基本思想是将复杂的非线性系统分解成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量，一直“后退”到整个系统，直到完成整个控制律的设计，使整个闭环系统满足期望的动静态性能指标。

假设被控对象为

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_1=x_2\\ {\stackrel{\·}{x}}_2=f(x,t)+b(x,t)u\end{array}\right.---\left(6\right)$

其中，b(x，t)≠0。基本反演控制方法设计步骤如下：

步骤1：

定义位姿误差

z₁＝x₁-z_d                                    (7)

其中z_d为指令信号。则

${\stackrel{\·}{z}}_1={\stackrel{\·}{x}}_1-{\stackrel{\·}{z}}_d=x_2-{\stackrel{\·}{z}}_d---\left(8\right)$

定义虚拟控制量

${\mathrm{\α}}_1={-c}_1{z}_1+{\stackrel{\·}{z}}_d---\left(9\right)$

其中c₁>0。

定义z₂＝x₂-α₁               (10)

定义Lyapunov函数

$V_1=\frac{1}{2}{z}_1^2---\left(11\right)$

则 $\stackrel{\·}{V}=z_1{\stackrel{\·}{z}}_1=z_1(x_2-{\stackrel{\·}{z}}_d)=z_1(z_2+{\mathrm{\α}}_1-{\stackrel{\·}{z}}_d)---\left(12\right)$

将式(11)代入式(12)得

${\stackrel{\·}{V}}_1={-c}_1{z}_1^2+z_1{z}_2---\left(13\right)$

如果z₂＝0，则 ${\stackrel{\·}{V}}_1\≤0$ 。为此，需要进行下一步设计。

步骤2：

定义Lyapunov函数

$V_2=V_1+\frac{1}{2}{z}_2^2---\left(14\right)$

由 ${\stackrel{\·}{z}}_2={\stackrel{\·}{x}}_2-{\stackrel{\·}{\mathrm{\α}}}_1=f(x,t)+b(x,t)u+c_1{\stackrel{\·}{z}}_1-{\stackrel{\·\·}{z}}_d---\left(15\right)$

则 ${\stackrel{\·}{V}}_2={\stackrel{\·}{V}}_1+z_2{\stackrel{\·}{z}}_2={-c}_1{z}_1^2+z_1{z}_2+z_2[f(x,t)+b(x,t)u+c_1{\stackrel{\·}{z}}_1-{\stackrel{\·\·}{z}}_d]---\left(16\right)$

为使 ${\stackrel{\·}{V}}_2\≤0,$ 设计控制器为

$u=\frac{1}{b(x,t)}[-f(x,t)-c_2{z}_2-z_1-c_1{\stackrel{\·}{z}}_1+{\stackrel{\·\·}{z}}_d]---\left(17\right)$

其中c₂>0。则

${\stackrel{\·}{V}}_2={-c}_1{z}_1^2-c_2{z}_2^2\≤0---\left(18\right)$

即当z₁→0和z₂→0时，有 $\stackrel{\·}{V}\→0$ 。通过控制律的设计，使得系统满足了Lyapunov稳定性理论条件，从而保证系统具有全局意义下的渐进稳定性。

根据反演设计方法，轨迹跟踪控制器的控制律设计为：

$U=\left[\begin{array}{c}v\\ \mathrm{\ω}\end{array}\right]=\left[\begin{array}{c}{v}_R\cos {\mathrm{\θ}}_e+k_3{x}_e\\ {\mathrm{\ω}}_R+k_1{v}_R{y}_e+k_2\sin {\mathrm{\θ}}_e\end{array}\right]---\left(19\right)$

其中，k₁，k₂，k₃为正常数。

利用Lyapunov函数设计稳定控制律，构造Lyapunov函数为

$V=\frac{k_1}{2}(x_e^2+y_e^2)-\cos {\mathrm{\θ}}_e+1---\left(20\right)$

显然，V≥0。如果P_E＝0，则V＝0；如果P_E≠0，则V>0。

对式(20)求导后将轨迹跟踪误差的微分方程代入：

$\stackrel{\·}{V}=k_1{x}_e{\stackrel{\·}{x}}_e+k_2{y}_e{\stackrel{\·}{y}}_e+\sin {\mathrm{\θ}}_e\·{\stackrel{\·}{\mathrm{\θ}}}_e$

$=k_1{x}_e(v_R\cos {\mathrm{\θ}}_e-v+y_e\mathrm{\ω})+k_1{y}_e(v_R\sin {\mathrm{\θ}}_e-x_e\mathrm{\ω})+\sin {\mathrm{\θ}}_e({\mathrm{\ω}}_R-\mathrm{\ω})$

$=k_1{x}_e{v}_R\cos {\mathrm{\θ}}_e-k_1{x}_{e}v+k_1{y}_e{v}_R\sin {\mathrm{\θ}}_e+\sin {\mathrm{\θ}}_e{\mathrm{\ω}}_R-\sin {\mathrm{\θ}}_e\mathrm{\ω}$

$=k_1{x}_e{v}_R\cos {\mathrm{\θ}}_e-k_1{x}_e{v}_R\cos {\mathrm{\θ}}_e-k_1{x}_e{k}_3{x}_e+k_1{y}_e{v}_R\sin {\mathrm{\θ}}_e+\sin {\mathrm{\θ}}_e{\mathrm{\ω}}_R---\left(21\right)$

$-\sin {\mathrm{\θ}}_e{\mathrm{\ω}}_R-\sin {\mathrm{\θ}}_e{k}_1{y}_e{v}_R-\sin {\mathrm{\θ}}_e{k}_2\sin {\mathrm{\θ}}_e$

$={-k}_1{x}_e{k}_3{x}_e-\sin {\mathrm{\θ}}_e{k}_2\sin {\mathrm{\θ}}_e$

$={-k}_1{k}_3{x}_e^2-k_2\sin {\mathrm{\θ}}_e^2\≤0$

显然V是一个弱Lyapunov函数(不严格小于零)，一个弱Lyapunov函数虽然不能像一个严格Lyapunov函数一样可以保证系统的全局稳定性，但可以用来设计一个全局稳定的控制律。因此，采用式(21)表示的控制律能够使WMR系统渐进收敛于稳定平衡点P_E＝0。

4.由分解模型建立求解目标

设WMR实际位姿为P_C＝[x_c，y_c，θ_c]^T，实际速度为[v_c，w_c]^T，参考位姿P_R＝[x_R，y_R，θ_R]^T，参考速度[v_R，w_R]^T，P_C和P_R之间的位姿误差矢量为P_E＝[x_e，y_e，θ_e]^T。如图2所示，WMR的轨迹跟踪问题就是对任意初始位姿和速度误差，寻求有界输入U＝[v_c，w_c]^T，使得具有实际位姿的WMR能够跟踪由P_R和速度控制量U_R＝[v_R，w_R]^T所描述的参考模型，并使得 $\underset{t\→\∞}{\lim }\left|\right|{[x_e,y_e,{\mathrm{\θ}}_e]}^T\left|\right|=0.$

由图2所示的几何关系，P_E可描述为

$\left[\begin{array}{c}{x}_e\\ y_e\\ {\mathrm{\θ}}_e\end{array}\right]=\left[\begin{array}{ccc}\cos {\mathrm{\θ}}_c& \sin {\mathrm{\θ}}_c& 0\\ -\sin {\mathrm{\θ}}_c& \cos {\mathrm{\θ}}_c& 0\\ 0& 0& 1\end{array}\right]\·\left[\begin{array}{c}{x}_R-x_c\\ y_R-y_c\\ {\mathrm{\θ}}_R-{\mathrm{\θ}}_c\end{array}\right]---\left(22\right)$

对式(22)求导可得到轨迹跟踪误差的微分方程

$\left[\begin{array}{c}{\stackrel{\·}{x}}_e\\ {\stackrel{\·}{y}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e\end{array}\right]=\left[\begin{array}{c}{v}_R\cos {\mathrm{\θ}}_e-v+y_e\mathrm{\ω}\\ v_R\sin {\mathrm{\θ}}_e-x_e\mathrm{\ω}\\ {\mathrm{\ω}}_R-\mathrm{\ω}\end{array}\right]---\left(23\right)$

轨迹跟踪问题实际上就是根据移动机器人位姿误差，通过移动机器人的速度跟踪控制器调节自身的跟踪位姿，减少位姿误差，达到跟踪理想轨迹的目的。图3为控制系统结构示意图，根据WMR的位姿误差及其参考输入产生控制输入U，然后通过WMR自身的速度跟踪控制器生成实际的U_c最终控制WMR的位姿。

5.利用QPSO算法对求解目标进行优化

QPSO算法与其他进化类算法相类似，具有进化和群体智能的特点。在QPSO算法中，每个候选解称为‘粒子’，若干个候选解就构成了群体。每个粒子没有重量和体积，通过目标函数确定它的适应值。每个粒子在解空间中运动，粒子通过追随自身的个体极值与群体的极值来动态的调整自己的位姿信息。

QPSO算法的描述如下：

假设算法的搜索空间为D维，粒子群的规模为N，每个粒子包含下列信息：

x_i＝(x_i1，x_i2，…x_iD)：粒子的当前位姿；

P_i＝(P_i1，P_i2，…P_iD)：粒子i的当前最优位姿，也可记为pbest；

P_g＝(P_g1，P_g2，…P_gD)：粒子群的全局最优位姿，也可记为gbest。

每个粒子都按照以下的进化公式来更新自己的位姿信息：

$\mathrm{mbest}\left(t\right)=\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{P}_i\left(t\right)=(\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{P}_{i1,}\left(t\right)\·\·\·,\frac{1}{N}\underset{i=1}{\overset{N}{\mathrm{\Σ}}}{P}_{\mathrm{iD}}\left(t\right))---\left(24\right)$

p_id(t)＝φ·P_id(t)+(1-φ)·P_gd(t)，φ＝rand   (25)

x_id(t+1)＝p_id(t)±α·|mbest_d(t)-x_id(t)|·ln(1/u)，u＝rand  (26)

其中，t是当前的迭代次数，mbest称为平均最优位姿，它是所有粒子自身最优位姿的中心点；p_id为P_id与P_gd构成的超矩形中的一个随机点；参数α称为压缩-扩张因子，可以用来控制粒子的收敛速度，采用如下的取值方式，

α＝(1.0-0.5)×(MAXITER-t)/MAXITER+0.5        (27)

其中，t是当前迭代次数，MAXITER算法的最大迭代次数。

在式(19)控制律方程中，有三个正常数k₁、k₂、k₃没有确定的数值，由前述分析可知控制律中参数设置不同在很大程度上影响着移动机器人跟踪控制的效果。因此，需要根据跟踪控制的目标设置合适的k₁、k₂、k₃数值，使得移动机器人跟踪达到较好的品质。而传统的控制器参数设置方法一般为试凑法，根据经验通过不断的带入不同的参数值来观察移动机器人跟踪的效果，直到符合跟踪控制的要求。由前面章节可知，群体智能算法在目标问题不提供全局模型的前提下，能够快速的搜寻到适合问题的解。因此，这里使用群体智能算法搜索合适的跟踪控制器的参数k₁、k₂、k₃的值。

移动机器人轨迹跟踪的目标是使得机器人能够从实际的初始位姿快速的和跟踪轨迹保持一致，即尽可能的在条件允许的情况下使得x_e，y_e小，并且由于实际应用中，移动机器人的线速度v不可能有非常大的值，因此在仿真中我们设置群体智能算法适应度函数为

$f=\frac{1}{2}\mathrm{\Σ}(x_e^2+y_e^2+0.1\·v_e^2)---\left(28\right)$

其中，v_e是理想速度和实际速度的差值，此问题即为一个最小化优化问题。

QPSO算法完成轮式移动机器人轨迹跟踪参数优化的步骤：

步骤1：初始化算法参数，包括粒子数，问题维数，初始化空间及搜索空间，粒子的初始位置，初始最优值等；

步骤2：由式(24)计算群体的平均最优位置mbest(t)；

步骤3：由式(25)计算随机位置p_id(t+1)；

步骤4：由式(26)计算粒子的新位置x_id(t+1)；

步骤5：由式(7)计算粒子新位置的适应度fitness(x_i(t+1))；

步骤6：更新粒子的当前最优位置，即：如果

fitness(x_i(t+1))<fitness(pbest_i(t))，则pbest_i(t+1)＝x_i(t+1)，否则，

pbest_i(t+1)＝pbest_i(t)；

步骤7：更新群体的最优位置，即：如果fitness(pbest_i(t+1))<fitness(gbest(t))，则gbest(t+1)＝pbest_i(t+1)；

步骤8：循环步骤2～7，直至满足一定的结束条件，然后输出群体的全局最优位置gbest，即为控制器的最优参数。

在本发明的实例验证中，采用的模型如下：

跟踪线速度和角速度均为匀速运动的圆轨迹，圆的轨迹方程为(x-3)²+(y-3)²＝9，取v_r＝1.0m/s，则ω_r＝1.0rad/s， ${\stackrel{.}{v}}_r=0,$ 位姿指令p_r＝(x_r y_r θ_r)^T为

$\left\{\begin{array}{c}{x}_r=3+3\cos \left({\mathrm{\&omega;}}_{r}t\right)=3+3\cos t\\ y_r=3+3\sin \left({\mathrm{\&omega;}}_{r}t\right)=3+3\sin t\\ {\mathrm{\&theta;}}_r={\mathrm{\&omega;}}_{r}t=t\end{array}\right.---\left(29\right)$

位姿误差初始值为[3.2  -1  0]^T。

实例条件如下：

分别采用经典粒子群算法(PSO)和量子行为粒子群算法(QPSO)对实例进行了仿真。对于PSO算法，惯性系数w采用线性递减的方式取值，起始值为0.9，终止值为0.4；对于QPSO算法，控制参数α采用线性递减方式取值，起始值为1.0，终止值为0.5。每个算法的进化次数为50次，分别独立运行30轮。表1中给出了两种算法仿真的目标函数值，及其对应的控制参数k₁、k₂、k₃的值。图4、5、6是PSO算法优化后的控制器跟踪圆的性能；图7、8、9是QPSO算法优化后的控制器跟踪圆的性能。从表1中可以看出，QPSO算法优化得到的目标函数值更优；并且从图4—9的性能图可以看到，用QPSO算法优化的控制器跟踪圆的性能比PSO算法的更好。

表1 使用两种算法求解实例1的函数值及系数

Claims (6)

1、一种基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征在于该方法包含如下步骤：

步骤一：首先在机器人运动的二维平面上，建立两个独立的平面坐标系，从而来共同标识出轮式移动机器人某个时刻的空间位姿与朝向；

步骤二：在步骤一建立的坐标系基础上，使用Euler-Lagrange方程建立轮式移动机器人的运动学模型；

步骤三：将步骤二得到的运动学方程利用反演设计方法分解成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量，直到完成整个控制律的设计；

步骤四：根据步骤三得到的子系统建立参考位姿与实际位姿的误差函数；

步骤五：利用量子行为粒子群算法求解步骤四得到的误差函数，直到符合跟踪控制的要求。

2、如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是：步骤一中需要用两个独立的平面坐标系来共同标识出机器人某时刻的空间位置与朝向，在机器人学中统称为位姿，也就是把一个在空间中占有一定体积的非完整移动机器人系统抽象为一个参考点，用参考点的坐标来代表非完整移动机器人平台。

3、如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是：步骤二中设[x，y]^T为轮式移动机器人质心的笛卡儿坐标，θ为其前进方向与x轴夹角，v、ω分别为WMR的平移速度和旋转速度，由于从动轮在运动中仅仅起支撑作用，在运动学模型中的影响可忽略不计，不考虑外界干扰及建模误差带来的影响，将具有非完整约束的WMR模型用经典Euler-Lagrange方程表示出。

4、如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是：步骤三中应用反演设计方法将复杂的非线性系统分解成不超过系统阶数的子系统，然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量，一直后退到整个系统，直到完成整个控制律的设计，使整个闭环系统满足期望的动静态性能指标。

5、如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是：步骤四中对任意初始位姿和速度误差，寻求有界输入U＝[v_c，w_c]^T，使得具有实际位姿的WMR能够跟踪由参考位姿P_R和速度控制量U_R＝[v_R，w_R]^T所描述的参考模型，并使得 $\underset{t\&RightArrow;\&infin;}{\lim }\left|\right|{[x_e,y_e,{\mathrm{\&theta;}}_e]}^T\left|\right|=0.$

6、如权利要求1所述的基于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法，其特征是：步骤五以步骤四中得到的误差函数作为求解的目标，应用量子行为粒子群算法求解得一组控制参数，具体为：在解空间初始化一组粒子，计算粒子的目标函数值，然后粒子通过量子行为粒子群算法的搜索策略，寻找个体最优位姿与全局最优位姿经过若干次的迭代以后完成寻优过程，最后输出全局最优解，即为最优的控制参数。

Images