CN104950893A - 基于单应性矩阵的最短路径视觉伺服控制方法 - Google Patents

Abstract

本发明公开了一种基于单应性矩阵的最短路径视觉伺服控制方法，是将机器人的运动分解成三个阶段，首先使机器人旋转到其面向目标图像所在位置点，简称目标点，然后机器人平移至该目标点，最后机器人旋转到目标位姿的角度，每个阶段可依据相应的控制律对机器人的运动进行控制。本发明的方法直接利用单应性矩阵中的元素构成误差函数，不需要进行单应性矩阵的分解，对相机参数和图像噪声的鲁棒性较好，同时解决了单目相机未知深度信息的问题和履带式机器人的非完整约束的问题，使机器人以最短路径稳定的达到目标位姿。

Description

基于单应性矩阵的最短路径视觉伺服控制方法

技术领域

本发明涉及一种视觉伺服控制方法，尤其涉及一种基于单应性矩阵的最短路径视觉伺服控制方法。

背景技术

自动寻物机器人已经逐步被投用于工业生产中，现有的自动寻物机器人通常采用已知地图的高精确定位寻物，灵活性差并且实现成本高。一些机器人采用基于视觉信息的伺服控制方法控制机器人运动以提高灵活性和稳定性。基于图像的视觉伺服控制系统反馈信息定义在图像平面，其控制目标是使当前获取的图像状态s和给定的目标图片状态s*的误差最小。利用机器视觉的相关知识，可以通过对当前图像和目标图像匹配得到对应的特征点，用图像特征点的像素坐标结合相机参数构造图片状态s，设计控制器使s和s*间的误差减小以实现控制目标。但是利用单目相机获取图像信息进行控制的方案中一个十分重要的问题就是无法获取深度信息。近年来有许多学者提出了不同的办法以试图解决这一问题，如利用线性化系统模型并结合扩展卡尔曼滤波(EKF)算法以补偿深度信息，使用单目全景摄像机系统的反馈完成沿墙壁运动、跟踪标记或位置校准等等。但是，使用EKF算法估计深度信息存在需要对模型进行线性化处理的缺点。除EKF算法外，有学者尝试利用从连续图像的光流信息中得到的表观速度去估计深度信息，但是光流技术需要大量的图像处理工作，会导致机器人控制方面的实时性变差。也有学者在工作空间中增加一个不需标定的固定相机以开发自适应跟踪控制器，用以弥补相机参数和机器人动力学建模的误差，但这种方法需要相机到机器人运动平面的距离维持不变。

机器人的非完整约束是指机器人在移动过程中同时受到空间位置和运动速度的约束，并且运动速度不能积分以转换为空间位置。由于存在非完整约束，机器人的控制变量的维数小于机器人状态空间的维数，使对机器人的运动控制变得困难。非完整约束在机器人系统中占据着十分重要的位置，许多移动机器人、角动量守恒空间机器人以及冗余度机器人等系统结构中都包含着非完整约束的应用。对非完整约束早期的研究主要是运用微分方程理论和变分原理等对系统进行建模分析等等，随后有学者将微分几何、群论等数学工具应用于非线性系统分析，使得对非完整系统的研究有了较快发展，但是分析的过程仍然较为复杂。

现有的自动寻物机器人通常采用已知地图的高精确定位寻物，灵活性差并且实现成本高，有些通过视觉伺服控制的机器人需要双目相机来计算深度信息，使得运算复杂并且实现成本较高。

发明内容

本发明的目的在于针对现有技术的不足，提供一种简单稳定、易于实现的基于单应性矩阵的最短路径视觉伺服控制方法。

本发明的基于单应性矩阵的最短路径视觉伺服控制方法，包括如下步骤：

1)以机器人前进方向为z轴，垂直地面向下为y轴，建立机器人的右手坐标系；

2)将机器人捕捉的实时画面与给定的目标图像进行匹配得到相应的实时的单应性矩阵：

$H=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ 0& 1& 0\\ h_{31}& h_{32}& h_{33}\end{array}\right]$

分三个阶段控制机器人的运动，根据实时计算出的单应性矩阵得到每一阶段实时的控制律如下：

阶段Ⅰ： $v=0,w=-k_{w1}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

当w＝0时，阶段Ⅰ结束，机器人的运动进入阶段Ⅱ；

阶段Ⅱ： $v=-k_v{h}_{32},w=-k_{w2}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

当v＝0时，阶段Ⅱ结束，机器人的运动进入阶段Ⅲ；

阶段Ⅲ：v＝0,w＝-k_w3h₁₃

当w＝0时，机器人到达目标位姿，运动结束；

其中，v为机器人实时线速度，w为机器人实时角速度，k_v、k_w1、k_w2、k_w3为比例因子，且k_v∈(0，1)，k_w1∈(0，0.3)，k_w2∈(0，0.3)，k_w3∈(0，2)，α_x为机器人的相机的焦距在图像平面像素水平排列方向上占据的单位像素个数。

上述技术方案所依据的控制思路如下：

将机器人的运动分解成三个阶段，首先使机器人旋转到其面向目标图像所在位置点，简称目标点，然后机器人平移至该目标点，最后机器人旋转到目标位姿的角度，比较每个阶段单应性矩阵的各元素的差异得出速度的控制方程。

对于一个平面上的机器人，以其前进方向为z轴，垂直地面向下为y轴，建立机器人的右手坐标系，如图1所示；可以将它相对于目标位姿的旋转矩阵和平移矩阵化简如下：

$R=\left[\begin{array}{ccc}\cos \mathrm{\φ}& 0& \sin \mathrm{\φ}\\ 0& 1& 0\\ -\sin \mathrm{\φ}& 0& \cos \mathrm{\φ}\end{array}\right],C={(x,0,z)}^T$

其中，x、z分别为机器人当前位置在目标位姿坐标系中的表示，φ为机器人实时位姿与目标位姿间的夹角，机器人的当前位姿可以由状态向量(x,z,φ)表示。

相应的机器人的单应性矩阵可以写成：

$H=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ 0& 1& 0\\ h_{31}& h_{32}& h_{33}\end{array}\right]$

其中各元素分别由下式计算得出:

$h_{11}=cos\mathrm{\φ}+(xcos\mathrm{\φ}+zsin\mathrm{\φ})\frac{n_z}{d}$

$h_{12}=\frac{{\mathrm{\α}}_x}{{\mathrm{\α}}_y}(xcos\mathrm{\φ}+zsin\mathrm{\φ})\frac{n_y}{d}$

$h_{13}={\mathrm{\α}}_x(sin\mathrm{\φ}+(xcos\mathrm{\φ}+zsin\mathrm{\φ}\left)\frac{n_z}{d}\right)$

$h_{31}=\frac{1}{{\mathrm{\α}}_x}(-sin\mathrm{\φ}+(-xsin\mathrm{\φ}+zcos\mathrm{\φ}\left)\frac{n_x}{d}\right)$

$h_{32}=\frac{1}{{\mathrm{\α}}_y}(-xsin\mathrm{\φ}+zcos\mathrm{\φ})\frac{n_y}{d}$

$h_{33}=cos\mathrm{\φ}+(-xsin\mathrm{\φ}+zcos\mathrm{\φ})\frac{n_z}{d}$

其中，α_x为机器人的相机的焦距在图像平面像素水平排列方向上占据的单位像素个数，

α_y为机器人的相机的焦距在图像平面像素垂直排列方向上占据的单位像素个数，

[n_x,n_y,n_z]^T为给定的目标图像的法向量在目标位姿坐标系中的表示，

d为目标位姿坐标系的原点到给定的目标图像的距离，

将机器人的运动分解为三个阶段：第一阶段使机器人旋转到其相机指向目标位置，即只做旋转运动；第二阶段使机器人平移到目标位置，即只做平移运动；第三阶段使机器人旋转到正确的角度，即只做旋转运动；三个阶段的运动过程如图2所示。

下面将分析每一阶段中单应性矩阵的变化情况。

机器人刚开始运动时，设机器人当前位置与目标位置的连线与目标位姿坐标系的z轴所形成的夹角为φ_t，当第一阶段结束时，φ＝φ_t，将该式子代入单应性矩阵中可以得出第一阶段完成时的单应性矩阵的表达式：

$\begin{array}{c}H(\mathrm{\φ}={\mathrm{\φ}}_t)\\ \text{=}\left[\begin{array}{ccc}{\mathrm{cos\φ}}_t& 0& {\mathrm{\α}}_x{\mathrm{sin\φ}}_t\\ 0& 1& 0\\ \frac{1}{{\mathrm{\α}}_x}(-{\mathrm{sin\φ}}_t+\frac{z}{{\mathrm{cos\φ}}_t}\frac{n_x}{d})& \frac{1}{{\mathrm{\α}}_y}\frac{z}{{\mathrm{cos\φ}}_t}\frac{n_y}{d}& {\mathrm{cos\φ}}_t+\frac{z}{{\mathrm{cos\φ}}_t}\frac{n_z}{d}\end{array}\right]\end{array}$

第二阶段中，机器人始终指向目标位置，此时一直有φ＝φ_t，第二阶段完成时，x＝0,z＝0,φ＝φ_t，将上式代入单应性矩阵中可以得出第二阶段完成时的单应性矩阵的表达式：

$H(x=0,z=0,\mathrm{\φ}={\mathrm{\φ}}_t)=\left[\begin{array}{ccc}{\mathrm{cos\φ}}_t& 0& {\mathrm{\α}}_x{\mathrm{sin\φ}}_t\\ 0& 1& 0\\ \frac{-{\mathrm{sin\φ}}_t}{{\mathrm{\α}}_x}& 0& {\mathrm{cos\φ}}_t\end{array}\right]$

第三阶段中，机器人在原地旋转到正确的角度，最终达到目标位姿，此时有x＝0,z＝0,φ＝0，将上式代入单应性矩阵中可以得出第三阶段完成时的单应性矩阵的表达式：

H(x＝0,z＝0,φ＝φ_t)＝I

假定相机的标定矩阵是已知的，则可以得到焦距长度α_x的值，分析比较这三阶段中单应性矩阵的变化情况，可以得出每一阶段的控制律：

第一阶段的运动目标是达到φ＝φ_t的状态，也就是使机器人指向目标点，这个过程中v始终为0，当φ＝φ_t时，从相应的单应性矩阵中可以得出:

$h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1=0$

且机器人由φ变化到φ_t的过程中，上式单调变化至0，因此，第一阶段v和ω的控制式可以写为：

$v=0,w=-k_w(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

第二阶段中，机器人应平移到目标位置，此时x＝0，z＝0，在该过程中z逐渐收敛至0，可以看出单应性矩阵中的元素h₃₂的变化规律与z的变化规律一致，且考虑到测量的误差以及环境噪声的影响，仍旧取第一阶段的w控制律作为本阶段的w控制律，因此，第二阶段v和ω的控制式可以写为：

$v=-k_v{h}_{32},w=-k_w(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

第三阶段中，机器人在原地旋转到目标位姿，此时速度v＝0，φ＝0，在该过程中，φ逐渐收敛至0，可以看出单应性矩阵中的元素h₁₃的变化规律与φ的变化规律一致，因此，第三阶段v和ω的控制式可以写为：

v＝0,w＝-k_wh₁₃

综上所述，整个运动过程中的控制律为：

阶段Ⅰ： $v=0,w=-k_{w1}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

当w＝0时，阶段Ⅰ结束，机器人的运动进入阶段Ⅱ；

阶段Ⅱ： $v=-k_v{h}_{32},w=-k_{w2}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

当v＝0时，阶段Ⅱ结束，机器人的运动进入阶段Ⅲ；

阶段Ⅲ：v＝0,w＝-k_w3h₁₃

当w＝0时，机器人到达目标位姿，运动结束。

相对于现有技术，本发明的有益效果是：

本发明的基于单应性矩阵的伺服控制方法直接利用单应性矩阵中的元素构成误差函数，不需要进行单应性矩阵的分解，对相机参数和图像噪声的鲁棒性较好，此外，本发明避免了对目标物体未知深度信息的使用，且运动控制律的设计和计算是定义在机器人的前进速度和旋转速度上，使输出的速度指令隐式地满足了非完整约束方程，同时解决了单目相机未知深度信息的问题和履带式机器人的非完整约束的问题，能在未知地图的环境下实现非完整约束的履带式机器人的视觉伺服控制，使机器人以最短路径稳定的达到目标位姿。本发明的控制方法简单，避免了估计深度信息的复杂计算，减小了误差，易于实现。

附图说明

图1为机器人当前位姿与目标位姿的示意图。

图2为本发明的控制率下机器人的运动示意图。图中：Oxz坐标系为目标位姿坐标系，O’x’z’坐标系为机器人当前位置坐标系。

具体实施方式

本实例中采用了AS-RF广茂达能力风暴机器人，AS-RF机器人拥有开放式的软硬件架构，模块化的结构设计以及先进的配件体系和丰富的开发库资源，采用Windows XP的操作系统和Visual C++的开发平台，拥有高强度的铝合金机身，可以抗击5m/s速度下的直接冲撞。它自带一台PC机，并且提供了强大的库函数，偏于编写程序和调试，拥有三个自由度但只有两个驱动，机器人的运动为非完整约束。

在对机器人的运动进行控制之前，先按常规方法对机器人的相机进行标定，获取相机内参，得到参数α_x，即机器人的相机的焦距在图像平面像素水平排列方向上占据的单位像素个数。

以机器人前进方向为z轴，垂直地面向下为y轴，建立机器人的右手坐标系。

将机器人捕捉的实时画面与给定的目标图像进行匹配得到相应的实时的单应性矩阵：

$H=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ 0& 1& 0\\ h_{31}& h_{32}& h_{33}\end{array}\right]$

计算单应性矩阵时需对两幅图像进行特征点检测和提取，关于特征点检测、提取的算法有很多，如SURF(Speeded Up Robust Features)、SIFT(Scale-invariant Feature Transform)、ORB(ORiented Brief)算法等，综合考虑算法的尺度不变性、快速高效性、鲁棒性、旋转不变性等，实验中采用SURF算法，并用FLANN(Fast ApproximateNearest Neighbor Search Library)算法实现快速高效匹配。再利用提取得到的一组对应特征点，可以直接调用OpenCV的函数计算得到两幅图像间的单应性矩阵。

分三个阶段控制机器人的运动，根据实时计算出的单应性矩阵得到每一阶段对机器人的线速度v和角速度w实时的控制律如下：

1)阶段Ⅰ： $v=0,w=-k_{w1}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

其中，k_w1为比例因子，实验中取0.1，以使角速度大小合适。

第一阶段使机器人旋转到其相机指向目标位置，即只做旋转运动，当w＝0时，阶段Ⅰ结束，机器人的运动进入阶段Ⅱ；

2)阶段Ⅱ： $v=-k_v{h}_{32},w=-k_{w2}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

其中，k_v、k_w2为比例因子，实验中取k_v＝0.1、k_w2＝0.1，以使线速度和角速度大小合适。

第二阶段使机器人平移到目标位置，即只做平移运动，当v＝0时，阶段Ⅱ结束，机器人的运动进入阶段Ⅲ；

3)阶段Ⅲ：v＝0,w＝-k_w3h₁₃

其中，k_w3为比例因子，实验中取1，以使角速度大小合适。

第三阶段使机器人旋转到正确的角度，即只做旋转运动，当w＝0时，机器人到达目标位姿，运动结束。

Claims (1)

1.基于单应性矩阵的最短路径视觉伺服控制方法，其特征在于，包括如下步骤：

1)以机器人前进方向为z轴，垂直地面向下为y轴，建立机器人的右手坐标系；

2)将机器人捕捉的实时画面与给定的目标图像进行匹配得到相应的实时的单应性矩阵：

$H=\left[\begin{array}{ccc}{h}_{11}& h_{12}& h_{13}\\ 0& 1& 0\\ h_{31}& h_{32}& h_{33}\end{array}\right]$

分三个阶段控制机器人的运动，根据实时计算出的单应性矩阵得到每一阶段实时的控制律如下：

阶段Ⅰ： $v=0,w=-k_{w1}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

当w＝0时，阶段Ⅰ结束，机器人的运动进入阶段Ⅱ；

阶段Ⅱ： $v=-k_v{h}_{32},w=-k_{w2}(h_{11}^2+\frac{h_{13}^2}{{\mathrm{\α}}_x^2}-1)$

当v＝0时，阶段Ⅱ结束，机器人的运动进入阶段Ⅲ；

阶段Ⅲ：v＝0,w＝-k_w3h₁₃

当w＝0时，机器人到达目标位姿，运动结束；

其中，v为机器人实时线速度，w为机器人实时角速度，k_v、k_w1、k_w2、k_w3为比例因子，且k_v∈(0，1)，k_w1∈(0，0.3)，k_w2∈(0，0.3)，k_w3∈(0，2)，α_x为机器人的相机的焦距在图像平面像素水平排列方向上占据的单位像素个数。