用dcm算法求取imu+wheel位姿思路_dcm位姿解算算法-CSDN博客

本文链接：https://blog.csdn.net/u010949023/article/details/115653768

本文讨论了如何在imu和轮子数据对齐时，准确计算每帧轮子对应的绝对方向矩阵（DCM）。关键步骤包括逐帧累加imu增量、处理虚拟轮子帧，并强调了在处理最后阶段imu与image对齐时的区别。策略涉及连续计算imu-wheeledcm增量，适用于imu和轮子数据密集对齐场景。

摘要由CSDN通过智能技术生成

难点在于imu和wheel是帧对齐，我们需要求出每一帧wheel对应的dcm姿态。
基本思路：
1.我们在做imu和wheel对齐时，就求出每一帧wheel对应的绝对dcm姿态。（我们原来的wheel_q,求的是每一帧wheel到上一次image之间的姿态）如下图所示：
在这里插入图片描述

注意：
1.wheel_dcm一直是一个累加的绝对值，求取wheel_dcm不能再像wheel_q那样求取，我们必须求出每一段wheel对应的imu，而不是像以前那样，求出wheel和i帧image之间的imu即可。
而且，在对齐的最后的一段，wheel是j帧image与上一帧wheel虚拟出来的（红色部分），最后一段的wheel_q它是直接等于imu_delta_q(在imu与image对齐时已求出，即两帧image之间的姿态),故这一段是没有通过对齐wheel和imu来求wheel_q的，但wheel_dcm只能通过这种对齐的方式求取。
综上所述，如果我们需要求出每一帧wheel对应的dcm姿态，我们不能再像上面求取wheel_q的方式，我们必须求出两帧wheel对应的imu，而且还得考虑image.
策略：
1.第0帧wheel odo0_t < imu0_t 求出ow0
wheel_dcm(ow0dt1)
2.第1帧得求出ow1
wheel_dcm(w0dt2)
wheel_dcm(w1dt)
wheel_dcm(w2dt)
wheel_dcm(ow1dt3)
3.第2帧与2同
4.最后一帧是image虚拟出来的
wheel_dcm(w6dt6)
wheel_dcm(wj*dt7)
在这里插入图片描述

//last_w 记录上一次更新dcm的w
//last_t 记录上一次更新dcm的时间戳  这两个一直要使用的
// j 记录odo_t < imui_t时的imu序号，下一次从这帧imu开始    两帧image之间使用
for(i=j;  i<imu.size();i++)
{
	if(odo_t < imu[i]_t)
	{
		j = i;
		odo_w = last_w last_t imui_t imui_w
		dcm(odo_w*(odo_t - last_t))
		last_w = odo_w
		last_t = odo_t
		break;
	}else{
		dcm(imu[i]_w,imu[i]_t-last_t)
		last_w = imu[i]_w
		last_t = imu[i]_t
	}
}