多机器人协调队形控制方法

摘要

本发明一种多机器人协调队形控制方法，是一种跟随机器人通过递推预测，进而实现与领航机器人的队形控制。跟随机器人结合视觉、码盘信息，获取当前采样时刻k与之前的Npmax次采样时刻相对于领航机器人的距离和观测方位角dk-s，借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-Npmax时的领航机器人在跟随机器人局部坐标系下的位置以及采样时刻k时的跟随机器人前向摄像机的光心Copt在下的位置计算出预测的领航机器人运动方向和领航机器人到Copt方向之间的角度进而结合期望角度ψijd以及跟随机器人与领航机器人之间的距离(即dk)和期望距离Lijd，综合视觉信息，进行运动决策，得到速度νf和转角θf。本发明适合通讯失效的情况，为多机器人在军事等方面的应用提供技术支持。

权利要求

1.一种多机器人协调队形控制方法，用于多机器人系统；其特征在于，包括步骤：A)跟随机器人依据领航机器人上的色标筒，结合视觉与码盘信息，获取当前采样时刻k与之前的Npmax次采样时刻相对于领航机器人的距离和观测方位角dk-s， (s＝0，…，Npmax)；

B)跟随机器人借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-Npmax时的领航机器人在跟随机器人局部坐标系 下的位置以及当前采样时刻k时的跟随机器人前向摄像机的光心Copt在

坐标系下的位置

C)基于位置Lk、 和 跟随机器人计算出预测的领航机器人运动方向和领航机器人到Copt方向之间的角度D)结合角度 以及期望角度ψijd、跟随机器人与领航机器人之间的距离 以及期望距离Lijd，综合视觉信息，进行运动决策，得到速度vf和转角θf，实现对跟随机器人速度大小和方向的控制；

其中Ri和Rj分别表示跟随机器人和领航机器人。

2.如权利要求1所述的多机器人协调队形控制方法，其特征在于，领航机器人上的色标筒为中空的圆柱形筒，由至少两种颜色上下组合而成，放于机器人上，其中心与机器人的中心保持一致。

3.如权利要求1所述的多机器人协调队形控制方法，其特征在于，队形控制是在跟随机器人对领航机器人感知推理的基础上实现的，机器人之间没有通讯。

4.如权利要求1所述的多机器人协调队形控制方法，其特征在于，所述B)步中的跟随机器人局部坐标系 是与采样时刻k-Npmaz相对应，其原点取前向摄像机光心Copt所在的位置， 的方向与采样时刻k-Npmax时的跟随机器人运动方向保持一致。

5.如权利要求1所述的多机器人协调队形控制方法，其特征在于，所述C)步中的跟随机器人对领航机器人运动方向的预测，是根据当前采样时刻k与该时刻之前第Npmax次采样时刻的领航机器人位置信息得出的，且这两个采样时刻的领航机器人位置信息均建立在跟随机器人采样时刻k之前的第Npmax次采样时刻所对应的局部坐标系下。

6.如权利要求1所述的多机器人协调队形控制方法，其特征在于，所述D)步中的跟随机器人与领航机器人之间的距离 为dk。

7.如权利要求1、4或5所述的多机器人协调队形控制方法，其特征在于，所述Npmax≥1。

说明书

多机器人协调队形控制方法

技术领域

[0001] 本发明涉及机器人技术领域，特别是一种多机器人协调队形控制方法，用于多机器人系统。

背景技术

[0002] 多机器人队形控制，指的是多个机器人组成的团队在执行任务的过程中，在适应环境约束的条件下，相互之间保持预定的几何关系。多机器人队形在军事、航空航天等方面有广阔的应用前景，它的实现有利于缩短任务的执行时间，提高系统的整体性能。常用的队形控制方法有领航者跟随方法、基于行为的方法和虚拟结构法，很多研究工作致力于对这些方法进行研究和改进以提高系统运行稳定性和环境适应性。

[0003] 对于一个由N个机器人组成的系统来说，一种思路是将队形控制问题分解为N-1组两个机器人的队形控制问题进行实现，每组的两个机器人一个为跟随机器人、一个为领航机器人，它们致力于保持预先设定的几何形状。通过系统中机器人两两队形控制，整体上呈现出协调的形状。为了实现队形控制，跟随机器人需要获取领航机器人的相关信息，常用的获取信息的手段是通讯。考虑到通讯网络延迟、数据包丢失乃至通讯失效以及执行机构的误差累积等问题，设计一种无通讯、通过对其领航机器人的感知推理来实现协调队形控制的方法是有意义的。

发明内容

[0004] 本发明的目的是提供一种多机器人协调队形控制方法，是基于感知推理的方法，使得跟随机器人依靠感知推理实现与领航机器人的队形控制，达到令人满意的效果。

[0005] 为实现上述目的，本发明的技术解决方案是：

[0006] 一种多机器人协调队形控制方法，包括步骤：

[0007] A)跟随机器人依据领航机器人上的色标筒，结合视觉与码盘信息，获取当前采样k-s时刻k与之前的Npmax次采样时刻相对于领航机器人的距离和观测方位角d ， (s＝
0，…，Npmax)；

[0008] B)跟随机器人借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-Npmax时的领航机器人在跟随机器人局部坐标系 下的位置以及当前采样时刻k时的跟随机器人前向摄像机的光心Copt在
坐标系下的位置

[0009] C)基于位置Lk、 和 跟随机器人计算出预测的领航机器人运动方向和领航机器人到Copt方向之间的角度

[0010] D)结合角度 以及期望角度ψijd、跟随机器人与领航机器人之间的距离 (即kd)以及期望距离Lijd，综合视觉信息，进行运动决策，得到速度vf和转角θf，实现对跟随机器人速度大小和方向的控制。

[0011] 所述的多机器人协调队形控制方法，其领航机器人上的色标筒为中空的圆柱形筒，由至少两种颜色上下组合而成，放于机器人上，其中心与机器人的中心保持一致。

[0012] 所述的多机器人协调队形控制方法，其队形控制是在跟随机器人对领航机器人感知推理的基础上实现的，机器人之间没有通讯。

[0013] 所述的多机器人协调队形控制方法，其所述B)步中的跟随机器人局部坐标系是与采样时刻k-Npmax相对应，其原点 取前向摄像机光心Copt所在的位置， 的方向与采样时刻k-Npmax时的机器人运动方向保持一致。

[0014] 所述的多机器人协调队形控制方法，其所述C)步中的跟随机器人对领航机器人运动方向的预测，是根据当前采样时刻k与该时刻之前第Npmax次采样时刻的领航机器人位置信息得出的，且这两个采样时刻的领航机器人位置信息均建立在跟随机器人采样时刻k之前的第Npmax次采样时刻所对应的局部坐标系下。

[0015] 所述的多机器人协调队形控制方法，其所述D)步中的跟随机器人与领航机器人k之间的距离 为d。

[0016] 所述的多机器人协调队形控制方法，其所述Npmax≥1。

[0017] 本发明的多机器人协调队形控制方法，适合通讯失效的情况，为多机器人在军事等方面的应用提供技术支持。

附图说明

[0018] 图1是本发明一种多机器人协调队形控制方法的控制框图；

[0019] 图2是两机器人队形控制图；

[0020] 图3是本发明一种多机器人协调队形控制方法的递推预测图；

[0021] 图4是本发明一种多机器人协调队形控制方法的跟随机器人运动决策图；

[0022] 图5是用本发明方法的领航机器人和跟随机器人的运动轨迹。

具体实施方式

[0023] 本发明提供了一种多机器人协调队形控制方法。跟随机器人结合视觉与自身码盘信息，获取当前采样时刻k与之前的Npmax次采样时刻相对于领航机器人的距离和观测方位角信息。借助码盘信息，通过局部坐标系变换，递推得到采样时刻k与该时刻之前的第Npmax次采样时刻时的领航机器人在跟随机器人局部坐标系(与采样时刻k-Npmax相对应)下的位置，以及当前采样时刻k时的跟随机器人前向摄像机(它的光轴方向与机器人运动方向相一致)的光心在该局部坐标系下的位置。基于上述位置，跟随机器人计算出预测的领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的角度，进而结合领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的期望角度以及跟随机器人与领航机器人之间的距离和期望距离，综合视觉信息，进行运动决策，得到跟随机器人的速度和转角，实现对跟随机器人速度大小和方向的k-s控制。多机器人协调队形控制方法的控制框图如图1所示，其中，d ， (s＝0，…，Npmax)为当前采样时刻k与之前的Npmax次采样时刻相对于领航机器人的距离和观测方位角， 和 分别为采样时刻k和k-Npmax时的领航机器
人在 坐标系(跟随机器人与采样时刻k-Npmax相对应的局部坐
标系)下的位置， 为当前采样时刻k时的跟随机器人前向摄像机的光心在
坐标系下的位置， 和ψijd分别为预测的领航机器人运动方向和
k
领航机器人到跟随机器人前向摄像机的光心方向之间的角度与期望角度， (即d)和Lijd分别为跟随机器人与领航机器人之间的距离和期望距离，vf和θf分别为跟随机器人的速度和转角。

[0024] 图2为两机器人队形控制图，其中Ri和Rj分别表示跟随机器人和领航机器人，l是机器人两驱动轮中心之间的距离，XOY是世界坐标系，XfOfYf被选取作为Ri的局部坐标系，Of取前向摄像机Cf的光心Copt所在的位置，dc是Of与机器人中心之间的距离，Yf的方向与机器人运动方向保持一致。当Ri递推预测出领航机器人Rj的运动时，基于Lijd，ψijd，得到期望队形点Pfd，随着Ri奔向Pfd，Lij→Lijd，ψij→ψijd，最终建立一种协调队形关系，其中Lij，Lijd分别为Ri与Rj之间的距离和期望距离，ψij(ψij∈[0，2π))，ψijd分别为Rj运动方向与Rj到Ri前向摄像机的光心方向之间的角度和期望角度。

[0025] 1、领航机器人位置估计：

[0026] 领航机器人配备一个色标筒，它是一个中空的圆柱形筒，由至少两种颜色上下组合而成，色标筒放于领航机器人上，其中心与机器人中心保持一致。跟随机器人通过对色标筒的视觉识别完成对领航机器人的识别，结合视觉标定，进而获取估计的相对定位信息。令为采样时刻k相对于领航机器人的距离和观测方位角。

[0027] 定义(u，v)是图像坐标系上一点，(x，y，z)是其在以前向摄像机光心为原点的坐标系wcamera上的坐标，有：

[0028]

[0029] 其中M是摄像机的内参数矩阵，由摄像机标定得到。

[0030] 记(u1，v1)和(u2，v2)分别为色标筒顶端和底端的点，(x1，y1，z1)和(x2，y2，z2)是它们在wcamera上的坐标，u1＝u2，z1＝z2，y2-y1是色标筒的实际高度，则dk可估算如下：

[0031]

[0032] 式中αy由M得到，v2-v1是色标筒在图像中的像素高度差。

[0033] 记(uT，vT)是色标筒的中心，ud，θv分别为图像的宽度和视野的宽度，则 可估算如下：

[0034]

[0035] 受外部环境或视野等影响，跟随机器人利用前向摄像机Cf可能会看不见领航机器人，此时需要根据码盘和以前的视觉信息对领航机器人位置进行估计。若跟随机器人经过连续的Ncs次采样依然观测不到领航机器人，放弃估计；一旦跟随机器人通过其它摄像机看到领航机器人，它首要考虑的是通过旋转使得前向摄像机尽快发现领航机器人，此时，它放弃估计。令 表示前一个采样时刻领航机器人的位置信息，Dl，Dr分别表示上一周期左右轮行驶的距离，则有：

[0036]

[0037]

[0038] 其中，αk-1表示上一周期的机器人转角，Dk-1为上一周期机器人中心行驶的距离。

[0039] 当跟随机器人直行时， 估算如下：

[0040]

[0041]

[0042] 当跟随机器人圆弧运动时，有：

[0043]

[0044]

[0045] 2、预测领航机器人运动方向：

[0046] 为了预测领航机器人的运动方向，跟随机器人需要获取当前采样时刻k与之前的k-sNpmax(Npmax≥1)次采样时刻相对于领航机器人的距离和观测方位角d ， (s＝0，…，Npmax)以及有关码盘信息。 定义为跟随机器人与采样时刻k-Npmax相
对应的局部坐标系，记 (s＝0，…，Npmax)为采样时刻k，...，k-Npmax时的领航机器人在 坐标系下的位置， (s＝0，…，Npmax)为
采样时刻k，...，k-Npmax时的跟随机器人前向摄像机的光心Copt在
坐标系下的位置，如图3所示。

[0047] 跟随机器人在采样时刻k-Npmax的位姿 可以表示为：

[0048]

[0049] 依据码盘信息，得出跟随机器人在采样时刻k-s(s＝0，…，Npmax-1)的位姿[0050]

[0051] 其中，

[0052]

[0053]

[0054]

[0055] S(k-s-1)(k-s)表示跟随机器人直线运动(type＝1)时在采样时刻k-s-1和k-s之间(k-s-1)(k-s) (k-s-1)(k-s)行驶的距离，θ 和r 分别表示跟随机器人从采样时刻k-s-1到k-s做圆弧
运动(type＝0)时对应的圆弧的圆心角和半径。

[0056] 结合相对于领航机器人的距离和观测方位角，跟随机器人估计领航机器人在坐标系下的位置 (s＝0，…，Npmax)如下：

[0057]

[0058] 基于 与Lk，跟随机器人得到 坐标系下预测的领航k+1
机器人运动方向，其方向角α 为：

[0059]

[0060] 3、计算领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心Copt方向之间的角度

[0061] 跟随机器人前向摄像机的光心Copt在 坐标系下的位置(s＝0，…，Npmax)计算如下：

[0062]

[0063] 基于 和Lk，得到βk(见图3)：

[0064]

[0065] 于是，可得预测的领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心Copt方向之间的角度 如下：

[0066]

[0067] 考虑到传感数据的不精确性， 限制在一个与期望角度 相关的区间内，例如[0068] 4、运动决策：

[0069] 由 和 (即dk)，结合期望的 以及Lijd，得出跟随机器人的理想队形点 如图4所示。 是 和 之间的距离，γ1是 与 之间的夹角，则：

[0070]

[0071]

[0072] 记 为跟随机器人和领航机器人之间的相对距离与期望距离的偏差， 为领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的角度与期望角度的偏差。当efld≤dfl且eflψ≤ψfl，跟随机器人认为队形实现，停止运动；当efld≤dfl且eflψ>ψfl，跟随机器人沿着它到领航机器人方向的反方向倒退直到efld超过dfl为止；当efld>dfl且eflψ≤ψfl，跟随机器人在与领航机器人的连线上运动；
当efld>dfl且eflψ>ψfl，它的速度vf和转角θf如下计算：

[0073]

[0074]

[0075] 式中， ΔT是采样周期，vmax和θmax分别为最大速度和转角。

[0076] 当跟随机器人无法预测、估计领航机器人时，若跟随机器人的非前向摄像机看到领航机器人，它应旋转尽快使得前向摄像机发现领航机器人，如果没有发现领航机器人，它会旋转搜索，若跟随机器人前向摄像机看到领航机器人，它会在期望距离的限制下向领航机器人运动。

[0077] 实施例

[0078] 本发明多机器人协调队形控制方法应用于两个机器人的队形控制中，机器人采用中国科学院自动化研究所研制的AIM智能机器人。每个机器人都配有CCD摄像机和码盘，单个摄像机视野范围约60°。实验中跟随机器人采用与运动方向一致的前向摄像机Cf对领航机器人进行识别、位置估计；其它摄像机的作用在于识别出领航机器人后尽快使得Cf发现领航机器人。vmax＝0.2m/s，θmax＝20°，Ncs＝5，Npmax＝5，l＝0.27m，dc＝5cm，dfl＝0.2m，ψfl＝20°。跟随机器人与领航机器人之间的初始距离
1.54m，初始角度189°，队形要求两机器人之间期望距离为1.2m，期望角度是210°。采用本发明所提供的方法，能够满足需求，图5给出了领航者和跟随者(领航机器人和跟随机器人的简称)的运动轨迹，Sl和Sf分别表示领航者与跟随者的起始点，Gl和Gf分别表示领航者与跟随者的终止点。