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

圆锥曲线轨道

轨道六根数指：
半长轴(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}$$
平近点角(mean anomaly, $M$)
真近点角(true anomaly, $\theta$)
偏近点角(eccentric anomaly, $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}\theta =& 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}$$

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

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

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

自用的刚体姿态动力学推导

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}$$