-
随着机器人应用领域的不断扩大，对其自身定位的精度要求越来越高[1-11].针对机器人在导航时对精确定位的要求，本文提出了一种融合的自定位算法.利用扩展卡尔曼滤波(EKF)算法将里程计、单目视觉与激光雷达信息相融合，最后对机器人的位置进行修正，得到精确的位置估计.
HTML
-
在机器人导航领域得以广泛应用的是航位推算定位原理(DR)[12].该方法将机器人运动视为在固定平面区域内二维方向上的运动，在机器主体的初始位置与位移-时间曲线既定的情况下，按照位移矢量累加的理论和方式对机器人方位进行精确判定.
本文选用全局参考坐标系进行相应的研究，使用位置坐标(x，y)对其位置进行确定(图 1).
对于初始时刻t0，设定此时对应的位置是(x0，y0)，对于tn(n>0)，其位置坐标(xn，yn)求解公式为：
式中：θi是ti时刻机器人的航向角；di是ti到ti+1时刻机器人行驶过的距离；Δθi是ti到ti+1时刻机器人航向的变化量.当采样周期恒定时，则式(1)-(3)可表示为：
其中：T表示采样周期，b和r分别表示机器人左右轮的间距大小和轮半径，ωli表示左驱动轮转速，ωri表示右驱动轮转速.若采样周期T很小，则在每个采样周期内，可认为ωli，ωri是常量.
-
摄像机的角度测量原理如图 2所示. A，B和O分别表示两个目标点和光心.根据A，B在摄像机中的成像可以计算出AOB之间的夹角.
这里，设Sa，Sb分别表示光心A，B在图像平面的投影和So光心O在图像平面的投影，那么：
该角度余弦值的准确性只与图像分辨率有关.本文假设视觉噪声服从高斯分布.
-
设环境特征i到传感器的距离和方向分别为ρi(k)和φi(k)，扫描数据可表示为：
其中Zi为激光测量距离，这里设定激光雷达测量值能够受到噪声的影响，同时符合高斯白噪声的基本特征.
-
机器人在初始两个时刻的位置由里程计求得.在不同时刻，机器人对摄像头与任意两个环境特征点之间的夹角变换进行计算，由激光雷达获得周围环境密集准确的距离和角度数据，通过激光雷达和视觉传感器的特征配对，得到标志点的坐标，如图 3所示.
1.1. 里程计航位推算
1.2. 视觉测量
1.3. 激光测量
1.4. 位置修正
-
卡尔曼滤波是一种递推线性最小方差估计技术，目前已发展为信息处理体系的关键理论[12].对于机器人的精确定位问题，该方法在构建恰当的概率模型的基础上，能够对机器人的当前状态与对应参量进行较为理想地预估和设定.由于系统是非线性模型，本文采用扩展卡尔曼滤波算法.
首先，要对机器人的方位坐标进行确定，需要构建机器人局部参考坐标系ORXRYR与平面全局参考坐标系OXY，将ORXRYR的原点设置于两驱动轮轴心连线的中心位置OR，同时将此点当成机器人方位参考点来看待，交汇在点OR的对应轴取为XR与YR.
-
考虑到机器人结构庞大的实际特点，获得较为理想的实验效果就需要对机器人主体结构进行必要的简化.这里选用具有两轮结构的驱动型小车当成实验平台，对应的模型见图 4.
在OXY内，OR的具体方位由x与y进行确定，参考系OXY与ORXRYR两者的角度差根据θ大小进行定义.此时，完全能够使用向量[x，y，θ]T对机器人的位置进行准确定位.在图 4所示机器人模型中，b和r分别表示左右驱动轮的间距和轮半径，ωl，ωr分别表示左右两轮的角速度，OR点对应的速度计算公式是：
在投影至OXY内，可以得到：
在该时刻，机器人角速度大小为：
由此，得出机器人的运动方程为：
按照一定的规则对方程(8)做极坐标与直角坐标的转化处理，xi=ρicosθ，yi=ρisinθ，同时掺合相应的模型噪声，整理形成机器人对应的空间状态方程：
机器人的位置坐标可用式(15)来表达，w(k)表示模型误差大小，对应为零均值条件下的高斯白噪声，对应的方差大小是Q(k).
结合式(4)，(5)，(6)可以得出f(X(k))的具体表达式：
-
根据观测模型，可以采用式(17)来描述传感器数据与机器人位置之间的关系：
在全局坐标系中，环境特征i的坐标记为Xi=(xi，yi)，v(k)表示零均值高斯噪声.根据激光雷达所观测和捕获的数据信息，机器人对主体位置进行校正处理. h (X(k)，Xi)表示主体位置X(k)与目标位置Xi的逻辑关系.选取并设定区域特征i与传感器的距离与方向分别为ρi(k)和φi(k)，采用观测模型将传感器坐标系与机器人坐标系合二为一，那么可以得出测量函数：
不难发现，此项测量函数具备非线性基本特征，需要在经过线性化处理之后，才能按照EKF算法做相应的运算. h(X(k)，Xi)在X(k)处的Jacobian矩阵为：
视觉传感器的观测方程：
就是由机器人位置坐标到各个特征点的向量之间的夹角余弦组成的向量.在估计的位置得到估计的观测量，通过这个量与实际观测值(摄像机直接测量)之间的差别来修正估计的位置.
-
作为数据库技术的关键内容，近年来数据关联技术发展迅速.对于一般的EKF算法来说，该方法能够对实际观测和此时的环境特点建立较为准确的关联.在出现某观测和相应区域环境特征无法关联的情况下，会出现滤波器发散的不良现象，这将导致后续的预测操作不准确，从而不能进行恰当的定位操作.
本文选用最近邻关联算法建立实际观测和预测观测两者的内在关系，并通过求解此时的新息与协方差数值，获得卡尔曼增益的大小，在此基础上准确校正先验机器人方位，获得较为准确的后验定位结果.
-
融合里程计、单目视觉与激光雷达信息的EKF定位流程图如图 5所示，定位算法执行步骤如下所示：
1) 对机器人对应的误差协方差矩阵P0与状态向量
${\mathop { \mathit{\boldsymbol{X}}}\limits^ \wedge} _0$ 进行初始化处理.2) 对其位置进行估测.
由k时刻的方位推测出k+1时刻对应的方位：
其中F表示f的Jacobian矩阵，P表示X的误差协方差矩阵，可得
3) 观测
从时刻k+1得到传感器观测值集合Z (k+1).观测值集合由不同传感器提取的单独观测值Zj(k+1)组成.
4) 观测预测
根据CCD观测到的夹角余弦向量、机器人的位置预估
$\mathop X\limits^ \wedge \left( {\frac{{k + 1}}{k}} \right)$ 和环境特征的位置Xi=(xi，yi)，应用测量函数产生环境特征的预测观测值：将实际值和预测值差值当做预测状态的检验参量，这就说明新息是：
对应的协方差矩阵为：
H是测量方程中h(X(k))的Jacobian矩阵.
5) 匹配
根据最近邻关联算法理论，对实际值与预测值实施匹配关联操作，可以求解出新息、协方差矩阵以及卡尔曼增益.
6) 纠正
算法对应的增益矩阵W进一步变化成：
此时机器主体的状态纠正为：
此时的协方差矩阵为：
7) 转入第二步继续按步骤2)-6)进行运算，直到达到收敛条件.
8) 得到最终位置估计.
2.1. 机器人运动模型
2.2. 传感器观测模型
2.3. 数据关联处理
2.4. 融合里程计、单目视觉与激光雷达信息的EKF定位算法
-
使用项目组自己研制的移动机器人在走廊环境中对定位算法进行实验测试.分别在有、无定位系统的条件下对定位准确度进行评价和对比，得出相应的实验结论.机器人运动控制应用了机器人导航方法.给定机器人的初始位置S，终点位置G.为检验机器人避障的能力，在其行走路线上，随机增加几个障碍物.在仅使用里程计的运动中，机器人主体单一按照里程计数据粗略预估自身方位.观察发现，图 6所示的机器人没有到达目标位置.
图 7中三角形标志点为人工设置用来对机器人路径情况做标记的标志点.
实验发现，应用了融合里程计、单目视觉和激光雷达信息的定位方法后，当机器人在多转角、长距离环境下，能够有效地消除里程计误差带来的干扰.机器人采用定位算法后行走的结果如图 7所示，可以看到，定位的结果非常理想，很好地解决了移动机器人的准确定位问题.
-
本文通过多传感器信息融合原理，创造性地把多种感测设备捕获的有效信息进行加工和处理，实现机器人自身的高精度定位与判定.在多转角、长距离的情况下可以取得满意的效果.但实验建立在地图中的标志点建立在人工提取和识别并成功匹配的基础上.因此，在环境条件动态未知，特征点需自行提取和识别的情况下，机器人的精确定位将是进一步研究的方向.
DownLoad: