LOAM中关于坐标转换与IMU融合

激光雷达slam之LOAM中的坐标转换与IMU融合

需要用到的一些知识和假设:

(1)  来源于 github中的讨论:

由于IMU累积推算位置的误差大,程序中粗略地计算了IMU的位置漂移。

_imuPositionShift = _imuCur.position - _imuStart.position - _imuStart.velocity * relSweepTime;

上式成立的前提是认为一个扫描周期内,Lidar的运动是匀速的,上式计算出了非线性误差部分。

(2) X、Y、Z轴对应俯仰(pitch)、航向(yaw)、横滚(roll)机动,可知Lidar坐标系为“右下前”坐标系。

(3)  从Lidar系到global IMU系,类似于惯导系统中的C(b->n),即载体系到地理系的转换。

旋转顺序为:横滚->俯仰->航向

rotateZXY(point, roll, pitch, yaw);

从global IMU系到Lidar系,旋转顺序正好相反。

rotateYXZ(point, -yaw, -pitch, -roll);

(4)  transform代表将k时刻的点云转换到k+1时刻下,与视觉slam中的相对位姿定义相同。

坐标转换与IMU融合

1、 transformToStartIMU

注册点云时(MultiScanRegistration.cpp中),当判断有IMU数据时,会进行一步坐标转换的预处理,体现在函数transformToStartIMU中。

如上图所示,我们可以作如下描述:坐标系O-XY逆时针旋转theta(或顺时针旋转-theta)后与坐标系O'-X'Y'重合,或者坐标系O‘-X’Y‘顺时针旋转theta(或逆时针旋转-theta)后与坐标系O-XY重合。利用简单的之间三角形的余弦定理和正玄定理就可以得到以下公式

这里transformToStartIMU,先旋转回到imu的原点,然后再旋转到i=0时刻。

其中旋转回原点:类似于把o'-X'Y'坐标系下的点转换到o-XY坐标系下。

而从原点旋转到i=0时刻:类似于把o-XY转换到o'-X'Y'下。

经过此步处理后,点云是在imu中某一起始时刻下的点,即所有点都依据imu角度进行了对齐,对齐到某一起始时刻。

这个函数进行了三步处理:

(1)       rotateZXY(point, _imuCur.roll, _imuCur.pitch, _imuCur.yaw);

将原始点云从当前Lidar系转换到global IMU系下;

(2)       补偿了_imuPositionShift,也即估算的IMU位置漂移;

(3)       rotateYXZ(point, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);

将global IMU系下的点云转换到Start时刻的Lidar系下。

         经过这个函数的处理,点云的position部分处于当前位姿的Lidar下一个相对准确的位置上(基于扫描周期内匀速运动的假设),但点云的Rotation部分是Start时刻Lidar下观察所得的,而非处于当前Lidar下。更清晰地来说,即此时观察到的点云坐标,是以当前Lidar的坐标(一个估计值)为原点,而坐标轴是与Start时刻的Lidar系的坐标轴对齐的。

2、  OD初始化:

根据第一次开始扫描时的IMU pitch与roll,作为累积位姿的初始值。  

 _transformSum.rot_x += _imuPitchStart;

_transformSum.rot_z += _imuRollStart;

Yaw角度和pos部分都未赋初值,即假设开始时刻的偏航角为0,位于global系下的原点位置。

3、  运动估计初值:

(1)       _transform.pos -= _imuVeloFromStart * _scanPeriod; 

其中,imuVeloFromStart的计算,可知imuVeloFromStart为Start时刻Lidar系下的速度变化矢量:

imuVelocityFromStart = _imuCur.velocity - _imuStart.velocity;

  rotateYXZ(imuVelocityFromStart, -_imuStart.yaw, -_imuStart.pitch, -_imuStart.roll);

平移量的初值赋值为加减速的位移量,为其梯度下降的方向(沿用上次转换的T(一个sweep匀速模型);

同时在其基础上减去匀速运动位移,即只考虑加减速的位移量);

S = v_2*t+0.5*(v_2-v_1)*t;   这段时间的位移,相比上个时刻,多了0.5*(v_2-v_1)*t

对于匀速运动假设的一个补偿,并且基于运动曲线的连续性,做了递推形式的计算,可能乘以1/2会更合适?

(2)       _transform的rotation部分未赋初值,认为为0。

4、  transformToStart

在进行KDTree最近点搜索前,首先将进行畸变处理后的点云转换到每一次扫描的开始时刻。

这里考虑,由于运动,当前的点云数据,存在由于运动导致的偏移;需要转换到开始时刻即start时刻,去除匀速运动产生的漂移

类似于把o-XY转换到o'-X'Y'坐标系下 

注意,code中transform初始值如何赋值;负值,反方向旋转角;把坐标从o-xy转换到o'-X'Y',反向处理;

即(P-T)*R_inv = P';效果等同于

先根据匀速运动假设计算出当前点时刻Lidar的位移和旋转。

5、  transformToEnd

(1)       先进行transformToStart,此时点云处于start时刻的Lidar系下;

(2)       通过_transform转换到end时刻的Lidar系下;即o-XY转换到o'-X'Y'坐标系下;

(3)       rotateZXY(point, _imuRollStart, _imuPitchStart, _imuYawStart);

转换到global系下; 即o’-X'Y'转换到o-XY坐标系下;

(4)       rotateYXZ(point, -_imuYawEnd, -_imuPitchEnd, -_imuRollEnd);

转换到end时刻的Lidar系下;即o-XY转换到o'-X'Y'坐标系下;

总结点云的旋转过程从1->5, 可用公式表示为:

6、accumulateRotation

该函数的作用是将计算的两帧之间的位姿“累加”起来,获得相对于第一帧的旋转矩阵,具体公式如下:

                                                                            

7、 pluginIMURotation

该函数与accumulateRotation,联合起来完成了更新_transformSum的rotation部分的工作。该函数可视为transformToEnd的下部分的逆过程。具体公式如下:

                                                         

8. 关于transformToStart,transformToStartIMU的理解

查看loam_velodyne代码中,可以发现,transformToStartIMU中使用的是ShiftToStartIMU和VeloToStartIMU先计算imuShiftFromStartXCur和imuVeloFromStartXCur,在TransformToStartIMU中消除加减速产生的位移畸变;

2021-11-18 added)loam类的去畸变:①imu角速度积分,根据一帧数据的首尾点时间,去除旋转带来的畸变;②imu加速度积分,去除一帧数据中,由于加减速带来的畸变;实际中①②是一起处理的;③,经过①②后,一帧数据中只剩下由于匀速运动带来的畸变,之后可以使用计算出来的运动量,去除平移方面的畸变;【由于imu积分,平移部分随着时间推移,累计误差比较大】

transformToStart  中,是利用帧间匹配,计算出的motion,消除由于匀速运动产生的畸变;

以下部分参考:l1323的博客_易子岚_优快云博客 

 关于imu消除重力加速度的理解:

 

关于AccumulateRotation函数的理解

AccumulateRotation计算当前帧终点相对于世界坐标系的欧拉角,即上一帧的位姿结合当前帧的运动量,累加计算出来当前帧的世界坐标系位姿

transformSum: 前一帧数据的终点在当前世界坐标系下的位姿

transform:当前帧起点到终点的位姿

参考V-LOAM源码解析(二) - zhch_pan - 博客园中的注释

公式推导如下:

 可以看出程序中对应的公式与推算的R23,R13,R33,R21,R22一致。

关于PluginIMURotation函数

PluginIMURotation,可以理解为将之前计算的rx,ry,rz,累加上imu的旋转变化量;主要操作为Mbc*Mbl.inv()*Mal,展开后与代码中公式一致。

主要还是绕Z-X-Y旋转的变换,公式推导如下:

LOAM中激光里程计部分

如上公式所示,把之前通过点到线的距离以及点到面的距离构建的误差函数转化成关于Tk+1的函数,即源码中的transform[i]数组,这部分和建图部分求导不同在于transform数组表示的是当前帧的起始点相对于终点的位姿,欧拉角表示的是Y,X,Z的旋转,旋转矩阵对应上式的R矩阵,通过链式法则求导,计算过程如下:

la,lb,lc代表距离对起始点的偏导,在前面已经计算过了即源码对应的coeff.x,coeff.y,coeff.z

计算出来的arx和源码一模一样,依次可算出ary,arz,源码如下

对平移分量tx,ty,tz的偏导计算如下:

和源码中的atx一模一样,以此类推即可计算出aty,atz

<think>好的,用户想了解如何用C++实现Lidar、IMU、轮速计和GNSS的多传感器融合定位算法。这个问题挺复杂的,我得一步步来思考。 首先,用户可能对多传感器融合的基本概念有所了解,但具体实现细节可能不太清楚。需要先介绍整体框架,再分模块讲解。可能需要先解释每个传感器的优缺点,再说明融合的必要性。 接下来,传感器特性方面,Lidar提供高精度的点云数据,适合建图和定位,但计算量大,而且受环境影响大,比如雨天或雾天。IMU测量角速度和加速度,数据频率高,但存在漂移问题。轮速计提供轮子的转速和转向,可以推算航迹,但容易打滑导致误差。GNSS给出全局位置,但在遮挡环境下信号差。这些互补的传感器融合起来能提升鲁棒性和精度。 然后是融合框架的选择。常见的有多传感器卡尔曼滤波(如EKF、UKF)、因子图优化(如GTSAM、Ceres)、滑动窗口优化。需要比较它们的优缺点。卡尔曼滤波计算量小,适合实时,但线性假设可能不准确;因子图优化处理非线性问题更好,适合后端优化,但计算量大。可能需要分层处理,前端用滤波,后端用优化。 数据预处理部分,各个传感器的数据同步和坐标转换是关键。比如,Lidar和IMU需要时间同步,外参标定。轮速计需要标定轮子半径和轴距。GNSS的坐标系转换到局部坐标系。这部分需要详细说明,否则用户可能在实际实现时遇到问题。 接着是各个模块的具体实现。IMU的预积分,减少状态量的重复积分,这在因子图优化中常用。轮速计的航迹推算模型,比如差分驱动的运动学模型。Lidar的匹配算法,如ICP、NDT,或者更现代的LOAM、LIO-SAM中的方法。GNSS的位置补偿,可能需要考虑不同坐标系转换,以及如何局部地图对齐。 状态估计部分,可能需要用卡尔曼滤波来预测和更新状态,或者用图优化整合多传感器的约束。比如,IMU提供高频的状态预测,Lidar和GNSS提供低频的校正。这里需要解释预测和更新的步骤,以及如何融合不同传感器的观测数据。 时间同步和延迟处理也是难点,特别是不同传感器的数据时间戳可能不一致,需要插值或者缓存处理。比如,IMU的数据频率很高,而Lidar可能只有10Hz,如何处理两者的时间差,可能需要使用IMU的积分来预测到Lidar的时间点。 代码结构方面,用户可能需要一个示例框架,包括数据接口、预处理、核心算法模块、输出等。推荐使用ROS,因为可以方便地处理传感器数据的订阅和发布,以及现有的库支持。比如,使用GTSAM来做因子图优化,PCL处理点云,Eigen做矩阵运算。 最后,优化验证部分,需要说明如何调参和测试,比如使用仿真环境或真实数据集,评估定位精度和鲁棒性。可能还要提到实际部署中的问题,比如计算资源限制,实时性要求。 用户可能的疑问包括:如何选择融合框架?如何处理传感器之间的外参标定?如何解决计算效率问题?是否需要使用GPU加速?如何评估融合算法的性能?这些可能需要进一步考虑,但在回答中要尽量覆盖。 另外,用户可能希望了解现有的开源项目或库,比如LIO-SAM、LVI-SAM,这些项目已经实现了类似的融合,可以参考其架构和实现细节。提到这些可以帮助用户快速入门,避免重复造轮子。 总结来说,回答需要结构清晰,分步骤讲解,涵盖传感器特性、融合框架、各模块实现、代码结构以及优化验证等方面,同时给出实际开发中的建议和推荐工具库。</think>多传感器融合定位算法开发需要系统化的工程实现,以下分步骤介绍关键实现要点: --- ### **一、传感器特性分析** | 传感器 | 数据特性 | 误差来源 | 输出频率 | |---------|---------------------------|---------------------------|------------| | Lidar | 3D点云,高精度几何信息 | 运动畸变、环境反射 | 10-20Hz | | IMU | 角速度/线加速度,高频数据 | 零偏、温漂、噪声累积 | 100-1000Hz | | 轮速计 | 线速度/角速度,机械测量 | 打滑、轮胎变形 | 10-100Hz | | GNSS | 全局位置,绝对坐标 | 多径效应、信号遮挡 | 1-10Hz | --- ### **二、融合框架选择** #### 1. 分层融合架构 ```cpp // 伪代码示例 class FusionSystem { public: void ProcessIMU(const ImuData& imu); // 高频IMU预测 void ProcessWheel(const WheelData& wheel); void ProcessLidar(const PointCloud& cloud); // 低频观测修正 void ProcessGNSS(const GnssData& gnss); private: KalmanFilter ekf_; // 前端滤波 FactorGraph optimizer_; // 后端优化 }; ``` #### 2. 典型方案对比 | 方法 | 优点 | 缺点 | 适用场景 | |--------------|---------------------------|---------------------------|------------------| | EKF | 实现简单,实时性好 | 线性化误差大 | 嵌入式系统 | | Factor Graph | 高精度,支持闭环优化 | 计算资源消耗大 | 离线/高性能平台 | | LIO-SAM | 紧耦合设计,鲁棒性强 | 依赖IMU质量 | 自动驾驶 | --- ### **三、核心模块实现** #### 1. IMU预积分 $$ \Delta v_{ij} = \int_{t_i}^{t_j} (R_t \cdot a_t - b_a) dt $$ ```cpp class IMUPreintegrator { public: void Integrate(const ImuData& imu) { dt_ = imu.timestamp - last_timestamp_; // 中值积分法 mid_angular_vel = 0.5*(last_gyro + imu.gyro) - bias_gyro_; mid_linear_acc = 0.5*(last_acc + imu.acc) - bias_acc_; UpdateRotation(mid_angular_vel * dt_); UpdateVelocity(mid_linear_acc * dt_); UpdatePosition(); } }; ``` #### 2. 轮速计运动模型 差分驱动模型: $$ \begin{cases} v = \frac{v_l + v_r}{2} \\ \omega = \frac{v_r - v_l}{L} \end{cases} $$ ```cpp void WheelOdom::ComputeOdom() { double vl = left_wheel_rpm_ * wheel_radius_; double vr = right_wheel_rpm_ * wheel_radius_; linear_vel_ = (vl + vr) / 2.0; angular_vel_ = (vr - vl) / wheel_base_; } ``` --- ### **四、关键代码结构** ``` project/ ├── include/ │ ├── fusion_node.h // ROS节点封装 │ ├── imu_integrator.h // IMU预积分 │ └── lidar_align.h // 点云匹配 ├── src/ │ ├── ekf.cpp // 卡尔曼滤波实现 │ ├── factor_graph.cpp // 因子图优化 │ └── utils.cpp // 坐标转换工具 └── launch/ └── fusion.launch // 参数配置文件 ``` --- ### **五、优化验证** 1. **时间同步策略** - 使用**线性插值**补偿传感器延迟 - 创建统一时间基准(如ROS Time) 2. **外参标定** ```python # 标定工具示例(ROS) rosrun lidar_imu_calibration calibrator \ --bag calibration.bag \ --topic /imu /lidar ``` 3. **性能评估指标** - **绝对轨迹误差(ATE)**: $$ ATE = \sqrt{\frac{1}{N}\sum_{i=1}^N \|t_{est,i} - t_{gt,i}\|^2} $$ - **相对位姿误差(RPE)** --- ### **六、推荐开发工具** 1. **数学库**: Eigen, Ceres-Solver 2. **点云处理**: PCL, Open3D 3. **优化框架**: GTSAM, VINS-Fusion 4. **仿真环境**: CARLA, Gazebo 实际开发中可参考开源项目如[LIO-SAM](https://github.com/TixiaoShan/LIO-SAM),其实现了紧耦合的Lidar-IMU融合。重点注意传感器时空对齐误差建模的准确性。
评论 16
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值