機器人學的基本工具已經了解完畢，現在開始了解移動機器人，這部分包括機器人平台、導航、定位。所謂機器人平台就是指機器人的實體結構及其驅動方式。本文将學習兩種典型移動機器人平台(四旋翼和輪式車)的運動與控制，研究輸入控制信号怎麼控制位姿随着時間變化。
機器人學的基本工具已經了解完畢，現在開始了解移動機器人，這部分包括機器人平台、導航、定位。
所謂機器人平台就是指機器人的實體結構及其驅動方式。本文将學習兩種典型移動機器人平台(四旋翼和輪式車)的運動與控制，研究輸入控制信号怎麼控制位姿随着時間變化。
到這一部分網上的筆記突然變少 ≈ 0
但我想從這一部分開始，就更有意思了。
0207注：好難啊，機器人不愧是屠龍之術。
0212注：此文很長。已經到了可以分幾篇發出去的程度...由于此文很長建議結合目錄來看，每個小标題右側都可以召喚目錄。
本系列參考資料：
- 《Robotics, Vision and Control》
- B站公開課：
- 台灣交通大學機器人學公開課
- Peter Corke 配套視訊
- 這本書的中文公開課(品質不算高但還算實用)【公開課】機器人學視覺控制- matlab仿真_哔哩哔哩_bilibili
00 機動性
0210附：機動性這部分實體、力學、控制概念極多，非相關專業會看着比較難受，推薦一個實體教育網站，閑暇可以玩玩：
實體概況 - 主題索引 (hypertextbook.com)
以及一個控制、力學相關的系列文章：
https://zhuanlan.zhihu.com/p/48662038
學習兩種平台之前，有一個一般性的概念：機動性，可以幫助之後的了解。
所謂機動性，其實是機器人實作空間運動的相關概念集合。
-
位形空間(Configuration space / C-space)：
所有的可能的位置形态集合，我們記為C：
\[位置坐标\dot{q}\in \mathcal{C}
\]
-
任務空間(task space)
機器人所有可能的位姿集合，任務空間依賴于具體應用或任務。記為T：
\[位姿\xi\in\mathcal{T}
-
自由度
确定系統在空間中的位置所需要的最小坐标數。
-
廣義坐标
用來描述系統位形所需要的獨立參數或者最小參數
-
欠驅動系統
系統獨立控制變量個數小于系統的自由度
即輸入的量要比控制的量要少的系統
- 我們可以舉幾個運動系統的例子來了解了解這幾個概念
- 火車
- 對于火車來說，火車的位置可以用其沿軌道的距離資料來描述，即一個标量參數q來決定火車的位置；
- 位形空間(C空間)：即q描述的火車的位置構成的空間，q∈C；
-
任務空間：依賴于具體任務的意思就是說，比如火車是要沿直線導軌運動，我們隻需要關心火車在一個直線上的位置，此時：
\[\mathcal{T}\subset R
同理，考慮火車在平面上的位置則：
\[\mathcal{T}\subset R^2
三維同理。
-
自由度：1維，因為火車由一個獨立的标量參數q來完全決定
但聯系一下C空間和task space，當火車的任務空間維數超過位形空間維數時，那麼火車不能随便到達任意一個任務空間中的位置，因為它是被限制在一條導軌上的。
🌂
- 廣義坐标：即這個q
- 驅動情況：全驅動系統，因為火車有一個驅動器，推動火車沿着軌道前進或者後退
- 氣墊船
- 氣墊船有兩個螺旋槳，軸線平行，兩個螺旋槳推力的合理形成一個向前的推動力。可以用q=(x,y,θ)來決定其位置；
-
位形空間，即q描述的位置構成的空間，q∈C；
PS：
\[這裡的C\subset{}R^2×S
- 任務空間同上個例子的讨論
- 自由度，可見最小獨立描述坐标數為3(x , y , θ)，自由度為3；
- 廣義坐标：q
- 驅動情況：兩個驅動器，比自由度少，是以為欠驅動系統。
如果給這個船側向垂直于原兩個螺旋槳的方向增加一個螺旋槳，那麼就實作了平面上任意方向的運動，前進、側移、旋轉。這時，任務空間等效于位形空間。欠驅動系統會限制自由運動的能力，我們任意時刻都可以控制氣墊船的前向加速度和旋轉加速度，但氣墊船的側向加速度始終為0；但是我們總能通過其他的運動來代替直接側向移動（雖然麻煩了一點）。
但是對于這些工具來說，這滿足了最常用的前進的功能要求，并且驅動器較少。
- 直升機
- 簡單介紹一下，4個驅動器+一個重力驅動，位姿由6個廣義坐标描述q=(x,y,z,θrθpθy)，即三維空間中的位置xyz，以及滾轉-俯仰-偏轉的姿态角度。
- 自由度為6.是欠驅動系統。
- \[任務空間\mathcal{T}\subset SE(3)
- 固定翼
- 與直升機類似，驅動器（向前推進器、副翼、升降舵和方向舵）位形空間等與直升機相同。
- 汽車/坦克
- 兩驅動器，一個前後運動、另一個改變行駛方向
- q = (x, y, θ)，三個次元
- 欠驅動
- 車體轉彎時由于由橫向摩擦力，是以不需要額外驅動器提供這個力
- 可以實作側滾的輪子已經研發出來了：全向輪、麥克納姆輪：麥克納姆輪淺談
- 火車
- 補充幾個概念：
-
再次感謝這個系列對于我認知概念的指引，機動性這部分力學内容很多：
思考(8)李括号, 完整與非完整限制的差別
- 完整限制和非完整限制：
- 完整限制可以用位形q的一個方程F(q, t)來描述；
-
非完整限制限制方程中包含坐标對時間的導數（如運動限制），
\[F(q,\dot{q},t)且不能通過積分轉化為F(q,t)
而且方程不可能積分成有限形式，這類限制成為非完整限制。
- 非完整系統、完整系統：
- 非完整系統是指典型的受非完整限制系統，比如：車輛；标志是時變控制政策。
- 反之為完整系統。
-
這裡這些概念的了解其實有待商榷，我沒有完全在腦海裡形成對這些概念的具象印象，我搜了很多，大多是運動規劃方面的名詞，但是并不完全對應(因為我對運動規劃也并不了解)，這對我的學習造成了很大的困難。
是以我認真研究了書上的例子，對這些名詞進行對應，大概好了一點。
01 輪式機器人
四輪機器人的常用模型是雙輪自行車模型，即兩後輪固定在車體上，兩前輪可以繞水準軸轉動以實作車輛轉向。
01-1 概念及模型
01-1-1 模型的相關概念引入
模型圖如下，淺灰色是車體，深色是近似的自行車兩輪，位形q=(x,y,θ)∈C（SE(2)）。其中x是在世界坐标系O中x的坐标，y是O中y的坐标，θ是指 車身與O坐标系x的夾角。
- 根據機器人學1部分的内容，對車輛建立物體坐标系V（圖中紅色），γ是前輪的轉向角，是前輪與Vx軸的夾角，
- 左上角的ICR是小車的旋轉瞬心（後輪前輪的中垂線交點，轉彎時可以看作圓周運動；
- R1和R2分别是後輪前輪到ICR的舉例
- L是輪基的長度。
- V中速度及角速度：
-
\[^V\dot{x}=v\\
^v\dot{y}=0\\
\dot\theta=\frac{v}{R_1}
（因為後輪與v方向一緻）
-
易知圖中轉向的圓心角為γ，則
\[R_1 = \frac{L}{\tan{\gamma}}
- 從上式可以看出，轉彎半徑R1會随着車輛長度(輪基L)的增加而增加，γ的大小是由機械結構限制的，R1最小時，γ最大。
- R2>R1，前輪會比後輪多走一段路，并且兩個前輪的情況也不同，是以兩個前輪的轉向角度也應該有所不同，可以通過阿卡曼(模型)轉向機構實作：
人工智能與智能系統3-> 機器人學3 | 移動機器人平台 阿克曼模型 ≠ 自行車模型
當然，兩個前車輪轉彎時要以不同的速度轉動，是以發動機和驅動輪之間要按轉一套差動齒輪箱。
-
- 坐标系O中速度及角速度：
-
\[\dot{x}=v\cos\theta\\
\dot{y}=v\sin\theta\\
\dot{\theta}=\frac{v}{L}\tan\gamma
- 這就是小車的運動學模型，即隻描述了小車的運動速度，沒有涉及力和力矩的模組化；
- 第三個式子表達了θ的變化率，也叫轉動速率，可以通過陀螺儀(上一部分的慣性導航部分🌂)直接測量，也可以通過左右輪的角速度值推算出來；
-
\[\dot{y}\cos\theta-\dot{x}\sin\theta\equiv0
世界坐标系中y方向的表達式，是一個非完整限制(無法積分為x,y,θ表達式)
-
01-1-2 simulink模型
simulink的模型是很好讀和了解的。
A Bicycle 模型
可見，輸入為速度 v和角度 γ，接着分别是是速度限制和加速度限制、轉向角限制和轉向速率限制，tan(u/L)中u代表的就是這個γ，計算出了θ(輸出3)，與cos和sin混合後，乘上v對時間進行積分得到x(輸出1)和y(輸出2)；
我們還可以用一個單獨的手刹子產品向積分子產品輸出一個常量信号，将積分器強制調回到初始條件
這就是上面運動學模型的子產品計算展現。
下面我們使用這個子產品實作一個變換車道的運動：
B sl_lanechange模型
可以看到最左側的脈沖驅動發生器是先産生一個左轉信号(z正值)，一段時間後右轉(負值)，結合前面的小車模型圖就可以料想我們就實作了一個左轉變換車道後右轉恢複正向。
下面我們檢視一下其他值的情況：
sl_lanechange;
r = sim('sl_lanechange');
Y = r.find('y');
T = r.find('t');
subplot(2,2,1);
plot(T,Y(:,1));
xlabel('t'),ylabel('X');
subplot(2,2,2);
plot(T,Y(:,2));
xlabel('t'),ylabel('Y');
subplot(2,2,3);
plot(T,Y(:,3));
xlabel('t'),ylabel('theta');
subplot(2,2,4);
plot(T,Y(:,4));
xlabel('t'),ylabel('gamma');
% 圖表如下：分别是X、Y、Theta、gamma的時間變化曲線
01-2 移動到一個點
01-2-1 控制條件
讨論一下輪式向平面上某個點(x1,y1)移動的問題，我們預設小車的速度大小與離目标的距離成正比。由如下等式：
\[(1).{}v_1=K_v\sqrt{(x_1-x)^2+(y_1-y)^2}
轉向目标角使用世界坐标系中的相對角度θ1：
\[(2).\theta_1 = \arctan\frac{y_1-y}{x_1-x}
我們使用比例微分控制器(PID)來調整車輛的轉向角(θ是小車目前與世界坐标系x軸的夾角)：
\[(3).\gamma = K_h(\theta_1\ominus\theta),K_h > 0\\
(\ominus是符合特定條件的-)\\
作差後的角度是逆時針的
了解這個PID，當Kh=1時，我們小車就将與車目标點連線平行
如果Kh更大(>1)，則會向與連線交錯的方向前進。
有了相關的控制條件就可以進行仿真：
Simulink仿真
可以看到這個模型使用的bicycle子產品輸入是速度和轉向角，輸出θ，對于速度v我們采用的即為上面(1)式控制，γ角即為(2)(3)式控制，
Warning：此時運作報錯，說參數範圍不對
"Simulink will stop the simulation of model 'sl_drivepoint' because..."，
上網搜尋了一下，發現是simulink内置的angdiff的參數順序與我們預想正好反了，修改即可：
人工智能與智能系統3-> 機器人學3 | 移動機器人平台 參考：
sl_drivepoint 模型 consecutive zero crossings
sl_drivepoint;;
xg = [5 5];
x0 = [8 5 pi/2];
r = sim('sl_drivepoint');
q = r.find('y');%書上此處不對
plot(q(:,1), q(:,2))
% 輸出三個黑圖和自己畫的位置變化圖：
值得注意的是最後出現了一段直線。
01-3 跟蹤一條直線
01-3-1 控制條件
下面我們探讨跟蹤平面上由ax+by+cz=0定義的一條線，我們會需要兩個控制器，第一個控制機器人與直線的距離，以使得其與直線垂直距離最短。距離為：
\[(1).d=\frac{(a,b,c)\cdot(x,y,1)}{\sqrt{a^2+b^2}}
值得注意的是這個公式沒有加絕對值，在上方為正，在下方為負。
用下式PID控制機器人的向目标直線運動：
\[(2). \alpha_d = -K_dd,K_d>0
第二個要調整小車的位姿平行于直線，θ1是直線的斜率對應的角度：
\[(3).\alpha_h=K_h(\theta_1\ominus\theta),K_h>0\\
(4).\theta_1 = \arctan\frac{-a}{b}
我們可以将兩個控制律綜合起來：
\[(5).\gamma=-K_dd+K_h(\theta_1\ominus\theta)
我們沒有控制速度，因為直線是無限長的，速度限制不強，隻需要取一個常數即可。
01-3-2 Simulink仿真
可見速度是一個常數1，相關設定不一一解釋，實作的就是上面的控制律。
sl_driveline;
% 設定目标直線
L = [1 -2 4];
% 車的位置和位姿
x0 = [8 5 pi/2];
r = sim('sl_driveline');
同上個例子，角度輸入順序不對，同樣需要調整一下angdiff的參數順序，不然會反複報錯。
01-4 跟蹤一般路線
01-4-1 控制條件
除了直線，更普遍的是更具一般性的曲線。而曲線無疑在機器人規劃中具有重要意義，當我們讨論機器人導航，傳感器或規劃器産生的路徑很有可能就是一般曲線。
如何考慮這件事情呢，可以采用純追蹤算法，即目标點沿着曲線恒速運動，而機器人向目标點運動。即在01-2移動到一個點的基礎上使這個目标點移動。
小車不能超過目标點，需要一個安全距離（跟蹤誤差）：
\[(1).e = \sqrt{(x_1-x)^2+(y_1-y)^2}-d^*
意思是，當小車到達距目标點d*處時，小車速度不能超過目标點速度。
我們需要限制小車的速度：
\[(2).v_1 = K_ve+K_i\int{e}dt
積分項是用來確定當e趨于0時仍能提供一個有限的速度需求量v1，第二個控制律需要控制小車轉向目标點：
\[(3). \alpha = K_h(\theta_1\ominus\theta),K_h>0\\
(4).\theta_1 = \arctan\frac{y-y_1}{x-x_1}
理想情況是保持相對靜止。
01-4-2 Simulink仿真
最左側的trajectory子產品是利用插值方法的軌迹生成器；
sl_pursuit;
r = sim('sl_pursuit');
% 輸出：
可見紅色虛線為我們的一般路徑，藍色即為小車的仿真路線。在直線部分的貼合效果很好，在拐彎的地方兩者都改變運動狀态，小車需要調整。
01-5 運動到某個位姿
01-5-1 控制條件
如何小車運動到一個特定的位姿呢，之前我們隻實作了控制小車到指定目标點，但并沒有控制位姿，或者說，最終的位姿會依賴于起始位姿。
下面我們嘗試控制最終的目标位姿，雖然隻增加了一個條件但是需要考慮的事情複雜了一些：
回憶一下我們之前的運動控制式：
我們将其重寫為：
\[\left[\begin{matrix}
\dot{x}\\
\dot{y}\\
\dot{\theta}
\end{matrix}\right]
=
\left[\begin{matrix}
\cos\theta & 0\\
\sin\theta & 0\\
0 & 1
v \\
\dot{\gamma}
可見差别是第三行，但實際上是等價的，這是因為：
\[\dot\gamma = \frac{v}{R_1}=\frac{v}{L}\tan\gamma\\
∴\dot\gamma=\dot\theta
這是個很容易了解但很重要的結果。
我們根據上圖再對變量做一下變換(極坐标)：
\[\rho=\sqrt{\Delta_x^2+\Delta_y^2}\\
\alpha = \arctan\frac{\Delta_y}{\Delta_x}-\theta\\
\beta=-\theta-\alpha
- θ是車身與世界坐标系X軸的夾角
- α是小車後輪(速度方向)與ρ的夾角
- γ是前輪轉向角
- β是始末兩車中心連線與X軸的夾角，加負号是因為方向
帶入後我們得到：
\dot{\rho}\\
\dot{\alpha}\\
\dot{\beta}
-\cos\alpha & 0\\
\frac{\sin\alpha}{\rho} & -1\\
-\frac{\sin\alpha}{\rho} & 0
\cdot
{v}\\
\end{matrix}\right]\\
若\alpha\in[-\frac{\pi}{2},\frac{\pi}{2}]
我們來簡單了解一下：
第一行，即速度v在ρ分方向上投影的速度vcosα，負号表示向左轉；
第三行，也很好了解，設想一個微分的情況，即繞上圖G坐标系原點G做一個微小的旋轉，線速度為vsinα，半徑來不及改變故為ρ；利用角速度公式即可得；
第二行，利用上面的 β=-α-θ ，而：
\[\dot{\theta}=\dot{\gamma}
假設目标在小車前方，則采用下面的線性控制方法：
\[v = K_\rho\rho\\
\dot{\gamma} = K_\alpha\alpha+K_\beta\beta
Kρρ和Kρα控制小車沿一條直線向目标運動（ρ->0，α->0），而Kρβ用來轉動該直線使得β->0。再次代入，閉環控制系統為：
-k_\rho\rho\cos\alpha\\
k_\rho\sin\alpha-k_\alpha\alpha-k_\beta\beta\\
-k_\rho\sin\alpha
\\
穩定條件為：k_\rho>0,k_\beta<0,k_\alpha-k_\rho>0\\
PS:書上右側矩陣第一行少\rho,是預設為1了
當目标在機器人後方，隻需把控制算法中的v和γ符号變為負号，v不改變符号，取決于α初值。
到這裡為止還不夠，因為我們讨論的還是到(0, 0)處任意位姿的控制律，而要想得到其他任一點(x, y)的控制律，坐标變換即可：
\[\Delta{x}=x-x_1\\
\Delta{y}=y-y_1\\
\theta'=\theta\\
\beta=\beta'+\theta*
- 前兩行容易了解
- 第三行代表θ沒有改變
- 第四行意思是，最終的β角度由兩部分組成，其一是轉過的角度，其二是目标位姿與世界坐标系X軸的夾角。
01-5-2 Simulink仿真
這個圖還是挺好看的，值得注意的是：
- 右側偏上的desired position中為x1，y1，0，與下方x，y，theta作差，輸出Δx、Δy、theta進入後面to_polar中轉化為極坐标；
- to_polar輸出ρ、α、β’
- to_polar中direction，會輸出一個1和-1，來調整相關值的符号；
- xg(3)是θ*，與左側來的β' 相加，得到β；
- 左側的系數就比較好讀了。
sl_drivepose;
% 目标位姿
xg = [5 5 pi/2];
% 初始位姿
x0 = [8 5 pi/2];
r = sim('sl_drivepose');
y = r.find('y');
plot(y(:,1),y(:,2));
% 輸出：
01-6 階段總結
總結一下上面四個模型，都是通過控制v和γ來控制小車運動。
-
移動到一個點
追蹤一個固定位置的點，到達目的地速度必須為零，沒到達目标點時需要控制γ使得小車轉向目标點，但不能控制小車最終的位姿，最終位姿依賴于初始位姿。
-
跟蹤一條直線
γ的設定要使小車不斷接近目标直線，并且小車到達直線時小車平行于直線，由于直線無限長，小車到直線上時仍有速度，是以不控制速度。
-
跟蹤一般路徑
追蹤一個沿固定路徑運動的目标點，v仍受距離控制，不同的是當小車運動到安全距離值時，小車會調整自己的速度使自己不超過目标點。
-
運動到一個位姿
将運動學模型改寫，并寫成極坐标形式，采用線性控制使得小車沿着一條直線向目标運動，并不斷調整這條線的角度。
02 飛行機器人
講道理這部分隻是很簡單的模組化介紹，關于無人機飛控算法，有很多書專門講這個，我們先了解一下控制過程即可，可能會有點繁瑣和困難。先放兩個教程連結：
- 多旋翼飛行器設計與控制 北航
- MATLAB-官方視訊 四軸無人機的仿真與控制
飛行機器人與地面機器人的一些技術參數不同，飛行機器人有6個自由度，位形空間q∈SE(3)，且是由動力驅動的，是以運動模型中必須包含力和力矩，而不僅僅是對于速度和角度的限制，這就要建立動力學模型了。
飛行機器人最簡單的模型就是四旋翼飛行器。
02-1 動力學分析與模組化
02-1-1 建立坐标系
首先我們需要建立兩個坐标系，世界坐标系O和機體坐标系B，機體坐标系B固聯在飛行器上，原點與質心重合，橫軸OX正方向指向1号電機，縱軸OY正方向指向4号電機，OZ垂直于OXY平面，正方向垂直OXY向下。
02-1-2 四旋翼模型
圖中顯示了4個旋翼以及推力矢量Ti和旋轉方向，旋翼1、3逆時針(從上往下看)，旋翼2、4順時針旋轉。
02-1-3 複習歐拉角
主要是複習三角度表示法中的翻滾-俯仰-偏航角（卡爾丹角）：
\[R = R_x(\theta_r)R_y(\theta_p)R_z(\theta_y)
- 翻滾角Φ，表示機體坐标系繞OX軸旋轉的角度，如果從飛行器尾部順縱軸向前看，如果OZ軸位于鉛垂面的右側，則Φ為正；
- 俯仰角θ，表示機體坐标系繞OY軸旋轉的角度，旋轉後飛行器橫軸指向水準面上方為正；
- 偏航角ψ，表示機體繞OZ軸旋轉的角度，是飛行器縱軸OY在水準面的初始投影與OX軸的夾角，垂直地面看，如果OX轉至投影是逆時針，則為正；
02-1-4 質心運動學模型
設旋翼轉速為ω，産生一個向上的推力向量，指向z軸負方向：
\[(1).T_i = b\omega_i^2,i=1,2,3,4
其中b>0，是升力常數，取決于空氣密度、旋翼葉片半徑立方、葉片弦長；
該飛行器在世界坐标系中的移動動力學方程：
\[(2).m\dot{v}=
0\\
mg
-
{}^oR_B
T
解釋：簡單講就是重力減去推力，總推力T本在機體坐标系下，将其轉化到世界坐标系。
- v是飛行器在世界坐标系中的速度；
- g是重力加速度；
- m是飛行器的總品質；
- T是向上的總推力；
- oRB這個矩陣是将後面的向量轉化到世界坐标系下；
\[(3).{^oR_B}=R_xR_yR_z=
\cos\psi\cos\phi & \cos\psi\sin\theta\sin\phi & \cos\psi\sin\theta\cos\theta+\sin\psi\sin\phi\\
\sin\psi\cos\theta & \sin\psi\sin\theta\sin\theta & \sin\psi\sin\theta\cos\phi-\sin\phi\cos\psi\\
-\sin\theta & \cos\theta\sin\phi & \cos\theta\cos\phi
綜合(1)(2)(3)可得質心運動模型：
\[(4).\ddot{x}=\sum_{i=1}^4b\omega_i^2(\cos\psi\sin\theta\cos\theta+\sin\psi\sin\phi)/m\\
\ddot{y} = \sum_{i=1}^4\omega_i^2(\sin\psi\sin\theta\cos\phi-\sin\phi\cos\psi)/m\\
\ddot{z} =
\sum_{i=1}^4\omega_i^2(\cos\theta\cos\phi)/m-g
02-1-5 機體角運動模型
上面是豎直方向上的限制，還需要讨論各個旋轉角的動力限制。每對旋翼之間的推力差異就會導緻飛行器旋轉。
-
x軸上産生的轉矩即橫滾力矩
\[(5).\mathcal{T}_x=dT_4-dT_2\\
\Downarrow\\
(6).\mathcal{T}_x=db(\omega_2^2-\omega_4^2)
-
y軸産生俯仰力矩
\[(7).\mathcal{T}_y=db(\omega_1^2-\omega_3^2)
- d是電機到飛行器質心的距離
-
對于z軸，考慮電機施加給每個旋翼的轉矩，會有一個與之方向相反的空氣阻力轉矩：
\[(8).Q_i = k\omega_i^2
這個轉矩作用是使整個機體産生繞Z軸的轉動，方向與旋翼方向相反，可以得出Z軸總轉矩為：
\[(9).\mathcal{T}_z=Q_1-Q_2+Q_3-Q_4=k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2)
可見調整旋翼轉速即可實作偏航力矩。
k與升力常數影響因素相同。
-
旋轉加速度 ( 角加速度 )：
\[(10).\mathcal{J}\dot{\omega}=-\omega×\mathcal{J}\omega+\Gamma
- ω 是角速度向量 (3×1)
- Γ=(τx, τy, τz)T 是作用在機體各個方向上的力矩
-
J 是3×3的機體慣性矩陣，因為四旋翼是對稱的剛體：
\[(11).J=
J_x & 0 & 0\\
0 & J_y & 0\\
0 & 0 & J_z
02-1-6 總的運動模型
整合02-1-4和02-1-5，我們可以得到四旋翼的運動模型：
\[(12).
T\\
\tau_x\\
\tau_y\\
\tau_z
\Gamma
-b & -b & -b & -b\\
0 & -db & 0 & db \\
db & 0 & -db & 0 \\
k & -k & k & -k
\omega_1^2\\
\omega_2^2\\
\omega_3^2\\
\omega_4^2
A
所有力的力矩都是旋翼轉速的函數，如果d>0，則A是滿秩的，可以通過求逆得到:
\[(13).
A^{-1}
這樣就可以求出對機體施加特定的力和力矩所需要的各旋翼轉速。
02-1-7 附：旋轉角加速度的計算
最後補充一下旋轉角加速度的計算：
由
\[(10).\mathcal{J}\dot{\omega}=-\omega×\mathcal{J}\omega+\Gamma\\
(11).J=
(13).
dT_4-dT_2\\
dT_3-dT_1\\
k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2)
知：
\[(14).
\dot{\omega_x}\\
\dot{\omega_y}\\
\dot{\omega_z}
-\omega_x\\
-\omega_y\\
-\omega_z
×
\omega_x\\
\omega_y\\
\omega_z
+
化簡後即可得到：
\[(15).\dot{\omega_x}=\frac{d(T_4-T_2)+(J_z-J_y)\omega_y\omega_z}{J_x}\\
\dot{\omega_t}=\frac{d(T_3-T_1)+(J_x-J_z)\omega_x\omega_z}{J_y}\\
\dot{\omega_z}=\frac{k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2)+(J_y-J_z)\omega_x\omega_y}{J_z}\\
02-2 控制條件
我們用一種嵌套式的控制結構來控制飛行器的運動，我們以繞y軸俯仰以及沿x軸平移為例說明。
具體控制思路為：
- 我們根據期望位置與實際位置的差異，控制轉速，因為根據前面的動力學分析可以知道，轉速同時影響俯仰和位移的加速度；
- 根據1的控制結果，我們控制俯仰力矩和X方向平移的力，使飛行器運動。
02-2-1 繞Y軸俯仰力矩
最内層用一個PID計算機體的(繞y軸的)俯仰力矩：
\[\tau_y = K_p(\theta_p^*-\theta_p)+K_d(\dot\theta_p^*-\dot\theta_p)
θ*p是目标俯仰角，θp是實際角度，實際角度θp是通過慣導系統估算得出的。
計算主要是基于目标俯仰角與實際角度之間的誤差，Kp和Kd的确定可以先根據近似動力學模型用經典控制設計方法給出初始值，再進行修正，來達到最佳控制效果。
02-2-2 沿X方向位移
為了研究運動變化的友善，我們在機體上連接配接一個坐标系{V}，它與變化前的{B}相同，在變化中保持不變。
要使機體沿X軸方向位移，我們要使機頭下俯(也即繞Y軸旋轉一個小角度)，來産生一個向X方向的分力fx：
\[f=R_y(\theta_p)
T\sin\theta_p\\
T\cos\theta_p
f_x=T\sin\theta_p\approx{}T\theta_p\quad(1)
我們采用比例控制法控制這個方向的加速度：
\[f_x^*=mK_f(^Vv_x^*-{}^Vv_x)\quad (2)
聯立(1)(2)，得：
\[\theta_p^*=\frac{m}{T}K_f(^Vv_x^*-{}^Vv_x)
即為為達預定前進速度所需要的俯仰角。上式中的實際飛行器速度可以有慣導或GPS接收器估算得到。
豎直方向上有重力和推力總和相等：mg=T。我們在仿真中需要這個條件來限制我們的轉速。
02-2-3 速度控制
接下來考慮最外層的控制律，即根據實際位置與目标位置的PID來控制速度v和ω。來實作上面所說的内層的俯仰和位移。
假設p表示機器人位置，v1是預期速度，則：
\[v_1 = K_p(p_1-p)\quad(p\in{}R^2)
我們将這個v1轉化到坐标系V中：
\[^Vv={}^VR_0(\theta_y)v={}^0R_V^T(\theta_y)v\\
^Vv_x \\
^Vv_y
={}
^0R_V^T(\theta_y)
v_x \\
v_y
02-3 Simulink仿真
這個圖具體實作還有一點複雜，但公式就是前面02-1和02-2推導的公式。教材上沒有将其中的一些子產品封裝，而此圖已經封裝完成，每個子產品的功能都标注出來了。看着也比較清晰。
mdl_quadrotor;
sim('sl_quadrotor');
about(result);
plot(result(:,1),result(:,2:3));
代碼結果是實作了飛行器起飛繞{V}的Z軸一圈然後用plot畫出來投影線。
03 簡單總結 | Review
按照慣例，通過本部分的學習，我們也需要總結出幾個宏觀的印象，以便于後續的深入學習：
- 機動性、位形空間、任務空間這些基本概念
- 以輪式機器人自行車模型為代表的運動學模型
- 以飛行機器人四旋翼為代表的動力學模型