一种用于小型地面载体的实时位姿估计系统的制作方法

1.本发明涉及一种用于小型地面载体的实时位姿估计系统，属于导航定位领域。


背景技术：

2.针对激光slam(simultaneous location and mapping，同时定位和制图)导航定位，目前主流算法主要有三个理论支撑方向：基于滤波器、基于优化理论和基于图优化理论。2011年，stefan和johannes提出了基于优化理论的hector slam算法。不同于基于滤波器的gmapping算法与基于图优化理论的cartographer slam算法，基于优化理论的激光slam算法可以依靠激光雷达独立工作，且更利于大型未知场景的建图。
3.现有基于优化的激光slam算法的流程图是迭代式的工作原理，在整个系统开始工作时，由初始激光雷达匹配数据以及固定或随机给出的载体位姿信息确定历史地图库的信息与位姿信息，注意历史转换矩阵与历史地图库仅指的是上一周期发布的地图信息与载体位姿信息。当前时刻激光雷达扫描数据得到后先对其进行预处理，包括无效点信息的过滤(小于激光雷达探测最小距离与大于最大距离的数据过滤)、信息坐标系的转换(从激光雷达坐标系到本体坐标系，具体坐标系信息将在算法改进部分定义)、激光点云(处理后)数据库的更新与发布。
4.获得当前时刻点云信息后，采用二次插值或者三次插值的办法表示地图上某点的占用概率及其对数据坐标的偏导数(因为扫描匹配部分的高斯-牛顿法推导得到的位姿信息优化解需要点占用概率的梯度，而非连续数据采用插值法代替)。之后利用下式目标优化函数进行载体位姿更新迭代，其中，和分别代表载体上一时刻和当前时刻的位姿变量，代表利用表示的栅格地图第i个像素点坐标，代表第i个像素点占用概率，argmin代表求极小值。即将栅格地图某像素点占用概率用载体上一时刻位姿变量表示，利用概率占用最大化迭代求得最佳的位姿变化量。
[0005][0006]
之后，利用载体上一时刻位姿信息进行更新，并在新的姿态矩阵基础上进行栅格地图发布。现有hector slam算法在载体进行低速运动时可以稳定工作，但在载体进行高动态机动时会出现严重的叠图现象，使得位姿估计有很大偏差，实用性能迅速下降乃至不可用。


技术实现要素：

[0007]
本发明技术解决问题：在载体进行高动态机动(实验条件：1.5rad/s旋转角速度转弯，0.3m/s速度直行)时，现有基于优化的激光slam算法所建栅格地图会出现严重的叠图现象，使得载体位姿估计出现较大偏差，导致slam算法不可用。本发明克服以上现有技术的载体高动态运动失效问题，提供一种用于小型地面载体的实时位姿估计系统，在不引入较大
计算量的情况下，极大得提高现有技术在载体高动态机动状态下的工作性能。
[0008]
本发明技术解决方案之一：
[0009]
一种用于小型地面载体的实时位姿估计系统，包括：当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)、载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)、下一时刻载体位姿预测模块(5)和激光slam扫描回调函数(6)；
[0010]
当前载体位姿结果处理模块(1)，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块(2)和载体位姿历史矩阵更新模块(3)；初始化时，当前时刻载体位姿信息由外部gnss(global navigation satellite system，全球导航卫星系统)系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光slam扫描回调函数(6)给出；
[0011]
当前时刻环境栅格地图发布模块(2)，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光slam扫描回调函数(6)；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光slam扫描回调函数(6)；
[0012]
载体位姿历史矩阵更新模块(3)，根据当前时刻载体位姿信息，利用定权重或者退化权重方法对载体位姿历史矩阵进行数据更新，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块(5)处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块(4)；所述矩阵容量设置为十个或者二十个迭代周期；
[0013]
载体运动参量估计模块(4)，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计，将运动参量估计结果输出到下一时刻载体位姿预测模块(5)；
[0014]
下一时刻载体位姿预测模块(5)，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光slam扫描回调函数(6)；
[0015]
激光slam扫描回调函数(6)，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块(1)，完成闭环。
[0016]
通过(1)～(6)的迭代周期的循环，整个载体位姿估计系统以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配，通过迭代更新，输出载体的实时位姿估计。
[0017]
本发明技术解决方案之二：
[0018]
一种用于小型地面载体的实时位姿估计系统，包括：当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)、载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)、下一时刻载体位姿预测模块(5)和激光slam扫描回调函数(6)；
[0019]
当前载体位姿结果处理模块(1)，将获得的当前时刻载体位姿信息输出显示，同时
提供给当前时刻栅格地图发布模块(2)和载体位姿历史矩阵更新模块(3)；初始化时，当前时刻载体位姿信息由外部gnss系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光slam扫描回调函数(6)给出；
[0020]
当前时刻环境栅格地图发布模块(2)，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光slam扫描回调函数(6)；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光slam扫描回调函数(6)；
[0021]
载体位姿历史矩阵更新模块(3)，根据当前时刻载体位姿信息，利用里程计与imu(inertial measurement unit，惯性测量单元)惯性系统信息融合结果对当前时刻位姿进行平滑，平滑处理后的结果存入载体位姿历史矩阵，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块(5)处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块(4)；所述设定的矩阵容量为十个或者二十个迭代周期；
[0022]
载体运动参量估计模块(4)，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计；之后，将运动参量估计结果与惯性信息融合的估计结果进行加权处理，减小由地面载体颤动与非匀加速启动或制动带来的建图与定位偏差；最后，将运动参量估计结果输出到下一时刻载体位姿预测模块(5)；
[0023]
下一时刻载体位姿预测模块(5)，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光slam扫描回调函数(6)；
[0024]
激光slam扫描回调函数(6)，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块(1)，完成闭环。
[0025]
依上述步骤，通过(1)～(6)的迭代周期的循环，整个载体位姿估计方法以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配。通过迭代更新，输出载体的实时位姿估计。
[0026]
本发明与现有技术相比的优点在于：
[0027]
(1)在现有激光slam实时位姿估计系统内引入位姿预测环节，通过对载体运动模型较精确建模实现载体运动状态判断与位姿预测，利用载体历史位姿估计信息对当前位姿进行预测，当前时刻位姿预测值代替上一时刻位姿估计值进入激光slam帧间匹配环节，获得当前时刻位姿估计值，发布栅格地图后，继续进行下一时刻载体位姿的预测，实现基于预测值的迭代更新。如图1，现有激光slam算法只包含当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)与激光slam扫描回调函数(6)三部分。本发明设计的实时位姿估计系统引入了载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)和下一时刻载体位姿预测模块(5)，实际应用中，可以在不引入较大计算量的情况下，极大地提高算法在
载体高动态机动状态下的工作性能。
[0028]
(2)技术解决方案之二相较于之一加入了惯性辅助部分，即轮速里程计与mems(micro-electro-mechanical system，微机电系统)惯性传感器。小型地面载体在启动、制动或者地面打滑时，载体会低频振动，同时启动制动时快速的加速度变化会使得载体位姿估计结果不准确，为了解决这个问题，本发明提出了技术解决方案二。如图2所示，相较于图1所示的解决方案一，加入了惯性辅助部分，可以利用高数据率的惯性数据对低数据率的激光slam帧间匹配函数输出进行平滑，提高了当前时刻载体位姿的估计精度，降低了载体加减速与低频振动的影响。
附图说明
[0029]
图1为本发明技术解决方案一的方框图；
[0030]
图2为本发明技术解决方案二的方框图；
[0031]
图3为载体高速旋转下的现有激光slam实时位姿估计系统与本发明实时位姿估计系统环境栅格地图建图对比；
[0032]
图4为载体组合机动下的现有激光slam实时位姿估计系统与本发明实时位姿估计系统环境栅格地图建图对比；
[0033]
图5为两轮与四轮载体模型的载体运动参量估计模块(4)和下一时刻载体位姿预测模块(5)部分流程图；
[0034]
图6为全向轮载体模型的载体运动参量估计模块(4)和下一时刻载体位姿预测模块(5)部分流程图。
具体实施方式
[0035]
下面结合附图及实施例对本发明进行详细说明。
[0036]
如图1所示，本发明一种用于小型地面载体的实时位姿估计系统实施例1，包括：当前载体位姿结果处理模块1、当前时刻环境栅格地图发布模块2、载体位姿历史矩阵更新模块3、载体运动参量估计模块4、下一时刻载体位姿预测模块5和激光slam扫描回调函数6；
[0037]
当前载体位姿结果处理模块1，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块2和载体位姿历史矩阵更新模块3；初始化时，当前时刻载体位姿信息由外部gnss系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光slam扫描回调函数6给出；
[0038]
当前时刻环境栅格地图发布模块2，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光slam扫描回调函数6；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光slam扫描回调函数6；环境栅格地图的发布过程：首先将激光雷达获得的雷达系下点云数据进行过滤，超过激光雷达仪器扫描半径的点云数据丢弃不再使用；第二步，将数据过滤后的雷达系点云信息转换到本体系，转换关系由激光雷达在载体上的安装角确定；第三步，将本体系点云信息转换到环境地图坐标系，转换关系由当前时刻载体位姿信息确定；最后，利用环境地图坐标系下的点云数据确定环境地图坐标系各点的占用概率并发布，占用概率高为阻挡墙壁或者物体不可通过，占用概率低为可通过点。
[0039]
载体位姿历史矩阵更新模块3，根据当前时刻载体位姿信息，利用定权重或者退化权重方法对载体位姿历史矩阵进行数据更新，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块5处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块4；所述矩阵容量设置为十个或者二十个迭代周期；
[0040]
载体运动参量估计模块4，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计，将运动参量估计结果输出到下一时刻载体位姿预测模块5；
[0041]
下一时刻载体位姿预测模块5，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光slam扫描回调函数6；
[0042]
载体运动参量估计模块4与下一时刻载体位姿预测模块5中的载体运动模型具体实现如图5和图6所示。
[0043]
载体运动模型有两轮、四轮和全向轮模型，两轮和四轮载体运动模型的转弯与位移运动耦合，全向轮模型的转弯和位移运动是解耦的。因此，图5为两轮四轮载体运动模型的实现过程，而图6为全向轮载体模型的实现过程。
[0044]
具体实现流程介绍如下：
[0045]
(a)两轮与四轮模型
[0046]
载体模型为两轮或者四轮模型时，如图5所示。获得载体位姿历史矩阵更新模块输入的历史矩阵和迭代周期dt，载体位姿历史矩阵分为代表载体位置x和载体姿态角的矩阵yaw，矩阵x包含两个变量x、y，分别代表载体位置x、y坐标。矩阵容量大小为n，在实际使用时可取为10或更大。
[0047]
之后，进行转弯判断，若abs(yaw[n]-yaw[0])《thre_yaw，则载体未进行转弯，进入位移判断；反之，载体正处于转弯状态，进入右转判断。其中转弯判断门限thre_yaw一般取为0.01rad约1.145
°
。
[0048]
位移判断，若||x[n]-x[0]||2＜thre_x，则载体静止，位移判断门限thre_x一般取0.05，下一时刻载体位姿预测结果仍然是当前时刻载体位姿信息，即反之，载体进行位移，首先通过最小二乘估计载体位移速度，等权重利用式退化权重利用式其中为待计算的载体速度信息，z为载体位姿历史矩阵中各时刻载体位姿信息，h为载体速度与各时刻载体位姿信息之间的积分关系矩阵，w为载体位姿历史矩阵更新模块3的退化权重方法的权重值，一般距离当前时刻越近的时刻载体位姿信息权重越大。之后利用下式进行载体位姿预测：
[0049]
位置预测：k＝1,2
…
n-1,n
[0050][0051]
姿态角预测：
[0052]
右转判断，对于两轮或者四轮模型，当载体旋转运动与位移运动耦合时，可以利用载体的姿态角构建载体不同时刻载体位置信息关系如下，其中为载体位姿历史矩阵中第k个时刻载体位置信息，x
r0
为载体右旋转点在载体本体系下的坐标，ψk、δψk分别为第k个时刻载体姿态角、第k个时刻到第n个时刻载体姿态角变化量，为载体第n个时刻的位置预测值。若n＝10，可以构造9组由x[k]预测的组合，其中k＝1,2
…
,n-1。
[0053][0054][0055]
ψk＝yaw[k],δψk＝yaw[n]-yaw[k],k＝1,2
…
n-2,n-1
[0056][0057]
最终取第n个时刻载体位置预测值为若载体位姿历史矩阵更新模块(3)使用退化权重更新载体位姿历史矩阵，那么其中wi、wk分别为第i个、第k个时刻载体位姿信息的权重值，若最终时载体处于右转状态，反之处于左转状态，右转判断门限thre_x一般取为0.05。
[0058]
获取左转或右转状态后，通过最小二乘法估计载体旋转角速度之后进行如下预测，即利用第1到第n个时刻载体历史位姿预测载体下一时刻位姿信息。其中，为利用第k时刻位姿信息求载体下一时刻的位置预测值，姿态角预测值则为x
z0
为载体旋转点在载体本体系下的坐标，右转状态x
z0
＝x
r0
即载体右旋转点在载体本体系下的坐标，左转状态x
z0
＝x
l0
即载体左旋转点在载体本体系下的坐标。
[0059]
[0060][0061][0062][0063]
和载体右转判断类似，最终的载体下一时刻位姿预测结果取值为：非退化权重位置预测为姿态预测为退化权重位置预测为姿态预测为其中wi、wk分别为第i个、第k个时刻载体位姿信息的权重值。获得的下一时刻载体位姿预测结果输出到激光slam扫描回调函数(6)。
[0064]
(b)全向轮模型
[0065]
载体模型为全向轮模型时，如图6所示。获得载体位姿历史矩阵更新模块输入的历史矩阵和迭代周期dt，载体位姿历史矩阵分为代表载体位置x和载体姿态角的矩阵yaw，矩阵x包含两个变量x、y，分别代表载体位置x、y坐标，矩阵容量大小为n，在实际使用时可取为10或更大。
[0066]
由于全向轮模型载体转弯与位移运动是解耦的，所以运动参量估计与下一时刻载体位姿预测算法较为简单，可以分解为x方向位移判断、y方向位移判断与转弯判断三个分支独立进行。
[0067]
三个分支的判断条件为abs(x[k]-x[0])《thre_x，abs(y[k]-y[0])《thre_y，位移判断门限thre_x和thre_y为0.05m，abs(yaw[n]-yaw[0])《thre_yaw，旋转判断门限thre_yaw为0.01rad。之后三个分支若判断有效，利用最小二乘法估计位移速度或旋转角速度。之后，载体下一时刻位姿预测流程可用下式表示，即流程图中其中x[k]、y[k]来自于x矩阵中第k个时刻载体位置坐标即k取1到n，即分别利用第1到第n时刻的载体时刻位置坐标预测下一时刻载体位置坐标，预测结果再进行平均或加权平均，得到最终结果。而姿态角的预测与位置坐标类似不再赘述。
[0068]
位置预测：k＝1,2
…
n-1,n；
[0069][0070][0071]
姿态预测：k＝1,2
…
n-1,n
[0072][0073]
最终，与(a)部分类似，有即每个x位置量都包含x、y两个坐标。最后获得的下一时刻载体位姿预测结果进入激光slam扫描回调函数6进行下一步处理。
[0074]
激光slam扫描回调函数6，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块1，完成闭环。不同于原始激光slam系统，此时位姿优化公式改变为下式，其中代表下一时刻载体位姿预测结果即代表利用表示的栅格地图第i个像素点坐标，代表第i个像素点占用概率，利用概率占用最大化求得最佳的位姿迭代变化量。
[0075][0076]
最终的位姿变化量使得优化公式迭代速度更快，或者有限资源下迭代精度更高，所以本发明系统在载体进行高动态机动时，相较于原始激光slam系统有更好的工作性能。
[0077]
通过1～6的迭代周期的循环，整个载体位姿估计系统以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配，通过迭代更新，输出载体的实时位姿估计。
[0078]
如图2所示，本发明的一种用于小型地面载体的实时位姿估计系统实施例2，包括：当前载体位姿结果处理模块1、当前时刻环境栅格地图发布模块2、载体位姿历史矩阵更新模块3、载体运动参量估计模块4、下一时刻载体位姿预测模块5和激光slam扫描回调函数6；
[0079]
当前载体位姿结果处理模块1，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块2和载体位姿历史矩阵更新模块3；初始化时，当前时刻载
体位姿信息由外部gnss系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光slam扫描回调函数6给出；
[0080]
当前时刻环境栅格地图发布模块2，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光slam扫描回调函数6；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光slam扫描回调函数6；
[0081]
载体位姿历史矩阵更新模块3，根据当前时刻载体位姿信息，利用里程计与imu惯性系统信息融合结果对当前时刻位姿进行平滑，平滑处理后的结果存入载体位姿历史矩阵，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块5处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块4；所述设定的矩阵容量为十个或者二十个迭代周期；
[0082]
载体运动参量估计模块4，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计；之后，将运动参量估计结果与惯性信息融合的估计结果进行加权处理，减小由地面载体颤动与非匀加速启动或制动带来的建图与定位偏差，惯性辅助的权重可以；最后，将运动参量估计结果输出到下一时刻载体位姿预测模块5；
[0083]
下一时刻载体位姿预测模块5，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光slam扫描回调函数6；
[0084]
激光slam扫描回调函数6，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块1，完成闭环。
[0085]
依上述步骤，通过1～6的迭代周期的循环，整个载体位姿估计方法以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配。通过迭代更新，输出载体的实时位姿估计。
[0086]
上述实施例2中的其他部分与实施例1相同。
[0087]
本发明的小型地面载体定义：
[0088]
实验载体参数：
[0089]
长30cm、宽20cm和高20cm以内。实际实验载体为麦克纳母轮小车底盘，可进行原地转弯与掉头。
[0090]
数据支持：
[0091]
由于高速转弯相较于直行对算法位姿估计技术检验效果更明显，所以这里给出同一旋转速度下现有算法与本发明算法的建图效果(由于栅格地图发布是在当前时刻载体位姿估计结果基础上进行的，所以最能直观体现局部环境中载体位姿估计偏差)。
[0092]
如图3所示，载体旋转角速率1.5rad/s，即约86
°
/s，原地旋转360
°
，正常的环境栅格地图建图效果应该为，载体位置估计旋转前后为同一位置，同时发布的栅格地图无旋转叠图。图3中的(b)为本发明实时位姿估计系统建图效果，可以看到其基本满足正常建图效
果要求；而图3中的(a)现有激光slam实时位姿估计系统建图效果则不满足要求，首先所建立的栅格地图有明显的旋转叠图，其次图中心实线为旋转过程中当前时刻载体位姿估计结果形成的移动路线，显然不再一个点处，已经漂出了比较远的距离。由以上对比可以看出本发明提出的实时位姿估计系统相较于现有激光slam实时位姿估计系统在载体进行高速旋转时工作性能有极大改善。
[0093]
如图4所示，载体组合机动(旋转角速率1.5rad/s即约86
°
/s，直行速率0.3m/s)。载体机动路径为从旋转测试相同起始点出发，做“s”形机动行至阳台，掉头，做“s”形机动行至门口，掉头返回起始点。比较图4中的(a)和(b)可以发现现有激光slam系统建图效果较差，出现了和图3中的(a)一样的旋转叠图，系统失效不可用。尤其是在两个掉头转弯处影响最大，位姿估计结果路线明显偏离正常值，而图4中的(b)本发明位姿估计系统表现正常。以上说明了本发明实时位姿估计系统在载体进行高速机动时相较于现有激光slam实时位姿估计系统性能有很大提升。
[0094]
供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。