搞起来，从建模到控制，再到仿真

Link: 【控制】《多无人机协同控制技术》周伟老师-第3章-面向协同控制的无人机单机控制

---

1. 坐标系

2. 角度介绍

绕三个轴的旋转值 pitch，yaw，roll 来自航空界的叫法，翻译为俯仰角，偏航角，翻滚角，非常形象。它们不一定如上所述，但一定分别代表绕 y，z，x 的旋转值。

一般定义载体的右、前、上三个方向构成右手系：

• 绕向前（x轴）的轴旋转就是横滚角；

• 绕向右（y轴）的轴旋转就是俯仰角；

• 绕向上（z轴）的轴旋转就是航向角。

航向角 yaw angel

俯仰角 pitch angle

横滚角 roll angle

3. 控制原理

可以看到建模的其实就是为了得到输入的力与加速度的关系，通过牛顿方程可以得到平动的加速度，通过欧拉方程可以得到角加速度，所以这种建模方法也叫牛顿-欧拉方程：

坐标系

1. Inertial frame：固定的全局坐标系，又叫惯性坐标系，世界坐标系
2. Translating frame：固定在运动体上的坐标系，原点在质心，但轴平行于 Inertial frame
3. Body frame：固定在运动体上的坐标系，原点在质心，轴随着质点运动会改变方向

3.1 基础知识

力矩

力矩，表示力对物体作用时所产生的转动效应的物理量。力矩能使物体获得角加速度。力 $F$ 对矩心 $O$ 点的矩简称力矩，用 $M(F)$ 表示，其大小等于力 $F$ 的大小与力臂 $r$ 的乘积。即：
$$M(F)=F\times r=\frac{dL}{dt}$$

其中，$F$ 为矩心 $O$ 点的力，$r$ 是矩心相对 $O$ 点的位置矢量，$L$为 角动量，根据角动量定理得到。

转矩

转矩，使机械元件转动的力矩称为转动力矩，简称转矩，又称扭矩，与转动惯量 $I$ 具有如下关系：
$$M(F)=I\frac{dw}{dt}$$

其中，$I$ 为转动惯量，$w$ 为角速度。

角动量

角动量
$$L=r\times p$$

其中，$p$ 为质点 $O$ 点的动量，$r$ 是质点相对 $O$ 点的位置矢量。

欧拉运动学方程

欧拉运动学方程

$A\in {\mathbb{R}}^3$ 代表一个刚体。$\sigma (r)$ 代表 $r\in \mathbb{A}$ 的一点的刚体密度，$m(A)$ 的总质量为：
$$m={\int }_A\sigma (r)dr$$

$dr=d{r}_1\cdot d{r}_2\cdot d{r}_3$ 表示一个柱状的提及，$p\in {\mathbb{R}}^3$ 表示 $A$ 的质心，$p=(p_1,p_2,p_3)$：
$$p_i=\frac{1}{m}{\int }_A{r}_i\sigma (r)dr$$

由于刚体的内部力相互抵消，外部边界的力的集合可以合成一个力 $F$ 和力矩 $N$ 作用在质点 $p$：
$$F=\sum f$$

$$N=\sum r\times f$$

移动

刚体不旋转只移动：动量 $D=m\dot{p}$

牛顿第二定理：$F(u)=\frac{dD}{dt}=m\ddot{p}$

可以得到六个状态转移方程：
$${\dot{p}}_i=v_i,\text{for}i=1,2,3$$

$${\dot{v}}_i=u_i=F_i(u)/m,\text{for}i=1,2,3$$

Expand:
$$\{\begin{array}{rl}& {\dot{p}}_x=v_x\\ & {\dot{p}}_y=v_y\\ & {\dot{p}}_z=v_z\\ & {\dot{v}}_x=u_x=F_x(u)/m\\ & {\dot{v}}_y=u_y=F_y(u)/m\\ & {\dot{v}}_z=u_z=F_z(u)/m\end{array}\text{\hspace{1em}(1)}$$

旋转

旋转可以同移动进行解耦，所以现在只在 translating frame 中考虑纯粹的旋转运动。
$L$ 为角动量，则力矩为：
$$M(u)=\frac{dL}{dt}$$

参考上图，旋转角速度：$v$ 是单位向量，表示旋转方向，$\theta$ 表示旋转角度
$$w=v\frac{d\theta }{dt}$$

将此角速度，转化为 yaw-pitch-roll 形式表示的旋转角速度：
$$\left[\begin{array}{c}\dot{\gamma }\\ \dot{\beta }\\ \dot{\alpha }\end{array}\right]=\frac{1}{\cos \beta }\left[\begin{array}{ccc}\cos \alpha & \sin \alpha & 0\\ -\sin \alpha \cos \beta & \cos \alpha \cos \beta & 0\\ \cos \alpha \sin \beta & \sin \alpha \sin \beta & -\cos \beta \end{array}\right]\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\end{array}\right]$$

惯性矩

通常平面刚片且是均质的，则也称为转动惯量

$E$ 是整个刚体的角动量。

经过以下一系列操作，主要是点乘和叉乘之间的转换：
$$E={\int }_{A(q)}((r\cdot r)w-(r\cdot w)r)\sigma (r)dr$$

$$E=({\int }_{A(q)}((r\cdot r)I_3-r{r}^T)\sigma (r)dr)w$$

惯性矩阵 $3\times 3$ 对称矩阵：
$$I(q)=({\int }_{A(q)}((r\cdot r)I_3-r{r}^T)\sigma (r)dr)$$

则刚体的角动量：
$$E=Iw$$

From:
$$M=I\frac{dw}{dt}=\frac{dE}{dt}$$

刚体所受到的力矩：
$$N(u)=\frac{dE}{dt}=\frac{d(Iw)}{dt}=I\frac{dw}{dt}+\frac{dI}{dt}w$$

注意上面求得的惯性矩阵 $I$ 是在 translating frame 中考虑的，它会随着旋转 $q$ 的变化而变化：
$$I(q)=\left[\begin{array}{ccc}{I}_{11}(q)& I_{12}(q)& I_{13}(q)\\ I_{21}(q)& I_{22}(q)& I_{23}(q)\\ I_{31}(q)& I_{32}(q)& I_{33}(q)\end{array}\right]$$

这里我们将惯性矩阵定义在 body frame 中，表示成 $I$，则 $I$ 与 $I(q)$ 的关系（其中 $R$ 是 body frame 到 translating frame 的旋转矩阵）：
$$I(q)=RI$$

则刚体所受到的力矩简化为：
$$N(u)=I\frac{dw}{dt}+w\cdot (Iw)$$

如果选择的 body frame 等于惯性主轴，那么惯性矩阵就简化为：
$$I=\left[\begin{array}{ccc}{I}_{11}(q)& 0& 0\\ 0& I_{22}(q)& 0\\ 0& 0& I_{33}(q)\end{array}\right]$$

列写动力学方程

这里假设选择的 body frame 等于惯性主轴，求得角加速度 $\dot{w}$ 方程：
$$\left[\begin{array}{c}{N}_1(u)\\ N_2(u)\\ N_3(u)\end{array}\right]=\left[\begin{array}{ccc}{I}_{11}& 0& 0\\ 0& I_{22}& 0\\ 0& 0& I_{33}\end{array}\right]\left[\begin{array}{c}{\dot{w}}_1\\ {\dot{w}}_2\\ {\dot{w}}_3\end{array}\right]+\left[\begin{array}{ccc}0& -w_3& w_2\\ w_3& 0& -w_1\\ -w_2& w_1& 0\end{array}\right]\left[\begin{array}{ccc}{I}_{11}& 0& 0\\ 0& I_{22}& 0\\ 0& 0& I_{33}\end{array}\right]\left[\begin{array}{c}{w}_1\\ w_2\\ w_3\end{array}\right]$$

---

Ref: 机器人动力学（Basic Newton-Euler Mechanics）

Ref: 机器人运动学(a simple car)

Ref: 机器人系统动力学——惯性参数

Ref: 四旋翼姿态解算原理

Ref: 如何对四旋翼飞行器进行精确的数学建模?

Ref: 四旋翼建模、控制与仿真（理论部分）

Ref: 四旋翼飞行器13——欧拉中的俯仰、横滚、偏航角

Ref: 横滚角，俯仰角，航向角

Ref: 四旋翼无人机飞行控制入门（一）

Ref: 四旋翼飞行控制入门（二）

Ref: （三）专家PID控制理论基础