獨輪機器人姿態控制與慣性導航的設計與實現

李文良，王子豪，徐媛媛，唐笠雄，顏斌，陳龍

（杭州電子科技大學 電子信息學院，浙江杭州，310018）

0 引言

獨輪車機器人是一種依靠單輪接地平衡的非穩定性系統，比起兩輪平衡車自平衡系統，雖然獨輪機器人自平衡系統的結構精簡，將地面接觸面積降到最小，但也因此需要通過多維的動態平衡控制來實現系統達到自平衡狀態[3]。而獨輪機器人系統概念的提出，必然引出很多難題：如系統的結構設計、動力學建模和姿態控制等，進一步引起姿態更新算法與控制策略趨于多維模型的創新和變革。

截至目前，研究關于獨輪機器人的系統方面的論文百花齊放，深入到獨輪機器人系統的方方面面，例如：動力學建模與控制策略[4~6]、姿態更新算法的創新與延伸[7~11]等等。本文則是提出一種可行性高、響應速度快、抗干擾性強以及導航精準度高的獨輪機器人姿態控制與慣性導航的實現方法。

1 系統總體方案設計

系統總體設計框圖如圖1 所示。實際車模圖如圖2 所示。在系統硬件設計上，獨輪機器人系統的核心控制器使用英飛凌公司的TC264 單片機，將其作為主控模板。姿態傳感器模塊則選用九軸傳感器IMU963RA，電機控制模塊由直流電機模塊和無刷電機模塊組成，速度傳感器模塊選用1024 線霍爾編碼器，此外還在主板上增設了許多配件模塊。

圖1 系統總體設計框圖

圖2 實際車模圖

而在系統軟件設計上，主要功能實現使用四元數融合濾波解算姿態角，姿態控制選用串級PID 控制，采用簡略捷聯式慣性導航確定位置坐標。在調試過程中的輔助功能：使用OLED 模塊與按鍵模塊相配合實現在OLED 屏幕上顯示參數和參數調整功能；使用UART 串口實時傳輸濾波數據到電腦上的匿名上位機進行調試；使用SD 卡模塊存儲獨輪機器人調試時的參數。

2 系統硬件電路設計

2.1 硬件電路總體設計

硬件總體設計采用TC264 為運算核心的設計方案，整套硬件系統可分為三個模塊，分別為主控模塊、電機驅動模塊、電源模塊。

2.2 主控模塊設計

主控模塊的設計采用TC264 單片機，由于此單片機功率較大，在電源模式選擇中選用外部3.3V、內部LDO1.3V電源模式供電[12]。主控核心原理圖如圖3 所示。

圖3 主控核心原理圖

2.3 電機模塊設計

電機驅動電路一般采用驅動芯片和MOS 管組合搭建H橋，對于行進輪有刷直流電機我們采用兩片全橋驅動芯片BTN8962，該芯片雖然導通內阻較大發熱較嚴重、死區時間較長，但鑒于獨輪機器人速度較慢，使用BTN8962 完全足夠。有刷電機驅動電路如圖4 所示。

圖4 有刷電機驅動電路圖

平衡飛輪則使用直流無刷電機驅動。由于電機自帶驅動電路，所以只需接線即可。直流無刷電機驅動原理如圖5所示。

圖5 直流無刷電機驅動原理圖圖

2.4 電源模塊設計

電源模塊主要分成三個部分：主板電源模塊、3.8V 電源轉換、3.3V 電源轉換。

主板電源模塊為主板提供所需要的各種電源，主電源由鋰電池提供，此電池提供的電壓經測量范圍為22.2V~25.6V，首先需要對24V 進電電源進行濾波。同時，對電壓大小進行檢測，實時充電。電源原理如圖6 所示。

圖6 電源原理圖

3.8 V 電源由24V 電源轉換得到，采用TPS5430 開關電源進行轉換，MP1584 電壓輸入范圍4.5V~28V，最大載流可達3A，開關頻率可達1.5MHZ，效率很高，雖然是開關電源，在合理選擇參數和布局的情況下輸出紋波可以控制在50mV 以下。24V 轉3.8V 原理如圖7 所示。

圖7 24V 轉3.8V 原理圖

3.3 V 電源由AMS1117 轉換得到，作為一款經典的線性穩壓芯片，AMS1117 具有輸出電壓穩定、電源紋波小、電路簡單的特點，實際運用中采用AMS1117 為TC264 供電來減少電源紋波的干擾。AMS1117轉換3.3V原理如圖8所示。

圖8 AMS1117 轉換3.3V 原理圖

3 系統軟件設計

3.1 軟件流程設計

系統軟件部分主要由：串級PID 平衡控制、四元數姿態解算、簡略捷聯式慣性導航系統這三個部分組成。獨輪機器人系統在串級PID 平衡的控制下可以保持直立，在慣性導航確定目標點后，可以通過速度環以動態速度到達目標點位，還能夠使用路徑規劃，規劃不同的路徑到達。

3.2 串級PID 控制

串級PID 控制由三個大環組成：Pitch 直立環、Yaw 轉向環、Roll 直立環；其中Pitch 直立環用于前后姿態控制；Roll 直立環用于左右姿態控制；Yaw 轉向環用于獨輪機器人的轉向與壓彎調控。總體串級PID 控制結構設計如圖9所示。

圖9 總體串級PID 控制結構設計圖

串級PID 控制是基于PID 控制器，將可觀測到的多個輸入量全部用于統籌控制輸出量。如圖10 所示就是以Pitch 直立環為例子演示串級系統的構成與應用：將可觀測到的速度（編碼器）、角度（四元數姿態解算）、角速度（陀螺儀）用于控制電機實現控速、平衡、抗干擾性強、響應速度快、紋波小等功能。

圖10 單環串級PID 流程圖

其中角度環與角速度環采用非線性PD 控制[13]，速度環采用積分分離PID 控制[14]。正常形式的PID 的原理如公式(1)所示。

式中：Kp為比例時間系數；Ki為積分時間系數；KD為微分時間系數。

而非線性PD 控制的原理如公式(2)所示。

式中：Kp為比例時間系數；Ki為積分時間系數。

對比正常形式的PID 算法，在串級系統中內環去除不必要的積分時間系數，減小積分累積帶來不必要的波紋以及干擾，以便于系統迅速到達并且穩定在期望值附近。

3.3 四元數姿態解算

在本文中四元數姿態解算采用madgwick算法，也被稱為AHRS(姿態和航向參考系統)，包含方向估計算法，提供相對于重力和地球磁場方向的完整方向測量將不同的傳感器數據融合成最優的方向角度估計[15]。

如圖11~圖13 則是四元數姿態解算的代碼應用實際效果。

圖11 Pitch 姿態角對比示意圖

由圖11 可知：解算出的Pitch 姿態角響應速度快，并且精準度高，抗抖性強。

由圖12 可知：采樣原始加速度計數據的毛刺多，受環境影響較大；而在解算出的Pitch 姿態角中幾乎不含毛刺。

圖12 Pitch 姿態角毛刺示意圖

由圖13 可知：Yaw的漂移非常微小，實測數據：大約30 分鐘才會造成1°的偏差。如果再加上三軸磁力計數據后就會如同Ya*w，會根據三軸磁力計數據進行自我矯正漂移，在沒有外來磁場干擾采樣地磁場時，可以做到Ya*w幾乎不存在漂移。

圖13 六軸Yaw 與九軸Ya*w 對比圖

4 結語

本文設計與實現了獨輪機器人姿態控制與慣性導航系統。

在三大環串級PID 系統的控制中：其中兩大環（Pitch直立環與Roll 直立環）用于控制系統的平衡，并且Pitch直立環可以任意人為定義與改變獨輪機器人的速度，Roll直立環還可以與Yaw 轉向環進行輔助的轉向控制，Yaw 轉向環實現兩種轉向控制，使獨輪機器人可以實現高速平滑的轉向，精準到達所需方位。

使用九軸傳感器進行四元數解算，獲取獨輪機器人的姿態角，用以實現簡略捷聯式慣性導航系統；其中還包括編碼器采集的速度數據對系統進行不斷修正，減少環境對于九軸傳感器所帶來的干擾，實現高分辨率，低漂移的簡略捷聯式慣性導航系統。并且在此系統上可以進行人為定義的目標點的設定以及路徑的規劃，實現不同方位，多變化地到達目標點位。

最終設計并實現了可行性高、響應速度快、抗干擾性強以及導航精準度高的獨輪機器人姿態控制與慣性導航的系統。但是系統中還未進行在不同惡劣道路狀況下的應用實踐，無法確定是否能克服惡劣的道路環境和輪胎打滑情況。其功能性和實用性還有待加強，功能模塊上可加入攝像頭識別功能與機械臂搬運功能，算法上還可嘗試改進并細化捷聯式慣性導航的初始對準問題，以實現精度更高的慣性導航。