本发明专利技术公开一种激光雷达/IMU/车辆运动学约束紧耦合SLAM系统及算法，属于自动驾驶技术领域。本发明专利技术首先利用IMU、后轴轮速和前轮转角构建车辆运动学约束，通过将车辆运动约束的位移和姿态信息解耦处理，分别构建位移和姿态的约束，从而提高优化结果的准确性；其次根据特征点数量和转向角度，引入自适应系数，用于实时调节车辆运动学约束的权重。最后采用IMU和后轴轮速构建里程计，为紧耦合优化提供精准的初始值，有效避免陷入局部最优。相比单一激光雷达的方案，可提供更精确的定位和地图。
【技术实现步骤摘要】
本专利技术属于自动驾驶，具体涉及一种激光雷达/imu/车辆运动学约束紧耦合slam系统及算法。
技术介绍
1、自动驾驶车辆包含感知、决策、控制三个关键模块。其中定位是自动驾驶车辆的关键感知技术，是实现后续路径规划、自动导航、避障等功能的重要前提条件。为了更好地解决自动驾车辆的定位问题，目前广泛采用同时定位与地图构建(simultaneouslocalization and map-ping，slam)，为车辆提供定位信息和环境地图。slam主要以激光雷达或者相机为主，在估计位姿的同时，依托传感器丰富的环境感知信息建立环境地图。但采用单一传感器的slam方案，存在精度差，鲁棒性不强的问题，难以保证自动驾驶车辆在复杂多变环境下的定位要求。
技术实现思路
1、针对上述现有技术的缺点，本专利技术提出一种激光雷达/imu/车辆运动学约束紧耦合slam系统及算法。
2、一种激光雷达/imu/车辆运动学约束紧耦合slam系统，包括里程计模块、车辆运动学模块、激光雷达模块和紧耦合优化模块；
3、激光雷达模块以激光雷达作为传感器，以激光雷达的点云作为数据，以当前帧的雷达点云中的角点、平面点和当前帧之前建立的全局点云地图中保存的角点、平面点作为输入数据，采用点到线、点到面的方式构建点云残差，将点云残差作为紧耦合优化模块的一个数据输入；
4、里程计模块以imu的角速度和车辆的后轴轮速作为数据来源，构建里程计残差，将里程计残差作为紧耦合优化模块的一个数据输入；
<
p>5、车辆运动学模块以imu的角速度、车辆的后轴轮速和车辆的前轮转角作为数据来源，构建车辆运动学残差，将车辆运动学残差作为紧耦合优化模块一个数据输入；6、紧耦合优化模块以点云残差、里程计残差和车辆运动学残差作为输入数据，将三个残差转化为最小二乘，以里程计残差作为紧耦合优化的初始值，采用非线性优化方法求解该最小二乘；采用优化后的系统状态量将当前帧的点云注册到全局点云地图。
7、作为本专利技术的优选实施方式，所述激光雷达模块以激光雷达作为传感器，以激光雷达的点云作为数据，并实时处理每一帧点云数据；首先对点云进行预处理，去除无效的点；其次对预处理完成后的点云进行特征点提取，提取点云中的角点和平面点；然后进行点云残差构建，其中点云残差构建以当前帧的雷达点云中的角点、平面点和当前帧之前建立的全局点云地图中保存的角点、平面点作为输入数据，采用点到线、点到面的方式构建点云残差；最后将构建完成的点云残差作为紧耦合优化模块的一个数据输入。
8、作为本专利技术的优选实施方式，里程计模块以imu的角速度和车辆的后轴轮速作为数据来源；首先进行预积分，得到在imu坐标系下的位姿；然后构建里程计残差；最后将里程计残差作为紧耦合优化模块的一个数据输入。
9、作为本专利技术的优选实施方式，车辆运动学模块以imu的角速度、车辆的后轴轮速和车辆的前轮转角作为数据来源；首先利用车辆运动学模型以车辆的后轴轮速和前轮转角作为数据输入计算在车辆坐标系下的速度向量和角速度向量；其次对车辆运动学模型的速度向量和imu的角速度进行预积分，得到六自由度的位姿，单独对车辆运动学模型的角速度向量进行预积分，得到三自由度的姿态；然后采用预积分得到的六自由度位姿和三自由度的姿态构建车辆运动学残差；最后将车辆运动学残差作为紧耦合优化模块一个数据输入。
10、作为本专利技术的优选实施方式，紧耦合优化模块以点云残差、里程计残差和车辆运动学残差作为输入数据，将三个残差转化为最小二乘，以里程计残差作为紧耦合优化的初始值，采用非线性优化方法求解该最小二乘；采用优化后的系统状态量将当前帧的点云注册到全局点云地图。
11、基于上述系统，本专利技术还要求保护一种激光雷达/imu/车辆运动学约束紧耦合的slam算法，包括如下步骤：
12、(1)构建所述激光雷达/imu/车辆运动学约束紧耦合slam系统；
13、(2)对imu的角速度和后轴轮速计的速度极性预积分得到imu坐标系下的位姿，然后构建里程计残差；
14、(3)首先计算在车辆坐标系下的速度向量和角速度向量；其次对速度向量和角速度进行预积分，得到六自由度的位姿，对角速度向量进行预积分，得到三自由度的姿态；然后构建车辆运动学残差；
15、(4)使用预处理和特征法构建点云残差约束；
16、(5)将里程计残差、车辆运动学残差和点云残差联合构建紧耦合优化问题，并转化为最小二乘问题，以里程计残差作为紧耦合优化的初始值，采用非线性优化方法求解最小二乘问题。
17、作为本专利技术的优选实施方式，所述里程计模块以100hz的频率运行，并构建里程计残差。车辆运动学模块以100hz的频率运行，并构建车辆运动学约束。激光雷达模块以5hz的频率运行，构建点云残差。系统状态量的求解频率为5hz，对系统状态量进行优化，每一次系统状态量求解完成后，采用求解完成后的系统状态量将该帧对应的原始点云数据注册到全局点云地图中。
18、作为本专利技术的优选实施方式，所述系统状态量定义如下:
19、
20、其中xk为k时刻的系统状态量，每一时刻的系统状态量包含三个数据，为激光雷达坐标系在世界坐标下的平移向量，为激光雷达坐标系在世界坐标下四元数，bω表示imu角速度的零偏。
21、作为本专利技术的优选实施方式，所述步骤(2)中，里程计残差构建具体包括：
22、s2-1：以imu的角速度和车辆的后轴轮速作为原始数据，同时考虑原始数据存在噪声，建立imu的角速度的测量模型：
23、
24、其中为t时刻imu的原始角速度测量，ωx为x轴的角速度，ωy为y轴角速度，ωz为z轴角速度，t为转置运算，ωt为t时刻imu的真实角速度，为t时刻imu的测量零偏，nω为imu的角速度噪声，服从零均值的高斯噪声；
25、建立后轴轮速的测量模型:
26、
27、为t时刻的后轴轮速测量值，vt为t时刻的后轴轮速真实值，nv为后轴轮速噪声，服从零均值的高斯噪声；
28、s2-2：通过的分量ωz将分解到x、y、z轴，从而可得t时刻的速度向量vt:
29、
30、其中δt为t到t+1时刻的时间间隔；
31、s2-3：采用速度向量vt和imu的角速度进行里程计积分：
32、旋转积分公式如下：
33、
34、其中exp()为指数映射，为t+1时刻的旋转矩阵，为t时刻的旋转矩阵；
35、位姿积分公式如下：
36、
37、其中为t+1时刻的位移，pti为t时刻的位移；
38、s2-4：构建系统状态k到k+1帧之间的里程计残差如下：
39、
40、上述公式存在数据时间戳对齐，k表示的系统状态的时刻，即帧率，是一个低频率的时间，而里程计数据的频率很高用t表示，因此本文档来自技高网
...
【技术保护点】
1.一种激光雷达/IMU/车辆运动学约束紧耦合SLAM系统，其特征在于，包括里程计模块、车辆运动学模块、激光雷达模块和紧耦合优化模块；
2.一种激光雷达/IMU/车辆运动学约束紧耦合的SLAM算法，其特征在于，包括如下步骤：
3.如权利要求2所述激光雷达/IMU/车辆运动学约束紧耦合的SLAM算法，其特征在于，所述步骤(2)中，里程计残差构建具体包括：
4.如权利要求2所述激光雷达/IMU/车辆运动学约束紧耦合的SLAM算法，其特征在于，所述步骤(3)中，车辆运动学残差构建具体包括：
5.如权利要求2所述激光雷达/IMU/车辆运动学约束紧耦合SLAM算法，其特征在于，所述步骤(4)中，构建点云残差，具体包括：
6.如权利要求2所述激光雷达/IMU/车辆运动学约束紧耦合SLAM算法，其特征在于，所述步骤(5)中，最小二乘问题的代价函数为：
7.如权利要求6所述激光雷达/IMU/车辆运动学约束紧耦合SLAM算法，其特征在于，所述步骤(5)中，采用非线性优化LM方法进行求解最小二乘问题。
【技术特征摘要】
1.一种激光雷达/imu/车辆运动学约束紧耦合slam系统，其特征在于，包括里程计模块、车辆运动学模块、激光雷达模块和紧耦合优化模块；
2.一种激光雷达/imu/车辆运动学约束紧耦合的slam算法，其特征在于，包括如下步骤：
3.如权利要求2所述激光雷达/imu/车辆运动学约束紧耦合的slam算法，其特征在于，所述步骤(2)中，里程计残差构建具体包括：
4.如权利要求2所述激光雷达/imu/车辆运动学约束紧耦合的slam算法，其特征在于，所述步...
【专利技术属性】
技术研发人员：杨秀建，颜绍祥，雷基林，吴相稷，白永瑞，高晋，
申请(专利权)人：昆明理工大学，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。