人工智慧與智慧系統3-> 機器人學3 | 移動機器人平臺

機器人學的基本工具已經瞭解完畢，現在開始瞭解移動機器人，這部分包括機器人平臺、導航、定位。

所謂機器人平臺就是指機器人的物理結構及其驅動方式。本文將學習兩種典型移動機器人平臺(四旋翼和輪式車)的運動與控制，研究輸入控制訊號怎麼控制位姿隨著時間變化。

到這一部分網上的筆記突然變少 ≈ 0

但我想從這一部分開始，就更有意思了。

0207注：好難啊，機器人不愧是屠龍之術。

0212注：此文很長。已經到了可以分幾篇發出去的程度...由於此文很長建議結合目錄來看，每個小標題右側都可以召喚目錄。

本系列參考資料：

1. 《Robotics, Vision and Control》

2. B站公開課：

   1. 臺灣交通大學機器人學公開課
   2. Peter Corke 配套視訊
   3. 這本書的中文公開課(質量不算高但還算實用)【公開課】機器人學視覺控制- matlab模擬_嗶哩嗶哩_bilibili

00 機動性

0210附：機動性這部分物理、力學、控制概念極多，非相關專業會看著比較難受，推薦一個物理教育網站，閒暇可以玩玩：

物理概況 - 主題索引 (hypertextbook.com)

以及一個控制、力學相關的系列文章：

https://zhuanlan.zhihu.com/p/48662038

學習兩種平臺之前，有一個一般性的概念：機動性，可以幫助之後的理解。

所謂機動性，其實是機器人實現空間運動的相關概念集合。

1. 位形空間(Configuration space / C-space)：

   所有的可能的位置形態集合，我們記為C：

   \[位置座標\dot{q}\in \mathcal{C} \]

2. 任務空間(task space)

   機器人所有可能的位姿集合，任務空間依賴於具體應用或任務。記為T：

   \[位姿\xi\in\mathcal{T} \]

3. 自由度

   確定系統在空間中的位置所需要的最小座標數。

4. 廣義座標

   用來描述系統位形所需要的獨立引數或者最小引數

5. 欠驅動系統

   系統獨立控制變數個數小於系統的自由度

   即輸入的量要比控制的量要少的系統

   欠驅動系統

6. 我們可以舉幾個運動系統的例子來理解理解這幾個概念

   • 火車

     1. 對於火車來說，火車的位置可以用其沿軌道的距離資料來描述，即一個標量引數q來決定火車的位置；

     2. 位形空間(C空間)：即q描述的火車的位置構成的空間，q∈C；

     3. 任務空間：依賴於具體任務的意思就是說，比如火車是要沿直線導軌運動，我們只需要關心火車在一個直線上的位置，此時：

        \[\mathcal{T}\subset R \]

        同理，考慮火車在平面上的位置則：

        \[\mathcal{T}\subset R^2 \]

        三維同理。

     4. 自由度：1維，因為火車由一個獨立的標量引數q來完全決定

        但聯絡一下C空間和task space，當火車的任務空間維數超過位形空間維數時，那麼火車不能隨便到達任意一個任務空間中的位置，因為它是被約束在一條導軌上的。

        ?

     5. 廣義座標：即這個q

     6. 驅動情況：全驅動系統，因為火車有一個驅動器，推動火車沿著軌道前進或者後退

   • 氣墊船

     1. 氣墊船有兩個螺旋槳，軸線平行，兩個螺旋槳推力的合理形成一個向前的推動力。可以用q=(x,y,θ)來決定其位置；

     2. 位形空間，即q描述的位置構成的空間，q∈C；

        PS：

        \[這裡的C\subset{}R^2×S \]

     3. 任務空間同上個例子的討論

     4. 自由度，可見最小獨立描述座標數為3(x , y , θ)，自由度為3；

     5. 廣義座標：q

     6. 驅動情況：兩個驅動器，比自由度少，所以為欠驅動系統。

        欠驅動系統會限制自由運動的能力，我們任意時刻都可以控制氣墊船的前向加速度和旋轉加速度，但氣墊船的側向加速度始終為0；但是我們總能通過其他的運動來代替直接側向移動（雖然麻煩了一點）。

        但是對於這些工具來說，這滿足了最常用的前進的功能要求，並且驅動器較少。

        如果給這個船側向垂直於原兩個螺旋槳的方向增加一個螺旋槳，那麼就實現了平面上任意方向的運動，前進、側移、旋轉。這時，任務空間等效於位形空間。

   • 直升機

     1. 簡單介紹一下，4個驅動器+一個重力驅動，位姿由6個廣義座標描述q=(x,y,z,θ_rθ_pθ_y)，即三維空間中的位置xyz，以及滾轉-俯仰-偏轉的姿態角度。

     2. 自由度為6.是欠驅動系統。

     3. \[任務空間\mathcal{T}\subset SE(3) \]

   • 固定翼

     1. 與直升機類似，驅動器（向前推進器、副翼、升降舵和方向舵）位形空間等與直升機相同。

   • 汽車/坦克

     1. 兩驅動器，一個前後運動、另一個改變行駛方向
     2. q = (x, y, θ)，三個維度
     3. 欠驅動
     4. 車體轉彎時由於由橫向摩擦力，所以不需要額外驅動器提供這個力
     5. 可以實現側滾的輪子已經研發出來了：全向輪、麥克納姆輪：麥克納姆輪淺談

7. 補充幾個概念：

   1. 再次感謝這個系列對於我認知概念的指引，機動性這部分力學內容很多：

      思考(8)李括號, 完整與非完整約束的區別

   2. 完整約束和非完整約束：

      1. 完整約束可以用位形q的一個方程F(q, t)來描述；

      2. 非完整約束約束方程中包含座標對時間的導數（如運動約束），

         \[F(q,\dot{q},t)且不能通過積分轉化為F(q,t) \]

         而且方程不可能積分成有限形式，這類約束成為非完整約束。

   3. 非完整系統、完整系統：

      • 非完整系統是指典型的受非完整約束系統，比如：車輛；標誌是時變控制策略。
      • 反之為完整系統。

這裡這些概念的理解其實有待商榷，我沒有完全在腦海裡形成對這些概念的具象印象，我搜了很多，大多是運動規劃方面的名詞，但是並不完全對應(因為我對運動規劃也並不瞭解)，這對我的學習造成了很大的困難。

所以我認真研究了書上的例子，對這些名詞進行對應，大概好了一點。

01 輪式機器人

四輪機器人的常用模型是雙輪自行車模型，即兩後輪固定在車體上，兩前輪可以繞水平軸轉動以實現車輛轉向。

01-1 概念及模型

01-1-1 模型的相關概念引入

模型圖如下，淺灰色是車體，深色是近似的自行車兩輪，位形q=(x,y,θ)∈C（SE(2)）。其中x是在世界座標系O中x的座標，y是O中y的座標，θ是指 車身與O座標系x的夾角。

1. 根據機器人學1部分的內容，對車輛建立物體座標系V（圖中紅色），γ是前輪的轉向角，是前輪與Vx軸的夾角，

2. 左上角的ICR是小車的旋轉瞬心（後輪前輪的中垂線交點，轉彎時可以看作圓周運動；

3. R1和R2分別是後輪前輪到ICR的舉例

4. L是輪基的長度。

5. V中速度及角速度：

   1. \[^V\dot{x}=v\\ ^v\dot{y}=0\\ \dot\theta=\frac{v}{R_1} \]

      （因為後輪與v方向一致）

   2. 易知圖中轉向的圓心角為γ，則

      \[R_1 = \frac{L}{\tan{\gamma}} \]

   3. 從上式可以看出，轉彎半徑R1會隨著車輛長度(輪基L)的增加而增加，γ的大小是由機械結構限制的，R1最小時，γ最大。

   4. R2>R1，前輪會比後輪多走一段路，並且兩個前輪的情況也不同，所以兩個前輪的轉向角度也應該有所不同，可以通過阿卡曼(模型)轉向機構實現：

   

   阿克曼模型 ≠ 自行車模型

   當然，兩個前車輪轉彎時要以不同的速度轉動，所以發動機和驅動輪之間要按轉一套差動齒輪箱。

6. 座標系O中速度及角速度：

   1. \[\dot{x}=v\cos\theta\\ \dot{y}=v\sin\theta\\ \dot{\theta}=\frac{v}{L}\tan\gamma \]

   2. 這就是小車的運動學模型，即只描述了小車的運動速度，沒有涉及力和力矩的建模；

   3. 第三個式子表達了θ的變化率，也叫轉動速率，可以通過陀螺儀(上一部分的慣性導航部分?)直接測量，也可以通過左右輪的角速度值推算出來；

   4. \[\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的引數順序與我們預想正好反了，修改即可：

參考：

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 控制條件

如何小車運動到一個特定的位姿呢，之前我們只實現了控制小車到指定目標點，但並沒有控制位姿，或者說，最終的位姿會依賴於起始位姿。

下面我們嘗試控制最終的目標位姿，雖然只增加了一個條件但是需要考慮的事情複雜了一些：

回憶一下我們之前的運動控制式：

\[\dot{x}=v\cos\theta\\ \dot{y}=v\sin\theta\\ \dot{\theta}=\frac{v}{L}\tan\gamma \]

我們將其重寫為：

\[\left[\begin{matrix} \dot{x}\\ \dot{y}\\ \dot{\theta} \end{matrix}\right] = \left[\begin{matrix} \cos\theta & 0\\ \sin\theta & 0\\ 0 & 1 \end{matrix}\right] \left[\begin{matrix} v \\ \dot{\gamma} \end{matrix}\right] \]

可見差別是第三行，但實際上是等價的，這是因為：

\[\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軸的夾角，加負號是因為方向

帶入後我們得到：

\[\left[\begin{matrix} \dot{\rho}\\ \dot{\alpha}\\ \dot{\beta} \end{matrix}\right] = \left[\begin{matrix} -\cos\alpha & 0\\ \frac{\sin\alpha}{\rho} & -1\\ -\frac{\sin\alpha}{\rho} & 0 \end{matrix}\right] \cdot \left[\begin{matrix} {v}\\ \dot{\gamma} \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。再次代入，閉環控制系統為：

\[\left[\begin{matrix} \dot{\rho}\\ \dot{\alpha}\\ \dot{\beta} \end{matrix}\right] = \left[\begin{matrix} -k_\rho\rho\cos\alpha\\ k_\rho\sin\alpha-k_\alpha\alpha-k_\beta\beta\\ -k_\rho\sin\alpha \end{matrix}\right] \\ 穩定條件為：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和γ來控制小車運動。

1. 移動到一個點

   追蹤一個固定位置的點，到達目的地速度必須為零，沒到達目標點時需要控制γ使得小車轉向目標點，但不能控制小車最終的位姿，最終位姿依賴於初始位姿。

2. 跟蹤一條直線

   γ的設定要使小車不斷接近目標直線，並且小車到達直線時小車平行於直線，由於直線無限長，小車到直線上時仍有速度，所以不控制速度。

3. 跟蹤一般路徑

   追蹤一個沿固定路徑運動的目標點，v仍受距離控制，不同的是當小車運動到安全距離值時，小車會調整自己的速度使自己不超過目標點。

4. 運動到一個位姿

   將運動學模型改寫，並寫成極座標形式，採用線性控制使得小車沿著一條直線向目標運動，並不斷調整這條線的角度。

02 飛行機器人

講道理這部分只是很簡單的建模介紹，關於無人機飛控演算法，有很多書專門講這個，我們先了解一下控制過程即可，可能會有點繁瑣和困難。先放兩個教程連結：

1. 多旋翼飛行器設計與控制 北航
2. 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) \]

1. 翻滾角Φ，表示機體座標系繞OX軸旋轉的角度，如果從飛行器尾部順縱軸向前看，如果OZ軸位於鉛垂面的右側，則Φ為正；
2. 俯仰角θ，表示機體座標系繞OY軸旋轉的角度，旋轉後飛行器橫軸指向水平面上方為正；
3. 偏航角ψ，表示機體繞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}= \left[\begin{matrix} 0\\ 0\\ mg \end{matrix}\right] - {}^oR_B \left[\begin{matrix} 0\\ 0\\ T \end{matrix}\right] \]

解釋：

1. v是飛行器在世界座標系中的速度；
2. g是重力加速度；
3. m是飛行器的總質量；
4. T是向上的總推力；
5. oRB這個矩陣是將後面的向量轉化到世界座標系下；

簡單講就是重力減去推力，總推力T本在機體座標系下，將其轉化到世界座標系。

\[(3).{^oR_B}=R_xR_yR_z= \left[\begin{matrix} \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 \end{matrix}\right] \]

綜合(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 \]

  1. ω 是角速度向量 (3×1)

  2. Γ=(τ_x, τ_y, τ_z)^T  是作用在機體各個方向上的力矩

  3. J 是3×3的機體慣性矩陣，因為四旋翼是對稱的剛體：

     \[(11).J= \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right] \]

02-1-6 總的運動模型

整合02-1-4和02-1-5，我們可以得到四旋翼的運動模型：

\[(12). \left[\begin{matrix} T\\ \tau_x\\ \tau_y\\ \tau_z \end{matrix}\right] = \left[\begin{matrix} T\\ \Gamma \end{matrix}\right] = \left[\begin{matrix} -b & -b & -b & -b\\ 0 & -db & 0 & db \\ db & 0 & -db & 0 \\ k & -k & k & -k \end{matrix}\right] \left[\begin{matrix} \omega_1^2\\ \omega_2^2\\ \omega_3^2\\ \omega_4^2 \end{matrix}\right] = A \left[\begin{matrix} \omega_1^2\\ \omega_2^2\\ \omega_3^2\\ \omega_4^2 \end{matrix}\right] \]

所有力的力矩都是旋翼轉速的函式，如果d>0，則A是滿秩的，可以通過求逆得到:

\[(13). \left[\begin{matrix} \omega_1^2\\ \omega_2^2\\ \omega_3^2\\ \omega_4^2 \end{matrix}\right] = A^{-1} \left[\begin{matrix} T\\ \tau_x\\ \tau_y\\ \tau_z \end{matrix}\right] \]

這樣就可以求出對機體施加特定的力和力矩所需要的各旋翼轉速。

02-1-7 附：旋轉角加速度的計算

最後補充一下旋轉角加速度的計算：

由

\[(10).\mathcal{J}\dot{\omega}=-\omega×\mathcal{J}\omega+\Gamma\\ (11).J= \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right]\\ (13). \Gamma = \left[\begin{matrix} \tau_x\\ \tau_y\\ \tau_z \end{matrix}\right] = \left[\begin{matrix} dT_4-dT_2\\ dT_3-dT_1\\ k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2) \end{matrix}\right] \]

知：

\[(14). \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right] \left[\begin{matrix} \dot{\omega_x}\\ \dot{\omega_y}\\ \dot{\omega_z} \end{matrix}\right] = \left[\begin{matrix} -\omega_x\\ -\omega_y\\ -\omega_z \end{matrix}\right] × \left[\begin{matrix} J_x & 0 & 0\\ 0 & J_y & 0\\ 0 & 0 & J_z \end{matrix}\right] \left[\begin{matrix} \omega_x\\ \omega_y\\ \omega_z \end{matrix}\right] + \left[\begin{matrix} dT_4-dT_2\\ dT_3-dT_1\\ k(\omega_1^2+\omega_3^2-\omega_2^2-\omega_4^2) \end{matrix}\right] \]

化簡後即可得到：

\[(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. 我們根據期望位置與實際位置的差異，控制轉速，因為根據前面的動力學分析可以知道，轉速同時影響俯仰和位移的加速度；
2. 根據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) \left[\begin{matrix} 0\\ 0\\ T \end{matrix}\right] = \left[\begin{matrix} T\sin\theta_p\\ 0\\ T\cos\theta_p \end{matrix}\right]\\ 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表示機器人位置，v₁是預期速度，則：

\[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\\ \Downarrow\\ \left[\begin{matrix} ^Vv_x \\ ^Vv_y \end{matrix}\right] ={} ^0R_V^T(\theta_y) \left[\begin{matrix} v_x \\ v_y \end{matrix}\right] \]

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

按照慣例，通過本部分的學習，我們也需要總結出幾個巨集觀的印象，以便於後續的深入學習：

1. 機動性、位形空間、任務空間這些基本概念
2. 以輪式機器人自行車模型為代表的運動學模型
3. 以飛行機器人四旋翼為代表的動力學模型