最近有小白反映上一道菜內容過於豐富，吃撐了，本菜雞今天給大家做一道開胃小菜，下面開始表演。

主料 —— 無人機動力學模型，NMPC模型

輔料 —— Quadrotor場景檔案

炊具 —— V-REP，CasADi，MATLAB

1。 無人機動力學模型

直接上圖，就是這麼簡單粗暴

[5](含原始碼)基於非線性模型預測控制(NMPC)的四旋翼軌跡跟蹤

慣性系，機體系，姿態角，力與力矩方向定義

可能有些小白還是一臉懵逼。簡而言之就是，我們把無人機（確切滴說是四旋翼型別的）看做一個剛體，有四個旋翼提供力和力矩，控制無人機的位姿和速度。回顧剛體的運動學與動力學，我們知道描述剛體運動一種方式就是在剛體上任意點（可以是剛體所佔據空間內的，也可以是剛體以外空間的）固聯一個座標系

O_{B}-X_{B}Y_{B}Z_{B}

（一般放在質心，本菜在這裡也放在質心，有些情形放在其他位置更好，比如多剛體系統放在剛體連線處），描述這個座標系相對於某參考座標系

O_O-XYZ

的位姿（位置和姿態），速度（線速度，角速度以及姿態引數變化）。

位置 --

\vec{OO}_{B}

向量在

O_O-XYZ

座標系的表示

^O\left( x,y,z \right)_{OB}^{T}

姿態 --

旋轉矩陣

^OR_B

（

O_{B}-X_{B}Y_{B}Z_{B}

座標系的座標軸在

O_O-XYZ

座標系的分量矩陣）的ZYX尤拉角引數化

\left( \phi,\theta,\psi \right)^{T}

，滿足

^OR_B=R_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right)

［注意：不管尤拉角的旋轉順序是怎樣的，一律按照繞

x,y,z

軸的旋轉角順序排列引數］

線速度 --

{d\vec{OO}_{B}}/{dt}

向量在

O_O-XYZ

座標系的表示

^O\left( \dot{x},\dot{y},\dot{z} \right)_{OB}^{T}

角速度 --

O_{B}-X_{B}Y_{B}Z_{B}

座標系相對於

O_O-XYZ

座標系轉動的角速度向量

\omega_B

在

O_{B}-X_{B}Y_{B}Z_{B}

座標系的表示

^B\omega_{B}=^B\left( \omega_{x},\omega_{y},\omega_{z} \right)_{B}^{T}

，在

O_O-XYZ

座標系的表示

^O\omega_{B}=^O\left( \omega_{x},\omega_{y},\omega_{z} \right)_{B}^{T}

姿態引數變化 --

\left( \dot{\phi},\dot{\theta},\dot{\psi} \right)^{T}

平動部分運動學

很簡單，沒啥說的。

轉動部分運動學

的推導需要先弄清楚以下幾個東東：

#FormatImgID_25# 與 #FormatImgID_26# 的關係

先引入一個符號

\left[ \cdot \right]\times

表示把一個三維向量變成反對稱陣的形式，也就是叉乘對映的矩陣表示。根據旋轉矩陣求導的定義有，

^O\dot{R}_B=\left[ ^O\omega_{B} \right]\times{^OR_B}=^OR_B\left[ ^B\omega_{B} \right]\times

，即

\left[ ^B\omega_{B} \right]\times={^OR_B^{T}}\left[^O \omega_{B} \right]\times{^OR_B}\\

這個式子的意義其實就是同一線性對映（這裡是叉乘）在不同基的變換。直觀一點看，把這個等式左右兩邊右乘一個向量在

O_{B}-X_{B}Y_{B}Z_{B}

座標系的座標，那麼左邊得到叉乘後的向量在

O_{B}-X_{B}Y_{B}Z_{B}

座標系的座標；右邊的話，先是一個

^OR_B

作用得到在

O_O-XYZ

座標系的座標，然後得到叉乘後向量在

O_O-XYZ

座標系的座標，最後再用

^OR^{T}_B

拉回到

O_{B}-X_{B}Y_{B}Z_{B}

座標系。同時，我們也得到

\left[ ^O\omega_{B} \right]\times={^OR_B}\left[ ^B\omega_{B} \right]\times{^OR_B^{T}}

關於繞座標軸旋轉矩陣的導數

進一步滴，我們得到，當角速度和

R

矩陣的等效轉軸共線，那麼這兩個角速度的表示是一樣的。也就是說，對於繞座標軸的基本旋轉，我們可以有如下一組等式（記轉角為

\alpha

，三階單位矩陣

I_{3\times3}

的三列分別為

e_{x},e_{y},e_{z}

）：

\begin{align} \dot{R}_{x}R_{x}^{T}=R_{x}^{T}\dot{R}_{x}=\dot{\alpha}\left[e_{x}\right]\times\\ \dot{R}_{y}R_{y}^{T}=R_{y}^{T}\dot{R}_{y}=\dot{\alpha}\left[ e_{y} \right]\times\\ \dot{R}_{z}R_{z}^{T}=R_{z}^{T}\dot{R}_{z}=\dot{\alpha}\left[ e_{z} \right]\times \end{align}\\

一個有用等式

其實這個等式不需要什麼高深的知識，就是同一個向量在不同座標系的變換而已。考慮

^O\omega_{B}

和

^{B}\omega_B

是 座標系

O_{B}-X_{B}Y_{B}Z_{B}

相對於

O_O-XYZ

座標系轉動的角速度向量在不同座標系的表示而已，直接有

^B\omega_{B}={^OR_B^{T}}{^O\omega_{B}}

和

^O\omega_{B}={^OR_B}{^B\omega_{B}}

，然後把這倆式子同時寫成反對稱矩陣形式，有

\left[ ^B\omega_{B} \right]\times=\left[ {^OR_B^{T}}{^O\omega_{B}} \right]\times

和

\left[ {^O\omega_{B}} \right]\times=\left[ {^OR_B}{^B\omega_{B}}\right]\times

，再聯絡前面，我們有：

\begin{align} \left[{^B\omega_{B}}\right]\times={^OR^{T}_B}{^O\dot{R}_B}=\left[{^OR^{T}_B}{^O\omega_{B}} \right]\times={^OR^{T}_B}\left[ {^O\omega_{B}} \right]\times{^OR_B} \\ \left[{^O\omega_{B}}\right]\times={^O\dot{R}_B}{^OR^{T}_B}=\left[ {^OR_B}{^B\omega_{B}} \right]\times={^OR_B}\left[ {^B\omega_{B}} \right]\times{^OR^{T}_B} \end{align}\\

現在可以開始推導了。

用尤拉角引數化旋轉矩陣

^OR_B=R_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right) \\

將引數化的旋轉矩陣及其導數帶入旋轉矩陣定義式

\left[ {^B\omega_{B}} \right]\times={^OR^{T}_B}{^O\dot{R}_B}=R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)R_{z}^{T}\left( \psi \right)\left( \dot{R}_{z}\left( \psi \right) R_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{z}\left( \psi \right)\dot{R}_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{z}\left( \psi \right)R_{y}\left( \theta \right)\dot{R}_{x}\left( \phi \right)\right) \\

乘進去，有

\left[ {^B\omega_{B}} \right]\times=R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)R_{z}^{T}\left( \psi \right) \dot{R}_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)\dot{R}_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{x}^{T}\dot{R}_{x}\left( \phi \right) \\

帶入繞座標軸旋轉矩陣的導數，有

\left[ {^B\omega_{B}} \right]\times=R_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)\dot{\psi}\left[ e_{z} \right]\times{R}_{y}\left( \theta \right)R_{x}\left( \phi \right)+R_{x}^{T}\left( \phi \right)\dot{\theta}\left[ e_{y} \right]\times{R}_{x}\left( \phi \right)+\dot{\phi}\left[ e_{x} \right]\times \\

帶入一個有用等式，有

\left[ {^B\omega_{B}} \right]\times=\left[{R}_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right)\dot{\psi}{e}_{z} \right]\times+\left[ R_{x}^{T}\left( \phi \right)\dot{\theta}e_{y} \right]\times+\left[\dot{\phi}e_{x} \right]\times \\

兩邊一起脫貧摘帽，即得到角速度與尤拉角引數變化率之間的關係

{^B\omega_{B}}=\left( e_{x}, R_{x}^{T}\left( \phi \right)e_{y},{R}_{x}^{T}\left( \phi \right)R_{y}^{T}\left( \theta \right){e}_{z} \right)\left( \dot{\phi},\dot{\theta},\dot{\psi}\right)^{T} \\

簡記為

{^B\omega_{B}}=W\left( \phi,\theta,\psi \right)\left(\dot{\phi},\dot{\theta},\dot{\psi}\right)^{T} \\

話說，有些小白是不是納悶為啥用

\omega_{B}

而不用

\omega_{O}

。那是因為我們要用

Lagrangian

方法推導動力學，而轉動慣量在機體座標系是常數矩陣，便於轉動動能的表示。

動力學推導

系統廣義座標與廣義速度分別為

q=\left( x,y,z,\phi,\theta,\psi\right)^{T} \\

\dot{q}=\left( \dot{x},\dot{y},\dot{z},\dot{\phi},\dot{\theta},\dot{\psi}\right)^{T} \\

系統狀態變數為12維向量

X=\left( q^{T},\dot{q}^{T} \right)^{T} \\

控制輸入為4維向量，分量為各個旋翼的角速度平方

U=\left( u_{1},u_{2},u_{3},u_{4} \right)^{T} \\

旋翼的推進力和力矩係數分別為

k,b

，旋翼轉軸距離機體座標系原點距離為

l

，則機體所受到的合力和合力矩在座標系

O_{B}-X_{B}Y_{B}Z_{B}

內表示為

^Bf_{B}=\left( 0,0,k\sum_{i=1}^{4}{u}_{i} \right)^{T} \\

^B\tau_{B}=\left( kl\left( -u_{2}+ u_{4}\right),kl\left( -u_{1}+ u_{3}\right), b\sum_{i=1}^{4}{u_{i}}\right)^{T} \\

系統的

Lagrangian

L=\frac{1}{2}\left( \dot{x},\dot{y},\dot{z} \right)M\left( \dot{x},\dot{y},\dot{z} \right)^{T}+\frac{1}{2}\omega_{B}^{T}I\omega_{B}-mgz \\

將如下各式帶入

L

M=diag(\left[ m,m,m \right]) \\

{^B\omega_{B}}=W\left( \phi,\theta,\psi \right)\left( \dot{\phi},\dot{\theta},\dot{\psi} \right)^{T} \\

^Bf_B=\left( 0,0,k\sum_{i=1}^{4}{u}_{i} \right)^{T} \\

^Of_{B}={^OR_B}{^Bf_{B}} \\

^OR_B=R_{z}\left( \psi \right)R_{y}\left( \theta \right)R_{x}\left( \phi \right) \\

^B\tau_{B}=\left( kl\left( -u_{2}+ u_{4}\right),kl\left( -u_{1}+ u_{3}\right), b\sum_{i=1}^{4}{u_{i}}\right)^{T} \\

根據

\frac{d}{dt}\left( \frac{\partial{L}}{\partial\left( {dq/dt} \right)} \right)-\frac{\partial{L}}{\partial{q}}=\left(^O f_{B} ,^B\tau_{B}\right)^{T}

即可得動力學方程

\frac{dX}{dt}=f\left( X ,U\right) \\

2。 NMPC模型

這個例程套用標準形式（二次型損失函式，動力學約束，控制與狀態約束）。在編寫程式碼時，用CasADi幫助實現符號計算與自動微分，得到1中動力學模型與損失函式的符號表示，然後用CasADi做好的非線性最佳化器呼叫ipopt進行NLP求解，具體的可以看

NMPC_problem_formulation.m

檔案。

CasADi連結

這裡要說一說CasADi求jacobian的一個坑。

首先記

J\left( \phi,\theta,\psi \right)=W^{T}\left( \phi,\theta,\psi \right)IW\left( \phi,\theta,\psi \right) \\

如果你去求解1中的動力學方程，你會遇到求

\frac{dJ\left( \phi,\theta,\psi \right)}{dt}

的問題。如果你用MATLAB的符號計算，可以在定義符號變數的直接採用如下定義：

syms

phi

（

t

）

theta

（

t

）

psi

（

t

）

這樣接著構建

J\left( \phi,\theta,\psi \right)

之後，可直接對

t

求導數，一步到位。然而CasADi不支援這樣定義，所以你只能用鏈式法則求，在此過程中你得特別注意

矩陣對於向量求導的結果排列順序

，這一點上MATLAB和CasADi是相同的，即把矩陣按照列的順序扯成一個列向量，然後結果矩陣的每一行就是這個大向量的每個元素對於自變數的每一個分量的偏導數。

我們的控制目標是實現軌跡跟蹤，系統的輸出就是狀態變數的前六個。目標軌跡我們給的是位置走8字形，尤拉角一直為零，詳見

QuadrotorReferenceTrajectory.m

檔案。如果想在V-REP中顯示期望路徑點以便於實際執行的結果對比，可以利用

QuadrotorReferenceTrajectory.m

檔案產生對應時間序列的期望點，然後抽取出位置值部分，儲存成CSV檔案，透過V-REP中的File->Import->Path from CSV載入顯示。

3。 製作場景檔案

菜雞不知道怎麼在V-REP實現流體模擬，所以這裡只能呼叫

sim.addForceAndTorque

函式直接給無人機施加力和力矩了。無人機模型的構造極其簡單，新增一個圓盤作為機身，然後新增四個力感測器（與旋翼轉軸重合），與機身固聯，接著再新增四個圓盤作為旋翼，與力感測器固聯。把這五個圓盤的動力學引數設定好（這裡需要等效質量與慣量的計算）之後，隱藏。最後新增視覺化模型（可以從V-REP的模型庫中複製），成品大概這樣：

[5](含原始碼)基於非線性模型預測控制(NMPC)的四旋翼軌跡跟蹤

4。 執行

做初始尤拉角為零和不為零的模擬。

[5](含原始碼)基於非線性模型預測控制(NMPC)的四旋翼軌跡跟蹤

初始姿態無誤差

https：//www。zhihu。com/video/1250437024727261184

[5](含原始碼)基於非線性模型預測控制(NMPC)的四旋翼軌跡跟蹤

初始姿態有誤差

https：//www。zhihu。com/video/1250437055714390016

——————————————————————————

本例程原始檔連結（切記只可用於學習，科研等非商業用途）

——————————————————————————-

做人吶，最重要的就是開心嘍。能幫到別人是最大的快樂，希望本菜雞能對小白們有所幫助，希望大佬們的批評能幫助本菜雞成長！