基于Unity物理引擎的多旋翼無人機仿真模型①

石百玉, 高 岑

1(中國科學院大學, 北京 100049)

2(中國科學院 沈陽計算技術研究所, 沈陽 110168)

采用虛擬仿真技術構建現實場景仿真系統已經是各行各業應用場景中比較成熟的一種技術應用模式. 虛擬仿真技術可以通過三維建模搭建可重復過程的特性很好的解決現實物理世界中不易復現的各類場景, 從而進行相關應用問題的分析. 應用虛擬仿真技術構建的仿真系統能夠在降低實物成本和保證人員安全的前提下,在逼真的虛擬環境中為用戶提供高真實感的體驗, 并全程實現量化數據的跟蹤, 進而有效提高用戶對實際場景的應變決策能力. 隨著近幾年各行業中信息化建設水平的不斷提高, 在軟硬件配套設施及相關工程管理技術發展創新的推動下, 虛擬仿真信息化過程也不斷演進變化, 從單純的三維模型建模、三維模型展示、三維場景過程展示向實景仿真及數據實時交互反饋評估等方向發展. 在此基礎上, 與各行業相匹配的虛擬仿真系統應運而生, 作為評估過程、演練演習的重要手段.

Unity 是行業領先的跨平臺實時3D開發引擎, 廣泛的應用在各種專業技術領域, 可用于創作、運行實時互動的2D和3D內容. 基于 Unity3D開發的虛擬仿真系統為用戶提供了豐富的學習條件與逼真的虛擬環境. Wang 等人[1]提出Unity在虛擬仿真和虛擬現實等方面具有廣泛應用的價值.

目前, 隨著相關領域的迅速發展, 多旋翼無人機廣泛的應用在警備、城市管理、農業、地質、氣象、電力、搶險救災、視頻拍攝等行業. 多旋翼無人機集成了飛行控制、慣性導航、光電偵察等多種設備于一身的復雜裝備, 其價格昂貴、壽命有限, 不便用戶直接使用無人機進行訓練來提高操作技術[2]. 在虛擬仿真技術發展背景下, 使用虛擬仿真無人機提高用戶操作能力,能夠減少訓練成本, 擴展無人機應用領域, 對探索更加科學有效的訓練方法具有重大意義.

對于虛擬仿真來說, 根據仿真應用的需求, 對應用領域的相關屬性進行科學的抽象和相應的描述, 是從現實世界邁進仿真世界的重要前提. 模型是仿真活動的基礎. 仿真模型不僅僅是使用數學公式對系統模型的運行規律進行建模, 而且還對虛擬環境和虛擬對象的視覺外觀、物理特性進行建模.

馬忠麗等人[3]、王小青等人[4]、彭玉元等人[5]的相關研究, 重點在于仿真平臺系統的設計應用, 將重點放在系統的設計上, 取得了較好的成果, 但并沒有對無人機的仿真模型進行深入的研究.

1 多旋翼無人機運動分析

研究無人機仿真模型, 首先需要分析多旋翼無人機的基本組成和飛行原理.

多旋翼無人機通常由機架、動力系統、控制系統組成. 多旋翼無人機機身常見布局如圖1所示, 包括三旋翼、四旋翼、六旋翼和八旋翼等.

圖1 多旋翼無人機旋翼常見布局

本文以QuadX型四旋翼無人機為例, 分析多旋翼無人機飛行原理. 無人機采用4個旋翼作為直接動力源, 通過改變機架上不同位置的電機轉速來調整飛行器自身的姿態實現空間內6自由度運動, 即垂直、俯仰、滾轉、偏航運動. 將機頭前進方向右側的電機命名為M1, 以機身逆時針方向將其與電機命名為M2,M3, M4. 通常M1, M3逆時針旋轉、M2, M4順時針旋轉來使相鄰電機的以不同轉向來抵消扭矩.

將4個電機的轉速同時增大或減小即可實現垂直運動; 將M1、M3的轉速增加或者將M2、M4的轉速減小, 實現向右偏航. 反之, 實現向左偏航; 將M1、M2的轉速減小或者將M3、M4轉速增加時, 四旋翼會產生前傾的扭矩, 實現俯仰運動, 螺旋槳拉力的垂直分力抵消重力, 在前進方向的分力使四旋翼向前飛行,如圖2所示. 同理可知無人機左右飛行原理. 其他機身布局飛行原理相似.

圖2 無人機前進運動

2 四旋翼無人機建模

本節就多旋翼無人機進行控制模型進行建模, 主要包含動力系統建模和控制系統建模. 其中信號傳遞如圖3所示.

圖3 模型信號傳遞

2.1 無人機動力系統建模

無人機動力系統建模分為3部分: 螺旋槳建模、電機建模、電池建模.

多旋翼無人機常采用定槳距螺旋槳, 根據Dai等人[6]的螺旋槳建模方法, 螺旋槳拉力T (單位: N)和轉矩M (單位: N·m)表示如下:

其中, CT為拉力系數, CM為轉矩系數, ω為螺旋槳轉速, DP為螺旋槳直徑, ρ為空氣密度. 文獻[7]給出CT,CM的計算公式為:

其中, HP為螺旋槳槳距, DP為螺旋槳直徑, BP為槳葉數, 同時給出了參數的平均取值: A=5, ε=0.85, λ=0.75,ζ=0.5, e=0.83, Cfd=0.015, α0=0, K0=6.11.

目前, 多旋翼無人機使用無刷直流電機, 對于無刷直流電機轉速N有:

其中, KV為電機KV值, 定義為電機輸入電壓每增加1伏特電機空轉轉速增加的轉速, 單位為RPM/V; U為電機輸入電壓. 在不考慮電機機械損耗和電機負載轉矩情況下, 視電機轉速N為螺旋槳轉速.

對于電池模型, 主要目的是利用電池電壓計算電機轉速, 進而根據螺旋槳模型求得某電壓下螺旋槳產生的拉力和轉矩. 因此簡化放電過程, 假設放電過程中電壓保持不變, 忽略電池、電調、電機內阻的損耗, 只關注電池總電壓.

考慮到模型的擴展性和易維護性, 根據模塊化編程思想, 創建Propeller類實現螺旋槳模型的拉力、扭矩算法, 便于模型升級與擴展. 為了減少機器計算量, 在構造方法中將固定已知的參數提前計算存儲. 電機類Motor、電池類Battery同理. 其中Motor類以引用的形式調用Propeller類、Battery類的方法, 以屬性的形式獲取或賦值當前電機的電壓參數, 實現在Unity腳本生命周期中, 每固定0.02 s調用一次的FixedUpdate()方法, 部分代碼如下:

var rotateSpeed = getRotateSpeed(EngineVol);var Pull = _Propeller.getPull(rotateSpeed);var counterTorque =_Propeller.getTorque(rotateSpeed) * Mathf.Sign((float)Direction);motorRigidbody.AddForce(transform.up * Pull); bodyRigidbody.AddTorque(bodyRigidbody.transform.up * counterTorque);

其中motorRigidbody、bodyRigidbody分別是電機三維模型、無人機三維模型的Rigidbody組件; Direction為枚舉類型當前電機轉向, 順時針賦值1, 逆時針賦值-1, 調用Mathf.Sign()方法獲得符號, rotateSpeed的值由式(6)求得.

Rigidbody.AddForce()、Rigidbody.AddTorque()方法是Unity腳本API, 作用分別是向 Rigidbody 添加力、相對扭矩.

至此, 無人機動力系統建模完畢.

2.2 無人機控制系統建模

控制系統要解決機體在哪、姿態如何、如何飛行的問題, 其中姿態估計是控制模型的決策的基礎, 姿態估計主要目的是估計姿態角, 測量加速度、角速度和位置等信息, 解決機體位置、機體姿態的問題.

多旋翼無人機通常使用IMU傳感器單元進行無人機的姿態估計, 使用GPS傳感器確定無人機的位置.慣性測量單元(inertial measurement unit, IMU)包括加速度計和陀螺儀, 用來檢測和測量物體加速度與旋轉運動的傳感器.

在Unity, 可直接調用Rigidbody.angularVelocity腳本API即可得物體的角速度, Transform.position可獲得物體位置. 創建Sensors類封裝該API, 但Unity的腳本API中并沒有可直接獲得加速度的方法, 因此根據加速度表達式(7)間接求出物體加速度, 角加速度同理.

在Unity腳本生命周期中, 每固定0.02 s調用一次FixedUpdate()方法, 調用間隔時間較短, 視為Δt. 線加速度計算代碼如下:

previousVelocity = currentVelocity;currentVelocity = rgb.velocity;acceleration = new Vector3((currentVelocity-previousVelocity).x/FixedUpdateTime,(currentVelocity-previousVelocity).y/FixedUpdateTime,(currentVelocity-previousVelocity).z/ FixedUpdateTime);

將上述代碼寫入FixedUpdate()方法中, 即可計算出物體每一固定幀的運動線加速度, 其中rgb為物體剛體Rigidbody組件, previousVelocity、currentVelocity為Vector3類型變量, FixedUpdateTime為float型變量,值為0.02f即FixedUpdate()的調用周期.

解決完機體在哪、機體姿態的問題后, 接下來需要解決如何飛行的問題, 這需要Controller類能夠從Sensors類中獲取機體姿態信息并根據輸入的控制指令計算出達成目標姿態所需的拉力和力矩, 然后對無人機電機進行動力分配, 實現無人機的運動控制.

為了使輸出能夠符合期望的輸入指令, 通常情況下選擇PID控制器. PID控制是應用最廣泛的一種工業控制算法, 距今已有近百年的歷史. 由于四旋翼無人機為數字控制系統, 選用離散式PID. 離散式PID控制器分為位置式和增量式兩種算法形式. 其中增量式計算結果只與最近3次的偏差有關, 解決位置式積分運算的誤差積分積累問題, 因此該控制系統模型選用增量離式PID控制算法. 離散式PID控制其表達式如下:

由式(8)得:

即:

其中, KP、KI、KD分別為比例系數、積分系數和微分系數, u(k)是第k次采樣時刻的計算結果, e(k)是第k次采樣時刻控制器的偏差. 實現代碼實現如下:

float err = pid.exp-pid.act;float increment =pid.kp * (err-pid.errK1) + pid.ki * err +pid.kd * (err-2.0f * pid.errK1+pid.errK2);pid.output+=increment;pid.errK2=pid.errK1;pid.errK1=err;

代碼中, 以結構體形式定義PID對象, 結構體內存儲PID控制參數、期望值、目標值、兩次誤差值和輸出值, 以引用形式傳入PID算法中, 在算法中讀寫相關參數, 即代碼中結構體變量pid.

當輸出量與被控制系統呈線性關系時, 單極PID能獲得較好的效果. 而多旋翼無人機通常可以簡化為一個二階阻尼系統[8], 是非線性系統. 螺旋槳轉速和升力是平方倍關系, 單極PID在多旋翼無人機上很難取得好效果. 而串級PID控制系統提高了穩定性、可靠性和有效性, 克服了傳統PID 精度不高的問題[9].

串級PID控制就是兩級PID控制串聯在一起, 分為內環PID控制和外環PID控制, 外環的輸出作為內環的輸入, 如圖4所示. 對無人機的3個姿態角做PID控制, 外環輸入反饋的是角度數據, 內環輸入反饋的是角速度數據. 這樣即使外環數據劇烈變化, 而內環數據不會發生突變.

圖4 串級PID控制系統示意圖

確定了控制算法后, 分析控制輸入值、輸出值和反饋值的關系. 在姿態控制中, 需要控制俯仰、橫滾、偏航3個姿態角, 即分別需要3個串級PID來控制無人機姿態. 這3個串級PID的外環的輸入值都是其期望的姿態角度, 輸出值是期望的角速度; 內環的輸入是外環輸出的期望角速度, 輸出值是期望的角加速度. 最終作用于物體后, 外環的反饋值是物體當前的實際姿態角, 內環反饋的是物體當前的實際角速度. 在模型中,期望值由輸入控制腳本提供, 測量的實際值由前文提及的Sensors類提供. 同時還要對無人機的飛行高度進行串級PID控制, 以此實現無人機的懸停運動, 高度控制PID外環輸入是無人機實際高度, 內環輸入是無人機垂直方向上的速度.

在實際應用時發現, 俯仰、橫滾運動中, 俯仰、橫滾角取值范圍在-90°～+90°控制幅度較小, 測量值即可作為輸入的實際, 而偏航角取值范圍在-180°～180°,當測量值超過180°時測量值符號會相反, 此時會造成輸入誤差, 需要特殊處理. 這里以向量方法求偏航角,構造單位向量:

由向量夾角公式:

求得向量間夾角, 再由向量叉乘確定夾角方向, 其結果作為偏航角控制的測量值.

對3個姿態角分別采用串級PID控制, 使多旋翼無人機控制系統的非線性多變量輸入多變量輸出控制問題簡化為兩變量輸入單變量輸出問題.

根據QuadX形布局的多旋翼無人機的運動原理知,電機M1轉速對俯仰運動有負向作用、對橫滾有正向作用, 對偏航有正向作用; 電機M2轉速對俯仰運動有負向作用、對橫滾有負向作用, 對偏航有負向作用; 電機M3轉速對俯仰運動有正向作用、對橫滾有負向作用,對偏航有正向作用; 電機M4轉速對俯仰運動有正向作用、對橫滾有正向作用, 對偏航有負向作用. 于是有:

其中, P1、P2、P3、P4分別代表驅動電機M1、 M2、M3、M4所需的PWM值, Ph為維持無人機飛行高度PWM值, Ppit、Prol、Pyaw分別為俯仰角、橫滾角和偏航角的所需PWM值, 最后結果值限制在電機驅動器能接受的范圍內. 將求得的輸入電壓分別驅動4個電機, 電機輸入電壓的改變引起電機轉速的改變, 進而改變整個系統的受力情況, 實現對無人機的姿態控制.

在該模型中, 不考慮電機的飽和區, PWM值為電機輸入電壓占電池總電壓的比值. 另外, 為了保證基本面姿態控制與抗風性需求, 一般多旋翼無人機在飛行時控留有制裕度, 油門指令應該小于0.9[7].

至此無人機控制系統建模完畢.

3 四翼無人機模型實現

3.1 Unity腳本實現

確定了多旋翼無人機的算法模型后, 著手對模型算法的實現, 編寫Unity腳本來實現對無人機對象的邏輯控制. 綜合考慮各個腳本之間的關系, 設計UML時序圖如圖5.

圖5 UML時序圖

UserInput腳本用于處理用戶輸入, 在多旋翼無人機中的遙控中, 需要控制3個姿態角和油門, 一共4個方向軸上的控制, 因此需要在Unity Input Manager中設置4個軸向控制來相應鍵盤輸入. 由于無人機的姿態控制可以維持無人機的姿態, 在此設計油門輸入實際是控制無人機的飛行高度. 依據航模遙控“美國手”手柄模式, 設置鍵盤w、s控制油門; 鍵盤a、d控制偏航; 方向鍵上下控制俯仰; 方向鍵左右控制橫滾.

UserInput腳本的輸入即為無人機的期望高度和期望姿態, Controller腳本獲得無人機期望姿態后, 調用PID算法計算期望輸出, 并將動力分配給Motor腳本,而后將目標轉速送入Propeller腳本中計算單個電機所產生的拉力和轉矩, 直接作用在無人機對象的剛體上產生力的作用, Sensors腳本實時監控無人機對象的狀態, 反饋給Controller腳本實現閉環控制. Controller腳本流程如圖6.

圖6 Controller腳本流程圖

3.2 Unity無人機對象組件設置

在Unity中使用包圍盒碰撞檢測, 在游戲對象上掛載Collider組件實現, 同時可以在腳本中實現碰撞后的受力效果, 無人機對象添加Collider效果如圖7.

圖7 無人機對象

在多旋翼無人機的運動中, 僅有機體位置和螺旋槳旋轉變化, 設置螺旋槳旋轉動畫會讓仿真更加逼真,給予用戶沉浸體驗. 創建Animator腳本, 該腳本獲取Motor腳本的電機轉速, 將其應用到螺旋槳上, 使螺旋槳產生旋轉效果. 然而在現實中, 螺旋槳旋轉由于視覺暫留而產生模糊效果, 轉速越快模糊效果越明顯, 而在Unity的旋轉是在每渲染幀刷新對象狀態, 使對象“瞬移”看起來不夠真實, 對此可以使用模糊紋理貼圖的代替三模模型旋轉, 當螺旋槳低速旋轉時使用真實三維模型旋轉, 當轉速逐漸提高時, 隱藏三維模型, 使用平面網格模型, 其材質使用提前繪制出模糊紋理的透明圖片(圖8), 實際旋轉效果如圖9, 其中M1, M2沒有使用模糊紋理替換, 看出有很明顯差別.

圖8 模糊紋理

圖9 模糊紋理替換旋轉效果

為了實現Unity中無人機的控制邏輯, 需要對無人機對象掛載所需的腳本及相關組件, 掛載組件方式如圖10.

圖10 無人機GameObject組件掛載關系

Rigidbody剛體組件是實現游戲對象物理特性必不可少的組件, 剛體為游戲對象賦予物理屬性, 使游戲對象在Unity物理引擎的驅動下產生力的作用, 實現真實世界中的運動效果. 本文將無人機受力模型直接應用在電機對應位置, 來仿真現實物體的受力狀態, 從而免去了對模型的剛體數學建模.

在Drone對象中, Body是無人機機體三維模型對象, Camera是云臺相機三維模型對象, Motors是所有電機、螺旋槳三維模型對象的父物體.

Motors對象中, Motor_1等是無人機的電機對象,每個電機對象中的Fixed Joint組件將電機剛體和父物體剛體固定在一起; 其中Motor腳本用于對父物體產生實際的力和扭矩.

在Motor腳本中, 計算螺旋槳產生的拉力扭矩并直接作用電機的剛體上, 電機模型剛體帶動整個無人機剛體運動. 所有的電機模型所產生的總拉力、總扭矩的作用效果由Unity物理引擎實現. 值得注意的是,在Controller腳本的初始化中, 根據4個電機的位置將無人機的重心設置在對角電機連線的交點處, 否則無人機在飛行過程中可能產生不穩定的效果.

另外Prop_1等是螺旋槳三維模型, 以Motor_1等對象作為父物體, 這樣可以使螺旋槳跟隨電機對象; 而Prop_Blur1等為上述模糊紋理螺旋槳對象.

4 運行效果

實驗分別以無人機懸停、俯仰、滾轉、偏航運動效果作為實驗結果, 實驗中無人機數據參數如表1.

表1 無人機數據參數

4.1 PID參數整定

無人機的姿態控制采用串級PID控制算法, 內環的輸出是最終結果, 所以先調內環參數, 再調外環參數.經過實驗測試, 確定該無人機姿態控制PID參數如表2.

表2 無人機PID參數

在PID參數整定過程中, 使用Unity調用Debug.Log()方法將無人機姿態數據實時打印出來, 將打印結果使用 Matlab 描繪控制曲線如圖11.

圖11 PID姿態控制曲線

由圖11可見, 在調整偏航角時會影響到俯仰和滾轉運動, 產生較大波動. 為解決此問題, 使機體在一定誤差范圍內調整完俯仰、滾轉運動后再調節偏航運動,實驗結果如圖12所示, 可見限制偏航運動在一定程度上增加了穩定性, 同時限制偏航運動的俯仰滾誤差與偏航響應時間呈現負向關, 限制誤差值越小, 偏航響應越慢.

圖12 限制偏航運動的PID姿態控制曲線

運行場景, 使用鍵盤控制無人機飛行, 進行姿態控制, 無人機能夠實現俯仰控制前進后退、 滾轉控制左移右移、偏航實現左轉右轉, 如圖13所示.

圖13 無人機模型實際運行效果

5 結論與展望

本文基于Unity物理引擎, 對四旋翼無人機這種典型非線性、多變量、欠驅動系統進行建模, 將無人機受力直接應用在電機對應位置, 更貼近現實物體受力狀態, 免去了對模型的剛體數學建模, 使建模化繁為簡;在模型的姿態控制采用串級PID算法對無人機姿態進行控制, 飛行實驗結果證明控制該模型的穩定性、有效性, 滿足無人機仿真要求. 該模型既滿足了仿真模擬的真實性又簡化了建模過程, 為無人機仿真模擬提供了一條新思路. 不過本文沒有考慮電機、電調、電池等物理特性、螺旋槳的氣動阻力等, 有待進一步研究.