宜春市建设局网站,如何优化网站到首页优化,html网页制作怎么做,网站部署 模板0. 预备
a. IMU测量值解释 IMU在测量时#xff0c;得到的角速度或者加速度均是相对于地心惯性系结果#xff0c;并且将该结果表示到Body坐标系下#xff0c;就形成了最终的IMU输出。
记作#xff1a; ω i b b \omega_{ib}^b ωibb#xff0c;表示body系相对于惯性系的…0. 预备
a. IMU测量值解释 IMU在测量时得到的角速度或者加速度均是相对于地心惯性系结果并且将该结果表示到Body坐标系下就形成了最终的IMU输出。
记作 ω i b b \omega_{ib}^b ωibb表示body系相对于惯性系的角速度测量在body下的表示。
a b i b a_{bi}^b abib表示body系相对于惯性系的加速度测量在body系下的表示。
b. IMU加速度计误差建模 a ~ i b b a i b b g b b a n a \tilde a_{ib}^b a_{ib}^b g^b b^a n^a a~ibbaibbgbbana 其中 a ~ i b b \tilde a_{ib}^b a~ibb 表示的是IMU最终输出的测量值
a i b b a_{ib}^b aibb 表示的是当前的加速度真值去除了各种误差
g b g^b gb重力加速度在body系下的表示
b a b^a ba 加速度计的bias它是一个恒定偏差并且随时间缓慢变化
n a n^a na加速度计的随机测量噪声建模为0均值高斯噪声。
对于上式的加速度计建模有时候也会建模到导航系下多数情况下重力加速度是在导航系下有比较简洁的表示 [ 0 , 0 , g ] T [0, 0, g]^T [0,0,g]T因此如果将加速度计的测量转到导航下就可以直接在导航系中简单的处理重力加速度。因此在实际使用时有时也可以建模为如下形式 a ~ i b b R b w ( a i b w g w ) b a n a \tilde a_{ib}^b R_{bw} (a_{ib}^w g^w) b^a n^a a~ibbRbw(aibwgw)bana c. IMU陀螺仪误差建模 ω ~ i b b ω i b b b g n g \tilde \omega_{ib}^b \omega_{ib}^b b^g n^g ω~ibbωibbbgng 其中 ω ~ i b b \tilde \omega_{ib}^b ω~ibb表示的是IMU最终输出的角速度测量值
ω i b b \omega_{ib}^b ωibb表示的是当前的角速度真值去除了各种误差
b g b^g bg 陀螺仪的bias并且随时间缓慢变化短时间内可以认为是一个恒定偏差
n g n^g ng 陀螺仪的随机测量噪声建模为0均值高斯噪声。
d. 考虑地球自转时的各个角速度关系 ω i n n ω i e n ω e n n \omega_{in}^n \omega_{ie}^n \omega_{en}^n ωinnωienωenn 其中 ω i n n \omega_{in}^n ωinn 表示导航系相对于惯性系的角速度在导航下的表示
ω i e n \omega_{ie}^n ωien表示地球相对于惯性系的自转角速度在导航下的表示
ω e n n \omega_{en}^n ωenn 表示导航系相对于地球的角速度在导航下的表示
IMU测量的角速度是相对于惯性系i而多数情况下进行导航姿态计算时是在导航系下的也就是说如果直接使用IMU的角速度进行解算就会把地球自转以及导航系变化带来的角速度都会引入到系统中在高精度的IMU中这会带来误差对于MEMS的IMU而言可以忽略这一点。
对于 ω i e n \omega_{ie}^n ωien的计算如下 ω i e n [ 0 ω i e cos L ω i e sin L ] \omega_{ie}^n [0 \quad \omega_{ie} \cos L \ \quad \omega_{ie} \sin L] ωien[0ωiecosL ωiesinL] 其中 ω i e \omega_{ie} ωie是地球的自转角速度取 7.2921151467 × 1 0 − 5 r a d / s 7.2921151467 \times 10^{-5} rad/s 7.2921151467×10−5rad/s。参考《捷联惯导算法与组合导航原理》 P54
L L L是当前位置对应的地理纬度
对于 ω e n n \omega_{en}^n ωenn的计算如下 ω e n n [ − v N R M h v E R N h v E R N h tan L ] \omega_{en}^n [-\frac{v_N}{R_M h} \quad \frac{v_E}{R_N h} \quad \frac{v_E}{R_Nh}\tan L] ωenn[−RMhvNRNhvERNhvEtanL] 其中 v E v_E vE, v N v_N vN是当前时刻载体在东北天坐标系下的速度分量 R M , R N R_M,R_N RM,RN分别为子午圈主曲率半径和卯酉圈曲率半径 R N R_N RN计算方法如下参考《捷联惯导算法与组合导航原理》 P46和P48 R N R e 1 − e 2 sin 2 L R_N \frac{R_e}{\sqrt{1 - e^2 \sin^2 L}} RN1−e2sin2L Re R M R e ( 1 − e 2 ) ( 1 − e 2 sin 2 L ) 3 2 R_M \frac{R_e(1-e^2)}{(1-e^2 \sin^2 L)^{\frac{3}{2}}} RM(1−e2sin2L)23Re(1−e2)
L , h L,h L,h分别是当前位置对应的地理纬度和高度 注意上面介绍了IMU的误差建模但是后续的推导均未考虑误差 1. 姿态更新 参考《捷联惯导算法与组合导航原理》 P79 a.惯性系下的IMU姿态解算 C ˙ b i C b i ∗ ( ω i b b × ) \dot C_b^i C_b^i * (\omega_{ib}^b \times) C˙biCbi∗(ωibb×) 上式是IMU的姿态的微分方程。如果要在惯性系下对上式进行解微分方程即可得到IMU姿态计算结果。结果如下 C b ( m ) i C b ( m − 1 ) i C b ( m ) b ( m − 1 ) C_{b(m)}^{i} C_{b(m-1)}^i C_{b(m)}^{b(m-1)} Cb(m)iCb(m−1)iCb(m)b(m−1) 其中 C b ( m ) i C_{b(m)}^{i} Cb(m)i 表示m时刻b系到i的变换矩阵
C b ( m ) b ( m − 1 ) C_{b(m)}^{b(m-1)} Cb(m)b(m−1) 表示由b系在m时刻到m-1时刻的变化矩阵它就是有IMU的角速度测量计算而来。
需要格外的关注的是 C b ( m ) b ( m − 1 ) C_{b(m)}^{b(m-1)} Cb(m)b(m−1)的计算它是IMU姿态计算的关键如果m-1时刻到m是IMU符合定轴转动则由下式严格成立 C b ( m ) b ( m − 1 ) e ∫ t m − 1 t m ω i b b ( t ) d t C_{b(m)}^{b(m-1)} e^{\int_{t_{m-1}}^{t_m} \omega_{ib}^b(t) dt} Cb(m)b(m−1)e∫tm−1tmωibb(t)dt 注意上式是在定轴假设下才严格成立的所谓定轴转动就是角速度的方向不发生变化但是很显然这个假设在实际情况下不严格成立但是幸运的是目前的IMU都是高频率的IMU它一次测量内很接近定轴转动。对于 e ∫ t m − 1 t m ω i b b ( t ) d t e^{\int_{t_{m-1}}{t_m} \omega_{ib}^b(t) dt} e∫tm−1tmωibb(t)dt的计算有多种方式有欧拉积分中值积分矢量二子样算法单子样前一周期等算法。参考《捷联惯导算法与组合导航原理》 P28~P30。尽管解算方法很多但是对于消费级的MEMS IMU使用中值积分就足够使用。
b.导航系下的IMU姿态解算
上面的过程是围绕着参考系为i系进行的多数情况下我们在使用时都是用的导航系下面就以东北线(ENU)导航系对IMU姿态解算进行推导介绍。导航下旋转矩阵对时间的微分方程如下 C ˙ b n C b n ∗ ( ω n b b × ) \dot C_b^n C_b^n * (\omega_{nb}^b \times) C˙bnCbn∗(ωnbb×) 当考虑上地球转速以及导航系变化的角速度时 ω n b b ω i b b − ω i n b \omega_{nb}^b \omega_{ib}^b - \omega_{in}^b ωnbbωibb−ωinb上式可以进一步列写为 C ˙ b n C b n ∗ ( ω n b b × ) C b n [ ( ω i b b − ω i n b ) × ] C b n ( ω i b b × ) − ( ω i n n × ) C b n \dot C_b^n C_b^n * (\omega_{nb}^b \times) C_b^n[(\omega_{ib}^b - \omega_{in}^b)\times] C_b^n(\omega_{ib}^b \times) - (\omega_{in}^n\times)C_b^n C˙bnCbn∗(ωnbb×)Cbn[(ωibb−ωinb)×]Cbn(ωibb×)−(ωinn×)Cbn 对上式进行求微分方程并进行离散化就可以得到导航系下的姿态更新方法。但是求解比较麻烦可以采用矩阵链式法则进行求解 C b ( m ) n ( m ) C n ( m − 1 ) n ( m ) C i n ( m − 1 ) C b ( m − 1 ) i C b ( m ) b ( m − 1 ) C n ( m − 1 ) n ( m ) C b ( m − 1 ) n ( m − 1 ) C b ( m ) b ( m − 1 ) C_{b(m)}^{n(m)} C_{n(m-1)}^{n(m)}C_{i}^{n(m-1)}C_{b(m-1)}^i C_{b(m)}^{b(m-1)} C_{n(m-1)}^{n(m)} C_{b(m-1)}^{n(m-1)} C_{b(m)}^{b(m-1)} Cb(m)n(m)Cn(m−1)n(m)Cin(m−1)Cb(m−1)iCb(m)b(m−1)Cn(m−1)n(m)Cb(m−1)n(m−1)Cb(m)b(m−1) 其中 C n ( m − 1 ) n ( m ) C_{n(m-1)}^{n(m)} Cn(m−1)n(m) 表示m-1时刻到m时刻导航系发生的姿态变化也相当于在m时刻对应的导航系下表示m-1时刻导航系的位姿计算方法是使用 ω i n n ω i e n ω e n n \omega_{in}^n \omega_{ie}^n \omega_{en}^n ωinnωienωenn.
C b ( m − 1 ) n ( m − 1 ) C_{b(m-1)}^{n(m-1)} Cb(m−1)n(m−1) 表示m-1时刻IMU相对于导航系的姿态
C b ( m ) b ( m − 1 ) C_{b(m)}^{b(m-1)} Cb(m)b(m−1) 表示m时刻到m-1时刻IMU发生的位姿变化它来自于IMU角速度测量的积分。
c. 导航系简化版姿态计算 C b ( m ) n ( m ) C n ( m − 1 ) n ( m ) C b ( m − 1 ) n ( m − 1 ) C b ( m ) b ( m − 1 ) C_{b(m)}^{n(m)} C_{n(m-1)}^{n(m)} C_{b(m-1)}^{n(m-1)} C_{b(m)}^{b(m-1)} Cb(m)n(m)Cn(m−1)n(m)Cb(m−1)n(m−1)Cb(m)b(m−1) 在MEMS IMU中上式可以忽略导航系的运动 C n ( m − 1 ) n ( m ) C_{n(m-1)}^{n(m)} Cn(m−1)n(m)因为消费级普通IMU无法敏感到这一项所以可以忽略。于是可以得到简化版的姿态解算 C b ( m ) n ( m ) C b ( m − 1 ) n ( m − 1 ) C b ( m ) b ( m − 1 ) C_{b(m)}^{n(m)} C_{b(m-1)}^{n(m-1)} C_{b(m)}^{b(m-1)} Cb(m)n(m)Cb(m−1)n(m−1)Cb(m)b(m−1) 其中 C b ( m ) b ( m − 1 ) e ∫ t m − 1 t m ω i b b ( t ) d t C_{b(m)}^{b(m-1)} e^{\int_{t_{m-1}}^{t_m} \omega_{ib}^b(t) dt} Cb(m)b(m−1)e∫tm−1tmωibb(t)dt对于它的求解与前述思路一样可以采用欧拉积分中值积分等求解 ∫ t m − 1 t m ω i b b ( t ) d t {\int_{t_{m-1}}^{t_m} \omega_{ib}^b(t) dt} ∫tm−1tmωibb(t)dt然后使用罗德里格斯公式完成指数映射。
2. 速度更新 参考《捷联惯导算法与组合导航原理》 P81 a. 精确速度微分建模 v ˙ e n n C b n f s f b − 2 ( w i e n w e n n ) × v e n n g n \dot v_{en}^n C_b^n f_{sf}^b - 2(w_{ie}^nw_{en}^n) \times v_{en}^n g^n v˙ennCbnfsfb−2(wienwenn)×venngn 其中 f s f b f_{sf}^b fsfb 为加速度计测量的比力
− 2 ( w i e n w e n n ) × v e n n - 2(w_{ie}^nw_{en}^n) \times v_{en}^n −2(wienwenn)×venn 为有害加速度
g n g^n gn 为重力加速度
以上便是对导航系下载体速度的精确微分建模对其积分即可得到速度的解算具体过程比较复杂参考《捷联惯导算法与组合导航原理》 P82。 在IMU加速度误差的建模中使用的是加速度一词这里使用的是比力实际上可以认为它们是同样的意义。 b. 简化版速度微分建模
在MEMS IMU中有害加速度这一项可以忽略并忽略一些类似划桨误差等可以得到简化版的速度更新方程 v ˙ e n n C b n f s f b g n \dot v_{en}^n C_b^n f_{sf}^b g^n v˙ennCbnfsfbgn 上式求积分之后改写成递推的形式为 v m n ( m ) v m − 1 n ( m − 1 ) Δ v s f ( m ) n v_{m}^{n(m)} v_{m-1}^{n(m-1)} \Delta v_{sf(m)}^n vmn(m)vm−1n(m−1)Δvsf(m)n 其中 v m − 1 n ( m − 1 ) v_{m-1}^{n(m-1)} vm−1n(m−1)为m-1时刻载体速度在导航下的表示。 Δ v s f ( m ) n ∫ m − 1 m ( C b n f s f b g n ) d t \Delta v_{sf(m)}^n \int_{m-1}^m (C_b^n f_{sf}^b g^n)dt Δvsf(m)n∫m−1m(Cbnfsfbgn)dt 上式的求解可以采用欧拉积分中值积分等完成。
3. 位置更新
对于位置的更新在参考《捷联惯导算法与组合导航原理》 P87 中采用的是经纬高来表示位置。而实际使用中多以导航系原点来表示位置因此此处不总结介绍经纬高的方式。将直接在导航系下进行解算。 p ˙ n ( m ) v n ( m ) \dot p^{n(m)} v^{n(m)} p˙n(m)vn(m) 对上式进行欧拉积分即可得到递推方程 p n ( m ) p n ( m − 1 ) ∫ m − 1 m v n ( t ) d t p^{n(m)} p^{n(m-1)} \int_{m-1}^m v^{n}(t)dt pn(m)pn(m−1)∫m−1mvn(t)dt 同样的方式可以采用欧拉积分中值积分等对 ∫ m − 1 m v n ( t ) d t \int_{m-1}^m v^{n}(t)dt ∫m−1mvn(t)dt进行求解。
例如使用中值积分并假设m-1到m时刻速度为线性变化则有如下式 p n ( m ) p n ( m − 1 ) v n ( m − 1 ) ∗ Δ t 1 2 ( f s f n ( m − 1 ) g n ) Δ t 2 p^{n(m)} p^{n(m-1)} v^{n(m-1)}*\Delta t \frac{1}{2} (f_{sf}^{n(m-1)} g^n) \Delta t^2 pn(m)pn(m−1)vn(m−1)∗Δt21(fsfn(m−1)gn)Δt2 注意IMU的姿态解算过程比较复杂在建模时可以做的非常精确这会导致模型非常复杂实际当中使用时普通的MEMS IMU无需做复杂的建模不需要考虑地球转速导航系的运动速度划桨误差等等。当我们有了IMU初始时刻的姿态、速度、位置之后就可以使用上述的递推方式计算出后续的姿态变化。但是对于低精度的IMU这个递推的过程维持不了多久就会发散。
