一种多假设联合相容分支定界算法

一种多假设联合相容分支定界算法

0多假设联合容分析定界算法同时，定位和地图创建（sla）也被称为cml（courrentremotecontroland局部管理），是移动机器实现独立导航的中心问题之一。在SALM领域所涉及的重难点问题中，数据关联是其中之一。数据关联源自目标跟踪中的数据融合技术，在SLAM中用于处理不同时空获得的传感器测量之间，测量与已有地图特征之间的对应关系，以确定它们是否来自共同源的问题，还包括了新特征的确定过程。由于状态估计是SLAM问题的核心，而数据关联又是状态估计的基础，因此直接影响到最终的定位与建图结果，不准确的数据关联甚至会导致SLAM的发散在SLAM数据关联问题的求解方法中，经典的数据关联方法有最邻近算法(NearestNeighbor,NN)随着机器人实际使用环境的日益复杂化，环境中各种不确定因素的干扰等进一步加大，需要建立更为有效的SLAM数据关联方法。为此，本文将JCBB算法融入到MHT的框架之下，吸取两种算法的优点，提出了一种多假设联合相容分支定界算法(MultiHypothesisJointCompatibilityBranchandBound,MHJCBB)。与NN、JCBB等算法每个时刻只保留了1个最优关联假设不同，MHJCBB算法在实施数据关联时，保留了多个联合相容的关联结果，形成多个机器人航迹假设分支，并计算每个航迹假设分支得分。为减少计算量保证计算效率，将得分较低的假设分支在剪枝过程中去除，而得分最高的假设分支则被选择输出。试验结果证明了MHJCBB方法的有效性。1地理单元模型特征的位置坐标采用经典的基于扩展卡尔曼滤波(ExtendedKalmanFilter,EKF)方法求解SLAM问题。定义系统在时刻k的状态变量式中:X其中的分量分别为各地图特征的位置坐标，对于静态地图特征，其位置坐标为常值。EKF假设系统状态为高斯分布，k时刻的状态可用估计均值X式中:P在使用概率方法求解SLAM问题时，通常将运动与观测模型按照一个马尔可夫过程来处理，即根据系统前一时刻的状态来预测后一个时刻的状态，与其他历史状态无关，由此可进行递推处理。系统的运动模型描述如下:式中:f在EKFSLAM中，利用运动模型进行预测的过程为式中:观测方程为式中:b(·)是状态变量X机器人在获得环境的最新探测数据后，可对状态向量的预测值予以滤波更新:式中:2数据关联检验SLAM时，机器人利用自身传感器探测环境，在k时刻，传感器探测得到数据z将每一个观测z假如已有数据关联结果H联合新息及协方差为式中:如果联合马氏距离满足则认为这种关联结果是可接受的，即观测zNeira等3多因素联合评价方法3.1数据关联算法JCBB算法根据所有观测值的综合关联情况来确定关联解，以提高关联的正确率。但在不确定性较多的复杂场合，这种处理方法也易造成误关联，使得关联正确率明显下降。MHT主要用于处理非常复杂环境下的目标跟踪问题，它将可能的关联情况都作为一种假设分支保留下来，通过多周期的信息积累，来判断那种关联选择是最佳的，并且可以对之前的错误关联进行回溯改正，但计算复杂度高。吸取MHT算法和JCBB算法的优点，设计了一种可用于复杂环境中SLAM的数据关联算法MHJCBB。常规JCBB算法在数据关联时仅保留配对数最大且联合马氏距离最小的1个关联假设，即使这个关联解是错误的，在后续过程中也无法进行修改。因此，为防止误关联，MHJCBB算法保留了配对数最大且联合马氏距离最小的N3.2假定分支机构生成航迹假设分支的生成过程如图2所示。在k=1时刻，保留了N3.3航迹得分tracksceare对于每条航迹假设分支，航迹的似然比可以用下式迭代计算:式中:P采用对数似然比(LogLikelihoodRatio)来表示航迹得分(TrackScore):因此，航迹得分的递归计算方式为在N-Best剪枝阶段，根据航迹得分的高低，保留得分高的N个航迹假设分支。与MHT在目标跟踪领域应用时存在多个目标共享观测导致要计算最优假设分支组合的情况不同，MHJCBB不存在组合爆炸问题，因为SLAM问题类似于单目标跟踪的问题，不存在假设分支相容组合爆炸问题，航迹得分最大的航迹假设分支即为最优假设。3.4航迹假设分支机构MHJCBB算法的性能和计算量主要通过以下2个参数进行控制:关联假设的保留数目NMHJCBB整个算法的计算量最终取决于保留的航迹假设分支数，与航迹假设分支数N大致成倍数关系。因为形成多个关联假设与常规JCBB计算量几乎差不多，而航迹得分计算中的v另外，需要注意的是，由于状态滤波更新的计算量比较大，可放在N-Best剪枝后进行，避免对被剪枝的航迹分支进行状态更新。4试验与结果分析4.1位姿运动方程在SLAM的状态向量中，机器人的位置和姿态可表示为通过航迹推算可获得k时刻机器人位姿的相对变化量运动方程为观测方程为式中，(x激光雷达的最大量程为4m，角度探测范围为[-π/2，π/2]。传感器测量均方根误差为σ，其中距离每米的误差为0.01m，角度误差为0.2°。4.2测量误差对关联正确率的影响试验环境为一方形走廊，其中共有168个静态特征，为模拟实际环境中的运动物体的干扰，假设在该区域内存在10个处于随机移动状态的物体，物体的移动速度为0.05m/s，加速度为0.01m/s利用MonteCarlo方法重复进行20次试验，并将获得数据进行平均用于比较分析。参数设置如下:P当测量误差为σ时，NN、JCBB和MHJCBB3种方法数据关联正确率的结果对比如图4所示。图5、6分别是此时x、y方向的估计误差比较。从图中可以看出，JCBB和MHJCBB都取得了90%的关联正确率，差别不明显，但是NN由于误差的积累，在后期关联正确率迅速下滑，导致估计误差迅速的增大，SLAM算法发散。测量误差为2σ时，3种算法数据关联正确率的比较如图7所示。可以看出此时MHJCBB开始体现出优势，尤其在50～130步之间其关联结果明显优于JCBB算法。进一步增加测量的不确定性，当测量误差达到4σ时，关联正确率的结果对比见图8。从图8可知，随着测量误差不断增加，JCBB的关联正确率会突然下降，类似于图4中NN的表现，很容易造成SLAM算法的发散。但是MHJCBB却保持了较好的性能，正确率都在0.72以上，而JCBB后期接近于0.42,MHJCBB优势明显。试验结果表明，传感器测量误差的增大，会引起整个处理过程不确定性的增加，此时NN、JCBB算法的关联准确率都会随之下降，易造成观测与特征之间的配对出错，影响机器人的位姿估计，并使得所建立的地图不确定性增加，甚至会导致SLAM算法的发散。而MHJCBB算法则通过保留多个航迹假设分支，获得了较好的稳定性和可靠性。5多假设跟踪的数据关联算法数据关联问题的求解是SLAM研究领域中的关键难点之一，尤其对于经常使用的经典滤波算法如卡尔曼滤波等，少数的几次误关联便容易导致算法的发散。JCBB算法是SLAM数据关联时广泛使用且行之有效的一种数据关联方法，但当环境中的不确定性增加时，其关联正确率将会出现明显下降。文中将多假设跟踪(MHT)算法与JCBB算法的优点有机结合，提出了一种基于多假设跟踪的MHJCBB。该算法的