自用的姿态与轨道动力学笔记

圆锥曲线轨道

轨道六根数指：
半长轴(a)、偏心率(e)，轨道倾角(i)，升交点赤经($\Omega$)、近地点幅角($\omega$)、真近点角($\varphi$)。
其它参数：
角动量 $\vec{h}$、半通径 $p$、远拱点 $r_a$、周期 $T$。
$$\begin{array}{rl}\mathcal{E}=& -\frac{\mu }{2a}=\frac{v^2}{2}-\frac{\mu }{r}=\frac{v_{inf}^2}{2}\\ \vec{h}=& \vec{r}\times \vec{v}\\ |\vec{h}|=& |\vec{r}||\vec{v}|\sin \theta \\ a=& \frac{p}{1-e^2}=-\frac{\mu }{2\mathcal{E}}=\frac{\mu r}{2\mu -r{v}^2}\\ p=& \frac{h^2}{\mu }\\ \vec{e}=& \frac{\vec{v}\times \vec{h}}{\mu }-\frac{\vec{r}}{|\vec{r}|}=(\frac{v^2}{\mu }-\frac{1}{r})\vec{r}-\frac{\vec{v}\cdot \vec{r}}{\mu }\vec{v}\\ e=& \sqrt{1-\frac{h^2}{a\mu }}\\ r=& \frac{p}{1+e\cos f}\\ f=& \arccos (\frac{1}{e}(\frac{h^2}{\mu r}-1))\\ r_a=& a(1+e)\\ T=& 2\pi \sqrt{\frac{a^3}{\mu }}\end{array}$$
由轨道元素计算位置和速度向量
$$\begin{array}{rl}R=& \left[\begin{array}{ccc}{c}_{\Omega }{c}_{\omega }-s_{\Omega }{c}_i{s}_{\omega }& -c_{\Omega }{s}_{\omega }-s_{\Omega }{c}_i{c}_{\omega }& s_{\Omega }{s}_i\\ s_{\Omega }{c}_{\omega }+c_{\Omega }{c}_i{s}_{\omega }& -s_{\Omega }{s}_{\omega }+c_{\Omega }{c}_i{c}_{\omega }& -c_{\Omega }{s}_i\\ s_i{s}_{\omega }& s_i{c}_{\omega }& c_i\end{array}\right]\\ \vec{r}=& R\left[\begin{array}{c}\frac{a(1-e^2)}{1+e\cos \theta }\cos \theta \\ \frac{a(1-e^2)}{1+e\cos \theta }\sin \theta \\ 0\end{array}\right]\\ \vec{v}=& \sqrt{\frac{\mu }{a(1-e^2)}}R\left[\begin{array}{c}-\sin \theta \\ e+\cos \theta \\ 0\end{array}\right]\end{array}$$
平近点角($M$)，真近点角($\theta$)，偏近点角($E$)
$$\begin{array}{rl}& E-e\sin E=M\\ & \frac{\tan \frac{E}{2}}{\tan \frac{\theta }{2}}=\sqrt{\frac{1-e}{1+e}}\end{array}$$
根据平近点角计算真近点角
$$\begin{array}{rl}f=& M+\left(2e-\frac{1}{4}{e}^3\right)\sin M+\frac{5}{4}{e}^2\sin 2M\\ & +\frac{13}{12}{e}^3\sin 3M+O(e^4)\end{array}$$
已知 $\mu$、初始位置 $\vec{r}$ 和速度 $\vec{v}$ 计算经过指定时间 $T_0$ 后的位置和速度

1. 根据初始位置速度计算轨道能量 $\mathcal{E}$ 和偏心率 $\vec{e}$
2. 根据 $\mu$ 和 $\mathcal{E}$ 计算半长轴 $a$ 进而计算轨道周期 $T$
3. 根据 $T_0$ 和 $T$ 计算平近点角进而计算偏近点角 $E$
4. 根据 $\vec{e}$ 计算半短轴 $b$ 和焦距 $c$，由椭圆参数方程得到 $\vec{r}$
5. 由 $\mathcal{E}$ 计算速度大小 $|\vec{v}|$
6. 由偏心率公式计算速度 $\vec{v}$

$$\begin{array}{rl}& \left[\begin{array}{c}e\\ 0\\ 0\end{array}\right]=(\frac{\vec{v}\times \vec{h}}{\mu }-\frac{\vec{r}}{|\vec{r}|})\left[\begin{array}{c}{r}_x\\ r_y\\ 0\end{array}\right]-\frac{v_x{r}_x+v_y{r}_y}{\mu }\left[\begin{array}{c}{v}_x\\ v_y\\ 0\end{array}\right]\\ & k=\frac{\vec{v}\times \vec{h}}{\mu }-\frac{\vec{r}}{|\vec{r}|}\\ & r_x{v}_x^2+r_y{v}_x{v}_y=\mu (k{r}_x-e)\\ & r_y{v}_y^2+r_x{v}_x{v}_y=\mu k{r}_y\\ & r_x^2{v}_x^2+r_x{r}_y{v}_x{v}_y=\mu (k{r}_x-e)r_x\\ & r_y^2{v}_y^2+r_x{r}_y{v}_x{v}_y=\mu k{r}_y{r}_y\\ & r_x^2{v}_x^2-r_y^2{v}_y^2=\mu (k{r}_x-e)r_x-\mu k{r}_y{r}_y=k_1\\ & v_x^2+v_y^2=v^2\\ & v_x^2=\frac{r_y^2{v}^2+k_1}{r^2}\\ & v_y^2=\frac{r_x^2{v}^2-k_1}{r^2}\\ & k_1=\mu k(r_x^2-r_y^2)-e{r}_x=(v^2-\frac{\mu }{r})(r_x^2-r_y^2)-\mu e{r}_x\end{array}$$

四元数、欧拉角、旋转矩阵

四元数、欧拉角、旋转矩阵笔记 -CSDN博客

角速度公式

设一向量 $\vec{x}(t)$ 绕旋转轴 $\vec{\omega }$ 作匀速圆周运动，
则$\vec{x}(t)$的线速度为
$$\dot{\vec{x}}(t)=\vec{\omega }\times \vec{x}(t)$$
证明： 由罗德里格斯(Rodrigues)旋转公式
$$R=\cos \theta I+(1-\cos \theta )\vec{n}{\vec{n}}^{\text{T}}+\sin \theta {\vec{n}}^{\wedge }$$
其中
$R$为旋转矩阵，
$n$为单位旋转轴，
$\theta$为旋转角度,
${\vec{n}}^{\wedge }$ 表示向量 $\vec{n}$ 叉乘对应的反对称矩阵。
将向量 $\vec{\omega }$ 写成 $\vec{\omega }=\omega \vec{n}$，
并将旋转矩阵对时间$t$求导得到
$$\begin{array}{rl}\frac{\text{d}}{\text{d}t}R(t)=& \frac{\text{d}}{\text{d}t}\left(\cos \omega tI+(1-\cos \omega t)\frac{\vec{\omega }}{\omega }\frac{{\vec{\omega }}^{\text{T}}}{\omega }+\sin \omega t\frac{{\vec{\omega }}^{\wedge }}{\omega }\right)\\ =& -\omega \sin \omega tI+\frac{\sin \omega t}{\omega }\vec{\omega }{\vec{\omega }}^{\text{T}}+\cos \omega t\cdot {\vec{\omega }}^{\wedge }\\ \dot{R}(t){\vec{x}}_0=& -\omega \sin \omega t{\vec{x}}_0+\frac{\sin \omega t}{\omega }{\vec{\omega }}^{\text{T}}{\vec{x}}_0\vec{\omega }+\cos \omega t\cdot \vec{\omega }\times {\vec{x}}_0\end{array}$$
另因为
$$\begin{array}{rl}\vec{\omega }\times R(t){\vec{x}}_0=& \cos \omega t\cdot \vec{\omega }\times {\vec{x}}_0+\frac{1-\cos \omega t}{{\omega }^2}\vec{\omega }\times \vec{\omega }{\vec{\omega }}^{\text{T}}{\vec{x}}_0\\ & +\frac{\sin \omega t}{\omega }\vec{\omega }\times (\vec{\omega }\times {\vec{x}}_0)\\ =& \cos \omega t\cdot \vec{\omega }\times {\vec{x}}_0+\frac{\sin \omega t}{\omega }({\vec{\omega }}^{\text{T}}{\vec{x}}_0\vec{\omega }-{\vec{\omega }}^{\text{T}}\vec{\omega }{\vec{x}}_0)\end{array}$$
所以
$$\dot{\vec{x}}(t)=\dot{R}(t){\vec{x}}_0=\vec{\omega }\times R(t){\vec{x}}_0=\vec{\omega }\times \vec{x}(t)$$

旋转坐标系下的速度和加速度

旋转坐标系 ${\mathcal{F}}_2$ 绕惯性坐标系 ${\mathcal{F}}_1$ 以角速度 $\vec{\omega }$ 旋转，
设 ${\mathcal{F}}_1$ 下位置向量的一阶导和二阶导分别为
$\dot{\vec{r}}$和$\ddot{\vec{r}}$，
${\mathcal{F}}_2$下位置向量的一阶导和二阶导为
$\stackrel{\circ }{\vec{r}}$和$\stackrel{\circ \circ }{\vec{r}}$，
满足
$$\begin{array}{rl}\dot{\vec{r}}=& \stackrel{\circ }{\vec{r}}+\vec{\omega }\times \vec{r}\\ \ddot{\vec{r}}=& \stackrel{\circ \circ }{\vec{r}}+2\vec{\omega }\times \stackrel{\circ }{\vec{r}}+\stackrel{\circ }{\vec{\omega }}\times \vec{r}+\vec{\omega }\times (\vec{\omega }\times \vec{r})\end{array}$$
证明：
设同一向量 $\vec{r}$ 在坐标系 ${\mathcal{F}}_1$ 和 ${\mathcal{F}}_2$ 下的坐标分别为
$$\vec{r}=\left[\begin{array}{ccc}{\vec{e}}_x& {\vec{e}}_y& {\vec{e}}_z\end{array}\right]\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]=\left[\begin{array}{ccc}{\vec{a}}_x& {\vec{a}}_y& {\vec{a}}_z\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]$$
其中 $[{\vec{e}}_x{\vec{e}}_y{\vec{e}}_z]$ 表示惯性坐标系 ${\mathcal{F}}_1$ 下的三轴单位向量，$[{\vec{a}}_x{\vec{a}}_y{\vec{a}}_z]$ 表示坐标系 ${\mathcal{F}}_2$ 的三轴单位向量在惯性坐标系 ${\mathcal{F}}_1$ 下的坐标，
三个单位向量张成旋转坐标系 ${\mathcal{F}}_2$ ，
则向量 $\vec{r}$ 的一阶导
$$\begin{array}{rl}\dot{\vec{r}}=& \left[\begin{array}{ccc}{\dot{\vec{a}}}_x& {\dot{\vec{a}}}_y& {\dot{\vec{a}}}_z\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]+\left[\begin{array}{ccc}{\vec{a}}_x& {\vec{a}}_y& {\vec{a}}_z\end{array}\right]\left[\begin{array}{c}{\dot{x}}_2\\ {\dot{y}}_2\\ {\dot{z}}_2\end{array}\right]\\ =& \vec{\omega }\times \left[\begin{array}{ccc}{\vec{a}}_x& {\vec{a}}_y& {\vec{a}}_z\end{array}\right]\left[\begin{array}{c}{x}_2\\ y_2\\ z_2\end{array}\right]+\stackrel{\circ }{\vec{r}}\\ =& \vec{\omega }\times \vec{r}+\stackrel{\circ }{\vec{r}}\end{array}$$
使用坐标系的记法写作
$$\begin{array}{rl}\dot{\vec{r}}=& \frac{\text{d}}{\text{d}t}({\mathcal{F}}_2{\vec{r}}_2)\\ =& {\mathcal{F}}_2{\dot{\vec{r}}}_2+\dot{{\mathcal{F}}_2}{\vec{r}}_2\\ =& {\mathcal{F}}_2{\dot{\vec{r}}}_2+\vec{\omega }\times {\mathcal{F}}_2{\vec{r}}_2\\ =& {\mathcal{F}}_2{\dot{\vec{r}}}_2+\vec{\omega }\times {\mathcal{F}}_1{\vec{r}}_1\end{array}$$
对上式进一步求导得
$$\begin{array}{rl}\ddot{\vec{r}}=& \vec{\omega }\times {\mathcal{F}}_2{\dot{\vec{r}}}_2+{\mathcal{F}}_2{\ddot{\vec{r}}}_2+\dot{\vec{\omega }}\times {\mathcal{F}}_1{\vec{r}}_1\\ & +\vec{\omega }\times (\vec{\omega }\times {\mathcal{F}}_2{\vec{r}}_2+{\mathcal{F}}_2{\dot{\vec{r}}}_2)\\ =& {\mathcal{F}}_2{\ddot{\vec{r}}}_2+2\vec{\omega }\times {\mathcal{F}}_2{\dot{\vec{r}}}_2\\ & +\dot{\vec{\omega }}\times {\mathcal{F}}_1{\vec{r}}_1+\vec{\omega }\times (\vec{\omega }\times {\mathcal{F}}_1{\vec{r}}_1)\\ =& \stackrel{\circ \circ }{\vec{r}}+2\vec{\omega }\times \stackrel{\circ }{\vec{r}}+\dot{\vec{\omega }}\times \vec{r}+\vec{\omega }\times (\vec{\omega }\times \vec{r})\end{array}$$
其中 $\vec{\omega }$ 在两个坐标系下的坐标相等，
即 $\dot{\vec{\omega }}=\stackrel{\circ }{\vec{\omega }}$ 。

刚体动力学中的欧拉方程

【班门弄斧系列】分析力学/进阶力学 01 多体系统
刚体动力学中的欧拉方程是如何推导出来的？-知乎
惯性矩、惯性积、转动惯量、惯性张量 -博客园
Euler-Lagrange equations for free rigid body -stackexchange
Jacobian of Euler’s rotation equations -stackexchange
4.5: Euler’s Equations of Motion -libretexts
13.17: Euler’s equations of motion for rigid-body rotation -libretexts
Euler’s equations (rigid body dynamics)
定点转动的欧拉动力学方程

离散形式下，
动量 ${\vec{P}}_a=m_a{\dot{\vec{r}}}_a$，
角动量 ${\vec{L}}_a={\vec{r}}_a\times {\vec{P}}_a$，
质心(Center of Mass, COM) $\vec{R}=\frac{1}{M}\sum _a{m}_a{\vec{r}}_a$，
总动量 $\vec{P}=M\dot{\vec{R}}=\sum _a{\vec{P}}_a$。
连续形式下，
总质量 $M=\iiint \rho (\vec{r})\text{d}v$，
质心 $\vec{R}=\frac{1}{M}\iiint \rho (\vec{r})\vec{r}\text{d}v$，
角动量 ${\vec{L}}_a=\iiint \rho (\vec{r})\vec{r}\times \dot{\vec{r}}\text{d}v$，
刚体的运动可以分解为刚体质心的运动和刚体相对于质心的运动，即 $\vec{r}=\vec{R}+{\vec{r}}^{\ast }$，其中 $\vec{R}$ 和 ${\vec{r}}^{\ast }$ 分别为质心坐标和刚体上每个点的坐标， ${\vec{r}}^{\ast }$ 满足 $\sum m{\vec{r}}^{\ast }=0$，或 $\iiint \rho ({\vec{r}}^{\ast }){\vec{r}}^{\ast }{\text{d}}^3{\vec{r}}^{\ast }=0$。
$$\begin{array}{rl}\vec{L}=& {\int }_v\rho (\vec{r})\vec{r}\times \dot{\vec{r}}\text{d}v\\ =& {\int }_v\text{d}v\left[\rho (\vec{r})\cdot (\vec{R}+{\vec{r}}^{\ast })\times (\dot{\vec{R}}+\dot{{\vec{r}}^{\ast }})\right]\\ =& {\int }_v\text{d}v\cdot \rho (\vec{r})\left[(\vec{R}\times \dot{\vec{R}})+({\vec{r}}^{\ast }\times \dot{{\vec{r}}^{\ast }})+(\vec{R}\times \dot{{\vec{r}}^{\ast }})+({\vec{r}}^{\ast }\times \dot{\vec{R}})\right]\\ =& {\int }_v\text{d}v\cdot \rho (\vec{r})(\vec{R}\times \dot{\vec{R}})+{\int }_v\text{d}v\cdot \rho (\vec{r})({\vec{r}}^{\ast }\times \dot{{\vec{r}}^{\ast }})\\ =& {\vec{L}}_{COM}+{\vec{L}}_{COM}^{\ast }\end{array}$$

两轴力矩控制三轴姿态

对被控对象
$$J\dot{\vec{\omega }}+\vec{\omega }\times J\vec{\omega }=H$$

QUEST 算法

  QUEST（QUaternion ESTimator）已知多个参考向量 $y_k$ 和对应的多个观测向量 $x_k$，求旋转矩阵 $A$ 使得 $y=Rx$ 的误差最小，即最小化指标函数
$$J_0=\frac{1}{2}\sum _{k=1}^n{\alpha }_k|y_k-A{x}_k{|}^2$$
或者最大化指标函数
$$J=\sum _{k=1}^n{\alpha }_k{y}_k^{\text{T}}A{x}_k$$
其中 $y_k$ 和 $x_k$ 均为单位向量，${\alpha }_k\ge 0$ 为加权因子，$\sum {\alpha }_k=1$，并且 $J=1-J_0$。

一组数据的情况

  首先研究一个向量 $y=Ax$，设 $B=y{x}^{\text{T}}$，利用关于矩阵的迹的两个公式
$$\begin{gathered} x^{\text{T}}y=\text{tr}(y{x}^{\text{T}})=\text{tr}(x{y}^{\text{T}}) \\ \text{tr}(AB)=\text{tr}(BA) \end{gathered}$$
则
$$\begin{array}{r}J=y^{\text{T}}Ax=\text{tr}(y(Ax{)}^{\text{T}})=\text{tr}(y{x}^{\text{T}}{A}^{\text{T}})=\text{tr}(B{A}^{\text{T}})=\text{tr}(A{B}^{\text{T}})\end{array}$$
将四元数与旋转矩阵的关系 (四元数、欧拉角、旋转矩阵笔记)
$$R=(w^2-q_v^{\text{T}}{q}_v)I+2q_v{q}_v^{\text{T}}-2w{q}_v^{\wedge }$$
代入 $J$ 得
$$\begin{array}{rl}J=& \text{tr}([(w^2-q_v^{\text{T}}{q}_v)I+2q_v{q}_v^{\text{T}}-2w{q}_v^{\wedge }]B^{\text{T}})\\ =& (w^2-q_v^{\text{T}}{q}_v)\text{tr}(B)+2\text{tr}(q_v{q}_v^{\text{T}}{B}^{\text{T}})-2w\text{tr}(q_v^{\wedge }{B}^{\text{T}})\end{array}$$
记
$$\begin{array}{rl}\sigma =& \text{tr}(B)\\ S=& B+B^{\text{T}}=y{x}^{\text{T}}+x{y}^{\text{T}}\\ Z=& y\times x\end{array}$$
则
$$\begin{array}{rl}J=& (w^2-q_v^{\text{T}}{q}_v)\sigma +2\text{tr}(q_v{q}_v^{\text{T}}x{y}^{\text{T}})-2w\text{tr}(q_v^{\wedge }x{y}^{\text{T}})\\ =& w^2\sigma -q_v^{\text{T}}{q}_v\sigma +\text{tr}(x{y}^{\text{T}}{q}_v{q}_v^{\text{T}})+q_v^{\text{T}}x\text{tr}(q_v{y}^{\text{T}})-2w\text{tr}(q_v^{\wedge }x{y}^{\text{T}})\\ =& w^2\sigma -q_v^{\text{T}}{q}_v\sigma +y^{\text{T}}{q}_v\text{tr}(x{q}_v^{\text{T}})+q_v^{\text{T}}x\text{tr}(y^{\text{T}}{q}_v)-2w{y}^{\text{T}}{q}_v^{\wedge }x\\ =& w^2\sigma -\sigma q_v^{\text{T}}{q}_v+q_v^{\text{T}}y{x}^{\text{T}}{q}_v+q_v^{\text{T}}x{y}^{\text{T}}{q}_v+2w{q}_v^{\text{T}}{y}^{\wedge }x\\ =& w^2\sigma -\sigma q_v^{\text{T}}{q}_v+q_v^{\text{T}}S{q}_v+2w{q}_v^{\text{T}}(y\times x)\\ =& w^2\sigma +q_v^{\text{T}}Zw+w{Z}^{\text{T}}{q}_v+q_v^{\text{T}}S{q}_v-\sigma q_v^{\text{T}}{q}_v\\ =& (w\sigma +q_v^{\text{T}}Z)w+(w{Z}^{\text{T}}+q_v^{\text{T}}(S-\sigma I))q_v\\ =& \left[\begin{array}{cc}w& q_v^{\text{T}}\end{array}\right]\left[\begin{array}{cc}\sigma & Z^{\text{T}}\\ Z& S-\sigma I\end{array}\right]\left[\begin{array}{c}w\\ q_v\end{array}\right]\\ =& q^{\text{T}}Kq\end{array}$$
其中第3行到第4行 $y^{\text{T}}{q}_v^{\wedge }x=-q_v^{\text{T}}{y}^{\wedge }x$ 实际上等价于
$$a\cdot (b\times c)=b\cdot (c\times a)=c\cdot (a\times b)$$
  考虑到单位四元数 $q$ 满足 $|q|=1$，使用拉格朗日乘子法，
$$\begin{array}{rl}L=& q^{\text{T}}Kq+\lambda (q^{\text{T}}q-1)\\ \frac{\partial L}{\partial q}=& 2Kq-2\lambda q=0\end{array}$$
  进一步扩展到多组数据并加入权重，取
$$\begin{array}{rl}B=& \sum _{k=1}^n{\alpha }_k{y}_k{x}_k^{\text{T}}\\ \sigma =& \text{tr}(B)\\ S=& B+B^{\text{T}}\\ Z=& \sum _{k=1}^n{\alpha }_k{y}_k\times x_k\end{array}$$