说明书一种基于位置估计的机器人编队控制方法
技术领域
本发明属于多机器人编队控制技术领域，具体涉及一种基于位置估计的机器人编队控制方法。
背景技术
对于多自主体，有时要用自治的方式来实现一个集体任务，由于上述情况，需要研究多自主体在只需通过局部或者部分测量数据的情况下分布式的编队控制。在控制研究进展中，可计算和可通信为多移动自主体系统的分散式控制提供了先进技术。
最常见的编队控制问题确立以后，开始研究自主体感知邻居节点的相对位置或通过控制相对位置坐标而不知道各自的绝对位置来实现期望编队队形等的情形，现有一些编队控制算法可以解决这类的编队问题并保证真实队形到期望队形的全局收敛。
大多数情况而言，期望获知多自主体各自的位置信息。首先，针对自主体的任务，有时候得到位置信息是至关重要的。比如，如果自主体收集到一些感知到的数据，位置信息需要作为收集信息的附加信息。对于现存的编队控制算法或者位置感知设备如GPS，这些位置信息通常是不能够准确得到的。其次，如果每个自主体准确的知道它们的真实位置和期望的位置点，那么期望的队形可以通过控制每个个体的位置而很容易实现，每个自主体可以沿着它们距离目标点的最近路径运动从而实现期望的队形即可，即而可以减少一些控制成本。
发明内容
有鉴于此，本发明的目的是针对现有编队控制中大都需要机器人的绝对位置信息这一缺陷，提出一种基于位置估计的机器人编队控制方法。
实现本发明的技术方案如下：
一种基于位置估计的机器人编队控制方法，其所针对的机器人系统为一阶系统，具体过程为：
101、设计一阶系统的位置估计器，如式(2)所示，
p^·i=ui+koΣj=1Naij(p^ji-pji)---(2)]]>
其中，ko＞0，i＝1,2，…,N,N为编队中机器人的总数；当机器人i可以获得机器人j的信息时，aij＝1，否则，aij＝0；ui为机器人i的控制输入，为机器人i的估计位置的导数，为机器人i与机器人j的相对估计位置，pji为机器人i与机器人j的相对实际位置；
102、基于一阶系统的位置估计器，设计一阶系统位置控制器，如式(5)所示；
ui=kcp*i-kcp^i---(5)]]>
其中kc＞0，是机器人i的期望位置；
103、根据一阶系统位置控制器，实现对机器人编队的控制。
一种基于位置估计的机器人编队控制方法，其所针对的机器人系统为二阶系统，具体过程为：
201、设计二阶系统的位置估计器，如式(8)所示，
p^·i=vi+koΣj=1Naij(p^ji-pji)---(8)]]>
其中，ko＞0，i＝1,2，…,N,N为编队中机器人的总数；当机器人i可以获得机器人j的信息时，aij＝1，否则，aij＝0；vi为机器人i的绝对速度，为机器人i的估计位置的导数，为机器人i与机器人j的相对估计位置，pji为机器人i与机器人j的相对实际位置；
202、基于二阶系统的位置估计器，设计二阶系统位置控制器，如式(9)所示；
ui=-Σj=1Naij(t)[(p^i-pid-(p^j-pjd))+γ(vi-vj)]---(9)]]>
其中，ui为机器人i的控制输入，pid和pjd是期望队形中机器人i和j的期望位置，和是机器人i和j的估计位置，vi和vj是机器人i和j的绝对速度，γ＞0；
203、根据二阶系统位置控制器，实现对机器人编队的控制。
一种基于位置估计的机器人编队控制方法，其所针对的机器人系统为差动系统，具体过程为：
301、设计差动系统的位置估计器，如式(2)所示，
p^·i=ui+koΣj=1Naij(p^ji-pji)---(2)]]>
其中，ko＞0，i＝1,2，…,N,N为编队中机器人的总数；当机器人i可以获得机器人j的信息时，aij＝1，否则，aij＝0；ui为机器人i的控制输入，为机器人i的估计位置的导数，为机器人i与机器人j的相对估计位置，pji为机器人i与机器人j的相对实际位置；
302、设计机器人编队控制输入：
vi=kccosθisinθi(pi*-p^i)T]]>
θ·i=ωi---(13)]]>
ωi＝cos(ktt)
其中，kc＞0且kt＞0，t表示时间，是机器人i的期望位置；
基于差动系统的位置估计器，设计差动系统位置控制器，如式(14)所示，
ui=kcM(θi)(pi*-p^i)---(14)]]>
其中
M(θi)=cos2θisinθicosθisinθicosθisin2θi.]]>
303、根据差动系统位置控制器，实现对机器人编队的控制。
有益效果
第一，使用了编队过程中位置估计的思想，可以在编队中避免了使用每个机器人的绝对位置这一条件。
第二，结合位置估计器，分别作用在多机器人形成的一阶系统、二阶系统和差动系统模型中，在多机器人形成的拓扑图结构中，每个模型分别在满足一定条件下，可以实现有效的编队运动，达到期望的编队队形。编队过程开始时，机器人的位置可以任意，都可以形成期望的编队效果。
第三，考虑到编队过程的真实情况，在编队过程中加入避碰控制输入，有效地完成了在避免碰撞的情况下实现编队。
第四，本发明可通过MATLAB仿真来验证验法的有效性，并在实验室自主搭建的多机器人移动实验平台上进行实验测其可行。
附图说明
图1为本发明基于位置估计的机器人编队控制方法的流程图；
图2基于分布式位置估计的编队控制结构框图；
图3一阶系统多机器人交互拓扑图；
图4一阶系统基于位置估计的二维平面运动轨迹；
图5一阶系统基于位置估计的估计误差轨迹；
图6一阶系统加避碰项二维平面运动轨迹；
图7一阶系统加避碰项估计误差轨迹；
图8二阶系统交互拓扑图；
图9二阶系统基于位置估计的二维平面运动轨迹；
图10二阶系统基于位置估计的估计误差轨迹；
图11差动模型基于位置估计的二维平面运动轨迹；
图12差动模型基于位置估计的误差轨迹；
图13E-PUCK机器人x坐标的位置曲线；
图14E-PUCK机器人y坐标的位置曲线。
具体实施方式
下面结合附图和具体实施方式对本发明进行详细说明。
在本发明中符号上方的“〃”表示该符号所表示物理意义的导数，符号上方的“＾”表示该符号所表示物理意义的估计值。
针对一阶系统机器人，如图1-2所示，基于位置估计的编队控制方法的过程为：
101、设机器人一阶系统模型如式(1)所示，
p·=u---(1)]]>
其中，p表示机器人的位置，u为机器人的控制输入；
设计一阶系统的位置估计器，如式(2)所示，
p^·i=ui+koΣj=1Naij(p^ji-pji)=ui+koΣj=1Naij[(p^j-p^i)-(pj-pi)]---(2)]]>
其中，ko＞0，i＝1,2，…,N,N为编队中机器人的总数，当机器人i可以获得机器人j的信息时，aij＝1，否则，aij＝0；ui为机器人i的控制输入，为机器人j的估计位置，为机器人i的估计位置，为机器人i与机器人j的相对估计位置，pji为机器人i与机器人j的相对实际位置，可见，此处不需要各机器人的确切位置，它所需要的仅仅是相对实际位置pji和信息
把机器人编队的估计动态模型重写为：
p^·=-ko(L⊗In)(p^-p)+u---(3)]]>
L=Σj=2Na1j-a12...-a1N-a11Σj=1,j≠2Na1j...-a1N............-a11-a12...Σj=1,j≠NNa1j]]>
其中，In表示n阶单位矩阵，表示克罗内克积，p＝[p1,p2…pN]T,u＝[u1,u2…uN]T；
再通过定义系统误差把整个系统的估计误差改写成：
p~·=-ko(L⊗In)p~---(4)]]>
其中，p~=[p~1,p~2...,p~N];]]>
可以证明对于动态估计模型式(2)，存在一个有限的向量使得当且仅当多机器人拓扑形成的图有一个生成树时，可以达到全局渐近趋于的效果，其中为常数，表示机器人的估计误差。
102、基于一阶系统的位置估计器，设计一阶系统位置控制器；
由于对机器人i来说，向量和都是可以得到的，机器人编队的编队控制器设计为：
ui=kcp*i-kcp^i---(5)]]>
其中kc＞0，是机器人i的期望位置。
对于机器人编队来说整个动态系统可以被重写为：
p·=kcp*-kcp^]]> (6)
p^·=kcp*-kcp^+ko(L⊗In)(p-p^)]]>
通过证明式(6)是稳定收敛，因此可知基于位置估计器的编队控制器式(5)的稳定性和有效性。
103、根据一阶系统位置控制器，实现对机器人编队的控制。
针对二阶系统机器人，如图1-2所示，基于位置估计的编队控制器方法的过程为：
201、设机器人二阶系统模型如式(7)所示，
p·=vv·=u---(7)]]>
其中，v表示机器人的绝对速度；
同样地，考虑当不能确切得到机器人i的绝对位置时的情况，但假设绝对速度v是可以得到的，即向量v是已知量。
类似于一阶系统中位置估计器的设计，设计二阶系统的位置估计器，如式(8)所示，
p^·i=vi+koΣj=1Naij(p^ji-pji)---(8)]]>
采用与一阶系统相同的方法证明式(8)所示的位置估计器具有全局渐近稳定的效果。
202、基于二阶系统的位置估计器，设计二阶系统位置控制器，如式(9)所示；
ui=-Σj=1Naij(t)[(p^i-pid-(p^j-pjd))+γ(vi-vj)]---(9)]]>
其中，pid是期望队形中机器人i的期望位置，和是用式(8)计算出的机器人i和j的估计位置，vi和vj是机器人i和j的绝对速度，且γ＞0。
通过令和vi＝ζi，把上述控制输入写成另外的形式：
ui=-Σj=1naij[(ξi-ξj)+γ(ζi-ζj)]---(10)]]>
通过证明式(10)是稳定的，因此可以得知本发明提出的编队控制器式(9)是稳定、有效的。
203、根据二阶系统位置控制器，实现对机器人编队的控制。
针对差动系统机器人，如图1-2所示，基于位置估计的编队控制方法的过程为：
301、设机器人差动系统模型如式(11)所示，
p·=u---(11)]]>
u=v(cosθ,sinθ)T,θ·=ω---(12)]]>
其中，ω为机器人的角速度；
由式(11)可见，与一阶模型的系统相似，所以对于该系统的位置估计器
可以同样一阶系统的位置估计器，如式(2)所示，
p^·i=ui+koΣj=1Naij(p^ji-pji)=ui+koΣj=1Naij[(p^j-p^i)-(pj-pi)]---(2)]]>
302、设计其编队控制输入：
vi=kccosθisinθi(pi*-p^i)T]]> (13)
ωi＝cos(ktt)
其中kc＞0且kt＞0，t表示时间。
基于差动系统的位置估计器，设计差动系统位置控制器，如式(14)所示，
ui=kcM(θi)(pi*-p^i)---(14)]]>
其中
M(θi)=cos2θisinθicosθisinθicosθisin2θi]]>
则把整个误差动态系统重新写成：
e·=-kcH(θ)e+kcH(θ)p~]]>
θ·=cos(ktt)1N]]>
p~·=-koL⊗I2p~---(15)]]>
e＝p-p*
H(θ)＝diag(M(θ1),M(θ2)…,M(θN))
通过证明式(15)是稳定，因此可以得知本发明编队控制器式(14)是稳定和有效的。
考虑到编队过程中可能存在的机器人之间的碰撞的情况，因此使用势能场函数形式为：
Vij(xip,xjp)=(min{0,||xip-xjp||2-R2||xip-xjp||2-r2})2---(16)]]>
其中，代表机器人i的位置状态，代表机器人j的位置状态，R代表机器人的可以碰撞安全距离，r代表机器人间的可以允许的最小半径。
由式(16)，机器人i需要碰撞时，位置控制器的输入需要增加避障控制输入，如式17所示：
τia=-Σj=1;j≠iN∂Vij(xip,xjp)T∂xip---(17)]]>
其中，为机器人i的避障控制输入，代表了标量函数V关于向量x的梯度(是一个行向量)，x的元素是独立的变量。
实例：
为了验证上述多机器人系统基于位置估计的编队控制方法的有效性，本发明通过MATLAB仿真来验证验法的有效性，并在实验室自主搭建的多机器人移动实验平台上进行实验测其可行。
1、一阶模型仿真
采用的机器人组成的交互拓扑图如图3所示，仿真时采用的初始位置分别为p(1)＝(-3,-0.5),p(2)＝(-0.5,0.2),p(3)＝(0,-0.4),p(4)＝(-0.4,-0.3),p(5)＝(0.3,0.4),p(6)＝(0,0.45)。图4和图5显示了单积分器模型的多机器人系统在估计律(2)和控制律(5)作用下的结果。
加入避碰项后，机器人的运动轨迹和估计误差控制误差结果如图6和图7所示。由图4和图6作对比可见，起点为(-0.4，-0.3)和(-0.1，-0.4)的两点与起点为(0.3，0.4)和(0，0.45)的两点在运动中会产生相互碰撞的可能性，加入避碰项后，两点在原轨迹的基础上会相互绕行，且不影响编队效果。
2、二阶模型仿真
此处对于多机器人形成的二阶系统模型中，使用的多机器人系统的交互拓 扑图如图8所示。
图9和图10展示了二阶模型机器人在估计律(8)和控制律(9)的作用下，且γ＝1时的仿真结果。
3、差动独轮模型仿真
对于差动独轮模型系统，选用的拓扑结构同一阶系统中的图3所示的六机器人系统，图11和图12描述了差动系统在估计律式(2)和控制律(13)作用下的仿真结果。由仿真图可见，估计位置渐渐收敛，且编队队形最终可以达到期望的编队队形。
4、基于多机器人移动平台的实验
使用本发明中提出的基于位置估计的方法，应用于实验室自主开发的多机器人移动平台之中。本实验中选用三个机器人，分别标志颜色为红色，蓝色和黄色的三个小车，编号分别为一号车，二号车和三号车，任意给定E-PUCK小车的初始位置，期望的编队队形为三角形，初始位置给出为p1d＝(0,0)p2d＝(0.4,0)，和p3d＝(0.2,0.3)，算出它们的相对期望位置为p12＝(0.4,0)，p23＝(0.2,-0.3)和p13＝(0.2,0.3)，上位PC机采集并保存到了E-PUCK小车的实时位置信息，将数据信息导入到外部文件中，把该文件使用MATLAB的文件操作函数，提取出位置坐标并拟合出实时运动曲线，如图13和14所示。
综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。