CN111552293A - 一种视野约束下基于图像的移动机器人编队控制方法 - Google Patents

Abstract

本发明公开了一种视野约束下基于图像的移动机器人编队控制方法，包括以下步骤：S1、建立领航者‑跟随者系统框架，并利用领航者‑跟随者系统框架对领航者和跟随者之间的相对运动学建模，得到在归一化图像空间中领航者和跟随者的相对视觉运动学；S2、定义基于归一化图像空间的具有预定性能规范以及视野约束的误差变换；S3、设计一种参数自适应估计律用于在线估计跟随者机载单目相机光学中心与领航者身上单个特征点之间的相对高度倒数；S4、解算出跟随者所需要的角速度和线速度，进而完成跟随者对领航者的跟踪。通过参数自适应估计律和具有预定性能规范以及视野约束的性能函数定义误差变换，具有计算简便、视场约束有效和瞬态、稳态性能良好的优点。

Description

一种视野约束下基于图像的移动机器人编队控制方法

技术领域

本发明属于移动机器人编队控制技术领域，具体涉及一种视野约束下基于图像的移动机器人编队控制方法。

背景技术

多移动机器人编队控制在处理复杂任务时，常常表现出高效率，强鲁棒性与安全性等特点，被广泛应用于工业生产、地形探测和灾难应对等领域。多移动机器人编队控制策略主要分为四类：领航者-跟随者方法，基于行为的方法，虚拟结构方法和滚动时域控制(或模型预测控制)方法。其中，领航者-跟随者方法因为其实现的简单性和应用的可伸缩性，被广为关注。但是，在领航者-跟随者编队控制中的大多数先前工作都假定每个机器人的全局位置信息是已知的，并且机器人之间的通信可用。在GPS受限和通信退化的环境中，编队控制问题变得更具挑战，期望扩大应用范围并改善机器人编队的自主性。与其他传统传感器相比，视觉相机可以以较低的成本提供更丰富的信息，从而使视觉相机在仅使用机载感应器的编队控制中被作为一个非常受欢迎的感应器选择。

各种基于视觉的领航者-跟随者编队控制方法可以归纳为两种类型：基于位置的视觉伺服(PBVS)和基于图像的视觉伺服(IBVS)。在PBVS方法中，利用车载视觉信息重建领航者与追随者之间的相对姿态。与PBVS方法相比，IBVS方法通过直接控制当前图像坐标和期望图像坐标的误差来实现对领航者期望姿势的跟踪。因此，IBVS方法更适用于移动机器人的编队控制，因为它对相机校准和对象模型错误不太敏感。但是基于视觉的控制方法存在机载摄像机固有的有限视野的问题，这对系统的控制施加了额外的视野约束。

发明内容

本发明的目的在于提供一种视野约束下基于图像的移动机器人编队控制方法，其不需要估计领航者与跟随者之间的相对角度、位置以及领航者的角速度和线速度，并且所提出的参数自适应估计律的计算复杂度非常低，计算简便。

本发明的目的通过以下技术方案实现：提供一种视野约束下基于图像的移动机器人编队控制方法，包括以下步骤：

S1、建立领航者-跟随者系统框架，并利用领航者-跟随者系统框架对领航者和跟随者之间的相对运动学建模，得到在归一化图像空间中领航者和跟随者的相对视觉运动学；

S2、定义基于归一化图像空间的具有预定性能规范以及视野约束的误差变换；

S3、设计一种参数自适应估计律用于在线估计跟随者机载单目相机光学中心与领航者身上单个特征点之间的相对高度倒数；

S4、解算出跟随者所需要的角速度和线速度，进而完成跟随者对领航者的跟踪。

作为进一步的改进，所述步骤S1具体包括以下内容：

S11、建立领航者-跟随者框架，将领航者相对于跟随者框架的位置定义为：

式中，r_lf＝[x_lf,y_lf]^T为领航者相对于跟随者的位置，x_lf和y_lf分别表示相对位置的横坐标和纵坐标，T表示矩阵转置，在世界坐标系内r_l为领航者的位置，r_f为跟随者的位置，θ_f为跟随者的方位；

S12、对公式(1)求一阶微分得到领航者相对于跟随者的速度为：

式中，

为x_lf的一阶微分，

为y_lf的一阶微分，ω_f为跟随者的角速度，v_l为领航者的线速度，v_f为跟随者的线速度，ω_l为领航者的角速度，θ_lf表示领航者和跟随者之间的相对角度，

为θ_lf的一阶微分，其中：

θ_lf＝θ_l-θ_f (3)

式中，θ_l表示领航者的方位；

S13、领航者身上单个特征点在跟随者机载单目相机框架中的归一化坐标定义为：

式中，P＝[X,Y,Z]^T为领航者身上单个特征点相对于跟随者机载单目相机框架中的三维位置坐标，T表示矩阵转置，p和q分别表示特征点在归一化图像空间的横坐标和纵坐标；

S14、归一化图像空间中领航者和跟随者的相对视觉运动学为：

式中，

为p的一阶微分，

为q的一阶微分，h为跟随者机载单目相机的光学中心与领航者身上单个特征点之间的相对高度，且

为s＝[p,q]^T的一阶微分,s＝[p,q]^T为归一化图像坐标。

作为进一步的改进，所述步骤S2中误差变换ε的计算过程如下：

S21、输入领航者的特征点在跟随者机载单目相机框架中的期望恒定视觉特征向量s^*＝[p^*,q^*]^T，其中，p^*和q^*分别表示特征点在归一化图像空间期望的横坐标和纵坐标；

S22、利用霍夫圆检测技术实时获取领航者身上单个特征点在跟随者机载单目相机框架中的图像平面坐标[m,n]^T，则归一化图像坐标s＝[p,q]^T和图像平面坐标[m,n]^T的关系如下所示：

其中，a_m、a_n分别为图像在水平方向和垂直方向上的尺度因子，且a_m>0，a_n>0，[m₀,n₀]^T是图像平面的主点坐标，m₀、n₀分别为图像平面主点的横坐标和纵坐标，m、n分别为特征点在图像空间的横坐标和纵坐标；

S23、令图像误差向量为e＝[e₁,e₂]^T，实时测量跟随者机载单目相机框架中的归一化图像坐标s＝[p,q]^T与期望恒定视觉特征向量s^*＝[p^*,q^*]^T的误差，其中：

e＝[e₁,e₂]^T＝[p-p^*,q-q^*]^T (7)

S24、用性能函数ρ_k定义图像误差向量e中的图像误差e_k的边界：

其中，C_k和

为正参数，k＝{1,2}，且性能函数ρ_k由下式定义：

式中，ρ_∞＞0表示最终误差界参数，e^-lt为关于参数l＞0的指数衰减函数；

S25：定义具有预定性能规范以及视野约束的误差变换ε_k：

其中，ξ_k＝e_k/ρ_k，k＝{1,2}。

作为进一步的改进，所述步骤S3的具体内容为：

S31、对公式(7)中图像误差向量e求一阶微分式：

其中，λ＝1/h，G表示矩阵，f表示向量，且G和f的表达式分别为：

S32、误差变换向量ε＝[ε₁,ε₂]^T的一阶微分式为：

其中，J表示，δ表示，且

和

被定义为：

式(15)和(16)中

为ρ_k的一阶微分；

S33、对于h的估计，间接用

估计λ＝1/h，参数自适应估计律为：

其中，

为

的一阶微分，ε^T为ε的转置矩阵，sgn(h)为关于参数h的符号函数，|h|的上下限定义为

上限

下限h均为正参数，g＝[pq,q²]^T，γ为正参数。

作为进一步的改进，所述步骤S4具体包括以下步骤：

S41、为了使跟随者跟踪领航者，定义

如下：

式中，

表示v_f的平均值，

表示ω_f的平均值，T表示矩阵转置，T表示矩阵转置，K₁、K₂分别为正定矩阵；

S42、为了使跟随者跟踪领航者，令跟随者的角速度和线速度输入如下：

S43、通过公式(19)解算出跟随者所需要的角速度和线速度。

与现有技术相比，本发明具有以下有益效果：

1)所设计的解决方案只需要最小的感测功能，因为它仅依赖与跟随者身上的一台单目摄像机和与领航者相连的一个特征点。

2)设计的编队控制方法不需要估计领航者与跟随者之间的相对角度、位置以及领航者的速度，计算简便。

3)直接在图像空间中处理视野约束，选择具有预定性能规范以及视野约束的性能函数定义误差变换，所提出的策略可确保图像误差在指定的瞬态和稳态性能指标下收敛，并且不会违反视野约束。

附图说明

利用附图对本发明作进一步说明，但附图中的实施例不构成对本发明的任何限制，对于本领域的普通技术人员，在不付出创造性劳动的前提下，还可以根据以下附图获得其它的附图。

图1为本发明实施例提供的一种视野约束下基于图像的移动机器人编队控制方法的流程图。

图2为本发明实施例提供的一种视野约束下基于图像的移动机器人编队控制方法的原理图。

图3为本发明实施例的领航者-跟随者系统框架示意图。

图4为本发明实施例的领航者和跟随者在仿真中的关系拓扑示意图。

图5为本发明实施例的领航者和跟随者在仿真中的编队过程轨迹示意图。

图6为本发明实施例的领航者和跟随者在仿真中的编队误差收敛示意图。

图7为本发明实施例的领航者和跟随者在仿真中的相对高度倒数估计示意图。

图8为本发明实施例的机器人R₂在仿真中的归一化图像误差

收敛过程示意图。

图9为本发明实施例的机器人R₂在仿真中的归一化图像误差

收敛过程示意图。

图10为本发明实施例的机器人R₃在仿真中的归一化图像误差

收敛过程示意图。

图11为本发明实施例的机器人R₃在仿真中的归一化图像误差

收敛过程示意图。

图12为本发明实施例的机器人R₄在仿真中的归一化图像误差

收敛过程示意图。

图13为本发明实施例的机器人R₄在仿真中的归一化图像误差

收敛过程示意图。

图14为本发明实施例的机器人R₅在仿真中的归一化图像误差

收敛过程示意图。

图15为本发明实施例的机器人R₅在仿真中的归一化图像误差

收敛过程示意图。

具体实施方式

为了使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施例对本发明作进一步详细的描述，需要说明的是，在不冲突的情况下，本申请的实施例及实施例中的特征可以相互组合。

图1为本发明实施例提供的一种视野约束下基于图像的移动机器人编队控制方法的流程图。图2为本发明实施例提供的一种视野约束下基于图像的移动机器人编队控制方法的原理图。参见图1和图2，本发明提供了一种视野约束下基于图像的移动机器人编队控制方法，包括以下步骤：

S1、建立领航者-跟随者系统框架，并利用领航者-跟随者系统框架对领航者和跟随者之间的相对运动学建模，得到在归一化图像空间中领航者和跟随者的相对视觉运动学，图3即为本发明实施例的领航者-跟随者系统框架示意图；

优选地，该步骤S1具体包括以下内容：

S11、建立领航者-跟随者框架，将领航者相对于跟随者框架的位置定义为：

式中，r_lf＝[x_lf,y_lf]^T为领航者相对于跟随者的位置，x_lf和y_lf分别表示相对位置的横坐标和纵坐标，T表示矩阵转置，在世界坐标系内r_l为领航者的位置，r_f为跟随者的位置，θ_f为跟随者的方位；

S12、对公式(1)求一阶微分得到领航者相对于跟随者的速度为：

式中，

为x_lf的一阶微分，

为y_lf的一阶微分，ω_f为跟随者的角速度，v_l为领航者的线速度，v_f为跟随者的线速度，ω_l为领航者的角速度，θ_lf表示领航者和跟随者之间的相对角度，

为θ_lf的一阶微分，其中：

θ_lf＝θ_l-θ_f (3)

式中，θ_l表示领航者的方位；

S13、领航者身上单个特征点在跟随者机载单目相机框架中的归一化坐标定义为：

式中，P＝[X,Y,Z]^T为领航者身上单个特征点相对于跟随者机载单目相机框架中的三维位置坐标，T表示矩阵转置，p和q分别表示特征点在归一化图像空间的横坐标和纵坐标；

S14、归一化图像空间中领航者和跟随者的相对视觉运动学为：

式中，

为p的一阶微分，

为q的一阶微分，h为跟随者机载单目相机的光学中心与领航者身上单个特征点之间的相对高度，且

为s＝[p,q]^T的一阶微分,s＝[p,q]^T为归一化图像坐标；

S2、定义基于归一化图像空间的具有预定性能规范以及视野约束的误差变换ε；需要说明的是误差变换ε的具体计算过程如下：

S21、输入领航者的特征点在跟随者机载单目相机框架中的期望恒定视觉特征向量s^*＝[p^*,q^*]^T，其中，p^*和q^*分别表示特征点在归一化图像空间期望的横坐标和纵坐标；

S22、利用霍夫圆检测技术实时获取领航者身上单个特征点在跟随者机载单目相机框架中的图像平面坐标[m,n]^T，则归一化图像坐标s＝[p,q]^T和图像平面坐标[m,n]^T的关系如下所示：

其中，a_m、a_n分别为图像在水平方向和垂直方向上的尺度因子，且a_m>0，a_n>0，[m₀,n₀]^T是图像平面的主点坐标，m₀、n₀分别为图像平面主点的横坐标和纵坐标，m、n分别为特征点在图像空间的横坐标和纵坐标；

S23、令图像误差向量为e＝[e₁,e₂]^T，实时测量跟随者机载单目相机框架中的归一化图像坐标s＝[p,q]^T与期望恒定视觉特征向量s^*＝[p^*,q^*]^T的误差，其中：

e＝[e₁,e₂]^T＝[p-p^*,q-q^*]^T (7)

S24、用性能函数ρ_k定义图像误差向量e中的图像误差e_k的边界：

其中，C _k和

为正参数，k＝{1,2}，且性能函数ρ_k由下式定义：

式中，ρ_∞＞0表示最终误差界参数，e^-lt为关于参数l＞0的指数衰减函数；

S25：定义具有预定性能规范以及视野约束的误差变换ε_k：

其中，ξ_k＝e_k/ρ_k，k＝{1,2}。

S3、设计一种参数自适应估计律用于在线估计跟随者机载单目相机光学中心与领航者身上单个特征点之间的相对高度倒数；

优选地，步骤S3具体表现为：

S31、对公式(7)中图像误差向量e求一阶微分式：

其中，λ＝1/h，G表示矩阵，f表示向量，且G和f的表达式分别为：

S32、误差变换向量ε＝[ε₁,ε₂]^T的一阶微分式为：

其中，

和

被定义为：

式(15)和(16)中

为ρ_k的一阶微分；

S33、对于h的估计，间接用

估计λ＝1/h，参数自适应估计律为：

其中，

为

的一阶微分，ε^T为ε的转置矩阵，sgn(h)为关于参数h的符号函数，|h|的上下限定义为

上限

下限h均为正参数，g＝[pq,q²]^T，γ为正参数。

S4、解算出跟随者所需要的角速度和线速度，进而完成跟随者对领航者的跟踪。需要说明的是，上述步骤所述步骤S4具体包括以下步骤：

S41、为了使跟随者跟踪领航者，定义

如下：

式中，

表示v_f的平均值，

表示ω_f的平均值，T表示矩阵转置，T表示矩阵转置，K₁、K₂分别为正定矩阵；

S42、为了使跟随者跟踪领航者，令跟随者的角速度和线速度输入如下：

S43、通过公式(19)解算出跟随者所需要的角速度和线速度。

参见图4，本发明实施例中移动机器人编队有五个非完整的移动机器人R₁、R₂、R₃、R₄和R₅，设定跟随者R_i(i∈{2,3,4,5})上的摄像头和领航者R_j(j∈{1,2,3})上的特征点之间的相对高度分别为是h₁₂＝h₁₃＝-0.27，h₂₄＝h₃₅＝0.27，单目相机的固有参数为：m₀＝320像素，n₀＝240像素，a_m＝a_n＝616像素，R₁-R₂、R₂-R₄、R₁-R₃、R₃-R₅的所需相对位置矢量分别设置为r_d12＝r_d24＝[0.8,-0.2]^T，r_d13＝r_d35＝[0.8,0.2]^T，每个跟随机器人的期望标准化坐标为

领航者R₁的线速度和角速度为v₁＝1和ω₁＝0，跟随者控制器增益设置为K_1i＝diag(0.05,0.08)和K_2i＝diag(0.005,0.002)，并给出观察者增益由γ_i＝0.5，其中(i＝2,…,5)；在性能函数中，最大允许稳态误差设置为ρ_∞＝0.0081(5个像素)，收敛速度设置为l＝2，通过上述进行仿真实验。

图5显示了整个编队形成的轨迹，其中每2秒提供一次当前机器人的位置，从图中看出所需的V编队在大约2秒时实现。在图6中，编队误差被定义为

其中r_ij和

分别是领航者R_i和跟随者R_j之间的实际和期望相对位置，可以看出，所有跟随者的编队误差迅速接近零。在图7中，尽管使用了非常粗糙的初始值，但仍可以确保所有估计值都保持有界。在图8-图15中

表示第i个跟随者R_i的归一化图像特征误差，从图中可以看出所有的归一化图像特征误差不仅收敛迅速，而且保留在预定的性能范围内，因此始终满足摄像机视野的约束。通过前述方法，归一化特征误差达到了约0.005(3像素)的满意的稳态性能，具有视场约束的有效性和良好的瞬态、稳态性能。

上面的描述中阐述了很多具体细节以便于充分理解本发明，但是，本发明还可以采用其他不同于在此描述的其他方式来实施，因此，不能理解为对本发明保护范围的限制。

总之，本发明虽然列举了上述优选实施方式，但是应该说明，虽然本领域的技术人员可以进行各种变化和改型，除非这样的变化和改型偏离了本发明的范围，否则都应该包括在本发明的保护范围内。

Claims (5)

1.一种视野约束下基于图像的移动机器人编队控制方法，其特征在于，包括以下步骤：

S1、建立领航者-跟随者系统框架，并利用领航者-跟随者系统框架对领航者和跟随者之间的相对运动学建模，得到在归一化图像空间中领航者和跟随者的相对视觉运动学；

S2、定义基于归一化图像空间的具有预定性能规范以及视野约束的误差变换；

S3、设计一种参数自适应估计律用于在线估计跟随者机载单目相机光学中心与领航者身上单个特征点之间的相对高度倒数；

S4、解算出跟随者所需要的角速度和线速度，进而完成跟随者对领航者的跟踪。

2.根据权利要求1所述的视野约束下基于图像的移动机器人编队控制方法，其特征在于，所述步骤S1具体包括以下内容：

S11、建立领航者-跟随者框架，将领航者相对于跟随者框架的位置定义为：

式中，r_lf＝[x_lf,y_lf]^T为领航者相对于跟随者的位置，x_lf和y_lf分别表示相对位置的横坐标和纵坐标，T表示矩阵转置，在世界坐标系内r_l为领航者的位置，r_f为跟随者的位置，θ_f为跟随者的方位；

S12、对公式(1)求一阶微分得到领航者相对于跟随者的速度为：

式中，

为x_lf的一阶微分，

为y_lf的一阶微分，ω_f为跟随者的角速度，v_l为领航者的线速度，v_f为跟随者的线速度，ω_l为领航者的角速度，θ_lf表示领航者和跟随者之间的相对角度，

为θ_lf的一阶微分，其中：

θ_lf＝θ_l-θ_f (3)

式中，θ_l表示领航者的方位；

S13、领航者身上单个特征点在跟随者机载单目相机框架中的归一化坐标定义为：

式中，P＝[X,Y,Z]^T为领航者身上单个特征点相对于跟随者机载单目相机框架中的三维位置坐标，T表示矩阵转置，p和q分别表示特征点在归一化图像空间的横坐标和纵坐标；

S14、归一化图像空间中领航者和跟随者的相对视觉运动学为：

式中，

为p的一阶微分，

为q的一阶微分，h为跟随者机载单目相机的光学中心与领航者身上单个特征点之间的相对高度，且

为s＝[p,q]^T的一阶微分,s＝[p,q]^T为归一化图像坐标。

3.根据权利要求2所述的视野约束下基于图像的移动机器人编队控制方法，其特征在于，所述步骤S2中误差变换的计算过程如下：

S21、输入领航者的特征点在跟随者机载单目相机框架中的期望恒定视觉特征向量s^*＝[p^*,q^*]^T，其中，p^*和q^*分别表示特征点在归一化图像空间期望的横坐标和纵坐标；

S22、利用霍夫圆检测技术实时获取领航者身上单个特征点在跟随者机载单目相机框架中的图像平面坐标[m,n]^T，则归一化图像坐标s＝[p,q]^T和图像平面坐标[m,n]^T的关系如下所示：

其中，a_m、a_n分别为图像在水平方向和垂直方向上的尺度因子，且a_m>0，a_n>0，[m₀,n₀]^T是图像平面的主点坐标，m₀、n₀分别为图像平面主点的横坐标和纵坐标，m、n分别为特征点在图像空间的横坐标和纵坐标；

S23、令图像误差向量为e＝[e₁,e₂]^T，实时测量跟随者机载单目相机框架中的归一化图像坐标s＝[p,q]^T与期望恒定视觉特征向量s^*＝[p^*,q^*]^T的误差，其中：

e＝[e₁,e₂]^T＝[p-p^*,q-q^*]^T (7)

S24、用性能函数ρ_k定义图像误差向量e中的图像误差e_k的边界：

其中，C_k和

为正参数，k＝{1,2}，且性能函数ρ_k由下式定义：

式中，ρ_∞＞0表示最终误差界参数，e^-lt为关于参数l＞0的指数衰减函数；

S25：定义具有预定性能规范以及视野约束的误差变换ε_k：

其中，ξ_k＝e_k/ρ_k，k＝{1,2}。

4.根据权利要求3所述的视野约束下基于图像的移动机器人编队控制方法，其特征在于，所述步骤S3的具体内容为：

S31、对公式(7)中图像误差向量e求一阶微分式：

其中，λ＝1/h，G表示矩阵，f表示向量，且G和f的表达式分别为：

S32、误差变换向量ε＝[ε₁,ε₂]^T的一阶微分式为：

其中

和

被定义为：

式(15)和(16)中

为ρ_k的一阶微分；

S33、对于h的估计，间接用

估计λ＝1/h，参数自适应估计律为：

其中，

为

的一阶微分，ε^T为ε的转置矩阵，sgn(h)为关于参数h的符号函数，|h|的上下限定义为

上限

下限h均为正参数，g＝[pq,q²]^T，γ为正参数。

5.根据权利要求4所述的视野约束下基于图像的移动机器人编队控制方法，其特征在于，所述步骤S4具体包括以下步骤：

S41、为了使跟随者跟踪领航者，定义

如下：

式中，

表示v_f的平均值，

表示ω_f的平均值，T表示矩阵转置，K₁、K₂分别为正定矩阵；

S42、为了使跟随者跟踪领航者，令跟随者的角速度和线速度输入如下：

S43、通过公式(19)解算出跟随者所需要的角速度和线速度。

Images