一种基于多传感器的机器人未知环境目标跟踪方法技术

本发明专利技术公开了一种基于多传感器的机器人未知环境目标跟踪方法，该方法包括如下步骤：(1)首先提出了基于动点和静点误差优化的摄像机与激光传感器参数标定方法，在线实现了不同传感器坐标系转换参数优化；(2)其次提出了基于卡尔曼和概率数据关联的多源环境特征状态融合方法；(3)最后提出基于多传感器信息融合的对象状态估计算法。该方法将信息融合技术应用于解决SLAMOT问题，能够提高对象识别和锁定能力、估计准确性、空间覆盖范围、时间覆盖范围的有效途径，机器人能够利用装备的主动和被动传感器充分采集环境和目标信息，并将多源信息进行融合以提高SLAMOT的实际应用能力。

【技术实现步骤摘要】
一种基于多传感器的机器人未知环境目标跟踪方法
本专利技术涉及基于多传感器信息融合的机器未知环境下目标跟踪方法
，具体为一种基于多传感器的机器人未知环境目标跟踪方法。
技术介绍
目前SLAMOT解决方法大多数基于单一传感器，并未考虑多传感器融合在SLAMOT问题上的应用，由于基于单一传感器的SLAMOT算法获取信息种类有限，存在以下几点问题：(1)对象识别和锁定能力差。目前SLAMOT算法在应用中通常采用激光扫描仪作为信息获取手段，激光扫描仪虽然能够获得较为准确的角度和距离信息，但是对象身份识别能力差，不能满足实际应用要求。图像信息的利用可以有效解决对象识别和锁定问题，而图像信息无法提供对象深度信息，因此单独应用于SLAMOT问题也较为困难。总之，由于单传感器获取信息种类单一，对象识别和锁定能力并不理想。(2)环境特征利用程度低。机器人运动环境存在大量有价值的环境信息，例如：环境直线特征、角点特征、标志物特征。而单传感器只能利用某种环境特征作为环境地图构建的依据，例如，激光扫描仪通常利用扫描平面内的环境直线特征或角点特征作为环境地图，无法利用垂直平面的直线特征以及其他环境特征作为构建地图的要素，因此环境信息综合利用程度低。(3)环境侦测范围有限。单一传感器的覆盖范围有限，容易产生观测盲区，例如，当机器人对多目标进行跟踪时，若仅利用激光扫描仪观测，那么，当目标间的运动轨迹存在交叉重叠时，容易产生对目标和环境特征的观测盲区，影响SLAMOT的估计准确性。
技术实现思路
本专利技术的目的在于提供一种基于多传感器的机器人未知环境目标跟踪方法，以解决上述
技术介绍
中提出的问题。为实现上述目的，本专利技术提供如下技术方案：一种基于多传感器的机器人未知环境目标跟踪方法，该方法包括如下步骤：(1)首先提出了基于动点和静点误差优化的摄像机与激光传感器参数标定方法，在线实现了不同传感器坐标系转换参数优化；(2)其次提出了基于卡尔曼和概率数据关联的多源环境特征状态融合方法，利用卡尔曼框架完成单对单对象状态融合，利用概率数据关联框架完成单对多对象状态融合；(3)最后提出基于多传感器信息融合的对象状态估计算法，设计基于全关联卡尔曼滤波算法，从而建立不同对象状态估计关联性，提高状态估计准确性；优选的，所述的基于多传感器信息融合的机器未知环境下目标跟踪方法，所述的步骤(1))首先提出了基于动点和静点误差优化的摄像机与激光传感器参数标定方法包括环境动态和静态物体图像平面投影状态融合方法和摄像机与激光测距仪传感器联合标定优化方法。优选的，所述的基于多传感器信息融合的机器未知环境下目标跟踪方法，所述的环境动态和静态物体图像平面投影状态融合方法采用协方差交集方法，采用该方法的原因在于融合过程将来自激光传感器和摄像机传感器的运动物体观测信息同等考虑，认为这两种传感器提供的信息量相同，融合过程并不偏重于任何一方，另外，目标状态融合需要针对不同传感器观测值描述的相同性质状态分量进行，对于摄像机和激光扫描仪而言，它们所描述的相同状态分量为目标方向状态，因此，在图像像素坐标系中，只对u分量值进行融合处理；利用同一性原则的移动物体扫描点检验方法获取环境动态对象产生的激光反射点得到的是运动物体扫描点群，需要通过运动物体点群得到运动物体状态，运动目标t对应的投影扫描点群为：{P1,P2,....,Pn-1,Pn},Pi＝[uivi]T，误差阵为：则目标t在图像上的投影点Pt_s＝[ut_svt_s]T与误差阵可表示为：则根据协方差交集状态融合公式[14]，可知u分量CI融合过程为：dut_fuse＝(ω·(dut_camshift)-1+(1-ω)·(dut_s)-1)-1(4)其中，ω∈[0,1]为融合系数，为了体现多传感器信息对等性，此处将融合系数设为0.5(此时CI融合对等于信息滤波器融合)；ut_fuse,dut_fuse为融合后目标在图像像素坐标系中的方向u分量值和误差范围；ut_camshift，dut_camshift为利用Camshift跟踪方法得到的目标状态的u分量值和误差范围；ut_s,dut_s为利用式(9)(10)得到的动点检测目标状态的u分量值和误差范围；摄像机与激光测距仪传感器联合标定优化方法；标定参数优化思想为：利用不同时刻图像平面内融合前以及融合后的运动物体和环境垂线方向分量差值构造误差累积数据，并通过非线性优化方法求解初始标定参数的优化值，假设共跟踪了N帧图像，系统优化目标函数为：参数优化目标函数源于运动物体和静止物体在像素平面中的方向分量误差，因此，具体优化对象为摄像机与激光扫描仪联合标定参量Φ,Δ中同投影点方向分量生成相关的自变量。为了计算方便，重写式如下：设Δ'＝-Φ·Δ，则式(44)为：PC＝Φ·PL+Δ'(7)根据摄像机小孔成像模型[9]可知，与图像投影状态方向分量u相关的Φ,Δ'分量为：Φ11,Φ13,Δ′1，在构造误差函数时，将对这些分量进行优化，其余分量不变，那么，i时刻的马氏距离误差ei为：其中为运动物体融合前后方向误差绝对值，静止特征融合前后方向误差绝对值，其值为：其中uit_fuse为目标方向分量值融合值，uit_s为得到的运动物体图像像素状态中的方向分量值，duit_fuse和为它们对应的方差值。同样，uil_fuse为得到的环境垂线特征方向分量融合值，uil_s为角点在图像平面投影状态的方向分量，duil_fuse和duil_s为它们对应的方差值，结合摄像机小孔成像模型可知系统的最终目标函数为：求解式(11)，得到优化值则最终优化标定参数为：优选的，所述的基于多传感器信息融合的机器未知环境下目标跟踪方法，所述的步骤(2)提出了基于卡尔曼和概率数据关联的多源环境特征状态融合方法包括基于卡尔曼滤波的激光扫描点投影和图像直线特征的二源状态融合方法和基于概率数据关联的激光扫描点投影和图像直线特征的二源状态融合方法。优选的，所述的基于多传感器信息融合的机器未知环境下目标跟踪方法，所述的基于卡尔曼滤波的激光扫描点投影和图像直线特征的二源状态融合方法：首先提取激光扫描仪对环境的角点观测值，并获取摄像机对环境中的垂直直线观测值后，再运用投影函数和误差传播公式确定激光扫描点在图像中的投影状态和误差分布，利用卡尔曼滤波方法对这两种传感器环境特征观测值进行数据融合，可以将一个信息源当做对象状态模型看待，将另一个信息源当做对象观测模型看待，以激光扫描仪信息为状态模型，以图像信息为观测模型，而此时直线特征的状态仅在图像坐标系的X轴方向进行，则系统状态模型为：该式为激光扫描仪观测值对应的特征状态函数，因此，对应的协方差预测阵为：而激光扫描仪的观测模型为：假设观测误差阵为。根据卡尔曼滤波方法可知，更新后的特征状态和方差为：结合式(15)式，将摄像机图像提取的特征状态当成系统实际观测值，则，系统观测残差可表示为：即，其中，为利用图像信息提取的环境直线特征的X轴分量值。而(16)(17)式中的卡尔曼增益为：由(15)式可知，Hk为1，而系统观测协方差阵为：其中，观测误差阵对应摄像机图像提取的特征误差阵，结合(14)式，(521)式可写为：将(19)(20)(21)式带入(16)(17)式可得到基于卡尔曼滤波的融合公式为：优选的，本文档来自技高网...

【技术保护点】
1.一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：该方法包括如下步骤：(1)首先提出了基于动点和静点误差优化的摄像机与激光传感器参数标定方法，在线实现了不同传感器坐标系转换参数优化；(2)其次提出了基于卡尔曼和概率数据关联的多源环境特征状态融合方法，利用卡尔曼框架完成单对单对象状态融合，利用概率数据关联框架完成单对多对象状态融合；(3)最后提出基于多传感器信息融合的对象状态估计算法，设计基于全关联卡尔曼滤波算法，从而建立不同对象状态估计关联性，提高状态估计准确性。

【技术特征摘要】
1.一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：该方法包括如下步骤：(1)首先提出了基于动点和静点误差优化的摄像机与激光传感器参数标定方法，在线实现了不同传感器坐标系转换参数优化；(2)其次提出了基于卡尔曼和概率数据关联的多源环境特征状态融合方法，利用卡尔曼框架完成单对单对象状态融合，利用概率数据关联框架完成单对多对象状态融合；(3)最后提出基于多传感器信息融合的对象状态估计算法，设计基于全关联卡尔曼滤波算法，从而建立不同对象状态估计关联性，提高状态估计准确性。2.根据权利要求1所述的一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：所述的步骤(1))首先提出了基于动点和静点误差优化的摄像机与激光传感器参数标定方法包括环境动态和静态物体图像平面投影状态融合方法和摄像机与激光测距仪传感器联合标定优化方法。3.根据权利要求1所述的一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：所述的环境动态和静态物体图像平面投影状态融合方法采用协方差交集方法，采用该方法的原因在于融合过程将来自激光传感器和摄像机传感器的运动物体观测信息同等考虑，认为这两种传感器提供的信息量相同，融合过程并不偏重于任何一方，另外，目标状态融合需要针对不同传感器观测值描述的相同性质状态分量进行，对于摄像机和激光扫描仪而言，它们所描述的相同状态分量为目标方向状态，因此，在图像像素坐标系中，只对u分量值进行融合处理；利用同一性原则的移动物体扫描点检验方法获取环境动态对象产生的激光反射点得到的是运动物体扫描点群，需要通过运动物体点群得到运动物体状态，运动目标t对应的投影扫描点群为：{P1,P2,....,Pn-1,Pn},Pi＝[uivi]T，误差阵为：则目标t在图像上的投影点Pt_s＝[ut_svt_s]T与误差阵可表示为：则根据协方差交集状态融合公式[14]，可知u分量CI融合过程为：ut_fuse＝dut_fuse·(ω·(dut_camshift)-1·ut_camshift+(1-ω)·(dut_s)-1·ut_s)(3)dut_fuse＝(ω·(dut_camshift)-1+(1-ω)·(dut_s)-1)-1(4)其中，ω∈[0,1]为融合系数，为了体现多传感器信息对等性，此处将融合系数设为0.5(此时CI融合对等于信息滤波器融合)；ut_fuse,dut_fuse为融合后目标在图像像素坐标系中的方向u分量值和误差范围；ut_camshift，dut_camshift为利用Camshift跟踪方法得到的目标状态的u分量值和误差范围；ut_s,dut_s为利用式(9)(10)得到的动点检测目标状态的u分量值和误差范围；摄像机与激光测距仪传感器联合标定优化方法；标定参数优化思想为：利用不同时刻图像平面内融合前以及融合后的运动物体和环境垂线方向分量差值构造误差累积数据，并通过非线性优化方法求解初始标定参数的优化值，假设共跟踪了N帧图像，系统优化目标函数为：参数优化目标函数源于运动物体和静止物体在像素平面中的方向分量误差，因此，具体优化对象为摄像机与激光扫描仪联合标定参量Φ,Δ中同投影点方向分量生成相关的自变量。为了计算方便，重写式如下：设Δ'＝-Φ·Δ，则式(44)为：PC＝Φ·PL+Δ'(7)根据摄像机小孔成像模型[9]可知，与图像投影状态方向分量u相关的Φ,Δ'分量为：Φ11,Φ13,Δ′1，在构造误差函数时，将对这些分量进行优化，其余分量不变，那么，i时刻的马氏距离误差ei为：其中为运动物体融合前后方向误差绝对值，静止特征融合前后方向误差绝对值，其值为：其中uit_fuse为目标方向分量值融合值，uit_s为得到的运动物体图像像素状态中的方向分量值，duit_fuse和为它们对应的方差值。同样，uil_fuse为得到的环境垂线特征方向分量融合值，uil_s为角点在图像平面投影状态的方向分量，duil_fuse和duil_s为它们对应的方差值，结合摄像机小孔成像模型可知系统的最终目标函数为：求解式(11)，得到优化值则最终优化标定参数为：4.根据权利要求1所述的一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：所述的步骤(2)提出了基于卡尔曼和概率数据关联的多源环境特征状态融合方法包括基于卡尔曼滤波的激光扫描点投影和图像直线特征的二源状态融合方法和基于概率数据关联的激光扫描点投影和图像直线特征的二源状态融合方法。5.根据权利要求1所述的一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：所述的基于卡尔曼滤波的激光扫描点投影和图像直线特征的二源状态融合方法：首先提取激光扫描仪对环境的角点观测值，并获取摄像机对环境中的垂直直线观测值后，再运用投影函数和误差传播公式确定激光扫描点在图像中的投影状态和误差分布，利用卡尔曼滤波方法对这两种传感器环境特征观测值进行数据融合，可以将一个信息源当做对象状态模型看待，将另一个信息源当做对象观测模型看待，以激光扫描仪信息为状态模型，以图像信息为观测模型，而此时直线特征的状态仅在图像坐标系的X轴方向进行，则系统状态模型为：该式为激光扫描仪观测值对应的特征状态函数，因此，对应的协方差预测阵为：而激光扫描仪的观测模型为：假设观测误差阵为。根据卡尔曼滤波方法可知，更新后的特征状态和方差为：结合式(15)式，将摄像机图像提取的特征状态当成系统实际观测值，则，系统观测残差可表示为：即，其中，为利用图像信息提取的环境直线特征的X轴分量值。而(16)(17)式中的卡尔曼增益为：由(15)式可知，Hk为1，而系统观测协方差阵为：其中，观测误差阵对应摄像机图像提取的特征误差阵，结合(14)式，(521)式可写为：将(19)(20)(21)式带入(16)(17)式可得到基于卡尔曼滤波的融合公式为：6.根据权利要求1所述的一种基于多传感器的机器人未知环境目标跟踪方法，其特征是：所述的步骤(3)提出基于多传感器信息融合的对象状态估计算法，基于EKF的机器人SLAMOT算法：首先将系统状态表示成机器人状态，目标状态和环境特征状态组合的形式，即：其中为机器人k时刻的状态值，为目标k时刻的位置速度状态值，为当前已发现的n个环境特征位置状态组合，环境特征可以是环境角点特征，此时其状态为：也可以为环境直线特征，此时其状态为：系统最初的状态向量只包括机器人状态而目标状态和环境特征状态LMk是在机器人取得观测值后逐步扩充进来的。假设机器人k时刻的观测值集合为该集合为环境特征观测值集合目标观测值集合和新环境特征观测值集合的并集，相应的，系统协方差阵具有如下形式：其中，rk表示机器人状态tk表示目标状态lmi表示环境特征lmi状态此处环境特征状态省去了时间下标k，C(a,b)代表向量a和向量b的互相关矩阵，由式(7-2)可知该系统主要存在6种相互依赖关系，分别是：机器人自身的依赖关系C(rk,rk)，目标自身的依赖关系C(tk,tk)，环境特征自身的依赖关系C(lmi,lmi)，机器人和目标之间的依赖关系C(rk,tk)，机器人和环境特征之间的依赖关系C(rk,lmi)，目标和环境特征之间的依赖关系C(tk,lmi)以...

【专利技术属性】
技术研发人员：伍明，
申请(专利权)人：中国人民解放军火箭军工程大学，
类型：发明
国别省市：陕西,61