欢迎来到知嘟嘟! 联系电话:13336804447 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13336804447
知嘟嘟经纪人
收藏
专利号: 2023115710214
申请人: 昆明理工大学
专利类型:发明专利
专利状态:已下证
专利领域: 一般车辆
更新日期:2026-11-16
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

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方法进行求解最小二乘问题。