CN101685309A - 多机器人协调队形控制方法 - Google Patents

Abstract

本发明一种多机器人协调队形控制方法，是一种跟随机器人通过递推预测，进而实现与领航机器人的队形控制。跟随机器人结合视觉、码盘信息，获取当前采样时刻k与之前的N_{p max}次采样时刻相对于领航机器人的距离和观测方位角d^k－s，φ^k－s(s＝0，…，N_{p max})；借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k－N_{p max}时的领航机器人在跟随机器人局部坐标系(见式1)下的位置L^K(x_L ^k，y_L ^k)、(见式2)，以及采样时刻k时的跟随机器人前向摄像机的光心C_opt在(见式3)下的位置O_f ^k(x_of ^k，y_of ^k)；计算出预测的领航机器人运动方向和领航机器人到C_opt方向之间的角度ψ_ij ^k，进而结合期望角度ψ_ijd以及跟随机器人与领航机器人之间的距离L_ij ^k(即d^k)和期望距离L_ijd，综合视觉信息，进行运动决策，得到速度v_f和转角θ_f。本发明适合通讯失效的情况，为多机器人在军事等方面的应用提供技术支持。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

B)跟随机器人借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-N_p max时的领航机器人在跟随机器人局部坐标系

下的位置L^k(x_L ^k，y_L ^k)、以及当前采样时刻k时的跟随机器人前向摄像机的光心C_opt在

坐标系下的位置O_f ^k(x_of ^k，y_of ^k)；

C)基于位置L^k、

和O_f ^k，跟随机器人计算出预测的领航机器人运动方向和领航机器人到C_opt方向之间的角度ψ_ij ^k；

D)结合角度ψ_ij ^k以及期望角度ψ_ijd、跟随机器人与领航机器人之间的距离L_ij ^k(即d^k)以及期望距离L_ijd，综合视觉信息，进行运动决策，得到速度v_f和转角θ_f，实现对跟随机器人速度大小和方向的控制。

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

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

所述的多机器人协调队形控制方法，其所述B)步中的跟随机器人局部坐标系

是与采样时刻k-N_pmax相对应，其原点

取前向摄像机光心C_opt所在的位置，

的方向与采样时刻k-N_pmax时的机器人运动方向保持一致。

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

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

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

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

附图说明

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

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

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

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

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

具体实施方式

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

为当前采样时刻k与之前的N_pmax次采样时刻相对于领航机器人的距离和观测方位角，L^k(x_L ^k，y_L ^k)和

分别为采样时刻k和k-N_pmax时的领航机器人在

坐标系(跟随机器人与采样时刻k-N_pmax相对应的局部坐标系)下的位置，O_f ^k(x_of ^k，y_of ^k)为当前采样时刻k时的跟随机器人前向摄像机的光心在

坐标系下的位置，ψ_ij ^k和ψ_ijd分别为预测的领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的角度与期望角度，L_ij ^k(即d^k)和L_ijd分别为跟随机器人与领航机器人之间的距离和期望距离，v_f和θ_f分别为跟随机器人的速度和转角。

图2为两机器人队形控制图，其中R_i和R_j分别表示跟随机器人和领航机器人，l是机器人两驱动轮中心之间的距离，XOY是世界坐标系，X_fO_fY_f被选取作为R_i的局部坐标系，O_f取前向摄像机C_f的光心C_opt所在的位置，d_c是O_f与机器人中心之间的距离，Y_f的方向与机器人运动方向保持一致。当R_i递推预测出领航机器人R_j的运动时，基于L_ijdψ_ijd，得到期望队形点P_fd，随着R_i奔向P_fd，L_ij→L_ijd，ψ_ij→ψ_ijd，最终建立一种协调队形关系，其中L_ij，L_ijd分别为R_i与R_j之间的距离和期望距离，ψ_ij(ψ_ij∈[0，2π))，ψ_ijd分别为R_j运动方向与R_j到R_i前向摄像机的光心方向之间的角度和期望角度。

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

领航机器人配备一个色标筒，它是一个中空的圆柱形筒，由至少两种颜色上下组合而成，色标筒放于领航机器人上，其中心与机器人中心保持一致。跟随机器人通过对色标筒的视觉识别完成对领航机器人的识别，结合视觉标定，进而获取估计的相对定位信息。令(d^k，

)为采样时刻k相对于领航机器人的距离和观测方位角。

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

$z\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=M\left[\begin{array}{c}x\\ y\\ z\end{array}\right]=\left[\begin{array}{ccc}{\mathrm{\α}}_x& 0& u_0\\ 0& {\mathrm{\α}}_y& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ y\\ z\end{array}\right]$

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

记(u₁，v₁)和(u₂，v₂)分别为色标筒顶端和底端的点，(x₁，y₁，z₁)和(x₂，y₂，z₂)是它们在w_camera上的坐标，u₁＝u₂，z₁＝z₂，y₂-y₁是色标筒的实际高度，则d^k可估算如下：

$d^k=z_1=z_2=\frac{{\mathrm{\α}}_y(y_2-y_1)}{v_2-v_1}$

式中α_y由M得到，v₂-v₁是色标筒在图像中的像素高度差。

记(u_T，v_T)是色标筒的中心，u_d，θ_v分别为图像的宽度和视野的宽度，则

可估算如下：

受外部环境或视野等影响，跟随机器人利用前向摄像机C_f可能会看不见领航机器人，此时需要根据码盘和以前的视觉信息对领航机器人位置进行估计。若跟随机器人经过连续的N_cs次采样依然观测不到领航机器人，放弃估计；一旦跟随机器人通过其它摄像机看到领航机器人，它首要考虑的是通过旋转使得前向摄像机尽快发现领航机器人，此时，它放弃估计。

令(d^k-1，

)表示前一个采样时刻领航机器人的位置信息，D_l，D_r分别表示上一周期左右轮行驶的距离，则有：

${\mathrm{\α}}_{k-1}=\frac{D_r-D_l}{l}$

$D_{k-1}=\frac{D_l+D_r}{2}$

其中，α_k-1表示上一周期的机器人转角，D_k-1为上一周期机器人中心行驶的距离。

当跟随机器人直行时，(d^k，

)估算如下：

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

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

为了预测领航机器人的运动方向，跟随机器人需要获取当前采样时刻k与之前的N_pmax(N_pmax≥1)次采样时刻相对于领航机器人的距离和观测方位角d^k-s，

(s＝0，…，N_pmax)以及有关码盘信息。

定义为跟随机器人与采样时刻k-N_pmax相对应的局部坐标系，记L^k-s(x_L ^k-s，y_L ^k-s)(s＝0，…，N_pmax)为采样时刻k，...，k-N_pmax时的领航机器人在

坐标系下的位置，O_f ^k-s(x_of ^k-s，y_of ^k-s)(s＝0，…，N_pmax)为采样时刻k，...，k-N_pmax时的跟随机器人前向摄像机的光心C_opt在

坐标系下的位置，如图3所示。

跟随机器人在采样时刻k-N_pmax的位姿

可以表示为：

$T_{O_f}^{k-N_{p\max }}=\left[\begin{array}{ccc}{T}_{11}^{k-N_{p\max }}& T_{12}^{k-N_{p\max }}& T_{13}^{k-N_{p\max }}\\ T_{21}^{k-N_{p\max }}& T_{22}^{k-N_{p\max }}& T_{23}^{k-N_{p\max }}\\ T_{31}^{k-N_{p\max }}& T_{32}^{k-N_{p\max }}& T_{33}^{k-N_{p\max }}\end{array}\right]=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& 0\\ 0& 0& 1\end{array}\right]$

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

$T_{O_f}^{k-s}=\left[\begin{array}{ccc}{T}_{11}^{k-s}& T_{12}^{k-s}& T_{13}^{k-s}\\ T_{21}^{k-s}& T_{22}^{k-s}& T_{23}^{k-s}\\ T_{31}^{k-s}& T_{32}^{k-s}& T_{33}^{k-s}\end{array}\right]=\left\{\begin{array}{cc}{T}_{O_f}^{k-s-1}{T}_{\mathrm{line}}^{(k-s-1)(k-s)}& \mathrm{whentype}=1\\ T_{O_f}^{k-s-1}{T}_{\mathrm{arc}}^{(k-s-1)(k-s)}& \mathrm{whentype}=0\end{array}\right.$

其中，

$T_{\mathrm{line}}^{(k-s-1)(k-s)}=\left[\begin{array}{ccc}1& 0& 0\\ 0& 1& S^{(k-s-1)(k-s)}\\ 0& 0& 1\end{array}\right],$

$T_{\mathrm{arc}}^{(k-s-1)(k-s)}=\left[\begin{array}{cc}\cos \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)& -\sin \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)\\ \sin \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)& \cos \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)\\ 0& 0\end{array}\right.$

${\left.\begin{array}{c}(\cos \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)-1)(r^{(k-s-1)(k-s)}+l/2)-d_c\sin \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)\\ \sin \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)(r^{(k-s-1)(k-s)}+l/2)+d_c(\cos \left({\mathrm{\θ}}^{(k-s-1)(k-s)}\right)-1)\\ 1\end{array}\right]}_{3\×3}$

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

结合相对于领航机器人的距离和观测方位角，跟随机器人估计领航机器人在

坐标系下的位置L^k-s(x_L ^k-s，y_L ^k-s)(s＝0，…，N_pmax)如下：

基于

与L^k，跟随机器人得到

坐标系下预测的领航机器人运动方向，其方向角α^k+1为：

$\tan {\mathrm{\α}}^{k+1}=\frac{y_L^k-y_L^{k-N_{p\max }}}{x_L^k-x_L^{k-N_{p\max }}}(x_L^k\≠x_L^{k-N_{p\max }})\left|\right|{\mathrm{\α}}^{k+1}=\frac{\mathrm{\π}}{2}(x_L^k=x_L^{k-N_{p\max }})$

3、计算领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心C_opt方向之间的角度ψ_ij ^k：

跟随机器人前向摄像机的光心C_opt在

坐标系下的位置O_f ^k-s(x_of ^k-s，y_of ^k-s)(s＝0，…，N_pmax)计算如下：

$\left[\begin{array}{c}{x}_{\mathrm{of}}^{k-s}\\ y_{\mathrm{of}}^{k-s}\\ 1\end{array}\right]=T_{O_f}^{k-s}\left[\begin{array}{c}0\\ 0\\ 1\end{array}\right]$

基于O_f ^k和L^k，得到β^k(见图3)：

$\tan {\mathrm{\β}}^k=\frac{y_L^k-y_{\mathrm{of}}^k}{x_L^k-x_{\mathrm{of}}^k}(x_L^k\≠x_{\mathrm{of}}^k)\left|\right|{\mathrm{\β}}^k=\frac{\mathrm{\π}}{2}(x_L^k=x_{\mathrm{of}}^k)$

于是，可得预测的领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心C_opt方向之间的角度ψ_ij ^k如下：

${\mathrm{\ψ}}_{\mathrm{ij}}^k=\mathrm{\π}-{\mathrm{\α}}^{k+1}+{\mathrm{\β}}^k$

考虑到传感数据的不精确性，ψ_ij ^k限制在一个与期望角度ψ_ij ^d相关的区间内，例如[ψ_ij ^d-ψ_ij ^d-，ψ_ij ^d+ψ_ij ^d-u]。

4、运动决策：

由ψ_ij ^k和L_ij ^k(即d^k)，结合期望的ψ_ij ^d以及L_ij ^d，得出跟随机器人的理想队形点P_fd ^k，如图4所示。d_P ^k是O_f ^k和P_fd ^k之间的距离，γ₁是O_f ^kL^k与O_f ^kP_fd ^k之间的夹角，则：

$d_P^k=\sqrt{{\left(L_{\mathrm{ij}}^k\right)}^2+{\left(L_{\mathrm{ijd}}\right)}^2-2L_{\mathrm{ij}}^k{L}_{\mathrm{ijd}}\cos ({\mathrm{\ψ}}_{\mathrm{ijd}}-{\mathrm{\ψ}}_{\mathrm{ij}}^k)}$

${\mathrm{\γ}}_1=\arccos \frac{{\left(L_{\mathrm{ij}}^k\right)}^2+{\left(d_P^k\right)}^2-{\left(L_{\mathrm{ijd}}\right)}^2}{2L_{\mathrm{ij}}^k{d}_P^k}$

记

为跟随机器人和领航机器人之间的相对距离与期望距离的偏差，为领航机器人运动方向和领航机器人到跟随机器人前向摄像机的光心方向之间的角度与期望角度的偏差。当e_fld≤d_fl且e_flψ≤ψ_fl，跟随机器人认为队形实现，停止运动；当e_fld≤d_fl且e_flψ＞ψ_fl，跟随机器人沿着它到领航机器人方向的反方向倒退直到e_fld超过d_fl为止；当e_fld＞d_fl且e_flψ≤ψ_fl，跟随机器人在与领航机器人的连线上运动；当e_fld＞d_fl且e_flψ＞ψ_fl，它的速度v_f和转角θ_f如下计算：

${\mathrm{\&theta;}}_f=\left\{\begin{array}{cc}{\mathrm{\&theta;}}_{\max },& {\mathrm{\&theta;}}_f^{\&prime;}\&GreaterEqual;{\mathrm{\&theta;}}_{\max }\\ {\mathrm{\&theta;}}_f^{\&prime;},& -{\mathrm{\&theta;}}_{\max }<{\mathrm{\&theta;}}_f^{\&prime;}<{\mathrm{\&theta;}}_{\max }\\ -{\mathrm{\&theta;}}_{\max },& {\mathrm{\&theta;}}_f^{\&prime;}\&le;-{\mathrm{\&theta;}}_{\max }\end{array}\right.$

式中，

ΔT是采样周期，v_max和θ_max分别为最大速度和转角。

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

实施例

本发明多机器人协调队形控制方法应用于两个机器人的队形控制中，机器人采用中国科学院自动化研究所研制的AIM智能机器人。每个机器人都配有CCD摄像机和码盘，单个摄像机视野范围约60°。实验中跟随机器人采用与运动方向一致的前向摄像机C_f对领航机器人进行识别、位置估计；其它摄像机的作用在于识别出领航机器人后尽快使得C_f发现领航机器人。v_max＝0.2m/s，θ_max＝20°，N_cs＝5，N_pmax＝5，

l＝0.27m，d_c＝5cm，d_fl＝0.2m，ψ_fl＝20°。跟随机器人与领航机器人之间的初始距离1.54m，初始角度189°，队形要求两机器人之间期望距离为1.2m，期望角度是210°。采用本发明所提供的方法，能够满足需求，图5给出了领航者和跟随者(领航机器人和跟随机器人的简称)的运动轨迹，S_l和S_f分别表示领航者与跟随者的起始点，G_l和G_f分别表示领航者与跟随者的终止点。

Claims (7)

1.一种多机器人协调队形控制方法，用于多机器人***；其特征在于，包括步骤：

A)跟随机器人依据领航机器人上的色标筒，结合视觉与码盘信息，获取

当前采样时刻k与之前的N_pmax次采样时刻相对于领航机器人的距离和观测方位角d^k-s，(s＝0，…，N_pmax)；

B)跟随机器人借助码盘信息，通过局部坐标系变换，递推得到采样时刻k和k-N_pmax时的领航机器人在跟随机器人局部坐标系

下的位置L^k(x_L ^k，y_L ^k)、

以及当前采样时刻k时的跟随机器人前向摄像机的光心C_opt在

坐标系下的位置O_f ^k(x_of ^k，y_of ^k)；

C)基于位置L^k、

和O_f ^k，跟随机器人计算出预测的领航机器人运动方向和领航机器人到C_opt方向之间的角度ψ_ij ^k；

D)结合角度ψ_ij ^k以及期望角度ψ_ijd、跟随机器人与领航机器人之间的距离L_ij ^k以及期望距离L_ijd，综合视觉信息，进行运动决策，得到速度v_f和转角θ_f，实现对跟随机器人速度大小和方向的控制。

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

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

4.如权利要求1所述的多机器人协调队形控制方法，其特征在于，所述B)步中的跟随机器人局部坐标系是与采样时刻k-N_pmax相对应，其原点

取前向摄像机光心C_opt所在的位置，

的方向与采样时刻k-N_pmax时的机器人运动方向保持一致。

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

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

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

Images