11 算法附录:旋转矢量 · 圆锥补偿 · 划桨补偿¶
本文是 SINS 机械编排底层的三个数学基石,正文(08/09 篇)只用到结果,这里补齐推导与代码逐行解析。 代码入口:
rv2qPSINS.cpp#L408 L429,CIMU::UpdatePSINS.cpp#L3129 L3184。
一、旋转矢量(Rotation Vector)与四元数的关系¶
1.1 为什么不用欧拉角积分,也不用四元数直接积分¶
欧拉角:存在 90° 奇异(万向锁),且角速度→欧拉角的变换含 \(\tan\) 项数值易炸。 四元数直接积分 \(\dot q = \tfrac12 q\otimes\omega\):对恒定轴转动是精确的,但载体角速度方向在积分周期内旋转时(动态圆锥环境),单点采样的 \(\omega\) 会丢失转动方向变化信息,产生不可交换误差。 旋转矢量 \(\boldsymbol\phi = \alpha\,\mathbf u\)(轴 \(\mathbf u\)、转角 \(\alpha\))显式编码了转动的"方向沿时间旋转",用它做积分 + 补偿后转回四元数,能抵消不可交换误差——这就是圆锥补偿的理论基础。
1.2 旋转矢量 → 四元数(标准变换)¶
即 rv2q 返回的结构 [w=cos(α/2), xyz=sin(α/2)·ẑ]。姿态更新就是右乘:
(见 08 篇式 中 qnb = qnb*rv2q(imu.phim)。)
二、rv2q 逐行解析¶
2.1 代码¶
CQuat rv2q(const CVect3 &rv)
{
#define F1 ( 2 * 1) // Fk = 2^k * k!
#define F2 (F1*2 * 2)
#define F3 (F2*2 * 3)
#define F4 (F3*2 * 4)
#define F5 (F4*2 * 5)
double n2 = rv.i*rv.i+rv.j*rv.j+rv.k*rv.k, c, f;
if(n2 < (PI/180.0*PI/180.0)) // ‖rv‖² < (1°)²
{
double n4 = n2*n2;
c = 1.0 n2*(1.0/F2) + n4*(1.0/F4);
f = 0.5 n2*(1.0/F3) + n4*(1.0/F5);
}
else {
double n_2 = sqrt(n2)/2.0;
c = cos(n_2);
f = sin(n_2)/n_2*0.5;
}
return CQuat(c, f*rv.i, f*rv.j, f*rv.k);
}
2.2 系数推演(泰勒级数,小角度分支)¶
记 \(\theta=\|\boldsymbol\phi\|\)、\(\alpha=\theta/2\)。四元数:
对 \(\alpha\ll1\) 做泰勒展开:
\(\cos\alpha = 1 \dfrac{\alpha^2}{2} + \dfrac{\alpha^4}{24} \cdots = 1 \dfrac{\theta^2}{8} + \dfrac{\theta^4}{384} \cdots\) \(\dfrac{\sin\alpha}{\theta/2}\cdot\dfrac12 = \dfrac{\sin(\theta/2)}{\theta} = \dfrac12 \dfrac{\theta^2}{48} + \dfrac{\theta^4}{3840} \cdots\)
宏展开验证阶乘:\(F_2=8,\ F_3=48,\ F_4=384,\ F_5=3840\),与上面对应系数完全一致(\(n_2=\theta^2, n_4=\theta^4\)):
c = 1 n2/F2 + n4/F4; // = 1 θ²/8 + θ⁴/384 ⟵ cos(θ/2)
f = 0.5 n2/F3 + n4/F5; // = 0.5 θ²/48 + θ⁴/3840 ⟵ sin(θ/2)/θ
2.3 为什么分裂个分支(数值技巧精髓)¶
小角度时直接算 sin(θ/2)/θ:分子、分母同为极小量,舍入误差会把 \(f\) 污染,阶数越高越严重;而 θ≈0 时真值应收敛到 f→0.5。 分支判断 \(n_2<(\pi/180)^2\) 即转角 \(<1°\) 用级数(保留到 \(\theta^4\) 已足够,误差 \(\sim\theta^6\)),否则用精确 \(\cos/\sin\)。 不这么做,车载高频小角度震动的每个周期都会给四元数注入噪声——这是嵌入式惯导与桌面仿真的关键差异。
三、圆锥补偿(Coning Compensation)¶
3.1 问题:为什么单点角增量会丢信息¶
圆锥运动:载体角速度方向在一个周期内匀速绕轴旋转(弹体/机身的高频章动典型)。此时虽然角速度模长恒定,但每个采样周期的方向都在变。若只用"该周期总角增量"做四元数积分,转动的不可交换量(转动顺序不可交换导致的姿态差)被丢弃,姿态会系统性漂移。
多子样锥补偿用前几个子周期的角增量组合逼近该不可交换量。双子样经典算法:
叉积项就是补偿量,系数 \(2/3\) 来自圆锥运动的精确解。
3.2 代码实现(CIMU::Update)¶
static double conefactors[5][4] = { // nSamples=2..5 的补偿系数
{2./3}, // n=2
{9./20, 27./20}, // n=3
{54./105, 92./105, 214./105}, // n=4
{250./504, 525./504, 650./504, 1375./504} // n=5
};
...
if(nSamples==1 && onePlusPre) { // 单子样 + 用上一周期
...
cm = 1.0/12*wm_1; sm = 1.0/12*vm_1;
}
for(i=0; i<nSamples 1; i++) { // 只累加前 n 1 个子样本
cm += pcf[i]*pwm[i];
sm += pcf[i]*pvm[i];
wmm += pwm[i]; vmm += pvm[i];
}
wm_1=pwm[i]; vm_1=pvm[i];
wmm += pwm[i]; vmm += pvm[i]; // 第 n 个子样本单独加
phim = wmm + cm*pwm[i]; // 角增量:ΣΔθ + 圆锥补偿
dvbm = vmm + 1.0/2*wmm*vmm + (cm*pvm[i]+sm*pwm[i]); // 比力积分 + 划桨补偿
关键:cm 累计了前 \(n 1\) 个样本的圆锥系数,最后乘上第 n 个样本 \(\Delta\boldsymbol\theta_n\),构成 \(\sum_i c_i\,\Delta\boldsymbol\theta_i\times\Delta\boldsymbol\theta_n\) 的补偿。
3.3 one plus previous(本工程实际走的路径)¶
main.cpp 的 kf.Update(&wm,&vm, 1, TS, 5) 传的是 nSamples=1,CIMU 默认 onePlusPre=true,因此走:
即用上一周期样本 \(\Delta\theta_{k 1}\) 当作"第二子样",系数 \(1/12\)。这是"低成本近似双子样"——比纯单子样多一阶精度,又不需要改变采样率。
四、划桨补偿(Sculling Compensation)¶
4.1 问题¶
比力积分 \(\int f^b dt\) 假设角速度与比力模长恒定。但载体同时做角振动 + 线振动(划桨效应)时,仅用每周期平均比力会遗失"角运动改变比力方向"导致的额外速度。
4.2 代码¶
vmm:子样本速度增量之和(主体积分)。 1/2·wmm×vmm:旋转补偿(速度方向随载体旋转)。 cm*pvm[i] + sm*pwm[i]:多子样划桨补偿(角 线振动耦合的高阶项)。与圆锥补偿对称地复用同一组系数 \(c_i,s_i\)。
圆锥补偿服务"姿态",划桨补偿服务"速度",二者都是动态环境下的不可交换/不不可交换误差高阶修正;低速慢偏的桌面静态测试下几乎无影响,但 VTOL 高机动(如你的 2.5g 转弯)时必须保证多子样路径触发。
五、本工程到底触发哪些补偿¶
| 调用 | nSamples | 触发路径 | 补偿项 |
|---|---|---|---|
kf.Update(&wm,&vm,1,TS,5)(08/09) | 1 + onePlusPre | 「1.0/12」近似双子样 | 低阶圆锥 + 划桨 |
| 多子样(nSamples≥2,本工程未用) | 2 5 | conefactors 全系数 | 满阶圆锥 + 划桨 |
结论:本工程实际只有"单子样 + 上一周期"的低阶近似补偿。若后续 H743 高机动需要更高的姿态/速度积分精度,应提高 IMU 子采样率并以 nSamples≥2 调用——这也是 PSINS 提供 nSamples 参数的原因。
参考资料¶
严恭敏, 严热宝. 捷联惯导算法与组合导航原理. 西北工业大学出版社 —— 圆锥/划桨/旋转矢量章节 严恭敏教授 PSINS 官网:http://www.psins.org.cn/ Savage, P. G. Strapdown Analytics(圆锥/划桨严格推导) 本 wiki:08_SINS/GNSS 静态组合.md(qnb = qnb*rv2q(imu.phim) 使用处)、./14_H743移植指南.md(高机动移植)
导航¶
- 上一篇:./10_机械编排与惯导解算.md
- 下一篇:./12_CKalman核心与工程机制.md
- 回到总览:./00_总览与架构.md