方向余弦阵微分方程及其求解

1.方向余弦阵微分方程
\qquad假设bbb系与iii系具有共同的原点，bbb系相对于iii系转动的角速度为ωib\omega_{ib}ωib，从iii系到bbb系的坐标系变换矩阵记作CbiC_b^iCbi，为时变矩阵。假设rrr为iii系中一固定矢量，在矢量rrr在bbb系与iii系之间的转换关系为:ri=Cbirb(1.1)r^i=C_b^ir^b \tag{1.1}ri=Cbirb(1.1) \qquad将式(1.1)两边对时间微分得:r˙i=C˙birb+Cbir˙b(1.2)\dot{r}^i=\dot{C}_b^ir^b+C_b^i\dot{r}^b \tag{1.2}r˙i=C˙birb+Cbir˙b(1.2) \qquad 其中rrr为iii系中的固定矢量，则有r˙i=0\dot{r}^i=0r˙i=0。由于bbb系相对于iii系转动的角速度为ωib\omega_{ib}ωib，那么在bbb系上观察rrr的角速度应为−ωib-\omega_{ib}−ωib，且r˙b=−ωibb×rb\dot{r}^b=-\omega_{ib}^b\times r^br˙b=−ωibb×rb，则式(2)可表示为:0=C˙birb+Cbi(−ωibb×rb)即C˙birb=Cbi(ωibb×)rb(1.3)0=\dot{C}_b^ir^b+C_b^i(-\omega_{ib}^b\times r^b)即\ \dot{C}_b^ir^b=C_b^i(\omega_{ib}^b\times) r^b \tag{1.3}0=C˙birb+Cbi(−ωibb×rb)即 C˙birb=Cbi(ωibb×)rb(1.3) \qquad由于式(3)对iii系中任意固定矢量rrr均成立，则任选三个不共面得到向量r1,r2,r3r_1,r_2,r_3r1,r2,r3，则有:C˙bi[r1br2br3b]T=Cbi(ωibb×)[r1br2br3b]T(1.4)\dot{C}_b^i\begin{bmatrix}r_1^b&r_2^b&r_3^b \end{bmatrix}^T=C_b^i(\omega_{ib}^b\times) \begin{bmatrix}r_1^b&r_2^b&r_3^b \end{bmatrix}^T \tag{1.4}C˙bi[r1br2br3b]T=Cbi(ωibb×)[r1br2br3b]T(1.4) \qquad显然矩阵[r1br2br3b]T\begin{bmatrix}r_1^b&r_2^b&r_3^b \end{bmatrix}^T[r1br2br3b]T可逆。所以总有:C˙bi=Cbi(ωib×)(1.5)\dot{C}_b^i=C_b^i(\omega_{ib}\times) \tag{1.5}C˙bi=Cbi(ωib×)(1.5) \qquad式(5)即方向余弦微分方程，或称姿态阵微分方程，它建立了动坐标系与参考坐标之间的方向余弦阵与动坐标系运动角速度之间的关系。
\qquad 矢量rrr在bbb系与iii中的线速度关系可表示为:ωibi×ri=Cbi(ωibb×rb)(1.6)\omega_{ib}^i\times r^i=C_b^i(\omega_{ib}^b\times r^b) \tag{1.6}ωibi×ri=Cbi(ωibb×rb)(1.6)
\qquad进一步可推出:ωibi×ri=Cbi(ωibb×rb)=Cbi(ωibb×)rb=Cbi(ωibb×)Cibri(1.7)\omega_{ib}^i\times r^i=C_b^i(\omega_{ib}^b\times r^b)=C_b^i (\omega_{ib}^b\times) r^b= C_b^i (\omega_{ib}^b\times) C_i^b r^i\tag{1.7}ωibi×ri=Cbi(ωibb×rb)=Cbi(ωibb×)rb=Cbi(ωibb×)Cibri(1.7) \qquad则有:(ωibi×)=Cbi(ωibb×)Cib(1.8)(\omega_{ib}^i\times)=C_b^i (\omega_{ib}^b\times) C_i^b \tag{1.8}(ωibi×)=Cbi(ωibb×)Cib(1.8) \qquad CbiC_b^iCbi为单位正交矩阵，可知(Cbi)−1=(Cbi)T=Cib(C_b^i)^{-1}=(C_b^i)^T=C_i^b(Cbi)−1=(Cbi)T=Cib。根据式(5)和式(8)可得，以下四种方向余弦阵微分方程相互等价:C˙bi=Cbi(ωibb×)(1.9)\dot{C}_b^i=C_b^i(\omega_{ib}^b\times) \tag{1.9}C˙bi=Cbi(ωibb×)(1.9) C˙bi=(ωibi×)Cbi(1.10)\dot{C}_b^i=(\omega_{ib}^i\times)C_b^i \tag{1.10}C˙bi=(ωibi×)Cbi(1.10) C˙ib=(ωibb×)Cib(1.11)\dot{C}_i^b=(\omega_{ib}^b\times)C_i^b \tag{1.11}C˙ib=(ωibb×)Cib(1.11) C˙ii=Cib(ωibi×)(1.12)\dot{C}_i^i=C_i^b (\omega_{ib}^i\times) \tag{1.12}C˙ii=Cib(ωibi×)(1.12)

2.方向余弦阵微分方程求解
\qquad为了方便书写及运算推导，略去复杂的上下标，记反对称矩阵Ω(t)=[ωibb(t)×]\Omega(t)=[\omega_{ib}^b(t)\times]Ω(t)=[ωibb(t)×]，姿态阵微分方程表示为:C˙(t)=C(t)Ω(t)(2.1)\dot{C}(t)=C(t)\Omega(t) \tag{2.1}C˙(t)=C(t)Ω(t)(2.1) \qquad首先对式(2.1)在时间区间[0,t][0,t][0,t]上积分，有:C(t)=C(0)+∫0tC(τ)Ω(τ)dτ(2.2)C(t)=C(0)+\int_0^tC(\tau)\Omega(\tau)d\tau \tag{2.2}C(t)=C(0)+∫0tC(τ)Ω(τ)dτ(2.2) \qquad由于式(2.2)右侧被积函数中依然含有待求项C(t)C(t)C(t)，将式(2.2)重复带入积分号内，第一次带入有:C(t)=C(0)+∫0t[C(0)+∫0τC(τ1)Ω(τ1)dτ1]Ω(τ)dτ=C(0)[I+∫0tΩ(τ)dτ]+∫0t∫0τC(τ1)Ω(τ1)dτ1Ω(τ)dτ(2.3)\begin{aligned}C(t)&=C(0)+\int_0^t[C(0)+\int_0^\tau C(\tau_1)\Omega(\tau_1)d\tau_1]\Omega(\tau)d\tau \\ &=C(0)[I+\int_0^t\Omega(\tau)d\tau]+\int_0^t\int_0^\tau C(\tau_1)\Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau \end{aligned} \tag{2.3}C(t)=C(0)+∫0t[C(0)+∫0τC(τ1)Ω(τ1)dτ1]Ω(τ)dτ=C(0)[I+∫0tΩ(τ)dτ]+∫0t∫0τC(τ1)Ω(τ1)dτ1Ω(τ)dτ(2.3) \qquad第二次带入有:C(t)=C(0)[I+∫0tΩ(τ)dτ+∫0t∫0τΩ(τ1)dτ1Ω(τ)dτ]+∫0t∫0τ∫0τ1C(τ2)Ω(τ2)dτ2Ω(τ1)dτ1Ω(τ)dτ(2.4)C(t)=C(0)[I+\int_0^t\Omega(\tau)d\tau+\int_0^t\int_0^\tau \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau ]\\+\int_0^t\int_0^\tau \int_0^{\tau_1} C(\tau_2)\Omega(\tau_2)d\tau_2 \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau \tag{2.4}C(t)=C(0)[I+∫0tΩ(τ)dτ+∫0t∫0τΩ(τ1)dτ1Ω(τ)dτ]+∫0t∫0τ∫0τ1C(τ2)Ω(τ2)dτ2Ω(τ1)dτ1Ω(τ)dτ(2.4) \qquad经过不断迭代，可得到无限重积分，即毕卡级数:C(t)=C(0)[I+∫0tΩ(τ)dτ+∫0t∫0τΩ(τ1)dτ1Ω(τ)dτ]+∫0t∫0τ∫0τ1C(τ2)Ω(τ2)dτ2Ω(τ1)dτ1Ω(τ)dτ+……](2.5)C(t)=C(0)[I+\int_0^t\Omega(\tau)d\tau+\int_0^t\int_0^\tau \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau ]\\+\int_0^t\int_0^\tau \int_0^{\tau_1} C(\tau_2)\Omega(\tau_2)d\tau_2 \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau +…… ] \tag{2.5}C(t)=C(0)[I+∫0tΩ(τ)dτ+∫0t∫0τΩ(τ1)dτ1Ω(τ)dτ]+∫0t∫0τ∫0τ1C(τ2)Ω(τ2)dτ2Ω(τ1)dτ1Ω(τ)dτ+……](2.5) \qquad 式(2.5)所示级数显然是收敛的，但一般情况下无法得出闭合解。
\qquad 假设对于任意时间t,τ∈[0,T]t,\tau \in[0,T]t,τ∈[0,T]，转动角速度满足以下可交换条件:Ω(t)Ω(τ)=Ω(τ)Ω(t)(2.6)\Omega(t)\Omega(\tau)=\Omega(\tau)\Omega(t) \tag{2.6}Ω(t)Ω(τ)=Ω(τ)Ω(t)(2.6) \qquad 则有∫0tΩ(t)Ω(τ)dτ=∫0tΩ(τ)Ω(t)dτ⇒Ω(t)∫0tΩ(τ)dτ=∫0tΩ(τ)dτΩ(t)(2.7)\int_0^t\Omega(t)\Omega(\tau)d\tau=\int_0^t\Omega(\tau)\Omega(t)d\tau \rArr \Omega(t)\int_0^t\Omega(\tau)d\tau=\int_0^t\Omega(\tau)d\tau\Omega(t) \tag{2.7}∫0tΩ(t)Ω(τ)dτ=∫0tΩ(τ)Ω(t)dτ⇒Ω(t)∫0tΩ(τ)dτ=∫0tΩ(τ)dτΩ(t)(2.7) \qquad 在式(2.6)成立的条件下，对下列微分有:ddt[∫0tΩ(τ)dτ]2=ddt[(∫0tΩ(τ)dτ)⋅(∫0tΩ(τ)dτ)]=Ω(t)∫0tΩ(τ)dτ+∫0tΩ(τ)dτΩ(t)=2∫0tΩ(τ)dτΩ(t)(2.8)\begin{aligned} \frac{d}{dt}[\int_0^t\Omega(\tau)d\tau]^2&=\frac{d}{dt}[(\int_0^t\Omega(\tau)d\tau)\cdot(\int_0^t\Omega(\tau)d\tau)]\\ &=\Omega(t)\int_0^t\Omega(\tau)d\tau+\int_0^t\Omega(\tau)d\tau\Omega(t) \\&=2\int_0^t\Omega(\tau)d\tau\Omega(t) \end{aligned} \tag{2.8}dtd[∫0tΩ(τ)dτ]2=dtd[(∫0tΩ(τ)dτ)⋅(∫0tΩ(τ)dτ)]=Ω(t)∫0tΩ(τ)dτ+∫0tΩ(τ)dτΩ(t)=2∫0tΩ(τ)dτΩ(t)(2.8) \qquad 由式(2.8)可知:∫0t∫0τΩ(τ1)dτ1Ω(τ)dτ=12[∫0tΩ(τ)dτ]2(2.9)\int_0^t\int_0^\tau \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau =\frac{1}{2}[\int_0^t\Omega(\tau)d\tau]^2 \tag{2.9}∫0t∫0τΩ(τ1)dτ1Ω(τ)dτ=21[∫0tΩ(τ)dτ]2(2.9) \qquad同理有:∫0t∫0τ∫0τ1C(τ2)Ω(τ2)dτ2Ω(τ1)dτ1Ω(τ)dτ=16[∫0tΩ(τ)dτ]3(2.10)\int_0^t\int_0^\tau \int_0^{\tau_1} C(\tau_2)\Omega(\tau_2)d\tau_2 \Omega(\tau_1)d\tau_1 \Omega(\tau)d\tau =\frac{1}{6}[\int_0^t\Omega(\tau)d\tau]^3 \tag{2.10}∫0t∫0τ∫0τ1C(τ2)Ω(τ2)dτ2Ω(τ1)dτ1Ω(τ)dτ=61[∫0tΩ(τ)dτ]3(2.10) \qquad在满足式(2.6)的可交换条件下，式(2.5)所示毕卡级数可化为闭合解形式:C(t)=C(0)[I+∫0tΩ(τ)dτ+12!(∫0tΩ(τ)dτ)2+13![∫0tΩ(τ)dτ]3+……]=C(0)e∫0tΩ(τ)dτ(2.11)\begin{aligned}C(t)&=C(0)[I+\int_0^t\Omega(\tau)d\tau+\frac{1}{2!}(\int_0^t\Omega(\tau)d\tau)^2+\frac{1}{3!}[\int_0^t\Omega(\tau)d\tau]^3+……] \\ &=C(0)e^{\int_0^t\Omega(\tau)d\tau}\end{aligned} \tag{2.11}C(t)=C(0)[I+∫0tΩ(τ)dτ+2!1(∫0tΩ(τ)dτ)2+3!1[∫0tΩ(τ)dτ]3+……]=C(0)e∫0tΩ(τ)dτ(2.11) \qquad对于时间区间[0,T][0,T][0,T]，记角度增量θ(T)=∫0Tω(τ)dτ\theta(T)=\int_{0}^T\omega(\tau)d\tauθ(T)=∫0Tω(τ)dτ且模值θ(T)=∣θ(T)∣\theta(T)=|\theta(T)|θ(T)=∣θ(T)∣，则有角度增量的矩阵指数函数为:e∫0TΩ(τ)dτ=e(θ(T)×)=I+sinθ(T)θ(T)[θ(T)×]+1−cosθ(T)θ(T)[θ(T)×]2(2.12)e^{\int_{0}^T\Omega(\tau)d\tau}=e^{(\theta(T)\times)}=I+\frac{sin\theta(T)}{\theta(T)}[\theta(T)\times]+\frac{1-cos\theta(T)}{\theta(T)}[\theta(T)\times]^2 \tag{2.12}e∫0TΩ(τ)dτ=e(θ(T)×)=I+θ(T)sinθ(T)[θ(T)×]+θ(T)1−cosθ(T)[θ(T)×]2(2.12) \qquad因此式(2.11)在TTT时刻的解为:C(t)=C(0)CT0(2.13)C(t)=C(0)C_T^0 \tag{2.13}C(t)=C(0)CT0(2.13) \qquad其中 CT0=I+sinθ(T)θ(T)[θ(T)×]+1−cosθ(T)θ(T)[θ(T)×]2(2.14)C_T^0 =I+\frac{sin\theta(T)}{\theta(T)}[\theta(T)\times]+\frac{1-cos\theta(T)}{\theta(T)}[\theta(T)\times]^2 \tag{2.14}CT0=I+θ(T)sinθ(T)[θ(T)×]+θ(T)1−cosθ(T)[θ(T)×]2(2.14) \qquad则对于时间区间[tm−1,tm][t_{m-1},t_m][tm−1,tm]，假设tm−1t_{m-1}tm−1时刻的方向余弦矩阵为Cb(m−1)iC_{b(m-1)}^iCb(m−1)i，[tm−1,tm][t_{m-1},t_m][tm−1,tm]时间段内的角度增量为Δθm=∫tm−1tmωibb(t)dt\Delta\theta_m=\int_{t_{m-1}}^{t_m}\omega_{ib}^b(t)dtΔθm=∫tm−1tmωibb(t)dt且记模值Δθm=∣Δθm∣\Delta\theta_m=|\Delta\theta_m|Δθm=∣Δθm∣，则tmt_mtm时刻的姿态阵Cb(m)iC_{b(m)}^iCb(m)i为:Cb(m)i=Cb(m−1)iCb(m)b(m−1)(2.15)C_{b(m)}^i=C_{b(m-1)}^iC_{b(m)}^{b(m-1)} \tag{2.15}Cb(m)i=Cb(m−1)iCb(m)b(m−1)(2.15) Cb(m)b(m−1)=I+sinΔθmΔθm[Δθm×]+1−cosΔθmΔθm[Δθm×]2(2.16)C_{b(m)}^{b(m-1)}=I+\frac{sin\Delta\theta_m}{\Delta\theta_m}[\Delta\theta_m\times]+\frac{1-cos\Delta\theta_m}{\Delta\theta_m}[\Delta\theta_m\times]^2 \tag{2.16}Cb(m)b(m−1)=I+ΔθmsinΔθm[Δθm×]+Δθm1−cosΔθm[Δθm×]2(2.16) \qquad式(2.15)和式(12.6)即姿态阵离散化更新递推公式，但式(16)成立的前提是系统在[tm−1,tm][t_{m-1},t_m][tm−1,tm]时间段内做定轴转动。对比式(2.13)和式(2.16)，可以发现二者在形式上完全一致，因而定轴转动的角增量Δθm\Delta\theta_mΔθm是以btm−1b_{t_{m-1}}btm−1系为参考，btmb_{t_m}btm系相对于btm−1b_{t_{m-1}}btm−1系转动的等效旋转矢量。当式(2.6)可交换性条件不成立时，如果我们依然简单的利用式(2.16)进行计算就会引起姿态求解的不可交换误差。对于非定轴转动条件下的姿态更新，主要通过等效旋转矢量来进行不可交换误差的补偿。

参考文献
[1] 严恭敏, 翁浚. 捷联惯导算法与组合导航原理[M]. 西安:西北工业大学出版社, 2020.

方向余弦阵微分方程及其求解相关推荐

机器学习（MACHINE LEARNING）MATLAB中微分方程的求解
文章目录 1 MATLAB之极限.积分.微分 2 matlab中微分方程的求解 2.1 一阶微分方程 2.2 求解二阶线性微分方程是指含有未知函数及其导数的关系式.解微分方程就是找出未知函数.微分方 ...
信号与系统 chapter9 关于信号与系统中微分方程的求解
微分方程的求解许多同学之所以觉得信号与系统难的原因之一就在于它的数学推导,特别是对于一些高等数学基础比较薄弱的同学来说,且不说后面的求解傅里叶变换部分,目前的LTI连续系统微分方程的求解,已经是的有 ...
matlab求解f非线性微分方程数值解,非线性﹑微分方程数值求解.PPT
非线性﹑微分方程数值求解数学系 University of Science and Technology of China DEPARTMENT OF MATHEMATICS 计算方法(B) 主 ...
Matlab微分方程的求解
Matlab微分方程的求解求解常微分方程的通解求解常微分方程的初边值问题求解常微分方程组: 求解常微分方程的通解试解常微分方程: x2+y+(x−2y)y′=0x^2+y+\left( x-2 ...
matlab微分方程求法,matlab微分方程的求解的方法ppt课件
<matlab微分方程的求解的方法ppt课件>由会员分享,可在线阅读,更多相关<matlab微分方程的求解的方法ppt课件(44页珍藏版)>请在人人文库网上搜索. 1.定义:含 ...
【Matlab 控制】微分方程 ode45() 求解并绘制曲线
Matlab 微分方程 ode45 求解并绘制曲线 2. 用 ode45() 求解 2.1 ode45() 函数用法 2.2 示例:求解一阶微分方程 2.2.1 Matlab 代码如下 2.2.2 代 ...
关于线性微分方程的求解（常数变易法）
关于线性微分方程的求解 1.1 线性方程首先讲一下什么叫线性方程,含有变量的最高次幂不超过1次的方程,允许0次的存在 . eg. ax+by+cz+d=0; @线性方程的本质是等式两边乘以任何相同的 ...
线性非齐次微分方程的求解套路
[第一步]列出微分方程的特征方程 [第二步]求取特征方程的特征根 [第三步]根据特征根写出微分方程的通解 [第四步]写出微分方程的特解 [第五步]写出微分方程的完全响应表达式 [第六步]将完全解分别代 ...
【信号与系统】3.1系统的微分方程及其求解
微分方程的建立一般形式根据力学.电学等物理学规律,得到输入和输出之间的数学表达式,即列处描述系统特性的微分方程. 一个nnn阶连续时间系统可以用nnn阶微分方程来描述,其一般形式为 y(n)(t) ...

方向余弦阵微分方程及其求解

方向余弦阵微分方程及其求解相关推荐

最新文章

热门文章