[发明专利]基于2D激光雷达的三维激光SLAM系统及控制方法

权利要求书

1.一种基于2D激光雷达的三维激光SLAM系统的控制方法，使用基于2D激光雷达的三维激光SLAM系统实现，该系统包括：装置主体，路由器，计算机；

其中，装置主体包括：嵌入式开发板、直流电源、2D激光雷达、舵机和导电滑环；

所述导电滑环包括：导电滑环转子与导电滑环定子；

所述嵌入式开发板与路由器相连接，路由器与计算机相连接；直流电源分别与导电滑环定子、嵌入式开发板和舵机相连接；导电滑环定子与导电滑环转子相连接，导电滑环转子与2D激光雷达相连接，2D激光雷达与嵌入式开发板相连接；舵机与嵌入式开发板相连接；

所述路由器，实现嵌入式开发板与计算机的实时通讯；

所述计算机，用来接收嵌入式开发板专递过来的数据，显示三维点云地图，并向嵌入式开发板传递控制指令；

所述嵌入式开发板，所有程序都在嵌入式开发板上面运行，被编程的程序用来实现三维图的创建与定位，并给舵机发送运动控制命令，使其匀速转动，同时读取舵机内部编码器的数据和2D激光雷达的数据，实现两种传感器数据同步；

所述直流电源，为嵌入式开发板、2D激光雷达和舵机提供供电电源；

所述2D激光雷达，通过USB线与嵌入式开发板相连接，将2D激光雷达的获取传感器信号传递给嵌入式开发板；

所述舵机，给2D激光雷达提供旋转，配合2D激光雷达生成三维点云，舵机内部含有编码器，可为系统提供旋转角度信息，并将该角度信息传递给嵌入式开发板；

所述导电滑环，避免因2D激光雷达的旋转产生的缠绕；

所述系统控制方法，其特征在于，具体包括底层运动控制过程和上层地图重建过程：

(1)底层运动控制过程，具体流程如下：

步骤1：初始化：令舵机角度值为零；

步骤2：若得到从计算机传递过来启动指令，则启动底层运动控制过程，并控制舵机匀速转动，否则继续步骤1初始化过程；

步骤3：读取舵机内部编码器与2D激光雷达的同步数据；

步骤4：使用舵机内部编码器与2D激光雷达的同步数据，生成三维点云；

步骤5：将生成点云的结果发送给计算机；

步骤6：若计算机传递过来结束指令，则结束底层运动控制过程，否则返回步骤3，继续读取舵机内部编码器与2D激光雷达的同步数据；

(2)上层地图重建过程，具体流程如下：

步骤1：数据预处理：将底层运动控制过程传递给计算机的点云计算结果作为数据预处理对象，采用PCL库中的方法对三维点云进行滤波出去离群点，得到未扭曲的三维点云；

步骤2：基于未扭曲的三维点云，使用标定算法进行地图定位，消除安装误差对三维点云造成的影响，得到粗略的定位信息；

所述上层地图重建过程中标定算法，具体流程如下：

步骤2.1：初始化标定角：设定一个初始值{α₀,β₀}，角度θ₀设定为一个固定值；

步骤2.2：获取新的在笛卡尔坐标系下的三维点云坐标：

其中，C(·)和S(·)分别代表cos和sin函数，α、β、θ分别是激光雷达x、y、z轴因为安装误差产生的偏移角度，ρ为激光点到激光头的距离，是从激光雷达获取的数据；

步骤2.3：使用一致性算法对三维点云随机采样，进行分割提取平面点，获得拥有最大内点个数的P个平面和平面方程；

步骤2.4：计算代价函数E：

其中，N是有效深度的数据点的总个数，有效点指的是抛弃错误的和超出规定距离的数据点，N_j为第j个平面的内点个数，d_j,i表示第i个内点到其所对应的第j个平面的距离；P是平面个数；

步骤2.5：若代价函数E小于阈值，则输出标定角度，结束标定算法，否则以当前的标定角度为初始化角度，重新获取新的在笛卡尔坐标系下的三维点云坐标，重复步骤2.2～步骤2.6；

步骤3：使用地图重建算法得到粗略的地图信息；

步骤4：使用地图优化算法，消除累积漂移，得到最终的地图和精确定位信息。