基于反步法和四元数的姿态控制

基于反步法和四元数的姿态控制

参考资料

1.动力学模型

以最简单的单刚体姿态动力学模型为例

$$\mathbf{J}\dot{\mathbf{\omega }}+\mathbf{\omega }\times \mathbf{J}\mathbf{\omega }=\mathbf{T}\text{\hspace{1em}(1)}$$

其中$\mathbf{J}$为相对于刚体质心的主转动惯量在本体系下的列阵,$\mathbf{\omega }$、$\dot{\mathbf{\omega }}$分别为角速度和角加速度在本体系下的列阵,$\mathbf{T}$控制力矩在本体系下的列阵。

2.运动学模型

2.1 四元数

使用欧拉角对姿态进行表示会出现奇异的现象，使用欧拉角动力学进行控制，在奇异点附近会不稳定，四元数解决了这一问题，但是四元数没有欧拉角直观，对四元数，及其运算和一些tips进行总结。

2.1.1 定义

单位四元数可以用来表示旋转

$$\mathbf{q}=\left[\begin{array}{c}s\\ \mathbf{v}\end{array}\right]=\left[\begin{array}{c}cos\theta \\ \mathbf{n}sin\theta \end{array}\right]$$

其中$\mathbf{q}$为四元数，$s$为四元数标量部分，$\mathbf{v}$为四元数矢量部分，对于其物理意义，$\mathbf{n}$为转轴，$\theta$为旋转角度的一半。四元数满足$\left|\begin{array}{c}q\end{array}\right|=1$。

2.1.2 对于四元数的计算(及其物理意义TODO)

设

$${\mathbf{q}}_1=\left[\begin{array}{c}{s}_1\\ {\mathbf{v}}_1\end{array}\right]$$

$${\mathbf{q}}_2=\left[\begin{array}{c}{s}_2\\ {\mathbf{v}}_2\end{array}\right]$$

加法运算

$${\mathbf{q}}_1+{\mathbf{q}}_2=\left[\begin{array}{c}{s}_1+s_2\\ {\mathbf{v}}_1+{\mathbf{v}}_2\end{array}\right]$$
加法运算之后便不是我们使用的用于表示旋转的四元数。

积运算
$${\mathbf{q}}_1\otimes {\mathbf{q}}_2=\left[\begin{array}{cc}{s}_1& -{\mathbf{v}}_1^{\mathrm{T}}\\ {\mathbf{v}}_1& s_1\mathbf{I}+{\tilde{\mathbf{v}}}_1\end{array}\right]{\mathbf{q}}_2=\left[\begin{array}{cc}{s}_2& -{\mathbf{v}}_2^{\mathrm{T}}\\ {\mathbf{v}}_2& s_2\mathbf{I}-{\tilde{\mathbf{v}}}_2\end{array}\right]{\mathbf{q}}_1$$

其中

$$\tilde{\mathbf{v}}=\left[\begin{array}{ccc}0& -v_z& v_y\\ v_z& 0& -v_x\\ -v_y& v_x& 0\end{array}\right]$$

四元数的积运算可以计算相对姿态，如

已知1系到2系的四元数为${\mathbf{q}}_2^1$,2系到3系的四元数为${\mathbf{q}}_3^2$，则1系到3系的四元数${\mathbf{q}}_3^1$可以表示为

$${\mathbf{q}}_3^1={\mathbf{q}}_3^2\otimes {\mathbf{q}}_2^1$$

因此积运算可以用来求误差四元数

参考资料

共轭与逆

${\mathbf{q}}_1$的共轭$\overline{\mathbf{q}}$和逆${\mathbf{q}}^{-1}$为

$$\overline{\mathbf{q}}={\mathbf{q}}^{-1}=\left[\begin{array}{c}{s}_1\\ -{\mathbf{v}}_1\end{array}\right]$$
其表示与${\mathbf{q}}_1$方向相反大小相等的旋转

参考资料
参考资料

tips

四元数的定义中有$cos$和$sin$，其以$2\pi$为一周期，如下图所示。

其中蓝线为$cos$值，橙线为$sin$值，横坐标值为$\theta /2$，即横坐标$45^{\circ }$位置表示逆时针转$90^{\circ }$。四元数表示两个坐标系之间的姿态关系，图中红色点的位置，表示两个坐标系重合。两个绿色点的位置表示相同的相同的相对位置关系。由于周期性，正$180^{\circ }$和$-180^{\circ }$可以看作一个点。

因此可以看出，四元数对于两坐标相对位置关系的表示具有二值性。很好理解，比如绕$\mathbf{a}$轴转$\alpha$角度，等效于绕$-\mathbf{a}$轴转$-\alpha$角度。因此对于姿态表示，蓝色虚线范围内的四元数足以标志所有的姿态对应关系，因此在进行姿态解算或四元数积分时，可以只取$cos\theta >0$的部分，即$s>0$。若解算出来$s<0$，可令$\mathbf{q}=-\mathbf{q}$，两者表示的姿态相同。

利用四元数求两姿态的差值，得的差值范围为$-180^{\circ }$到$180^{\circ }$对应的四元数。我们通常将$\mathbf{q}=[1,0,0,0{]}^{\mathrm{T}}$，即图中$0^{\circ }$的位置，视为误差为0的点，但是可以看到，如果误差四元数为上图中右边绿色的点，如果控制到$0^{\circ }$位置，其需要绕大半圈，前文中提到过$0^{\circ }$、$-180^{\circ }$、$180^{\circ }$都是两坐标重合的位置，因此如果控制到$180^{\circ }$，则只用烧很小的角度。所有在利用四元数进行控制时，当$s_e>0$时目标四元数取$\mathbf{q}=[1,0,0,0{]}^{\mathrm{T}}$，当$s_e<0$时目标四元数取$\mathbf{q}=[-1,0,0,0{]}^{\mathrm{T}}$，当然我们也可以将$-180^{\circ }$到$180^{\circ }$的误差四元数归一化到$-90^{\circ }$到$90^{\circ }$，这样目标四元数就统一到$\mathbf{q}=[1,0,0,0{]}^{\mathrm{T}}$。

2.2 基于四元数的运动学模型

参考资料

四元数的导数可以表示为

$$\dot{\mathbf{q}}=\frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{w}}_i\end{array}\right]\otimes \mathbf{q}=\frac{1}{2}\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ \mathbf{v}& s\mathbf{I}-\tilde{\mathbf{v}}\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{w}}_i\end{array}\right]$$

或者

$$\dot{\mathbf{q}}=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{w}}_b\end{array}\right]=\frac{1}{2}\left[\begin{array}{cc}s& -{\mathbf{v}}^{\mathrm{T}}\\ \mathbf{v}& s\mathbf{I}+\tilde{\mathbf{v}}\end{array}\right]\left[\begin{array}{c}0\\ {\mathbf{w}}_b\end{array}\right]\text{\hspace{1em}(2)}$$

利用四元数导数进行积分时有

$${\mathbf{q}}_t={\mathbf{q}}_{t-\Delta t}+\dot{\mathbf{q}}\times \Delta t$$

此时 $\boldsymbol{q}_t $会不满足$\begin{vmatrix}q\end{vmatrix} = 1$的约束，也就是说，其不再是一个表示旋转的四元数，因此需要将四元数归一化到1。

$${\mathbf{q}}_t^{{}^{\prime }}=\frac{{\mathbf{q}}_t}{|{\mathbf{q}}_t|}$$

3 控制算法

3.1 基于四元数的控制

定义刚体姿态四元数为

$$\mathbf{q}=\left[\begin{array}{c}s\\ \mathbf{v}\end{array}\right]$$

期望姿态四元数为

$${\mathbf{q}}_d=\left[\begin{array}{c}{s}_d\\ {\mathbf{v}}_d\end{array}\right]\text{\hspace{1em}(3)}$$

则误差四元数为

$${\mathbf{q}}_e={\bar{\mathbf{q}}}_d\otimes \mathbf{q}=\left[\begin{array}{c}{s}_e\\ {\mathbf{v}}_e\end{array}\right]$$

与欧拉角不同的是，误差四元数表示了两个距离间的最短路径，当${\mathbf{q}}_e={\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right]}^{\mathrm{T}}$时，目标姿态和当前姿态重合。

对式3求导可得

$$\begin{gathered} {\dot{\mathbf{q}}}_e={\dot{\bar{\mathbf{q}}}}_d\otimes \mathbf{q}+{\bar{\mathbf{q}}}_d\otimes \dot{\mathbf{q}}= \\ \frac{1}{2}{\bar{\mathbf{q}}}_d\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_d\end{array}\right]\otimes \mathbf{q}+\frac{1}{2}{\bar{\mathbf{q}}}_d\otimes \mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_b\end{array}\right]= \\ \frac{1}{2}{\bar{\mathbf{q}}}_d\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_d\end{array}\right]\otimes \mathbf{q}+\frac{1}{2}{\mathbf{q}}_e\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_b\end{array}\right] \end{gathered}$$

这样得不到最简形式，我们想要尽量化简，需要利用

$${\mathbf{q}}_d\otimes {\mathbf{q}}_e=\mathbf{q}$$

等式两边同时求导得

$${\dot{\mathbf{q}}}_d\otimes {\mathbf{q}}_e+{\mathbf{q}}_d\otimes {\dot{\mathbf{q}}}_e=\dot{\mathbf{q}}$$

$$\frac{1}{2}{\mathbf{q}}_d\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_d\end{array}\right]\otimes {\mathbf{q}}_e+{\mathbf{q}}_d\otimes {\dot{\mathbf{q}}}_e=\frac{1}{2}\mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_b\end{array}\right]$$

等式两边同时右乘${\bar{\mathbf{q}}}_d$得到

$$\frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_d\end{array}\right]\otimes {\mathbf{q}}_e+{\dot{\mathbf{q}}}_e=\frac{1}{2}{\mathbf{q}}_d\otimes \mathbf{q}\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_b\end{array}\right]$$

即

$${\dot{\mathbf{q}}}_e=-\frac{1}{2}\left[\begin{array}{c}0\\ {\mathbf{\omega }}_d\end{array}\right]\otimes {\mathbf{q}}_e+\frac{1}{2}{\mathbf{q}}_e\otimes \left[\begin{array}{c}0\\ {\mathbf{\omega }}_b\end{array}\right]$$

将其展开可得

$${\dot{\mathbf{q}}}_e=\left[\begin{array}{c}-\frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)\\ \frac{1}{2}[s_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)+{\mathbf{v}}_e\times ({\mathbf{\omega }}_b+{\mathbf{\omega }}_d)]\end{array}\right]=\left[\begin{array}{c}{\dot{s}}_e\\ {\dot{\mathbf{v}}}_e\end{array}\right]$$

利用反步法，构造第一级李雅普诺夫函数

$$V_1=\frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}{\mathbf{v}}_e+\frac{1}{2}(1-s_e{)}^2$$

$$\begin{gathered} {\dot{V}}_1={\mathbf{v}}_e^{\mathrm{T}}(\frac{1}{2}[s_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)+{\mathbf{v}}_e\times ({\mathbf{\omega }}_b+{\mathbf{\omega }}_d)])+(1-s_e)(\frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}({\mathbf{\omega }}_b-{\mathbf{\omega }}_d))= \\ \frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}{s}_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)+{\mathbf{v}}_e\cdot ({\mathbf{v}}_e\times ({\mathbf{\omega }}_b+{\mathbf{\omega }}_d))+ \\ \frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)-\frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}{s}_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)= \\ \frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}({\mathbf{\omega }}_b-{\mathbf{\omega }}_d) \end{gathered}$$

当 ${\mathbf{\omega }}_b={\mathbf{\omega }}_d-2K_1{\mathbf{v}}_e$时，有，${\dot{V}}_1=-{\mathbf{v}}_e{K}_1{\mathbf{v}}_e\le 0$，当$K_1>0$时。

将${\mathbf{\omega }}_b$作为中间控制量，令${\mathbf{\omega }}_t$为${\mathbf{\omega }}_b$虚拟跟踪的量。

$${\mathbf{\omega }}_t={\mathbf{\omega }}_d-2K_1{\mathbf{v}}_e$$

则，虚拟误差${\mathbf{\omega }}_e$为

$${\mathbf{\omega }}_e={\mathbf{\omega }}_b-{\mathbf{\omega }}_t={\mathbf{\omega }}_b-{\mathbf{\omega }}_d+2K_1{\mathbf{v}}_e$$

可得

$$\begin{gathered} {\dot{\mathbf{\omega }}}_e={\dot{\mathbf{\omega }}}_b+{\dot{\mathbf{\omega }}}_d+2K_1{\dot{\mathbf{v}}}_e= \\ {\mathbf{J}}^{-1}(\mathbf{T}-{\mathbf{\omega }}_b\times \mathbf{J}{\mathbf{\omega }}_b)+{\dot{\mathbf{\omega }}}_d+K_1[s_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)+{\mathbf{v}}_e\times ({\mathbf{\omega }}_b+{\mathbf{\omega }}_d)] \end{gathered}$$

则将$V_1$，用${\mathbf{\omega }}_e$重新表示为

$${\dot{V}}_1=-{\mathbf{v}}_e^{\mathrm{T}}{K}_1{\mathbf{v}}_e+\frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}{\mathbf{\omega }}_e$$

定义考虑${\mathbf{v}}_e$和${\mathbf{\omega }}_e$的李亚普诺夫函数

$$V_2=V_1+\frac{1}{2}{\mathbf{\omega }}_e^{\mathrm{T}}{\mathbf{\omega }}_e$$

则

$$\begin{gathered} {\dot{V}}_2={\dot{V}}_1+{\mathbf{\omega }}_e^{\mathrm{T}}{\dot{\mathbf{\omega }}}_e=-{\mathbf{v}}_e^{\mathrm{T}}{K}_1{\mathbf{v}}_e+\frac{1}{2}{\mathbf{v}}_e^{\mathrm{T}}{\mathbf{\omega }}_e+ \\ {\mathbf{\omega }}_e^{\mathrm{T}}[{\mathbf{J}}^{-1}(\mathbf{T}-{\mathbf{\omega }}_b\times \mathbf{J}{\mathbf{\omega }}_b)+{\dot{\mathbf{\omega }}}_d+K_1[s_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)+{\mathbf{v}}_e\times ({\mathbf{\omega }}_b+{\mathbf{\omega }}_d)]] \end{gathered}$$

我们令

$$\begin{gathered} \mathbf{T}={\mathbf{\omega }}_b\times \mathbf{J}{\mathbf{\omega }}_b+ \\ \mathbf{J}({\dot{\mathbf{\omega }}}_d-K_1(s_e({\mathbf{\omega }}_b-{\mathbf{\omega }}_d)+{\mathbf{v}}_e\times ({\mathbf{\omega }}_b+{\mathbf{\omega }}_d))-K_2{\mathbf{\omega }}_e-\frac{1}{2}{\mathbf{v}}_e) \end{gathered}$$

得到

$${\dot{V}}_2=-{\mathbf{v}}_e^{\mathrm{T}}{K}_1{\mathbf{v}}_e+-{\mathbf{\omega }}_e^{\mathrm{T}}{K}_2{\mathbf{\omega }}_e$$

当$K_1<0$，$K_2<0$时，${\dot{V}}_2\le 0$。

其中四元数有双倍覆盖问题，使用$sign(s_e)$，替换$s_e$，或者归一化$q_e$以避免绕大圈。

下图是未考虑双倍覆盖问题时的控制

下图时考虑双倍覆盖时的控制