买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：昆明理工大学

摘要：本发明公开一种激光雷达IMU车辆运动学约束紧耦合SLAM系统及算法，属于自动驾驶技术领域。本发明首先利用IMU、后轴轮速和前轮转角构建车辆运动学约束，通过将车辆运动约束的位移和姿态信息解耦处理，分别构建位移和姿态的约束，从而提高优化结果的准确性；其次根据特征点数量和转向角度，引入自适应系数，用于实时调节车辆运动学约束的权重。最后采用IMU和后轴轮速构建里程计，为紧耦合优化提供精准的初始值，有效避免陷入局部最优。相比单一激光雷达的方案，可提供更精确的定位和地图。

主权项：1.一种激光雷达IMU车辆运动学约束紧耦合的SLAM算法，其特征在于，包括如下步骤：1构建激光雷达IMU车辆运动学约束紧耦合SLAM系统；所述激光雷达IMU车辆运动学约束紧耦合SLAM系统包括里程计模块、车辆运动学模块、激光雷达模块和紧耦合优化模块；激光雷达模块以激光雷达作为传感器，以激光雷达的点云作为数据，以当前帧的雷达点云中的角点、平面点和当前帧之前建立的全局点云地图中保存的角点、平面点作为输入数据，采用点到线、点到面的方式构建点云残差，将点云残差作为紧耦合优化模块的一个数据输入；里程计模块以IMU的角速度和车辆的后轴轮速作为数据来源，构建里程计残差，将里程计残差作为紧耦合优化模块的一个数据输入；车辆运动学模块以IMU的角速度、车辆的后轴轮速和车辆的前轮转角作为数据来源，构建车辆运动学残差，将车辆运动学残差作为紧耦合优化模块一个数据输入；紧耦合优化模块以点云残差、里程计残差和车辆运动学残差作为输入数据，将三个残差转化为最小二乘，以里程计残差作为紧耦合优化的初始值，采用非线性优化方法求解该最小二乘；采用优化后的系统状态量将当前帧的点云注册到全局点云地图；2对IMU的角速度和后轴轮速计的速度极性预积分得到IMU坐标系下的位姿，然后构建里程计残差；具体包括：S2-1：以IMU的角速度和车辆的后轴轮速作为原始数据，同时考虑原始数据存在噪声，建立IMU的角速度的测量模型： 其中为t时刻IMU的原始角速度测量，ωx为X轴的角速度，ωy为Y轴的角速度，ωz为Z轴的角速度，T为矩阵中的转置运算，ωt为t时刻IMU的真实角速度，为t时刻IMU的测量零偏，nω为IMU的角速度噪声，服从零均值的高斯噪声；建立后轴轮速的测量模型: 为t时刻的后轴轮速测量值，vt为t时刻的后轴轮速真实值，nv为后轴轮速噪声，服从零均值的高斯噪声；S2-2：通过的分量ωz将分解到X、Y、Z轴，从而可得t时刻的速度向量Vt: 其中Δt为t到t+1时刻的时间间隔；S2-3：采用速度向量Vt和IMU的角速度进行里程计积分：旋转积分公式如下： 其中exp为指数映射，为t+1时刻的旋转矩阵，为t时刻的旋转矩阵；位姿积分公式如下： 其中为t+1时刻的位移，PtI为t时刻的位移；S2-4：构建系统状态k到k+1帧之间的里程计残差如下： 其中里程计残差存在数据时间戳对齐，k表示的系统状态的时刻，即帧率，表示四元数的乘法，*表示四元数的共轭，和分别表示对齐k+1、k时刻的里程计积分的位移，和分别表示对齐k+1、k时刻的里程计积分模型的姿态，同时为四元数形式，RLI表示IMU坐标系到雷达坐标系的旋转矩阵，qLI是RLI的四元数形式，为k时刻的系统状态量中的位移，为k+1时刻的系统状态量中的位移，为k时刻的系统状态量中的四元数，为k+1时刻的系统状态量中的四元数，bωk为k时刻的系统状态量中的IMU零偏，bωk+1为k+1时刻的系统状态量中的IMU零偏；3首先计算在车辆坐标系下的速度向量和角速度向量；其次对速度向量和角速度进行预积分，得到六自由度的位姿，对角速度向量进行预积分，得到三自由度的姿态；然后构建车辆运动学残差；4使用预处理和特征法构建点云残差约束；5将里程计残差、车辆运动学残差和点云残差联合构建紧耦合优化问题，并转化为最小二乘问题，以里程计残差作为紧耦合优化的初始值，采用非线性优化方法求解最小二乘问题。

全文数据：

权利要求：

百度查询： 昆明理工大学 一种激光雷达/IMU/车辆运动学约束紧耦合SLAM系统及算法