CN116429116A - 一种机器人定位方法及设备 - Google Patents
一种机器人定位方法及设备 Download PDFInfo
- Publication number
- CN116429116A CN116429116A CN202310450125.3A CN202310450125A CN116429116A CN 116429116 A CN116429116 A CN 116429116A CN 202310450125 A CN202310450125 A CN 202310450125A CN 116429116 A CN116429116 A CN 116429116A
- Authority
- CN
- China
- Prior art keywords
- robot
- point cloud
- cloud data
- imu
- state quantity
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 69
- 230000010354 integration Effects 0.000 claims abstract description 47
- 238000005457 optimization Methods 0.000 claims abstract description 44
- 238000010276 construction Methods 0.000 claims abstract description 22
- 230000006870 function Effects 0.000 claims description 22
- 238000005259 measurement Methods 0.000 claims description 19
- 238000006073 displacement reaction Methods 0.000 claims description 14
- 239000011159 matrix material Substances 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 10
- 238000007781 pre-processing Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 3
- 238000002203 pretreatment Methods 0.000 claims description 2
- 238000012163 sequencing technique Methods 0.000 claims description 2
- 238000004364 calculation method Methods 0.000 description 9
- 230000004927 fusion Effects 0.000 description 7
- 238000010586 diagram Methods 0.000 description 5
- 230000001133 acceleration Effects 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 238000012545 processing Methods 0.000 description 3
- 238000000354 decomposition reaction Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 238000000605 extraction Methods 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002123 temporal effect Effects 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/4802—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00 using analysis of echo signal for target characterisation; Target signature; Target cross-section
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Computer Networks & Wireless Communication (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种机器人定位方法及设备，属于机器人技术领域，用于解决仅依靠地图匹配进行机器人定位的方法定位误差较大，精度较低，影响机器人的正常运行的技术问题。方法包括：通过即时定位与地图构建法，确定机器人的先验位姿；根据相邻的两帧点云数据，构建激光里程计约束模型；基于预积分理论，构建相邻两帧点云数据的IMU预积分约束模型；在所述先验位姿的基础上，添加所述激光里程计约束模型以及所述IMU预积分约束模型，得到联合优化模型；通过所述联合优化模型对所述机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息；其中，所述最优状态量信息至少包括机器人的当前帧最优位姿。
Description
技术领域
本申请涉及机器人技术领域，尤其涉及一种机器人定位方法及设备。
背景技术
随着机器人技术的不断发展，机器人的应用场景也在不断增加。机器人在未知环境中从一个未知位置开始移动时，为使机器人进行自身定位，人们提出了即时定位与地图构建(Simultaneous Localization and Mapping,SLAM)方法，能够使机器人在移动过程中根据位置和地图进行自身定位，同时在自身定位的基础上建造增量式地图，实现机器人的自主定位和导航。
然而目前的机器人定位技术在SLAM的基础上，若只依靠地图匹配进行定位，在机器人实际行驶过程中容易产生较大误差。尤其在缺乏结构特征的室外环境下，地图匹配的定位效果较差，容易使机器人偏离线路，影响机器人的正常使用。
发明内容
本申请实施例提供了一种机器人定位方法及设备，用于解决如下技术问题：仅依靠地图匹配进行机器人定位的方法定位误差较大，精度较低，影响机器人的正常运行。
本申请实施例采用下述技术方案：
一方面，本申请实施例提供了一种机器人定位方法，方法包括：通过即时定位与地图构建法，确定机器人的先验位姿；根据相邻的两帧点云数据，构建激光里程计约束模型；基于预积分理论，构建相邻两帧点云数据的IMU预积分约束模型；在所述先验位姿的基础上，添加所述激光里程计约束模型以及所述IMU预积分约束模型，得到联合优化模型；通过所述联合优化模型对所述机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息；其中，所述最优状态量信息至少包括机器人的当前帧最优位姿。
在一种可行的实施方式中，通过即时定位与地图构建方法，确定机器人的先验位姿，具体包括：采用即时定位与地图构建法SLAM，构建机器人所在环境的三维点云地图；通过激光雷达采集扫描范围内的点云数据；通过体素滤波方法，对采集的点云数据进行预处理，以保留原始三维点云数据的特征信息，同时减少点云的数据量；所述预处理方法至少包括下采样处理；通过激光点云匹配算法NDT，将预处理后的所述点云数据与所述三维点云地图进行匹配定位，得到所述机器人的先验位姿。
在一种可行的实施方式中，根据相邻时刻采集的点云数据，构建激光里程计约束模型，具体包括：通过欧式聚类法，对预处理后的每一帧点云数据进行聚类，以剔除每一帧点云数据中的离群点和无效点；聚类完成后，计算每一帧点云数据中每个目标点的曲率值，并根据所述曲率值，确定出每一帧点云数据中的特征点；其中，所述特征点包括边缘特征点以及平面特征点；将当前帧点云数据及之前的多帧点云数据按照时间顺序进行排列，构建局部地图，并在所述局部地图中添加每帧点云数据的特征点；将所述局部地图中间时刻的点云数据，定义为所述局部地图的枢轴点；将当前帧点云数据以及所述局部地图中枢轴点之后的点云数据，分别与局部地图中的特征点进行匹配，完成所述激光里程计约束模型的构建。
在一种可行的实施方式中，计算每一帧点云数据中每个目标点的曲率值，并根据所述曲率值，确定出每一帧点云数据的平面特征点，具体包括：根据
计算当前帧点云数据中每个目标点所在的扫描层；其中，(x,y,z)表示目标点的坐标；s表示目标点所在的扫描层；Bu和Bl分别表示激光雷达垂直角度的上界和下界；L表示激光雷达的线数；根据/>
计算当前帧点云数据中所有目标点的曲率c；其中，/>
表示第K扫描层的第i个目标点；/>
表示/>
周围的目标点；S表示/>
周围的点集；将所有目标点按照所在扫描层以及在扫描层中所在的位置进行区域划分，并将每个区域中的目标点按照曲率值大小进行排序；将每个区域中曲率值大于预设阈值的点确定为边缘特征点，曲率值小于预设阈值的点确定为平面特征点。
在一种可行的实施方式中，将当前帧点云数据以及所述局部地图中枢轴点之后的点云数据，分别与局部地图中的平面特征点进行匹配，得到当前时刻机器人位姿相对于所述局部地图中间时刻的位姿变换关系，完成所述激光里程计约束模型的构建，具体包括：获取每个平面特征点
在局部地图中匹配的邻近点集/>
通过平面拟合方程：
对所述邻近点集/>
进行平面拟合，得到所述平面特征点匹配的拟合平面；其中，ωT表示平面方程的法向量；d表示平面到坐标原点的距离；x′表示平面特征点的邻近点；将三个参数组合在一起记为m＝[x,ω,d]；确定所述平面特征点与匹配的拟合平面之间的距离；根据所述平面特征点与匹配的拟合平面之间的距离、所述平面特征点的曲率值，通过柯西函数构建激光里程计的约束项，完成所述激光里程计约束模型的构建。
在一种可行的实施方式中，基于预积分理论，构建IMU预积分约束模型，用于获取相邻时刻机器人的状态量残差，具体包括：通过预积分理论，对IMU运动模型进行优化，以使所述IMU运动模型可将预设时间段内的IMU测量数据进行独立的积分运算；通过优化后的所述IMU运行模型，建立机器人相邻两时刻状态量信息之间的联系，得到机器人在相邻两时刻之间的位移信息；其中，所述位移信息至少包括位移、速度以及角度增量；根据所述机器人在相邻两时刻之间的位移信息，确定所述相邻两时刻之间机器人的状态量残差，完成所述IMU预积分约束模型的构建。
在一种可行的实施方式中，通过预积分理论，对IMU运动模型进行优化，以使所述IMU运动模型可将预设时间段内的IMU测量数据进行独立的积分运算，具体包括：通过预积分理论，将IMU运动模型相对于世界坐标系的姿态分解为：
得到优化后的IMU运动模型；其中，/>
表示IMU运动模型第t时刻相对于世界坐标系的姿态；
表示IMU运动模型第i时刻相对于世界坐标系的姿态，/>
表示IMU运动模型第t时刻相对于第i时刻的惯性测量单元坐标系的姿态；通过优化后的IMU运动模型，对预设时间段内的IMU测量数据进行积分运算，得到优化后的IMU运动模型的预积分量。
在一种可行的实施方式中，在所述先验位姿的基础上，添加所述激光里程计约束模型以及所述IMU预积分约束模型，得到联合优化模型；通过所述联合优化模型对所述机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息，具体包括：将所述先验位姿作为位姿初始值，并添加激光里程计约束模型以及IMU预积分约束模型，构成所述联合优化模型；通过滑动窗口，确定机器人最近时刻的状态量节点；将所述机器人最近时刻的状态量节点对应的状态量作为优化变量，构建目标函数；通过所述联合优化模型，采用非线性优化方法对目标函数优化求解，输出机器人的最优状态量。
在一种可行的实施方式中，将所述机器人最近时刻的状态量节点对应的状态量作为优化变量，构建目标函数，并采用非线性优化的方法对目标函数优化求解，输出机器人的最优状态量，具体包括：将所述机器人最近时刻的状态量作为优化变量，构建目标函数J(X)：
另一方面，本申请实施例还提供了一种机器人定位设备，包括：至少一个处理器；以及，与所述至少一个处理器通信连接的存储器；其中，所述存储器存储有能够被所述至少一个处理器执行的指令，以使所述至少一个处理器能够执行根据上述任一实施方式所述的一种机器人定位方法。
相比于现有技术，本申请实施例提供的一种机器人定位方法及设备，具有如下有益效果：
本发明在地图匹配的先验位姿的基础上，添加了激光里程计约束和IMU预积分约束，然后通过非线性优化的方法优化机器人的状态量。本发明将先验地图数据、激光雷达数据和IMU数据充分结合，并通过滑动窗口的方法保留并优化机器人最近时刻的状态量节点，满足机器人定位实时性的同时，进一步提升了机器人位姿的定位精度，实现了机器人的实时准确定位。
附图说明
为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请中记载的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。在附图中：
图1为本申请实施例提供的一种机器人定位方法流程图；
图2为本申请实施例提供的一种局部地图示意图；
图3为本申请实施例提供的一种联合优化模型示意图；
图4为本申请实施例提供的一种机器人定位设备的结构示意图。
具体实施方式
为了使本技术领域的人员更好地理解本申请中的技术方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本说明书实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都应当属于本申请保护的范围。
本申请实施例提供了一种机器人定位方法，如图1所示，机器人定位方法具体包括步骤S101-S104：
S101、通过即时定位与地图构建法，确定机器人的先验位姿。
具体地，首先根据激光雷达采集的周围环境的点云数据，采用即时定位与地图构建法SLAM，构建机器人所在环境的三维点云地图。然后通过激光雷达采集初始时刻扫描范围内的点云数据，并通过体素滤波方法，对采集的初始时刻点云数据进行预处理，预处理方法可以是下采样处理，以保留原始三维点云数据的特征信息，同时减少点云的数据量，减轻后续计算压力，以满足定位算法的实时性。
进一步地，通过激光点云匹配算法NDT，将预处理后的点云数据与三维点云地图进行匹配定位，得到机器人的先验位姿。
S102、根据相邻的两帧点云数据，构建激光里程计约束模型。
为了进一步提升机器人的定位精度，需要结合上一时刻和当前时刻的两帧点云数据构建激光里程计约束模型。
构建激光里程计约束模型的具体步骤为：
步骤1021：通过欧式聚类法，对预处理后的每一帧点云数据进行聚类，以剔除每一帧点云数据中的离群点和无效点。
步骤1022：点云聚类完成后，计算每一帧点云数据中每个目标点的曲率值，并根据曲率值，确定出每一帧点云数据中的边缘特征点和平面特征点。
步骤1023：依据提取的特征点进行匹配，从而建立激光里程计约束。由于单帧点云数据中的特征点较稀疏，容易产生特征点匹配偏差，因此本发明采用构建局部地图的方法，在局部地图中添加多帧点云数据的特征点，从而保证当前帧特征点匹配的准确性。
具体地，激光里程计约束的构建主要包括点云的聚类和特征提取，然后依据提取的特征点进行匹配，从而建立相邻两帧点云之间的约束关系。具体的实施方式如下：
步骤1021：采用欧式聚类的方法对点云数据进行处理，将点云数据中的任意一点定义为P0，然后寻找该点附近的其他目标点，并将该点附近目标点的集合定义为S。设定临界值r，计算目标点集合S中各目标点与P0之间的距离，当两者之间的距离小于临界值r时，将该目标点放入同一类别的容器Q中。
进一步地，在容器Q内寻找除P0外的任意一点，重复距离计算的操作，更新容器Q；通过不断的对容器Q进行更新，直至搜索完全部目标点。容器Q中的点即表示点云数据聚类后的结果。
步骤1022：点云聚类完成后，计算每一帧点云中各个目标点的曲率，并根据曲率值提取边缘特征点和平面特征点。首先计算当前帧点云数据中每个目标点所在的扫描层，方便后期通过扫描层进行点云特征提取。计算方式如下：
式中：(x,y,z)表示目标点的坐标；s表示目标点所在的扫描层；Bu和Bl分别表示激光雷达垂直角度的上界和下界；L表示激光雷达线数。
进一步地，扫描层计算完成后，将点云数据分别按层数和区域划分，并从各个区域中提取相应的特征点。通过对特征点进行分区域提取，防止特征点出现聚集的现象。通过下式计算当前帧点云数据中所有目标点的曲率：
式中：
表示第K扫描层的第i个目标点；/>
表示/>
周围的目标点；S表示/>
周围的点集。
进一步地，在各个目标点完成曲率计算后，根据曲率值对各扫描层的目标点进行区域划分，并将每个区域中的目标点按照曲率值大小进行排序。将每个区域中曲率值大于预设阈值的点确定为边缘特征点，曲率值小于预设阈值的点确定为平面特征点。即曲率较大的点为边缘特征点，曲率较小的点为平面特征点。
步骤1023：依据提取的特征点进行匹配从而建立激光里程计约束，由于单帧点云数据中的特征点较稀疏，容易产生特征点匹配偏差，因此本发明采用构建局部地图的方法，在局部地图中添加多帧点云数据的特征点，从而保证当前帧特征点匹配的准确性。
图2为本申请实施例提供的一种局部地图示意图，如图2所示，该局部地图中添加了第o时刻到第i时刻的所有关键帧，其中第p时刻的点云数据为该局部地图的枢轴点。第j时刻的点云数据为当前时刻的点云数据，第i时刻的点云数据则表示当前点云数据的前一帧。局部地图中的所有特征点信息将统一建立在第p时刻的激光雷达坐标系下，因此采用优化后的位姿信息对特征点进行坐标变换。在构建激光里程计约束时，其约束的状态量包括当前帧以及第p时刻之后的全部关键帧。
进一步地，完成局部地图构建后，将当前帧第j时刻的点云数据以及局部地图中第p时刻之后的关键帧，分别与局部地图中的特征点进行匹配，从而得到当前帧点云数据以及第p时刻之后的关键帧相对于第p时刻关键帧的位姿变换关系。为了降低激光里程约束的复杂度，同时提高后续联合优化的效率，本发明仅通过平面特征点来构建激光里程计约束。
在特征点匹配过程中，假设提取的平面特征点为
则匹配后在局部地图中的邻近点集用/>
表示。对于邻近点集/>
进行平面拟合。拟合后的平面方程为：
式中：ωT表示平面方程的法向量；d表示平面到坐标原点的距离；x′表示平面特征点的邻近点；将三个参数组合在一起记为m＝[x,ω,d]。
进一步地，通过激光里程计约束对位姿进行优化，每次优化的状态量都包括当前帧的位姿、局部地图中第p时刻的位姿
以及局部地图中第p时刻之后的关键帧位姿/>
α∈{p+1,...,j}。
由于融合定位系统的状态量定义在惯性测量单元坐标系中，而本发明中的激光里程计约束定义在激光雷达坐标系中，因此需要引入两坐标系之间的外参
在局部地图中，第p时刻及其后续时刻的关键帧之间位姿变换关系可以定义为：
上式中，
表示第p时刻关键帧与第α时刻关键帧之间位姿变换矩阵，
进一步地，根据特征点到匹配平面的距离，可以建立如下方程：
进一步地，为了降低特征点误匹配对定位系统的影响，将引入柯西函数
通过柯西函数构建激光里程计约束项，以提升系统的鲁棒性。
根据柯西函数得到激光里程计约束项：
S103、基于预积分理论，构建相邻两帧点云数据的IMU预积分约束模型。
根据IMU运动模型，通过对陀螺仪和加速度计在一段时间内的测量数据进行积分，可以求解出下一时刻的机器人位姿信息。为了避免重复计算并减轻计算压力，本发明采用预积分理论对IMU运动模型进行优化，将IMU的测量数据进行独立的积分运算，构建相邻两帧点云数据的IMU预积分约束模型。
构建IMU预积分约束模型的具体步骤为：
步骤1031：通过预积分理论，对IMU运动模型进行优化，以使IMU运动模型可将预设时间段内的IMU测量数据进行独立的积分运算；
步骤1032：通过优化后的IMU运行模型，建立机器人相邻两时刻状态量信息之间的联系，得到机器人在相邻两时刻之间的位移信息；其中，位移信息至少包括位移、速度以及角度增量。
步骤1033：根据机器人在相邻两时刻之间的位移信息，进一步确定相邻两时刻之间机器人的状态量残差，完成IMU预积分约束模型的构建。
具体地，激光里程计约束构建完成后，通过预积分模型建立相邻两时刻状态量之间的联系，从而建立IMU预积分约束，具体的实施方式如下：
步骤1031：采用预积分理论对IMU运动模型进行优化。IMU与激光雷达的运动状态可以通过外参相结合，融合定位系统中所有运动状态为：
上式中，下标w表示世界坐标系，下标b表示IMU坐标系，
表示第i时刻IMU的状态量，/>
分别表示IMU的位置、速度和姿态，/>
表示第i时刻加速度计的零偏，/>
表示第i时刻陀螺仪的零偏。/>
表示两坐标系之间的变换关系，即外参。其中
和/>
分别为两个坐标系之间的平移量和旋转量。融合定位系统需要实时估计并优化状态量/>
当融合定位系统不能提供外参/>
时，外参也将作为融合定位系统的状态变量进行优化。IMU与激光雷达之间的外参初始值通过标定获得，同时为了提高融合定位系统的稳定性，应提供较为精确的外参初始值。
进一步地，在IMU的运动模型中，主要通过第i时刻的系统初始状态和IMU采集的加速度
和角速度/>
求解第j时刻的系统状态。通过对测量数据进行积分运算，进一步得到位置、速度和角度信息。IMU运动积分模型可以表示为：
式中：gω表示重力加速度，为了避免重复计算并减轻计算压力，采用预积分理论对IMU的运动模型进行优化，将IMU姿态
进行分解如下：/>
式中：
表示IMU运动模型第t时刻相对于世界坐标系的姿态；/>
表示IMU第i时刻相对于世界坐标系的姿态，/>
表示IMU第t时刻相对于第i时刻惯性测量单元坐标系的姿态。
进一步地，分解之后，IMU的预积分模型可通过以下公式来表达：
由于该公式中的积分项只涉及IMU的测量数据，当i时刻的姿态发生变化时，积分项仍保持不变。因此可以通过对IMU任意一段时间内的测量数据进行积分运算，从而直接获得预积分量，令：
其中，
表示从i时刻到j时刻的位置测量数据积分运算值；/>
表示从i时刻到j时刻的速度测量数据积分运算值。
步骤1032：通过预积分模型建立相邻两时刻状态量之间的联系，从而得到相邻两时刻的位移、速度和角度增量：
步骤1043：将相邻两时刻的位移、速度和角度增量等数据进一步优化为两个时刻之间状态量残差的形式：
其中，rp表示平移预积分残差；rq表示旋转预积分残差；rv表示速度预积分残差；rba表示加速度偏置残差；rbg表示角加速度偏置残差。
S104、在先验位姿的基础上，通过激光里程计约束模型以及IMU预积分约束模型，对机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息。
在先验位姿的基础上，添加激光里程计约束和IMU预积分约束，然后通过非线性联合优化输出机器人优化后的状态量信息。
具体步骤如下：
步骤1041：将先验位姿作为位姿初始值，在非线性联合优化模型中添加激光里程计约束模型以及IMU预积分约束模型。
步骤1042：随着机器人行驶距离的增加，优化状态量节点也在不断增加，为了满足机器人定位的实时性，采用滑动窗口的方法，仅保留并优化机器人最近时刻的状态量节点。
步骤1043：将机器人最近时刻的状态量节点对应的状态量作为优化变量，构建目标函数，并采用非线性优化的方法对目标函数优化求解，输出机器人的最优状态量。
具体地，激光里程计约束和IMU预积分约束构建完成后，在地图匹配先验位姿的基础上添加两种约束，然后通过非线性优化的方法对机器人位姿进行优化，具体实施方式如下：
步骤1041：在地图匹配先验位姿的基础上添加激光里程计约束和IMU预积分约束，构成联合优化模型。
图3为本申请实施例提供的一种联合优化模型示意图，如图3所示，地图匹配先验位姿为位姿初始值，相邻两帧待优化雷达位姿之间添加了激光里程计约束以及IMU预积分约束。
步骤1042：随着机器人行驶距离的增加，优化状态量节点也在不断增加，为了满足机器人定位的实时性，采用滑动窗口的方法仅保留并优化机器人最近时刻的状态量节点，剔除旧的状态量节点。如图3所示，滑动窗口范围内仅保留机器人最近时刻的待优化节点，优化完毕的雷达位姿不在滑动窗口范围内。
步骤1043：将机器人最近时刻的状态量作为优化变量构建目标函数，将地图匹配的定位结果作为位姿初始值，在非线性联合优化模型中添加激光里程计约束
即激光雷达点到其匹配平面的残差，其中/>
表示协方差矩阵。添加IMU预积分约束/>
其中/>
是协方差矩阵；
添加边缘化残差||rp(X)||2；从而构建出联合优化的目标函数：
式中：
表示各优化帧与局部地图组成的匹配特征点对。
最后，采用非线性优化的方法对目标函数进行求解，输出机器人的最优状态量信息。最优状态量信息中至少包括机器人的当前帧最优位姿。
另外，本申请实施例还提供了一种机器人定位设备，如图4所示，设备具体包括：至少一个处理器；以及，与至少一个处理器通信连接的存储器；其中，存储器存储有能够被至少一个处理器执行的指令，以使至少一个处理器能够执行：
通过即时定位与地图构建法，确定机器人的先验位姿；
根据相邻的两帧点云数据，构建激光里程计约束模型；
基于预积分理论，构建相邻两帧点云数据的IMU预积分约束模型；
在所述先验位姿的基础上，添加所述激光里程计约束模型以及所述IMU预积分约束模型，得到联合优化模型；通过所述联合优化模型对所述机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息；
其中，所述最优状态量信息至少包括机器人的当前帧最优位姿。
本申请中的各个实施例均采用递进的方式描述，各个实施例之间相同相似的部分互相参见即可，每个实施例重点说明的都是与其他实施例的不同之处。尤其，对于设备实施例而言，由于其基本相似于方法实施例，所以描述的比较简单，相关之处参见方法实施例的部分说明即可。
上述对本申请特定实施例进行了描述。另外，在附图中描绘的过程不一定要求示出的特定顺序或者连续顺序才能实现期望的结果。在某些实施方式中，多任务处理和并行处理也是可以的或者可能是有利的。
以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请的实施例可以有各种更改和变化。凡在本申请实施例的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。
Claims (10)
1.一种机器人定位方法，其特征在于，所述方法包括：
通过即时定位与地图构建法，确定机器人的先验位姿；
根据相邻的两帧点云数据，构建激光里程计约束模型；
基于预积分理论，构建相邻两帧点云数据的IMU预积分约束模型；
在所述先验位姿的基础上，添加所述激光里程计约束模型以及所述IMU预积分约束模型，得到联合优化模型；通过所述联合优化模型对所述机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息；
其中，所述最优状态量信息至少包括机器人的当前帧最优位姿。
2.根据权利要求1所述的一种机器人定位方法，其特征在于，通过即时定位与地图构建方法，确定机器人的先验位姿，具体包括：
采用即时定位与地图构建法SLAM，构建机器人所在环境的三维点云地图；
通过激光雷达采集扫描范围内的点云数据；
通过体素滤波方法，对采集的点云数据进行预处理，以保留原始三维点云数据的特征信息，同时减少点云的数据量；所述预处理方法至少包括下采样处理；
通过激光点云匹配算法NDT，将预处理后的所述点云数据与所述三维点云地图进行匹配定位，得到所述机器人的先验位姿。
3.根据权利要求1所述的一种机器人定位方法，其特征在于，根据相邻时刻采集的点云数据，构建激光里程计约束模型，具体包括：
通过欧式聚类法，对预处理后的每一帧点云数据进行聚类，以剔除每一帧点云数据中的离群点和无效点；
聚类完成后，计算每一帧点云数据中每个目标点的曲率值，并根据所述曲率值，确定出每一帧点云数据中的特征点；其中，所述特征点包括边缘特征点以及平面特征点；
将当前帧点云数据及之前的多帧点云数据按照时间顺序进行排列，构建局部地图，并在所述局部地图中添加每帧点云数据的特征点；
将所述局部地图中间时刻的点云数据，定义为所述局部地图的枢轴点；
将当前帧点云数据以及所述局部地图中枢轴点之后的点云数据，分别与局部地图中的特征点进行匹配，完成所述激光里程计约束模型的构建。
4.根据权利要求3所述的一种机器人定位方法，其特征在于，计算每一帧点云数据中每个目标点的曲率值，并根据所述曲率值，确定出每一帧点云数据的平面特征点，具体包括：
根据
计算当前帧点云数据中每个目标点所在的扫描层；其中，(x,y,z)表示目标点的坐标；s表示目标点所在的扫描层；Bu和Bl分别表示激光雷达垂直角度的上界和下界；L表示激光雷达的线数；
根据
计算当前帧点云数据中所有目标点的曲率c；其中，/>
表示第K扫描层的第i个目标点；/>
表示/>
周围的目标点；S表示/>
周围的点集；
将所有目标点按照所在扫描层以及在扫描层中所在的位置进行区域划分，并将每个区域中的目标点按照曲率值大小进行排序；
将每个区域中曲率值大于预设阈值的点确定为边缘特征点，曲率值小于预设阈值的点确定为平面特征点。
5.根据权利要求3所述的一种机器人定位方法，其特征在于，将当前帧点云数据以及所述局部地图中枢轴点之后的点云数据，分别与局部地图中的平面特征点进行匹配，得到当前时刻机器人位姿相对于所述局部地图中间时刻的位姿变换关系，完成所述激光里程计约束模型的构建，具体包括：
获取每个平面特征点
在局部地图中匹配的邻近点集/>
通过平面拟合方程：
对所述邻近点集/>
进行平面拟合，得到所述平面特征点匹配的拟合平面；其中，ωT表示平面方程的法向量；d表示平面到坐标原点的距离；x′表示平面特征点的邻近点；将三个参数组合在一起记为m＝[x,ω,d]；
确定所述平面特征点与匹配的拟合平面之间的距离；
根据所述平面特征点与匹配的拟合平面之间的距离、所述平面特征点的曲率值，通过柯西函数构建激光里程计的约束项，完成所述激光里程计约束模型的构建。
6.根据权利要求1所述的一种机器人定位方法，其特征在于，基于预积分理论，构建IMU预积分约束模型，用于获取相邻时刻机器人的状态量残差，具体包括：
通过预积分理论，对IMU运动模型进行优化，以使所述IMU运动模型可将预设时间段内的IMU测量数据进行独立的积分运算；
通过优化后的所述IMU运行模型，建立机器人相邻两时刻状态量信息之间的联系，得到机器人在相邻两时刻之间的位移信息；其中，所述位移信息至少包括位移、速度以及角度增量；
根据所述机器人在相邻两时刻之间的位移信息，确定所述相邻两时刻之间机器人的状态量残差，完成所述IMU预积分约束模型的构建。
7.根据权利要求6所述的一种机器人定位方法，其特征在于，通过预积分理论，对IMU运动模型进行优化，以使所述IMU运动模型可将预设时间段内的IMU测量数据进行独立的积分运算，具体包括：
通过预积分理论，将IMU运动模型相对于世界坐标系的姿态分解为：
得到优化后的IMU运动模型；其中，/>
表示IMU运动模型第t时刻相对于世界坐标系的姿态；/>
表示IMU运动模型第i时刻相对于世界坐标系的姿态，
表示IMU运动模型第t时刻相对于第i时刻的惯性测量单元坐标系的姿态；
通过优化后的IMU运动模型，对预设时间段内的IMU测量数据进行积分运算，得到优化后的IMU运动模型的预积分量。
8.根据权利要求1所述的一种机器人定位方法，其特征在于，在所述先验位姿的基础上，添加所述激光里程计约束模型以及所述IMU预积分约束模型，得到联合优化模型；通过所述联合优化模型对所述机器人的各状态量节点进行优化，并输出当前状态量节点的最优状态量信息，具体包括：
将所述先验位姿作为位姿初始值，并添加激光里程计约束模型以及IMU预积分约束模型，构成所述联合优化模型；
通过滑动窗口，确定机器人最近时刻的状态量节点；
将所述机器人最近时刻的状态量节点对应的状态量作为优化变量，构建目标函数；
通过所述联合优化模型，采用非线性优化方法对目标函数优化求解，输出机器人的最优状态量。
9.根据权利要求8所述的一种机器人定位方法，其特征在于，将所述机器人最近时刻的状态量节点对应的状态量作为优化变量，构建目标函数，并采用非线性优化的方法对目标函数优化求解，输出机器人的最优状态量，具体包括：
将所述机器人最近时刻的状态量作为优化变量，构建目标函数J(X)：
其中，||rp(X)||2为边缘化残差值；
表示各优化帧与局部地图组成的匹配特征点对；/>
为所述激光里程计约束模型；/>
为激光里程计约束的协方差矩阵；p为局部地图的中间时刻；/>
为IMU预积分约束模型；/>
为IMU预积分约束的协方差矩阵；
通过非线性优化方法对所述目标函数进行求解，得到所述机器人的最优状态量。
10.一种机器人定位设备，其特征在于，所述设备包括：
至少一个处理器；以及，
与所述至少一个处理器通信连接的存储器；其中，
所述存储器存储有能够被所述至少一个处理器执行的指令，以使所述至少一个处理器能够执行根据权利要求1-9任一项所述的一种机器人定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310450125.3A CN116429116A (zh) | 2023-04-20 | 2023-04-20 | 一种机器人定位方法及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310450125.3A CN116429116A (zh) | 2023-04-20 | 2023-04-20 | 一种机器人定位方法及设备 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116429116A true CN116429116A (zh) | 2023-07-14 |
Family
ID=87085273
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310450125.3A Pending CN116429116A (zh) | 2023-04-20 | 2023-04-20 | 一种机器人定位方法及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116429116A (zh) |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115507836A (zh) * | 2021-06-23 | 2022-12-23 | 同方威视技术股份有限公司 | 用于确定机器人位置的方法以及机器人 |
| CN117451033A (zh) * | 2023-12-21 | 2024-01-26 | 广东石油化工学院 | 一种同步定位与地图构建方法、装置、终端及介质 |
-
2023
- 2023-04-20 CN CN202310450125.3A patent/CN116429116A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115507836A (zh) * | 2021-06-23 | 2022-12-23 | 同方威视技术股份有限公司 | 用于确定机器人位置的方法以及机器人 |
| CN115507836B (zh) * | 2021-06-23 | 2024-02-02 | 同方威视技术股份有限公司 | 用于确定机器人位置的方法以及机器人 |
| CN117451033A (zh) * | 2023-12-21 | 2024-01-26 | 广东石油化工学院 | 一种同步定位与地图构建方法、装置、终端及介质 |
| CN117451033B (zh) * | 2023-12-21 | 2024-05-14 | 广东石油化工学院 | 一种同步定位与地图构建方法、装置、终端及介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Shan et al. | Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping | |
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| Ye et al. | Tightly coupled 3d lidar inertial odometry and mapping | |
| CN106840148B (zh) | 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法 | |
| WO2021232470A1 (zh) | 基于多传感器融合的slam制图方法、系统 | |
| CN112634451B (zh) | 一种融合多传感器的室外大场景三维建图方法 | |
| CN112304307A (zh) | 一种基于多传感器融合的定位方法、装置和存储介质 | |
| CN113781582A (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN116429116A (zh) | 一种机器人定位方法及设备 | |
| CN114001733B (zh) | 一种基于地图的一致性高效视觉惯性定位算法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN112965063B (zh) | 一种机器人建图定位方法 | |
| CN112833892B (zh) | 一种基于轨迹对齐的语义建图方法 | |
| CN115479598A (zh) | 基于多传感器融合的定位与建图方法及紧耦合系统 | |
| CN116429084A (zh) | 一种动态环境3d同步定位与建图方法 | |
| Xu et al. | Direct visual-inertial odometry with semi-dense mapping | |
| Jaenal et al. | Improving visual SLAM in car-navigated urban environments with appearance maps | |
| CN115950414A (zh) | 一种不同传感器数据的自适应多重融合slam方法 | |
| CN114993338B (zh) | 基于多段独立地图序列的一致性高效视觉惯性里程计算法 | |
| CN116380039A (zh) | 一种基于固态激光雷达和点云地图的移动机器人导航系统 | |
| CN115984463A (zh) | 一种适用于狭窄巷道的三维重建方法和系统 | |
| CN115482282A (zh) | 自动驾驶场景下具有多目标追踪能力的动态slam方法 | |
| CN115046543A (zh) | 一种基于多传感器的组合导航方法及系统 | |
| Hu et al. | Efficient Visual-Inertial navigation with point-plane map | |
| Sun et al. | Indoor Li-DAR 3D mapping algorithm with semantic-based registration and optimization |
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 |