CN111458722B - 一种激光雷达小车在具有坡度环境下的地图构建方法 - Google Patents

Abstract

本发明公开了一种激光雷达小车在复杂具有坡度环境下的地图构建方法，以解决复杂激光雷达SLAM构图与自主导航能力变差的问题。首先进行激光雷达数据的预处理，将超出激光雷达有效范围的数据进行剔除。然后根据加速度计和陀螺仪数据递推出激光雷达小车的预测导航信息，并传送到坡度算法中进行激光雷达数据修正后，进行卡尔曼融合滤波，进而得到激光雷达小车的导航信息。最后，将滤波后的激光雷达小车的导航信息更新环境地图。在复杂坡度环境下中，通过本发明公开的SLAM方法，能够获得更高精度的激光雷达导航信息，并绘制出精准的地图。

Description

一种激光雷达小车在具有坡度环境下的地图构建方法

技术领域

本发明属于自动化控制技术领域，具体涉及一种激光雷达小车在具有坡度环境下的地图构建方法。

背景技术

随着现代化时代的到来，以美国全球定位系统(GPS)、中国北斗系统(BDS)、欧盟伽利略系统(Galileo)、俄罗斯格洛纳斯系统(GLONASS)为代表的全球卫星导航系统(GNSS)迅速发展，依附全球卫星导航系统的室外定位技术也逐渐发展成熟，为人类生产、生活和社会发展做出了巨大贡献。然而，近几年随着经济发展，越来越多的生产活动是在室内进行，与室外环境相比，在室内环境中GNSS系统不能够很好的满足人们自由导航定位的需求。目前，能够实现自主运行的无人搬运小车已经在物流货物工厂等室内场所得到了非常广泛的应用，并承担了简单的搬运，数据采集、目标识别等任务。所有这些任务，都离不开激光雷达室内定位和构建地图(SimultaneousLocationandMapping,SLAM)。但是，目前激光SLAM算法大多仅仅使用平坦无坡度室内环境构图导航，一旦地面出现高低不平坦坡度，就很难修正环境地图，造成较大误差，从而直接导致激光雷达SLAM构图与自主导航能力变差。

发明内容

本发明的目的在于提供一种激光雷达小车在具有坡度环境下的地图构建方法，以解决复杂激光雷达SLAM构图与自主导航能力变差的问题；通过本发明提供的方法，能够获得更高精度的激光雷达导航信息，并绘制出精准的地图。

本发明的具体步骤如下：

步骤一、构建全局坐标系并初始化环境地图和小车的初始导航信息。在激光雷达小车上建立相对坐标系；初始状态下，相对坐标系与全局坐标系保持一致。导航信息包括相对坐标系的X轴方向、Y轴方向、X轴速度分量、Y轴速度分量和航向。初始导航信息中的所有数据均为0。之后，行车行进，循环执行步骤二至九，直至地图构建完成；第t次执行步骤二至九的时刻记为t时刻。每次执行步骤二至九，t均增大1。

步骤二、小车上的激光雷达向周围发射和接受激光粒子，得到激光雷达小车各个方向的激光距离数据组成的距离数集S_t＝{s_t,1,s_t,2,...,s_t,n}。

步骤三、陀螺仪检测当前时刻的X轴加速度、Y轴加速度Z轴角速度和激光雷达传感器采集的距离数集S_t给控制模块3。

步骤四、根据上一时刻的导航信息ζ_t-1和当前时刻的X轴加速度a_t ^bx、Y轴加速度a_t ^by和Z轴角速度W_t ^bz，惯性递推出激光雷达小车的当前时刻的导航信息ζ_t＝[P^nx _t,P^ny _t,V^nx _t,V^ny _t,θⁿ _t]，其中P^nx _t、P^ny _t、V^nx _t、V^ny _t、θⁿ _t分别为当前时刻激光雷达小车在X坐标轴方向、Y坐标轴方向、X坐标轴速度分量、Y坐标轴速度分量和航向。

步骤六、通过坡度算法对步骤四中所得的激光雷达检测到的距离数集S_t进行补偿修正，得到修正后距离数集S′_t＝{s′_t,1,s′_t,2,...,s′_t,n}；具体过程如下：

①.陀螺仪检测当前时刻的前后俯仰角α、和左右翻滚角β。据此对距离数集S_t进行修正，得到修正后距离数集S′_t＝{s′_t,1,s′_t,2,...,s′_t,n}。确定容差角δ的取值。

若|α|>δ且|β|≤δ，则

若|α|≤δ且|β|>δ，则

若同时满足α>δ、β<-δ或者同时满足α<-δ、β>δ，则

若同时满足α>δ、β>δ或者同时满足α<-δ、β<-δ，则

否则，s_t′_,i＝s_t,i，i＝1,2,...,n。

②.根据修正后距离数集S′_t建立粗略环境地图。

步骤七、根据修正后距离数集S′_t，对导航信息ζ_t中的X坐标轴方向P^nx _t、Y坐标轴方向P^ny _t和航向θⁿ _t进行修正和更新，得到新的导航信息ζ_t。

步骤八、将经步骤七修正后的导航信息ζ_t进行卡尔曼融合滤波。

步骤九、根据步骤八中滤波后的导航信息ζ_t和步骤六中建立的粗略环境地图更新全局环境地图。

作为优选，步骤三执行后，控制模块3对激光雷达数据进行预处理，并依据三角测距原理和激光雷达的有效范围参数，将距离数集S_t中超出激光雷达有效范围的数据进行剔除；被剔除的数据根据相邻两侧的数据补齐。

作为优选，步骤七中的修正和更新采用图优化SLAM中的后端图优化方法。

作为优选，所述的小车包括激光雷达传感器、陀螺仪、直流电源供电模块、控制模块、电机驱动电路和小车底盘。激光雷达传感器、陀螺仪均安装在激光雷达小车上。小车主体包括车身、车轮和电机。四个车轮安装在车身底部，由四个电机分别驱动。电机由控制模块通过电机驱动电路控制。流电源供电模块为激光雷达传感器、陀螺仪、控制模块和电机供电。陀螺仪感应角度信息和加速度信息，并将数据传输给控制模块。激光雷达传感器不断向周围发射和接受激光数据，并将数据传输给控制模块。

作为优选，所述的电机驱动电路包括电机驱动器U1。电机驱动器U1的型号为TB6612FNG。电机驱动器U1的3、4、9及10引脚均接电阻R1的一端。电阻R1的另一端接地。电机驱动器U1的13、14、24引脚均接电容C1、电容C2及电容C3的一端。电容C1、电容C2及电容C3的另一端均接外部电压。电机驱动器U1的15及23引脚连接在一起，作为电机驱动电路4的转速控制端；电机驱动器U1的17及21引脚连接在一起，作为电机驱动电路4的第一转向控制端；电机驱动器U1的16及22引脚连接在一起，作为电机驱动电路4的第二转向控制端；电机驱动器U1的1、5引脚作为电机驱动电路4的第一电机控制接口。电机驱动器U1的8、12引脚作为电机驱动电路4的第二电机控制接口。电机驱动电路共有两个。其中一个电机驱动电路的第一电机控制接口、第二电机控制接口与位于小车同一侧的两个电机的接口分别连接。另一个电机驱动电路的第一电机控制接口、第二电机控制接口与位于小车另一侧的两个电机的接口分别连接。

作为优选，控制模块采用型号为BCM2837的树莓派。控制模块的6、9、14、20、25、30、34及39引脚均接地，2及4引脚均接电池经过稳压电路输出的供电电压5v电压。控制模块的第一I/O口、第二I/O口、第三I/O口与其中一个电机驱动电路的转速控制端、第一转向控制端、第二转向控制端分别连接。控制模块的第四I/O口、第五I/O口、第六I/O口与另一个电机驱动电路的转速控制端、第一转向控制端、第二转向控制端分别连接。激光雷达传感器的1至4引脚与控制模块的4个接线端分别连接。控制模块的引脚接激光雷达的M-EN及DEV-EN引脚，10引脚接激光雷达的TX引脚。控制模块的引脚接陀螺仪的SDA引脚，5引脚接陀螺仪的SCL引脚。

作为优选，所述的直流电源供电模块采用锂离子电池。

本发明具有的有益效果是：

1、本发明利用坡度信息修正激光雷达得到的数据，能够在复杂坡度环境下，通过二维激光雷达实现小车的自主导航，进而能够大大降低室内无人小车的成本。

2、本发明能够提高激光SLAM算法的鲁棒性和实时性，从而能够使得小车能够在更加复杂的环境下精准迅速的绘制地图，克服了常规的激光雷达小车只能只能在平坦地图运行的缺陷。

附图说明

图1为本发明采用的激光雷达小车的系统框图；

图2为本发明采用的激光雷达小车中电机驱动电路的电路原理图；

图3为本发明采用的激光雷达小车中控制模块的电路连接图。

具体实施方式

以下结合附图对本发明作进一步说明。

如图1所示，一种激光雷达小车在复杂坡度环境下的地图构建方法，采用的激光雷达小车包括激光雷达传感器2、陀螺仪4、直流电源供电模块5、控制模块3、电机驱动电路1和小车底盘。激光雷达传感器2、陀螺仪4均安装在激光雷达小车上。小车主体包括车身、车轮和电机。四个车轮安装在车身底部，由电机驱动为激光雷达小车提供动力。电机由控制模块3控制。直流电源供电模块5采用锂离子电池，为激光雷达传感器2、陀螺仪4、控制模块3和电机供电。陀螺仪的型号为MPU-6050。

陀螺仪4感应角度信息和加速度信息，并将数据传输给控制模块3。激光雷达传感器2不断向周围发射和接受激光数据，并将数据传输给控制模块3。电机驱动电路1通过电机驱动器在控制模块3控制下驱动电机。12V直流电源供电模块5为控制模块3以及电机驱动电路1提供电源。

如图2所示，电机驱动电路1包括电机驱动器U1。电机驱动器U1的型号为TB6612FNG。电机驱动器U1的3、4、9及10引脚均接电阻R1的一端。电阻R1的另一端接地。电机驱动器U1的13、14、24引脚均接电容C1、电容C2及电容C3的一端。电容C1、电容C2及电容C3的另一端均接外部电压VCC(5V电压)。电机驱动器U1的15及23引脚连接在一起，作为电机驱动电路4的转速控制端；电机驱动器U1的17及21引脚连接在一起，作为电机驱动电路4的第一转向控制端；电机驱动器U1的16及22引脚连接在一起，作为电机驱动电路4的第二转向控制端；电机驱动器U1的1、5引脚作为电机驱动电路4的第一电机控制接口。电机驱动器U1的8、12引脚作为电机驱动电路4的第二电机控制接口。

电机驱动电路1共有两个。其中一个电机驱动电路1的第一电机控制接口、第二电机控制接口与位于同一侧的两个电机的接口分别连接。另一个电机驱动电路4的第一电机控制接口、第二电机控制接口与位于另一侧的两个电机的接口分别连接。同一电机驱动电路1连接的两个电机在图2中分别标为M1、M2。其中一个电机驱动电路1的转速控制端、第一转向控制端、第二转向控制端分别标为PWM、IN1、IN2。另一个电机驱动电路4的转速控制端、第一转向控制端、第二转向控制端分别标为PWM'、IN1'、IN2'。

如图3所示，控制模块3采用型号为BCM2837的树莓派系统。控制模块3的6、9、14、20、25、30、34及39引脚均接地，2及4引脚均接电池经过稳压电路输出的供电电压5v电压。控制模块3的第一I/O口(36引脚)、第二I/O口(38引脚)、第三I/O口(40引脚)与其中一个电机驱动电路4的转速控制端PWM、第一转向控制端IN1、第二转向控制端IN2分别连接。控制模块的第四I/O口(33引脚)、第五I/O口(35引脚)、第六I/O口(37引脚)与另一个电机驱动电路4的转速控制端PWM'、第一转向控制端IN1'、第二转向控制端IN2'分别连接。激光雷达传感器2的1至4引脚与控制模块3的4个接线端分别连接。

控制模块3的1引脚接激光雷达2的M-EN及DEV-EN引脚(作为为激光雷达高电平使能)，10引脚(IO15，RXD)接激光雷达2的TX引脚(串口通信的数据输出口)。控制模块3的3引脚接陀螺仪4的SDA引脚(IIC通信数据线，图中的SDA1)，5引脚接陀螺仪的SCL引脚(IIC通信时钟线，图中的SCL1)。

该激光雷达小车在复杂坡度环境下的地图构建方法，具体如下：

步骤一、将激光雷达小车放置到需要构建地图的工作场地上，构建全局坐标系并初始化环境地图和小车的初始导航信息。在激光雷达小车上建立相对坐标系；初始状态下，相对坐标系与全局坐标系保持一致。导航信息包括相对坐标系的X轴方向、Y轴方向、X轴速度分量、Y轴速度分量和航向。初始导航信息ζ₀中的所有数据均为0。航向为方位角。之后，循环执行步骤二至九；第t次执行步骤二至九的时刻记为t时刻。每次执行步骤二至九，t均增大1。

步骤二、通过上位机控制的方式驱动激光雷达小车对周围环境进行地图构建。激光雷达小车在行进过程中，每隔预设时间向周围发射和接受激光粒子，得到激光雷达小车各个方向的激光距离数据(具体为障碍物距离数据)，距离数集S_t＝{s_t,1,s_t,2,...,s_t,n}。n为激光

步骤三、陀螺仪4检测当前时刻的X轴加速度、Y轴加速度Z轴角速度和激光雷达传感器2采集的数据传输给控制模块。

步骤四、控制模块对激光雷达数据进行预处理，并依据三角测距原理和激光雷达的有效范围参数，将距离数集S_t中超出激光雷达有效范围的数据进行剔除；被剔除的数据根据相邻两侧的数据补齐(具体为取相邻两侧数据的平均值)。

步骤五、根据上一时刻的导航信息ζ_t-1和当前时刻的X轴加速度a_t ^bx、Y轴加速度a_t ^by和Z轴角速度W_t ^bz，惯性递推出激光雷达小车的当前时刻的导航信息ζ_t＝[P^nx _t,P^ny _t,V^nx _t,V^ny _t,θⁿ _t]，其中P^nx _t、P^ny _t、V^nx _t、V^ny _t、θⁿ _t分别为t时刻激光雷达小车在X坐标轴方向、Y坐标轴方向、X坐标轴速度分量、Y坐标轴速度分量和航向。ζ₀为初始导航信息，五个参数均为0。

步骤六、通过坡度算法对步骤四中所得的激光雷达检测到的距离数集S_t进行补偿修正，得到修正后距离数集S′_t＝{s′_t,1,s′_t,2,...,s′_t,n}；具体过程如下：

①.陀螺仪检测当前时刻的前后俯仰角α、和左右翻滚角β。据此对距离数集S_t进行修正，得到修正后距离数集S′_t＝{s′_t,1,s′_t,2,...,s′_t,n}。确定容差角δ的取值。

若|α|>δ且|β|≤δ，则

若|α|≤δ且|β|>δ，则

若同时满足α>δ、β<-δ或者同时满足α<-δ、β>δ，则

若同时满足α>δ、β>δ或者同时满足α<-δ、β<-δ，则

否则，s′_t,i＝s_t,i，i＝1,2,...,n。

②.根据修正后距离数集S′_t建立粗略环境地图。

步骤七、根据修正后距离数集S′_t，通过用图优化SLAM中的后端图优化方法，对导航信息ζ_t中的X坐标轴方向P^nx _t、Y坐标轴方向P^ny _t和航向θⁿ _t进行修正和更新，得到新的导航信息ζ_t(三个参数被更新)。

步骤八、将经步骤七修正后的导航信息ζ_t进行卡尔曼融合滤波，进而得到激光雷达小车的导航信息。

步骤九、根据步骤八中滤波后的激光雷达小车的导航信息和步骤六中的粗略环境地图，通过覆盖栅图算法更新全局环境地图。

Claims (6)

1.一种激光雷达小车在具有坡度环境下的地图构建方法，其特征在于：步骤一、构建全局坐标系并初始化环境地图和小车的初始导航信息；在激光雷达小车上建立相对坐标系；初始状态下，相对坐标系与全局坐标系保持一致；导航信息包括相对坐标系的X轴方向、Y轴方向、X轴速度分量、Y轴速度分量和航向；初始导航信息中的所有数据均为0；之后，行车行进，循环执行步骤二至九，直至地图构建完成；第t次执行步骤二至九的时刻记为t时刻；每次执行步骤二至九，t均增大1；

步骤二、小车上的激光雷达向周围发射和接受激光粒子，得到激光雷达小车各个方向的激光距离数据组成的距离数集S_t＝{s_t,1,s_t,2,...,s_t,n}；

步骤三、陀螺仪检测当前时刻的X轴加速度、Y轴加速度、Z轴角速度和激光雷达传感器采集的距离数集S_t给控制模块；

步骤四、控制模块对激光雷达数据进行预处理，并依据三角测距原理和激光雷达的有效范围参数，将距离数集S_t中超出激光雷达有效范围的数据进行剔除；被剔除的数据根据相邻两侧的数据补齐；

步骤五、根据上一时刻的导航信息ζ_t-1和当前时刻的X轴加速度a_t ^bx、Y轴加速度a_t ^by和Z轴角速度W_t ^bz，惯性递推出激光雷达小车的当前时刻的导航信息ζ_t＝[P^nx _t,P^ny _t,V^nx _t,V^ny _t,θⁿ _t]，其中P^nx _t、P^ny _t、V^nx _t、V^ny _t、θⁿ _t分别为当前时刻激光雷达小车在X坐标轴方向、Y坐标轴方向、X坐标轴速度分量、Y坐标轴速度分量和航向；

步骤六、通过坡度算法对步骤四中所得的激光雷达检测到的距离数集S_t进行补偿修正，得到修正后距离数集S′_t＝{s′_t,1,s′_t,2,...,s′_t,n}；具体过程如下：

①.陀螺仪检测当前时刻的前后俯仰角α、和左右翻滚角β；据此对距离数集S_t进行修正，得到修正后距离数集S′_t＝{s′_t,1,s′_t,2,...,s′_t,n}；确定容差角δ的取值；

若|α|>δ且|β|≤δ，则

若|α|≤δ且|β|>δ，则

若同时满足α>δ、β<-δ或者同时满足α<-δ、β>δ，则

若同时满足α>δ、β>δ或者同时满足α<-δ、β<-δ，则

否则，s′_t,i＝s_t,i，i＝1,2,...,n；

②.根据修正后距离数集S′_t建立粗略环境地图；

步骤七、根据修正后距离数集S′_t，对导航信息ζ_t中的X坐标轴方向P^nx _t、Y坐标轴方向P^ny _t和航向θⁿ _t进行修正和更新，得到新的导航信息ζ_t；

步骤八、将经步骤七修正后的导航信息ζ_t进行卡尔曼融合滤波；

步骤九、根据步骤八中滤波后的导航信息ζ_t和步骤六中建立的粗略环境地图更新全局环境地图。

2.根据权利要求1所述的一种激光雷达小车在具有坡度环境下的地图构建方法，其特征在于：步骤七中的修正和更新采用图优化SLAM中的后端图优化方法。

3.根据权利要求1所述的一种激光雷达小车在具有坡度环境下的地图构建方法，其特征在于：所述的小车包括激光雷达传感器、陀螺仪、直流电源供电模块、控制模块、电机驱动电路和小车底盘；激光雷达传感器、陀螺仪均安装在激光雷达小车上；小车主体包括车身、车轮和电机；四个车轮安装在车身底部，由四个电机分别驱动；电机由控制模块通过电机驱动电路控制；直流电源供电模块为激光雷达传感器、陀螺仪、控制模块和电机供电；陀螺仪感应角度信息和加速度信息，并将数据传输给控制模块；激光雷达传感器不断向周围发射和接受激光数据，并将数据传输给控制模块。

4.根据权利要求3所述的一种激光雷达小车在具有坡度环境下的地图构建方法，其特征在于：所述的电机驱动电路包括电机驱动器U1；电机驱动器U1的型号为TB6612FNG；电机驱动器U1的3、4、9及10引脚均接电阻R1的一端；电阻R1的另一端接地；电机驱动器U1的13、14、24引脚均接电容C1、电容C2及电容C3的一端；电容C1、电容C2及电容C3的另一端均接外部电压；电机驱动器U1的15及23引脚连接在一起，作为电机驱动电路4的转速控制端；电机驱动器U1的17及21引脚连接在一起，作为电机驱动电路4的第一转向控制端；电机驱动器U1的16及22引脚连接在一起，作为电机驱动电路4的第二转向控制端；电机驱动器U1的1、5引脚作为电机驱动电路4的第一电机控制接口；电机驱动器U1的8、12引脚作为电机驱动电路4的第二电机控制接口；电机驱动电路共有两个；其中一个电机驱动电路的第一电机控制接口、第二电机控制接口与位于小车同一侧的两个电机的接口分别连接；另一个电机驱动电路的第一电机控制接口、第二电机控制接口与位于小车另一侧的两个电机的接口分别连接。

5.根据权利要求4所述的一种激光雷达小车在具有坡度环境下的地图构建方法，其特征在于：控制模块采用型号为BCM2837的树莓派；控制模块的6、9、14、20、25、30、34及39引脚均接地，2及4引脚均接电池经过稳压电路输出的供电电压5v电压；控制模块的第一I/O口、第二I/O口、第三I/O口与其中一个电机驱动电路的转速控制端、第一转向控制端、第二转向控制端分别连接；控制模块的第四I/O口、第五I/O口、第六I/O口与另一个电机驱动电路的转速控制端、第一转向控制端、第二转向控制端分别连接；激光雷达传感器的1至4引脚与控制模块的4个接线端分别连接；控制模块的1引脚接激光雷达的M-EN及DEV-EN引脚，10引脚接激光雷达的TX引脚；控制模块的3引脚接陀螺仪的SDA引脚，5引脚接陀螺仪的SCL引脚。

6.根据权利要求3所述的一种激光雷达小车在具有坡度环境下的地图构建方法，其特征在于：所述的直流电源供电模块采用锂离子电池。

Images