基于视觉的移动机器人相对位姿优化估计

黄双发，周乐来，2*，李贻斌，2

（1.山东大学控制科学与工程学院，济南 250061；2.智能无人系统教育部工程研究中心，济南 250061）

1 引 言

机器人在信息化时代的大量应用成为了当下生产效率提高的重要原因，移动机器人的广泛应用解放了大量劳动密集型产业，有效提高了工农业生产效率；同时其在高危、高温、水下等环境的应用，提高了人类面对复杂危险环境的应对能力。为了提高复杂环境下机器人的应对能力，同时满足越发复杂的任务需求，现主要有两种解决方案，一种是提高单机器人的感知能力与执行能力，通过加装更多的传感器与执行机构实现单机器人对复杂环境的适应能力；另一种是通过多机器人协同编组，将多个低成本的机器人编组成一个机器人团队，分别携带不同的传感器与执行机构，组合起来能够执行更加多样化的分布式任务，并提高任务执行效率，同时无线通信模块配置升级为多机器人协同编组控制面对复杂环境、执行复杂任务提供了更高的实时性与更好的鲁棒性［1］。多机器人协同编组相对于单机器人面对危险环境时具有更好的抗干扰、抗故障能力，其执行效率高、执行任务多样且对环境的适应性更好，可以实现“1+1>2”的效果。因此，多机器人协同编组控制成为了当下机器人发展的必要一环。

移动机器人控制技术的一个重要前提就是获取移动机器人每个时间的精确位姿，即移动机器人定位问题，移动机器人的定位方法主要有GPS卫星定位法、惯性导航定位法、视觉靶标定位法［2］。协同编组的多移动机器人的精确定位更是每个移动机器人模块完成相关分布式任务的基础，通过结合多样的定位传感器，即可实现从全局到局部的协同定位。多移动机器人编组定位方法多通过协同定位的方法，获取部分移动机器人模块的全局位姿以及其他机器人模块相对其的位姿信息，进而将各个机器人位姿展示在全局地图中。相对于单移动机器人，多移动机器人可以搭载更加多样的传感器，具有更好的环境感知能力与信息处理能力，复杂环境下的多机器人协同定位具有更好的定位鲁棒性。

多移动机器人协同控制技术的重点就是协同定位，即获取多移动机器人模块间的位姿，机器人定位技术可分为绝对式定位技术和相对式定位技术［3-4］。绝对式定位技术计算机器人在环境中的实时绝对位姿，不受机器人初始状态与历史状态影响，常用的定位技术有无线电定位、声波定位、环境辅助定位等方式，其中无线电定位与声波定位的定位精度较低，不适合近距离高精度的定位，环境辅助定位精度较高但计算量大且对环境要求较高。相对式定位技术则分为依靠初始状态与历史状态对当前位姿进行估算的本地测量，以及借助某些特征直接测定团队中其他机器人位姿的外感测量，常用的机器人相对定位技术有惯性导航定位技术（本地测量）和视觉测量定位技术（外感测量），惯性导航定位技术（加速度计、里程计、陀螺仪）定位精度较高但存在累计误差和漂移［5］，视觉测量定位技术（模板匹配［6］、特征识别［7］）定位精度更高但距离较近，受光照与视角影响。多机器人协同运作需要在靠近时获取高精度的相对位姿，故选取相对式定位技术来监测移动机器人的位姿信息。

如上所述，惯性导航定位系统与视觉测量定位技术存在互补的特性［8］，通过对两种传感器信息进行融合，可以解决视觉定位的丢帧等随机误差影响，并避免惯性导航系统的累计误差影响，可以获得精度更高、鲁棒性更好的相对位姿监测方式。惯性测量定位系统的精确短期估计与视觉监测系统的丰富环境感知是公认的互补定位方式，具有很好的应用研究前景。现有许多基于惯性测量单元配合视觉监测系统组成视觉惯性里程计进行定位估算的研究：Fu等采用RGBD相机和惯性测量单元相结合的方法对运动物体和静态物体进行分离，提高了SLAM系统在动态场景中的定位精度和适应性［9］；Schubert等提出了一种视觉配合惯性传感器的时间同步方法［10］；Leutenegger等提出了一种针对非线性系统的视觉惯性导航的算法［11］；同时也有针对多机器人的视觉惯性定位系统［12-13］。

现有的融合方法大多采用IMU等惯性传感器与RGBD或者鱼眼相机等视觉传感器。然而在实际工程应用中，IMU等惯性传感器较容易受到环境磁场的影响，导致定位精度的降低，为了减少环境干扰对惯性定位的影响，本文采用车辆里程计作为惯性输入量，误差较易进行估算。对于视觉传感器来说，RGBD相机成本较高且监测视野较小，鱼眼相机则需要进行高精度标定才能等效理想帧相机［14］，为了保证视觉系统能够实现全角度的监测，使相机能够监测全角度的移动机器人，同时减少计算量、提高解算速率，使用单目相机组成全角度的监测系统具有搭建简单、计算量小、成本更低的特点，故本实验选用4个单目相机组成全角度的视觉相对位姿监测系统。

针对多机器人多传感器数据融合问题，卡尔曼滤波及其衍生算法已经得到了广泛的研究［15-17］，通过搭建移动机器人先验模型对多传感器数据进行融合滤波优化。本文利用车辆里程计的信息对视觉定位进行优化，针对阿克曼转向的车辆模型［18］，搭建了阿克曼转向机器人先验模型，其状态方程中线速度与角速度由车辆里程计反馈得到，视觉系统的相对位姿作为系统观测值。对此非线性系统，采用扩展卡尔曼滤波算法，对视觉传感器与车辆里程计器信息进行融合优化，融合了两种传感器的互补特性，进一步提高了相对位姿监测精度，并且消除了丢帧与系统累计误差，提高了系统定位鲁棒性。

2 位姿监测系统

相对位姿监测系统的首要任务是通过视觉传感器与车辆里程计获取移动机器人之间的定位信息。为了能够在全方位监测到其他移动机器人，获取相对的位姿信息，在移动机器人上方搭建4个方向上的4个单目相机以实现移动机器人的全方位位姿监视，单目相机及其识别靶标在移动机器人上的安装位置如图1所示，其中识别靶标处于相机的正下方。

图1 机器人传感器安装示意图Fig.1 Schematic diagram of robot sensor installation

单目相机的监测范围与相机的视角有关，本实验相机采用4个方向正方形排列的视角为120°广角的USB单目相机，移动机器人为558 mm×492 mm×420 mm阿克曼转向机器人，单目相机安装示意图如图2所示。

图2 单目相机监视角度示意图Fig.2 Monocular camera surveillance angle diagram

式（1）为相机监测范围计算公式，考虑到车身尺寸对视觉监测的影响，同时为了保证单目相机能够实现全角度的位姿监测，经计算，以车辆中心为相机安装中心的最长安装距离a为208 mm，本文选取单目相机距离为200 mm。

本实验选用二维码靶标作为车辆识别靶标进行目标的识别，二维码靶标包含多个容易识别的特征。通过计算可以获取相机与二维码之间的相对位姿信息，并识别二维码编码信息，即可同时获取移动机器人的相对位姿及编号，具体监测步骤如下：

（1）筛选图像中二维码的轮廓，确定二维码靶标位置；

（2）监测二维码标记，识别二维码信息；

（3）结合相机内外参数以及二维码参数，计算相机姿态；

（4）重复监测二维码并计算更新相机位姿；

（5）输出相机相对于二维码靶标的相对位姿。

通过对二维码角点的PnP解算即可获得移动机器人之间的相对位姿信息(x，y，z；α，β，γ)，本实验为室内环境下相对位姿监测，可以忽略Z轴方向上的位姿变化，同时提高优化算法速度，提高系统实时性。简化后的相对位姿信息可表示为(x，y， 0；0，0，γ)。此外，考虑到相机以及靶标相对机器人中心的相对距离(△x，△y)以及误差偏角△γ，实际相对位姿可表示为(x+△x，y+△y， 0；0，0，γ+△γ)，式（2）为机器人中心相对位姿坐标变换方程，可实现机器人的协同定位。

对于阿克曼转向车辆，通过对前进电机与转向电机前进里程以及转角信息进行解算，即可得到车辆每个轮子的角速度与线速度，通过CAN总线与车载工控机连接起来，即可得到阿克曼转向车辆里程计的系统输入信息。车辆里程计信息有4个轮子线速度(vf1，vf2，vr1，vr2)与4个转向电机的转向角度(φf1，φf2，φr1，φr2)，经过解算作为系统状态方程的控制量。由于车辆编码器受车辆滑动等的影响，需要结合视觉监测系统进行融合优化。

3 模型搭建与优化

单目视觉监测移动靶标的相对位姿存在着环境干扰，且视觉方法容易受到环境光线等因素干扰；车辆里程计常常也受路面环境影响存在计算误差，其误差可以认为服从高斯分布，为了消除环境干扰对系统相对位姿监测的影响，本文提出一种基于扩展卡尔曼滤波的方法对视觉定位系统以及车辆里程计定位系统进行优化处理，得到更加准确的且鲁棒性更高的相对位姿估算方法。

3.1 阿克曼转向模型

对于阿克曼转向模型的车辆，不考虑车辆在Z轴方向的运动时，可以简化为平面模型，其线速度与转角信息可以通过车辆里程计反馈得到，并对四轮的线速度与角速度进行简化处理，阿克曼简化转向模型如图3所示。

图3 阿克曼简化转向模型Fig.3 Ackerman simplified steering model

其中(x，y)表示车辆中心相对于惯性坐标系的位置信息；γ表示车辆相对于惯性参考系X轴的夹角，即车辆的航向角；φf为前轮转角，φr为后轮转角，vf为前轮转速，vr为后轮转速；O为车辆转弯圆心，R表示车辆的转弯半径；v表示车辆线速度，β为车辆滑移角，本实验使用简化模型，低速运动状态下默认车辆滑移为0。

阿克曼转向模型的车辆转弯半径是重要的参数，车辆的转弯半径与前后轮的转角以及车身前后轮轴之间的距离le有关，le=2lr，式（3）给出了车辆转弯半径的计算方法，Rf表示前轮转弯半径。

车辆速度较低时，可以假设车辆的方向变化率等于车辆的角速度，故车辆的角速度可表示为

至此，获得了车辆的角速度与线速度反馈信息，可以给出车辆相对于惯性坐标系的线速度与角速度公式

式中，v表示车辆质心处的线速度，vx，vy分别表示车辆相对于惯性坐标系的横向和纵向线速度，ω=ωf为车辆相对惯性坐标系的角速度，vf、vr、φf、φr为通过CAN总线获取并解算的实时车辆状态信息，作为车辆的输入量，搭建出阿克曼转向车辆的数学模型。

由于本文关注多移动机器人相对位姿监测，需要关注机器人相对位姿，故将单一机器人位姿[xyθ]T，即平面位移与角度作为状态方程状态量，将式（5）中的惯性坐标系中速度信息转换到相对于惯性坐标系的位姿信息，并将位姿信息方程离散化，如式（6）所示，此即为对于本实验所用的阿克曼转向模型的状态方程。

由于本文通过单目视觉监视车辆靶标的方式测量车辆的相对位姿，其观测量选取为车辆的相对位姿，故系统的观测方程可以表示为式（7），其中(x′k，y′k，θ′k)表示为前车k时刻在惯性坐标系的位姿，(x″k，y″k，θ″k)表示为后车k时刻在惯性坐标系的位姿。

同时由于依靠车辆编码器获取的车轮运动转角信息受道路环境影响存在系统噪声，表示为wk，该噪声服从高斯分布P(w)～N(0，Q)；同时环境干扰也会对视觉观测的车辆定位造成影响，其随机误差干扰可以表示为vk，同样服从高斯分布P(v)～N(0，R)，故在考虑系统噪声的情况下，实际状态方程可以表示为式（8）。其中，系统控制量表示为Xk=[xk′yk′θk′x″ky″kθ″k]T，即为车辆状态传感器估算的前后车的位姿信息，系统观测量表示为Zk=[zxkzykzθk]，即为单目视觉测量的前后车辆相对位姿。

3.2 滤波优化

针对上节提到的相对位姿监测系统中存在的相对误差，由于该误差服从高斯分布，故可以通过卡尔曼滤波的方式对系统观测值进行优化估算。针对式（8）的非线性系统，本实验采用扩展卡尔曼滤波的方式对系统进行滤波优化，以减少环境变化对视觉监测的消极影响，提高系统的监测精度以及对环境的适应能力。为此将式（8）的非线性模型在处，即k-1时刻的状态估计值处进行线性化处理，得到线性化的状态方程为式（9）所示，其中

式中，A′k、A″k分别为k时刻每辆车的状态转移矩阵在处的雅克比矩阵，具体如式（10）所示。对于扩展卡尔曼算法，其系统矩阵与上一时刻的状态估计量X̂k-1有关，需要在每次迭代过后重新计算。Hk=[I-I]为k时刻每辆车的系统观测矩阵。

扩展卡尔曼滤波算法是一种递归算法，需要给出系统迭代所需的初始值，本文关注目标为前后车辆的相对位姿，同时为了简化计算，选取尾车的初始状态X″0为坐标原点，头车的初始状态X′0为单目视觉的初始监测相对位姿，系统误差方差矩阵为Q=0.09I6，R=[0.08 0.08 0.03]。选取卡尔曼滤波算法中系统的状态误差协方差矩阵初始值P0=I6。

扩展卡尔曼滤波可以分为预测与更新两个步骤，首先进行系统状态预测如式（11）所示，先假设不存在系统噪声，代入上一时刻的后验估计值，即可得到当前时刻的系统先验估计值

获取扩展卡尔曼滤波系统的预测结果后，需继续对预测结果进行更新，进而获取下一时刻的滤波优化的结果。首先对扩展卡尔曼滤波的卡尔曼增益进行更新，

获取系统当前时刻的卡尔曼增益后，即可对状态量与观测量进行数据融合，获得系统的下一时刻的状态估计后验值

最后获取系统下一时刻的后验状态估计误差协方差矩阵，并进入下一时刻的卡尔曼滤波递归。

本文所采用的相对位姿监测系统的系统框架如图4所示。相对位姿监测系统将视觉监测结果与车辆状态信息通过扩展卡尔曼进行融合滤波优化，将每个时刻的后验估计作为系统状态输出值，并依此获取车辆的相对位姿信息。

图4 系统框架图Fig.4 System framework diagram

至此实现了对视觉相对位姿监测信息以及车辆里程信息的数据融合，通过扩展卡尔曼滤波，对数据信息进行了优化分析，并对视觉监测系统中的系统误差进行了优化。

4 实验与数据分析

上节介绍了本实验中阿克曼转向模型相对位姿监测系统的视觉与车辆状态监测系统，以及针对视觉监测及里程计信息存在的系统误差进行融合优化处理的扩展卡尔曼滤波算法。本节将针对该算法进行实物实验，搭建实物平台并对实验数据进行分析。

针对上文对阿克曼转向模型车辆相对位姿监测系统的分析，搭建阿克曼转向实物模型如图5所示，在车辆上方架设固定4个方向的单目相机，并将二维码靶标设置在单目相机下方作为目标监测特征，监测目标靶标获取视觉的相对位姿，并对二维码信息进行识别，进而实现车辆识别。通过CAN总线实现工控机与车身控制器的连接，将车辆角度与线速度反馈信息进行解算，并获得相对位姿监测系统的输入信息。

图5 单机器人系统实物图（红色为单目相机位置，蓝色为定位靶标位置）Fig.5 Ackerman robot and the camera mounted on（monocular camera is marked in red and the positioning target is marked in blue）

通过监视其他移动机器人上的二维码靶标，获得相对其他机器人的相对位置与偏角作为系统观测值，将视觉监测数据与车辆状态传感数据对齐时间后进入扩展卡尔曼滤波优化系统进行融合优化分析，即可得到最终的相对位姿估算值，并将其坐标变换信息可视化如图6所示，其中car2_location表示通过视觉监测得到的相对位姿，car2表示经过融合滤波优化后的相对位姿。

图6 可视化系统相对位姿Fig.6 Visualization of the relative posture

为了进一步判断相对位姿监测系统效果，本文设计了基于全局视野下的位姿判定。在室内环境下搭建全局相机视野如图7所示，通过使用全局相机监测移动平台顶部二维码靶标，计算移动机器人位姿信息；将其作为系统的位姿变化实际值，对滤波优化后的相对位姿监测效果进行分析评估。

图7 全局位姿监测Fig.7 Global posture monitoring

将车辆的状态传感器信息、视觉监测系统融合优化后的位姿信息作为基于视觉的相对位姿监测系统的输出值，全局视野下的车辆位姿信息作为参照值，将两个数据通过MATLAB展示出来。图8展示了双车系统在监测中的位置变化过程，图9、图10展示了双车系统角度变化，即系统估算值与全局相机位姿参照值比较。

图8 机器人位置变化Fig.8 Robot position change diagram

图9 前置机器人角度变化Fig.9 Front robot angle change diagram

图10 后置机器人角度变化Fig.10 Rear robot angle change diagram

将双车系统中每辆车的位姿代入观测方程中，即可得到双车的相对位姿，双车相对位置变化如图11所示，双车相对角度变化如图12所示。

图11 机器人相对位置变化Fig.11 Diagram of relative position change of robot

图12 机器人相对角度变化Fig.12 Diagram of relative angle change of robot

通过在MATLAB中对相对位姿监测系统的双机器人位姿信息进行比较分析，经扩展卡尔曼滤波算法融合两类传感器数据后的相对位姿对比全局相机观测到的相对位姿参考值，其位置误差标准差为6.02 cm，角度误差标准差为8.42°。经扩展卡尔曼融合滤波后的检测精度相对直接测量综合提升了20%～25%；同时系统输出值稳定平滑，有效减少了环境误差的随机影响，提高了系统检测鲁棒性。

5 结 论

本文提出了一种针对多移动机器人的相对位姿定位方法，通过融合视觉监测与车辆状态反馈的互补特性，弥补了视觉监测定位易受环境影响导致随机误差以及车辆状态反馈定位存在累计误差的不足。将两类传感器信息作为观测量与输入量通过扩展卡尔曼滤波进行优化，基于阿克曼模型对系统位姿做出后验估计，减少传感测量的系统误差。

本实验搭建实物样机进行实验并搭建全局的视觉定位监测，通过对实验数据进行分析，验证了本文提出的基于视觉的相对位姿监测系统相比于单一的传感器定位，定位精度更高且定位鲁棒性更好，受环境影响更小。

本文提出的基于视觉的机器人相对定位系统，通过结合多种传感器实现了更加精确的相对定位效果，针对多机器人协同定位，还面临着一些挑战：

（1）对于多机器人的协同定位，如何将现有的双车系统扩展为多车系统，并在直接视野丢失时通过多机器人间的相对定位对整体相对位姿做出整合估算还需进一步研究。

（2）本实验传感器误差的选取为固定的高斯误差，在机器人定位时视觉与车辆状态误差常与环境光线与路面状况有关，如何根据系统环境变化自适应设置系统误差对机器人协同定位具有重要研究价值。