基于传感器融合的旋转激光实时定位建模系统及方法与流程

1.本发明涉及机器人建模技术领域，尤其涉及一种基于传感器融合的旋转激光实时定位建模系统及方法。


背景技术：

2.对于移动机器人的自主导航，机器人首先需要对所处环境进行感知，其中重要的是机器人可以知道其在环境中的位置，并且感知周围环境的状态，机器人的即时定位和建图，是这一能力的实现技术。机器人的即时定位和建图，也即slam(simultaneous localization and mapping)，是指机器人通过其自身携带的传感器获取周围环境的状态信息，将获得的传感器状态信息进行计算处理，可以估计出机器人在环境中的位置姿态，同时获得机器人所处周围环境的情况。
3.对于机器人即时定位和建图技术的研究，根据采用传感器的不同，可以分为基于视觉传感器和基于激光传感器的方法。基于视觉传感器的slam方法，具有的优点是视觉摄像头体积小、成本低，在结构特征少的场景也可以有良好的效果，然而视觉slam具有受光照变化影响大，在弱纹理情况下容易失败的缺点。基于激光雷达的slam方法，可以应对光照变化的影响，在弱纹理条件下可靠工作的特点，然而其具有传感器体积大，在结构特征少的情况下容易失败的缺点。
4.基于激光雷达的即时定位和建图方法的研究，已有一些文献和专利进行了研究说明。中国专利“cn107462897a基于激光雷达的三维建图的方法”提出一种基于二维激光雷达的三维建图方法，其仅使用二维激光雷达和电机的组合，就可以完成实时三维点云地图的建立，可用于移动机器人对地形数据的测绘。该方法在环境特征少的情况下，单纯使用二维激光点云进行匹配，由于激光本身数量比较少，会出现相应特征点无法正确匹配的情况，从而使得建图失败。中国专利“cn106652028a一种环境三维建图方法及装置”提出了一种环境三维建图方法及装置，使用二维测距传感器和三维测距传感器构建三维环境地图，利用二维测距传感器得到可移动装置在其所处环境的位姿，利用三维测距传感器采集所处环境的三维环境信息，从而避免了进行三维特征提取和特征匹配，减少了计算量。由于该装置和方法也只使用二维激光器来进行位姿估计，其同样也会在特征比较少的场景下出现匹配失败的情况，因此也会受限于本身二维激光测距器的缺陷。
5.因此，目前的研究仍然存在一些问题，由于激光传感器数据频率低，使得在快速运动和大幅度旋转情况下激光雷达接收的点云数据具有较大的运动畸变，使得运动估计出错，同时对于狭长场景中，由于激光雷达传感器的视野有限，单纯的使用激光雷达进行定位和建图，容易因特征缺少出现匹配失败的情况的，从而导致整个定位和建图模块失败。


技术实现要素：

6.针对上述现有技术的不足，本发明提供一种基于传感器融合的旋转激光实时定位建模系统及方法。
7.为解决上述技术问题，本发明所采取的技术方案是：一种基于传感器融合的旋转激光实时定位建模系统，系统包括三维激光雷达、imu惯性测量单元、激光雷达驱动器、云台电机、感知处理单元、电源模块、通信模块和底层控制器；
8.所述三维激光雷达与imu惯性测量单元固定连接后利用连接件与所述云台电机进行连接，使得云台电机带动固定连接好的三维激光雷达与imu惯性测量单元一起旋转；
9.所述激光雷达驱动器负责将激光雷达原始数据处理，然后通过rj45接口发送给感知处理单元；
10.所述感知处理单元通过其内部运行的定位与建图系统算法程序，对三维激光雷达与imu惯性测量单元的数据进行处理，并通过通信模块采用无线通讯的方式实现与远程交互端的信息交互；
11.所述底层控制器包括微控制单片机和电平转换模块，微控制单片机的控制信号经电平转换模块转换后发送到云台电机，实现对云台电机的工作模式以及具体参数的控制；
12.所述电源模块通过外接直流源来实现，并通过降压处理模块实现对三维激光雷达、云台电机、通信模块的供电。
13.进一步的，所述感知处理单元、激光雷达驱动器与固定连接好的三维激光雷达与imu惯性测量单元之间通过滑环连接，避免转动带来的线路缠绕；所述固定连接好的三维激光雷达与imu惯性测量单元连接滑环转子，所述感知处理单元与激光雷达驱动器连接滑环定子。
14.所述微控制单片机通过电平转换模块将ttl电平的控制信号转换为485电平传输给云台电机。
15.所述imu惯性测量单元通过usb连接的方式与所述感知处理单元进行数据传输，用于反馈imu惯性测量单元的数据。
16.另一方面，本发明还提供一种采用上述系统进行建模的方法，包括如下步骤：
17.步骤1：对相对位置固定的三维激光雷达与imu惯性测量单元进行空间位置标定，得到三维激光雷达与imu惯性测量单元各自坐标系之间的相对位置变换，即外参变换；
18.步骤2：利用imu惯性测量单元的输出数据对三维激光雷达的点云数据去畸变处理，去除三维激光雷达点云由于装置运动产生的运动畸变，过程如下：
19.步骤2.1：将三维激光雷达数据中第一个激光点对应时刻的位姿作为当前激光帧的位姿
20.步骤2.2：当前帧激光中任意一激光点所对应时刻为t
c
，其中时刻t
c
是全局下的绝对时间，用于和imu惯性测量单元输出数据的时间标准一致；由于激光雷达和imu的数据频率不同，所以无法直接找到当前时刻激光点的imu数据，因此对imu惯性测量单元输出数据进行插值处理：
21.查找时刻t
c
前后两帧imu惯性测量单元输出数据，相应的imu位姿分别为计算时刻t
c
和前后两帧imu数据时间的时间比值，前后两帧imu数据时间为t
i
、t
i 1
，时刻t
c
相对于前后两帧imu数据时间的时间比值分别为和相应当前时刻对应的imu位姿为：
[0022][0023]
步骤2.3：由于三维激光雷达和imu惯性测量单元进行固定连接，因此三维激光雷达和imu惯性测量单元具有相同的运动变化，通过当前时刻激光点的imu位姿乘以步骤1得到的外参变换，从而得到当前时刻的激光点位姿；
[0024][0025]
其中，为三维激光雷达和imu惯性测量单元的外参变换；
[0026]
步骤2.4：计算当前激光点转换到当前激光帧坐标系下的变换：
[0027][0028]
从而消除装置运动产生的运动畸变。
[0029]
步骤3：对去运动畸变处理后的激光点云数据进行特征提取和特征匹配，实现对装置运动姿态的实时估计，过程如下：
[0030]
步骤3.1：获取去运动畸变处理后的激光点云数据所在激光线的线束标号，选取当前激光点周围同一标号上一定数量的激光点，来计算当前激光点附近的曲率：
[0031][0032]
其中，表示三维激光雷达坐标系下第k帧激光中，当前第i个激光点的三维坐标，相应是第i个激光点周围第j个激光点的三维坐标，m为周围点集；
[0033]
步骤3.2：将计算的曲率作为激光点云数据的特征，将当前帧激光提取得到的特征，和上一帧激光做特征匹配，计算当前帧激光相对于上一帧的运动状态。
[0034]
进一步的，所述将计算的曲率作为激光点云数据的特征具体分为直线特征和平面特征两类；将曲率值大的归为直线特征点，曲率值小的归为平面特征点；
[0035]
所述特征匹配的过程为：将当前帧提取得到的直线特征点和上一帧的直线特征点进行匹配，当前帧的平面特征点和上一帧的平面特征点进行匹配。
[0036]
所述计算当前帧激光相对于上一帧的运动状态的过程为：
[0037]
s1：基于imu的测量数据，计算得到两帧间的相对变换，以imu计算的相对变换作为初始值，将当前帧特征点转换到上一帧坐标系下；
[0038]
s2：当前帧中的直线特征点，上一帧直线特征点中寻找距离其最近的两个特征点，计算当前帧激光点到相应两个最近特征点构成直线的距离d
l
；当前帧平面特征点，在上一帧平面特征点中寻找距离其最近的三个特征点，计算当前帧平面特征点到相应最近三个特征点构成平面的距离d
p
；
[0039]
s3：将上述计算的距离问题，转换为当前帧激光位姿使匹配距离值最小的一个非线性最小二乘问题，因此通过将匹配的距离值进行趋于零的迭代优化，求解得到两帧间的运动变化，从而得出里程计的运动估计。
[0040]
步骤4：基于装置运动姿态的估计，将激光点云数据进行拼接，从而得到基于三维激光雷达采样数据的周围环境模型，得到物理环境对应的三维点云地图，实现对环境的实时三维重建，过程如下：
[0041]
步骤4.1：将三维激光雷达点云基于里程计数据进行拼接，获得所在环境的局部特征点云地图，从而实时进行地图构建；
[0042]
步骤4.2：为了消除一定的里程计累积误差，进行激光帧与局部地图的地图匹配，优化里程计的位姿估计值；将局部特征点云地图进行体素降采样处理，同时限制局部特征点云地图的范围，只选取特定长度空间范围的局部特征点云；
[0043]
步骤4.3：基于地图的匹配计算，将当前帧激光数据的特征点，和步骤4.2处理之后的局部特征点云地图进行匹配，减少由于里程计的运动估计带来的累积漂移；
[0044]
步骤4.4：考虑到基于特征地图匹配的计算复杂性，为了能够达到实时的运动估计，降低当前帧激光数据的特征点与地图的匹配执行频率，从而实现实时、高精度的三维地图重建。
[0045]
采用上述技术方案所产生的有益效果在于：
[0046]
1、本发明提供的基于传感器融合的旋转激光实时定位建模系统使用云台电机将三维激光雷达和imu惯性测量单元的组合装置进行旋转，可以实现平稳可靠的旋转；
[0047]
2、本发明利用微型处理器对采集数据进行处理和与底层控制器的交互，实现了装置的小巧、轻便，满足多种场景的应用需求，同时也具有基于无线的远程通信，提高了系统的交互能力和使用效率；
[0048]
3、本发明通过融合三维激光雷达和imu数据，实现激光雷达点云的有效去畸变处理，从而实现装置运动状态的准确估计；
[0049]
4、本发明采用将三维激光进行旋转的方式，扩大激光雷达的有效视野，提高视野特征提取和匹配的可靠性，同时增加地图点云密度，基于鲁棒、准确的位姿估计，从而可以实时构建周围环境稠密、高精度的点云地图；
[0050]
5、本发明通过将所有系统装置进行整合封装，使得整个结构成为一个整体，可装载于机器人系统，也可用于人员手持，方便了系统在多种场景下的应用。可以实现移动机器人的实时鲁棒运动估计，可以实现在狭长环境下的可靠定位和建图，同时能够构建高精度、稠密的点云地图，不仅可以用于机器人的自主导航，同时可用于室内和室外的移动三维测绘。
附图说明
[0051]
图1为本发明实施例中提供的基于传感器融合的旋转激光实时定位建模系统的结构示意爆炸图；
[0052]
其中，1
‑
三维激光雷达，2
‑
imu惯性测量单元，3
‑
微控制单片机，4
‑
电源模块，5
‑
网关模块，6
‑
感知处理单元，7
‑
激光雷达驱动器，8
‑
封装外壳，9
‑
云台电机固定结构，10
‑
云台电机，11
‑
通信模块。
[0053]
图2为本发明实施例中三维激光雷达和云台电机采用连接件连接的示意图；
[0054]
图3为本发明实施例中基于传感器融合的旋转激光实时定位建模系统的整体封装结构图；
[0055]
图4为本发明实施例中采用基于传感器融合的旋转激光实时定位建模系统进行建模的方法流程图；
[0056]
图5为本发明实施例中需要室内建模的真实环境图；
[0057]
图6为本发明实施例中采用本发明的系统和方法进行建模的效果图。
具体实施方式
[0058]
下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。
[0059]
如图1所示，本实施例中基于传感器融合的旋转激光实时定位建模系统如下所述：
[0060]
系统包括三维激光雷达1、imu惯性测量单元2、激光雷达驱动器7、云台电机10、感知处理单元6、电源模块4、通信模块11和底层控制器；
[0061]
本实施例中，采用16线三维激光雷达，型号为velodyne vlp
‑
16。
[0062]
所述三维激光雷达1与imu惯性测量单元2固定连接后利用连接件与所述云台电机10进行连接，使得云台电机10带动固定连接好的三维激光雷达1与imu惯性测量单元2一起旋转，如图2所示；
[0063]
所述激光雷达驱动器7负责将激光雷达原始数据处理，然后通过rj45接口发送给感知处理单元；
[0064]
所述感知处理单元6通过其内部运行的定位与建图系统算法程序，对三维激光雷达1与imu惯性测量单元2的数据进行处理，并通过通信模块11采用无线通讯的方式实现与远程交互端的信息交互；
[0065]
所述底层控制器包括微控制单片机3和电平转换模块，微控制单片机3的控制信号经电平转换模块转换后发送到云台电机10，实现对云台电机的工作模式以及具体参数的控制；
[0066]
所述电源模块4通过外接直流源来实现，并通过降压处理模块实现对三维激光雷达1、云台电机10、通信模块11的供电。
[0067]
本实施例中，如图3所示，基于传感器融合的旋转激光实时定位建模系统中的感知处理单元6、微控制单片机3、无线ap模块(即通讯模块11)、电源模块4、网关模块5和激光雷达驱动器7整体封装于封装外壳8内，并将云台电机使用螺丝与云台电机固定结构9进行连接固定，然后再将云台电机固定结构9通过螺丝跟设备封装外壳8进行固定。
[0068]
进一步的，所述感知处理单元6、激光雷达驱动器7与固定连接好的三维激光雷达1与imu惯性测量单元2之间通过滑环连接，避免转动带来的线路缠绕；所述固定连接好的三维激光雷达1与imu惯性测量单元2连接滑环转子，所述感知处理单元6与激光雷达驱动器7连接滑环定子。
[0069]
所述微控制单片机3通过电平转换模块将ttl电平的控制信号转换为485电平传输给云台电机10。
[0070]
所述imu惯性测量单元2通过usb连接的方式与所述感知处理单元6进行数据传输，用于反馈imu惯性测量单元2的数据。
[0071]
另一方面，本发明还提供一种采用上述系统进行建模的方法，如图4所示，包括如下步骤：
[0072]
步骤1：对相对位置固定的三维激光雷达1与imu惯性测量单元2进行空间位置标定，得到三维激光雷达1与imu惯性测量单元2各自坐标系之间的相对位置变换，即外参变换；
[0073]
步骤2：利用imu惯性测量单元2的输出数据对三维激光雷达1的点云数据去畸变处理，去除三维激光雷达点云由于装置运动产生的运动畸变，过程如下：
[0074]
步骤2.1：将三维激光雷达数据中第一个激光点对应时刻的位姿作为当前激光帧的位姿
[0075]
步骤2.2：当前帧激光中任意一激光点所对应时刻为t
c
，其中时刻t
c
是全局下的绝对时间，用于和imu惯性测量单元输出数据的时间标准一致；由于激光雷达和imu的数据频率不同，所以无法直接找到当前时刻激光点的imu数据，因此对imu惯性测量单元输出数据进行插值处理：
[0076]
查找时刻t
c
前后两帧imu惯性测量单元输出数据，相应的imu位姿分别为计算时刻t
c
和前后两帧imu数据时间的时间比值，前后两帧imu数据时间为t
i
、t
i 1
，时刻t
c
相对于前后两帧imu数据时间的时间比值分别为和相应当前时刻对应的imu位姿为：
[0077][0078]
步骤2.3：由于三维激光雷达和imu惯性测量单元进行固定连接，因此三维激光雷达和imu惯性测量单元具有相同的运动变化，通过当前时刻激光点的imu位姿乘以步骤1得到的外参变换，从而得到当前时刻的激光点位姿；
[0079][0080]
其中，为三维激光雷达和imu惯性测量单元的外参变换；
[0081]
步骤2.4：计算当前激光点转换到当前激光帧坐标系下的变换：
[0082][0083]
从而消除装置运动产生的运动畸变。
[0084]
步骤3：对去运动畸变处理后的激光点云数据进行特征提取和特征匹配，实现对装置运动姿态的实时估计，过程如下：
[0085]
步骤3.1：获取去运动畸变处理后的激光点云数据所在激光线的线束标号，选取当前激光点周围同一标号上一定数量的激光点，来计算当前激光点附近的曲率：
[0086][0087]
其中，表示三维激光雷达坐标系下第k帧激光中，当前第i个激光点的三维坐标，相应是第i个激光点周围第j个激光点的三维坐标，m为周围点集；
[0088]
步骤3.2：将计算的曲率作为激光点云数据的特征，将当前帧激光提取得到的特征，和上一帧激光做特征匹配，计算当前帧激光相对于上一帧的运动状态。
[0089]
进一步的，所述将计算的曲率作为激光点云数据的特征具体分为直线特征和平面特征两类；将曲率值大的归为直线特征点，曲率值小的归为平面特征点；
[0090]
所述特征匹配的过程为：将当前帧提取得到的直线特征点和上一帧的直线特征点
进行匹配，当前帧的平面特征点和上一帧的平面特征点进行匹配。
[0091]
所述计算当前帧激光相对于上一帧的运动状态的过程为：
[0092]
s1：基于imu的测量数据，计算得到两帧间的相对变换，以imu计算的相对变换作为初始值，将当前帧特征点转换到上一帧坐标系下；
[0093]
s2：当前帧中的直线特征点，上一帧直线特征点中寻找距离其最近的两个特征点，计算当前帧激光点到相应两个最近特征点构成直线的距离d
l
；当前帧平面特征点，在上一帧平面特征点中寻找距离其最近的三个特征点，计算当前帧平面特征点到相应最近三个特征点构成平面的距离d
p
；
[0094]
s3：将上述计算的距离问题，转换为当前帧激光位姿使匹配距离值最小的一个非线性最小二乘问题，因此通过将匹配的距离值进行趋于零的迭代优化，求解得到两帧间的运动变化，从而得出里程计的运动估计。
[0095]
步骤4：基于装置运动姿态的估计，将激光点云数据进行拼接，从而得到基于三维激光雷达采样数据的周围环境模型，得到物理环境对应的三维点云地图，实现对环境的实时三维重建，过程如下：
[0096]
步骤4.1：将三维激光雷达点云基于里程计数据进行拼接，获得所在环境的局部特征点云地图，从而实时进行地图构建；
[0097]
步骤4.2：为了消除一定的里程计累积误差，进行激光帧与局部地图的地图匹配，优化里程计的位姿估计值；将局部特征点云地图进行体素降采样处理，同时限制局部特征点云地图的范围，只选取特定长度空间范围的局部特征点云；
[0098]
步骤4.3：基于地图的匹配计算，将当前帧激光数据的特征点，和步骤4.2处理之后的局部特征点云地图进行匹配，减少由于里程计的运动估计带来的累积漂移；
[0099]
步骤4.4：考虑到基于特征地图匹配的计算复杂性，为了能够达到实时的运动估计，降低当前帧激光数据的特征点与地图的匹配执行频率，从而实现实时、高精度的三维地图重建。
[0100]
本实施例中，由实验人员手持基于传感器融合的旋转激光实时定位建模系统在室内走一圈，室内情况如图5所示。通过本实施例的方法进行建模，得到的建模效果如图6所示。