CN114966629A - 一种基于ekf算法框架的车体激光雷达外参标定方法 - Google Patents
一种基于ekf算法框架的车体激光雷达外参标定方法 Download PDFInfo
- Publication number
- CN114966629A CN114966629A CN202210516094.2A CN202210516094A CN114966629A CN 114966629 A CN114966629 A CN 114966629A CN 202210516094 A CN202210516094 A CN 202210516094A CN 114966629 A CN114966629 A CN 114966629A
- Authority
- CN
- China
- Prior art keywords
- laser radar
- vehicle body
- imu
- coordinate system
- vehicle
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 40
- 238000004364 calculation method Methods 0.000 claims abstract description 33
- 238000009434 installation Methods 0.000 claims abstract description 19
- 238000005259 measurement Methods 0.000 claims abstract description 8
- 238000006073 displacement reaction Methods 0.000 claims description 8
- 230000003068 static effect Effects 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 3
- 238000012938 design process Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 description 9
- 238000010586 diagram Methods 0.000 description 2
- 230000032683 aging Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000010354 integration Effects 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/48—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S17/00
- G01S7/497—Means for monitoring or calibrating
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
- G06F17/10—Complex mathematical operations
- G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Theoretical Computer Science (AREA)
- Data Mining & Analysis (AREA)
- Computational Mathematics (AREA)
- Pure & Applied Mathematics (AREA)
- Mathematical Analysis (AREA)
- Mathematical Optimization (AREA)
- Radar, Positioning & Navigation (AREA)
- Computing Systems (AREA)
- Algebra (AREA)
- Remote Sensing (AREA)
- Databases & Information Systems (AREA)
- Software Systems (AREA)
- General Engineering & Computer Science (AREA)
- Computer Networks & Wireless Communication (AREA)
- Optical Radar Systems And Details Thereof (AREA)
Abstract
本发明公开了一种基于EKF算法框架的车体激光雷达外参标定方法。IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据；通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据；以激光雷达相对车体的安装参数作为EKF算法待估状态，将车体系IMU/轮速联合解算输出的位姿增量数据和激光雷达点云匹配实时解算输出的位置增量数据作为EKF量测，进行基于EKF算法的实时在线/离线估计，最终得到激光雷达相对车体的安装参数的最优估计值。本发明可直接将激光雷达坐标系转换至车体系而非IMU坐标系，不仅可适用于离线标定过程，同时也适用于在线标定过程。
Description
技术领域
本发明涉及智能驾驶系统传感器安装外参标定技术，具体涉及一种基于EKF算法框架的车体激光雷达外参标定方法。
背景技术
现有智能驾驶系统中，将激光雷达坐标系与INS（惯性导航系统）/GNSS（全球导航卫星系统）组合导航系统坐标系进行标定统一，往往采用手眼标定模型算法，过程为：将载有激光雷达和INS/GNSS组合导航系统的车辆沿着S弯或者8字型曲线运行，选择中间几个点停止，记录组合导航系统的全局位姿数据，同时记录对应的激光位姿数据；基于静止点位位姿数据，计算相邻时刻激光位姿增量和组合导航系统位姿增量数据；构建基于位姿增量数据的手眼标定方程，基于最小二乘或者非线性优化算法求解组合导航系统与激光雷达坐标系之间的坐标转换矩阵。然而，使用该方法进行激光雷达外参标定存在以下不足：
1）INS/GNSS组合导航系统静止点位的精度依赖于GNSS定位数据精度，而GNSS定位极易受到建筑物遮挡影响，因此标定场景的选取只能在室外开阔场景，同时又必须满足激光雷达定位需求；
2）INS/GNSS组合导航系统输出数据在应用于外参标定之前，需先开启运行一段时间，使INS/GNSS融合输出数据完全收敛才能应用于外参标定；
3）组合导航系统坐标系并不代表车体坐标系，将激光雷达坐标系统一到车体坐标系还需依赖组合导航系统坐标系与车体坐标系之间的标定精度。
发明内容
针对传统激光雷达安装外参标定相关问题，本发明提供一种基于EKF算法框架的车体激光雷达外参标定方法。
一种基于EKF算法框架的车体激光雷达外参标定方法，IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据；通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据；以激光雷达相对车体的安装参数（包括激光雷达相对于车体的姿态偏差和位移偏差）作为EKF算法待估状态，将车体系IMU/轮速联合解算输出的位姿增量数据和激光雷达点云匹配实时解算输出的位置增量数据作为EKF量测，进行基于EKF算法的实时在线/离线估计，最终得到激光雷达相对车体的安装参数的最优估计值。
所述的方法，IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据，在IMU和轮速联合解算时只需对IMU模块进行水平倾斜角补偿即可。
所述的方法，通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据。
所述的方法，在基于EKF算法框架下，以激光雷达相对车体的安装参数（包括激光雷达相对于车体的姿态偏差和位移偏差）作为EKF算法待估状，在EKF状态方程设计过程中将待估状态按常量设置；
所述的方法，将车体系IMU/轮速联合解算输出的位置增量数据作为EKF滤波器第一组观测量，与激光雷达点云匹配实时解算输出的位置增量数据构建第一组观测方程；
所述的方法，将车体系IMU/轮速联合解算输出的姿态增量数据作为EKF滤波器第二组观测量，和激光雷达点云匹配实时解算输出的姿态增量数据构建第二组观测方程，该组观测方程视计算机算力情况为可选项。
所述的方法，步骤如下：
步骤（1）IMU模块和激光雷达安装固定于车辆对应位置后，首先需对IMU模块进行静态水平安装倾角标定，通过车辆静止状态下收集IMU模块加速度计数据与重力加速度对比计算即可完成水平倾角标定补偿；
步骤（2）完成IMU水平倾角标定后，车辆以1~5m/s左右的速度绕8字轨迹运行，通过IMU模块和轮速联合解算输出基于车体坐标系的位姿增量数据，通过激光雷达点云ICP算法实时解算输出基于激光雷达坐标系的位姿增量数据，保存数据；
步骤（3）基于EKF算法框架，以激光雷达相对于车体的姿态偏差和位移偏差作为EKF算法待估状态，以车体系IMU/轮速联合解算输出的位置增量数据和激光雷达点云匹配实时解算输出的位置增量数据构建EKF滤波器第一组观测量及观测方程，以车体系IMU/轮速联合解算输出的姿态增量数据和激光雷达点云匹配实时解算输出的姿态增量数据构建EKF滤波器第二组观测量及观测方程，基于EKF算法流程完成激光雷达相对车体坐标系的外参估计。
本发明的有益效果：
1）基于水平倾角补偿后的IMU角速度数据和轮速速度数据联合解算的位姿增量为直接相对于车体坐标系下的增量数据，通过与激光坐标系位姿增量联合解算标定，可直接将激光雷达坐标系转换至车体系而非IMU坐标系。
2）激光雷达与车体坐标系标定不需要依赖GNSS，因此不必考虑标定场景的建筑物遮挡，可在室内环境操作。
3）基于EKF标定算法框架不仅可适用于离线标定过程，同时也适用于在线标定过程，即在车辆运行过程中持续实时利用车辆运行过程中位姿增量数据, 不断优化迭代安装参数估计值, 在此基础上定期更新安装标定参数可有效解决在运行过程产生由于振动、支架老化等因素引起的激光雷达坐标系偏移问题。
附图说明
图1为IMU在车体安装位置及车体坐标系示意图。
图2为基于EKF算法框架的车体激光雷达外参标定的一种原理框图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚，以下结合附图和实施例对本发明做进一步的阐述。
如图1所示，一种基于EKF算法框架的车体/激光雷达外参标定方法，在该方法中，IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系要求基本对齐即可。
如图2所示，在本实例中，IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据；通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据；以激光雷达相对车体的安装参数（包括激光雷达相对于车体的姿态偏差和位移偏差）作为EKF算法待估状态，将车体系IMU/轮速联合解算输出的位姿增量数据和激光雷达点云匹配实时解算输出的位置增量数据作为EKF量测，进行基于EKF算法的实时在线/离线估计，最终得到激光雷达相对车体的安装参数的最优估计值。
在本实例中，IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据，具体原理如下：
基于k时刻IMU角速度w x 、w y 、w z 轮速数据V ABS ，将k时刻位姿递推至k+1时刻，姿态角更新如下：
位置更新如下:
γk，θk，ψk为k时刻车辆相对参考系的横滚角、俯仰角、航向角，X k ，Y k ，Z k 为k时刻车辆相对参考系的位置，ΔT为时间增量，则可得k、k+1时刻之间的基于车体系的位姿增量为：
其中，R k 为k时刻姿态欧拉角对应的方向余弦矩阵：
tk为k时刻位置： 
ΔR I 、ΔT I 为车体系下姿态方向余弦增量和位置增量。
在本实例中，通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据，姿态增量数据为ΔR I 、位置增量数据为ΔT I ，位姿增量对应的前后时刻必须与基于IMU/轮速联合解算对应的位姿增量前后时刻准确对齐；
在本实例中，在基于EKF算法框架下，以激光雷达相对车体的安装参数（包括激光雷达相对于车体的姿态偏差和位移偏差）作为EKF算法待估状，在EKF状态方程设计过程中将待估状态按常量设置，状态方程如下：
其中
为激光雷达与车体系之间安装偏差角，
为激光雷达与车体之间的安装位置误差。
在本实例中，将车体系IMU/轮速联合解算输出的位置增量数据作为EKF滤波器第一组观测量，与激光雷达点云匹配实时解算输出的位置增量数据构建第一组观测方程，具体原理如下：
其中
为IMU/轮速联合解算姿态角增量，
为激光里程计解算的姿态角增量。
在本实例中，将车体系IMU/轮速联合解算输出的姿态增量数据作为EKF滤波器第二组观测量，和激光雷达点云匹配实时解算输出的姿态增量数据构建第二组观测方程，该组观测方程视计算机算力情况为可选项。
通过EKF算法流程可对标定参数进行有效估计，本实例涉及的EKF算法流程如下：
假设通过线性离散化后的组合定位系统的状态方程和量测方程为：
状态方程：
量测方程：
状态噪声方差阵和量测噪声方差阵：
系统离散化后可得系统矩阵：
系统噪声驱动矩阵：
，量测矩阵：
，结合状态噪声矩阵和量测噪声矩阵，卡尔曼滤波算法过程如下：
状态预估：
协方差预估：
最优增益计算：
状态最优估计：
协方差最优估计：
。
本实施涉及具体步骤如下：
步骤（1）IMU模块和激光雷达安装固定于车辆对应位置后，首先需对IMU模块进行静态水平安装倾角标定，通过车辆静止状态下收集IMU模块加速度计数据与重力加速度对比计算即可完成水平倾角标定补偿；
步骤（2）完成IMU水平倾角标定后，车辆以1~5m/s左右的速度绕8字轨迹运行，通过IMU模块和轮速联合解算输出基于车体坐标系的位姿增量数据，通过激光雷达点云ICP算法实时解算输出基于激光雷达坐标系的位姿增量数据，保存数据；
步骤（3）基于EKF算法框架，以激光雷达相对于车体的姿态偏差和位移偏差作为EKF算法待估状态，以车体系IMU/轮速联合解算输出的位置增量数据和激光雷达点云匹配实时解算输出的位置增量数据构建EKF滤波器第一组观测量及观测方程，以车体系IMU/轮速联合解算输出的姿态增量数据和激光雷达点云匹配实时解算输出的姿态增量数据构建EKF滤波器第二组观测量及观测方程，基于EKF算法流程完成激光雷达相对车体坐标系的外参估计。
Claims (7)
1.一种基于EKF算法框架的车体激光雷达外参标定方法，其特征是：IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据；通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据；以激光雷达相对车体的安装参数，包括激光雷达相对于车体的姿态偏差和位移偏差，作为EKF算法待估状态，将车体系IMU/轮速联合解算输出的位姿增量数据和激光雷达点云匹配实时解算输出的位置增量数据作为EKF量测，进行基于EKF算法的实时在线/离线估计，最终得到激光雷达相对车体的安装参数的最优估计值。
2.根据权利要求1所述的方法，其特征是：IMU模块安装固定于后轮中心点且IMU坐标系与车体坐标系对齐，通过IMU模块输出的三轴角速率数据和车辆后轮轮速数据，联合解算输出基于车体坐标系的位姿增量数据，在IMU和轮速联合解算时只需对IMU模块进行水平倾斜角补偿即可。
3.根据权利要求1所述的方法，其特征是：通过激光雷达点云ICP实时解算输出基于激光雷达坐标系的位姿增量数据。
4.根据权利要求1所述的方法，其特征是：在基于EKF算法框架下，以激光雷达相对车体的安装参数作为EKF算法待估状，在EKF状态方程设计过程中将待估状态按常量设置。
5.根据权利要求1所述的方法，其特征是：将车体系IMU/轮速联合解算输出的位置增量数据作为EKF滤波器第一组观测量，与激光雷达点云匹配实时解算输出的位置增量数据构建第一组观测方程。
6.根据权利要求1所述的方法，其特征是：将车体系IMU/轮速联合解算输出的姿态增量数据作为EKF滤波器第二组观测量，和激光雷达点云匹配实时解算输出的姿态增量数据构建第二组观测方程，该组观测方程视计算机算力情况为可选项。
7.根据权利要求1所述的方法，其特征是：步骤如下：
步骤（1）IMU模块和激光雷达安装固定于车辆对应位置后，首先需对IMU模块进行静态水平安装倾角标定，通过车辆静止状态下收集IMU模块加速度计数据与重力加速度对比计算即可完成水平倾角标定补偿；
步骤（2）完成IMU水平倾角标定后，车辆以1~5m/s左右的速度绕8字轨迹运行，通过IMU模块和轮速联合解算输出基于车体坐标系的位姿增量数据，通过激光雷达点云ICP算法实时解算输出基于激光雷达坐标系的位姿增量数据，保存数据；
步骤（3）基于EKF算法框架，以激光雷达相对于车体的姿态偏差和位移偏差作为EKF算法待估状态，以车体系IMU/轮速联合解算输出的位置增量数据和激光雷达点云匹配实时解算输出的位置增量数据构建EKF滤波器第一组观测量及观测方程，以车体系IMU/轮速联合解算输出的姿态增量数据和激光雷达点云匹配实时解算输出的姿态增量数据构建EKF滤波器第二组观测量及观测方程，基于EKF算法流程完成激光雷达相对车体坐标系的外参估计。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210516094.2A CN114966629A (zh) | 2022-05-12 | 2022-05-12 | 一种基于ekf算法框架的车体激光雷达外参标定方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210516094.2A CN114966629A (zh) | 2022-05-12 | 2022-05-12 | 一种基于ekf算法框架的车体激光雷达外参标定方法 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114966629A true CN114966629A (zh) | 2022-08-30 |
Family
ID=82981435
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210516094.2A Pending CN114966629A (zh) | 2022-05-12 | 2022-05-12 | 一种基于ekf算法框架的车体激光雷达外参标定方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114966629A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115435816A (zh) * | 2022-11-07 | 2022-12-06 | 山东大学 | 在线双舵轮agv内外参标定方法、系统、介质及设备 |
| CN115993089A (zh) * | 2022-11-10 | 2023-04-21 | 山东大学 | 基于pl-icp的在线四舵轮agv内外参标定方法 |
| CN116068538A (zh) * | 2023-04-06 | 2023-05-05 | 中汽研（天津）汽车工程研究院有限公司 | 一种用于批量式车辆激光雷达的可调节标定系统和方法 |
-
2022
- 2022-05-12 CN CN202210516094.2A patent/CN114966629A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115435816A (zh) * | 2022-11-07 | 2022-12-06 | 山东大学 | 在线双舵轮agv内外参标定方法、系统、介质及设备 |
| CN115993089A (zh) * | 2022-11-10 | 2023-04-21 | 山东大学 | 基于pl-icp的在线四舵轮agv内外参标定方法 |
| CN115993089B (zh) * | 2022-11-10 | 2023-08-15 | 山东大学 | 基于pl-icp的在线四舵轮agv内外参标定方法 |
| CN116068538A (zh) * | 2023-04-06 | 2023-05-05 | 中汽研（天津）汽车工程研究院有限公司 | 一种用于批量式车辆激光雷达的可调节标定系统和方法 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| CN112083726B (zh) | 一种面向园区自动驾驶的双滤波器融合定位系统 | |
| CN114966629A (zh) | 一种基于ekf算法框架的车体激光雷达外参标定方法 | |
| CN112505737B (zh) | 一种gnss/ins组合导航方法 | |
| CN104061899B (zh) | 一种基于卡尔曼滤波的车辆侧倾角与俯仰角估计方法 | |
| CN110307836B (zh) | 一种用于无人清扫车辆贴边清扫的精确定位方法 | |
| CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
| CN108387236B (zh) | 一种基于扩展卡尔曼滤波的偏振光slam方法 | |
| CN111006675B (zh) | 基于高精度重力模型的车载激光惯导系统自标定方法 | |
| CN112129297A (zh) | 一种多传感器信息融合的自适应校正室内定位方法 | |
| CN104215262A (zh) | 一种惯性导航系统惯性传感器误差在线动态辨识方法 | |
| JP5164645B2 (ja) | カルマンフィルタ処理における繰り返し演算制御方法及び装置 | |
| CN111220151B (zh) | 载体系下考虑温度模型的惯性和里程计组合导航方法 | |
| CN111751857A (zh) | 一种车辆位姿的估算方法、装置、存储介质及系统 | |
| CN114440881B (zh) | 一种融合多源传感器信息的无人车定位方法 | |
| CN114964222A (zh) | 一种车载imu姿态初始化方法、安装角估计方法及装置 | |
| CN114475581B (zh) | 基于轮速脉冲和imu卡尔曼滤波融合的自动泊车定位方法 | |
| JP5219547B2 (ja) | 車載ナビゲーションシステム及びナビゲーション方法 | |
| CN114184190A (zh) | 一种惯性/里程计组合导航系统及方法 | |
| CN110864688A (zh) | 一种用于车载方位开环水平姿态角闭环的航姿方法 | |
| CN111323167A (zh) | 一种车辆质心高度在线辨识方法及系统 | |
| CN110926483A (zh) | 一种用于自动驾驶的低成本传感器组合定位系统及方法 | |
| CN114674345B (zh) | 一种惯导/相机/激光测速仪在线联合标定方法 | |
| CN116338719A (zh) | 基于b样条函数的激光雷达-惯性-车辆融合定位方法 | |
| CN115290082A (zh) | 一种基于运动辅助惯性导航的误差估计方法 |
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 |