1.一种激光雷达/IMU/车辆运动学约束紧耦合的SLAM算法,其特征在于,包括如下步骤:(1)构建激光雷达/IMU/车辆运动学约束紧耦合SLAM系统;所述激光雷达/IMU/车辆运动学约束紧耦合SLAM系统包括里程计模块、车辆运动学模块、激光雷达模块和紧耦合优化模块;
激光雷达模块以激光雷达作为传感器,以激光雷达的点云作为数据,以当前帧的雷达点云中的角点、平面点和当前帧之前建立的全局点云地图中保存的角点、平面点作为输入数据,采用点到线、点到面的方式构建点云残差,将点云残差作为紧耦合优化模块的一个数据输入;
里程计模块以IMU的角速度和车辆的后轴轮速作为数据来源,构建里程计残差,将里程计残差作为紧耦合优化模块的一个数据输入;
车辆运动学模块以IMU的角速度、车辆的后轴轮速和车辆的前轮转角作为数据来源,构建车辆运动学残差,将车辆运动学残差作为紧耦合优化模块一个数据输入;
紧耦合优化模块以点云残差、里程计残差和车辆运动学残差作为输入数据,将三个残差转化为最小二乘,以里程计残差作为紧耦合优化的初始值,采用非线性优化方法求解该最小二乘;采用优化后的系统状态量将当前帧的点云注册到全局点云地图;
(2)对IMU的角速度和后轴轮速计的速度极性预积分得到IMU坐标系下的位姿,然后构建里程计残差;
具体包括:
S2-1:以IMU的角速度
其中
建立后轴轮速
S2-2:通过
其中Δt为t到t+1时刻的时间间隔;
S2-3:采用速度向量V
旋转积分公式如下:
其中exp()为指数映射,
位姿积分公式如下:
其中
S2-4:构建系统状态k到k+1帧之间的里程计残差其中里程计残差存在数据时间戳对齐,k表示的系统状态的时刻,即帧率,(3)首先计算在车辆坐标系下的速度向量和角速度向量;其次对速度向量和角速度进行预积分,得到六自由度的位姿,对角速度向量进行预积分,得到三自由度的姿态;然后构建车辆运动学残差;
(4)使用预处理和特征法构建点云残差约束;
(5)将里程计残差、车辆运动学残差和点云残差联合构建紧耦合优化问题,并转化为最小二乘问题,以里程计残差作为紧耦合优化的初始值,采用非线性优化方法求解最小二乘问题。
2.如权利要求1所述激光雷达/IMU/车辆运动学约束紧耦合的SLAM算法,其特征在于,所述步骤(3)中,车辆运动学残差构建具体包括:S3-1:将前轮转角δ和后轴轮速分别经过车辆线性二自由度模型,得到三维角速度向量ωl
S3-2:根据IMU的角速度
其中R
六自由位姿约束的位移约束公式如下:其中P
S3-3:根据三维角速度向量ω
其中
S3-4:车辆运动约束残差
其中车辆运动约束残差公式存在数据时间戳对齐,k表示的系统状态的时刻,即帧率,其中δ
3.如权利要求1所述激光雷达/IMU/车辆运动学约束紧耦合SLAM算法,其特征在于,所述步骤(4)中,构建点云残差,具体包括:S4-1:点到线的距离残差e
其中φ表示当前帧点云中的线特征点集合,R、T是当前帧点云坐标系相对于地图点云坐标系的旋转矩阵和平移向量,对应着系统状态量中S4-2:点到面的距离残差e
其中ξ表示当前帧点云中的面特征点集合;R、T是当前帧点云坐标系相对于地图点云坐标系的旋转矩阵和平移向量,对应着系统状态量中S4-3:点云残差
4.如权利要求1所述激光雷达/IMU/车辆运动学约束紧耦合SLAM算法,其特征在于,所述步骤(5)中,最小二乘问题的代价函数为:
5.如权利要求1所述激光雷达/IMU/车辆运动学约束紧耦合SLAM算法,其特征在于,所述步骤(5)中,采用非线性优化LM方法进行求解最小二乘问题。