CN112083726A - 一种面向园区自动驾驶的双滤波器融合定位系统 - Google Patents
一种面向园区自动驾驶的双滤波器融合定位系统 Download PDFInfo
- Publication number
- CN112083726A CN112083726A CN202010923478.7A CN202010923478A CN112083726A CN 112083726 A CN112083726 A CN 112083726A CN 202010923478 A CN202010923478 A CN 202010923478A CN 112083726 A CN112083726 A CN 112083726A
- Authority
- CN
- China
- Prior art keywords
- vehicle
- module
- pose
- representing
- particle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 230000004927 fusion Effects 0.000 title claims abstract description 23
- 239000002245 particle Substances 0.000 claims abstract description 64
- 238000001914 filtration Methods 0.000 claims abstract description 44
- 238000000034 method Methods 0.000 claims abstract description 29
- 238000005516 engineering process Methods 0.000 claims abstract description 13
- 239000011159 matrix material Substances 0.000 claims description 43
- 230000033001 locomotion Effects 0.000 claims description 26
- 238000004422 calculation algorithm Methods 0.000 claims description 21
- 238000005259 measurement Methods 0.000 claims description 17
- 239000000126 substance Substances 0.000 claims description 16
- 238000012952 Resampling Methods 0.000 claims description 15
- 238000004364 calculation method Methods 0.000 claims description 14
- 230000010354 integration Effects 0.000 claims description 11
- 230000009466 transformation Effects 0.000 claims description 11
- 238000005457 optimization Methods 0.000 claims description 10
- 238000000605 extraction Methods 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000015556 catabolic process Effects 0.000 claims description 6
- 230000008859 change Effects 0.000 claims description 6
- 238000006731 degradation reaction Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000012937 correction Methods 0.000 claims description 5
- 238000009795 derivation Methods 0.000 claims description 5
- 230000005540 biological transmission Effects 0.000 claims description 4
- 238000011478 gradient descent method Methods 0.000 claims description 4
- 238000010606 normalization Methods 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 230000006872 improvement Effects 0.000 description 6
- 230000008878 coupling Effects 0.000 description 4
- 238000010168 coupling process Methods 0.000 description 4
- 238000005859 coupling reaction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 238000013459 approach Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000009827 uniform distribution Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Traffic Control Systems (AREA)
- Navigation (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明公开了一种面向园区自动驾驶的双滤波器融合定位方法和系统，其目的是：设计一种面向园区自动驾驶的双滤波器融合定位方法和系统，通过扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，以获取车辆预测位姿。利用粒子滤波技术融合车辆预测位姿和激光里程计，以期提高园区环境下自动驾驶车辆定位精度与鲁棒性。
Description
技术领域
本发明涉及自动驾驶领域，特别是关于一种面向园区自动驾驶的双滤波器融合定位系统。
背景技术
近年来，自动驾驶技术研究已成为热点和趋势。自动驾驶企业正尝试在园区场景中推进自动驾驶技术的商业化落地，准确、鲁棒的实时定位是自动驾驶车辆安全行驶基础，确定车辆在全局坐标系中的位置信息为规划和控制模块提供依据。
自动驾驶车辆常用定位技术分为两类：1)基于全球卫星导航系统定位。2)基于自主式传感器定位。全球卫星导航系统定位精度较高，但容易受到使用环境的遮蔽影响而失效。基于自主式传感器的定位主要利用激光雷达、惯性测量单元以及里程计。利用里程计或IMU传感器进行航迹推算是一种低成本、自主的定位方式，抗干扰能力强且短时间内车辆定位精度较高，但航迹推算定位算法的误差会随时间不断累积，不适合长时间单独定位。
自动驾驶面临的难题之一在于单一传感器难以满足自动驾驶车辆定位精度要求。目前已有基于滤波器的融合定位相关技术与专利。专利文件CN111207743A公布一种基于编码器与惯性设备紧耦合实现厘米级精确定位的方法，采用紧耦合的方式融合编码器和惯导设备，实现车辆定位。但是两种定位均属于航迹推算定位算法，此类算法的误差会随时间不断累计，不适合长时间定位。因此，这种融合定位方式也会存在相同问题。此外。采用紧耦合方式会很大程度上增加算法复杂度，相比于松耦合来说，实际效果能否提升还值得商榷。专利文件CN109737957A公布一种采用级联FIR滤波的INS/LiDAR组合导航方法及系统，将INS与激光雷达分别测量的位置信息之差作为观测量输入FIR滤波算法，预估INS计算误差，获取最优位置信息。但是这种方法缺少车辆的平面运动约束。专利CN111240331A公布基于激光雷达和里程计SLAM的智能小车定位导航方法及系统，通过编码器和IMU获取小车速度信息和位姿信息，形成里程计信息。通过激光雷达扫描小车的周围环境形成激光雷达坐标系中的环境点云位置信息。这种方法也没有考虑车辆的平面运动约束。此外，激光SLAM模块并没有采用效果更好的优化方式。自动驾驶车辆在园区内主要以平面运动为主。需要引入车辆平面运动约束条件，改善车辆位姿估计结果。因此，设计一种面向园区自动驾驶的双滤波器融合定位方法和系统，通过滤波技术融合多种传感器数据，以期提高园区场景中自动驾驶车辆定位精度和鲁棒性。
发明内容
针对现有技术存在的不足，本发明的目的在于提供一种面向园区自动驾驶的双滤波器融合定位方法和系统，通过扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，以获取车辆预测位姿。利用粒子滤波技术融合车辆预测位姿和激光里程计，以期提高园区环境下自动驾驶车辆定位精度与鲁棒性。
为实现上述目的，本发明提供了如下技术方案：一种面向园区自动驾驶的双滤波器融合定位系统，包括：
车辆运动学模型模块，在两帧激光点云数据之间，利用轮速传感器和方向盘转角传感器数据构建车辆运动学模型，根据车辆运动学模型预测车辆运动状态，该预测位姿为扩展卡尔曼滤波算法提供预测值；
IMU积分模块，该模块由陀螺仪和加速度计组成，测量角速度ωb和加速度ab，通过积分惯性测量值获取j时刻车辆预测位姿：
其中，gw表示世界坐标系下的重力加速度，Δt表示相邻激光帧间的时间间隔，q表示四元数，
表示i时刻惯性坐标系到世界坐标系的旋转，
和
表示只与IMU测量值有关的积分量，IMU积分量按下式计算：
其中，
表示t时刻惯性坐标系到i时刻惯性坐标系的旋转变化，
表示四元数乘法；
扩展卡尔曼滤波模块，该模块融合IMU、轮速传感器以及方向盘转角传感器数据，采用扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，从而获取车辆预测位姿，其中通过车辆运动学模型模块提供预测值，而IMU积分模块提供测量值；
激光里程计模块，该模块包括：
点云畸变矫正模块，该点云畸变矫正模块基于匀速运动假设，利用扩展卡尔曼滤波模块提供的车辆预测位姿，通过线性插值方法矫正激光点云的运动畸变；点云特征提取模块，该点云特征提取模块利用去畸变点云的角度信息进行点云线束划分，每条激光扫描线上点云按下式计算曲率：
其中，sj表示点云的局部曲率，
表示点云测量值，A表示邻域点集合；
点云特征匹配模块，该点云特征匹配模块利用扩展卡尔曼滤波模块获取的车辆预测位姿作为点云匹配初值，采用相邻两帧点云特征匹配构建激光里程计残差；车辆位姿优化模块，该车辆位姿优化模块根据激光里程计残差构建代价函数，利用梯度下降法进行非线性优化；
粒子滤波模块，该模块包括粒子采样模块、权重更新模块、重采样模块和状态估计模块。
作为本发明的进一步改进，所述车辆运动学模块包含两类输入：轮速传感器直接提供车辆纵向速度v和方向盘转角传感器提供车辆横摆角速度ω计算所需的方向盘转角δs，其中车辆横摆角速度ω由纵向速度v、方向盘转角δs、转向器角传动比η以及轴距d共同决定，即：
作为本发明的进一步改进，所述车辆运动学模型模块预测车辆运动状态的具体步骤为：
步骤一，以上一时刻车辆模型坐标系作为参考坐标系，两时刻间车辆始终运行在平面上，根据车辆运动学模型预测两时刻间车辆位姿变化量：
其中，
表示相对于上一时刻的横摆角增量，δt表示相邻底层数据的时间间隔，下标v表示车辆模型坐标系，
表示相邻时刻间车辆始终处于平面运动状态，即：以上一时刻车辆模型坐标系作为参考坐标系，Z轴方向无运动；
步骤二，基于上一时刻已优化的车辆位姿
计算j时刻自动驾驶车辆在世界坐标系下的位姿
其中，
Rbv表示车辆模型坐标系到惯性坐标系的旋转变换，平面上的旋转矩阵
计算公式为：
其中，
表示相邻激光帧间横摆角变化量。
作为本发明的进一步改进，所述扩展卡尔曼滤波模块内的扩展卡尔曼滤波算法分为两个阶段：预测和更新，其中：
预测阶段：根据上一时刻优化状态量和系统协方差矩阵预测当前时刻车辆位姿和协方差矩阵，即：
其中，Σ表示系统协方差矩阵，Mj表示系统噪声协方差矩阵，g(·)表示状态方程，G表示状态转移矩阵，其推导公式如下：
其中，右乘BCH近似雅可比矩阵的逆
按下式计算：
其中，θa表示旋转矩阵R对应旋转向量，θ表示旋转角度，a表示旋转轴，∧为反对称符号，∨为解反对称符号，
更新阶段：计算卡尔曼增益并根据测量值和预测值更新车辆预测位姿和协方差矩阵，即：
其中，Kj表示卡尔曼增益，Hj表示系统观测矩阵，该矩阵为6×6单位矩阵，Nj表示观测噪声协方差矩阵。
作为本发明的进一步改进，所述粒子采样模块通过扩展卡尔曼滤波获取的预测位姿更新粒子集合
即，每个粒子均根据扩展卡尔曼滤波模块预测的车辆相对位姿变换，更新粒子对应位姿。
作为本发明的进一步改进，所述权重更新模块根据j时刻粒子预测位姿
激光里程计提供的优化位姿
以及上一时刻对应粒子权值，计算j时刻粒子权值
并进行归一化处理。重要性权值
的计算公式如下：
其中，
表示取第n个粒子对应预测位姿时获得观测的概率。
作为本发明的进一步改进，所述重采样模块采用自适应重采样策略，在重采样期间，增加权值较高的粒子，删除权值较低的粒子，通过计算有效粒子数衡量粒子退化程度，即：
设定有效样本数M＝0.8N作为阈值，当Neff＜M时，说明粒子退化严重，需进行重采样操作，获取新粒子集
用于计算下一时刻车辆位姿。
作为本发明的进一步改进，所述状态估计模块选取若干数量权重较高的粒子，根据粒子对应的位姿信息和重要性权值，计算当前时刻自动驾驶车辆准确位姿，即：
该位姿信息将分别传递给车辆运动学模型模块、IMU积分模块以及激光里程计模块用于下一时刻车辆位姿预测及其优化。
本发明的有益效果，1)本发明提出一种面向园区自动驾驶的双滤波器融合定位方法和系统，实现激光雷达、IMU、轮速传感器以及方向盘转角传感器的数据融合。2)本发明通过扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，利用粒子滤波技术融合激光里程计和扩展卡尔曼滤波提供的车辆预测位姿，双滤波器融合定位方法提高封闭园区环境下自动驾驶车辆定位精度与鲁棒性。3)本发明通过邻域点信息计算点云局部曲率，采用双邻域策略提取角点和平面点特征，降低邻域尺寸对局部曲率计算的影响，提高特征提取的精度和鲁棒性。4)本发明通过扩展卡尔曼滤波获取的车辆预测位姿为点云运动畸变矫正提供依据，基于线性插值方法获取准确激光点云，利于数据关联的准确性。5)本发明通过扩展卡尔曼滤波获取的车辆预测位姿为激光里程计算法提供良好初值，避免点云匹配算法陷入局部极值，提高定位算法效率与准确性。6)本发明扩展原车传感器用途，实现成本节约、复杂度降低的同时提升定位系统可靠性。
附图说明
图1是发明面向园区自动驾驶的双滤波器融合定位算法架构图；
图2是本发明多传感器数据积分示意图；
图3是本发明车辆运动学模型示意图。
具体实施方式
下面结合附图对本发明进行详细描述。
如图1所示，面向园区自动驾驶的双滤波器融合定位算法架构图共包含五个模块：车辆运动学模型模块、IMU积分模块、扩展卡尔曼滤波模块、激光里程计模块和粒子滤波模块。
该定位系统的输入包含四种传感器：激光雷达、IMU、轮速传感器和方向盘转角传感器。利用扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，车辆运动学模型提供预测值，IMU积分提供测量值，从而获取车辆预测位姿。通过激光里程计获取车辆优化位姿并作为粒子滤波的测量值，利用粒子滤波融合扩展卡尔曼滤波提供的车辆预测位姿和激光里程计提供的车辆优化位姿，实现园区环境下自动驾驶车辆准确定位。在j时刻，双滤波器融合定位系统状态量定义如下：
其中，w表示世界坐标系，b表示惯性坐标系。
表示j时刻惯性坐标系到世界坐标系的平移变换。
表示j时刻惯性坐标系与世界坐标系间旋转矩阵对应的李代数。即：R＝exp(φ∧)。
1、车辆运动学模型模块
车辆运动学模型模块包含两类输入：1)轮速传感器直接提供车辆纵向速度v。2)方向盘转角传感器提供车辆横摆角速度ω计算所需的方向盘转角δs。
在两帧激光点云数据之间，利用轮速传感器和方向盘转角传感器数据构建车辆运动学模型，根据车辆运动学模型预测车辆运动状态。该预测位姿为扩展卡尔曼滤波算法提供预测值。
车辆横摆角速度ω由纵向速度v、方向盘转角δs、转向器角传动比η以及轴距d共同决定，即：
以上一时刻车辆模型坐标系作为参考坐标系，两时刻间车辆始终运行在平面上，根据车辆运动学模型预测两时刻间车辆位姿变化量：
其中，
表示相对于上一时刻的横摆角增量。δt表示相邻底层数据的时间间隔。下标v表示车辆模型坐标系。
表示相邻时刻间车辆始终处于平面运动状态，即：以上一时刻车辆模型坐标系作为参考坐标系，Z轴方向无运动。
基于上一时刻已优化的车辆位姿
计算j时刻自动驾驶车辆在世界坐标系下的位姿
其中，
Rbv表示车辆模型坐标系到惯性坐标系的旋转变换。平面上的旋转矩阵
计算公式为：
其中，
表示相邻激光帧间横摆角变化量。
2、IMU积分模块
IMU积分模块根据惯性测量值预测车辆状态。基于IMU积分的车辆状态预测为扩展卡尔曼滤波算法提供测量值。
IMU由陀螺仪和加速度计组成，测量角速度ωb和加速度ab。通过积分惯性测量值获取j时刻车辆预测位姿：
其中，gw表示世界坐标系下的重力加速度。Δt表示相邻激光帧间的时间间隔。q表示四元数，
表示i时刻惯性坐标系到世界坐标系的旋转。
和
表示只与IMU测量值有关的积分量。IMU积分量按下式计算：
其中，
表示t时刻惯性坐标系到i时刻惯性坐标系的旋转变化。
表示四元数乘法。
3、扩展卡尔曼滤波模块
该模块融合IMU、轮速传感器以及方向盘转角传感器数据。采用扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，从而获取车辆预测位姿。车辆运动学模型模块提供预测值，而IMU积分模块提供测量值。
扩展卡尔曼滤波算法分为两个阶段：预测和更新。
预测阶段：根据上一时刻优化状态量和系统协方差矩阵预测当前时刻车辆位姿和协方差矩阵，即：
其中，∑表示系统协方差矩阵，Mj表示系统噪声协方差矩阵。g(·)表示状态方程，G表示状态转移矩阵，其推导公式如下：
其中，右乘BCH近似雅可比矩阵的逆
按下式计算：
其中，θa表示旋转矩阵R对应旋转向量，θ表示旋转角度，a表示旋转轴。∧为反对称符号，V为解反对称符号。
更新阶段：通过IMU积分获取测量值，计算卡尔曼增益并根据测量值和预测值更新车辆预测位姿和协方差矩阵，即：
其中，Kj表示卡尔曼增益。Hj表示系统观测矩阵，该矩阵为6×6单位矩阵。Nj表示观测噪声协方差矩阵。
4、激光里程计模块
激光里程计模块包含四个部分：点云畸变矫正、点云特征提取、点云特征匹配和车辆位姿优化，激光里程计模块利用扩展卡尔曼滤波模块提供的车辆预测位姿，通过线性插值方法矫正激光点云的运动畸变。去畸变点云用于后续局部曲率计算和点云匹配。
(1)点云畸变矫正
为保证点云匹配准确性，需进行点云畸变矫正。基于匀速运动假设，利用扩展卡尔曼滤波模块提供的车辆预测位姿，通过线性插值方法矫正激光点云的运动畸变。去畸变点云用于后续局部曲率计算和点云匹配。
(2)点云特征提取
利用去畸变点云的角度信息进行点云线束划分。每条激光扫描线上点云按下式计算曲率：
其中，sj表示点云的局部曲率。
表示点云测量值。A表示邻域点集合。
排序点云曲率并通过局部曲率和邻域点分布提取两类特征点：1)角点：局部曲率大于阈值的同时邻域点未发生突变。2)平面点：局部曲率小于阈值的同时邻域点未发生突变。在双重邻域范围内计算局部曲率并依此提取同时满足阈值的角点和平面点特征。双邻域特征提取算法避免邻域尺寸对局部曲率计算的影响，提高特征提取的精度和鲁棒性。
为实现特征点云均匀分布，针对每条激光线束进行区域划分。每个区域提取满足阈值条件的角点和平面点，构成角点集合
和平面点集合
在选择特征点时希望避免选取以下几类点：1)可能被遮挡的点。2)已经选过点的周围点。3)位于激光线接近平行的平面点。
(3)点云特征匹配
利用扩展卡尔曼滤波模块获取的车辆预测位姿作为点云匹配初值，采用相邻两帧点云特征匹配构建激光里程计残差。当前时刻特征点云集合
和
根据相对位姿关系实现点云投影，即，当前时刻特征点云转换到上一时刻对应激光雷达坐标系。基于上一时刻提取的角点和平面点集合，构建对应kd树并寻找角点对应的特征线和平面点对应的特征面，即，1)点线ICP：使用kd树算法快速寻找每个角点的两个最近点，利用最近点构造直线并计算点到直线的垂足坐标
2)点面ICP：使用kd树算法快速寻找每个平面点的三个最近点，利用最近点构造平面并计算点到平面的垂足坐标
对于j时刻激光点云中的特征点，将其投影到i时刻激光雷达坐标系下的值为：
其中，下标l表示激光雷达坐标系。Tbl表示激光雷达坐标系到惯性坐标系的变换矩阵。
表示j时刻激光特征点在激光雷达坐标系下的三维坐标。
表示将j时刻激光特征点转换到i时刻激光雷达坐标系下的理论值。T表示两个坐标系间的变换矩阵，包含旋转矩阵和平移向量，即：
三维坐标形式为：
通过约束同一时刻激光雷达的测量值构建激光里程计残差。利用点到直线和点到平面的距离表示激光里程计残差rl：
其中，
表示将j时刻激光测量点转换到i时刻激光雷达坐标系下的投影点。
表示投影点在i时刻激光雷达坐标系中的对应点。
即：
其中，c表示通过帧与局部地图匹配确定的特征点云对应关系。
通过激光里程计残差对系统状态量求偏导构建雅克比矩阵J：
推导可得：
(4)车辆位姿优化
根据激光里程计残差构建代价函数，利用梯度下降法进行非线性优化。在梯度下降法执行过程中需要用到代价函数的雅克比矩阵，在前述内容中已进行相关推导和描述，此处不再赘述。通过计算系统代价函数最小值获取待优化系统状态量X的最大后验估计，即：
利用马氏距离表示激光里程计残差。协方差矩阵∑L由激光雷达准确性决定。通过激光里程计算法获取自动驾驶车辆优化位姿
并将其传递给粒子滤波模块用作测量值。
5、粒子滤波模块
粒子滤波模块包含四个部分：粒子采样、权重更新、重采样和状态估计。
(1)粒子采样
通过扩展卡尔曼滤波获取的预测位姿更新粒子集合
即，每个粒子均根据扩展卡尔曼滤波模块预测的车辆相对位姿变换，更新粒子对应位姿。
(2)权重更新
根据j时刻粒子预测位姿
激光里程计提供的优化位姿
以及上一时刻对应粒子权值，计算j时刻粒子权值
并进行归一化处理。重要性权值
的计算公式如下：
其中，
表示取第n个粒子对应预测位姿时获得观测的概率。
(3)重采样
采用自适应重采样策略。在重采样期间，增加权值较高的粒子，删除权值较低的粒子。通过计算有效粒子数衡量粒子退化程度，即：
设定有效样本数M＝0.8N作为阈值，当Neff<M时，说明粒子退化严重，需进行重采样操作，获取新粒子集
用于计算下一时刻车辆位姿。
(4)状态估计
选取若干数量权重较高的粒子，根据粒子对应的位姿信息和重要性权值，计算当前时刻自动驾驶车辆准确位姿，即：
该位姿信息将分别传递给车辆运动学模型模块、IMU积分模块以及激光里程计模块用于下一时刻车辆位姿预测及其优化。
图2为多传感器数据积分示意图。相对于激光雷达帧率，IMU、轮速传感器以及方向盘转角传感器的频率较高，因此需进行积分操作，以获取相邻激光帧之间的车辆运动状态预测。
利用相邻激光帧之间的多个惯性测量数据进行积分，利用中值法计算IMU积分量
和
即，利用相邻时刻测量值a,ω的平均值计算位姿变换：
其中，bg、ba分别表示陀螺仪和加速度计的偏置。ng、na分别表示陀螺仪和加速度计的测量噪声。
利用相邻激光帧之间的多个车辆底层数据进行积分，利用中值法计算车辆运动学模型积分量
和
即，根据相邻时刻测量值v,δs的平均值计算位姿变换：
图3为车辆运动学模型示意图。车辆运动学模型建立原则是保证模型简单的同时应尽可能反映车辆真实运动特性。过于严格的车辆运动学模型不利于保证实时性。基于车辆结构和运动特点，前轮转向四轮车简化为两轮自行车模型。以车辆后轴中心O为原点建立车辆模型坐标系，沿车辆前进方向为Xb轴方向，垂直Xb轴指向车体左侧为Yb轴方向。
表示相邻时刻间车辆横摆角增量，d表示轴距，δf为前轮转角。为保证安全，自动驾驶系统不会进入极限工况，因此质心侧偏角较小，可忽略不计。通常情况下，汽车后轮不可控，故可认为自行车模型中后轮转角控制输入δr＝0。利用方向盘转角δs和转向器角传动比η可获得前轮转角δf，即：δf＝δs/η。
车辆运动学模型需要两个输入：车辆纵向速度v和前轮转角δf。以前一时刻车辆模型坐标系作为参考坐标系，车辆运动学模型表达式如下：
其中，vx和vy分别表示自动驾驶车辆在参考坐标系下Xb轴方向速度和Yb轴方向速度。
上述实施例仅用于说明本发明，其中各部件构成和操作步骤等都是可以有所变化的，凡是在本发明技术方案的基础上进行等同变换和改进，均不应排除在本发明的保护范围之外。
Claims (8)
1.一种面向园区自动驾驶的双滤波器融合定位系统，其特征在于：包括：
车辆运动学模型模块，在两帧激光点云数据之间，利用轮速传感器和方向盘转角传感器数据构建车辆运动学模型，根据车辆运动学模型预测车辆运动状态，该预测位姿为扩展卡尔曼滤波算法提供预测值；
IMU积分模块，该模块由陀螺仪和加速度计组成，测量角速度ωb和加速度ab，通过积分惯性测量值获取j时刻车辆预测位姿：
其中，gw表示世界坐标系下的重力加速度，Δt表示相邻激光帧间的时间间隔，q表示四元数，
表示i时刻惯性坐标系到世界坐标系的旋转，
和
表示只与IMU测量值有关的积分量，IMU积分量按下式计算：
其中，
表示t时刻惯性坐标系到i时刻惯性坐标系的旋转变化，
表示四元数乘法；
扩展卡尔曼滤波模块，该模块融合IMU、轮速传感器以及方向盘转角传感器数据，采用扩展卡尔曼滤波技术融合车辆运动学模型和IMU积分，从而获取车辆预测位姿，其中通过车辆运动学模型模块提供预测值，而IMU积分模块提供测量值；
激光里程计模块，该模块包括：
点云畸变矫正模块，该点云畸变矫正模块基于匀速运动假设，利用扩展卡尔曼滤波模块提供的车辆预测位姿，通过线性插值方法矫正激光点云的运动畸变；
点云特征提取模块，该点云特征提取模块利用去畸变点云的角度信息进行点云线束划分，每条激光扫描线上点云按下式计算曲率：
其中，sj表示点云的局部曲率，
表示点云测量值，A表示邻域点集合；
点云特征匹配模块，该点云特征匹配模块利用扩展卡尔曼滤波模块获取的车辆预测位姿作为点云匹配初值，采用相邻两帧点云特征匹配构建激光里程计残差；
车辆位姿优化模块，该车辆位姿优化模块根据激光里程计残差构建代价函数，利用梯度下降法进行非线性优化；
粒子滤波模块，该模块包括粒子采样模块、权重更新模块、重采样模块和状态估计模块。
2.根据权利要求1所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述车辆运动学模块包含两类输入：轮速传感器直接提供车辆纵向速度v和方向盘转角传感器提供车辆横摆角速度ω计算所需的方向盘转角δs，其中车辆横摆角速度ω由纵向速度v、方向盘转角δs、转向器角传动比η以及轴距d共同决定，即：
3.根据权利要求2所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述车辆运动学模型模块预测车辆运动状态的具体步骤为：
步骤一，以上一时刻车辆模型坐标系作为参考坐标系，两时刻间车辆始终运行在平面上，根据车辆运动学模型预测两时刻间车辆位姿变化量：
其中，
表示相对于上一时刻的横摆角增量，δt表示相邻底层数据的时间间隔，下标v表示车辆模型坐标系，
表示相邻时刻间车辆始终处于平面运动状态，即：以上一时刻车辆模型坐标系作为参考坐标系，Z轴方向无运动；
步骤二，基于上一时刻已优化的车辆位姿
计算j时刻自动驾驶车辆在世界坐标系下的位姿
其中，
Rbv表示车辆模型坐标系到惯性坐标系的旋转变换，平面上的旋转矩阵
计算公式为：
其中，
表示相邻激光帧间横摆角变化量。
4.根据权利要求1或2或3所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述扩展卡尔曼滤波模块内的扩展卡尔曼滤波算法分为两个阶段：预测和更新，其中：
预测阶段：根据上一时刻优化状态量和系统协方差矩阵预测当前时刻车辆位姿和协方差矩阵，即：
其中，Σ表示系统协方差矩阵，Mj表示系统噪声协方差矩阵，g(·)表示状态方程，G表示状态转移矩阵，其推导公式如下：
其中，右乘BCH近似雅可比矩阵的逆
按下式计算：
其中，θa表示旋转矩阵R对应旋转向量，θ表示旋转角度，a表示旋转轴，∧为反对称符号，V为解反对称符号，
更新阶段：计算卡尔曼增益并根据测量值和预测值更新车辆预测位姿和协方差矩阵，即：
其中，Kj表示卡尔曼增益，Hj表示系统观测矩阵，该矩阵为6×6单位矩阵，Nj表示观测噪声协方差矩阵。
5.根据权利要求1或2或3所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述粒子采样模块通过扩展卡尔曼滤波获取的预测位姿更新粒子集合
即，每个粒子均根据扩展卡尔曼滤波模块预测的车辆相对位姿变换，更新粒子对应位姿。
6.根据权利要求5所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述权重更新模块根据j时刻粒子预测位姿
激光里程计提供的优化位姿
以及上一时刻对应粒子权值，计算j时刻粒子权值
并进行归一化处理。重要性权值
的计算公式如下：
其中，
表示取第n个粒子对应预测位姿时获得观测的概率。
7.根据权利要求6所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述重采样模块采用自适应重采样策略，在重采样期间，增加权值较高的粒子，删除权值较低的粒子，通过计算有效粒子数衡量粒子退化程度，即：
设定有效样本数M＝0.8N作为阈值，当Neff＜M时，说明粒子退化严重，需进行重采样操作，获取新粒子集
用于计算下一时刻车辆位姿。
8.根据权利要求7所述的面向园区自动驾驶的双滤波器融合定位系统，其特征在于：所述状态估计模块选取若干数量权重较高的粒子，根据粒子对应的位姿信息和重要性权值，计算当前时刻自动驾驶车辆准确位姿，即：
该位姿信息将分别传递给车辆运动学模型模块、IMU积分模块以及激光里程计模块用于下一时刻车辆位姿预测及其优化。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010923478.7A CN112083726B (zh) | 2020-09-04 | 2020-09-04 | 一种面向园区自动驾驶的双滤波器融合定位系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010923478.7A CN112083726B (zh) | 2020-09-04 | 2020-09-04 | 一种面向园区自动驾驶的双滤波器融合定位系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112083726A true CN112083726A (zh) | 2020-12-15 |
| CN112083726B CN112083726B (zh) | 2021-11-23 |
Family
ID=73733147
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010923478.7A Active CN112083726B (zh) | 2020-09-04 | 2020-09-04 | 一种面向园区自动驾驶的双滤波器融合定位系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112083726B (zh) |
Cited By (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112729321A (zh) * | 2020-12-28 | 2021-04-30 | 上海有个机器人有限公司 | 一种机器人的扫图方法、装置、存储介质和机器人 |
| CN112762928A (zh) * | 2020-12-23 | 2021-05-07 | 重庆邮电大学 | 含有激光slam的odom与dm地标组合移动机器人及导航方法 |
| CN113379910A (zh) * | 2021-06-09 | 2021-09-10 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及系统 |
| CN113936044A (zh) * | 2021-12-17 | 2022-01-14 | 武汉锐科光纤激光技术股份有限公司 | 激光设备运动状态的检测方法、装置、计算机设备及介质 |
| CN114279452A (zh) * | 2021-12-29 | 2022-04-05 | 北京斯年智驾科技有限公司 | 无人集卡挂车姿态检测方法及检测系统 |
| CN114415224A (zh) * | 2022-01-25 | 2022-04-29 | 东北大学 | 一种复杂受限环境中车辆融合定位系统及方法 |
| CN114608553A (zh) * | 2022-02-21 | 2022-06-10 | 南京航空航天大学 | 一种自动驾驶汽车定位系统及方法 |
| CN116222588A (zh) * | 2023-05-08 | 2023-06-06 | 睿羿科技(山东)有限公司 | 一种gps与车载里程计融合的定位方法 |
| CN116222544A (zh) * | 2023-05-09 | 2023-06-06 | 浙江大学湖州研究院 | 一种面向饲养场的投料车自动导航定位方法及装置 |
| CN117421700A (zh) * | 2023-12-19 | 2024-01-19 | 湖南仕博测试技术有限公司 | 一种用于自动驾驶中的传感器数据过滤及融合方法及装置 |
| CN117584989A (zh) * | 2023-11-23 | 2024-02-23 | 昆明理工大学 | 一种激光雷达/imu/车辆运动学约束紧耦合slam系统及算法 |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
| CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
| CN107246876A (zh) * | 2017-07-31 | 2017-10-13 | 中北智杰科技（北京）有限公司 | 一种无人驾驶汽车自主定位与地图构建的方法及系统 |
| CN109947097A (zh) * | 2019-03-06 | 2019-06-28 | 东南大学 | 一种基于视觉和激光融合的机器人定位方法及导航应用 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| CN110308729A (zh) * | 2019-07-18 | 2019-10-08 | 石家庄辰宙智能装备有限公司 | 基于视觉与imu或里程计的agv组合导航定位方法 |
| CN111258313A (zh) * | 2020-01-20 | 2020-06-09 | 深圳市普渡科技有限公司 | 多传感器融合slam系统及机器人 |
-
2020
- 2020-09-04 CN CN202010923478.7A patent/CN112083726B/zh active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106840179A (zh) * | 2017-03-07 | 2017-06-13 | 中国科学院合肥物质科学研究院 | 一种基于多传感器信息融合的智能车定位方法 |
| CN107193279A (zh) * | 2017-05-09 | 2017-09-22 | 复旦大学 | 基于单目视觉和imu信息的机器人定位与地图构建系统 |
| CN107246876A (zh) * | 2017-07-31 | 2017-10-13 | 中北智杰科技（北京）有限公司 | 一种无人驾驶汽车自主定位与地图构建的方法及系统 |
| CN109947097A (zh) * | 2019-03-06 | 2019-06-28 | 东南大学 | 一种基于视觉和激光融合的机器人定位方法及导航应用 |
| CN110262495A (zh) * | 2019-06-26 | 2019-09-20 | 山东大学 | 可实现移动机器人自主导航与精确定位的控制系统及方法 |
| CN110308729A (zh) * | 2019-07-18 | 2019-10-08 | 石家庄辰宙智能装备有限公司 | 基于视觉与imu或里程计的agv组合导航定位方法 |
| CN111258313A (zh) * | 2020-01-20 | 2020-06-09 | 深圳市普渡科技有限公司 | 多传感器融合slam系统及机器人 |
Non-Patent Citations (3)
| Title |
|---|
| HONG HE 等: "Simultaneous Location and Map Construction Based on RBPF-SLAM Algorithm", 《2018 CHINESE CONTROL AND DECISION CONFERENCE (CCDC)》 * |
| 刘丹: "智能车辆同时定位与建图关键技术研究", 《中国博士学位论文全文数据库（电子期刊）》 * |
| 孙宁 等: "基于多传感器信息融合的车辆目标识别方法", 《汽车工程》 * |
Cited By (19)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112762928A (zh) * | 2020-12-23 | 2021-05-07 | 重庆邮电大学 | 含有激光slam的odom与dm地标组合移动机器人及导航方法 |
| CN112762928B (zh) * | 2020-12-23 | 2022-07-15 | 重庆邮电大学 | 含有激光slam的odom与dm地标组合移动机器人及导航方法 |
| CN112729321A (zh) * | 2020-12-28 | 2021-04-30 | 上海有个机器人有限公司 | 一种机器人的扫图方法、装置、存储介质和机器人 |
| CN113379910A (zh) * | 2021-06-09 | 2021-09-10 | 山东大学 | 基于slam的移动机器人矿山场景重建方法及系统 |
| CN113936044B (zh) * | 2021-12-17 | 2022-03-18 | 武汉锐科光纤激光技术股份有限公司 | 激光设备运动状态的检测方法、装置、计算机设备及介质 |
| CN113936044A (zh) * | 2021-12-17 | 2022-01-14 | 武汉锐科光纤激光技术股份有限公司 | 激光设备运动状态的检测方法、装置、计算机设备及介质 |
| CN114279452B (zh) * | 2021-12-29 | 2024-04-12 | 北京斯年智驾科技有限公司 | 无人集卡挂车姿态检测方法及检测系统 |
| CN114279452A (zh) * | 2021-12-29 | 2022-04-05 | 北京斯年智驾科技有限公司 | 无人集卡挂车姿态检测方法及检测系统 |
| CN114415224A (zh) * | 2022-01-25 | 2022-04-29 | 东北大学 | 一种复杂受限环境中车辆融合定位系统及方法 |
| CN114415224B (zh) * | 2022-01-25 | 2023-11-24 | 东北大学 | 一种复杂受限环境中车辆融合定位系统及方法 |
| CN114608553A (zh) * | 2022-02-21 | 2022-06-10 | 南京航空航天大学 | 一种自动驾驶汽车定位系统及方法 |
| CN114608553B (zh) * | 2022-02-21 | 2024-05-24 | 南京航空航天大学 | 一种自动驾驶汽车定位系统及方法 |
| CN116222588A (zh) * | 2023-05-08 | 2023-06-06 | 睿羿科技(山东)有限公司 | 一种gps与车载里程计融合的定位方法 |
| CN116222588B (zh) * | 2023-05-08 | 2023-08-04 | 睿羿科技(山东)有限公司 | 一种gps与车载里程计融合的定位方法 |
| CN116222544B (zh) * | 2023-05-09 | 2023-08-04 | 浙江大学湖州研究院 | 一种面向饲养场的投料车自动导航定位方法及装置 |
| CN116222544A (zh) * | 2023-05-09 | 2023-06-06 | 浙江大学湖州研究院 | 一种面向饲养场的投料车自动导航定位方法及装置 |
| CN117584989A (zh) * | 2023-11-23 | 2024-02-23 | 昆明理工大学 | 一种激光雷达/imu/车辆运动学约束紧耦合slam系统及算法 |
| CN117421700B (zh) * | 2023-12-19 | 2024-03-19 | 湖南仕博测试技术有限公司 | 一种用于自动驾驶中的传感器数据过滤及融合方法及装置 |
| CN117421700A (zh) * | 2023-12-19 | 2024-01-19 | 湖南仕博测试技术有限公司 | 一种用于自动驾驶中的传感器数据过滤及融合方法及装置 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112083726B (zh) | 2021-11-23 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
| CN112083725B (zh) | 一种面向自动驾驶车辆的结构共用多传感器融合定位系统 | |
| CN113945206B (zh) | 一种基于多传感器融合的定位方法及装置 | |
| CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN113819914B (zh) | 一种地图构建方法及装置 | |
| CN111707272B (zh) | 一种地下车库自动驾驶激光定位系统 | |
| CN107246876B (zh) | 一种无人驾驶汽车自主定位与地图构建的方法及系统 | |
| CN112762957B (zh) | 一种基于多传感器融合的环境建模及路径规划方法 | |
| JP2020530569A (ja) | 車両センサの較正及び位置特定 | |
| CN111238467A (zh) | 一种仿生偏振光辅助的无人作战飞行器自主导航方法 | |
| CN113819905A (zh) | 一种基于多传感器融合的里程计方法及装置 | |
| CN113238251B (zh) | 一种基于车载激光雷达的目标级语义定位方法 | |
| CN112099378B (zh) | 考虑随机测量时滞的前车侧向运动状态实时估计方法 | |
| CN115993825A (zh) | 一种基于空地协同的无人车集群控制系统 | |
| CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
| CN111257853B (zh) | 一种基于imu预积分的自动驾驶系统激光雷达在线标定方法 | |
| CN113008229B (zh) | 一种基于低成本车载传感器的分布式自主组合导航方法 | |
| Parra-Tsunekawa et al. | A kalman-filtering-based approach for improving terrain mapping in off-road autonomous vehicles | |
| CN116774247A (zh) | 基于ekf的多源信息融合的slam前端策略 | |
| Emran et al. | A cascaded approach for quadrotor's attitude estimation | |
| CN114475581B (zh) | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 | |
| CN116625394A (zh) | 一种未知路况下机器人环境感知与路径寻优系统 | |
| CN114440881B (zh) | 一种融合多源传感器信息的无人车定位方法 | |
| CN115826583A (zh) | 一种基于点云地图的自动驾驶车辆编队方法 | |
| Chen et al. | An integrated GNSS/INS/DR positioning strategy considering nonholonomic constraints for intelligent vehicle |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |