獨(dú)輪機(jī)器人姿態(tài)控制與慣性導(dǎo)航的設(shè)計(jì)與實(shí)現(xiàn)

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

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

0 引言

獨(dú)輪車機(jī)器人是一種依靠單輪接地平衡的非穩(wěn)定性系統(tǒng)，比起兩輪平衡車自平衡系統(tǒng)，雖然獨(dú)輪機(jī)器人自平衡系統(tǒng)的結(jié)構(gòu)精簡(jiǎn)，將地面接觸面積降到最小，但也因此需要通過多維的動(dòng)態(tài)平衡控制來實(shí)現(xiàn)系統(tǒng)達(dá)到自平衡狀態(tài)[3]。而獨(dú)輪機(jī)器人系統(tǒng)概念的提出，必然引出很多難題：如系統(tǒng)的結(jié)構(gòu)設(shè)計(jì)、動(dòng)力學(xué)建模和姿態(tài)控制等，進(jìn)一步引起姿態(tài)更新算法與控制策略趨于多維模型的創(chuàng)新和變革。

截至目前，研究關(guān)于獨(dú)輪機(jī)器人的系統(tǒng)方面的論文百花齊放，深入到獨(dú)輪機(jī)器人系統(tǒng)的方方面面，例如：動(dòng)力學(xué)建模與控制策略[4~6]、姿態(tài)更新算法的創(chuàng)新與延伸[7~11]等等。本文則是提出一種可行性高、響應(yīng)速度快、抗干擾性強(qiáng)以及導(dǎo)航精準(zhǔn)度高的獨(dú)輪機(jī)器人姿態(tài)控制與慣性導(dǎo)航的實(shí)現(xiàn)方法。

1 系統(tǒng)總體方案設(shè)計(jì)

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

圖1 系統(tǒng)總體設(shè)計(jì)框圖

圖2 實(shí)際車模圖

而在系統(tǒng)軟件設(shè)計(jì)上，主要功能實(shí)現(xiàn)使用四元數(shù)融合濾波解算姿態(tài)角，姿態(tài)控制選用串級(jí)PID 控制，采用簡(jiǎn)略捷聯(lián)式慣性導(dǎo)航確定位置坐標(biāo)。在調(diào)試過程中的輔助功能：使用OLED 模塊與按鍵模塊相配合實(shí)現(xiàn)在OLED 屏幕上顯示參數(shù)和參數(shù)調(diào)整功能；使用UART 串口實(shí)時(shí)傳輸濾波數(shù)據(jù)到電腦上的匿名上位機(jī)進(jìn)行調(diào)試；使用SD 卡模塊存儲(chǔ)獨(dú)輪機(jī)器人調(diào)試時(shí)的參數(shù)。

2 系統(tǒng)硬件電路設(shè)計(jì)

2.1 硬件電路總體設(shè)計(jì)

硬件總體設(shè)計(jì)采用TC264 為運(yùn)算核心的設(shè)計(jì)方案，整套硬件系統(tǒng)可分為三個(gè)模塊，分別為主控模塊、電機(jī)驅(qū)動(dòng)模塊、電源模塊。

2.2 主控模塊設(shè)計(jì)

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

圖3 主控核心原理圖

2.3 電機(jī)模塊設(shè)計(jì)

電機(jī)驅(qū)動(dòng)電路一般采用驅(qū)動(dòng)芯片和MOS 管組合搭建H橋，對(duì)于行進(jìn)輪有刷直流電機(jī)我們采用兩片全橋驅(qū)動(dòng)芯片BTN8962，該芯片雖然導(dǎo)通內(nèi)阻較大發(fā)熱較嚴(yán)重、死區(qū)時(shí)間較長(zhǎng)，但鑒于獨(dú)輪機(jī)器人速度較慢，使用BTN8962 完全足夠。有刷電機(jī)驅(qū)動(dòng)電路如圖4 所示。

圖4 有刷電機(jī)驅(qū)動(dòng)電路圖

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

圖5 直流無刷電機(jī)驅(qū)動(dòng)原理圖圖

2.4 電源模塊設(shè)計(jì)

電源模塊主要分成三個(gè)部分：主板電源模塊、3.8V 電源轉(zhuǎn)換、3.3V 電源轉(zhuǎn)換。

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

圖6 電源原理圖

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

圖7 24V 轉(zhuǎn)3.8V 原理圖

3.3 V 電源由AMS1117 轉(zhuǎn)換得到，作為一款經(jīng)典的線性穩(wěn)壓芯片，AMS1117 具有輸出電壓穩(wěn)定、電源紋波小、電路簡(jiǎn)單的特點(diǎn)，實(shí)際運(yùn)用中采用AMS1117 為TC264 供電來減少電源紋波的干擾。AMS1117轉(zhuǎn)換3.3V原理如圖8所示。

圖8 AMS1117 轉(zhuǎn)換3.3V 原理圖

3 系統(tǒng)軟件設(shè)計(jì)

3.1 軟件流程設(shè)計(jì)

系統(tǒng)軟件部分主要由：串級(jí)PID 平衡控制、四元數(shù)姿態(tài)解算、簡(jiǎn)略捷聯(lián)式慣性導(dǎo)航系統(tǒng)這三個(gè)部分組成。獨(dú)輪機(jī)器人系統(tǒng)在串級(jí)PID 平衡的控制下可以保持直立，在慣性導(dǎo)航確定目標(biāo)點(diǎn)后，可以通過速度環(huán)以動(dòng)態(tài)速度到達(dá)目標(biāo)點(diǎn)位，還能夠使用路徑規(guī)劃，規(guī)劃不同的路徑到達(dá)。

3.2 串級(jí)PID 控制

串級(jí)PID 控制由三個(gè)大環(huán)組成：Pitch 直立環(huán)、Yaw 轉(zhuǎn)向環(huán)、Roll 直立環(huán)；其中Pitch 直立環(huán)用于前后姿態(tài)控制；Roll 直立環(huán)用于左右姿態(tài)控制；Yaw 轉(zhuǎn)向環(huán)用于獨(dú)輪機(jī)器人的轉(zhuǎn)向與壓彎調(diào)控。總體串級(jí)PID 控制結(jié)構(gòu)設(shè)計(jì)如圖9所示。

圖9 總體串級(jí)PID 控制結(jié)構(gòu)設(shè)計(jì)圖

串級(jí)PID 控制是基于PID 控制器，將可觀測(cè)到的多個(gè)輸入量全部用于統(tǒng)籌控制輸出量。如圖10 所示就是以Pitch 直立環(huán)為例子演示串級(jí)系統(tǒng)的構(gòu)成與應(yīng)用：將可觀測(cè)到的速度（編碼器）、角度（四元數(shù)姿態(tài)解算）、角速度（陀螺儀）用于控制電機(jī)實(shí)現(xiàn)控速、平衡、抗干擾性強(qiáng)、響應(yīng)速度快、紋波小等功能。

圖10 單環(huán)串級(jí)PID 流程圖

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

式中：Kp為比例時(shí)間系數(shù)；Ki為積分時(shí)間系數(shù)；KD為微分時(shí)間系數(shù)。

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

式中：Kp為比例時(shí)間系數(shù)；Ki為積分時(shí)間系數(shù)。

對(duì)比正常形式的PID 算法，在串級(jí)系統(tǒng)中內(nèi)環(huán)去除不必要的積分時(shí)間系數(shù)，減小積分累積帶來不必要的波紋以及干擾，以便于系統(tǒng)迅速到達(dá)并且穩(wěn)定在期望值附近。

3.3 四元數(shù)姿態(tài)解算

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

如圖11~圖13 則是四元數(shù)姿態(tài)解算的代碼應(yīng)用實(shí)際效果。

圖11 Pitch 姿態(tài)角對(duì)比示意圖

由圖11 可知：解算出的Pitch 姿態(tài)角響應(yīng)速度快，并且精準(zhǔn)度高，抗抖性強(qiáng)。

由圖12 可知：采樣原始加速度計(jì)數(shù)據(jù)的毛刺多，受環(huán)境影響較大；而在解算出的Pitch 姿態(tài)角中幾乎不含毛刺。

圖12 Pitch 姿態(tài)角毛刺示意圖

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

圖13 六軸Yaw 與九軸Ya*w 對(duì)比圖

4 結(jié)語

本文設(shè)計(jì)與實(shí)現(xiàn)了獨(dú)輪機(jī)器人姿態(tài)控制與慣性導(dǎo)航系統(tǒng)。

在三大環(huán)串級(jí)PID 系統(tǒng)的控制中：其中兩大環(huán)（Pitch直立環(huán)與Roll 直立環(huán)）用于控制系統(tǒng)的平衡，并且Pitch直立環(huán)可以任意人為定義與改變獨(dú)輪機(jī)器人的速度，Roll直立環(huán)還可以與Yaw 轉(zhuǎn)向環(huán)進(jìn)行輔助的轉(zhuǎn)向控制，Yaw 轉(zhuǎn)向環(huán)實(shí)現(xiàn)兩種轉(zhuǎn)向控制，使獨(dú)輪機(jī)器人可以實(shí)現(xiàn)高速平滑的轉(zhuǎn)向，精準(zhǔn)到達(dá)所需方位。

使用九軸傳感器進(jìn)行四元數(shù)解算，獲取獨(dú)輪機(jī)器人的姿態(tài)角，用以實(shí)現(xiàn)簡(jiǎn)略捷聯(lián)式慣性導(dǎo)航系統(tǒng)；其中還包括編碼器采集的速度數(shù)據(jù)對(duì)系統(tǒng)進(jìn)行不斷修正，減少環(huán)境對(duì)于九軸傳感器所帶來的干擾，實(shí)現(xiàn)高分辨率，低漂移的簡(jiǎn)略捷聯(lián)式慣性導(dǎo)航系統(tǒng)。并且在此系統(tǒng)上可以進(jìn)行人為定義的目標(biāo)點(diǎn)的設(shè)定以及路徑的規(guī)劃，實(shí)現(xiàn)不同方位，多變化地到達(dá)目標(biāo)點(diǎn)位。

最終設(shè)計(jì)并實(shí)現(xiàn)了可行性高、響應(yīng)速度快、抗干擾性強(qiáng)以及導(dǎo)航精準(zhǔn)度高的獨(dú)輪機(jī)器人姿態(tài)控制與慣性導(dǎo)航的系統(tǒng)。但是系統(tǒng)中還未進(jìn)行在不同惡劣道路狀況下的應(yīng)用實(shí)踐，無法確定是否能克服惡劣的道路環(huán)境和輪胎打滑情況。其功能性和實(shí)用性還有待加強(qiáng)，功能模塊上可加入攝像頭識(shí)別功能與機(jī)械臂搬運(yùn)功能，算法上還可嘗試改進(jìn)并細(xì)化捷聯(lián)式慣性導(dǎo)航的初始對(duì)準(zhǔn)問題，以實(shí)現(xiàn)精度更高的慣性導(dǎo)航。