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

Link: 【控制】《多無人機(jī)協(xié)同控制技術(shù)》周偉老師-第3章-面向協(xié)同控制的無人機(jī)單機(jī)控制

文章目錄

• 1. 坐標(biāo)系
• 2. 角度介紹
• • 航向角 yaw angel
  • 俯仰角 pitch angle
  • 橫滾角 roll angle
• 3. 控制原理
• • 坐標(biāo)系
  • 3.1 基礎(chǔ)知識
  • • 力矩
    • 轉(zhuǎn)矩
    • 角動量
    • 歐拉運(yùn)動學(xué)方程
  • 移動
  • 旋轉(zhuǎn)
  • 慣性矩
  • 列寫動力學(xué)方程

---

1. 坐標(biāo)系

2. 角度介紹

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

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

• 繞向前（x軸）的軸旋轉(zhuǎn)就是橫滾角；

• 繞向右（y軸）的軸旋轉(zhuǎn)就是俯仰角；

• 繞向上（z軸）的軸旋轉(zhuǎn)就是航向角。

航向角 yaw angel

俯仰角 pitch angle

橫滾角 roll angle

3. 控制原理

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

坐標(biāo)系

Inertial frame：固定的全局坐標(biāo)系，又叫慣性坐標(biāo)系，世界坐標(biāo)系

Translating frame：固定在運(yùn)動體上的坐標(biāo)系，原點(diǎn)在質(zhì)心，但軸平行于 Inertial frame

Body frame：固定在運(yùn)動體上的坐標(biāo)系，原點(diǎn)在質(zhì)心，軸隨著質(zhì)點(diǎn)運(yùn)動會改變方向

3.1 基礎(chǔ)知識

力矩

力矩，表示力對物體作用時所產(chǎn)生的轉(zhuǎn)動效應(yīng)的物理量。力矩能使物體獲得角加速度。力 $F$ 對矩心 $O$ 點(diǎn)的矩簡稱力矩，用 $M(F)$ 表示，其大小等于力 $F$ 的大小與力臂 $r$ 的乘積。即：
$$M(F)=F\times r=\frac{dL}{dt}$$

其中，$F$ 為矩心 $O$ 點(diǎn)的力，$r$ 是矩心相對 $O$ 點(diǎn)的位置矢量，$L$為 角動量，根據(jù)角動量定理得到。

轉(zhuǎn)矩

轉(zhuǎn)矩，使機(jī)械元件轉(zhuǎn)動的力矩稱為轉(zhuǎn)動力矩，簡稱轉(zhuǎn)矩，又稱扭矩，與轉(zhuǎn)動慣量 $I$ 具有如下關(guān)系：
$$M(F)=I\frac{dw}{dt}$$

其中，$I$ 為轉(zhuǎn)動慣量，$w$ 為角速度。

角動量

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

其中，$p$ 為質(zhì)點(diǎn) $O$ 點(diǎn)的動量，$r$ 是質(zhì)點(diǎn)相對 $O$ 點(diǎn)的位置矢量。

歐拉運(yùn)動學(xué)方程

歐拉運(yùn)動學(xué)方程

$A\in {\mathbb{R}}^3$ 代表一個剛體。$\sigma (r)$ 代表 $r\in \mathbb{A}$ 的一點(diǎn)的剛體密度，$m(A)$ 的總質(zhì)量為：
$$m={\int }_A\sigma (r)dr$$

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

由于剛體的內(nèi)部力相互抵消，外部邊界的力的集合可以合成一個力 $F$ 和力矩 $N$ 作用在質(zhì)點(diǎn) $p$：
$$F=\sum f$$

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

移動

剛體不旋轉(zhuǎn)只移動：動量 $D=m\dot{p}$

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

可以得到六個狀態(tài)轉(zhuǎn)移方程：
$${\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)}$$

旋轉(zhuǎn)

旋轉(zhuǎn)可以同移動進(jìn)行解耦，所以現(xiàn)在只在 translating frame 中考慮純粹的旋轉(zhuǎn)運(yùn)動。
$L$ 為角動量，則力矩為：
$$M(u)=\frac{dL}{dt}$$

參考上圖，旋轉(zhuǎn)角速度：$v$ 是單位向量，表示旋轉(zhuǎn)方向，$\theta$ 表示旋轉(zhuǎn)角度
$$w=v\frac{d\theta }{dt}$$

將此角速度，轉(zhuǎn)化為 yaw-pitch-roll 形式表示的旋轉(zhuǎn)角速度：
$$?\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}?$$

慣性矩

通常平面剛片且是均質(zhì)的，則也稱為轉(zhuǎn)動慣量

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

經(jīng)過以下一系列操作，主要是點(diǎn)乘和叉乘之間的轉(zhuǎn)換：
$$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 中考慮的，它會隨著旋轉(zhuǎn) $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)$ 的關(guān)系（其中 $R$ 是 body frame 到 translating frame 的旋轉(zhuǎn)矩陣）：
$$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}?$$

列寫動力學(xué)方程

這里假設(shè)選擇的 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: 機(jī)器人動力學(xué)（Basic Newton-Euler Mechanics）

Ref: 機(jī)器人運(yùn)動學(xué)(a simple car)

Ref: 機(jī)器人系統(tǒng)動力學(xué)——慣性參數(shù)

Ref: 四旋翼姿態(tài)解算原理

Ref: 如何對四旋翼飛行器進(jìn)行精確的數(shù)學(xué)建模?

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

Ref: 四旋翼飛行器13——?dú)W拉中的俯仰、橫滾、偏航角

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

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

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

Ref: （三）專家PID控制理論基礎(chǔ)

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯，歡迎將生活随笔推薦給好友。