基于无线通信的移动机器人视觉镇定控制

Vision Stabilization Control of Mobile Robot Based on Wireless Communication

摘要: 该文基于无线传输技术，针对目标特征模型未知的情况，设计了一种轮式移动机器人动态场景中视觉镇定控制方案。首先建立了移动机器人视觉伺服系统坐标系，然后对特征点移动前后两期望坐标系和监视摄像机坐标系间的变换关系进行了分析与计算。通过XBee-Pro无线通信模块进行特征点信息传输，通过仿真和实验结果表明，该方案可以成功驱使移动机器人到达目标位姿处。

Abstract: Based on wireless transmission technology, this paper designs a scheme for visual stabilization control of a wheeled mobile robot in a dynamic scene in response to an unknown target feature model. First, the coordinate system of the mobile robot’s visual servo system was established, and then the transformation relationship between the two desired coordinate systems before and after the feature point movement and the surveillance camera coordinate system was analyzed and calculated. The XBee-Pro wireless communication module is used to transmit feature point information. Simulation and experimental results show that the scheme can successfully drive the mobile robot to the target pose.

1. 引言

随着科学技术的不断发展，机器人逐渐走入了人们的生活，使得人类的生活发生了日新月异的变化 [1]。机器人凭借其精度高、效率高、可适应复杂工作环境等优点，逐步在军事、智能家居、科学研究、工业生产等领域 [2] [3] 得到应用。

视觉传感器具有非接触测量、易于设置、灵活的设计、标准化的设置环境、价格低等优点，因此视觉传感器被广泛应用于轮式移动机器人。而且伴着智能控制、模式识别、过程控制等技术的日趋成熟，这使得视觉轮式移动机器人 [4] 更加的智能化且运用范围更加广泛。

早期应用视觉伺服方法的典型代表为工业机器人 [5]。相比移动机器人，工业机器人具有简单的约束条件，传统工业机器人大都集中在控制方法的研究上。移动机器人通过摄像机拍取运动场景信息，来得知目标位姿或机器人位姿的相对关系，进而通过控制器可以驱使移动机器人做出相应地动作。视觉伺服将图像处理、实时计算、运动控制等学科融为一体进行研究 [6] [7]。

移动机器人非线性控制方法和基于视觉的环境感知得到了深入研究 [8] [9] [10]。在已有的控制策略中，机器人期望位姿和目标特征点大多是固定的。本文基于无线通信技术，在机器人期望位姿随特征变化的动态场景中，对轮式移动机器人视觉伺服进行了研究。

2. 系统坐标系描述

轮式移动机器人视觉伺服系统坐标系如图1所示，整个坐标系系统由监视摄像机、特征点、移动机器人组成。监视摄像机坐标系定义为 ${\mathcal{F}}^m$。为了应用方便，假设机载相机和移动机器人坐标系重合。将移动机器人的当前位姿定义为 ${\mathcal{F}}^c$。坐标系 ${\mathcal{F}}^c$ 的原点为机载相机的光心。 ${\mathcal{F}}^c$ 的 $z^c{x}^c$ 平面表示机器人的运动平面，其中 $z^c$ 轴和摄像机的光轴在相同的方向上。 $y^c$ 垂直于移动机器人的运动平面 $z^c{x}^c$，并满足右手定则。 $P_i^d$ 是移动之前的特征点， $P_i^{*}$ 是移动之后的特征点。特征点移动前的期望位姿为 ${\mathcal{F}}^d$， ${\mathcal{F}}^{\ast }$ 为特征点移动之后的期望位姿，为使移动后的机器人期望坐标系位于机器人运动平面上，将移动后的期望坐标系 ${\mathcal{F}}^{\ast }$ 作投影，即坐标系 ${\mathcal{F}}^p$。

3. 控制策略

为解决实际应用一些场合中需实现特征点移动后期望位姿与原始期望位姿间的视觉镇定任务，提出了新的控制方法。基于移动后的特征点 $P_i^{*}$，运用无线通信技术来完成轮式移动机器人在特征点变化后期望位姿镇定至特征点移动前原始期望位姿的视觉伺服任务。运用两个无线通信模块XBee-Pro，其中一个XBee-Pro1模块安装在监视摄像机系统用于发送数据，另一个XBee-Pro2安装在移动机器人系统用于接收数据。控制策略框图如图2所示，控制策略可分为以下四个阶段。

Figure 1. Robot visual servoing system coordinate system

图1. 机器人视觉伺服系统坐标系

1) 第一阶段，机载摄像机在相对于目标特征点合理的位姿(特征点移动前期望位姿)处拍摄一张特征点移动之前的图像。然后移动机器人系统程序开始循环串口读取数据阶段，等待数据输入。接着固定监视摄像机拍取特征点移动之前的图像信息，运用XBee-Pro1发送。此时移动机器人系统无线通信模块XBee-Pro2接收XBee-Pro1发出的数据。机器人系统利用获取的数据经过六自由度单应矩阵分解得到 ${\mathcal{F}}^m$ 和 ${\mathcal{F}}^d$ 之间的变换矩阵。

2) 第二阶段，特征点移动后，为完成从特征点移动后期望坐标系到原始坐标系的视觉镇定任务，令移动机器人在特征点移动后期望位姿出发。监视摄像机拍取特征点移动后的图像信息，利用模块XBee-Pro1将图像信息发出。然后机载相机拍取移动后特征点信息且XBee-Pro2接收XBee-Pro1发出的实时数据。最终经过六自由度矩阵分解得出 ${\mathcal{F}}^m$ 与 ${\mathcal{F}}^c$ 间的变换矩阵。

3) 第三阶段，视觉伺服过程中，特征点的3D坐标是未知的，场景的深度信息亦不知，因此需要计算出两个不同深度信息之间的比例。然后经过坐标系变换，计算得出 ${\mathcal{F}}^c$ 与 ${\mathcal{F}}^d$ 间的变换关系。

4) 第四阶段，运用自适应控制器驱使轮式移动机器人从特征点移动后期望位姿到达移动前原始期望位姿处。

Figure 2. Control strategy block diagram

图2. 控制策略框图

4. 坐标系关系

移动机器人系统通过无线通信模块XBee-Pro2接收的图像数据和机载相机拍摄的特征点的图像信息，运用六自由度单应矩阵分解，可以得出坐标系 ${\mathcal{F}}^m$ 和坐标系 ${\mathcal{F}}^d$ 之间的旋转矩阵 ${}_m{}^{d}R\left(t\right)$ 和平移向量 ${}^d{T}_m\left(t\right)$ 为：

${}_m{}^{d}R=\left[\begin{array}{ccc}{}_m{}^{d}R{}_{11}& {}_m{}^{d}R{}_{12}& {}_m{}^{d}R{}_{13}\\ {}_m{}^{d}R{}_{21}& {}_m{}^{d}R{}_{22}& {}_m{}^{d}R{}_{23}\\ {}_m{}^{d}R{}_{31}& {}_m{}^{d}R{}_{32}& {}_m{}^{d}R{}_{33}\end{array}\right],{}^{d}T{}_m=\left[\begin{array}{c}\frac{{}^{d}T{}_{mx}}{d^m}\\ \frac{{}^{d}T{}_{my}}{d^m}\\ \frac{{}^{d}T{}_{mz}}{d^m}\end{array}\right]$ (1)

由于场景中深度未知，上式中将从监视摄像机坐标系 ${\mathcal{F}}^m$ 的原点沿着平面法向量 $n^{ma}$ 到移动之前特征点平面的深度设为 $d^m$。

当前坐标系 ${\mathcal{F}}^c$ 与监视相机坐标系 ${\mathcal{F}}^m$ 之间的旋转矩阵 ${}_m{}^{c}R\left(t\right)$ 和平移向量 ${}^c{T}_m\left(t\right)$ 为：

${}_m{}^{c}R=\left[\begin{array}{ccc}{}_m{}^{c}R{}_{11}& {}_m{}^{c}R{}_{12}& {}_m{}^{c}R{}_{13}\\ {}_m{}^{c}R{}_{21}& {}_m{}^{c}R{}_{22}& {}_m{}^{c}R{}_{23}\\ {}_m{}^{c}R{}_{31}& {}_m{}^{c}R{}_{32}& {}_m{}^{c}R{}_{33}\end{array}\right],{}^{c}T{}_m=\left[\begin{array}{c}\frac{{}^{c}T{}_{mx}}{d^{mb}}\\ \frac{{}^{c}T{}_{my}}{d^{mb}}\\ \frac{{}^{c}T{}_{mz}}{d^{mb}}\end{array}\right]$ (2)

上式中将从监视摄像机坐标系 ${\mathcal{F}}^m$ 的原点沿着平面法向量 $n^{mb}$ 到特征点移动之前的特征点平面的深度设为 $d^{mb}$。

由式(1)可知，监视摄像机坐标系与特征点移动前原始期望坐标系之间的变换矩阵为：

${}_m{}^{d}T=\left[\begin{array}{cc}{}_m{}^{d}R& \frac{{}^{d}T{}_m}{d^m}\\ 0_{1\times 3}& 1\end{array}\right]$ (3)

由式(2)可知，移动机器人在特征点移动后期望位姿出发，机器人当前位姿与监视摄像机坐标系之间的变换矩阵为：

${}_m{}^{c}T=\left[\begin{array}{cc}{}_m{}^{c}R& \frac{{}^{c}T{}_m}{d^{mb}}\\ 0_{1\times 3}& 1\end{array}\right]$ (4)

运用未知深度之间的比例关系，得出机器人当前位姿与监视摄像机坐标系之间的变换矩阵为：

${}_m{}^{c}T=\left[\begin{array}{cc}{}_m{}^{c}R& \frac{\lambda {}^{c}T{}_m}{d^{mb}}\\ 0_{1\times 3}& 1\end{array}\right]=\left[\begin{array}{cc}{}_m{}^{c}R& \frac{{}^{c}T{}_m}{d^m}\\ 0_{1\times 3}& 1\end{array}\right]$ (5)

运用坐标系变换关系，利用式(3)和(5)，可以得到轮式机器人在特征点移动后期望位姿开始出发，当前坐标系和原始期望坐标系之间的变换关系：

$\left[\begin{array}{cc}{}_d{}^{c}R& \frac{{}^{c}T{}_d}{d^m}\\ 0_{1\times 3}& 1\end{array}\right]=\left[\begin{array}{cc}{}_m{}^{c}R& \frac{{}^{c}T{}_m}{d^m}\\ 0_{1\times 3}& 1\end{array}\right]{\left[\begin{array}{cc}{}_m{}^{d}R& \frac{{}^{d}T{}_m}{d^m}\\ 0_{1\times 3}& 1\end{array}\right]}^{-1}$ (6)

式中 ${}_d{}^{c}R$ 和 $\frac{{}^{c}T{}_d}{d^m}$ 分别表示轮式移动机器人当前位姿与特征点前原始期望位姿之间的旋转矩阵和平移向量。

考虑到任务场景中存在未知深度，因此控制器应用一个自适应控制器来进行机器人的视觉伺服任务。基于李雅普诺夫稳定性理论，运用可测数据号 $\varphi \left(t\right)$， $\alpha \left(t\right)$ 和 $e_s\left(t\right)$，轮式机器人的速度可分别设为：

$v=\left(\gamma \cos \alpha \right)e_s$ (7)

$w=k\alpha +\hat{l}\gamma \frac{\cos \alpha \sin \alpha }{\alpha }\left(\alpha +q\varphi \right)$ (8)

式中 $\gamma ,k,q\in {ℝ}^{+}$ 是正的控制增益， $\hat{l}\left(t\right)\in ℝ$ 为和未知深度信息有关的常数值。而且设置深度估计 $\hat{l}$ 的自适应更新律为：

$\stackrel{\dot{}}{\hat{l}}=\Gamma \cdot {\text{Proj}}_{\hat{l}}\left(\gamma \left(\alpha +q\varphi \right)\sin \alpha \cos \alpha \right)$ (9)

上式中 $\Gamma \in ℝ$ 为更新增益。投影函数 ${\text{Proj}}_{\hat{l}}\left(X\right)$ 可以表示为：

${\text{Proj}}_{\hat{l}}\left(X\right)=\{\begin{array}{l}0,\hat{l}=l_{\min }或X<0\hfill \\ X,其他\hfill \end{array}$ (10)

式中 $l_{\min }\in {ℝ}^{+}$ 为l的正下界。

5. 仿真验证

对基于无线通信技术的轮式移动机器人从特征点变化后的期望位姿镇定至特征点移动前原始期望位姿的视觉伺服控制策略进行仿真。仿真中采用的模拟相机内部参数如下：

$K=\left[\begin{array}{ccc}998.0& 0& 369.5\\ 0& 998.0& 369.5\\ 0& 0& 1\end{array}\right]$ (11)

在系统参考坐标系下，设置轮式机器人的起始位姿为：

$\left(-2.0m,1.0m,0^{\circ }\right)$ (12)

仿真中将特征点移动设置为：

$\left(-2.3m,0.3m,0^{\circ }\right)$ (13)

机器人最终期望位姿应为 $\left(0m,0m,0^{\circ }\right)$，即移动机器人移动到特征点移动之前的期望位姿处。选取的控制增益和其它参数为：

$\gamma =1.38,k=1.0,q=2,{\hat{l}}_{\min }=1$ (14)

仿真验证中轮式机器人的移动路径图如图3所示，图中红三角为特征点移动前机器人的期望位姿，即参考坐标系的位姿。轮式移动机器人的运行过程中的线速度和角速度如图4所示，图中可以看出机器人的线速度和角速度在3秒左右逐渐平滑地收敛到零。图5显示出了特征点的运动轨迹。其中蓝色星形点是特征点移动之前在图像中的位置。圆形和方形点分别代表初始位姿时特征点位置和特征点移动后期望位姿处特征点在图像中的位置。图6表示的是机器人的运动状态 $\left({}^{p}T{}_{cx}\left(t\right),{}^{p}T{}_{cz}\left(t\right),\theta \left(t\right)\right)$ 随时间推移的变化过程，红色虚线为期望值，由图可得三个值都在3秒左右平滑收敛到所期望位姿的最终值。综上所述，在特征点3D坐标未知的情况下，轮式移动机器人在仿真过程中，成功地从特征点移动后的期望位姿镇定至原始期望位姿处。

Figure 3. Simulation results: wheeled robot movement path [Red triangle: desired pose]

图3. 仿真结果：轮式机器人移动路径[红三角：期望位姿]

Figure 4. Simulation results: speed change trend of the wheeled robot [Red dashed lines: desired values]

图4. 仿真结果：轮式机器人速度变化[红色虚线：期望值]

Figure 5. Simulation results: target feature path

图5. 仿真结果：目标特征路径

Figure 6. Simulation results: evolution of the robot pose [Red dashed lines: desired values]

图6. 仿真结果：移动机器人位姿变化[红虚线：期望值]

6. 实验验证

Figure 7. XBee-Pro module

图7. XBee-Pro模块

如图7所示，实验中选用无线通信模块为XBee-Pro，XBee-Pro模块是与ZigBee/IEEE802.15.4兼容的解决方案，设备间数据传输可靠等特征适用于科研实验室的需求，创新后的XBee-Pro模块在范围上是普通ZigBee模块的2.5倍左右。XBee-Pro模块不配置就可以建立通信，它的默认配置支持宽范围的数据系统应用，同样也能用AT命令来进行配置。

用实验来验证基于无线通信技术的轮式移动机器人，从特征点移动后期望位姿移动至特征点移动前期望位姿的视觉伺服控制策略的有效性。如图8所示，整个轮式机器人视觉镇定系统由两个XBee-Pro模块、先锋机器人、机载映美精摄像机、机载笔记本电脑、目标特征点格子板、固定监视映美精摄像机等搭建而成。

如图9所示，左图表示的是特征点移动之前特征点的位置和移动机器人相对于目标特征点期望位姿的位置。右图表示特征点向左前方移动后的位置，对机器人视觉伺服系统具体的操作流程如控制策略所述。在本实验中，移动机器人在特征点移动后相对应的期望位姿 $\left(-1.25\text{m},-0.55\text{m},-5^{\circ }\right)$ 处出发开始视觉伺服任务。选取合适的控制参数为： $\gamma =0.35,k=0.08,q=0.95,{\hat{l}}_{\min }=0.4$。

Figure 8. Experiments scene

图8. 实验场景图

Figure 9. Feature point movement [Left: before moving, Right: after moving]

图9. 特征点移动变化[左：移动前，右：移动后]

如图10为轮式机器人实验移动轨迹图。轮式机器人基本上实现了从特征点移动后期望位姿出发，到达红色三角形所标注的期望位姿处。移动机器人的实时位姿变化在图11可以看到， $\left({}^{p}T{}_{cz}\left(t\right),{}^{p}T{}_{cx}\left(t\right),\theta \left(t\right)\right)$ 都逐渐收敛至期望值。目标特征点的轨迹图如图12所示，图中蓝色星形点为特征点移动前在机载摄像机中的期望图像。特征点移动后，机载相机在相对应的期望位置拍摄的图像如蓝色圆形点所示。图中星形点和圆形点将近重合，证明了移动机器人准确地在移动后期望位置出发。绿色方形点代表了移动后特征点在机器人镇定停止时在相机中的图像。如图13为轮式机器人速度实时变化图，可以得出线速度和角速度都收敛于期望值。综合上述分析得出，在特征点模型未知的情况下，基于无线通信模块，可以运用设计的控制策略控制移动机器人从移动后期望位姿移动至原始期望位姿处。

Figure 10. Experimental results: wheeled robot motion path

图10. 实验结果：轮式机器人运动路径

Figure 11. Experimental results: evolution of the wheeled robot pose [Red dashed line: desired values]

图11. 实验结果：轮式机器人位姿变化[红色虚线：期望值]

Figure 12. Experimental results: feature point trajectory

图12. 实验结果：场景特征路径

Figure 13. Experimental results: speed change of wheeled robot [Red dashed line: zero values]

图13. 实验结果：轮式机器人的速度变化[红色虚线：零值]

7. 结论

本文基于无线传输技术，针对目标特征模型未知的情况，设计了一个移动机器人从特征点移动后期望位姿至特征点移动前原始期望位姿的控制方案。建立了移动机器人视觉伺服系统坐标系，对特征点移动前后两期望坐标系和监视摄像机坐标系间的变换关系进行了分析与计算。通过仿真和实验结果表明，该方案可以成功驱使移动机器人从特征点移动后期望位姿到达原始的期望位姿处。

基金项目

天津市教委科研计划项目(2017KJ084)。

参考文献