CN112099505A - 一种低复杂度的移动机器人视觉伺服编队控制方法 - Google Patents

Abstract

本发明公开了一种低复杂度的移动机器人视觉伺服编队控制方法，该方法首先根据领航者和跟随者之间的相对运动学模型得到图像空间中的相对视觉运动学模型，之后对相对视觉运动学模型进行视觉特征向量转换，得到解耦视觉运动学模型，接着对图像误差向量进行包含预定性能规范误差变换，最后设计出跟随者的角速度和线速度，进而得到基于跟随者角速度和线速度的控制律，实现跟随者对领航者的跟踪控制。本发明设计的控制器的计算量小，不需要机器人之间的相对角度、距离、领航者的速度以及特征点的深度信息。所有需要的信息都从图像上获取，从而减少对额外传感器的依赖，大大提高了系统的自主性。

Description

一种低复杂度的移动机器人视觉伺服编队控制方法

技术领域

本发明涉及移动机器人编队控制领域，特别是一种低复杂度的移动机器人视觉伺服编队控制方法。

背景技术

多移动机器人编队技术为多个机器人在到达目的地的过程中，保持某种队型，同时又要适应环境约束的一种控制技术。多移动机器人协调合作能提高完成任务的效率，增强系统的容错性、鲁棒性，完成单个机器人难以完成的任务，加强机器人的环境识别能力。因此多移动机器人在不同领域得到了广泛应用，包括军事领域、生产领域、服务领域。

在多移动机器人编队控制中，需要考虑每台机器人与其他成员的感知以获得协调运动所需的信息，同时需要考虑多移动机器人编队控制的运行环境。因此移动机器人会通常会搭载视觉传感器、距离传感器、速度传感器、角度传感器等，机载负荷高及控制算法复杂。而为了减少传感器的应用，目前基于视觉图像传感器的图像编队控制方法对领航者特征点的深度和领航者的速度未知，同时传统机载视觉传感器的视野存在限制，无法准确的进行多移动机器人进行编队控制。

如何保证在基于图象的多移动机器人编队控制的准确性，同时降低计算复杂度，降低机载负荷，成为目前需要解决的问题。

发明内容

本发明要解决的技术问题是，克服现有技术存在的上述缺陷，提供一种低复杂度的移动机器人视觉伺服编队控制方法，保证控制精度，简化机载感知工具，降低控制算法复杂度。

本发明解决技术问题采用的技术方案是，提供一种低复杂度的移动机器人视觉伺服编队控制方法，包括以下步骤：

S1：在移动机器人中确定一个领航者及多个跟随者；

S2：跟随者获取领航者的图像信息，进行特征检测，提取图像中位置信息；

S3：跟随者依据与领航者之间的相对运动学模型得到图像空间中的相对视觉运动学模型；

S4：对相对视觉运动学模型进行视觉特征向量转换，得到解耦视觉运动学模型；

S5：对实时视觉特征向量相对于期望恒定视觉特征向量的图像误差向量进行包含预定性能规范误差变换；

S6：对误差变换后的解耦视觉运动学模型设计跟随者所需要的基于角速度和线速度的控制律，实现跟随者对领航者的自动编队控制。

进一步的，所述步骤S3包括以下步骤：

S31：领航者相对于跟随者在世界坐标系的位置定位为：

其中，r_lf＝[x_lf,y_lf]^T为领航者相对于跟随者在世界坐标系中的位置，x_lf为领航者相对于跟随者在x坐标轴方向的相对位置，y_lf为领航者相对于跟随者在y坐标轴的相对位置；在世界坐标系内r_l为领航者的位置；r_f为跟随者的位置；θ_f为跟随者的方位；

S32：对领航者相对于跟随者的位置进行一阶微分换算，得到领航者相对于跟随者的相对运动学模型，模型描述为：

其中，

为x_lf的一阶微分；

为y_lf的一阶微分；ω_f为跟随者的角速度；v_f为跟随者的线速度；ω_l为领航者的角速度；v_l为领航者的线速度；θ_lf表示为θ_lf＝θ_l-θ_f；

为θ_lf的一阶微分；

S33：领航者的特征点在跟随者相机图像坐标系中的归一化坐标定义为：

其中，P＝[X,Y,Z]^T为特征点在跟随者相机图像坐标系中的三维坐标，归一化坐标为将三维空间点的坐标都除以Z，使其转化为归一的平面坐标，m为领航者特征点归一化后在x坐标轴方向的坐标值，n为领航者特征点归一化后在y坐标轴方向的坐标值；

S34：设领航者的特征点在领航者的相机坐标系x-y平面的坐标为(X_p,Y_P)以及跟随者的相机光学中心与领航者的特征点之间的相对高度为Z_p，则领航者和跟随者在图像空间中的相对视觉运动学模型为：

其中，

为s_o＝[m,n]^T的一阶微分，

为m的一阶微分；

为n的一阶微分。

进一步的，所述步骤S4包括以下步骤：

S41：定义视觉特征向量s＝[p,q]^T：

其中，

是关于图像坐标(m,n)的函数；

S42：对式(5)进行一阶微分，得到视觉特征向量一阶微分和图像坐标一阶微分的关系：

S43：在一阶微分后的视觉特征向量下领航者和跟随者在图像空间中的相对视觉运动学模型转换为解耦视觉运动学模型，模型表示为：

进一步的，所述步骤S5包括以下步骤：

S51：设置领航者的特征点在跟随者相机平面中的期望恒定视觉特征向量s^*＝[p^*,q^*]^T；

S52：依据解耦视觉运动学模型实时计算跟随者相机平面中的视觉特征向量s＝[p,q]^T与期望恒定视觉特征向量s^*＝[p^*,q^*]^T的误差，设图像误差向量为e＝[e₁,e₂]^T，则：

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

S53：用性能函数ρ_k(t)定义误差向量e＝[e₁,e₂]^T中的图像误差e_k的边界：

其中，-C _k和

为正参数，k＝{1,2}；

性能函数ρ_k(t)由下式定义：

其中，收敛速度l为正参数，误差极限ρ_∞为任意值；

S54：定义具有预定性能规范误差变换：

其中，

进一步的，所述步骤S6包括以下步骤：

S61：依据预定性能规范误差变换，设计跟随者的线速度和角速度v_f、w_f如下：

w_f＝-k₁ρε₁ (13)

其中，k₁和k₂为正参数，函数N(ε₂)定义为N(ε₂)＝cos(ε₂)ε₂，n为领航者特征点归一化后在跟随者图像平面y坐标轴方向的坐标值，ε₁为误差向量e₁通过预定性能规范误差变换得到，ε₂为误差向量e₂通过预定性能规范误差变换得到；

S62：依据跟随者的线速度和角速度v_f、w_f，得到跟随者基于线速度和角速度的控制律：

跟随者依据该控制律实现与领航者的自动编队控制。

进一步的，所述跟随者基于线速度和角速度的控制律，其系数矩阵为一个下三角矩阵，跟随者的编队控制由跟随者线速度v_f决定，不依赖于跟随者角速度w_f。

进一步的，所述移动机器人搭载相机传感器。

本发明采用移动机器人搭载的相机传感器获取图像信息，简化了机载传感器的数量，使得移动机器人机载负荷低；采用图像空间中的相对视觉运动学模型的坐标变换，及对图像误差向量进行的包含预定性能规范误差变换，设计跟随者所需要的角速度和线速度，得到跟随者基于角速度和线速度的控制律，实现跟随者对领航者的自动编队控制，而不需要计算移动机器人之间的相对角度、距离、领航者的速度及特征点的高度信息，有效降低了计算的复杂度；同时通过直接在图像空间中的处理可见性约束，选择适当性能函数，保证了图像坐标误差在指定的瞬态和稳态性能指标下收敛。

附图说明

图1为本发明提供的一种低复杂度的移动机器人视觉伺服编队控制方法的流程框图；

图2为本发明提供的一种低复杂度的移动机器人视觉伺服编队控制方法的算法过程示意图；

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

图4为本发明实施例的领航者和跟随者在仿真中的编队误差收敛示意图，其中，||r₁-r₀||表示机器人R₁与机器人R₀之间的编队误差，||r₂-r₀||表示机器人R₂与机器人R₀之间的编队误差；

图5为本发明实施例的连个跟随者的线速度和角速度曲线示意图，其中v₁为机器人R₁的线速度，w₁为机器人R₁的角速度，v₂为机器人R₂的线速度，w₂为机器人R₂的角速度；

图6为本发明实施例的机器人R₁的图像误差

收敛过程示意图；

图7为本发明实施例的机器人R₁的图像误差

收敛过程示意图；

图8为本发明实施例的机器人R₂的图像误差

收敛过程示意图；

图9为本发明实施例的机器人R₂的图像误差

收敛过程示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。

参考图1、图2，图1为本发明提供一种低复杂度的移动机器人视觉伺服编队控制方法，包括以下步骤：

S1：在移动机器人中确定一个领航者及多个跟随者；

S2：跟随者获取领航者的图像信息，进行特征检测，提取图像中位置信息；

S3：跟随者依据与领航者之间的相对运动学模型得到图像空间中的相对视觉运动学模型；

S4：对相对视觉运动学模型进行视觉特征向量转换，得到解耦视觉运动学模型；

S5：对实时视觉特征向量相对于期望恒定视觉特征向量的图像误差向量进行包含预定性能规范误差变换；

S6：对误差变换后的解耦视觉运动学模型设计跟随者所需要的基于角速度和线速度的控制律，实现跟随者对领航者的自动编队控制。

参考图3，考虑一组3个非完整的机器人R₀、R₁、R₂，R₀为领航者，R₁和R₂为跟随者。仿真设置为跟随者跟着领航者呈“8”字形的轨迹运动，领航者在x-y平面的参考轨迹为x_l＝2sin(0.1πt)和y＝3sin(0.05πt)，机器人相机传感器的固有参数为：m₀＝320像素，n₀＝240像素，a_m＝a_n＝616像素。

进一步的，所述步骤S3包括以下步骤：

S31：领航者相对于跟随者在世界坐标系的位置定位为：

其中，r_lf＝[x_lf,y_lf]^T为领航者相对于跟随者在世界坐标系中的位置，x_lf为领航者相对于跟随者在x坐标轴方向的相对位置，y_lf为领航者相对于跟随者在y坐标轴的相对位置；在世界坐标系内r_l为领航者的位置；r_f为跟随者的位置；θ_f为跟随者的方位；

S32：对领航者相对于跟随者的位置进行一阶微分换算，得到领航者相对于跟随者的相对运动学模型，模型描述为：

其中，

为x_lf的一阶微分；

为y_lf的一阶微分；ω_f为跟随者的角速度；v_f为跟随者的线速度；ω_l为领航者的角速度；v_l为领航者的线速度；θ_lf表示为θ_lf＝θ_l-θ_f；

为θ_lf的一阶微分；

S33：领航者的特征点在跟随者相机图像坐标系中的归一化坐标定义为：

其中，P＝[X,Y,Z]^T为特征点在跟随者相机图像坐标系中的三维坐标，归一化坐标为将三维空间点的坐标都除以Z，使其转化为归一的平面坐标，m为领航者特征点归一化后在x坐标轴方向的坐标值，n为领航者特征点归一化后在y坐标轴方向的坐标值；

S34：设领航者的特征点在领航者的相机坐标系x-y平面的坐标为(X_p,Y_P)以及跟随者的相机光学中心与领航者的特征点之间的相对高度为Z_p，R₁和R₂相机传感器和R₀上的特征点之间的相对高度分别假定为是h₀₁＝0.3，h₀₂＝-0.3。则领航者和跟随者在图像空间中的相对视觉运动学模型为：

其中，

为s_o＝[m,n]^T的一阶微分，

为m的一阶微分；

为n的一阶微分。

进一步的，所述步骤S4包括以下步骤：

S41：定义视觉特征向量s＝[p,q]^T：

其中，

是关于图像坐标(m,n)的函数；

S42：对式(5)进行一阶微分，得到视觉特征向量一阶微分和图像坐标一阶微分的关系：

S43：在一阶微分后的视觉特征向量下领航者和跟随者在图像空间中的相对视觉运动学模型转换为解耦视觉运动学模型，模型表示为：

进一步的，所述步骤S5包括以下步骤：

S51：设置领航者的特征点在跟随者相机平面中的期望恒定视觉特征向量s^*＝[p^*,q^*]^T；

R₁期望标准化坐标为(m₁ ^*,n₁ ^*)＝(0.1111,0.4176)或(p₁ ^*,q₁ ^*)＝(0.102,0.3826)，R₂期望标准化坐标为(m₂ ^*,n₂ ^*)＝(-0.375,-0.4176)或(p₂ ^*,q₂ ^*)＝(-0.3271,-0.3635)；

S52：依据解耦视觉运动学模型实时计算跟随者相机平面中的视觉特征向量s＝[p,q]^T与期望恒定视觉特征向量s^*＝[p^*,q^*]^T的误差，设图像误差向量为e＝[e₁,e₂]^T，则：

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

S53：用性能函数ρ_k(t)定义误差向量e＝[e₁,e₂]^T中的图像误差e_k的边界：

其中，-C _k和

为正参数，k＝{1,2}；

性能函数ρ_k(t)由下式定义：

其中，收敛速度l为正参数，误差极限ρ_∞为任意值。设置ρ_∞＝0.01(6个像素)，收敛速度为l＝0.8；

S54：定义具有预定性能规范误差变换：

其中，

进一步的，所述步骤S6包括以下步骤：

S61：依据预定性能规范误差变换，设计跟随者的线速度和角速度v_f、w_f如下：

w_f＝-k₁ρε₁ (13)

其中，k₁和k₂为正参数，函数N(ε₂)定义为N(ε₂)＝cos(ε₂)ε₂，n为领航者特征点归一化后在跟随者图像平面y坐标轴方向的坐标值，ε₁为误差向量e₁通过预定性能规范误差变换得到，ε₂为误差向量e₂通过预定性能规范误差变换得到。选择控制器增益为k₁＝0.2和k₂＝0.2；

S62：依据跟随者的线速度和角速度v_f、w_f，得到跟随者基于线速度和角速度的控制律：

跟随者依据该控制律实现与领航者的自动编队控制。

进一步的，所述跟随者基于线速度和角速度的控制律，其系数矩阵为一个下三角矩阵，跟随者的编队控制由跟随者线速度v_f决定，不依赖于跟随者角速度w_f。

进一步的，所述移动机器人搭载相机传感器。

参考图3-9所示，图3显示了平面中的编队轨迹。从图4可以看出，所有跟随者的形成误差迅速接近零。图5显示了跟随机器人的线速度和角速度的变化曲线。两个跟随者的图像坐标误差曲线如图6-图9所示。它们表明所有的图像特征误差不仅收敛迅速，所提出的控制器可以使所有图像特征误差保持在预定范围内，以使特征点始终处于相机的视野约束范围，编队误差具有满意的动态和稳态性能。

进一步的，所述移动机器人搭载相机传感器。

以上对本发明所提供的一种低复杂度的移动机器人视觉伺服编队控制方法进行了详细介绍，并通过实施例对本发明的具体原理及实施方式进行了阐述，这些说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也属于本发明权利要求的保护范围。

Claims (7)

1.一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，包括以下步骤：

S1：在移动机器人中确定一个领航者及多个跟随者；

S2：跟随者获取领航者的图像信息，进行特征检测，提取图像中位置信息；

S3：跟随者依据与领航者之间的相对运动学模型得到图像空间中的相对视觉运动学模型；

S4：对相对视觉运动学模型进行视觉特征向量转换，得到解耦视觉运动学模型；

S5：对实时视觉特征向量相对于期望恒定视觉特征向量的图像误差向量进行包含预定性能规范误差变换；

S6：对误差变换后的解耦视觉运动学模型设计跟随者所需要的基于角速度和线速度的控制律，实现跟随者对领航者的自动编队控制。

2.根据权利要求1所述的一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，所述步骤S3包括以下步骤：

S31：领航者相对于跟随者在世界坐标系的位置定位为：

其中，r_lf＝[x_lf,y_lf]^T为领航者相对于跟随者在世界坐标系中的位置，x_lf为领航者相对于跟随者在x坐标轴方向的相对位置，y_lf为领航者相对于跟随者在y坐标轴的相对位置；在世界坐标系内r_l为领航者的位置；r_f为跟随者的位置；θ_f为跟随者的方位；

S32：对领航者相对于跟随者的位置进行一阶微分换算，得到领航者相对于跟随者的相对运动学模型，模型描述为：

其中，

为x_lf的一阶微分；

为y_lf的一阶微分；ω_f为跟随者的角速度；v_f为跟随者的线速度；ω_l为领航者的角速度；v_l为领航者的线速度；θ_lf表示为θ_lf＝θ_l-θ_f；

为θ_lf的一阶微分；

S33：领航者的特征点在跟随者相机图像坐标系中的归一化坐标定义为：

其中，P＝[X,Y,Z]^T为特征点在跟随者相机图像坐标系中的三维坐标，归一化坐标为将三维空间点的坐标都除以Z，使其转化为归一的平面坐标，m为领航者特征点归一化后在x坐标轴方向的坐标值，n为领航者特征点归一化后在y坐标轴方向的坐标值；

S34：设领航者的特征点在领航者的相机坐标系x-y平面的坐标为(X_p,Y_P)以及跟随者的相机光学中心与领航者的特征点之间的相对高度为Z_p，则领航者和跟随者在图像空间中的相对视觉运动学模型为：

其中，

为s_o＝[m,n]^T的一阶微分，

为m的一阶微分；

为n的一阶微分。

3.根据权利要求2所述的一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，所述步骤S4包括以下步骤：

S41：定义视觉特征向量s＝[p,q]^T：

其中，

是关于图像坐标(m,n)的函数；

S42：对式(5)进行一阶微分，得到视觉特征向量一阶微分和图像坐标一阶微分的关系：

S43：在一阶微分后的视觉特征向量下领航者和跟随者在图像空间中的相对视觉运动学模型转换为解耦视觉运动学模型，模型表示为：

4.根据权利要求3所述的一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，所述步骤S5包括以下步骤：

S51：设置领航者的特征点在跟随者相机平面中的期望恒定视觉特征向量s^*＝[p^*,q^*]^T；

S52：依据解耦视觉运动学模型实时计算跟随者相机平面中的视觉特征向量s＝[p,q]^T与期望恒定视觉特征向量s^*＝[p^*,q^*]^T的误差，设图像误差向量为e＝[e₁,e₂]^T，则：

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

S53：用性能函数ρ_k(t)定义误差向量e＝[e₁,e₂]^T中的图像误差e_k的边界：

其中，-C _k和

为正参数，k＝{1,2}；

性能函数ρ_k(t)由下式定义：

其中，收敛速度l为正参数，误差极限ρ_∞为任意值；

S54：定义具有预定性能规范误差变换：

其中，

k＝{1,2}。

5.根据权利要求4所述的一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，所述步骤S6包括以下步骤：

S61：依据预定性能规范误差变换，设计跟随者的线速度和角速度v_f、w_f如下：

w_f＝-k₁ρε₁ (13)

其中，k₁和k₂为正参数，函数N(ε₂)定义为N(ε₂)＝cos(ε₂)ε₂，n为领航者特征点归一化后在跟随者图像平面y坐标轴方向的坐标值，ε₁为误差向量e₁通过预定性能规范误差变换得到，ε₂为误差向量e₂通过预定性能规范误差变换得到；

S62：依据跟随者的线速度和角速度v_f、w_f，得到跟随者基于线速度和角速度的控制律：

跟随者依据该控制律实现与领航者的自动编队控制。

6.根据权利要求5所述的一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，所述跟随者基于线速度和角速度的控制律，其系数矩阵为一个下三角矩阵，跟随者的编队控制由跟随者线速度v_f决定，不依赖于跟随者角速度w_f。

7.根据权利要求6所述的一种低复杂度的移动机器人视觉伺服编队控制方法，其特征在于，所述移动机器人搭载相机传感器。

Images