一、项目介绍、硬件选型指南、环境配置
这里使用旭日X3派开发板设计并实现了一款SLAM手持建图设备，它使用旭日X3派开发板的计算性能，通过激光 雷达实现一个纯激光里程计，通过IMU对激光数据进行畸变矫正和对SLAM的姿态矫正，实现一个手持的SLAM建图功能。
最近在进行一些无人机的SLAM建图设计，在没有轮里程计算的情况下智能通过春激光雷达实现地图的构建。之前也尝试了Hector算法但是效果不是很理想，在借鉴了Lio-Sam的代码之后，才有了此次的设计。此次设计使用IMU先对激光雷达数据进行点云畸变矫正，将处理好的点云数据进行特征点提取，然后使用PL-ICP计算得到一个Laser Odometry。在维护好TF的前提下，使用这个Laser Odometry + 一些常用的SLAM功能包（比如Gmapping）就可以实现手持建图的功能。
话不多说，先看功能演示（单个功能的演示视频在相应章的末尾呦）
当然，也可以使用Laser Odometry + 激光雷达数据纯手写一个SLAM！
硬件主要使用旭日x3开发板、Lakibeam 1激光雷达、JY61陀螺仪模块，其次是电源模块、12V锂电池、PCB板、亚克力结构和手柄结构。
旭日X3开发板是一款基于ARM架构的开发板，采用了Cortex-A53架构的Allwinner H3处理器，主频可达1.2GHz。该开发板具有多种接口和扩展接口，可以满足不同应用场景的需求。同时其支持Android和Linux操作系统，可以用于智能家居、智能物联网、嵌入式开发等领域。该开发板内置1GB DDR3内存和8GB eMMC存储，还支持TF卡扩展存储。此外，该开发板还支持WiFi和蓝牙功能，方便开发者进行无线通信。
Lakibeam 1激光雷达是一种高精度的激光雷达，采用固态激光器和高速扫描技术，可以实时获取周围环境的三维点云数据，并且具有高精度、高分辨率和高可靠性的特点。Lakibeam 1激光雷达还具有小巧轻便、低功耗、易于集成等优点，可以满足各种复杂环境下的应用需求。
JY61陀螺仪模块是一种集成了加速度计、陀螺仪和磁力计的传感器模块，能够测量物体的加速度、角速度和方向等数据。该模块采用MEMS技术，具有高精度、高稳定性、低功耗、小尺寸等特点，运用范围广。其采用I2C或串口通信方式，可以与微处理器或单片机进行连接和通信，方便用户进行数据处理和控制。同时，该模块还支持多种数据输出格式，包括原始数据、角度、角速度、加速度等，用户可以根据自己的需求选择合适的输出格式。在应用中，JY61陀螺仪模块可以用于姿态控制、导航、运动控制等方面。例如，在机器人中，JY61陀螺仪模块可以用于测量机器人的倾斜角度和运动状态，从而实现机器人的自动导航和控制；在自动驾驶车辆中，JY61陀螺仪模块可以用于测量车辆的加速度和角速度，从而实现车辆的稳定控制和自动驾驶。
以下是此次的硬件框架图。
整个系统使用12V锂电池进行供电。通过XL4016E1电源模块实现12V转5V 8A(Max)为旭日X3派开发板进行供电。激光雷达使用智驰的Lakibeam 1单线激光雷达，通过以太网接入到旭日X3派开发板中。陀螺仪使用维特智能的JY61模组（现已停产停售，可使用JY60/JY61P/JY901等代替），通过旭日X3派IO引脚的串口功能接入到系统中（该串口号为/dev/ttyS3）。
这里我简单的设计了PCB部分和结构部分，如下图。
原理图
PCB视图
PCB仿真图_顶部
PCB仿真图_底部
上下固定板（亚克力结构
手柄部分（3D打印）
这里我使用的是旭日X3派的Ubuntu 20.04 Desktop版本，接下来我们来配置下ROS环境。（ROS环境配置的步骤，包括换源、安装ros、执行rosdep、创建工作空间、配置环境变量，可以直接CSDN啥的复制）
在烧录Ubuntu系统镜像前，用户需要做如下准备：
- 准备至少8GB容量的Micro SD卡
- SD 读卡器
- 下载Ubuntu镜像压缩包，并解压出Ubuntu系统镜像文件：
system_sdcard.img - 下载镜像烧录工具balenaEtcher，下载链接：https://www.balena.io/etcher/ 从资源中心获取开发板的Ubuntu镜像文件，将其烧录到已准备的SD卡中，解压镜像文件并下载balenaEtcher,用balenaEtcher打开镜像文件
我选择的是桌面版的镜像，现在开始配置ROS Noetic环境，这里使用小鱼的一键安装www.fishros.com
启动旭日派，打开终端，输入小鱼一键安装指令
选择选项1，安装ROS
选择选项1，安装ROS；之后选择2，更换系统源并清理第三方源
选择安装的ROS版本是noetic，选择桌面版
到这儿一步就是安装成功了
接下来执行rosdep
接下来就是创建工作空间和功能包，该项目所使用的代码均已上传gitee，下载链接为：https://gitee.com/zhao-yuanqi/sunrise_slam.git
现在来看看我们做的实物吧
二、IMU驱动功能包设计、IMU数据可视化
imu_driver陀螺仪功能包：通过WT61CTTL六轴陀螺仪模块获取数据，并发布Imu数据。
功能：
通过读取WT61C TTL传感器的加速度、角速度、欧拉角，发布imu数据，在RVIZ下进行可视化显示。
功能包的详细组成如下：
cfg：配置文件
存放rviz的启动配置文件imu_display.rviz，在启动RVIZ进行测试的时候可以直接使用我配置好的内容。
include：无内容，无需考虑。
launch：启动文件
imu_driver.launch启动两个节点，其中WT61CTTL节点负责从IMU传感器获取数据，以及进行校准服务；
msg：话题类型文件
WT61CTTL.msg存放WT61C TTL传感器发出的数据类型，里面存储9个float64类型的数据。这是传感器的原始数据。
float64 x_acc <!--X轴加速度-->
float64 y_acc <!--Y轴加速度-->
float64 z_acc <!--Z轴加速度-->
float64 x_gyro <!--X轴角速度-->
float64 y_gyro <!--Y轴角速度-->
float64 z_gyro <!--Z轴角速度-->
float64 roll <!--伏仰角（X轴）-->
float64 pitch <!--翻滚角（Y轴）-->
float64 yaw <!--航向角（Z轴）-->srv：服务类型文件
setAccZero.srv服务负责加速度数据校准，内容如下：
---
int32 status传入任意参数，返回校准结果。
setYawZero.srv服务负责航向角数据校准，内容如下：
---
int32 status传入任意参数，返回校准结果。
随着运行时间的增加，加速度数值可能出现漂移，这时候造成的误差是比较大的，此时让机器人处于静止状态，调用该服务实现加速度数值校准。
航向角数字也是通过数学积分计算得到的，随着时间的增加会出现误差值，调用该服务实现Yaw数值置0。
scripts：脚本文件
demo.py是由厂商提供的示例程序，详情请查看
WT61CTTL.py是我根据示例程序修改得到的，具体请看下面详细注释。
#!/usr/bin/env python3
#coding:utf-8
#声明环境是Python3,编码格式为utf-8
import rospy #导入ros相关文件
from imu_driver.msg import WT61CTTL #导入自定义话题类型文件
from imu_driver.srv import * #导入自定义服务类型文件
import serial #导入串口通讯相关文件
#从串口读取的原始数据
ACCData=[0.0]*8
GYROData=[0.0]*8
AngleData=[0.0]*8
FrameState = 0 #通过0x后面的值判断属于哪一种情况
Bytenum = 0 #读取到这一段的第几位
CheckSum = 0 #求和校验位
#转换后的数据存储
a = [0.0]*3 #加速度
w = [0.0]*3 #角速度
Angle = [0.0]*3 #欧拉角
‘’‘
*函数名称：DueData
*函数功能：处理串口的数据，转换成WT61CTTL格式的数据，并发布该消息
*输入参数：inputdata，串口读取的原始数据
*输出参数：无
’‘’
def DueData(inputdata): #新增的核心程序，对读取的数据进行划分，各自读到对应的数组里
global FrameState #在局部修改全局变量，要进行global的定义
global Bytenum
global CheckSum
global a
global w
global Angle
for data in inputdata: #在输入的数据进行遍历
if data==0x55 and Bytenum==0: #0x55位于第一位时候，开始读取数据，增大bytenum
CheckSum=data
Bytenum=1
continue
elif data==0x51 and Bytenum==1:#在byte不为0 且 识别到 0x51 的时候，改变frame
CheckSum+=data
FrameState=1
Bytenum=2
elif data==0x52 and Bytenum==1: #同理
CheckSum+=data
FrameState=2
Bytenum=2
elif data==0x53 and Bytenum==1:
CheckSum+=data
FrameState=3
Bytenum=2
elif FrameState==1: # acc #已确定数据代表加速度
if Bytenum<10: # 读取8个数据
ACCData[Bytenum-2]=data # 从0开始
CheckSum+=data
Bytenum+=1
else:
if data == (CheckSum&0xff): #假如校验位正确
a = get_acc(ACCData)
CheckSum=0 #各数据归零，进行新的循环判断
Bytenum=0
FrameState=0
elif FrameState==2: # gyro
if Bytenum<10:
GYROData[Bytenum-2]=data
CheckSum+=data
Bytenum+=1
else:
if data == (CheckSum&0xff):
w = get_gyro(GYROData)
CheckSum=0
Bytenum=0
FrameState=0
elif FrameState==3: # angle
if Bytenum<10:
AngleData[Bytenum-2]=data
CheckSum+=data
Bytenum+=1
else:
if data == (CheckSum&0xff):
Angle = get_angle(AngleData)
d = list(a)+list(w)+list(Angle) #拼装数据
#print("a(g):%10.3f %10.3f %10.3f w(deg/s):%10.3f %10.3f %10.3f Angle(deg):%10.3f %10.3f %10.3f"%d)
jhr_imu = WT61CTTL() #建立WT61CTTL类型的数据jhr_imu
jhr_imu.x_acc = d[0] #提取X轴加速度值
jhr_imu.y_acc = d[1] #提取Y轴加速度值
jhr_imu.z_acc = d[2] #提取Z轴加速度值
jhr_imu.x_gyro = d[3] #提取X轴角速度值
jhr_imu.y_gyro = d[4] #提取Y轴角速度值
jhr_imu.z_gyro = d[5] #提取Z轴角速度值
jhr_imu.roll = d[6] #提取俯仰角值
jhr_imu.pitch = d[7] #提取翻滚角值
jhr_imu.yaw = d[8] #提取航向角值
wt61cttlpub.publish(jhr_imu) #发布数据
rate.sleep()
#标识数据初始化
CheckSum=0
Bytenum=0
FrameState=0
‘’‘
*函数名称：get_acc
*函数功能：通过串口原始数据，计算得到加速度标准数据
*输出参数：加速度数值部分的串口原始数据
*输出参数：X、Y、Z三个的加速度值
*特殊说明：函数get_gyro、函数get_angle和这里一样，具体计算请查阅数据手册
’‘’
def get_acc(datahex):
axl = datahex[0]
axh = datahex[1]
ayl = datahex[2]
ayh = datahex[3]
azl = datahex[4]
azh = datahex[5]
k_acc = 16.0
acc_x = (axh << 8 | axl) / 32768.0 * k_acc
acc_y = (ayh << 8 | ayl) / 32768.0 * k_acc
acc_z = (azh << 8 | azl) / 32768.0 * k_acc
if acc_x >= k_acc:
acc_x -= 2 * k_acc
if acc_y >= k_acc:
acc_y -= 2 * k_acc
if acc_z >= k_acc:
acc_z-= 2 * k_acc
return acc_x,acc_y,acc_z
def get_gyro(datahex):
wxl = datahex[0]
wxh = datahex[1]
wyl = datahex[2]
wyh = datahex[3]
wzl = datahex[4]
wzh = datahex[5]
k_gyro = 2000.0
gyro_x = (wxh << 8 | wxl) / 32768.0 * k_gyro
gyro_y = (wyh << 8 | wyl) / 32768.0 * k_gyro
gyro_z = (wzh << 8 | wzl) / 32768.0 * k_gyro
if gyro_x >= k_gyro:
gyro_x -= 2 * k_gyro
if gyro_y >= k_gyro:
gyro_y -= 2 * k_gyro
if gyro_z >=k_gyro:
gyro_z-= 2 * k_gyro
return gyro_x,gyro_y,gyro_z
def get_angle(datahex):
rxl = datahex[0]
rxh = datahex[1]
ryl = datahex[2]
ryh = datahex[3]
rzl = datahex[4]
rzh = datahex[5]
k_angle = 180.0
angle_x = (rxh << 8 | rxl) / 32768.0 * k_angle
angle_y = (ryh << 8 | ryl) / 32768.0 * k_angle
angle_z = (rzh << 8 | rzl) / 32768.0 * k_angle
if angle_x >= k_angle:
angle_x -= 2 * k_angle
if angle_y >= k_angle:
angle_y -= 2 * k_angle
if angle_z >=k_angle:
angle_z-= 2 * k_angle
return angle_x,angle_y,angle_z
‘’‘
*函数名称：set_acc_zero
*函数功能：通过串口发送对应的指令，实现加速度数值的校准
*输入参数：无
*输出参数：返回status值
’‘’
def set_acc_zero(req):
ser.write(b'\xFF\xAA\x67')
status = 0
return setAccZeroResponse(status)
‘’‘
*函数名称：set_yaw_zero
*函数功能：通过串口发送对应的指令，实现航向角数值的校准
*输入参数：无
*输出参数：返回status值
’‘’
def set_yaw_zero(req):
ser.write(b'\xFF\xAA\x52')
status = 0
return setYawZeroResponse(status)
if __name__=='__main__':
rospy.init_node('WT61CTTL') #初始化节点WT61CTTL
wt61cttlpub = rospy.Publisher('WT61CTTL', WT61CTTL, queue_size=10) #发布WT61CTTL数据
setacczero = rospy.Service('set_acc_zero', setAccZero, set_acc_zero) #创建加速度校准服务
setyawzero = rospy.Service('set_yaw_zero', setYawZero, set_yaw_zero) #创建航向角校准服务
rate = rospy.Rate(100) #以100Hz的频率发布
ser = serial.Serial('/dev/ttyUSB0',115200, timeout=0.5) #创建串口并打开
print(ser.is_open) #输出串口当前状态
while not rospy.is_shutdown(): #只要ros不关闭
datahex = ser.read(33) #串口读取数据
DueData(datahex) #处理数据校准通过发起服务请求完成。
imu_node.py是订阅WT61CTTL的数据，以sensor_msgs/IMU的格式发布，具体请看下面详细注释。
#!/usr/bin/env python2
#coding:utf-8
#声明环境为Python2,编码格式为utf-8
#因为出现了tf2在Python3不兼容的问题，所以降为使用Python2
import rospy #导入ros相关文件
import string
import math
import time
import sys
from tf.transformations import quaternion_from_euler #导入tf计算相关文件
from std_msgs.msg import Float32 #导入std_msgs的Float32话题类型文件
from sensor_msgs.msg import Imu #导入sensor_msgs的Imu话题类型文件
from imu_driver.msg import WT61CTTL #导入jhr_imu的WT61CTTL话题类型文件
degrees2rad = math.pi/180.0 #角度值转弧度值参数
imu_yaw_calibration = 0.0 #yaw校准预留，未使用可删除
accel_factor = 9.806/256.0 #加速度转换计算
seq = 0 #计数
‘’‘
*函数名称：WT61CTTL_callback
*函数功能：订阅回调函数，当订阅到WT61CTTL话题时执行，将WT61CTTL的数据格式转成Imu的数据格式，并发布
*输入参数：WT61CTTL消息耳的内容data
*输出参数：无
’‘’
def WT61CTTL_callback(data):
yaw_deg = data.yaw #获取yaw
yaw_deg = yaw_deg + imu_yaw_calibration #yaw校准，加0,老版本使用的，没有修改
yaw = yaw_deg*degrees2rad //yaw转弧度
pitch = float(data.pitch)*degrees2rad //获取pitch转弧度
roll = float(data.roll)*degrees2rad //获取roll转弧度
#Imu的加速度
imuMsg.linear_acceleration.x = float(data.x_acc)*accel_factor
imuMsg.linear_acceleration.y = float(data.y_acc)*accel_factor
imuMsg.linear_acceleration.z = float(data.z_acc)*accel_factor
#Imu的角速度
imuMsg.angular_velocity.x = float(data.x_gyro)*degrees2rad
imuMsg.angular_velocity.y = float(data.y_gyro)*degrees2rad
imuMsg.angular_velocity.z = float(data.z_gyro)*degrees2rad
#将欧拉角转成四元数
q = quaternion_from_euler(roll, pitch, yaw)
imuMsg.orientation.x = q[0]
imuMsg.orientation.y = q[1]
imuMsg.orientation.z = q[2]
imuMsg.orientation.w = q[3]
#四元数协方差估计矩阵
# Orientation covariance estimation
imuMsg.orientation_covariance = [
0.0025 , 0 , 0,
0, 0.0025, 0,
0, 0, 0.0025
]
#角速度协方差估计矩阵
# Angular velocity covariance estimation
imuMsg.angular_velocity_covariance = [
0.02, 0 , 0,
0 , 0.02, 0,
0 , 0 , 0.02
]
#加速度协方差估计矩阵
# linear acceleration covariance estimation
imuMsg.linear_acceleration_covariance = [
0.04 , 0 , 0,
0 , 0.04, 0,
0 , 0 , 0.04
]
#上面参数我是抄ROS小课堂张志杰老师的，有兴趣你可以自己算算，我这里能用
#Imu数据头部标识
global seq
imuMsg.header.stamp= rospy.Time.now()
imuMsg.header.frame_id = "base_imu_link"
imuMsg.header.seq = seq
seq = seq + 1
data_pub.publish(imuMsg) #发布Imu数据
rate.sleep()
rospy.init_node("imu_node") #初始化节点并命名为imu_node
data_pub = rospy.Publisher("imu_data", Imu, queue_size=1) #定义imu数据发布
rospy.Subscriber('WT61CTTL', WT61CTTL, WT61CTTL_callback) #定义WT61CTTL数据订阅
imuMsg = Imu() #实例化Imu类似数据
rate = rospy.Rate(100) #以20Hz的频率处理
rospy.spin()以上两部分为核心代码，WT61CTTL获取数据给imu_data，imu_data处理后以Imu的格式发布出来。
校准的功能我写在WT61CTTL里面的，因为校准是通过串口发送数据来实现的，WT61CTTL可以理解为IMU传感器的驱动功能包，而imu_data则是IMU传感器数据的处理功能包。后期维护请注意你要修改的是什么，不要同时都修改！！！
CMakeList.txt和packahe.xml的内容自行阅读。
遗留的BUG：
IMU启动大约20分钟左右会出现数据延迟2s的情况，重新校准加速度即可，延迟2s不影响使用，但是希望得到解决。
IMU的Yaw（Z轴）数值启动后存在误差，且随着时间推移误差值十分明显，特别是在剧烈运动时，猜测是积分计算的丢失，希望得到解决。
三、激光雷达配置及使用、激光雷达数据可视化、简单处理激光雷达数据
LakiBeam通过网线和电源与计算机建立连接，产品默认的网络配置为静态模式，设备端的IP地址为出厂设定192.168.198.2（Device端），需要先将连接雷达与计算机以太网端口的IP地址手动更改为192.168.198.1（Host端），此时通过计算机的web浏览器访问 http://192.168.198.2即可访问设备内建的web服务。
仪表板（Dashboard）页面提供了系统状态监测（包含CPU使用率，系统负载，内存使用率，以太网数据速率，系统运行时间），雷达状态监测（包含输入电压，输入电流，系统电压，核心温度，APD偏置电压，电机转速，激光测距状态），网卡信息，设备信息。
雷达设置（LiDAR Configuration）页面提供了雷达扫描相关配置（包含扫描频率设置，测距的启动和停止）；数据处理配置（包含数据输出范围设置和数据滤波设置）；数据发送目标配置（包含目标主机的IP地址及数据端口）；网络配置（包含DHCP和静态IP的模式切换，静态IP的设置）。需要注意的是更改网络配置后必须对设备进行重启操作，新的网络配置信息将在下一次启动时自动应用。
在配置好雷达IP和本地IP后，就可以来启动Lakibaem 1激光雷达，指令如下。
roslaunch lakibeam1 lakibeam1_scan.launch
激光雷达的数据类型是sensor_msgs/LaserScan，其数据格式如下。
在ros下大多数传感器数据都会带一个header，这里包含的时间戳和坐标系，雷达的数据存储的ranges数组和intensities数据，这是一个使用角度+距离进行的坐标描述——极坐标系。其他的都是雷达存储参数。
这里我们来做一个简单的Demo，订阅雷达的书记信息，将这个极坐标的描述转为空间直角坐标下的描述。代码如下。（代码出处https://www.zhihu.com/column/c_1314297528322764800）
#include <ros/ros.h>
#include <sensor_msgs/LaserScan.h>
// 声明一个类
class LaserScan
{
private:
ros::NodeHandle node_handle_; // ros中的句柄
ros::NodeHandle private_node_; // ros中的私有句柄
ros::Subscriber laser_scan_subscriber_; // 声明一个Subscriber
public:
LaserScan();
~LaserScan();
void ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg);
};
// 构造函数
LaserScan::LaserScan() : private_node_("~")
{
ROS_INFO_STREAM("LaserScan initial.");
// 将雷达的回调函数与订阅的topic进行绑定
laser_scan_subscriber_ = node_handle_.subscribe("scan", 1, &LaserScan::ScanCallback, this);
}
LaserScan::~LaserScan()
{
}
// 回调函数
void LaserScan::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
//输出雷达数据信息
ROS_INFO_STREAM(
"seqence: " << scan_msg->header.seq <<
", time stamp: " << scan_msg->header.stamp <<
", frame_id: " << scan_msg->header.frame_id <<
", angle_min: " << scan_msg->angle_min <<
", angle_max: " << scan_msg->angle_max <<
", angle_increment: " << scan_msg->angle_increment <<
", time_increment: " << scan_msg->time_increment <<
", scan_time: " << scan_msg->scan_time <<
", range_min: " << scan_msg->range_min <<
", range_max: " << scan_msg->range_max <<
", range size: " << scan_msg->ranges.size() <<
", intensities size: " << scan_msg->intensities.size());
// 第5个点的欧式坐标为
double range = scan_msg->ranges[4];
double angle = scan_msg->angle_min + scan_msg->angle_increment * 4;
double x = range * cos(angle);
double y = range * sin(angle);
ROS_INFO_STREAM(
// 第5个数据点对应的极坐标为:
"range = " << range << ", angle = " << angle <<
// 第5个数据点对应的欧式坐标为:
", x = " << x << ", y = " << y
);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lesson1_laser_scan_node"); // 节点的名字
LaserScan laser_scan;
ros::spin(); // 程序执行到此处时开始进行等待，每次订阅的消息到来都会执行一次ScanCallback()
return 0;
}这里对激光雷达的数据进行了订阅，将订阅的极坐标数据转换为执教坐标数据，并输出。
四、SLAM的前端、后端和回环
什么是SLAM？
SLAM（Simultaneous Localization and Mapping）是一种同时进行定位和地图构建的技术，通过使用传感器（如激光雷达、摄像头、惯性测量单元等）收集环境信息，同时估计机器人在环境中的位置和姿态，从而构建出环境的地图。
怎样实现一个SLAM？
实现SLAM需要以下步骤：
1、选择合适的传感器：激光雷达、摄像头、惯性测量单元等传感器可以用于收集环境信息。
2、数据预处理：对传感器数据进行校准、滤波、降噪等预处理操作，以提高数据质量。
3、特征提取：从传感器数据中提取特征点或特征描述符，用于后续的地图构建和定位。
4、建立地图：根据传感器数据和特征点，使用SLAM算法构建环境地图。
5、机器人定位：根据传感器数据和地图信息，使用SLAM算法估计机器人在环境中的位置和姿态。
6、地图更新：当机器人移动时，需要更新地图和机器人位置的估计值。
7、微调：对SLAM算法进行微调，以提高定位和地图构建的精度。常用的SLAM算法包括扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波（PF）和基于图优化的方法（如ORB-SLAM、LSD-SLAM等）。
常用的SLAM功能包有Gmapping、Hector、Cartographer、RtabMap等。
SLAM的前端、后端和回环是什么？有什么作用？
SLAM系统主要由三个部分组成：前端、后端和回环检测。
- 前端：前端是SLAM系统的数据处理部分，其主要任务是从传感器数据中提取特征点或特征描述符，并通过匹配特征点来估计机器人的运动轨迹和地图特征点的位置。前端通常包括特征提取、特征匹配、运动估计和地图构建等模块。
- 后端：后端是SLAM系统的数据优化部分，其主要任务是将前端提取的运动轨迹和地图特征点的位置进行优化，以生成一致的地图和机器人状态估计结果。后端通常使用优化算法，如扩展卡尔曼滤波（EKF）、无迹卡尔曼滤波（UKF）、粒子滤波（PF）和基于图优化的方法（如ORB-SLAM、LSD-SLAM等）。
- 回环检测：回环检测是SLAM系统的一个关键部分，其主要任务是检测机器人是否经过了之前经过的位置，以避免误差的累积。回环检测通常使用图优化方法，如位姿图（pose graph）和因子图（factor graph）等。
在SLAM系统中，前端和后端是相互协作的关系。前端提取的地图特征和机器人运动轨迹作为后端的优化变量，后端通过优化这些变量来得到更精确的地图和机器人状态估计结果。回环检测则在系统运行过程中定期执行，以避免误差的累积。
五、使用PL-ICP设计一个纯雷达里程计
里程计（Odometry）是一种利用机器人自身传感器数据来估计机器人运动轨迹的方法。里程计通常基于机器人的运动学模型，通过对机器人速度和方向的估计来计算机器人的位姿变化。里程计主要用于机器人的自主导航和定位，是SLAM系统中的一个重要组成部分。
里程计通常使用机器人底盘上的编码器或惯性测量单元（IMU）等传感器来测量机器人的运动信息。编码器可以测量机器人轮子的转动角度和速度，从而计算机器人的运动轨迹。IMU可以测量机器人的加速度和角速度，从而计算机器人的姿态和运动轨迹。在SLAM系统中，里程计可以用于初始化机器人的位置和姿态，或者作为SLAM系统的前端部分来估计机器人的运动轨迹和地图特征点的位置。
里程计的精度受到多种因素的影响，如编码器的分辨率、轮子的滑动、机器人的非完美运动模型等。因此，在实际应用中，里程计通常需要与其他传感器结合使用，如激光雷达、摄像头等，以提高定位和地图构建的精度。
雷达里程计（Lidar Odometry）是一种使用激光雷达测量机器人周围环境的距离信息来计算机器人运动轨迹的方法。雷达里程计通常使用机器人上安装的旋转式激光雷达，通过测量激光束与周围物体的距离来构建机器人周围的三维点云地图，并计算机器人的运动轨迹。
雷达里程计的基本原理是通过比较两次激光雷达扫描之间的点云数据，计算机器人的运动轨迹和姿态。具体来说，通过匹配两次扫描中的点云数据，可以计算机器人在两次扫描之间的位移和旋转，从而计算机器人的运动轨迹。雷达里程计通常使用ICP（Iterative Closest Point）算法或其改进版本来进行点云数据匹配，以提高匹配的精度和速度。
与传统里程计相比，雷达里程计可以在更广阔的环境中进行定位和地图构建，因为激光雷达可以测量较远距离的物体。同时，激光雷达可以提供更为准确的环境信息，从而提高定位和地图构建的精度。但与之相应的，雷达里程计的计算量较大，需要较高的计算能力和存储空间。
ICP（Iterative Closest Point）算法是一种常用的点云数据配准算法，常用于雷达里程计中进行点云数据匹配。ICP算法通过最小化两个点云之间的距离误差来实现点云的配准，从而计算机器人的运动轨迹和姿态。
ICP算法的基本流程如下：
1、初始化：将两个点云对齐，选择一个点云作为参考点云，另一个点云作为待匹配点云。
2、特征提取：从参考点云和待匹配点云中提取特征点或特征描述符，用于后续的点云匹配。
3、匹配：将待匹配点云中的每个点与参考点云中的最近邻点进行匹配，计算两个点云之间的距离误差。
4、迭代优化：通过最小化距离误差来优化点云配准，更新当前的位姿估计值，并重复执行步骤3和步骤4，直到收敛为止。
5、终止条件：当距离误差小于预设阈值或达到最大迭代次数时，停止迭代。ICP算法的优化方法有多种，如点对点（Point-to-Point）和点对平面（Point-to-Plane）等。点对点方法通常计算两个点云之间的欧几里德距离误差，而点对平面方法则利用法向量信息来计算点到平面的距离误差，可以提高匹配的精度。
PL-ICP（Point-to-Line Iterative Closest Point）算法是一种改进的ICP算法，主要用于点云数据的配准和匹配。与传统的ICP算法相比，PL-ICP算法采用了点到直线的距离度量来计算点云之间的距离误差，从而提高了匹配的精度。
PL-ICP算法的基本流程如下：
1、初始化：将两个点云对齐，选择一个点云作为参考点云，另一个点云作为待匹配点云。
2、特征提取：从参考点云和待匹配点云中提取特征点或特征描述符，用于后续的点云匹配。
3、匹配：将待匹配点云中的每个点与参考点云中的最近邻点进行匹配，并计算点到直线的距离误差。
4、迭代优化：通过最小化点到直线的距离误差来优化点云配准，更新当前的位姿估计值，并重复执行步骤3和步骤4，直到收敛为止。
5、终止条件：当距离误差小于预设阈值或达到最大迭代次数时，停止迭代。PL-ICP算法的优点是可以更好地处理点云中的噪声和局部形变，从而提高了匹配的精度和鲁棒性。同时，PL-ICP算法也具有较高的计算效率，可以在实时系统中应用。PL-ICP算法被广泛应用于机器人导航、自主驾驶和三维重建等领域。
这里我们使用Lakibeam 1激光雷达实现一个基于PL-ICP算法的纯激光里程计。具体代码如下。
#include "lesson3/plicp_odometry.h"
#include "tf2_geometry_msgs/tf2_geometry_msgs.h"
ScanMatchPLICP::ScanMatchPLICP() : private_node_("~"), tf_listener_(tfBuffer_)
{
ROS_INFO_STREAM("\033[1;32m----> PLICP odometry started.\033[0m");
laser_scan_subscriber_ = node_handle_.subscribe(
"scan", 1, &ScanMatchPLICP::ScanCallback, this);
odom_publisher_ = node_handle_.advertise<nav_msgs::Odometry>("odom", 50);
// 参数初始化
InitParams();
scan_count_ = 0;
// 第一帧雷达还未到来
initialized_ = false;
base_in_odom_.setIdentity();
base_in_odom_keyframe_.setIdentity();
input_.laser[0] = 0.0;
input_.laser[1] = 0.0;
input_.laser[2] = 0.0;
// Initialize output_ vectors as Null for error-checking
output_.cov_x_m = 0;
output_.dx_dy1_m = 0;
output_.dx_dy2_m = 0;
}
ScanMatchPLICP::~ScanMatchPLICP()
{
}
/*
* ros与csm的参数初始化
*/
void ScanMatchPLICP::InitParams()
{
private_node_.param<std::string>("odom_frame", odom_frame_, "odom");
private_node_.param<std::string>("base_frame", base_frame_, "base_link");
// **** keyframe params: when to generate the keyframe scan
// if either is set to 0, reduces to frame-to-frame matching
private_node_.param<double>("kf_dist_linear", kf_dist_linear_, 0.1);
private_node_.param<double>("kf_dist_angular", kf_dist_angular_, 5.0 * (M_PI / 180.0));
kf_dist_linear_sq_ = kf_dist_linear_ * kf_dist_linear_;
private_node_.param<int>("kf_scan_count", kf_scan_count_, 10);
// **** CSM 的参数 - comments copied from algos.h (by Andrea Censi)
// Maximum angular displacement between scans
if (!private_node_.getParam("max_angular_correction_deg", input_.max_angular_correction_deg))
input_.max_angular_correction_deg = 45.0;
// Maximum translation between scans (m)
if (!private_node_.getParam("max_linear_correction", input_.max_linear_correction))
input_.max_linear_correction = 1.0;
// Maximum ICP cycle iterations
if (!private_node_.getParam("max_iterations", input_.max_iterations))
input_.max_iterations = 10;
// A threshold for stopping (m)
if (!private_node_.getParam("epsilon_xy", input_.epsilon_xy))
input_.epsilon_xy = 0.000001;
// A threshold for stopping (rad)
if (!private_node_.getParam("epsilon_theta", input_.epsilon_theta))
input_.epsilon_theta = 0.000001;
// Maximum distance for a correspondence to be valid
if (!private_node_.getParam("max_correspondence_dist", input_.max_correspondence_dist))
input_.max_correspondence_dist = 1.0;
// Noise in the scan (m)
if (!private_node_.getParam("sigma", input_.sigma))
input_.sigma = 0.010;
// Use smart tricks for finding correspondences.
if (!private_node_.getParam("use_corr_tricks", input_.use_corr_tricks))
input_.use_corr_tricks = 1;
// Restart: Restart if error is over threshold
if (!private_node_.getParam("restart", input_.restart))
input_.restart = 0;
// Restart: Threshold for restarting
if (!private_node_.getParam("restart_threshold_mean_error", input_.restart_threshold_mean_error))
input_.restart_threshold_mean_error = 0.01;
// Restart: displacement for restarting. (m)
if (!private_node_.getParam("restart_dt", input_.restart_dt))
input_.restart_dt = 1.0;
// Restart: displacement for restarting. (rad)
if (!private_node_.getParam("restart_dtheta", input_.restart_dtheta))
input_.restart_dtheta = 0.1;
// Max distance for staying in the same clustering
if (!private_node_.getParam("clustering_threshold", input_.clustering_threshold))
input_.clustering_threshold = 0.25;
// Number of neighbour rays used to estimate the orientation
if (!private_node_.getParam("orientation_neighbourhood", input_.orientation_neighbourhood))
input_.orientation_neighbourhood = 20;
// If 0, it's vanilla ICP
if (!private_node_.getParam("use_point_to_line_distance", input_.use_point_to_line_distance))
input_.use_point_to_line_distance = 1;
// Discard correspondences based on the angles
if (!private_node_.getParam("do_alpha_test", input_.do_alpha_test))
input_.do_alpha_test = 0;
// Discard correspondences based on the angles - threshold angle, in degrees
if (!private_node_.getParam("do_alpha_test_thresholdDeg", input_.do_alpha_test_thresholdDeg))
input_.do_alpha_test_thresholdDeg = 20.0;
// Percentage of correspondences to consider: if 0.9,
// always discard the top 10% of correspondences with more error
if (!private_node_.getParam("outliers_maxPerc", input_.outliers_maxPerc))
input_.outliers_maxPerc = 0.90;
// Parameters describing a simple adaptive algorithm for discarding.
// 1) Order the errors.
// 2) Choose the percentile according to outliers_adaptive_order.
// (if it is 0.7, get the 70% percentile)
// 3) Define an adaptive threshold multiplying outliers_adaptive_mult
// with the value of the error at the chosen percentile.
// 4) Discard correspondences over the threshold.
// This is useful to be conservative; yet remove the biggest errors.
if (!private_node_.getParam("outliers_adaptive_order", input_.outliers_adaptive_order))
input_.outliers_adaptive_order = 0.7;
if (!private_node_.getParam("outliers_adaptive_mult", input_.outliers_adaptive_mult))
input_.outliers_adaptive_mult = 2.0;
// If you already have a guess of the solution, you can compute the polar angle
// of the points of one scan in the new position. If the polar angle is not a monotone
// function of the readings index, it means that the surface is not visible in the
// next position. If it is not visible, then we don't use it for matching.
if (!private_node_.getParam("do_visibility_test", input_.do_visibility_test))
input_.do_visibility_test = 0;
// no two points in laser_sens can have the same corr.
if (!private_node_.getParam("outliers_remove_doubles", input_.outliers_remove_doubles))
input_.outliers_remove_doubles = 1;
// If 1, computes the covariance of ICP using the method http://purl.org/censi/2006/icpcov
if (!private_node_.getParam("do_compute_covariance", input_.do_compute_covariance))
input_.do_compute_covariance = 0;
// Checks that find_correspondences_tricks gives the right answer
if (!private_node_.getParam("debug_verify_tricks", input_.debug_verify_tricks))
input_.debug_verify_tricks = 0;
// If 1, the field 'true_alpha' (or 'alpha') in the first scan is used to compute the
// incidence beta, and the factor (1/cos^2(beta)) used to weight the correspondence.");
if (!private_node_.getParam("use_ml_weights", input_.use_ml_weights))
input_.use_ml_weights = 0;
// If 1, the field 'readings_sigma' in the second scan is used to weight the
// correspondence by 1/sigma^2
if (!private_node_.getParam("use_sigma_weights", input_.use_sigma_weights))
input_.use_sigma_weights = 0;
}
/*
* 回调函数 进行数据处理
*/
void ScanMatchPLICP::ScanCallback(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
// 如果是第一帧数据，首先进行初始化，先缓存一下cos与sin值
// 将 prev_ldp_scan_,last_icp_time_ 初始化
current_time_ = scan_msg->header.stamp;
if (!initialized_)
{
// caches the sin and cos of all angles
CreateCache(scan_msg);
// 获取机器人坐标系与雷达坐标系间的坐标变换
if (!GetBaseToLaserTf(scan_msg->header.frame_id))
{
ROS_WARN("Skipping scan");
return;
}
LaserScanToLDP(scan_msg, prev_ldp_scan_);
last_icp_time_ = current_time_;
initialized_ = true;
return;
}
// step1 进行数据类型转换
start_time_ = std::chrono::steady_clock::now();
LDP curr_ldp_scan;
LaserScanToLDP(scan_msg, curr_ldp_scan);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
// std::cout << "\n转换数据格式用时: " << time_used_.count() << " 秒。" << std::endl;
// step2 使用PLICP计算雷达前后两帧间的坐标变换
start_time_ = std::chrono::steady_clock::now();
ScanMatchWithPLICP(curr_ldp_scan, current_time_);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
// std::cout << "整体函数处理用时: " << time_used_.count() << " 秒。" << std::endl;
}
/**
* 雷达数据间的角度是固定的，因此可以将对应角度的cos与sin值缓存下来，不用每次都计算
*/
void ScanMatchPLICP::CreateCache(const sensor_msgs::LaserScan::ConstPtr &scan_msg)
{
a_cos_.clear();
a_sin_.clear();
double angle;
for (unsigned int i = 0; i < scan_msg->ranges.size(); i++)
{
angle = scan_msg->angle_min + i * scan_msg->angle_increment;
a_cos_.push_back(cos(angle));
a_sin_.push_back(sin(angle));
}
input_.min_reading = scan_msg->range_min;
input_.max_reading = scan_msg->range_max;
}
/**
* 获取机器人坐标系与雷达坐标系间的坐标变换
*/
bool ScanMatchPLICP::GetBaseToLaserTf(const std::string &frame_id)
{
ros::Time t = ros::Time::now();
geometry_msgs::TransformStamped transformStamped;
// 获取tf并不是瞬间就能获取到的，要给1秒的缓冲时间让其找到tf
try
{
transformStamped = tfBuffer_.lookupTransform(base_frame_, frame_id,
t, ros::Duration(1.0));
}
catch (tf2::TransformException &ex)
{
ROS_WARN("%s", ex.what());
ros::Duration(1.0).sleep();
return false;
}
// 将获取的tf存到base_to_laser_中
tf2::fromMsg(transformStamped.transform, base_to_laser_);
laser_to_base_ = base_to_laser_.inverse();
return true;
}
/**
* 将雷达的数据格式转成 csm 需要的格式
*/
void ScanMatchPLICP::LaserScanToLDP(const sensor_msgs::LaserScan::ConstPtr &scan_msg, LDP &ldp)
{
unsigned int n = scan_msg->ranges.size();
ldp = ld_alloc_new(n);
for (unsigned int i = 0; i < n; i++)
{
// calculate position in laser frame
double r = scan_msg->ranges[i];
if (r > scan_msg->range_min && r < scan_msg->range_max)
{
// fill in laser scan data
ldp->valid[i] = 1;
ldp->readings[i] = r;
}
else
{
ldp->valid[i] = 0;
ldp->readings[i] = -1; // for invalid range
}
ldp->theta[i] = scan_msg->angle_min + i * scan_msg->angle_increment;
ldp->cluster[i] = -1;
}
ldp->min_theta = ldp->theta[0];
ldp->max_theta = ldp->theta[n - 1];
ldp->odometry[0] = 0.0;
ldp->odometry[1] = 0.0;
ldp->odometry[2] = 0.0;
ldp->true_pose[0] = 0.0;
ldp->true_pose[1] = 0.0;
ldp->true_pose[2] = 0.0;
}
/**
* 使用PLICP进行帧间位姿的计算
*/
void ScanMatchPLICP::ScanMatchWithPLICP(LDP &curr_ldp_scan, const ros::Time &time)
{
// CSM is used in the following way:
// The scans are always in the laser frame
// The reference scan (prevLDPcan_) has a pose of [0, 0, 0]
// The new scan (currLDPScan) has a pose equal to the movement
// of the laser in the laser frame since the last scan
// The computed correction is then propagated using the tf machinery
prev_ldp_scan_->odometry[0] = 0.0;
prev_ldp_scan_->odometry[1] = 0.0;
prev_ldp_scan_->odometry[2] = 0.0;
prev_ldp_scan_->estimate[0] = 0.0;
prev_ldp_scan_->estimate[1] = 0.0;
prev_ldp_scan_->estimate[2] = 0.0;
prev_ldp_scan_->true_pose[0] = 0.0;
prev_ldp_scan_->true_pose[1] = 0.0;
prev_ldp_scan_->true_pose[2] = 0.0;
input_.laser_ref = prev_ldp_scan_;
input_.laser_sens = curr_ldp_scan;
// 匀速模型，速度乘以时间，得到预测的odom坐标系下的位姿变换
double dt = (time - last_icp_time_).toSec();
double pr_ch_x, pr_ch_y, pr_ch_a;
GetPrediction(pr_ch_x, pr_ch_y, pr_ch_a, dt);
tf2::Transform prediction_change;
CreateTfFromXYTheta(pr_ch_x, pr_ch_y, pr_ch_a, prediction_change);
// account for the change since the last kf, in the fixed frame
// 将odom坐标系下的坐标变换 转换成 base_in_odom_keyframe_坐标系下的坐标变换
prediction_change = prediction_change * (base_in_odom_ * base_in_odom_keyframe_.inverse());
// the predicted change of the laser's position, in the laser frame
// 将base_link坐标系下的坐标变换 转换成 雷达坐标系下的坐标变换
tf2::Transform prediction_change_lidar;
prediction_change_lidar = laser_to_base_ * base_in_odom_.inverse() * prediction_change * base_in_odom_ * base_to_laser_;
input_.first_guess[0] = prediction_change_lidar.getOrigin().getX();
input_.first_guess[1] = prediction_change_lidar.getOrigin().getY();
input_.first_guess[2] = tf2::getYaw(prediction_change_lidar.getRotation());
// If they are non-Null, free covariance gsl matrices to avoid leaking memory
if (output_.cov_x_m)
{
gsl_matrix_free(output_.cov_x_m);
output_.cov_x_m = 0;
}
if (output_.dx_dy1_m)
{
gsl_matrix_free(output_.dx_dy1_m);
output_.dx_dy1_m = 0;
}
if (output_.dx_dy2_m)
{
gsl_matrix_free(output_.dx_dy2_m);
output_.dx_dy2_m = 0;
}
start_time_ = std::chrono::steady_clock::now();
// 调用csm进行plicp计算
sm_icp(&input_, &output_);
end_time_ = std::chrono::steady_clock::now();
time_used_ = std::chrono::duration_cast<std::chrono::duration<double>>(end_time_ - start_time_);
// std::cout << "PLICP计算用时: " << time_used_.count() << " 秒。" << std::endl;
tf2::Transform corr_ch;
if (output_.valid)
{
// 雷达坐标系下的坐标变换
tf2::Transform corr_ch_l;
CreateTfFromXYTheta(output_.x[0], output_.x[1], output_.x[2], corr_ch_l);
// 将雷达坐标系下的坐标变换 转换成 base_link坐标系下的坐标变换
corr_ch = base_to_laser_ * corr_ch_l * laser_to_base_;
// 更新 base_link 在 odom 坐标系下 的坐标
base_in_odom_ = base_in_odom_keyframe_ * corr_ch;
latest_velocity_.linear.x = corr_ch.getOrigin().getX() / dt;
latest_velocity_.angular.z = tf2::getYaw(corr_ch.getRotation()) / dt;
}
else
{
ROS_WARN("not Converged");
}
// 发布tf与odom话题
PublishTFAndOdometry();
// 检查是否需要更新关键帧坐标
if (NewKeyframeNeeded(corr_ch))
{
// 更新关键帧坐标
ld_free(prev_ldp_scan_);
prev_ldp_scan_ = curr_ldp_scan;
base_in_odom_keyframe_ = base_in_odom_;
}
else
{
ld_free(curr_ldp_scan);
}
last_icp_time_ = time;
}
/**
* 推测从上次icp的时间到当前时刻间的坐标变换
* 使用匀速模型，根据当前的速度，乘以时间，得到推测出来的位移
*/
void ScanMatchPLICP::GetPrediction(double &prediction_change_x,
double &prediction_change_y,
double &prediction_change_angle,
double dt)
{
// 速度小于 1e-6 , 则认为是静止的
prediction_change_x = latest_velocity_.linear.x < 1e-6 ? 0.0 : dt * latest_velocity_.linear.x;
prediction_change_y = latest_velocity_.linear.y < 1e-6 ? 0.0 : dt * latest_velocity_.linear.y;
prediction_change_angle = latest_velocity_.linear.z < 1e-6 ? 0.0 : dt * latest_velocity_.linear.z;
if (prediction_change_angle >= M_PI)
prediction_change_angle -= 2.0 * M_PI;
else if (prediction_change_angle < -M_PI)
prediction_change_angle += 2.0 * M_PI;
}
/**
* 从x,y,theta创建tf
*/
void ScanMatchPLICP::CreateTfFromXYTheta(double x, double y, double theta, tf2::Transform &t)
{
t.setOrigin(tf2::Vector3(x, y, 0.0));
tf2::Quaternion q;
q.setRPY(0.0, 0.0, theta);
t.setRotation(q);
}
/**
* 发布tf与odom话题
*/
void ScanMatchPLICP::PublishTFAndOdometry()
{
geometry_msgs::TransformStamped tf_msg;
tf_msg.header.stamp = current_time_;
tf_msg.header.frame_id = odom_frame_;
tf_msg.child_frame_id = base_frame_;
tf_msg.transform = tf2::toMsg(base_in_odom_);
// 发布 odom 到 base_link 的 tf
tf_broadcaster_.sendTransform(tf_msg);
nav_msgs::Odometry odom_msg;
odom_msg.header.stamp = current_time_;
odom_msg.header.frame_id = odom_frame_;
odom_msg.child_frame_id = base_frame_;
tf2::toMsg(base_in_odom_, odom_msg.pose.pose);
odom_msg.twist.twist = latest_velocity_;
// 发布 odomemtry 话题
odom_publisher_.publish(odom_msg);
}
/**
* 如果平移大于阈值，角度大于阈值，则创新新的关键帧
* @return 需要创建关键帧返回true, 否则返回false
*/
bool ScanMatchPLICP::NewKeyframeNeeded(const tf2::Transform &d)
{
scan_count_++;
if (fabs(tf2::getYaw(d.getRotation())) > kf_dist_angular_)
return true;
if (scan_count_ == kf_scan_count_)
{
scan_count_ = 0;
return true;
}
double x = d.getOrigin().getX();
double y = d.getOrigin().getY();
if (x * x + y * y > kf_dist_linear_sq_)
return true;
return false;
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "lesson3_scan_match_plicp_node"); // 节点的名字
ScanMatchPLICP scan_match_plicp;
ros::spin();
return 0;
}运行效果如图所示。
tf比较简单，laser和base_link之间是一个固定的位姿，这里直接发布了静态的tf；base_link和odom是有雷达里程计的数据提供了坐标关系，所以这是一个动态的tf，在获取到odom数据后一并计算发出。
再来看rqt_graph节点关系图部分，lesson3_plicp_odometry_node订阅了lakibeam_lidar的雷达数据和静态TF坐标系，odom数据在Rviz里面的显示通过tf发布。