一种基于多传感器触合的大规模园区建图定位方法

本发明属于地图构建与定位技术领域，更具体地说，特别涉及一种基于多传感器触合的大规模园区建图定位方法。

背景技术：

工业园区是自动驾驶技术的重要场景，目前园区自动驾驶技术已经成功应用于园区摆渡、仓储物流、环卫、医院物资运输等场景。与摇杆操作控制的方式不同，智能车是通过传感器对自身及周围环境感知定位，结合检测的障碍物信息和地图信息完成自主导航功能。通常采用全球定位系统(gps)完成定位，然而在现实中，外部干扰是不可避免的，在室内、严重遮挡或电子干扰的环境下会因为gps信号丢失导致定位失败。因此，如何给智能车提供精确、可靠的位置信息，已经成为智能车定位优先考虑的问题。

技术实现要素：

本发明为了解决上述提到的问题，提供了一种基于多传感器触合的大规模园区建图定位方法。

本发明解决其技术问题所采用的技术方案是：构造一种基于多传感器触合的大规模园区建图定位方法，包括：

通过激光雷达对大规模园区进行环境感知和三维点云信息采集，通过imu传感器的观测结果进行预积分来辅助lidar的帧-局部地图匹配的定位；

利用帧-局部地图匹配方法对大规模园区点云地图的构建；

采用ndt配准算法对大规模园区内所有机器人进行实时定位。

其中，在通过激光雷达对大规模园区进行环境感知和三维点云信息采集的步骤中，包括步骤：

通过激光雷达对大规模园区周围环境进行实时监测，包括障碍物距离信息和反射强度信息，对周围环境感知并采集三维点云信息；

利用距离信息和对应姿态角的三角函数值可以得到点云在激光雷达坐标系下的坐标值。

其中，在通过imu传感器的观测结果进行预积分来辅助lidar的帧-局部地图匹配的定位的步骤中，包括：

通过imu传感器的三轴垂直测量的加速度计，根据牛顿第二定律的电容变化测量得到三轴；

通过imu传感器的陀螺仪，测量运动中物体的惯性科氏力，从而间接测量三轴角速度。

其中，三维点云信息采集的步骤还包括：采用rtk差分定位技术，通过建立已知定位信息基站的卫导天线接受gps信号，随后移动站通过4g差分模块数传天线来接收差分位置信号，根据差分信号矫正移动站gps传感器接收的原始定位信息，从而得到准确的位置和方位信息。

其中，在利用帧-局部地图匹配方法对大规模园区点云地图的构建的步骤中，包括：

步骤1：定义坐标系与符号，将世界坐标系表示为w，机器人主体坐标系表示为b，假定imu坐标系与机器人主体坐标系重合，机器人的状态写成：

x＝[r^t，p^t，v^t，b^t]^t

其中r∈so(3)为旋转矩阵，p∈r³为位置向量，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＝[r^t，p^t，v^t，b^t]^t

其中r∈so(3)为旋转矩阵，p∈r³为位置向量，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嵌入式平台作为本系统的核心零部件，是基于nvidiapascaltm架构的一台ai单模块超级计算机，具有自己的操作系统，无需烧录，可以直接融合处理各传感器采集到的数据，并最终计算得到所构建的点云地图信息以及定位信息。运用tx2高性能嵌入式计算平台的目的在于加强终端计算能力，推广人工智能终端化，而不依赖于网络环境。

激光雷达可以对周围环境进行实时监测，包括障碍物距离信息和反射强度信息。根据发射与接收模块旋转对周围环境感知并采集三维点云信息，然后利用距离信息和对应姿态角的三角函数值可以得到点云在激光雷达坐标系下的坐标值。以太网口仅需将tx2的ip地址设置成与激光雷达同一网段即可接受数据。

imu传感器由三轴垂直测量的加速度计和陀螺仪组成。加速度计是根据牛顿第二定律的电容变化测量得到三轴；陀螺仪通过振动陀螺原理测量运动中物体的惯性科氏力，从而间接测量三轴角速度。本系统使用型号为sst810的动态倾角传感器，满足绝大多数恶劣环境的使用。imu传感器生成的can信号，需要经由can接口卡对can总线网络上协议进行解析后传给tx2。

oem615板卡采用全新的oem6硬件平台，能够实现对gps、glonass、galileo多个系统的信号接收，提高了gps信号接收器的可用性。采用rtk差分定位技术，通过建立已知定位信息基站的卫导天线接受gps信号，随后移动站通过4g差分模块数传天线来接收差分位置信号，根据差分信号矫正移动站gps传感器接收的原始定位信息，从而得到准确的位置和方位信息。gps传感器的数据以串口的形式输出，可以直接被主机识别，从而进行数据解析和预处理工作。

上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。