CN109459033A - 一种多重渐消因子的机器人无迹快速同步定位与建图方法 - Google Patents

Abstract

本发明提供一种多重渐消因子的机器人无迹快速同步定位与建图方法，属于移动机器人自主导航技术领域，系统初始化确定移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵；进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；将观测数据与预测观测数据关联；根据粒子观测值，计算多重渐消因子，量测更新计算粒子的位姿估计均值和协方差，构造高斯分布函数并采样；进行重采样，得到新的粒子集，完成机器人位姿估计；根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。本发明降低了线性化过程引入的误差，提高了系统的滤波精度，提高了滤波器对系统状态发生突变时的调节应变能力，增强了机器人导航定位系统的鲁棒性。

Description

一种多重渐消因子的机器人无迹快速同步定位与建图方法

技术领域

本发明属于移动机器人自主导航技术领域，具体涉及一种多重渐消因子的机器人无迹快速同步定位与建图方法。

背景技术

自主导航技术是移动机器人领域的难点与热点，Leonard和Durrant-Whyte在文献中明确指出解决下面三个问题即可解决移动机器人自主导航这一难题。首先第一个问题“where am I？”(我在哪里？)，这个问题是自定位问题，机器人必须确定自身当前位置才能为之后的任务作出决策，这是后面两个问题的基础。第二个问题“where is thedestination？(目的地在哪里？)”，即移动机器人的任务目标位置。第三个问题“How do Iget there？(我如何到达那里？)”，即路径规划与决策问题。对于前两个问题需要机器人依据自身携带的内部、外部传感器如IMU，激光雷达，摄像头等对自身状态进行估计，同时对周围环境信息进行地图构建。机器人在未知环境中导航移动机器人的定位问题和地图构建问题是相互关联的，位姿估计精度决定构建出的地图精度，同时地图精度又影响移动机器人自身的位姿精度。所以实现移动机器人自主导航的关键在于同步定位与建图技术(Simultaneous Localization and Mapping，SLAM)。

SLAM问题本质上是一个状态估计问题，贝叶斯滤波将SLAM问题转换为机器人位姿状态和地图信息的联合后验概率分布的递归估计问题，而且为SLAM实现提供了多种解决方法，其中典型的算法有基于扩展卡尔曼滤波的同步定位与建图算法(EKF-SLAM)算法和基于粒子滤波的同步定位与建图算法(FastSLAM)。EKF-SLAM算法主体采用扩展卡尔曼滤波进行状态估计，根据机器人运动模型进行位姿估计，同时根据观测模型对地图特征进行估计，该方法将自身位姿向量和地图特征向量联合构成一个高维状态向量，同时对位姿和地图特征进行估计。一般来说机器人运动模型和观测模型均为非线性，首先对其进行线性化处理，进而完成递归状态估计。EKF-SLAM算法由于联合了位姿向量和地图特征向量，在特征较多环境较复杂的条件下算法复杂度会显著增长，严重影响SLAM算法实时性，所以EKF-SLAM只适用于较小环境下的机器人定位与建图。

第二种主流SLAM算法是基于粒子滤波的FastSLAM算法，通过概率论条件独立性将SLAM问题分解成位姿估计和地图特征估计两部分，利用粒子滤波进行机器人位姿估计，扩展卡尔曼滤波进行地图特征估计，因为存在粒子滤波，所以无法避免粒子退化问题，退化问题根源在于无法从粒子后验分布函数中采样，而是在构造出的建议分布函数中得到，二者之间的差异导致最终的退化问题。所以建议分布如何选取直接影响FastSLAM算法估计精度。

在实际应用中，建立的机器人运动模型和观测模型可能与真实模型存在差异，同时环境中的不确定扰动可能对机器人自身状态造成一定影响，进而导致SLAM算法出现滤波精度下降的问题。所以提高SLAM算法在复杂环境下的鲁棒性显得尤为重要。

发明内容

本发明的目的在于提供了一种多重渐消因子的机器人无迹快速同步定位与建图方法，在宽带环境下对独立信源和相干信源的快速高精度测向。

本发明的目的是这样实现的：

一种多重渐消因子的机器人无迹快速同步定位与建图方法，具体的实现步骤如下：

步骤1.系统初始化确定全局坐标系以及在该坐标系下移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵，机器人位姿X_r＝[x_r y_r θ_r]^T，路标位置m_i＝[x_i y_i]^T均采用二维平面直角坐标，全局坐标系以机器人初始位置为原点，初始航向为X轴正方向；

步骤2.依据移动机器人系统的过程模型、观测模型进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；

步骤3.根据k+1时刻观测数据与预测观测数据进行数据关联，根据获得的观测数据，判断是否为环境中已有的地图特征，若为已存在特征，则采用单一兼容最近邻方法进行数据关联，判断若为新特征则转至步骤6进行地图更新；

步骤4.根据粒子观测值，计算多重渐消因子，与UT变换结合进行量测更新计算粒子的位姿估计均值和协方差，并构造高斯分布函数作为建议分布并采样；

步骤5.有效粒子数将有效粒子数与设定阈值N_g相比较，当N_eff＜N_g时进行重采样，得到新的粒子集，完成机器人位姿估计；

步骤6.根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。

步骤2所述的移动机器人系统为

其中x_k为机器人k时刻状态变量，在控制变量u_k作用下，服从p(x_k|x_k-1,u_k)概率分布，w_k表示方差为Q_k的系统噪声，z_k表示机器人对环境特征m_k的观测信息，v_k表示方差为R_k的量测噪声，观测过程服从p(z_k|x_k,m_k)分布，f和h分别表示机器人运动方程和观测方程；

步骤2所述的时间更新采用无迹卡尔曼滤波，根据k时刻位姿估计和位姿控制信息，利用移动机器人运动模型，估计k+1时刻位姿信息，其具体过程为：

步骤2.1.扩充并初始化第i个粒子中机器人的状态向量的均值和方差，机器人位姿为其中和代表机器人在二维平面坐标系内的坐标位置，代表机器人的航向角，角度范围(-π,π)，

其中为状态增广后机器人位姿状态向量，为机器人位姿状态向量，为状态增广后机器人位姿协方差矩阵；

步骤2.2.采用对称采样策略，采样L＝2N+1个sigma点，N为状态向量的维数，

其中n为矩阵的第n列，λ＝α²(N+κ)-N，选取α(0＜α＜1)，κ为权衡调节参数，L个sigma采样点通过运动模型和观测模型进行传递，得到预测后的采样点

步骤2.3.将预测后的sigma采样点加权处理，得到机器人位姿状态经过时间更新的均值和协方差估计

步骤4所述计算多重渐消因子的过程为：

其中tr[·]表示对矩阵求迹，μ_k+1为渐消因子，当μ_k+1＝1时，多渐消UKF退化为标准的UKF，e_k为新息残差序列，z_k+1为真实量测值，为估计量测值，ρ为遗忘因子，取值为0.95≤ρ≤0.995，R_k+1为量测噪声方差阵，

观测量均值和均方差估计为

机器人系统状态量与量测量的协方差估计为

UKF增益为

将多重渐消因子与UKF结合，计算出量测更新后的状态均值和方差

由此得到k时刻粒子i位姿的多重渐消因子UKF估计均值与方差从而构造粒子的后验位姿提议分布为

步骤6所述数据关联后的结果，k时刻第i个粒子的表达形式为

其中为第i个粒子的位姿估计，和表示在第i个粒子表示的地图中第N个路标的全局坐标系下的位置和均方差，若出现新路标，通过EKF算法对路标进行估计，完成地图特征更新

本发明的有益效果在于：本发明提供的这种基于多重渐消因子实现移动机器人SLAM的方法，是在FastSLAM算法基础之上，针对现有方法通过EKF得到建议分布函数会存在线性化截断误差的问题，利用UKF代替其获得建议分布函数，大大提高了算法处理非线性非高斯问题的能力；本发明提供的这种基于多重渐消因子实现移动机器人SLAM的方法，针对系统建模不准确或状态发生突变时，会导致FastSLAM算法出现滤波精度降低甚至发散的问题，引入多重渐消因子，用于生成参数可以自适应调节的建议分布函数。提高滤波器对系统状态发生突变时的调节应变能力，从而增强机器人导航定位系统的鲁棒性。

附图说明

图1为本发明一种改进的移动机器人同步定位与建图的方法原理图；

图2为本发明算法仿真环境，；

图3为本发明对比FastSLAM算法仿真效果；

图4为本发明MFUFastSLAM算法仿真效果；

图5为本发明MFUFastSLAM算法与FastSLAM算法航向角误差；

图6为本发明MFUFastSLAM算法与FastSLAM算法X轴方向位置误差；

图7为本发明MFUFastSLAM算法与FastSLAM算法Y轴方向位置误差；

图8为本发明MFUFastSLAM算法与FastSLAM算法路标估计位置误差；

图9为在某一时刻增大系统噪声后FastSLAM算法仿真效果；

图10为在某一时刻增大系统噪声后本发明MFUFastSLAM算法仿真效果；

图11为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法航向角误差；

图12为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法X轴方向位置误差；

图13为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法Y轴方向位置误差；

图14为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法路标估计位置误差；

具体实施方式

下面结合附图对本发明做进一步的描述：

实施例1

本发明为一种改进的移动机器人同步定位与建图的方法，流程图如图1所示，具体实现步骤为：

步骤1.系统初始化确定全局坐标系以及在该坐标系下移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵；

所述步骤1中机器人位姿X_r＝[x_r y_r θ_r]^T，路标位置m_i＝[x_i y_i]^T均采用二维平面直角坐标系，全局坐标系以机器人初始位置为原点，初始航向为X轴正方向；

步骤2.依据过程模型、观测模型进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；

所述步骤2中，移动机器人系统可表示为

x_k为机器人k时刻状态变量，在控制变量u_k作用下，服从p(x_k|x_k-1,u_k)概率分布，w_k表示方差为Q_k的系统噪声。z_k表示机器人对环境特征m_k的观测信息，v_k表示方差为R_k的量测噪声，观测过程服从p(z_k|x_k,m_k)分布。f和h分别表示机器人运动方程和观测方程。

所述的时间更新方程为：

步骤2.1扩充并初始化第i个粒子中机器人的状态向量的均值和方差。

机器人位姿为其中和代表机器人在二维平面坐标系内的坐标位置，代表机器人的航向角，角度范围(-π,π)，

其中为状态增广后机器人位姿状态向量；为机器人位姿状态向量；为状态增广后机器人位姿协方差矩阵；

步骤2.2.采样L＝2N+1个sigma点，本方法采用对称采样策略，N为状态向量的维数，

式中n为矩阵的第n列；λ＝α²(N+κ)-N，一般选取α(0＜α＜1)，κ为权衡调节参数，通常选取为0，2N+1个sigma采样点通过运动模型和观测模型进行传递，得到预测后的采样点

步骤2.3.将预测后的sigma采样点加权处理，得到机器人位姿状态经过时间更新的均值和协方差估计：

步骤3.根据k+1时刻观测数据与预测观测数据进行数据关联；

所述步骤3中，根据获得的观测数据，判断是否为环境中已有的地图特征，若为已存在特征，则采用单一兼容最近邻方法进行数据关联，判断若为新特征则转至步骤6进行地图更新。

步骤4.根据粒子观测值，计算多渐消因子，与UKF结合进行量测更新计算粒子的位姿估计均值和协方差，并构造高斯分布函数作为建议分布并采样；

所述的多重渐消因子矩阵的计算：

μ_k+1为渐消因子矩阵：分别为对应于每个状态的渐消因子

其中式中tr[·]表示对矩阵求迹；μ_k+1为渐消因子，当μ_k+1＝1时，多渐消UKF就退化为标准的UKF；e_k为新息残差序列，z_k+1为真实量测值，为估计量测值；ρ为遗忘因子，一般取值为0.95≤ρ≤0.995，R_k+1为量测噪声方差阵，其中

观测量均值和均方差估计为

机器人系统状态量与量测量的协方差估计为

UKF增益为

将多重渐消因子与UKF结合，计算出量测更新后的状态均值和方差；

由此得到k时刻粒子i位姿的多重渐消因子UKF估计均值与方差从而构造粒子的后验位姿提议分布为由多重渐消因子对状态方差进行自适应调节，可以充分利用观测数据中的有效信息，抑制环境干扰对运动模型和测量模型的异常影响，使建议分布更接近真实后验分布。

步骤5.进行重采样，得到新的粒子集，完成机器人位姿估计；

所述步骤五中重采样阶段引入有效粒子数将有效粒子数与设定阈值N_g相比较，只有当N_eff＜N_g时才进行重采样。当采样符合较为真实分布时，各粒子权值相近，二者差距越大，权值方差越大，因此可以利用N_eff作为采样粒子接近真实分布的评价函数。

步骤6.根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计；

所述步骤6中，k时刻第i个粒子的表达形式为：

其中为第i个粒子的位姿估计，和表示在第i个粒子表示的地图中第N个路标的全局坐标系下的位置和均方差。根据步骤三的数据关联结果，若出现新路标，通过EKF算法对路标进行估计，完成地图特征更新，

本发明采用的是FastSLAM算法，针对移动机器人系统模型不准确或系统状态发生突变的情况，利用多重渐消因子和UKF对FastSLAM算法进行改进以增加算法精度与鲁棒性。下面结合具体事例进行仿真分析，本发明实施例用来解释本发明，而不是对本发明进行限制，在发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。

利用MATLAB 2014a仿真软件进行仿真试验，搭建的仿真环境如图2所示，其中星号为预设路标，曲线表示移动机器人预设路径，环境中共有35个路标。仿真需要的运动模型和观测模型如下：

其中，(x_k,y_k,θ_k)为机器人k时刻的位姿，Δt为传感器数据频率，u＝[Vγ]为控制变量，表示k时刻的速度和航向角，δ_k为高斯噪声。

其中，(x_i,y_i)为地图中第i个路标在全局坐标系下的坐标；(r_i,φ_i)为传感器探测到该路标与机器人之间的距离和方向角，ε_k为高斯噪声。

仿真环境参数配置如下表

粒子数为60条件下，进行MFUFastSLAM算法和FastSLAM算法仿真，仿真效果如图3，图4所示。图5为本发明MFUFastSLAM算法与FastSLAM算法航向角误差；图6为本发明MFUFastSLAM算法与FastSLAM算法X轴方向位置误差；图7为本发明MFUFastSLAM算法与FastSLAM算法Y轴方向位置误差；图8为本发明MFUFastSLAM算法与FastSLAM算法路标估计位置误差。可以看出，MFUFastSLAM算法在路径估计和路标位置估计误差上均优于FastSLAM算法。本发明针对移动机器人系统模型不准确或系统状态发生突变的问题提出了改进方法，在仿真环境中为了模拟这一现象，在时间第4500步时扩大一倍运动噪声，图9为在某一时刻增大系统噪声后FastSLAM算法仿真效果；图10为在某一时刻增大系统噪声后本发明MFUFastSLAM算法仿真效果；图11为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法航向角误差；图12为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法X轴方向位置误差；图13为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法Y轴方向位置误差；

图14为在某一时刻增大系统噪声后本发明MFUFastSLAM算法与FastSLAM算法路标估计位置误差。显然，在扩大一倍运动噪声后FastSLAM算法效果出现明显偏差，MFUFastSLAM算法相比之下的路径估计与路标位置估计误差均维持在一个较小范围内，可见本发明方法在系统发生突变时，能够及时跟踪系统状态的变化，抑制环境干扰对算法的不良影响，提高了系统的滤波精度与鲁棒性。

综上，本发明一种改进的移动机器人同步定位与建图的方法，定位精度高，建图效果好且系统鲁棒性强，方法简单，易于实现。

实施例2

本发明提供了一种改进的移动机器人同步定位与建图的方法，针对系统建模不准确或状态发生突变时，会导致FastSLAM算法出现滤波精度降低甚至发散的问题，将多重渐消因子矩阵和无迹卡尔曼滤波引入到FastSLAM算法中，用于生成参数可以自适应调节的建议分布函数。与传统方法相比，将多渐消无迹卡尔曼滤波运用到移动机器人同步定位与建图中，降低了原本由于线性化过程引入的误差，不仅能提高系统的滤波精度，还能提高滤波器对系统状态发生突变时的调节应变能力，从而增强机器人导航定位系统的鲁棒性。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1.系统初始化确定全局坐标系以及在该坐标系下移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵；

步骤2.依据过程模型、观测模型进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；

步骤3.根据k+1时刻观测数据与预测观测数据进行数据关联；

步骤4.根据粒子观测值，计算多渐消因子，与UT变换结合进行量测更新计算粒子的位姿估计均值和协方差，并构造高斯分布函数作为建议分布并采样；

步骤5.进行重采样，得到新的粒子集，完成机器人位姿估计；

步骤6.根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。

Claims (4)

1.一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，具体的实现步骤如下：

步骤1.系统初始化确定全局坐标系以及在该坐标系下移动机器人k时刻初始位姿，初始状态估计均值及协方差矩阵，机器人位姿X_r＝[x_r y_r θ_r]^T，路标位置m_i＝[x_i y_i]^T均采用二维平面直角坐标，全局坐标系以机器人初始位置为原点，初始航向为X轴正方向；

步骤2.依据移动机器人系统的过程模型、观测模型进行时间更新，计算粒子在k+1时刻的预测位姿和协方差；

步骤3.根据k+1时刻观测数据与预测观测数据进行数据关联，根据获得的观测数据，判断是否为环境中已有的地图特征，若为已存在特征，则采用单一兼容最近邻方法进行数据关联，判断若为新特征则转至步骤6进行地图更新；

步骤4.根据粒子观测值，计算多重渐消因子，与UT变换结合进行量测更新计算粒子的位姿估计均值和协方差，并构造高斯分布函数作为建议分布并采样；

步骤5.有效粒子数将有效粒子数与设定阈值N_g相比较，当N_eff＜N_g时进行重采样，得到新的粒子集，完成机器人位姿估计；

步骤6.根据数据关联后的结果，使用EKF算法对新路标进行更新，完成对地图特征的估计。

2.根据权利要求1所述的一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，步骤2所述的移动机器人系统为

其中x_k为机器人k时刻状态变量，在控制变量u_k作用下，服从p(x_k|x_k-1,u_k)概率分布，w_k表示方差为Q_k的系统噪声，z_k表示机器人对环境特征m_k的观测信息，v_k表示方差为R_k的量测噪声，观测过程服从p(z_k|x_k,m_k)分布，f和h分别表示机器人运动方程和观测方程；

步骤2所述的时间更新采用无迹卡尔曼滤波，根据k时刻位姿估计和位姿控制信息，利用移动机器人运动模型，估计k+1时刻位姿信息，其具体过程为：

步骤2.1.扩充并初始化第i个粒子中机器人的状态向量的均值和方差，机器人位姿为其中和代表机器人在二维平面坐标系内的坐标位置，代表机器人的航向角，角度范围(-π,π)，

其中为状态增广后机器人位姿状态向量，为机器人位姿状态向量，为状态增广后机器人位姿协方差矩阵；

步骤2.2.采用对称采样策略，采样L＝2N+1个sigma点，N为状态向量的维数，

其中n为矩阵的第n列，λ＝α²(N+κ)-N，选取α(0＜α＜1)，κ为权衡调节参数，L个sigma采样点通过运动模型和观测模型进行传递，得到预测后的采样点

步骤2.3.将预测后的sigma采样点加权处理，得到机器人位姿状态经过时间更新的均值和协方差估计

3.根据权利要求1或2所述的一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，步骤4所述计算多重渐消因子的过程为：

其中tr[·]表示对矩阵求迹，μ_k+1为渐消因子，当μ_k+1＝1时，多渐消UKF退化为标准的UKF，e_k为新息残差序列，z_k+1为真实量测值，为估计量测值，ρ为遗忘因子，取值为0.95≤ρ≤0.995，R_k+1为量测噪声方差阵，

观测量均值和均方差估计为

机器人系统状态量与量测量的协方差估计为

UKF增益为

将多重渐消因子与UKF结合，计算出量测更新后的状态均值和方差

由此得到k时刻粒子i位姿的多重渐消因子UKF估计均值与方差从而构造粒子的后验位姿提议分布为

4.根据权利要求1所述的一种多重渐消因子的机器人无迹快速同步定位与建图方法，其特征在于，步骤6所述数据关联后的结果，k时刻第i个粒子的表达形式为

其中为第i个粒子的位姿估计，和表示在第i个粒子表示的地图中第N个路标的全局坐标系下的位置和均方差，若出现新路标，通过EKF算法对路标进行估计，完成地图特征更新