CN115683114A - 一种用于小型地面载体的实时位姿估计系统 - Google Patents

一种用于小型地面载体的实时位姿估计系统 Download PDF

Info

Publication number
CN115683114A
CN115683114A CN202211318943.XA CN202211318943A CN115683114A CN 115683114 A CN115683114 A CN 115683114A CN 202211318943 A CN202211318943 A CN 202211318943A CN 115683114 A CN115683114 A CN 115683114A
Authority
CN
China
Prior art keywords
carrier
pose
module
result
current
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211318943.XA
Other languages
English (en)
Inventor
王可东
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Weiayou Communication Technology Co ltd
Original Assignee
Nanjing Weiayou Communication Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Weiayou Communication Technology Co ltd filed Critical Nanjing Weiayou Communication Technology Co ltd
Priority to CN202211318943.XA priority Critical patent/CN115683114A/zh
Publication of CN115683114A publication Critical patent/CN115683114A/zh
Pending legal-status Critical Current

Links

Images

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种用于小型地面载体的实时位姿估计系统，包括：当前载体位姿结果处理模块、当前时刻环境栅格地图发布模块、载体位姿历史矩阵更新模块、载体运动参量估计模块、下一时刻载体位姿预测模块和激光SLAM扫描回调函数。本发明在不引入较大计算量的情况下，极大地提高现有技术在载体高动态机动状态下的工作性能。

Description

一种用于小型地面载体的实时位姿估计系统
技术领域
本发明涉及一种用于小型地面载体的实时位姿估计系统，属于导航定位领域。
背景技术
针对激光SLAM(Simultaneous Location and Mapping，同时定位和制图)导航定位，目前主流算法主要有三个理论支撑方向：基于滤波器、基于优化理论和基于图优化理论。2011年，Stefan和Johannes提出了基于优化理论的Hector SLAM算法。不同于基于滤波器的Gmapping算法与基于图优化理论的Cartographer SLAM算法，基于优化理论的激光SLAM算法可以依靠激光雷达独立工作，且更利于大型未知场景的建图。
现有基于优化的激光SLAM算法的流程图是迭代式的工作原理，在整个系统开始工作时，由初始激光雷达匹配数据以及固定或随机给出的载体位姿信息确定历史地图库的信息与位姿信息，注意历史转换矩阵与历史地图库仅指的是上一周期发布的地图信息与载体位姿信息。当前时刻激光雷达扫描数据得到后先对其进行预处理，包括无效点信息的过滤(小于激光雷达探测最小距离与大于最大距离的数据过滤)、信息坐标系的转换(从激光雷达坐标系到本体坐标系，具体坐标系信息将在算法改进部分定义)、激光点云(处理后)数据库的更新与发布。
获得当前时刻点云信息后，采用二次插值或者三次插值的办法表示地图上某点的占用概率及其对数据坐标的偏导数(因为扫描匹配部分的高斯-牛顿法推导得到的位姿信息优化解需要点占用概率的梯度，而非连续数据采用插值法代替)。之后利用下式目标优化函数进行载体位姿更新迭代，其中，
Figure BDA0003910533480000016
和
Figure BDA0003910533480000011
分别代表载体上一时刻和当前时刻的位姿变量，
Figure BDA0003910533480000012
代表利用
Figure BDA0003910533480000013
表示的栅格地图第i个像素点坐标，
Figure BDA0003910533480000014
代表第i个像素点占用概率，argmin代表求极小值。即将栅格地图某像素点占用概率用载体上一时刻位姿变量表示，利用概率占用最大化迭代求得最佳的位姿变化量。
Figure BDA0003910533480000015
之后，利用载体上一时刻位姿信息进行更新，并在新的姿态矩阵基础上进行栅格地图发布。现有Hector SLAM算法在载体进行低速运动时可以稳定工作，但在载体进行高动态机动时会出现严重的叠图现象，使得位姿估计有很大偏差，实用性能迅速下降乃至不可用。
发明内容
本发明技术解决问题：在载体进行高动态机动(实验条件：1.5rad/s旋转角速度转弯，0.3m/s速度直行)时，现有基于优化的激光SLAM算法所建栅格地图会出现严重的叠图现象，使得载体位姿估计出现较大偏差，导致SLAM算法不可用。本发明克服以上现有技术的载体高动态运动失效问题，提供一种用于小型地面载体的实时位姿估计系统，在不引入较大计算量的情况下，极大得提高现有技术在载体高动态机动状态下的工作性能。
本发明技术解决方案之一：
一种用于小型地面载体的实时位姿估计系统，包括：当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)、载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)、下一时刻载体位姿预测模块(5)和激光SLAM扫描回调函数(6)；
当前载体位姿结果处理模块(1)，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块(2)和载体位姿历史矩阵更新模块(3)；初始化时，当前时刻载体位姿信息由外部GNSS(Global Navigation Satellite System，全球导航卫星系统)系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光SLAM扫描回调函数(6)给出；
当前时刻环境栅格地图发布模块(2)，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光SLAM扫描回调函数(6)；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光SLAM扫描回调函数(6)；
载体位姿历史矩阵更新模块(3)，根据当前时刻载体位姿信息，利用定权重或者退化权重方法对载体位姿历史矩阵进行数据更新，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块(5)处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块(4)；所述矩阵容量设置为十个或者二十个迭代周期；
载体运动参量估计模块(4)，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者Kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计，将运动参量估计结果输出到下一时刻载体位姿预测模块(5)；
下一时刻载体位姿预测模块(5)，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光SLAM扫描回调函数(6)；
激光SLAM扫描回调函数(6)，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块(1)，完成闭环。
通过(1)～(6)的迭代周期的循环，整个载体位姿估计系统以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配，通过迭代更新，输出载体的实时位姿估计。
本发明技术解决方案之二：
一种用于小型地面载体的实时位姿估计系统，包括：当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)、载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)、下一时刻载体位姿预测模块(5)和激光SLAM扫描回调函数(6)；
当前载体位姿结果处理模块(1)，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块(2)和载体位姿历史矩阵更新模块(3)；初始化时，当前时刻载体位姿信息由外部GNSS系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光SLAM扫描回调函数(6)给出；
当前时刻环境栅格地图发布模块(2)，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光SLAM扫描回调函数(6)；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光SLAM扫描回调函数(6)；
载体位姿历史矩阵更新模块(3)，根据当前时刻载体位姿信息，利用里程计与IMU(Inertial Measurement Unit，惯性测量单元)惯性系统信息融合结果对当前时刻位姿进行平滑，平滑处理后的结果存入载体位姿历史矩阵，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块(5)处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块(4)；所述设定的矩阵容量为十个或者二十个迭代周期；
载体运动参量估计模块(4)，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者Kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计；之后，将运动参量估计结果与惯性信息融合的估计结果进行加权处理，减小由地面载体颤动与非匀加速启动或制动带来的建图与定位偏差；最后，将运动参量估计结果输出到下一时刻载体位姿预测模块(5)；
下一时刻载体位姿预测模块(5)，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光SLAM扫描回调函数(6)；
激光SLAM扫描回调函数(6)，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块(1)，完成闭环。
依上述步骤，通过(1)～(6)的迭代周期的循环，整个载体位姿估计方法以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配。通过迭代更新，输出载体的实时位姿估计。
本发明与现有技术相比的优点在于：
(1)在现有激光SLAM实时位姿估计系统内引入位姿预测环节，通过对载体运动模型较精确建模实现载体运动状态判断与位姿预测，利用载体历史位姿估计信息对当前位姿进行预测，当前时刻位姿预测值代替上一时刻位姿估计值进入激光SLAM帧间匹配环节，获得当前时刻位姿估计值，发布栅格地图后，继续进行下一时刻载体位姿的预测，实现基于预测值的迭代更新。如图1，现有激光SLAM算法只包含当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)与激光SLAM扫描回调函数(6)三部分。本发明设计的实时位姿估计系统引入了载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)和下一时刻载体位姿预测模块(5)，实际应用中，可以在不引入较大计算量的情况下，极大地提高算法在载体高动态机动状态下的工作性能。
(2)技术解决方案之二相较于之一加入了惯性辅助部分，即轮速里程计与MEMS(Micro-Electro-Mechanical System，微机电系统)惯性传感器。小型地面载体在启动、制动或者地面打滑时，载体会低频振动，同时启动制动时快速的加速度变化会使得载体位姿估计结果不准确，为了解决这个问题，本发明提出了技术解决方案二。如图2所示，相较于图1所示的解决方案一，加入了惯性辅助部分，可以利用高数据率的惯性数据对低数据率的激光SLAM帧间匹配函数输出进行平滑，提高了当前时刻载体位姿的估计精度，降低了载体加减速与低频振动的影响。
附图说明
图1为本发明技术解决方案一的方框图；
图2为本发明技术解决方案二的方框图；
图3为载体高速旋转下的现有激光SLAM实时位姿估计系统与本发明实时位姿估计系统环境栅格地图建图对比；
图4为载体组合机动下的现有激光SLAM实时位姿估计系统与本发明实时位姿估计系统环境栅格地图建图对比；
图5为两轮与四轮载体模型的载体运动参量估计模块(4)和下一时刻载体位姿预测模块(5)部分流程图；
图6为全向轮载体模型的载体运动参量估计模块(4)和下一时刻载体位姿预测模块(5)部分流程图。
具体实施方式
下面结合附图及实施例对本发明进行详细说明。
如图1所示，本发明一种用于小型地面载体的实时位姿估计系统实施例1，包括：当前载体位姿结果处理模块1、当前时刻环境栅格地图发布模块2、载体位姿历史矩阵更新模块3、载体运动参量估计模块4、下一时刻载体位姿预测模块5和激光SLAM扫描回调函数6；
当前载体位姿结果处理模块1，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块2和载体位姿历史矩阵更新模块3；初始化时，当前时刻载体位姿信息由外部GNSS系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光SLAM扫描回调函数6给出；
当前时刻环境栅格地图发布模块2，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光SLAM扫描回调函数6；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光SLAM扫描回调函数6；环境栅格地图的发布过程：首先将激光雷达获得的雷达系下点云数据进行过滤，超过激光雷达仪器扫描半径的点云数据丢弃不再使用；第二步，将数据过滤后的雷达系点云信息转换到本体系，转换关系由激光雷达在载体上的安装角确定；第三步，将本体系点云信息转换到环境地图坐标系，转换关系由当前时刻载体位姿信息确定；最后，利用环境地图坐标系下的点云数据确定环境地图坐标系各点的占用概率并发布，占用概率高为阻挡墙壁或者物体不可通过，占用概率低为可通过点。
载体位姿历史矩阵更新模块3，根据当前时刻载体位姿信息，利用定权重或者退化权重方法对载体位姿历史矩阵进行数据更新，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块5处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块4；所述矩阵容量设置为十个或者二十个迭代周期；
载体运动参量估计模块4，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者Kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计，将运动参量估计结果输出到下一时刻载体位姿预测模块5；
下一时刻载体位姿预测模块5，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光SLAM扫描回调函数6；
载体运动参量估计模块4与下一时刻载体位姿预测模块5中的载体运动模型具体实现如图5和图6所示。
载体运动模型有两轮、四轮和全向轮模型，两轮和四轮载体运动模型的转弯与位移运动耦合，全向轮模型的转弯和位移运动是解耦的。因此，图5为两轮四轮载体运动模型的实现过程，而图6为全向轮载体模型的实现过程。
具体实现流程介绍如下：
(A)两轮与四轮模型
载体模型为两轮或者四轮模型时，如图5所示。获得载体位姿历史矩阵更新模块输入的历史矩阵和迭代周期dt，载体位姿历史矩阵分为代表载体位置X和载体姿态角的矩阵YAW，矩阵X包含两个变量x、y，分别代表载体位置x、y坐标。矩阵容量大小为N，在实际使用时可取为10或更大。
之后，进行转弯判断，若abs(YAW[N]-YAW[0])<thre_yaw，则载体未进行转弯，进入位移判断；反之，载体正处于转弯状态，进入右转判断。其中转弯判断门限thre_yaw一般取为0.01rad约1.145°。
位移判断，若||X[N]-X[0]||2＜thre_X，则载体静止，位移判断门限thre_X一般取0.05，下一时刻载体位姿预测结果仍然是当前时刻载体位姿信息，即
Figure BDA0003910533480000071
反之，载体进行位移，首先通过最小二乘估计载体位移速度，等权重利用式
Figure BDA0003910533480000072
退化权重利用式
Figure BDA0003910533480000073
其中
Figure BDA0003910533480000074
为待计算的载体速度信息，z为载体位姿历史矩阵中各时刻载体位姿信息，H为载体速度与各时刻载体位姿信息之间的积分关系矩阵，W为载体位姿历史矩阵更新模块3的退化权重方法的权重值，一般距离当前时刻越近的时刻载体位姿信息权重越大。之后利用下式进行载体位姿预测：
位置预测：k＝1,2…N-1,N
Figure BDA0003910533480000075
姿态角预测：
Figure BDA0003910533480000076
右转判断，对于两轮或者四轮模型，当载体旋转运动与位移运动耦合时，可以利用载体的姿态角构建载体不同时刻载体位置信息关系如下，其中
Figure BDA0003910533480000077
为载体位姿历史矩阵中第k个时刻载体位置信息，Xr0为载体右旋转点在载体本体系下的坐标，ψk、Δψk分别为第k个时刻载体姿态角、第k个时刻到第N个时刻载体姿态角变化量，
Figure BDA0003910533480000078
为载体第N个时刻的位置预测值。若N＝10，可以构造9组由X[k]预测
Figure BDA0003910533480000079
的组合，其中k＝1,2…,N-1。
Figure BDA00039105334800000710
Figure BDA00039105334800000711
ψk＝YAW[k],Δψk＝YAW[N]-YAW[k],k＝1,2…N-2,N-1
Figure BDA00039105334800000712
最终取第N个时刻载体位置预测值为
Figure BDA00039105334800000713
若载体位姿历史矩阵更新模块(3)使用退化权重更新载体位姿历史矩阵，那么
Figure BDA00039105334800000714
其中Wi、Wk分别为第i个、第k个时刻载体位姿信息的权重值，若最终
Figure BDA00039105334800000715
时载体处于右转状态，反之处于左转状态，右转判断门限thre_X一般取为0.05。
获取左转或右转状态后，通过最小二乘法估计载体旋转角速度
Figure BDA0003910533480000081
之后进行如下预测，即利用第1到第N个时刻载体历史位姿预测载体下一时刻位姿信息。其中，
Figure BDA0003910533480000082
为利用第k时刻位姿信息求载体下一时刻的位置预测值，姿态角预测值则为
Figure BDA0003910533480000083
Xz0为载体旋转点在载体本体系下的坐标，右转状态Xz0＝Xr0即载体右旋转点在载体本体系下的坐标，左转状态Xz0＝Xl0即载体左旋转点在载体本体系下的坐标。
Figure BDA0003910533480000084
Figure BDA0003910533480000085
Figure BDA0003910533480000086
Figure BDA0003910533480000087
和载体右转判断类似，最终的载体下一时刻位姿预测结果取值为：非退化权重位置预测为
Figure BDA0003910533480000088
姿态预测为
Figure BDA0003910533480000089
退化权重位置预测为
Figure BDA00039105334800000810
姿态预测为
Figure BDA00039105334800000811
其中Wi、Wk分别为第i个、第k个时刻载体位姿信息的权重值。获得的下一时刻载体位姿预测结果输出到激光SLAM扫描回调函数(6)。
(B)全向轮模型
载体模型为全向轮模型时，如图6所示。获得载体位姿历史矩阵更新模块输入的历史矩阵和迭代周期dt，载体位姿历史矩阵分为代表载体位置X和载体姿态角的矩阵YAW，矩阵X包含两个变量x、y，分别代表载体位置x、y坐标，
Figure BDA00039105334800000812
矩阵容量大小为N，在实际使用时可取为10或更大。
由于全向轮模型载体转弯与位移运动是解耦的，所以运动参量估计与下一时刻载体位姿预测算法较为简单，可以分解为x方向位移判断、y方向位移判断与转弯判断三个分支独立进行。
三个分支的判断条件为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。之后三个分支若判断有效，利用最小二乘法估计位移速度或旋转角速度。之后，载体下一时刻位姿预测流程可用下式表示，即流程图中
Figure BDA0003910533480000091
其中x[k]、y[k]来自于X矩阵中第k个时刻载体位置坐标即
Figure BDA0003910533480000092
k取1到N，即分别利用第1到第N时刻的载体时刻位置坐标预测下一时刻载体位置坐标，预测结果再进行平均或加权平均，得到最终结果。而姿态角的预测与位置坐标类似不再赘述。
位置预测：k＝1,2…N-1,N；
Figure BDA0003910533480000093
Figure BDA0003910533480000094
姿态预测：k＝1,2…N-1,N
Figure BDA0003910533480000095
最终，与(A)部分类似，有
Figure BDA0003910533480000096
即每个X位置量都包含x、y两个坐标。最后获得的下一时刻载体位姿预测结果进入激光SLAM扫描回调函数6进行下一步处理。
激光SLAM扫描回调函数6，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块1，完成闭环。不同于原始激光SLAM系统，此时位姿优化公式改变为下式，其中
Figure BDA0003910533480000097
代表下一时刻载体位姿预测结果即
Figure BDA0003910533480000098
代表利用
Figure BDA0003910533480000099
表示的栅格地图第i个像素点坐标，
Figure BDA00039105334800000910
代表第i个像素点占用概率，利用概率占用最大化求得最佳的位姿迭代变化量。
Figure BDA00039105334800000911
最终的位姿变化量
Figure BDA0003910533480000101
使得优化公式迭代速度更快，或者有限资源下迭代精度更高，所以本发明系统在载体进行高动态机动时，相较于原始激光SLAM系统有更好的工作性能。
通过1～6的迭代周期的循环，整个载体位姿估计系统以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配，通过迭代更新，输出载体的实时位姿估计。
如图2所示，本发明的一种用于小型地面载体的实时位姿估计系统实施例2，包括：当前载体位姿结果处理模块1、当前时刻环境栅格地图发布模块2、载体位姿历史矩阵更新模块3、载体运动参量估计模块4、下一时刻载体位姿预测模块5和激光SLAM扫描回调函数6；
当前载体位姿结果处理模块1，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块2和载体位姿历史矩阵更新模块3；初始化时，当前时刻载体位姿信息由外部GNSS系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光SLAM扫描回调函数6给出；
当前时刻环境栅格地图发布模块2，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光SLAM扫描回调函数6；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光SLAM扫描回调函数6；
载体位姿历史矩阵更新模块3，根据当前时刻载体位姿信息，利用里程计与IMU惯性系统信息融合结果对当前时刻位姿进行平滑，平滑处理后的结果存入载体位姿历史矩阵，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块5处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块4；所述设定的矩阵容量为十个或者二十个迭代周期；
载体运动参量估计模块4，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者Kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计；之后，将运动参量估计结果与惯性信息融合的估计结果进行加权处理，减小由地面载体颤动与非匀加速启动或制动带来的建图与定位偏差，惯性辅助的权重可以；最后，将运动参量估计结果输出到下一时刻载体位姿预测模块5；
下一时刻载体位姿预测模块5，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光SLAM扫描回调函数6；
激光SLAM扫描回调函数6，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块1，完成闭环。
依上述步骤，通过1～6的迭代周期的循环，整个载体位姿估计方法以下一时刻载体位姿预测结果与当前时刻环境栅格地图为基础，利用下一时刻激光雷达点云输出结果进行帧间匹配。通过迭代更新，输出载体的实时位姿估计。
上述实施例2中的其他部分与实施例1相同。
本发明的小型地面载体定义：
实验载体参数：
长30cm、宽20cm和高20cm以内。实际实验载体为麦克纳母轮小车底盘，可进行原地转弯与掉头。
数据支持：
由于高速转弯相较于直行对算法位姿估计技术检验效果更明显，所以这里给出同一旋转速度下现有算法与本发明算法的建图效果(由于栅格地图发布是在当前时刻载体位姿估计结果基础上进行的，所以最能直观体现局部环境中载体位姿估计偏差)。
如图3所示，载体旋转角速率1.5rad/s，即约86°/s，原地旋转360°，正常的环境栅格地图建图效果应该为，载体位置估计旋转前后为同一位置，同时发布的栅格地图无旋转叠图。图3中的(b)为本发明实时位姿估计系统建图效果，可以看到其基本满足正常建图效果要求；而图3中的(a)现有激光SLAM实时位姿估计系统建图效果则不满足要求，首先所建立的栅格地图有明显的旋转叠图，其次图中心实线为旋转过程中当前时刻载体位姿估计结果形成的移动路线，显然不再一个点处，已经漂出了比较远的距离。由以上对比可以看出本发明提出的实时位姿估计系统相较于现有激光SLAM实时位姿估计系统在载体进行高速旋转时工作性能有极大改善。
如图4所示，载体组合机动(旋转角速率1.5rad/s即约86°/s，直行速率0.3m/s)。载体机动路径为从旋转测试相同起始点出发，做“S”形机动行至阳台，掉头，做“S”形机动行至门口，掉头返回起始点。比较图4中的(a)和(b)可以发现现有激光SLAM系统建图效果较差，出现了和图3中的(a)一样的旋转叠图，系统失效不可用。尤其是在两个掉头转弯处影响最大，位姿估计结果路线明显偏离正常值，而图4中的(b)本发明位姿估计系统表现正常。以上说明了本发明实时位姿估计系统在载体进行高速机动时相较于现有激光SLAM实时位姿估计系统性能有很大提升。
供以上实施例仅仅是为了描述本发明的目的，而并非要限制本发明的范围。本发明的范围由所附权利要求限定。不脱离本发明的精神和原理而做出的各种等同替换和修改，均应涵盖在本发明的范围之内。

Claims (2)

1.一种用于小型地面载体的实时位姿估计系统，其特征在于，包括：当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)、载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)、下一时刻载体位姿预测模块(5)和激光SLAM扫描回调函数(6)；
当前载体位姿结果处理模块(1)，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块(2)和载体位姿历史矩阵更新模块(3)；初始化时，当前时刻载体位姿信息由外部GNSS系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光SLAM扫描回调函数(6)给出；
当前时刻环境栅格地图发布模块(2)，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光SLAM扫描回调函数(6)；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光SLAM扫描回调函数(6)；
载体位姿历史矩阵更新模块(3)，根据当前时刻载体位姿信息，利用定权重或者退化权重方法对载体位姿历史矩阵进行数据更新，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块(5)处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块(4)；所述矩阵容量设置为十个或者二十个迭代周期；
载体运动参量估计模块(4)，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者Kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计，将运动参量估计结果输出到下一时刻载体位姿预测模块(5)；
下一时刻载体位姿预测模块(5)，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光SLAM扫描回调函数(6)；
激光SLAM扫描回调函数(6)，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块(1)，完成闭环。
2.一种用于小型地面载体的实时位姿估计系统，其特征在于包括：当前载体位姿结果处理模块(1)、当前时刻环境栅格地图发布模块(2)、载体位姿历史矩阵更新模块(3)、载体运动参量估计模块(4)、下一时刻载体位姿预测模块(5)和激光SLAM扫描回调函数(6)；
当前载体位姿结果处理模块(1)，将获得的当前时刻载体位姿信息输出显示，同时提供给当前时刻栅格地图发布模块(2)和载体位姿历史矩阵更新模块(3)；初始化时，当前时刻载体位姿信息由外部GNSS系统给定或者其他方法初始化位姿；非初始化时，当前时刻载体位姿信息由激光SLAM扫描回调函数(6)给出；
当前时刻环境栅格地图发布模块(2)，初始化时，利用激光雷达的第一帧点云数据和当前时刻载体位姿信息发布初始化环境栅格至地图激光SLAM扫描回调函数(6)；非初始化时，利用当前时刻激光雷达的点云数据与当前时刻载体位姿信息，发布当前时刻环境栅格地图至激光SLAM扫描回调函数(6)；
载体位姿历史矩阵更新模块(3)，根据当前时刻载体位姿信息，利用里程计与IMU惯性系统信息融合结果对当前时刻位姿进行平滑，平滑处理后的结果存入载体位姿历史矩阵，同时估计载体位姿结果处理迭代周期时间用于下一时刻载体位姿预测模块(5)处理；当载体位姿历史矩阵中的载体位姿数据数目到达设定的矩阵容量后，输出载体位姿历史矩阵到载体运动参量估计模块(4)；所述设定的矩阵容量为十个或者二十个迭代周期；
载体运动参量估计模块(4)，根据载体位姿历史矩阵，在线下针对载体运动模型精确建模的基础上，通过最小二乘法或者Kalman滤波对载体的运动参量进行估计，包括运行状态即静止、左转、右转、直行的判别和运动速度变量即旋转半径、旋转角速率、直行速率的估计；之后，将运动参量估计结果与惯性信息融合的估计结果进行加权处理，减小由地面载体颤动与非匀加速启动或制动带来的建图与定位偏差；最后，将运动参量估计结果输出到下一时刻载体位姿预测模块(5)；
下一时刻载体位姿预测模块(5)，根据数据迭代周期与运动参量估计结果，结合载体运动模型，利用载体运动速度变量进行数据迭代周期时间长度的积分预测，进而获得下一时刻载体位姿预测结果，将下一时刻载体位姿预测结果输出至激光SLAM扫描回调函数(6)；
激光SLAM扫描回调函数(6)，对于下一时刻载体位姿预测结果，等待激光雷达下一时刻点云结果输出；当下一时刻点云结果输出时，利用当前时刻发布的栅格地图进行帧间匹配，帧间匹配的结果便是载体下一时刻载体位姿估计值，获得结果后再输出到当前时刻载体位姿处理模块(1)，完成闭环。
CN202211318943.XA 2022-10-26 2022-10-26 一种用于小型地面载体的实时位姿估计系统 Pending CN115683114A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211318943.XA CN115683114A (zh) 2022-10-26 2022-10-26 一种用于小型地面载体的实时位姿估计系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211318943.XA CN115683114A (zh) 2022-10-26 2022-10-26 一种用于小型地面载体的实时位姿估计系统

Publications (1)

Publication Number Publication Date
CN115683114A true CN115683114A (zh) 2023-02-03

Family

ID=85099279

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211318943.XA Pending CN115683114A (zh) 2022-10-26 2022-10-26 一种用于小型地面载体的实时位姿估计系统

Country Status (1)

Country Link
CN (1) CN115683114A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117709130A (zh) * 2024-02-05 2024-03-15 江苏深蓝航天有限公司 载体的瞬时运动参数的求解方法和装置
CN117709130B (zh) * 2024-02-05 2024-06-11 江苏深蓝航天有限公司 载体的瞬时运动参数的求解方法和装置

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117709130A (zh) * 2024-02-05 2024-03-15 江苏深蓝航天有限公司 载体的瞬时运动参数的求解方法和装置
CN117709130B (zh) * 2024-02-05 2024-06-11 江苏深蓝航天有限公司 载体的瞬时运动参数的求解方法和装置

Similar Documents

Publication Publication Date Title
JP7086111B2 (ja) 自動運転車のｌｉｄａｒ測位に用いられるディープラーニングに基づく特徴抽出方法
JP7060625B2 (ja) 自動運転車において３ｄｃｎｎネットワークを用いてソリューション推断を行うｌｉｄａｒ測位
CN113386781B (zh) 一种基于数据驱动车辆动力学模型的智能车辆轨迹跟踪控制方法
US11364931B2 (en) Lidar localization using RNN and LSTM for temporal smoothness in autonomous driving vehicles
CN112083726B (zh) 一种面向园区自动驾驶的双滤波器融合定位系统
CN113945206A (zh) 一种基于多传感器融合的定位方法及装置
CN106476728B (zh) 用于车载的车辆传感器的运动补偿
CN111142091B (zh) 一种融合车载信息的自动驾驶系统激光雷达在线标定方法
CN113238251B (zh) 一种基于车载激光雷达的目标级语义定位方法
CN112977411A (zh) 一种智能底盘控制方法及装置
CN112099378B (zh) 考虑随机测量时滞的前车侧向运动状态实时估计方法
CN111257853B (zh) 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法
CN111452786B (zh) 一种无人车辆避障方法及系统
CN111323167B (zh) 一种车辆质心高度在线辨识方法及系统
CN115683114A (zh) 一种用于小型地面载体的实时位姿估计系统
CN113060143B (zh) 一种路面附着系数确定系统和方法
CN114442634A (zh) 一种车辆的路径规划方法、装置、设备及介质
CN111857112B (zh) 一种汽车局部路径规划方法及电子设备
CN114265406B (zh) 一种基于机器视觉的智能车编队控制系统及其控制方法
CN115973162B (zh) 用于确定车辆行驶轨迹的方法、装置、电子设备和介质
CN115571156B (zh) 基于传感器融合的前车横向与纵向运动状态联合估计方法
Chen et al. An Intelligent Navigation Control Approach for Autonomous Unmanned Vehicles via Deep Learning-Enhanced Visual SLAM Framework
Zhou et al. Laser Vision Fusion Based on Unscented Kalman Filtering for Pose Estimation of Indoor Mobile Robot
Pan et al. Outdoor SLAM Using Monocular Vision-Based Localization with LIDAR-Aided Mapping for service robot in Highway
CN113819914A (zh) 一种地图构建方法及装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination