姿态控制算法前期准备（四元数+PID算法理论+欧拉角理论）

一、需要的知识储备：

由于输入数据为四元组，因此需要对四元组有一些初步的了解：

**什么是四元数？**形如：
q = a + b ∗ i + c ∗ j + d ∗ k q=a+b*i+c*j+d*k q=a+b∗i+c∗j+d∗k

它是复数的推广，因此亦称超复数。它除了乘法交换律不成立外，通常代数的其余运算律都成立。其满足一下等式：
i 2 = j 2 = k 2 = i j k = − 1 i j = − j i = k , j k = − k j = i , k i = − i k = j i^2=j^2=k^2=ijk=-1 \\ ij=-ji=k,jk=-kj=i,ki=-ik=j i2=j2=k2=ijk=−1ij=−ji=k,jk=−kj=i,ki=−ik=j

四元数基础知识

四元数满足结合律和分配律，但是不满足乘法交换律。
对四元数 q = s + x i + y j + z k ，也可以用有序对即二元形式 [ s , v ] 来表示，其中， s ∈ R ， v ∈ R 3 s ∈ R ， v ∈ R 3 , s ∈ R ， v ∈ R 3 对四元数\textbf{q}=s+x\textbf{i}+y\textbf{j}+z\textbf{k}，也可以用有序对\\ 即二元形式[s,\textbf{v}]来表示，其中，s ∈ R ， v ∈ R 3 s\in R，v\in R^3,s∈R，v∈R^3 对四元数q=s+xi+yj+zk，也可以用有序对即二元形式[s,v]来表示，其中，s∈R，v∈R3s∈R，v∈R3,s∈R，v∈R3

1.1 四元数的模：
∣ q ∣ = s 2 + v 2 = s 2 + x 2 + y 2 + z 2 |q|=\sqrt{s^2 + v^2}=\sqrt{s^2+x^2+y^2+z^2} ∣q∣=s2+v2 =s2+x2+y2+z2

1.2共轭四元数
q ∗ = [ s , − v ] , q q ∗ = [ s 2 − < v , − v > , − s v + s v + v × ( − v ) ] = [ s 2 + v 2 , 0 ] = ∣ q ∣ 2 q^∗=[s,−v],qq^∗=[s^2−<v,−v>,−sv+sv+v×(−v)]=[s^2+v^2,0]=|q|^2 q∗=[s,−v],qq∗=[s2−<v,−v>,−sv+sv+v×(−v)]=[s2+v2,0]=∣q∣2
1.3四元数的逆
q − 1 = q ∗ ∣ q ∣ 2 q^{-1}=\frac{q^*}{|q|^2} q−1=∣q∣2q∗
1.4单位四元数

给定任意的向量v，我们可以把这个向量写成一个系数和一个单位方向向量的乘积：
s 2 + v 2 = 1 s^2+v^2=1 s2+v2=1
1.5四元数的旋转

四元数可以表示三维空间上任意的伸缩旋转变换，如果已知一个三维空间的伸缩旋转的转轴方向、旋转角度和伸缩比例，来求相应的四元数，是比较容易的。
四元数提供了一个很好的描述3D方位的方法，这使得它在计算机图像处理、机器人和虚拟现实以及任何涉及三维方位的地方有着广泛的应用，下面是几个例子：
1、计算机辅助设计（CAD）。其本质需要软件对物体的旋转动作进行平滑处理，并图像显示。
2、计算机游戏。Unity是开放3D游戏的平台，在很多地方我们都可以看到它的身影。很多游戏是需要图像的平移和旋转进行平滑过渡的，四元数就有了用武之地。
3、欧拉角有一个万向锁问题也是旋转问题，用四元数可以解决，也和上面应用相关。

二、可选用算法：

1、PID算法（可输入四元数）

算法理论：

P(比例环节)：给定一个速度的大致范围，计算公式：
K p ⋅ e ( t ) , K p 为比例系数 , e ( t ) 为偏差 K_p\cdot e(t),K_p为比例系数,e(t)为偏差 Kp⋅e(t),Kp为比例系数,e(t)为偏差
I(积分环节)：误差在一段时间范围的和，计算公式为：
K i ⋅ ∫ 0 t e ( x ) d x , K i 为积分系数 K_i\cdot \begin{aligned}\int\limits_0^t e(x) \mathrm{d} x\end{aligned},K_i为积分系数 Ki⋅0∫te(x)dx,Ki为积分系数
D(微分环节)：误差变换曲线在某出的倒数，计算公式为：
K d ⋅ d e ( t ) d t , K d 为比例系数， d e ( t ) d t 为在某处的导数 K_d\cdot \frac{de(t)}{dt},K_d为比例系数，\frac{de(t)}{dt}为在某处的导数 Kd⋅dtde(t),Kd为比例系数，dtde(t)为在某处的导数
遇到离散值时，我们需要将连续变量离散化，具体见下方图片：

2、欧拉角法（通过角度变换）

算法推演，角度变化：