[发明专利]一种移动机器人同步定位与地图构建方法

权利要求书

1.一种移动机器人同步定位与地图构建方法，包括如下步骤：

(1)建立系统模型，包括机器人运动模型，观测模型，环境特征模型；并将有色过程噪声模型转化为高斯白噪声模型；

(2)设定移动机器人同步定位与地图构建算法的初始参数，包括航位推算系统参数，激光雷达参数；

(3)状态预测：

状态预测过程是基于标准容积卡尔曼滤波的时间更新过程：

其中，P_k-1|k-1为k-1时刻的误差协方差；S_k-1|k-1为P_k-1|k-1乔里斯基因子；X_i,k-1|k-1为容积点集；m＝2n，n为状态向量的维数；记n维单位列向量e＝[1,0,…,0]^T，使用符号[1]表示对e的元素进行全排列和改变元素符号产生的点集，称为完整全对称点集，[1]_i表示点集[1]中的第i个点；为k-1时刻的状态估计值；为通过状态方程传播的容积点集；f(·)为非线性状态方程；为k时刻的状态预测值；P_k|k-1为k时刻的误差协方差预测值；Q_k-1为系统噪声协方差；

(4)观测：

通过第(1)步构建的机器人观测模型获取观测数据，即获取传感器探测范围内机器人探测到的路标特征，并计算出探测到的每个路标特征到机器人的距离r_i及其与机器人前进方向的夹角θ_i；

(5)数据关联：

采用独立兼容最近邻算法完成数据关联；

其特征在于，还包括如下步骤(6)，在标准容积卡尔曼滤波的量测更新的基础上，选取自适应因子α_k，自适应调整自相关协方差矩阵和互相关协方差矩阵，完成更新过程，同时能够平衡系统模型预测信息与量测信息之间的权比，从而提高定位精度与地图构建精度；

(6)更新：

如果第(5)步数据关联计算结果表明，新的测量对应着一个已经存在于地图中的特征时，需要利用观测信息，对已有特征的状态进行更新，该步骤基于标准容积卡尔曼滤波的量测更新过程：

z_i,k|k-1＝h(X_i,k|k-1)

其中，S_k|k-1为P_k|k-1的乔里斯基因子；X_i,k|k-1为容积点集；z_i,k|k-1为通过量测方程传播的容积点集；h(·)为非线性量测函数；为k时刻的量测预测值；

选取自适应因子α_k，并采用自适应因子α_k自适应调整自相关协方差矩阵和互相关协方差矩阵，自适应因子α_k选为：

其中，γ_k为预测残差，且协方差矩阵P为：自适应因子α_k的取值范围为：0≤α_k≤1，α_k初始值设为1；如果α_k取值合理，它能够基于预测残差γ_k，利用量测信息自适应地调节平衡系统模型预测信息与量测信息之间的权比；当CKF初值选择存在偏差或者系统模型存在异常扰动时，α_k将小于1，即系统模型预测信息在最终滤波结果所占的权值将尽量小；当系统模型预测信息明显异常时，α_k将趋于0，即系统模型预测信息被弃用；

采用自适应因子α_k自适应调整自相关协方差矩阵和互相关协方差矩阵：

其中，P_zz,k|k-1为自相关协方差矩阵；R为量测噪声协方差；P_xz,k|k-1为互相关协方差矩阵；K_k为卡尔曼增益；为k时刻的状态估计值；z_k为k时刻的量测值；P_k|k为k时刻的状态误差协方差估计值；

如果第(5)步数据关联计算结果表明新的测量未对应地图中已存在的特征，即观测到一个新特征时，需要对此特征在地图中进行初始化，进而进行状态增广，进入第(7)步；

(7)增量式构图：

在递推的每一步都进行系统模型噪声的白化处理；利用数据关联完成路标特征探测和地图匹配；通过状态增广不断扩大地图，递推地进行同步定位与地图构建。