跳转至

11 算法附录:旋转矢量 · 圆锥补偿 · 划桨补偿

本文是 SINS 机械编排底层的三个数学基石,正文(08/09 篇)只用到结果,这里补齐推导与代码逐行解析。 代码入口:rv2q PSINS.cpp#L408 L429,CIMU::Update PSINS.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 旋转矢量 → 四元数(标准变换)

\[ q = \begin{bmatrix} \cos\frac{\|\boldsymbol\phi\|}{2} \\[2pt] \dfrac{\boldsymbol\phi}{\|\boldsymbol\phi\|}\sin\frac{\|\boldsymbol\phi\|}{2}\end{bmatrix} \]

即 rv2q 返回的结构 [w=cos(α/2), xyz=sin(α/2)·ẑ]。姿态更新就是右乘:

\[ q_{nb,k+1} = q_{nb,k}\otimes rv2q(\boldsymbol\phi) \]

(见 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\)。四元数:

\[ w=\cos\frac{\theta}{2},\qquad \mathbf v = \hat{\boldsymbol\phi}\,\sin\frac{\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 问题:为什么单点角增量会丢信息

圆锥运动:载体角速度方向在一个周期内匀速绕轴旋转(弹体/机身的高频章动典型)。此时虽然角速度模长恒定,但每个采样周期的方向都在变。若只用"该周期总角增量"做四元数积分,转动的不可交换量(转动顺序不可交换导致的姿态差)被丢弃,姿态会系统性漂移。

多子样锥补偿用前几个子周期的角增量组合逼近该不可交换量。双子样经典算法:

\[ \boldsymbol\phi = \Delta\boldsymbol\theta_1+\Delta\boldsymbol\theta_2 + \frac{2}{3}\,\Delta\boldsymbol\theta_1\times\Delta\boldsymbol\theta_2 \]

叉积项就是补偿量,系数 \(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,因此走:

cm = 1.0/12*wm_1;   sm = 1.0/12*vm_1;

即用上一周期样本 \(\Delta\theta_{k 1}\) 当作"第二子样",系数 \(1/12\)。这是"低成本近似双子样"——比纯单子样多一阶精度,又不需要改变采样率。

四、划桨补偿(Sculling Compensation)

4.1 问题

比力积分 \(\int f^b dt\) 假设角速度与比力模长恒定。但载体同时做角振动 + 线振动(划桨效应)时,仅用每周期平均比力会遗失"角运动改变比力方向"导致的额外速度。

4.2 代码

dvbm = vmm + 1.0/2*wmm*vmm + (cm*pvm[i] + sm*pwm[i]);

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(高机动移植)


导航