基于动态和静态环境对象观测一致性约束的移动机器人多传感器标定优化方法

伍明， 张国良， 李琳琳， 付光远， 李承剑

(火箭军工程大学， 陕西 西安 710025)

基于动态和静态环境对象观测一致性约束的移动机器人多传感器标定优化方法

伍明， 张国良， 李琳琳， 付光远， 李承剑

(火箭军工程大学， 陕西 西安 710025)

为了解决机器人未知环境导航过程中的多源、异构传感器空间一致性观测问题，提出了基于动态和静态环境对象观测一致性约束的摄像机与激光测距传感器联合标定优化方法。利用协方差交集方法实现运动目标图像平面方向状态融合，同时采用卡尔曼滤波和概率数据关联滤波实现一对一和一对多信息源的静态角点特征图像平面方向状态融合；在此基础上，利用动态和静态物体融合前与融合后状态误差构造优化目标函数，并利用非线性优化方法实现标定参数优化。实验结果表明，该设计方法能够提高多传感器环境观测的一致性水平，验证了该方法的有效性。

控制科学与技术； 机器人同时定位与地图构建； 目标跟踪； 多传感器标定； 多传感器信息融合

0 引言

多传感器信息融合是提高移动机器人感知能力的有效途径，对于该领域的研究已成为机器人学界的关注热点。近几年，信息融合方法在机器人导航与控制[1-2]、机器人定位与环境构建[3-5]以及机器人目标跟踪[6]等方面均得到了成功应用。

多传感器信息融合在移动机器人领域的应用首先需要解决异构传感器时间、空间一致性观测问题，时间一致性指的是不同传感器观测数据在时间序列上的同步，空间一致性指的是不同传感器在空间维度及尺度上的对准。

针对单平台多传感器时间一致性问题已经出现了较为成熟的研究成果[7]。多传感器空间一致性观测要解决传感器局部坐标系间的联合标定问题，其解决思路是基于特定空间标定物上特征间的几何约束关系，利用摄像机与激光测距仪观测值产生优化函数并进行参数优化，进而得到坐标系间的旋转和平移参数。这些对象特征包括角点[8]和直线特征[9]或人为设置的激光反射条带[10]，而选用的空间标定物通常为人工制作的立体标定物(标定方块)[11]或平面标定物(标定板)[12]。这些方法存在的主要问题是：人工成分过多，原始优化数据有限，从而增大了初始估计误差。首先，无论是空间标定物还是标定物上的特征均为人工设计制造，标定物和特征本身的构造准确性直接影响标定结果；其次，标定过程非在线完成，标定物的空间摆放位置需要人工放置，费时费力，并且位姿分布有限，造成优化数据信息覆盖不全面，影响标定准确性。文献[13]设计了一种在线自标定方法但其主要标定对象为同构的多摄像机系统而并非异构的摄像机- 激光测距仪系统。尽管传感器标定的重点在于参数估计的准确性，并且标定物的人为设置在所难免，但在系统运行过程中传感器的初始位姿会随时间发生变化从而引入标定误差，因此研究机器人在线运行过程中的标定参数优化方法具有实际意义。

针对多传感器联合标定初始误差问题，本文提出基于运动和静止环境对象观测一致性约束的多传感器初始参数标定误差优化方法。方法利用不同传感器对环境物体融合前后的观测残差构造优化函数，在移动机器人运行过程中对初始参数标定误差进行在线优化。该方法人工干预程度较少，标定物为机器人运动环境中的运动和静止物体，标定优化在线运行，能够改进多传感器标定初始误差和参数漂移问题。另外，为了提高运动物体识别准确性对运动目标进行了人工颜色标记，同时采用环境垂直作为静止物体特征并通过限制识别区域和多线段特征融合方法以实现静止标定参考物的可靠获取。

本文首先介绍研究问题和系统处理流程；介绍不同传感器运动和静止对象图像平面观测值获取方法并设计了基于多传感器的运动和静止环境对象图像平面投影状态融合方法；在此基础上，提出了基于多源信息融合的运动和静止物体图像平面投影方向分量观测残差优化函数，并通过非线性优化方法实现摄像机与激光测距仪标定参数的优化；最后，针对提出方法进行了实验验证并分析了在线参数优化的效果。

1 问题描述及系统处理流程

假设激光扫描仪坐标系下的某扫描点状态为PL=[xL,yL,zL]T，该点在摄像机坐标系下的状态为PC=[xC,yC,zC]T，则存在：

PC=Φ(PL-Δ)，

(1)

式中：Φ为摄像机坐标系相对于扫描仪坐标系的标准正交旋转矩阵；Δ为摄像机坐标系相对于扫描仪坐标系的位移向量。

摄像机与激光扫描仪参数标定的目的就是确定Φ和Δ，使得多传感器的观测值之间建立起空间一致性关系。文献[9]通过求解标定物特征几何约束算式，能够实现对Φ和Δ参数的初步估计，但是由于标定物本身结构误差和位姿覆盖不全问题，使得Φ和Δ存在初始估计误差。系统将利用移动机器人运行过程中的动态和静态环境对象状态图像投影方向分量误差构造优化函数，以优化参数Φ和Δ中相关分量的估计误差。系统总体处理流程如图1所示。

图1 系统处理流程图Fig.1 Flow chart of system processing

系统处理过程主要分为实时处理和标定参数优化处理两部分。

标定参数优化处理部分：利用一段时间内获得的所有优化数据组成目标函数，通过非线性优化Levenberg-Marquardt方法，求解优化值Φo、Δo.

2 不同传感器运动和静止环境物体图像平面观测值获取

2.1 基于栅格地图同一性检验的激光传感器运动物体侦测

本文采用文献[14]提出的基于“同一性原则”的移动物体扫描点检验方法获取环境动态对象产生的激光反射点，此处不再赘述。

2.2 激光传感器环境静态角点侦测

通过计算某个扫描点相对前后两个邻近扫描点的深度梯度值，当梯度值大于某个门限时就认为该扫描点对应环境角点特征，某点的深度梯度值为

Gi=-0.5ri-1+0.5ri+1,

(2)

式中：Gi为第i个扫描点的深度梯度值；ri-1和ri+1为第i个扫描点前后相邻扫描点的深度值。这样处理存在的问题是当第i个扫描点为单一突出扫描点时，该方法无法完成判定，如图2所示。

图2 单一突出扫描点判定示意图Fig.2 Judgement of single outstanding scanning point

从图2中可知，当扫描点Pi为单一突出点时，利用(2)式的计算结果为0，此时无法有效判定Pi为角点。为了克服该问题，将扫描点Pi深度梯度值计算表示为

Gi=|0.5ri-0.5ri-1|+|0.5ri+1-0.5ri|，

(3)

也就是将深度梯度表示为Pi与前点和后点的梯度绝对值之和。利用(3)式进行角点判断时，对应深度边界的连续两个扫描点都会被认定为角点，而在融合过程中系统只保留相对靠前的角点作为融合角点。另外，若角点中包含运动物体扫描点则需将它们滤除。经过以上处理得到的角点分布如图3所示。

图3 角点提取结果图Fig.3 Extracted results of corner points

2.3 激光扫描点图像平面投影及不确定范围计算

动态和静态物体状态融合是在图像平面坐标系中完成的，因此，需要将激光测距仪坐标系下的扫描点投影至图像像素平面坐标系中，同时，利用误差传播公式计算其在像素平面坐标系中的不确定范围。

扫描点图像平面投影过程为：1)利用(1)式完成激光坐标系到摄像机坐标系的转换；2)利用摄像机小孔成像模型[15]完成摄像机坐标系到图像像素平面坐标系的转换。将以上转换过程表示为函数：

Y=f(X)，

(4)

式中：Y=[uv]T表示激光点在图像上的像素坐标值；

(5)

P2×1,L=[rLθL]T为扫描点在激光测距仪局部坐标系中的距离rL和角度θL状态，f2×1为摄像机的像素焦距向量，k5×1为摄像机的镜头畸变参数，c2×1为像平面中心坐标，R3×1是与Φ对应的激光扫描仪至摄像机坐标系的旋转向量，其计算过程参见文献[16].

设X的误差阵形式为

ΣX=diag[ΣPL,ΣΔ,ΣR,Σf,Σc,Σk]17×17，

(6)

式中：Σ•为相关子分量的误差阵。由映射函数Y=f(X)确定的图像投影点不确定性范围为

(7)

式中：Jf为Y=f(X)对不同变量的雅可比阵。

2.4 基于摄像机的运动目标跟踪

本文利用文献[17-18]提出的Camshift方法计算目标状态Xt_c=[ut_cvt_c]T及其误差阵ΣXt_c=diag[(dut_c)2(dvt_c)2]，此处不再详细介绍。

2.5 基于摄像机信息的环境垂线特征提取

基于摄像机信息的环境垂线特征提取流程如图4所示。

图4 基于摄像机信息的环境垂线特征提取流程Fig.4 Flow chart of vertical line feature extraction based on camera information

具体流程如下：

1) 在“垂线特征提取区域确定”环节，利用3.2节和3.3节介绍的方法根据角点在图像平面上的不确定分布确定垂线特征提取范围。这样处理的原因有两点：首先，由激光扫描点得到的角点特征在图像中大多对应着边缘特征；其次，为了减少计算量以提高系统实时执行能力。

2) 在“图像边缘检测”环节，由于系统只利用到图像中的垂直线段，因此，对传统Canny算法进行修改，将像素点(m,n)的梯度幅度值M(m,n)表示为

(8)

也就是说只利用x方向的偏导数Px(i,j)来代表像素点梯度大小，采用该方法的处理效果如图5所示。从图5可见，右图能够较为精确地检验出图像中的垂直边缘二值像点。

图5 基于Canny算子的垂直边缘检测效果图Fig.5 Vertical line extraction based on Canny operator

3) 在“边缘像素聚类”环节，首先对边缘二值图像进行“细化”处理，在保持图像边缘连接状态的同时使边缘成为单像素的线条；之后通过定位边缘“交汇点”和“尽头点”来确定图像中的独立边缘个数并且找到不同边缘与边缘像素的对应关系。

4) 在“线段边界点生成”环节，利用分裂与合并法[19]实现线段的初步提取。

5) 在“线段过滤”环节。一方面，为了进一步减小系统处理数据量，将只保留长度大于某一阈值的直线特征。另一方面，系统只利用与地面垂直的直线特征，因此，对直线特征再进行一次过滤以滤除倾斜角小于某一固定值Aa或大于180°-Aa的直线。

6) 在“线段合并”环节，通过计算不同直线特征状态之间的马式距离以确定直线之间的对应关系，对属于相同直线特征的所有检测直线进行合并得到最终的环境垂直直线特征状态。

图像垂直直线特征提取综合处理结果如图6所示。

图6 图像垂直直线特征提取效果图Fig.6 Results of vertical line feature extraction in image

图6(a)显示了某一时刻图像垂直直线特征提取效果，长度阈值为Al=12，角度阈值为Aa=85°. 黄色虚线长方形为边缘检测区域，黄色十字点为角点在图像平面上的投影点。图6(b)和图6(c)为图6(a)中区域A，区域B的局部放大图，彩色直线为对应区域中提取的线段特征，白色直线为线段合并后得到的垂直直线特征，共提取3条环境直线特征，其中两条为房屋边缘产生，一条为背景噪声产生(对应背景树丛)。

3 环境动态和静态物体图像平面投影状态融合方法

3.1 运动物体状态融合方法

运动物体观测状态融合采用协方差交集(CI)方法[20]，采用该方法的原因在于融合过程将来自激光传感器和摄像机传感器的运动物体观测信息同等考虑，认为这两种传感器提供的信息量相同，融合过程并不偏重于任何一方。另外，目标状态融合需要针对不同传感器观测值描述的相同性质状态分量进行，对于摄像机和激光扫描仪而言，它们所描述的相同状态分量为目标方向状态，因此，在图像像素坐标系中，只对u分量值进行融合处理。

(9)

ΣPt_s=diag(Pn-P1)+(ΣPn+ΣP1)，

(10)

式中：ut_s、dut_s为动点检测目标状态的u分量值和误差范围

根据协方差交集状态融合公式[14]，可知u分量CI融合过程为

ut_f=dut_f(ω(dut_c)-1ut_c+(1-ω)(dut_s)-1ut_s)，

(11)

dut_f=(ω(dut_c)-1+(1-ω)(dut_s)-1)-1，

(12)

式中：ω∈[0,1]为融合系数，为了体现多传感器信息对等性，此处将融合系数设为0.5(此时CI融合对等于信息滤波器融合) 。

3.2 静止垂线特征状态融合方法

机器人的运行背景环境复杂，当进行环境垂线特征提取时，尽管采用了限制提取范围的手段，但仍有可能产生伪观测值(如图6中垂线特征3就是在提取房屋右侧边缘垂线时产生的伪观测值)。相对来说，激光角点判定更为可靠，因此环境垂线特征状态融合以激光角点图像平面投影状态为主，采用基于卡尔曼滤波框架的融合方法实现。在该过程中，针对每一个投影到图像平面中的角点进行融合，当在角点投影区域内检测到一条垂线时利用卡尔曼滤波实现融合，当检测到多条垂线时则利用概率数据关联实现融合。

3.2.1 基于卡尔曼滤波的一对一信息源融合方法

利用卡尔曼滤波方法对一对一信息源进行融合，可以将一个信息源当做对象状态模型看待，将另一个信息源当做对象观测模型看待。这里以激光扫描仪信息为状态模型，以图像信息为观测模型，而此时直线特征的状态仅在图像坐标系的x轴方向进行，则系统状态模型为

(13)

(13)式为激光扫描仪观测值对应的特征状态函数，因此，对应的协方差预测阵为

(14)

而激光扫描仪的观测模型为

(15)

假设观测误差阵为Sk，根据卡尔曼滤波方法可知，更新后的特征状态和方差为

(16)

(17)

式中：Kk为卡尔曼增益，

(18)

式中：Hk为系统的观测阵。

结合(15)式，将摄像机图像提取的特征状态当成系统实际观测值，则系统观测残差可表示为

(19)

即

(20)

由(15)式可知，Hk为1，而系统观测协方差阵为

(21)

式中：观测误差阵Rc对应摄像机图像提取的特征误差阵。结合(14)式、(21)式可写为

(22)

将(18)式、(20)式和(22)式代入(16)式和(17)式可得到基于卡尔曼滤波的融合公式，即

(23)

(24)

3.2.2 基于概率数据关联的一对多信息源融合方法

概率数据关联(PDA)方法[21]利用同一对象的多个观测值展开滤波，并假设系统观测模型相同，对于一对多信息源数据融合来说，不同信息源的观测模型不同，利用PDA进行异构多源信息融合的处理流程如图7所示。

图7 基于PDA的一对多信息融合处理流程Fig.7 Flow chart of information fusion based on PDA

定义符号如下：

1)xk表示在k时刻对象主信息源状态估计值；

2)pk表示在k时刻对象主信息源状态误差阵；

3)XOk={xok,1,xok,2,…,xok,mk}表示在k时刻对象多信息源状态估计值集合，并且xok,i为k时刻第i个从信息源对象的状态估计值，mk为k时刻落入检验门限范围内的对象状态估计个数，即从信息源个数；

4)POk={pok,1,pok,2,…,pok,mk}为k时刻对象多信息源状态方差阵集合；

5)xfk表示在k时刻经过PDA融合处理后对象状态估计值；

6)pfk表示在k时刻经过PDA融合处理后对象状态误差阵。

基于PDA的多源信息融合就是利用XOk和POk对xk和pk进行加权修正以计算xfk和pfk，具体处理过程如下：

1)针对k时刻第i个信息源xok,i，计算量测误差为

vk,i=xok,i-xk,i=1,2,…,mk.

(25)

2)计算k时刻第i个信息源对应的系统观测误差阵为

Sk,i=Hkpk(Hk)T+pok,i.

(26)

此时系统的观测阵Hk为单位阵，因此(26)式可写为

Sk,i=pk+pok,i.

(27)

3)计算k时刻第i个信息源对应的残差为

(28)

(29)

假设信号源不正确的发生概率满足均匀分布， 当i=0时，说明所有通过检测门的信号源均不是对象的正确描述，此时有

(30)

式中：Vk,i为第i个信息源对应的检验门体积(由于不同信号源不确定范围不同造成了检验门体积Vk,i也不同)。

对于i=1,2,…,mk的不同情况有

(31)

(32)

式中：PD表示伪值通过检测门限的概率。

对于i=1,2,…,mk的不同情况有

(33)

综合以上分析，结合(30)式～(33)式得

(34)

式中：

(35)

而

(36)

式中：

fk,j=|Sk,j|-1/2ek,jVk,j.

(37)

5)k时刻第i个信息源对应的卡尔曼增益阵Kk,i为

Kk,i=pk(Hk)T(Sk,i)-1=pk(Sk,i)-1.

(38)

6) 计算此时对象状态融合值xfk为

(39)

7) 计算此时对象状态方差阵融合值pfk为

(40)

式中：

(41)

(42)

4 摄像机与激光测距仪传感器联合标定优化

标定参数优化思想为：利用不同时刻图像平面内融合前以及融合后的运动物体和环境垂线方向分量差值构造误差累积数据，并通过非线性优化方法求解初始标定参数的优化值。假设共跟踪了N帧图像，系统优化目标函数为

(43)

参数优化目标函数源于运动物体和静止物体在像素平面中的方向分量误差，因此，具体优化对象为摄像机与激光扫描仪联合标定参量Φ、Δ中同投影点方向分量生成相关的自变量。为了计算方便，重写(1)式为

PC=fPL→PC(PL,Δ,Φ)=Φ(PL-Δ)=ΦPL-ΦΔ.

(44)

设Δ′=-ΦΔ，则(44)式为

PC=ΦPL+Δ′.

(45)

根据摄像机小孔成像模型[15]可知，与图像投影状态方向分量u相关的Φ、Δ′分量为Φ11、Φ13、Δ′1，在构造误差函数时，将对这些分量进行优化，其余分量不变。

k时刻的马氏距离误差ek为

(46)

(47)

(48)

结合摄像机小孔成像模型及(45)式可知系统的最终目标函数为

(49)

(50)

5 实验与结果分析

5.1 实验方法

为了验证方法的有效性，本节分别从运动物体状态融合、环境垂线特征状态融合和摄像机与激光测距传感器联合标定优化3方面展开实验。实验在Matlab R2011a平台下进行，并利用Peynot等提供的实体机器人室外环境数据集[23]完成验证。实验在室外环境进行，行人始终保持在机器人观测范围内。机器人自身定位利用里程表数据加近邻点迭代方法完成。实验持续1 min，采集520帧图像并同步进行520次环境扫描。本文重点研究标定优化方法，因此在运动物体状态融合、环境垂线特征状态融合实验部分只对520次融合中有代表性的1次融合结果进行分析。另外，优化精度依赖于激光测距仪的角分辨率，数据集采用德国西克公司生产的SICK LMS221激光测距仪，其角度分辨率为0.25°，8 m观测范围内的点间误差为毫米级水平，满足优化要求。

5.2 实验结果分析

5.2.1 运动物体状态融合结果

图8为第15帧图像运动物体状态融合结果。图8中分别显示了ω分别为0.75、0.50、0.25时，经过协方差交集进行方向分量u值融合后的结果。图8(a)为不同ω值下方向分量u的概率p分布，图8(b)为不同ω值下目标在图像中的不确定范围。由图8可见：随着ω的减小，融合值逐步靠近动点检测得到的结果；相反，随着ω的增大，融合值逐步靠近Camshift的结果。在实际应用中取ω=0.5. 另外，从图8(b)中还可以看出，由于摄像机传感器在投影过程丢失了深度信息，因此融合方法只针对u值展开，导致融合后的洋红色椭圆长轴v值并未发生变化。

图8 第15帧图像运动物体方向信息融合结果图Fig.8 Result of bearing state fusion of moving object at 15th frame

5.2.2 环境垂线特征状态融合结果

当角点图像投影不确定区域只检验出一条直线时采用4.2.1节一对一信息源融合方法得到的融合结果如图9所示。

图9 第180帧图像一对一信息源角点方向状态卡尔曼滤波融合结果图Fig.9 Result of “one-to-one”Kalman filter fusion at 180th frame

图9为对第180帧图像的处理结果，图9(a)为融合前后角点分布投影区域变化情况，图9(b)为图9(a)白色虚线区域放大图。由图9(b)可见，由于初始标定误差使得融合前角点投影状态(黄色圆点处)远离了墙体右侧边缘，经过融合后角点投影状态(绿色圆点处)已经和墙体边缘重合。图9(c)为融合前后角点投影方向状态正态分布对比图。融合前后角点投影的方向分量方差并未发生太大变化(从融合前的37.5，变为融合后的36.3)，主要变化体现在均值上(从融合前的563.0，变为融合后的518.0)。而融合前后垂线的状态均值基本未发生变化(从融合前的520.0，变为融合后的518.0)，方差变化却较大(从融合前的1.5，变为融合后的36.3)。实际上，融合后角点特征和垂线特征融为一个新特征，该特征在保持原角点特征深度信息的同时提高了方向信息的精确性。

当角点投影区域检验出多条直线并且这些直线与角点的马氏距离小于判定门限时，采用4.2.2节一对多信息源融合方法得到的融合结果如图10所示。

图10 第14帧图像一对多信息源概率数据关联融合结果图Fig.10 Result of“one-to-many”PDA filter fusion at 14th frame

图10是对第14帧图像的处理结果。图10(a)为融合前后角点分布投影区域变化情况，图10(b)为图10(a)相应区域放大图。从图10(a)可知，在角点分布范围内(黄色虚线长方形范围内)共检测出两条直线特征(蓝色直线)，这两条垂线特征对应轿车门梁两侧边缘。融合前角点投影位置(黄色带圆点椭圆)由于初始标定参数误差影响，并未和轿车门梁匹配，经过PDA融合后角点投影位置(绿色带圆点椭圆)已经较好地与轿车门梁匹配。另外，从图10中可见，由于融合仅针对角点方向状态分量进行，因此融合前后椭圆v轴长度并未发生变化。图10(c)为融合前后角点投影方向状态正态分布对比图, 符号颜色含义同样与图9一致。图10(c)中融合前角点方向状态均值和方差为(1 247,43.73)，而两条垂线状态均值和方差分别为l1:(1 230,2)、l2:(1 245,1.5)，融合后新角点方向状态均值和方差为(1 237,7.6)。对比一对一信息源融合方法来说，PDA方法在对角点状态均值产生纠正作用的同时，对方差也产生了较大纠正作用。

5.2.3 摄像机与激光测距传感器联合标定优化结果

初始旋转与位移参量初始值分别为

设Levenberg-Marquardt方法的精度门限为10-4，对应优化迭代次数为23次，得到的旋转与位移参量优化值为

图11 摄像机与激光测距仪联合标定优化结果对比图Fig.11 Comparison of calibration optimization parameters and calibration initial parameters

利用优化前后标定参数分别对激光扫描点进行图像投影得到的第20帧、第179帧、 第231帧、第468帧结果对比如图11所示。

图11中红色圆点为运动物体扫描点在图像平面上的投影，蓝色圆点为静止物体扫描点在图像上的投影。从图11中可见，利用优化标定参数得到的红色圆点更贴近人体，而蓝色边缘扫描点和背景物体边缘也更加契合。说明此时观测一致性较好，多传感器坐标转换参数更精确。

图12 优化前后运动点和角点观测残差的变化曲线Fig.12 ||,|| error curves of moving and corner points before and after optimization

图13 1 000次蒙特卡洛实验优化前后参数分布Fig.13 Parameter distributions before and after optimization of 1 000 Monte Carlo tests

由图13可见，由于引入了人为参数噪声使初始参数分布较为分散，经过优化后，不同实验得到的参数均集中在初始优化值附近。另外，一些参数优化结果存在较大误差，分析原因在于，当初始参数误差过大时，在进行环境垂线特征检验时引入了较多错误检验值，利用这些错误垂线特征进行融合会引入更多误差，从而影响优化结果。由此可见，特征提取的准确性是设计方法的关键。

6 结论

基于多传感器信息融合的机器人感知要求实现多传感器系统的一致性观测。本文围绕摄像机和激光测距仪传感器空间一致性观测展开研究，完成了移动机器人动态环境中运动和静止环境特征观测值的精确获取和多传感器观测系统参数标定的在线优化。参数优化的参照物采用静态和动态两种环境特征，丰富了特征种类和来源，同时，误差控制屏蔽了噪声干扰。实验结果表明，该设计方法能够有效提高动态物体和静态特征的状态估计精度，优化摄像机和激光扫描仪之间坐标系转换参数，提高多传感器一致性观测水平，验证了利用动态和静态两种自然标定物进行多传感器在线标定优化的可行性和有效性。标定物的可靠提取是优化的关键，在未来研究中计划将标定物识别可信度引入到优化过程中，使该方法能够根据识别可信度大小确定不同标定物对参数优化的贡献程度，从而提高方法对标定物错误识别的纠错能力。

References)

[1] Zhang Y L, Hong D P. Navigation of mobile robot using low-cost GPS[J]. International Journal of Precision Engineering and Manufacturing, 2015, 16(4):847-860.

[2] Huang H C. An evolution optimal fussy system with information fusion of heterogeneous distributed computing and polar-space dynamic model for online motion control of Swedish redundant robot[J]. IEEE Transactions on Industrial Electronics, 2016, 9(9):1-10.

[3] Doki K, Suyama K, Funabora Y, et al. Robust localization for mobile robot by K-L divergence based sensor data fusion[C]∥Proceedings of 41st Annual Conference of the IEEE Industrial Electronics Society. Yokohama, Japan: IEEE, 2015:15753693.

[4] Zhou B, Qian K, Fang F. Multi-sensor fusion robust localization for indoor mobile robots based on a set-membership estimator[C]∥Proceedings of the IEEE International Conference on Cyber Technology in Automation, Control,and Intelligent Systems. Chicago, IL, US: IEEE, 2015:157-162.

[5] Machado J, Micael L. A sensor fusion layer to cope with reduced visibility in SLAM[J]. Journal of Intelligent and Robotic System, 2015, 80(3):34-49.

[6] Yuan J, Chen H, Sun F C. Multisensor information fusion for people tracking with a mobile robot: a particle filtering approach[J]. IEEE Transactions on Instrumentation and Measurement, 2015, 64(9): 2427-2442.

[7] 韩崇昭, 朱洪艳, 段战胜. 多源信息融合[M]. 北京: 清华大学出版社, 2006. HAN Chong-zhao, ZHU Hong-yan, DUAN Zhan-sheng. Milti-source information fusion[M]. Beijing: Tsinghua University Press, 2006.(in Chinese)

[8] Li L L, Zhao W C, Wu F, et al. Analysis and improvement of characteristic points extraction algorithms in camera calibration[J]. Acta Optica Sinica, 2014, 34(5): 0515002.

[9] Zhang Q, Pless R. Extrinsic calibration of a camera and laser range finder[C]∥Proceedings of the IEEE International Conference on Intelligent Robots and Systems. Louis, MO, US: IEEE, 2004:2301-2306.

[10] Cobzas D, Zhang H, Jagersand M. A comparative analysis of geometric and image-based volumetric and intensity data registration algorithms[C]∥IEEE International Conference on Robotics and Automation. Edmonton, Alta, Canada: IEEE, 2002:2506-2511.

[11] Chen Z, Zhuo L. Extrinsic calibration of a camera and a laser range finder using point to line constraint[J]. Procedia Engineering, 2012, 29(3): 4348-4352.

[12] Bok Y, Choi D G, Kweon I S. Extrinsic calibration of a camera and a 2D laser without overlap[J]. Robotics and Autonomous System, 2016, 78(18):17-28.

[13] Heng L, Lee G H, Pollefeys M. Self-calibration and visual SLAM with a multi-camera system on a micro aerial vehicle[J].Autonomous Robots, 2015, 39(3):259-277.

[14] 伍明, 李琳琳, 尹宗瑞. 未知环境下机器人定位与运动目标侦测[J]. 山东科技大学学报:自然科学版, 2012, 31(3):66-73. WU Ming, LI Lin-lin, YIN Zong-rui. Robot localization and detection of moving objects in unknown environment[J]. Journal of Shandong University of Scinence and Technology:Natural Science, 2012, 31(3):66-73.(in Chinese)

[15] Hartley R, Zisserman A. Multiple view geometry in computer vision[M]. Cambridge, UK: Cambridge University Press, 2003.

[16] Peynot T, Kassir A. Laser-camera data discrepancies and reliable perception in outdoor robotics[C]∥2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. Taipei, Taiwan: IEEE, 2010:2625-2632

[17] Kassir M M, Palhang M. A region based Camshift tracking with a moving camera[C]∥Proceedings of the RSI/ISM International Conference on Robotics and Mechatronics. Tehran, Iran: IEEE, 2014:451-455.

[18] Comaniciu D, Ramesh V, Meer P. Kernel-based object tracking[J]. IEEE Transactions on Pattern Analysis Machine Intelligence, 2003, 25(5): 564-577.

[19] Nguyen V, Gachter S. A comparison of line extraction algorithms using 2D range data for indoor mobile robotics[J]. Autonomous Robots, 2007, 23(2):97-111.

[20] Chen L J, Arambel P O, Mehra R K. Fusion under unknown correlation-covariance intersection as a special case[C]∥Proceedings of the Fifth International Conference on Information Fusion. Piscataway, NJ, US: IEEE, 2002: 905-912.

[21] Bar-Shalom Y, Tse E. Tracking in a cluttered environment with probabilistic data association[J]. Automatica, 1975, 11(5): 451-460.

[22] Marquardt D. An algorithm for least-squares estimation of nonlinear parameters[J]. SIAM Journal on Applied Mathematics, 1963, 11(5): 431-441.

[23] Peynot T, Scheding S, Terho S. The Marulan data sets: multi-sensor perception in natural environment with challenging conditions[J]. International Journal of Robotics Research, 2010, 29(13): 1602-1607.

Muli-sensor Calibration Optimization Method of Mobile Robot Based on Stationary and Moving Object Observation Consistency Constraint

WU Ming, ZHANG Guo-liang, LI Lin-lin, FU Guang-yuan, LI Cheng-jian

(Rocket Force University of Engineering, Xi’an 710025, Shaanxi, China)

A calibration optimization method of camera and laser rangefinder based on stationary and moving object observation consistency constraint is proposed to address the problem of spatial observation consistency from heterogeneous multi-sensor in the process of mobile robot navigation in unknown environment. A covariance intersection method is used to fuse the bearing state of moving object on image plane, and Kalman and probabilistic data association filters are used to resolve the bearing state fusion of corner feature in the situations of “one-to-one” and “one-to-many”. On this basis, the objective function is generated using the bearing errors before and after fusion of image projections of stationary and moving objects, and the calibration parameters of camera and laser rangefinder are optimized using nonlinear optimization method. Experimental results show that the proposed method can be used to improve the observation consistency of multi-sensor, and the effectiveness of the mentioned methods is verified.

control science and technology; SLAM; object tracking; multi-sensor calibration; multi-sensor information fusion

2016-11-10

国家自然科学基金项目(61503389)；陕西省自然科学基金项目(2016JM6061)

伍明(1981—)，男，讲师。E-mail: hyacinth531@163.com

TP249

A

1000-1093(2017)08-1630-12

10.3969/j.issn.1000-1093.2017.08.022