搞起來，從建模到控制，再到仿真

Link: 【控制】《多無人機協同控制技術》周偉老師-第3章-面向協同控制的無人機單機控制

文章目錄

• 1. 坐標系
• 2. 角度介紹
• • 航向角 yaw angel
  • 俯仰角 pitch angle
  • 橫滾角 roll angle
• 3. 控制原理
• • 坐標系
  • 3.1 基礎知識
  • • 力矩
    • 轉矩
    • 角動量
    • 歐拉運動學方程
  • 移動
  • 旋轉
  • 慣性矩
  • 列寫動力學方程

---

1. 坐標系

2. 角度介紹

繞三個軸的旋轉值 pitch，yaw，roll 來自航空界的叫法，翻譯為俯仰角，偏航角，翻滾角，非常形象。它們不一定如上所述，但一定分別代表繞 y，z，x 的旋轉值。

一般定義載體的右、前、上三個方向構成右手系：

• 繞向前（x軸）的軸旋轉就是橫滾角；

• 繞向右（y軸）的軸旋轉就是俯仰角；

• 繞向上（z軸）的軸旋轉就是航向角。

航向角 yaw angel

俯仰角 pitch angle

橫滾角 roll angle

3. 控制原理

可以看到建模的其實就是為了得到輸入的力與加速度的關系，通過牛頓方程可以得到平動的加速度，通過歐拉方程可以得到角加速度，所以這種建模方法也叫牛頓-歐拉方程：

坐標系

Inertial frame：固定的全局坐標系，又叫慣性坐標系，世界坐標系

Translating frame：固定在運動體上的坐標系，原點在質心，但軸平行于 Inertial frame

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?d{r}_2?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 形式表示的旋轉角速度：
$$?\begin{array}{c}\dot{\gamma }\\ \dot{\beta }\\ \dot{\alpha }\end{array}?=\frac{1}{\cos \beta }?\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}??\begin{array}{c}{w}_1\\ w_2\\ w_3\end{array}?$$

慣性矩

通常平面剛片且是均質的，則也稱為轉動慣量

$E$ 是整個剛體的角動量。

經過以下一系列操作，主要是點乘和叉乘之間的轉換：
$$E={\int }_{A(q)}((r?r)w?(r?w)r)\sigma (r)dr$$

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

慣性矩陣 $3\times 3$ 對稱矩陣：
$$I(q)=({\int }_{A(q)}((r?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)=?\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}?$$

這里我們將慣性矩陣定義在 body frame 中，表示成 $I$，則 $I$ 與 $I(q)$ 的關系（其中 $R$ 是 body frame 到 translating frame 的旋轉矩陣）：
$$I(q)=RI$$

則剛體所受到的力矩簡化為：
$$N(u)=I\frac{dw}{dt}+w?(Iw)$$

如果選擇的 body frame 等于慣性主軸，那么慣性矩陣就簡化為：
$$I=?\begin{array}{ccc}{I}_{11}(q)& 0& 0\\ 0& I_{22}(q)& 0\\ 0& 0& I_{33}(q)\end{array}?$$

列寫動力學方程

這里假設選擇的 body frame 等于慣性主軸，求得角加速度 $\dot{w}$ 方程：
$$?\begin{array}{c}{N}_1(u)\\ N_2(u)\\ N_3(u)\end{array}?=?\begin{array}{ccc}{I}_{11}& 0& 0\\ 0& I_{22}& 0\\ 0& 0& I_{33}\end{array}??\begin{array}{c}{\dot{w}}_1\\ {\dot{w}}_2\\ {\dot{w}}_3\end{array}?+?\begin{array}{ccc}0& ?w_3& w_2\\ w_3& 0& ?w_1\\ ?w_2& w_1& 0\end{array}??\begin{array}{ccc}{I}_{11}& 0& 0\\ 0& I_{22}& 0\\ 0& 0& I_{33}\end{array}??\begin{array}{c}{w}_1\\ w_2\\ w_3\end{array}?$$

---

Ref: 機器人動力學（Basic Newton-Euler Mechanics）

Ref: 機器人運動學(a simple car)

Ref: 機器人系統動力學——慣性參數

Ref: 四旋翼姿態解算原理

Ref: 如何對四旋翼飛行器進行精確的數學建模?

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

Ref: 四旋翼飛行器13——歐拉中的俯仰、橫滾、偏航角

Ref: 橫滾角，俯仰角，航向角

Ref: 四旋翼無人機飛行控制入門（一）

Ref: 四旋翼飛行控制入門（二）

Ref: （三）專家PID控制理論基礎

總結

以上是生活随笔為你收集整理的【控制】四旋翼无人机姿态角分析的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。