CN104807465A - 机器人同步定位与地图创建方法及装置 - Google Patents

Abstract

本发明实施例公开一种机器人同步定位与地图创建方法及装置，属于机器人同步定位与地图创建领域。该方法包括：根据外部传感器采集的外部环境信息，得到观测信息数据集；根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。本发明实施例通过容积变换，避免了直接线性化过程，减少了计算雅可比矩阵的计算量，提高了精度，减小了截断误差对于系统稳定性的影响，改进了通过迭代测量更新，改进了容积点采样规则，减小采样点在高度非线性环境下的失真。

Description

机器人同步定位与地图创建方法及装置

技术领域

本发明涉及机器人同步定位与地图创建领域，尤其涉及一种机器人同步定位与地图创建方法及装置。

背景技术

机器人定位和地图创建问题是移动机器人导航研究中的关键问题，可靠导航是自主机器人最基本、最重要的一项功能，对移动机器人的研究有非常重要的意义。

目前解决移动机器人同步定位与地图创建问题的主要方法是应用卡尔曼滤波计算。例如，申请号为201410581627.0的申请文件提出一种卡尔曼滤波方法，根据机器人的运动学方程和基于相对方位的量测方程，给出多移动机器人自定位的动态模型；采用SR-UKF滤波算法，以相对方位为输入对系统状态进行整体更新，实现多移动机器人的协同自定位。申请文件201410581627.0采用的卡尔曼滤波法姿态解算方法建模简单、实时性较好，但是忽略了非线性因素尤其是载体位移加速度对姿态测量信息的影响。

为了提高机器人的姿态解算精度，申请号为201410489632.9的专利申请文件提出一种基于改进的扩展卡尔曼滤波算法的两轮自平衡机器人姿态解算方法，利用STM32交互采集处理两轮自平衡机器人惯性姿态测量数据；利用两轮自平衡机器人姿态测量系统建立MEMS陀螺仪误差数学模型；利用两轮自平衡机器人姿态测量系统建立MEMS IMU姿态解算数学模型；利用MEMSIMU姿态解算数学模型构建扩展卡尔曼滤波器模型；利用改进的扩展卡尔曼滤波算法对两轮自平衡机器人姿态测量数据进行数据融合；利用二阶近似解算减少非线性函数非线性截断误差；利用位移加速度判断公式，自适应量测噪声矩阵对两轮自平衡机器人姿态进行补偿；利用两步量测更新的方法提高改进的扩展卡尔曼滤波的先验解算精度。

但是该种方法未充分考虑到机器人模型非线性化的程度，如果机器人模型非线性化程度较高，易产生较大的位姿估计误差，影响非线性的环境下的系统定位精度。

发明内容

本发明实施例提供一种机器人同步定位与地图创建方法及装置，能够减小采样点在高度非线性环境下的失真。

本发明实施例采用如下技术方案：

一方面提供一种机器人同步定位与地图创建方法，包括：

根据外部传感器采集的外部环境信息，得到观测信息数据集；

根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；

利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；

根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。

可选的，所述根据外部传感器采集的外部环境信息，得到观测信息数据集之前，还包括：建立机器人运动模型；

建立所述机器人运动模型应用的机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距，m_k表示K时刻机器人的位姿，m_k包括机器人K时刻X方向上的位移(m_x,k)，K时刻Y方向上的位移(m_y,k)，K时刻角度位移(m_θ,k)，相应的参数如m_x,k-1表示为机器人K-1时刻X方向上的位移，α_k为机器人转角。

可选的，所述根据外部传感器采集的外部环境信息，得到观测信息数据集之前，还包括：建立观测模型；

建立所述观测模型应用的机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点i的位置，z_k表示机器人相对于观测到的地图特征点的位置。

可选的，所述根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布包括：

确定采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为：其中，和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来，其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量，ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列， ${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\left\{\begin{array}{c}\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\end{array}\right\};$

将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计(先验概率分布)；

通过容积变换，将每个容积点的先验估计进行拟合，得到下一时刻运动控制信息的先验概率分布，并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递，其中容积变换公式为：

$I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{{2n}_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ});$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{cccc}{x}_{k|k-1}^1-S_{k|k-1}& x_{k|k-1}^2-S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}-S_{k|k-1}\end{array}\right];$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。

可选的，所述利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布包括：

通过所述观测模型建立新的容积点集，任意一个容积点迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集；

根据所述下一时刻运动控制信息的先验概率分布和根据所述观测模型得到的观测数据，得到下一时刻机器人运动信息的后验估计；经由所述观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息： ${\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}=\frac{1}{{2n}_s}\underset{j=1}{\overset{{2n}_s}{\mathrm{\Σ}}}{z}_{k|k-1}^{i,j,\left(l\right)};$

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left(\begin{array}{c}{\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\end{array}\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T;$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{{2n}_s}}\left[\begin{array}{cccc}{z}_{k|k-1}^1-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得；

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B_{k|k-1}^{\left(l\right)}}^T$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1};$

计算下一时刻平方根因子，用于递增更新，迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-{P_{k|k-1}^{\mathrm{xz},\left(l\right)}}^T\·{B_{k|k-1}^{i,\left(l\right)}}^{-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量，当迭代终止时，各数据实现更新：

另一方面提供一种机器人同步定位与地图创建装置，包括：

第一确定模块，用于根据外部传感器采集的外部环境信息，得到观测信息数据集；

第二确定模块，用于根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；

第一更新模块，用于根据所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；

第二更新模块，用于根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。

可选的，还包括：第一建模模块，用于建立机器人运动模型；

建立所述机器人运动模型应用的机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距，m_k表示K时刻机器人的位姿，m_k包括机器人K时刻X方向上的位移(m_x,k)，K时刻Y方向上的位移(m_y,k)，K时刻角度位移(m_θ,k)，相应的参数如m_x,k-1表示为机器人K-1时刻X方向上的位移，α_k为机器人转角。

可选的，还包括：第二建模模块，用于建立观测模型；

建立所述观测模型应用的机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点i的位置，z_k表示机器人相对于观测到的地图特征点的位置。

可选的，所述第二确定模块具体用于：

确定采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为：其中，和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来，其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量，ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列， ${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\left\{\begin{array}{c}\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\end{array}\right\};$

将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计(先验概率分布)；

通过容积变换，将每个容积点的先验估计(先验概率分布)进行拟合，得到下一时刻运动控制信息的先验概率分布，并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递，其中容积变换公式为：

$I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{{2n}_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ});$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{cccc}{x}_{k|k-1}^1-S_{k|k-1}& x_{k|k-1}^2-S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}-S_{k|k-1}\end{array}\right];$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。

可选的，所述第一更新模块具体用于：

通过所述观测模型建立新的容积点集，任意一个容积点迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集；

根据所述下一时刻运动控制信息的先验估计(先验概率分布)和根据所述观测模型得到的观测数据，得到下一时刻机器人运动信息的后验估计；经由所述观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息： ${\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}=\frac{1}{{2n}_s}\underset{j=1}{\overset{{2n}_s}{\mathrm{\Σ}}}{z}_{k|k-1}^{i,j,\left(l\right)};$

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left(\begin{array}{c}{\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\end{array}\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T;$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{{2n}_s}}\left[\begin{array}{cccc}{z}_{k|k-1}^1-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得；

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B_{k|k-1}^{\left(l\right)}}^T$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1};$

计算下一时刻平方根因子，用于递增更新，迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-{P_{k|k-1}^{\mathrm{xz},\left(l\right)}}^T\·{B_{k|k-1}^{i,\left(l\right)}}^{-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量，当迭代终止时，各数据实现更新：

基于上述技术方案的机器人同步定位与地图创建方法及装置，通过容积变换，避免了直接线性化过程，减少了计算雅可比矩阵的计算量，提高了精度；利用协方差矩阵的平方根因子代替协方差矩阵传递，减小了截断误差对于系统稳定性的影响；改进了通过迭代测量更新，改进了容积点采样规则，减小采样点在高度非线性环境下的失真。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本公开。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。

图1为本发明实施例1示出的一种机器人同步定位与地图创建方法的流程图；

图2为本发明实施例1示出的另一种机器人同步定位与地图创建方法的流程图；

图3为本发明实施例2示出的一种移动机器人的系统结构图；

图4为本发明实施例2示出的一种机器人同步定位与地图创建方法的流程图；

图5为本发明实施例3示出的一种机器人同步定位与地图创建装置的结构示意图；

图6为本发明实施例3示出的一种机器人同步定位与地图创建装置的结构示意图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

如图1所示，本实施例提供一种机器人同步定位与地图创建方法，该方法包括：

11、根据外部传感器采集的外部环境信息，得到观测信息数据集；

12、根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；

13、利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；

14、根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。

可选的，如图2所示上述步骤11之前，还可以包括：10a建立机器人运动模型；

建立所述机器人运动模型应用的机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距。

可选的，如图2所示，上述步骤11之前，还可以包括：10b建立观测模型；

建立所述观测模型应用的机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点_i的位置。

可选的，所述根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布包括：

确定采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为：其中，和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来，其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量，ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列， ${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\left\{\begin{array}{c}\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\end{array}\right\};$

将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计(先验概率分布)；

通过容积变换，将每个容积点的先验估计(先验概率分布)进行拟合，得到下一时刻运动控制信息的先验估计(先验概率分布)，并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递，其中容积变换公式为：

$I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{{2n}_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ});$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{cccc}{x}_{k|k-1}^1-S_{k|k-1}& x_{k|k-1}^2-S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}-S_{k|k-1}\end{array}\right];$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。

可选的，所述利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布包括：

通过所述观测模型建立新的容积点集，任意一个容积点迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集；

根据所述下一时刻运动控制信息的先验估计(先验概率分布)和根据所述观测模型得到的观测数据，得到下一时刻机器人运动信息的后验估计；经由所述观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息： ${\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}=\frac{1}{{2n}_s}\underset{j=1}{\overset{{2n}_s}{\mathrm{\Σ}}}{z}_{k|k-1}^{i,j,\left(l\right)};$

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left(\begin{array}{c}{\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\end{array}\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T;$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{{2n}_s}}\left[\begin{array}{cccc}{z}_{k|k-1}^1-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得；

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B_{k|k-1}^{\left(l\right)}}^T$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1};$

计算下一时刻平方根因子，用于递增更新，迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-{P_{k|k-1}^{\mathrm{xz},\left(l\right)}}^T\·{B_{k|k-1}^{i,\left(l\right)}}^{-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量，当迭代终止时，各数据实现更新：

本实施例中，可以在上述步骤14之后，更新当前时刻地图特征信息，更新当前时刻机器人控制信息，实现位姿更新：

本实施例的机器人同步定位与地图创建方法，通过容积变换，避免了直接线性化过程，减少了计算雅可比矩阵的计算量，提高了精度；利用协方差矩阵的平方根因子代替协方差矩阵传递，减小了截断误差对于系统稳定性的影响；改进了通过迭代测量更新，改进了容积点采样规则，减小采样点在高度非线性环境下的失真。

实施例2

本实施例提供一种机器人同步定位与地图创建方法，该方法可以通过图3所示的移动机器人的系统结构图实现，如图4所示，该方法包括：

S1：建立机器人运动模型和观测模型：

S1.1：机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距，m_k表示的是K时刻机器人的位姿，m_k包括机器人K时刻X方向上的位移(m_x,k)，K时刻Y方向上的位移(m_y,k)，K时刻角度位移(m_θ,k)。相应的参数如m_x,k-1表示为机器人K-1时刻X方向上的位移。α_k为机器人转角。

S1.2：机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]:$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点i的位置，z_k表示为机器人相对于观测到的地图特征点的位置(亦可称为机器人观测相对位姿)，它包括l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角。m_y,k如上F1解释为K时刻Y方向上的位移，其他同理。

步骤2：观测模块通过外部传感器采集外部环境信息，得到观测信息数据集。

其中，整个机器人主体包括两个部分，即运动模块和观测模块，运动模块用于根据内部参数计算机器人的位姿(位置和角度)，观测模块通过外部传感器(外部传感器是观测模块的一部分，在机器人前端)感知环境，得到自身位姿数据。

本发明实施例中，外部传感器获取到与特征点i的相对位置m_x,i,m_y,i(距离i点的X方向相对距离，距离i点的Y方向相对距离)，通过公式 $z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$ 得到i点的观测信息数据。i点特指一个点，实际情况是能观测到多个点的。多个特征点的观测信息数据集合成观测信息数据集。

S3：观测阶段，利用该时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布。

例如，K时刻的后验概率分布由K-1时刻确定，初始时刻的后验概率分布经预先设定。

K时刻的运动控制信息即 $m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]$ 由K-1时刻的已知信息和动力学方程 $m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$ 得到。初始时刻K时刻运动控制信息需要事先设定。

进一步的，所述S3包括：

S3.1：计算采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为： 和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来。本发明实施例中，各容积点是一系列容积点集，它是通过一系列的特定的点集，经过数学公式转化而来。容积点表示为：(K-1时刻，第j个序列的容积点)。它由公式得到。

其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量。ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列，

${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\left\{\begin{array}{c}\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\end{array}\right\}.$

S3.2：将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计(先验概率分布)。以容积点j为例：

本发明实施例中，K时刻下定义如下变量：表示机器人K时刻系统状态信息，其中(此处即是机器人运动模型中的m_k)表示机器人位姿信息，表示K时刻地图特征向量，n_s表示系统状态向量维数。U_1:k＝[u₁,u₂,…,u_k]^T表示机器人K时刻控制向量，n_u表示机器人控制向量维数。Z_1:k＝[z₁,z₂,…_k]^T表示机器人K时刻观测向量，n_z表示机器人观测向量维数。

是容积点，它通过函数(此函数包含了一系列公式，此处抽象为一个函数)得到了(表示为在已知K-1时刻容积点信息的条件下推测出的K时刻的容积点信息的估计。)

S3.3：通过容积变换，将每个容积点的先验估计(先验概率分布)进行拟合，得到下一时刻运动控制信息的先验概率分布。并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递。其中容积变换公式为： $I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{{2n}_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ}).$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{cccc}{x}_{k|k-1}^1-S_{k|k-1}& x_{k|k-1}^2-S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}-S_{k|k-1}\end{array}\right],$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。qr(A_k|k-1 ^T)中qr()是一种处理矩阵的手段，叫作QR分解。A_k|k-1 ^T是中间数据，QR分解A_k|k-1 ^T后，同时得到Q矩阵和R矩阵，R的转置矩阵为R^T，R^T＝C_k|k-1，此处C_k|k-1表示平方根因子，用于下一阶段处理。

S4：更新阶段，利用下一时刻的先验概率分布和观测信息数据集更新后验概率分布。

进一步的，所述的步骤4包括：

S4.1：通过观测模型建立新的容积点集。以第i个特征点为例，假设迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集。其中，特征点为机器人在移动环境中观测到的障碍物，或者称为标志点。

S4.2：结合S3所得到的机器人下一时刻运动控制信息的先验估计(先验概率分布)和S1得到的观测数据，得到下一时刻机器人运动信息的后验估计。经由观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息：其中，表示为在K-1时刻的已知信息下得到的K时刻的关于地图特征点i的，关于容积点序列j的第L次迭代的观测信息，由观测方程函数h()得到，h()为一系列处理方式，包含K-1时刻，关于地图特征点i的，第L次迭代的机器人位姿信息，运动控制信息和各特征点估计信息；是对上面数据的一种拟合方法，用于下面具体的数据处理。

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left(\begin{array}{c}{\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\end{array}\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T.$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{{2n}_s}}\left[\begin{array}{cccc}{z}_{k|k-1}^1-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得。

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B_{k|k-1}^{\left(l\right)}}^T$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1}$

S4.3：继续计算下一时刻平方根因子，用于递增更新。迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-{P_{k|k-1}^{\mathrm{xz},\left(l\right)}}^T\·{B_{k|k-1}^{i,\left(l\right)}}^{-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

S4.4：设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量。重复S3.1至S3.3，当迭代终止时，各数据实现更新： 其中，预先设置的迭代次数，迭代完成后，得到最终的在K-1时刻已知的情况下K时刻综合估计出的机器人位姿信息；为预先设置好的迭代次数，迭代完成后，得到最终的在K-1时刻已知的情况下K时刻综合估计出的机器人平方根因子。

S5：更新当前时刻地图特征信息，更新当前时刻机器人控制信息。实现位姿更新： $S_k=S_{k|k-1}+W_k(z_k^i-{\stackrel{\‾}{z}}_{k|k-1}^i).$ 其中， $S_k=S_{k|k-1}+W_k(z_k^i-{\stackrel{\‾}{z}}_{k|k-1}^i),$ S_k为机器人最终信息(包括位置信息和地图信息)，S_k由两部分相加而成。S_k|k-1表示已知K-1时刻机器人信息估计的K时刻信息(前述步骤得出)。表示偏差量，由卡尔曼增益W_k乘以(观测量偏差：)。

${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\left\{\begin{array}{c}\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\end{array}\right\}$ 中有2(n_s+n_u)列。

S6、根据更新后的信息控制机器人。

本实施例的机器人同步定位与地图创建方法，通过容积变换，避免了直接线性化过程，减少了计算雅可比矩阵的计算量，提高了精度；利用协方差矩阵的平方根因子代替协方差矩阵传递，减小了截断误差对于系统稳定性的影响；改进了通过迭代测量更新，改进了容积点采样规则，减小采样点在高度非线性环境下的失真。

实施例3

如图5所示，本实施例提供一种机器人同步定位与地图创建装置，包括：

第一确定模块51，用于根据外部传感器采集的外部环境信息，得到观测信息数据集；

第二确定模块52，用于根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；

第一更新模块53，用于根据所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；

第二更新模块54，用于根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。

可选的，如图6所示，还包括：第一建模模块50a，用于建立机器人运动模型；

建立所述机器人运动模型应用的机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距，m_k表示的是K时刻机器人的位姿，m_k包括机器人K时刻X方向上的位移(m_x,k)，K时刻Y方向上的位移(m_y,k)，K时刻角度位移(m_θ,k)。相应的参数如m_x,k-1表示为机器人K-1时刻X方向上的位移。α_k为机器人转角。

可选的，如图6所示，还包括：第二建模模块50b，用于建立观测模型；

建立所述观测模型应用的机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点i的位置，z_k表示为机器人相对于观测到的地图特征点的位置(亦可称为机器人观测相对位姿)，它包括l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角。m_y,k如上F1解释为K时刻Y方向上的位移，其他同理。

其中，整个机器人主体包括两个部分，即运动模块和观测模块，运动模块用于根据内部参数计算机器人的位姿(位置和角度)，观测模块通过外部传感器(外部传感器是观测模块的一部分，在机器人前端)感知环境，得到自身位姿数据。

本发明实施例中，外部传感器获取到与特征点i的相对位置m_x,i,m_y,i(距离i点的X方向相对距离，距离i点的Y方向相对距离)，通过公式 $z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$ 得到i点的观测信息数据。i点特指一个点，实际情况是能观测到多个点的。多个特征点的观测信息数据集合成观测信息数据集。

可选的，所述第二确定模块具体用于：

确定采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为：其中，和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来，其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量，ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列， ${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\left\{\begin{array}{c}\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right],\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\end{array}\right\};$

将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计(先验概率分布)；

通过容积变换，将每个容积点的先验估计(先验概率分布)进行拟合，得到下一时刻运动控制信息的先验估计(先验概率分布)，并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递，其中容积变换公式为：

$I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{{2n}_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ});$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{cccc}{x}_{k|k-1}^1-S_{k|k-1}& x_{k|k-1}^2-S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}-S_{k|k-1}\end{array}\right];$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。

可选的，所述第一更新模块具体用于：

通过所述观测模型建立新的容积点集，任意一个容积点迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集；

根据所述下一时刻运动控制信息的先验估计(先验概率分布)和根据所述观测模型得到的观测数据，得到下一时刻机器人运动信息的后验估计；经由所述观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息： ${\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}=\frac{1}{{2n}_s}\underset{j=1}{\overset{{2n}_s}{\mathrm{\Σ}}}{z}_{k|k-1}^{i,j,\left(l\right)};$

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left(\begin{array}{c}{\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\end{array}\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T;$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{{2n}_s}}\left[\begin{array}{cccc}{z}_{k|k-1}^1-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}-{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得；

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B_{k|k-1}^{\left(l\right)}}^T$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1};$

计算下一时刻平方根因子，用于递增更新，迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-{P_{k|k-1}^{\mathrm{xz},\left(l\right)}}^T\·{B_{k|k-1}^{i,\left(l\right)}}^{-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量，当迭代终止时，各数据实现更新：

本实施例的机器人同步定位与地图创建装置，通过容积变换，避免了直接线性化过程，减少了计算雅可比矩阵的计算量，提高了精度；利用协方差矩阵的平方根因子代替协方差矩阵传递，减小了截断误差对于系统稳定性的影响；改进了通过迭代测量更新，改进了容积点采样规则，减小采样点在高度非线性环境下的失真。

以上已经描述了本发明的各实施例，上述说明是示例性的，并非穷尽性的，并且也不限于所披露的各实施例。在不偏离所说明的各实施例的范围和精神的情况下，对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。本文中所用术语的选择，旨在最好地解释各实施例的原理、实际应用或对市场中的技术的改进，或者使本技术领域的其它普通技术人员能理解本文披露的各实施例。

本领域技术人员在考虑说明书及实践这里公开的公开后，将容易想到本公开的其它实施方案。本申请旨在涵盖本公开的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本公开的一般性原理并包括本公开未公开的本技术领域中的公知常识或惯用技术手段。

Claims (10)

1.一种机器人同步定位与地图创建方法，其特征在于，包括：

根据外部传感器采集的外部环境信息，得到观测信息数据集；

根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；

利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；

根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。

2.根据权利要求1所述的方法，其特征在于，所述根据外部传感器采集的外部环境信息，得到观测信息数据集之前，还包括：建立机器人运动模型；

建立所述机器人运动模型应用的机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距，m_k表示K时刻机器人的位姿，m_k包括机器人K时刻X方向上的位移(m_x,k)，K时刻Y方向上的位移(m_y,k)，K时刻角度位移(m_θ,k)，相应的参数如m_x,k-1表示为机器人K-1时刻X方向上的位移，α_k为机器人转角。

3.根据权利要求1或2所述的方法，其特征在于，所述根据外部传感器采集的外部环境信息，得到观测信息数据集之前，还包括：建立观测模型；

建立所述观测模型应用的机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点i的位置，z_k表示机器人相对于观测到的地图特征点的位置。

4.根据权利要求3所述的方法，其特征在于，所述根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布包括：

确定采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为：其中，和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来，其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量，ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列，

${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\{\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right]\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right]\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right]\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\};$

将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计；

通过容积变换，将每个容积点的先验估计进行拟合，得到下一时刻运动控制信息的先验概率分布，并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递，其中容积变换公式为：

$I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{2n_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ});$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{ccccccc}{x}_{k|k-1}^1& -S_{k|k-1}& x_{k|k-1}^2& -S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}& -S_{k|k-1}\end{array}\right];$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。

5.根据权利要求4所述的方法，其特征在于，所述利用所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布包括：

通过所述观测模型建立新的容积点集，任意一个容积点迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集；

根据所述下一时刻运动控制信息的先验估计和根据所述观测模型得到的观测数据，得到下一时刻机器人运动信息的后验估计；经由所述观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息： ${\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}=\frac{1}{2n_s}\underset{j=1}{\overset{2n_s}{\mathrm{\Σ}}}{z}_{k|k-1}^{i,j,\left(l\right)};$

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left({\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T;$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{2n_s}}\left[\begin{array}{ccccccc}{z}_{k|k-1}^1& -{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2& -{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}& -{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得；

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B}_{k|k-1}^{\left(l\right)T}$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1};$

计算下一时刻平方根因子，用于递增更新，迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-P_{k|k-1}^{\mathrm{xz},\left(l\right)T}\·B_{k|k-1}^{i,\left(l\right)-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量，当迭代终止时，各数据实现更新：

6.一种机器人同步定位与地图创建装置，其特征在于，包括：

第一确定模块，用于根据外部传感器采集的外部环境信息，得到观测信息数据集；

第二确定模块，用于根据当前时刻的后验概率分布和下一时刻的运动控制信息，得到下一时刻的先验概率分布；

第一更新模块，用于根据所述下一时刻的先验概率分布和所述观测信息数据集更新后验概率分布；

第二更新模块，用于根据更新后的后验概率分布更新当前时刻地图特征信息并更新当前时刻机器人控制信息。

7.根据权利要求6所述的装置，其特征在于，还包括：第一建模模块，用于建立机器人运动模型；

建立所述机器人运动模型应用的机器人运动方程为：

$m_k=\left[\begin{array}{c}{m}_{x,k}\\ m_{y,k}\\ m_{\mathrm{\θ},k}\end{array}\right]=\left[\begin{array}{c}{m}_{x,k-1}+T\·v_k\·\cos (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{y,k-1}+T\·v_k\·\sin (m_{\mathrm{\θ},k-1}+{\mathrm{\α}}_k)\\ m_{\mathrm{\θ},k-1}+T\·v_k\·\frac{\sin \left({\mathrm{\α}}_k\right)}{L}\end{array}\right]$

其中，m_k表示机器人位姿，T表示系统内部采样间隔，v_k表示机器人行进速度，α_k表示转角，L表示轮距，m_k表示K时刻机器人的位姿，m_k包括机器人K时刻X方向上的位移(m_x,k)，K时刻Y方向上的位移(m_y,k)，K时刻角度位移(m_θ,k)，相应的参数如m_x,k-1表示为机器人K-1时刻X方向上的位移，α_k为机器人转角。

8.根据权利要求6或7所述的装置，其特征在于，还包括：第二建模模块，用于建立观测模型；

建立所述观测模型应用的机器人观测方程为：

$z_k=\left[\begin{array}{c}{l}_k\\ {\mathrm{\β}}_k\end{array}\right]=\left[\begin{array}{c}\sqrt{{(m_{x,i}-m_{x,k})}^2+{(m_{y,i}-m_{y,k})}^2}\\ \arctan \left(\frac{m_{y,i}-m_{y,k}}{m_{x,i}-m_{x,k}}\right)-m_{\mathrm{\θ},k}\end{array}\right]$

其中，l_k表示机器人与观测到地图特征点的距离，β_k表示机器人与观测到地图特征点的夹角，(m_x,i,m_y,i)为机器人观测到的地图特征点i的位置，z_k表示机器人相对于观测到的地图特征点的位置。

9.根据权利要求8所述的装置，其特征在于，所述第二确定模块具体用于：

确定采样容积点集，其中各容积点包含了机器人的运动信息，控制信息，和地图特征信息；其数学表达式为：其中，和由数学公式 $S_{k-1}^A=\left[\begin{array}{c}{S}_{k-1}\\ u_k\end{array}\right],L_{k-1}^A=\left[\begin{array}{cc}{L}_{k-1}& 0\\ 0& \sqrt{Q_k}\end{array}\right]$ 处理而来，其中机器人状态信息S_k-1和运动信息u_k要增广为高斯噪声变量，ξ_i表示为互相正交的完全对称容积点集，表示为其中ξ_i存在2(n_s+n_u)列， ${\mathrm{\ξ}}_i=\sqrt{n_s+n_u}\{\left[\begin{array}{c}1\\ 0\\ ...\\ 0\end{array}\right]\left[\begin{array}{c}0\\ 1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ 1\\ 0\end{array}\right],\left[\begin{array}{c}0\\ ...\\ 0\\ 1\end{array}\right],...,\left[\begin{array}{c}-1\\ 0\\ ...\\ 0\end{array}\right]\left[\begin{array}{c}0\\ -1\\ ...\\ 0\end{array}\right],...,\left[\begin{array}{c}0\\ ...\\ -1\\ 0\end{array}\right]\left[\begin{array}{c}0\\ ...\\ 0\\ -1\end{array}\right]\};$

将采样的容积点通过非线性模型传递，得到随机变量的统计数据，进而得到下一时刻关于每个容积点的机器人运动信息、控制信息和地图特征信息的先验估计；

通过容积变换，将每个容积点的先验估计进行拟合，得到下一时刻运动控制信息的先验概率分布，并且对产生的协方差误差矩阵做QR分解处理，得到平方根因子用于下一步传递，其中容积变换公式为：

$I=\∫f\left(y\right)N(y;\mathrm{\μ},\mathrm{\Σ})\mathrm{dy}\≈\frac{1}{2n_y}\mathrm{\Σf}(\sqrt{\mathrm{\Σ}}{\mathrm{\ξ}}_i+\mathrm{\μ});$

协方差误差矩阵为：

$A_{k|k-1}=\frac{1}{\sqrt{2(n_s+n_u)}}\left[\begin{array}{ccccccc}{x}_{k|k-1}^1& -S_{k|k-1}& x_{k|k-1}^2& -S_{k|k-1}& ...& x_{k|k-1}^{2(n_s+n_u)}& -S_{k|k-1}\end{array}\right];$

QR分解为：[Q R]＝qr(A_k|k-1 ^T),C_k|k-1＝R^T。

10.根据权利要求9所述的装置，其特征在于，所述第一更新模块具体用于：

通过所述观测模型建立新的容积点集，任意一个容积点迭代初始值分别为S_k|k-1和C_k|k-1，第l次迭代机器人位姿信息和平方根因子分别为由数学公式：得到更新的容积点集；

根据所述下一时刻运动控制信息的先验估计和根据所述观测模型得到的观测数据，得到下一时刻机器人运动信息的后验估计；经由所述观测方程，容积点传递后得到的容积点观测信息拟合各容积点信息： ${\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}=\frac{1}{2n_s}\underset{j=1}{\overset{2n_s}{\mathrm{\Σ}}}{z}_{k|k-1}^{i,j,\left(l\right)};$

QR分解新的协方差误差矩阵 $\left[\begin{array}{cc}Q& R\end{array}\right]=\mathrm{qr}\left({\left[\begin{array}{cc}{B}_{k|k-1}^{i,\left(l\right)}& \sqrt{R_k}\end{array}\right]}^T\right),P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T;$

新的协方差矩阵由：

$B_{k|k-1}^{i,\left(l\right)}=\frac{1}{\sqrt{2n_s}}\left[\begin{array}{ccccccc}{z}_{k|k-1}^1& -{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& z_{k|k-1}^2& -{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}& ...& z_{k|k-1}^{2(n_s+n_u)}& -{\stackrel{\‾}{z}}_{k|k-1}^{i,\left(l\right)}\end{array}\right]$ 可得；

$P_{k|k-1}^{\mathrm{zz},\left(l\right)}=R^T,P_{k|k-1}^{\mathrm{xz},\left(l\right)}=A_{k|k-1}^{\left(l\right)}{B}_{k|k-1}^{\left(l\right)T}$

进一步求得卡尔曼增益： $W_k^{\left(l\right)}=P_{k|k-1}^{\mathrm{xz},\left(l\right)}\·{(P_{k|k-1}^{\mathrm{zz},\left(l\right)T}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)})}^{-1};$

计算下一时刻平方根因子，用于递增更新，迭代l+1次时机器人位姿信息和平方根因子

$S_{k|k-1}^{(l+1)}=S_{k|k-1}+W_k^{\left(l\right)}[z^k-h\left(S_{k|k-1}^{\left(l\right)}\right)-P_{k|k-1}^{\mathrm{xz},\left(l\right)T}\·B_{k|k-1}^{i,\left(l\right)-1}(S_{k|k-1}-S_{k|k-1}^{\left(l\right)})]$

$C_{k|k-1}^{(l+1)}=C_{k|k-1}-W_k^{\left(l\right)}\·P_{k|k-1}^{\mathrm{zz},\left(l\right)}\·{\left(W_k^{\left(l\right)}\right)}^T$

设置迭代终止条件，l＝L_max，L_max为迭代最大次数，是预先设置的固定常量，当迭代终止时，各数据实现更新：