本发明的目的是克服
现有技术中存在的不足,提供一种可以解决非线性 与非完整性给轨迹跟踪过程带来的困难、大大地提高了跟踪
精度和速度的基 于量子行为粒子群算法的轮式移动机器人轨迹跟踪方法。
按照本发明提供的技术方案,所述基于量子行为粒子群算法的轮式移动 机器人轨迹跟踪方法包含如下步骤:
步骤一:首先在机器人运动的二维平面上,建立两个独立的平面
坐标系, 从而来共同标识出轮式移动机器人某个时刻的空间
位姿与朝向;
步骤二:在步骤一建立的坐标系
基础上,使用Euler-Lagrange方程建立 轮式移动机器人的运动学模型;
步骤三:将步骤二得到的运动学方程利用反演设计方法分解成不超过系 统阶数的子系统,然后为每个子系统分别设计Lyapunov函数和中间虚拟控制 量,直到完成整个控制律的设计;
步骤四:根据步骤三得到的子系统建立参考位姿与实际位姿的误差函数;
步骤五:利用量子行为粒子群算法求解步骤四得到的误差函数,直到符 合跟踪控制的要求。
上述步骤一即建立坐标系:由于机器人运动本身具有方向性,所以在机 器人运动的二维平面上,需要用两个独立的平面坐标系来共同标识出机器人 某时刻的空间
位置与朝向,在机器人学中统称为位姿,也就是把一个在空间 中占有一定体积的非完整移动机器人系统抽象为一个参考点,用参考点的坐 标来代表非完整移动机器人平台;
上述步骤二即建立运动学模型:设[x,y]T为轮式移动机器人(WMR)质心 的笛卡儿坐标,θ为其前进方向与X轴夹
角,v、ω分别为WMR的平移速度 和旋转速度。由于从动轮在运动中仅仅起
支撑作用,在运动学模型中的影响 可忽略不计。不考虑外界干扰及建模误差带来的影响,具有非完整约束的 WMR模型可以用经典Euler-Lagrange方程表示出;
上述步骤三即简化运动学模型:利用反演设计方法降低系统的阶数,主 要方法是将复杂的非线性系统分解成不超过系统阶数的子系统,然后为每个 子系统分别设计Lyapunov函数和中间虚拟控制量,一直“后退”到整个系统, 直到完成整个控制律的设计,使整个
闭环系统满足期望的动静态性能指标;
上述步骤四即确定目标函数:轨迹跟踪问题实际上就是根据移动机器人 位姿误差,通过移动机器人的速度跟踪控制器调节自身的跟踪位姿,减少位 姿误差,达到跟踪理想轨迹的目的。设WMR实际位姿为PC=[xc,yc,θc]T,实 际速度为[vc,wc]T,参考位姿PR=[xR,yR,θR]T,参考速度[vR,wR]T,PC和PR之间的 位姿误差矢量为PE=[xe,ye,θe]T。WMR的轨迹跟踪问题就是对任意初始位姿和 速度误差,寻求有界输入U=[vc,wc]T,使得具有实际位姿的WMR能够跟踪 由参考位姿PR和速度控制量UR=[vR,wR]T所描述的参考模型,并使得
上述步骤五即求解参数:利用量子行为粒子群算法求解步骤四得到的误 差函数,直到符合跟踪控制的要求,即在解空间初始化一组粒子,计算粒子 的目标函数值,然后粒子通过量子行为粒子群算法的搜索策略,寻找个体最 优位姿与全局最优位姿经过若干次的
迭代以后完成寻优过程,最后输出全局 最优解,即为最优的控制参数。
本发明与已有技术相比,设计过程简单,系统灵活,鲁棒性强,跟踪速 度快。因此,本发明解决了非线性与非完整性给轨迹跟踪过程带来的困难, 大大地提高了跟踪精度和速度。
附图说明
图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是以二维平面中某一点为原点的全局坐标 系,X1-Y1是固定于非完整移动机器人本身的横轴正方向始终与机器人朝向 相一致的局部坐标系。为了简化机器人的数学模型,通常把机器人的质心平 移到
驱动轴中心,局部坐标系的原点就是驱动轴中心,(x,y)为移动机器人质 心的笛卡儿坐标,即机器人的位姿。θ为其前进方向与X轴夹角,即机器人 的朝向,也称为方向角,v和ω分别为移动机器人的平移速度和旋转速度。vL 和vR分别是移动机器人左轮和右轮的线速度。R为非
完整机器人的驱动轮半 径,L为非完整移动机器人两个驱动轮轮心之间的距离。这样,非完整移动 机器人的位姿q就可以表示为[x,y,θ]T。
假设WMR的运动是一种无侧滑、纯滚动运动,则在WMR运动过程中, O1点的速度与位姿角θ之间应满足约束关系
式(1)表示WMR的瞬时运动约束在2
自由度方向,其物理意义是WMR 在两驱动轮轴线上的速度始终为0,WMR瞬时速度的方向始终与WMR的朝 向保持一致。显然此约束条件是不可积的,因此式(1)就是WMR的非完整 约束,非完整约束条件表明机器人只能在垂直于轮轴的方向上运动。非完整 约束要求WMR的运动除了满足起始点、终点和障碍物等基本要求外,还必 须满足方向或者轨迹形状的特殊要求,即:有纯几何方法计算得到的适用于 完整系统的轨迹,并不一定适用与非完整系统。
2.轮式移动
机器人运动学模型的建立
图1所示的移动机器人为轮式移动机器人,[x,y]T为WMR质心的笛卡儿 坐标,θ为其前进方向与X轴夹角,v、ω分别为WMR的平移速度和旋转速 度,从动轮在运动中仅仅起支撑作用,在运动学模型中的影响可忽略不计。
不考虑外界干扰及建模误差带来的影响,具有非完整约束的WMR模型用 经典Euler-Lagrange方程表示为:
式(2)、(3)中,q∈Rn为n维广义坐标,M(q)∈Rn×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)化为
。
令m为WMR的整体质量,I为
车轮的转动惯量,τ1为驱动力矩,τ2为转 动力矩,则式(1)可以转化为:
其中,
图1所示的WMR受到式(4)的非完整约束,可求得A(q)=[sin θ,-cos θ,0]T, 那么可求得S(q)=[cos θ,sin θ,0;0,0,1]T。如果选择输入向量U=[v,ω]T,则WMR的运 动学模型为
3.利用反演设计方法分解运动学模型
反演设计方法的基本思想是将复杂的非线性系统分解成不超过系统阶数 的子系统,然后为每个子系统分别设计Lyapunov函数和中间虚拟控制量,一 直“后退”到整个系统,直到完成整个控制律的设计,使整个闭环系统满足 期望的动静态性能指标。
假设被控对象为
其中,b(x,t)≠0。基本反演控制方法设计步骤如下:
步骤1:
定义位姿误差
z1=x1-zd (7)
其中zd为指令
信号。则
定义虚拟控制量
其中c1>0。
定义z2=x2-α1 (10)
定义Lyapunov函数
则
将式(11)代入式(12)得
如果z2=0,则
。为此,需要进行下一步设计。
步骤2:
定义Lyapunov函数
由
则
为使
设计控制器为
其中c2>0。则
即当z1→0和z2→0时,有
。通过控制律的设计,使得系统满足了 Lyapunov稳定性理论条件,从而保证系统具有全局意义下的渐进稳定性。
根据反演设计方法,轨迹跟踪控制器的控制律设计为:
其中,k1,k2,k3为正常数。
利用Lyapunov函数设计稳定控制律,构造Lyapunov函数为
显然,V≥0。如果PE=0,则V=0;如果PE≠0,则V>0。
对式(20)求导后将轨迹跟踪误差的微分方程代入:
显然V是一个弱Lyapunov函数(不严格小于零),一个弱Lyapunov函 数虽然不能像一个严格Lyapunov函数一样可以保证系统的全局稳定性,但可 以用来设计一个全局稳定的控制律。因此,采用式(21)表示的控制律能够 使WMR系统渐进收敛于稳定平衡点PE=0。
4.由分解模型建立求解目标
设WMR实际位姿为PC=[xc,yc,θc]T,实际速度为[vc,wc]T,参考位姿 PR=[xR,yR,θR]T,参考速度[vR,wR]T,PC和PR之间的位姿误差矢量为 PE=[xe,ye,θe]T。如图2所示,WMR的轨迹跟踪问题就是对任意初始位姿和速 度误差,寻求有界输入U=[vc,wc]T,使得具有实际位姿的WMR能够跟踪由PR 和速度控制量UR=[vR,wR]T所描述的参考模型,并使得
由图2所示的几何关系,PE可描述为
对式(22)求导可得到轨迹跟踪误差的微分方程
轨迹跟踪问题实际上就是根据移动机器人位姿误差,通过移动机器人的 速度跟踪控制器调节自身的跟踪位姿,减少位姿误差,达到跟踪理想轨迹的 目的。图3为控制系统结构示意图,根据WMR的位姿误差及其参考输入产 生控制输入U,然后通过WMR自身的速度跟踪控制器生成实际的Uc最终控 制WMR的位姿。
5.利用QPSO算法对求解目标进行优化
QPSO算法与其他进化类算法相类似,具有进化和
群体智能的特点。在 QPSO算法中,每个候选解称为‘粒子’,若干个候选解就构成了群体。每个粒 子没有重量和体积,通过目标函数确定它的适应值。每个粒子在解空间中运 动,粒子通过追随自身的个体极值与群体的极值来动态的调整自己的位姿信 息。
QPSO算法的描述如下:
假设算法的搜索空间为D维,粒子群的规模为N,每个粒子包含下列信 息:
xi=(xi1,xi2,…xiD):粒子的当前位姿;
Pi=(Pi1,Pi2,…PiD):粒子i的当前最优位姿,也可记为pbest;
Pg=(Pg1,Pg2,…PgD):粒子群的全局最优位姿,也可记为gbest。
每个粒子都按照以下的进化公式来更新自己的位姿信息:
pid(t)=φ·Pid(t)+(1-φ)·Pgd(t),φ=rand (25)
xid(t+1)=pid(t)±α·|mbestd(t)-xid(t)|·ln(1/u),u=rand (26)
其中,t是当前的迭代次数,mbest称为平均最优位姿,它是所有粒子自 身最优位姿的中心点;pid为Pid与Pgd构成的超矩形中的一个随机点;参数α称 为压缩-扩张因子,可以用来控制粒子的收敛速度,采用如下的取值方式,
α=(1.0-0.5)×(MAXITER-t)/MAXITER+0.5 (27)
其中,t是当前迭代次数,MAXITER算法的最大迭代次数。
在式(19)控制律方程中,有三个正常数k1、k2、k3没有确定的数值, 由前述分析可知控制律中参数设置不同在很大程度上影响着移动机器人跟 踪控制的效果。因此,需要根据跟踪控制的目标设置合适的k1、k2、k3数值, 使得移动机器人跟踪达到较好的品质。而传统的控制器参数设置方法一般为 试凑法,根据经验通过不断的带入不同的参数值来观察移动机器人跟踪的效 果,直到符合跟踪控制的要求。由前面章节可知,群体智能算法在目标问题 不提供全局模型的前提下,能够快速的搜寻到适合问题的解。因此,这里使 用群体智能算法搜索合适的跟踪控制器的参数k1、k2、k3的值。
移动机器人轨迹跟踪的目标是使得机器人能够从实际的初始位姿快速 的和跟踪轨迹保持一致,即尽可能的在条件允许的情况下使得xe,ye小,并 且由于实际应用中,移动机器人的线速度v不可能有非常大的值,因此在仿 真中我们设置群体智能算法适应度函数为
其中,ve是理想速度和实际速度的差值,此问题即为一个最小化优化问 题。
QPSO算法完成轮式移动机器人轨迹跟踪参数优化的步骤:
步骤1:初始化算法参数,包括粒子数,问题维数,初始化空间及搜索空 间,粒子的初始位置,初始最优值等;
步骤2:由式(24)计算群体的平均最优位置mbest(t);
步骤3:由式(25)计算随机位置pid(t+1);
步骤4:由式(26)计算粒子的新位置xid(t+1);
步骤5:由式(7)计算粒子新位置的适应度fitness(xi(t+1));
步骤6:更新粒子的当前最优位置,即:如果
fitness(xi(t+1))
pbesti(t+1)=pbesti(t);
步骤7:更新群体的最优位置,即:如果fitness(pbesti(t+1))步骤8:循环步骤2~7,直至满足一定的结束条件,然后输出群体的全局 最优位置gbest,即为控制器的最优参数。
在本发明的实例验证中,采用的模型如下:
跟踪线速度和角速度均为匀速运动的圆轨迹,圆的轨迹方程为 (x-3)2+(y-3)2=9,取vr=1.0m/s,则ωr=1.0rad/s, 位姿指令 pr=(xr yr θr)T为
位姿误差初始值为[3.2 -1 0]T。
实例条件如下:
分别采用经典粒子群算法(PSO)和量子行为粒子群算法(QPSO)对实例进 行了仿真。对于PSO算法,惯性系数w采用线性递减的方式取值,起始值为 0.9,终止值为0.4;对于QPSO算法,控制参数α采用线性递减方式取值, 起始值为1.0,终止值为0.5。每个算法的进化次数为50次,分别独立运行 30轮。表1中给出了两种算法仿真的目标函数值,及其对应的控制参数k1、 k2、k3的值。图4、5、6是PSO算法优化后的控制器跟踪圆的性能;图7、8、 9是QPSO算法优化后的控制器跟踪圆的性能。从表1中可以看出,QPSO 算法优化得到的目标函数值更优;并且从图4—9的性能图可以看到,用QPSO 算法优化的控制器跟踪圆的性能比PSO算法的更好。
表1 使用两种算法求解实例1的函数值及系数