CN112967392A - 一种基于多传感器触合的大规模园区建图定位方法 - Google Patents
一种基于多传感器触合的大规模园区建图定位方法 Download PDFInfo
- Publication number
- CN112967392A CN112967392A CN202110246946.6A CN202110246946A CN112967392A CN 112967392 A CN112967392 A CN 112967392A CN 202110246946 A CN202110246946 A CN 202110246946A CN 112967392 A CN112967392 A CN 112967392A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- positioning
- sensor
- scale park
- scale
- 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 54
- 238000013507 mapping Methods 0.000 title claims abstract description 21
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 23
- 230000010354 integration Effects 0.000 claims abstract description 9
- 230000008447 perception Effects 0.000 claims abstract description 7
- 230000007613 environmental effect Effects 0.000 claims abstract description 6
- 238000012546 transfer Methods 0.000 claims abstract description 5
- 238000005259 measurement Methods 0.000 claims description 19
- 230000005484 gravity Effects 0.000 claims description 18
- 230000006870 function Effects 0.000 claims description 16
- 230000009466 transformation Effects 0.000 claims description 16
- 239000013598 vector Substances 0.000 claims description 15
- 239000011159 matrix material Substances 0.000 claims description 12
- 230000033001 locomotion Effects 0.000 claims description 11
- 238000009795 derivation Methods 0.000 claims description 6
- 238000005516 engineering process Methods 0.000 claims description 6
- 238000005457 optimization Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 claims description 4
- 230000008859 change Effects 0.000 claims description 4
- 230000008569 process Effects 0.000 claims description 4
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000000605 extraction Methods 0.000 claims description 3
- 230000009191 jumping Effects 0.000 claims description 3
- 238000012544 monitoring process Methods 0.000 claims description 3
- 238000011946 reduction process Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 238000010276 construction Methods 0.000 abstract description 7
- 238000004891 communication Methods 0.000 description 13
- 238000000889 atomisation Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000001914 filtration Methods 0.000 description 2
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 238000007405 data analysis Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/05—Geographic models
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T17/00—Three dimensional [3D] modelling, e.g. data description of 3D objects
- G06T17/20—Finite element generation, e.g. wire-frame surface description, tesselation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T19/00—Manipulating 3D models or images for computer graphics
- G06T19/20—Editing of 3D images, e.g. changing shapes or colours, aligning objects or positioning parts
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2219/00—Indexing scheme for manipulating 3D models or images for computer graphics
- G06T2219/20—Indexing scheme for editing of 3D models
- G06T2219/2004—Aligning objects, relative positioning of parts
Abstract
本发明涉及提供一种基于多传感器触合的大规模园区建图定位方法，包括步骤：通过激光雷达对大规模园区进行环境感知和三维点云信息采集，通过IMU传感器的观测结果进行预积分来辅助Lidar的帧‑局部地图匹配的定位；利用帧‑局部地图匹配方法对大规模园区点云地图的构建；采用NDT配准算法对大规模园区内所有机器人进行实时定位。通过本发明，能够在没有环境信息的基础上，对周围环境进行地图构建并定位，提高了智能车地图构建和即时定位的能力，精度高，实时性好，应用前景广泛。
Description
技术领域
本发明属于地图构建与定位技术领域，更具体地说，特别涉及一种基于多传感器触合的大规模园区建图定位方法。
背景技术
工业园区是自动驾驶技术的重要场景，目前园区自动驾驶技术已经成功应用于园区摆渡、仓储物流、环卫、医院物资运输等场景。与摇杆操作控制的方式不同，智能车是通过传感器对自身及周围环境感知定位，结合检测的障碍物信息和地图信息完成自主导航功能。通常采用全球定位系统(GPS)完成定位，然而在现实中，外部干扰是不可避免的，在室内、严重遮挡或电子干扰的环境下会因为GPS信号丢失导致定位失败。因此，如何给智能车提供精确、可靠的位置信息，已经成为智能车定位优先考虑的问题。
发明内容
本发明为了解决上述提到的问题，提供了一种基于多传感器触合的大规模园区建图定位方法。
本发明解决其技术问题所采用的技术方案是：构造一种基于多传感器触合的大规模园区建图定位方法，包括：
通过激光雷达对大规模园区进行环境感知和三维点云信息采集，通过IMU传感器的观测结果进行预积分来辅助Lidar的帧-局部地图匹配的定位；
利用帧-局部地图匹配方法对大规模园区点云地图的构建；
采用NDT配准算法对大规模园区内所有机器人进行实时定位。
其中，在通过激光雷达对大规模园区进行环境感知和三维点云信息采集的步骤中，包括步骤：
通过激光雷达对大规模园区周围环境进行实时监测，包括障碍物距离信息和反射强度信息，对周围环境感知并采集三维点云信息；
利用距离信息和对应姿态角的三角函数值可以得到点云在激光雷达坐标系下的坐标值。
其中，在通过IMU传感器的观测结果进行预积分来辅助Lidar的帧-局部地图匹配的定位的步骤中，包括：
通过IMU传感器的三轴垂直测量的加速度计，根据牛顿第二定律的电容变化测量得到三轴；
通过IMU传感器的陀螺仪，测量运动中物体的惯性科氏力，从而间接测量三轴角速度。
其中，三维点云信息采集的步骤还包括：采用RTK差分定位技术，通过建立已知定位信息基站的卫导天线接受GPS信号，随后移动站通过4G差分模块数传天线来接收差分位置信号，根据差分信号矫正移动站GPS传感器接收的原始定位信息，从而得到准确的位置和方位信息。
其中，在利用帧-局部地图匹配方法对大规模园区点云地图的构建的步骤中，包括：
步骤1：定义坐标系与符号，将世界坐标系表示为W，机器人主体坐标系表示为B，假定IMU坐标系与机器人主体坐标系重合，机器人的状态写成：
x＝[RT，pT，vT，bT]T
其中R∈SO(3)为旋转矩阵，p∈R3为位置向量，v为速度，b为IMU偏置；从B到W的变换T∈SE(3)表示为T＝[R|p]；
步骤2：IMU传感器的角速度和加速度测量值通过公式定义：
其中
和t是时间t时B中的原始IMU测量值；
和t受到缓慢变化的偏置
和白噪声
的影响；
是从W到B的旋转矩阵；g是以W为单位的恒定重力矢量；
步骤3：使用IMU传感器的测量结果来推断机器人的运动；机器人在时间t+Δt的速度、位置和旋转可计算如下所示:
步骤4：获得两个时间步长之间的相对机器人运动，时间i和j之间的预积分测量值vij、pij和Rij使用如下公式计算:
其中，具备通信功能的电子雾化装置与所述集成无线通信模块的共享电子雾化设备管理装置之间的通信方式兼容USART通信、脉冲通信或NFC近场通信通信方式。
其中，当激光雷达的新的点云扫描到达时，首先执行特征提取；通过评估局部区域上的点的粗糙度来提取边缘和平面特征；粗糙度值大的点被归类为边缘特征；类似地，平面特征由小粗糙度值分类；构建激光雷达拼接后点云Fi时提取的所有特征。
其中，将新获得的拼接后的点云Fi+1采用迭代最近点(ICP)与IMU预积分来匹配，但是随着时间的增长，激光雷达惯性里程计会存在漂移的情况，此时就需要GPS信号作为绝对参考值来抑制漂移。
其中，在进行定位之前对点云地图利用VoxelGrid降采样方法进行滤波；以减少点云数据中的点的数量，同时能够保持点云的形状特征；其原理是通过输入的点云数据创建一系列三维的体素栅格，类似于一个个微小的空间三维立方体的集合，然后在每一个体素中内，用这个体素栅格内所有点的重心来近似的显示体素中的其他点；具体过程如下：
(1)依据点云数据坐标集合，求取X、Y、Z三个坐标轴上的最大值xmax、ymax、zmax和最小值xmin、ymin、zmin；
(2)根据公式lx＝xmax-xmin、ly＝ymax-ymin、lz＝zmax-zmin求得最小包围盒的边长lx、ly、lz；
(3)设置体素小栅格边长cell，将X、Y、Z三个坐标轴均等划分为M、N、L份，则最小包围盒划分成M×N×L个体素小栅格；
(4)对每个体素小栅格编号，编号为(i，j，k)，确定每个数据点所属的体素小栅格；
(5)进行点云精简，计算每个体素小栅格重心，以重心代替小栅格内所有的点；如果重心点不存在，则用小栅格内距离所求重心最近的数据点代替该小栅格内所有的点，至此完成整个精简过程。
其中，采用NDT配准算法对大规模园区内所有机器人进行实时定位的步骤包括：
步骤1：将参考点云所占的空间划分成指定大小的立方体，并计算每个网格的多维正态分布参数：
步骤2：计算网格的概率分布模型：
步骤3：变换要配准的点云到参考坐标系下(参考点云的坐标系)：
步骤4：根据正态分布参数计算每个转换点落在对应立方体中的概率：
步骤5：计算NDT配准得分，即对应点落在对应立方体中的概率之和：
步骤6：根据牛顿优化算法对目标函数score(p)进行优化，即寻找变换参数p使得目标函数的值最大：
令
则：
根据链式求导法则以及向量、矩阵求导的公式，s梯度方向为：
其中
对变换参数pi的偏导数
即为变换T的雅克比矩阵：
根据上面梯度的计算结果，继续求s关于变量pi与pj的二阶偏导：
根据变换方程，向量
对变换参数p的二阶导数的向量为：
步骤7：跳转到步骤3继续执行，直到达到收敛条件为止。
本发明与现有技术相比，本发明的基于多传感器触合的大规模园区建图定位方法，包括步骤：通过激光雷达对大规模园区进行环境感知和三维点云信息采集，通过IMU传感器的观测结果进行预积分来辅助Lidar的帧-局部地图匹配的定位；利用帧-局部地图匹配方法对大规模园区点云地图的构建；采用NDT配准算法对大规模园区内所有机器人进行实时定位。通过本发明，能够在没有环境信息的基础上，对周围环境进行地图构建并定位，提高了智能车地图构建和即时定位的能力，精度高，实时性好，应用前景广泛。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1是本发明提供的一种基于多传感器触合的大规模园区建图定位方法的流程示意图。
图2是本发明提供的一种基于多传感器触合的大规模园区建图定位方法中NDT算法的逻辑示意图。
图3是本发明提供的一种基于多传感器触合的大规模园区建图定位方法对应系统的结构示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明做进一步的详细说明。应当理解，此外所描述的具体实施例仅用以解释本发明，但并不用于限定本发明。基于本发明中的实施例，本领域普通人员在没有做出创造性劳动前提下所获得的所有其他实施例，都将属于本发明保护的范围。
图1所示，本发明提供了一种基于多传感器触合的大规模园区建图定位方法，包括：
通过激光雷达对大规模园区进行环境感知和三维点云信息采集，通过IMU传感器的观测结果进行预积分来辅助Lidar的帧-局部地图匹配的定位；
利用帧-局部地图匹配方法对大规模园区点云地图的构建；
采用NDT配准算法对大规模园区内所有机器人进行实时定位。
其中，在通过激光雷达对大规模园区进行环境感知和三维点云信息采集的步骤中，包括步骤：
通过激光雷达对大规模园区周围环境进行实时监测，包括障碍物距离信息和反射强度信息，对周围环境感知并采集三维点云信息；
利用距离信息和对应姿态角的三角函数值可以得到点云在激光雷达坐标系下的坐标值。
其中，在通过IMU传感器的观测结果进行预积分来辅助Lidar的帧-局部地图匹配的定位的步骤中，包括：
通过IMU传感器的三轴垂直测量的加速度计，根据牛顿第二定律的电容变化测量得到三轴；
通过IMU传感器的陀螺仪，测量运动中物体的惯性科氏力，从而间接测量三轴角速度。
其中，三维点云信息采集的步骤还包括：采用RTK差分定位技术，通过建立已知定位信息基站的卫导天线接受GPS信号，随后移动站通过4G差分模块数传天线来接收差分位置信号，根据差分信号矫正移动站GPS传感器接收的原始定位信息，从而得到准确的位置和方位信息。
本发明通过使用SLAM领域前沿的开源算法：LIO-SAM算法和NDT算法，实现大规模点云地图的构建和实时定位功能。
由于LOAM算法直接存储全局体素地图而不是局部地图，从而很难执行回环检测以修正漂移，或者组合GPS等测量进行位姿修正。并且体素地图的使用效率会随时间降低。为了克服该问题，使用帧-局部地图匹配代替LOAM的帧-全局地图匹配，提高了帧图匹配的效率，故本发明采用LIO-SAM算法实现大规模点云地图的构建。
本发明利用传感器的观测结果来估计机器人的状态及其轨迹。首先，来自惯性测量单元(IMU)预积分的估计运动消除点云畸变，并为其里程计优化产生了初始猜测。获得的里程计用于估计惯性测量单元的偏差。为了确保高实时性能，本发明将之前扫描边缘化以进行姿态优化。以局部尺度进行扫描匹配可以显著提高系统的实时性能，将地图点云进行拼接，对于特征点匹配算法，本发明采用迭代最近点(ICP)找到两个距离最近的特征点来匹配，在利用帧-局部地图匹配方法对大规模园区点云地图的构建的步骤中，包括：
步骤1：定义坐标系与符号，将世界坐标系表示为W，机器人主体坐标系表示为B，假定IMU坐标系与机器人主体坐标系重合，机器人的状态写成：
x＝[RT，pT，vT，bT]T
其中R∈SO(3)为旋转矩阵，p∈R3为位置向量，v为速度，b为IMU偏置；从B到W的变换T∈SE(3)表示为T＝[R|p]；
步骤2：IMU传感器的角速度和加速度测量值通过公式定义：
其中
和t是时间t时B中的原始IMU测量值；
和t受到缓慢变化的偏置
和白噪声
的影响；
是从W到B的旋转矩阵；g是以W为单位的恒定重力矢量；
步骤3：使用IMU传感器的测量结果来推断机器人的运动；机器人在时间t+Δt的速度、位置和旋转可计算如下所示:
步骤4：获得两个时间步长之间的相对机器人运动，时间i和j之间的预积分测量值vij、pij和Rij使用如下公式计算:
其中，具备通信功能的电子雾化装置与所述集成无线通信模块的共享电子雾化设备管理装置之间的通信方式兼容USART通信、脉冲通信或NFC近场通信通信方式。
其中，当激光雷达的新的点云扫描到达时，首先执行特征提取；通过评估局部区域上的点的粗糙度来提取边缘和平面特征；粗糙度值大的点被归类为边缘特征；类似地，平面特征由小粗糙度值分类；构建激光雷达拼接后点云Fi时提取的所有特征。
其中，将新获得的拼接后的点云Fi+1采用迭代最近点(ICP)与IMU预积分来匹配，但是随着时间的增长，激光雷达惯性里程计会存在漂移的情况，此时就需要GPS信号作为绝对参考值来抑制漂移。
在目前已有的自动驾驶系统的定位问题中，如果利用纯SLAM来实现在没有地图的情况下的点位功能，还存在定位精度、可靠性无法达到自动驾驶系统需求的情况。因此，本发明提出一种在已拥有高精度地图的前提下，基于NDT配准算法的定位方法。该算法可以实现在没有环境信息的基础上，对周围环境进行地图构建之后的定位功能，其计算用时短的特点也能很好地满足自动驾驶系统对于实时性地要求。
其中，在进行定位之前对点云地图利用VoxelGrid降采样方法进行滤波；以减少点云数据中的点的数量，同时能够保持点云的形状特征；其原理是通过输入的点云数据创建一系列三维的体素栅格，类似于一个个微小的空间三维立方体的集合，然后在每一个体素中内，用这个体素栅格内所有点的重心来近似的显示体素中的其他点；具体过程如下：
(1)依据点云数据坐标集合，求取X、Y、Z三个坐标轴上的最大值xmax、ymax、zmax和最小值xmin、ymin、zmin；
(2)根据公式lx＝xmax-xmin、ly＝ymax-ymin、lz＝zmax-zmin求得最小包围盒的边长lx、ly、lz；
(3)设置体素小栅格边长cell，将X、Y、Z三个坐标轴均等划分为M、N、L份，则最小包围盒划分成M×N×L个体素小栅格；
(4)对每个体素小栅格编号，编号为(i，j，k)，确定每个数据点所属的体素小栅格；
(5)进行点云精简，计算每个体素小栅格重心，以重心代替小栅格内所有的点；如果重心点不存在，则用小栅格内距离所求重心最近的数据点代替该小栅格内所有的点，至此完成整个精简过程。
经过降采样的点云地图作为NDT算法中的参考点云地图。NDT算法的基本思想是先将参考点云(即高精度地图)转换为多维变量的正态分布，如果变换参数能使得两幅激光数据匹配的很好，那么变换点在参考系中的概率密度将会很大。因此，用优化的方法求出使得概率密度之和最大的变换参数，此时两幅激光点云数据将匹配的最好。
如图2所示，采用NDT配准算法对大规模园区内所有机器人进行实时定位的步骤包括：
步骤1：将参考点云所占的空间划分成指定大小的立方体，并计算每个网格的多维正态分布参数：
步骤2：计算网格的概率分布模型：
步骤3：变换要配准的点云到参考坐标系下(参考点云的坐标系)：
步骤4：根据正态分布参数计算每个转换点落在对应立方体中的概率：
步骤5：计算NDT配准得分，即对应点落在对应立方体中的概率之和：
步骤6：根据牛顿优化算法对目标函数score(p)进行优化，即寻找变换参数p使得目标函数的值最大：
令
则：
根据链式求导法则以及向量、矩阵求导的公式，s梯度方向为：
其中
对变换参数pi的偏导数
即为变换T的雅克比矩阵：
根据上面梯度的计算结果，继续求s关于变量pi与pj的二阶偏导：
根据变换方程，向量
对变换参数p的二阶导数的向量为：
步骤7：跳转到步骤3继续执行，直到达到收敛条件为止。
基于本发明的建图与定位方法，可构建对应的大规模园区场景建图与定位系统，该系统可以应用于园区无人驾驶、无人机实时定位以及室内测绘建图等场合。整体框架图如图3所示。
将差分GPS信号和激光雷达数据分别通过串口通讯和以太网口传输至TX2嵌入式计算平台，可直接被主机识别，唯有IMU传感器生成的CAN信号，需要经由CAN接口卡对CAN总线网络上协议进行解析后传给TX2，计算平台负责利用LIO-SAM算法和NDT算法完成大规模场景园区环境的地图构建和车辆在地图中的高精度定位任务。
TX2嵌入式平台作为本系统的核心零部件，是基于NVIDIA PascalTM架构的一台AI单模块超级计算机，具有自己的操作系统，无需烧录，可以直接融合处理各传感器采集到的数据，并最终计算得到所构建的点云地图信息以及定位信息。运用TX2高性能嵌入式计算平台的目的在于加强终端计算能力，推广人工智能终端化，而不依赖于网络环境。
激光雷达可以对周围环境进行实时监测，包括障碍物距离信息和反射强度信息。根据发射与接收模块旋转对周围环境感知并采集三维点云信息，然后利用距离信息和对应姿态角的三角函数值可以得到点云在激光雷达坐标系下的坐标值。以太网口仅需将TX2的IP地址设置成与激光雷达同一网段即可接受数据。
IMU传感器由三轴垂直测量的加速度计和陀螺仪组成。加速度计是根据牛顿第二定律的电容变化测量得到三轴；陀螺仪通过振动陀螺原理测量运动中物体的惯性科氏力，从而间接测量三轴角速度。本系统使用型号为SST810的动态倾角传感器，满足绝大多数恶劣环境的使用。IMU传感器生成的CAN信号，需要经由CAN接口卡对CAN总线网络上协议进行解析后传给TX2。
OEM615板卡采用全新的OEM6硬件平台，能够实现对GPS、GLONASS、Galileo多个系统的信号接收，提高了GPS信号接收器的可用性。采用RTK差分定位技术，通过建立已知定位信息基站的卫导天线接受GPS信号，随后移动站通过4G差分模块数传天线来接收差分位置信号，根据差分信号矫正移动站GPS传感器接收的原始定位信息，从而得到准确的位置和方位信息。GPS传感器的数据以串口的形式输出，可以直接被主机识别，从而进行数据解析和预处理工作。
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。
Claims (9)
1.一种基于多传感器触合的大规模园区建图定位方法，其特征在于，包括：
通过激光雷达对大规模园区进行环境感知和三维点云信息采集，通过IMU传感器的观测结果进行预积分来辅助Lidar的帧-局部地图匹配的定位；
利用帧-局部地图匹配方法对大规模园区点云地图的构建；
采用NDT配准算法对大规模园区内所有机器人进行实时定位。
2.根据权利要求1所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，在通过激光雷达对大规模园区进行环境感知和三维点云信息采集的步骤中，包括步骤：
通过激光雷达对大规模园区周围环境进行实时监测，包括障碍物距离信息和反射强度信息，对周围环境感知并采集三维点云信息；
利用距离信息和对应姿态角的三角函数值可以得到点云在激光雷达坐标系下的坐标值。
3.根据权利要求1所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，在通过IMU传感器的观测结果估计进行预积分来辅助Lidar的帧-局部地图匹配的定位的步骤中，包括：
通过IMU传感器的三轴垂直测量的加速度计，根据牛顿第二定律的电容变化测量得到三轴；
通过IMU传感器的陀螺仪，测量运动中物体的惯性科氏力，从而间接测量三轴角速度。
4.根据权利要求1所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，三维点云信息采集的步骤还包括：采用RTK差分定位技术，通过建立已知定位信息基站的卫导天线接受GPS信号，随后移动站通过4G差分模块数传天线来接收差分位置信号，根据差分信号矫正移动站GPS传感器接收的原始定位信息，从而得到准确的位置和方位信息。
5.根据权利要求1所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，在利用帧-局部地图匹配方法对大规模园区点云地图的构建的步骤中，包括：
步骤1：定义坐标系与符号，将世界坐标系表示为W，机器人主体坐标系表示为B，假定IMU坐标系与机器人主体坐标系重合，机器人的状态写成：
x＝[RT，pT，vT，bT]T
其中R∈SO(3)为旋转矩阵，p∈R3为位置向量，v为速度，b为IMU偏置；从B到W的变换T∈SE(3)表示为T＝[R|p]；
步骤2：IMU传感器的角速度和加速度测量值通过公式定义：
其中
和t是时间t时B中的原始IMU测量值；
和t受到缓慢变化的偏置
和白噪声
的影响；
是从W到B的旋转矩阵；g是以W为单位的恒定重力矢量；
步骤3：使用IMU传感器的测量结果来推断机器人的运动；机器人在时间t+Δt的速度、位置和旋转可计算如下所示:
步骤4：获得两个时间步长之间的相对机器人运动，时间i和j之间的预积分测量值vij、pij和Rij使用如下公式计算:
6.根据权利要求5所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，当激光雷达的新的点云扫描到达时，首先执行特征提取；通过评估局部区域上的点的粗糙度来提取边缘和平面特征；粗糙度值大的点被归类为边缘特征；类似地，平面特征由小粗糙度值分类；构建激光雷达拼接后点云Fi时提取的所有特征。
7.根据权利要求4所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，将新获得的拼接后的点云Fi+1采用迭代最近点(ICP)与IMU预积分来匹配，但是随着时间的增长，激光雷达惯性里程计会存在漂移的情况，此时就需要GPS信号作为绝对参考值来抑制漂移。
8.根据权利要求1所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，在进行定位之前对点云地图利用VoxelGrid降采样方法进行滤波；以减少点云数据中的点的数量，同时能够保持点云的形状特征；其原理是通过输入的点云数据创建一系列三维的体素栅格，类似于一个个微小的空间三维立方体的集合，然后在每一个体素中内，用这个体素栅格内所有点的重心来近似的显示体素中的其他点；具体过程如下：
(1)依据点云数据坐标集合，求取X、Y、Z三个坐标轴上的最大值xmax、ymax、zmax和最小值xmin、ymin、zmin；
(2)根据公式lx＝xmax-xmin、ly＝ymax-ymin、lz＝zmax-zmin求得最小包围盒的边长lx、ly、lz；
(3)设置体素小栅格边长cell，将X、Y、Z三个坐标轴均等划分为M、N、L份，则最小包围盒划分成M×N×L个体素小栅格；
(4)对每个体素小栅格编号，编号为(i，j，k)，确定每个数据点所属的体素小栅格；
(5)进行点云精简，计算每个体素小栅格重心，以重心代替小栅格内所有的点；如果重心点不存在，则用小栅格内距离所求重心最近的数据点代替该小栅格内所有的点，至此完成整个精简过程。
9.根据权利要求8所述的基于多传感器触合的大规模园区建图定位方法，其特征在于，采用NDT配准算法对大规模园区内所有机器人进行实时定位的步骤包括：
步骤1：将参考点云所占的空间划分成指定大小的立方体，并计算每个网格的多维正态分布参数：
步骤2：计算网格的概率分布模型：
步骤3：变换要配准的点云到参考坐标系下(参考点云的坐标系)：
步骤4：根据正态分布参数计算每个转换点落在对应立方体中的概率：
步骤5：计算NDT配准得分，即对应点落在对应立方体中的概率之和：
步骤6：根据牛顿优化算法对目标函数score(p)进行优化，即寻找变换参数p使得目标函数的值最大：
令
则：
根据链式求导法则以及向量、矩阵求导的公式，s梯度方向为：
其中
对变换参数pi的偏导数
即为变换T的雅克比矩阵：
根据上面梯度的计算结果，继续求s关于变量pi与pj的二阶偏导：
根据变换方程，向量
对变换参数p的二阶导数的向量为：
步骤7：跳转到步骤3继续执行，直到达到收敛条件为止。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110246946.6A CN112967392A (zh) | 2021-03-05 | 2021-03-05 | 一种基于多传感器触合的大规模园区建图定位方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110246946.6A CN112967392A (zh) | 2021-03-05 | 2021-03-05 | 一种基于多传感器触合的大规模园区建图定位方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN112967392A true CN112967392A (zh) | 2021-06-15 |
Family
ID=76276795
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110246946.6A Pending CN112967392A (zh) | 2021-03-05 | 2021-03-05 | 一种基于多传感器触合的大规模园区建图定位方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112967392A (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113520812A (zh) * | 2021-08-26 | 2021-10-22 | 山东大学 | 一种四足机器人导盲系统及方法 |
| CN113959437A (zh) * | 2021-10-14 | 2022-01-21 | 重庆数字城市科技有限公司 | 一种用于移动测量设备的测量方法及系统 |
| CN114088104A (zh) * | 2021-07-23 | 2022-02-25 | 武汉理工大学 | 一种自动驾驶场景下的地图生成方法 |
| CN114526745A (zh) * | 2022-02-18 | 2022-05-24 | 太原市威格传世汽车科技有限责任公司 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
| WO2023226155A1 (zh) * | 2022-05-24 | 2023-11-30 | 芯跳科技(广州)有限公司 | 多源数据融合定位方法、装置、设备及计算机存储介质 |
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN114526745B (zh) * | 2022-02-18 | 2024-04-12 | 太原市威格传世汽车科技有限责任公司 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112014857A (zh) * | 2020-08-31 | 2020-12-01 | 上海宇航系统工程研究所 | 用于智能巡检的三维激光雷达定位导航方法及巡检机器人 |
-
2021
- 2021-03-05 CN CN202110246946.6A patent/CN112967392A/zh active Pending
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112014857A (zh) * | 2020-08-31 | 2020-12-01 | 上海宇航系统工程研究所 | 用于智能巡检的三维激光雷达定位导航方法及巡检机器人 |
Non-Patent Citations (4)
| Title |
|---|
| BIBER P 等: "The normal distributions transform: A new approach to laser scan matching", 《PROCEEDINGS 2003 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS 2003)》 * |
| SHAN T 等: "Lio-sam: Tightly-coupled lidar inertial odometry via smoothing and mapping", 《2020 IEEE/RSJ INTERNATIONAL CONFERENCE ON INTELLIGENT ROBOTS AND SYSTEMS (IROS)》 * |
| 张少将: "基于多传感器信息融合的智能车定位导航系统研究", 《中国优秀硕士学位论文全文数据库工程科技Ⅱ辑》 * |
| 李运川 等: "一种用于点云配准的改进迭代最近点算法", 《软件导刊》 * |
Cited By (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114088104A (zh) * | 2021-07-23 | 2022-02-25 | 武汉理工大学 | 一种自动驾驶场景下的地图生成方法 |
| CN114088104B (zh) * | 2021-07-23 | 2023-09-29 | 武汉理工大学 | 一种自动驾驶场景下的地图生成方法 |
| CN113520812A (zh) * | 2021-08-26 | 2021-10-22 | 山东大学 | 一种四足机器人导盲系统及方法 |
| CN113959437A (zh) * | 2021-10-14 | 2022-01-21 | 重庆数字城市科技有限公司 | 一种用于移动测量设备的测量方法及系统 |
| CN114526745A (zh) * | 2022-02-18 | 2022-05-24 | 太原市威格传世汽车科技有限责任公司 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
| CN114526745B (zh) * | 2022-02-18 | 2024-04-12 | 太原市威格传世汽车科技有限责任公司 | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 |
| WO2023226155A1 (zh) * | 2022-05-24 | 2023-11-30 | 芯跳科技(广州)有限公司 | 多源数据融合定位方法、装置、设备及计算机存储介质 |
| CN117367412A (zh) * | 2023-12-07 | 2024-01-09 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
| CN117367412B (zh) * | 2023-12-07 | 2024-03-29 | 南开大学 | 一种融合捆集调整的紧耦合激光惯导里程计与建图方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| You et al. | Data fusion of UWB and IMU based on unscented Kalman filter for indoor localization of quadrotor UAV | |
| CN110178048B (zh) | 交通工具环境地图生成和更新的方法和系统 | |
| CN109709801B (zh) | 一种基于激光雷达的室内无人机定位系统及方法 | |
| CN112967392A (zh) | 一种基于多传感器触合的大规模园区建图定位方法 | |
| CN109991636A (zh) | 基于gps、imu以及双目视觉的地图构建方法及系统 | |
| CN113781582B (zh) | 基于激光雷达和惯导联合标定的同步定位与地图创建方法 | |
| CN106017463A (zh) | 一种基于定位传感装置的飞行器定位方法 | |
| CN111426320B (zh) | 一种基于图像匹配/惯导/里程计的车辆自主导航方法 | |
| CN112987065B (zh) | 一种融合多传感器的手持式slam装置及其控制方法 | |
| CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| Zheng et al. | An optimization-based UWB-IMU fusion framework for UGV | |
| CN113763549A (zh) | 融合激光雷达和imu的同时定位建图方法、装置和存储介质 | |
| CN115046540A (zh) | 一种点云地图构建方法、系统、设备和存储介质 | |
| CN115950418A (zh) | 多传感器融合定位方法 | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
| Srinara et al. | Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme | |
| WO2023283987A1 (zh) | 无人系统的传感器安全性检测方法、设备及存储介质 | |
| CN116429112A (zh) | 多机器人协同定位方法和装置、设备及存储介质 | |
| CN115562076A (zh) | 用于无人驾驶矿车的仿真系统、方法以及存储介质 | |
| CN113960614A (zh) | 一种基于帧-地图匹配的高程图构建方法 | |
| CN113093759A (zh) | 基于多传感器信息融合的机器人编队构造方法及系统 | |
| Zhou et al. | Localization for unmanned vehicle | |
| Wang | A driverless vehicle vision path planning algorithm for sensor fusion |
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 | ||
| RJ01 | Rejection of invention patent application after publication |
Application publication date: 20210615 |
|
| RJ01 | Rejection of invention patent application after publication |