基于STM32輪式機器人姿態系統的綜合測試平臺設計與實現

王鋒，周翟和，陳如意

(南京航空航天大學 自動化學院，江蘇 南京 210016)

0 引言

兩輪自平衡機器人作為一種特殊的倒立擺式的移動機器人，具有非完整性、非線性、欠驅動和不穩定等特點，這使得它能夠成為驗證各種控制算法的理想平臺[1]。姿態系統需要對它的運動姿態等信息有準確的估計，以實現動態平衡。但是由于感知系統采用的傳感器極易受到噪聲、溫度等環境因素的干擾，影響系統估計的準確性。

為了提高姿態系統的開發效率和質量，分析實際的姿態測量過程，測試姿態系統的性能。結合軟硬件，針對輪式機器人姿態系統，設計了一個綜合測試平臺，對提高輪式機器人姿態系統的可靠性具有重要意義[2]。

1 系統硬件設計

圖1為所設計的綜合測試平臺的硬件框圖。綜合測試平臺的硬件部分主要由測試點模塊、主控模塊、顯示模塊組成。主控制器模塊是整個測試平臺的核心，用來采集測試點數據，對數據進行處理以及運算，并將處理后的數據傳輸給顯示模塊，借助顯示模塊測試信號濾波前后的效果。

圖1 測試平臺硬件框圖

1.1 主控模塊介紹

主控模塊是測試平臺的核心，主要實現數據采集、數據的預處理、卡爾曼濾波處理等工作。主控模塊采用STM32控制器，內嵌了ADC以及IIC通訊接口，被測試系統可以以從機的模式通過IIC與主控模塊進行通訊[4]。

1.2 顯示模塊介紹

為了實時顯示被測信號的變化，本文針對被測系統的性能，利用MATLAB的GUIDE快速開發環境設計了一套基于MATLAB圖形用戶界面(GUI)信號測試界面，通過設置各控件屬性以及回調函數實現系統功能[4]。上位機可以通過建立好的MATLAB GUI界面可以實時顯示被測系統的工作狀態，將被測信號進行實時顯示，并對采集到的數據進行存儲以及分析；另一方面，可根據需要，隨時編寫相應的程序，升級測試平臺的功能。

1.3 測試點傳感器電路設計

測試點主要是被測系統的輸入模塊，即姿態傳感器。被測系統采用的姿態傳感器分別為MMA7361加速度計以及ENC03陀螺儀。加速度計傳感器MMA7361是美國Freescale推出的一款低成本單芯片三軸加速度傳感器。該微型電容式加速度傳感器提供了兩種加速度測量范圍： ±1.5 g和6 g[5]。可通過芯片引腳的設置來選擇量程范圍，并輸出與加速度成正比的電壓信號。以X軸為例，所受加速度與輸出電壓關系如下[6]：

(1)

式(1) 表示，當加速度計輸入量分別為為1g、0g、-1g的重力加速度信號時，理論輸出模擬電壓分別為2.45V、1.65V、0.85V。由式(1)也可以看出，MMA7361加速度計所受加速度從-1g～+1g，與對應輸出電壓成線性關系[7]。測試點MMA7361模塊電路圖如圖2所示。

測試點陀螺儀傳感器選用的是村田ENC03型號，它利用了旋轉坐標系中的物體會受到科里奧利力的原理，在器件中利用壓電陶瓷做成振動單元。當旋轉器件時會改變振動頻率從而反映出物體旋轉的角速度[8]。測試點陀螺儀ENC03模塊的電路如圖3所示。測試點的傳感器模塊可以作為從機使用，通過IIC與主機STM32進行通信。

圖2 測試點MMA7361模塊電路

圖3 測試點ENC03模塊電路

為了降低30～33kHz的噪聲(陀螺儀諧振頻率)，在圖4的ENC03模塊電路中，設計了截止頻率比傳感器頻率(50Hz)更高的低通濾波器，對陀螺儀信號進行硬件濾波處理。傳感器溫度漂移的部分，可以經A/D采樣后，由軟件對采樣的原始數據進行處理，消除溫漂。

2 系統軟件設計

2.1 總體流程設計

測試平臺的主程序流程圖如圖4所示，主要由IIC初始化、姿態信息初始化、姿態傳感器數據采集和通訊、卡爾曼濾波處理等部分組成。此部分的卡爾曼濾波器目的是模擬被測系統中的卡爾曼濾波器，以此來測試被測系統中卡爾曼濾波器的數據處理效果。

在讀取測試點數據之前，首先需要對IIC總線進行初始化，IIC的初始化需要設置總線的時鐘頻率、中斷允許標志位以及模塊的工作模式。然后利用IIC總線寫指令對姿態傳感器進行初始化[9]。在完成數據換算之后，利用卡爾曼濾波算法對采集的加速度計和陀螺儀數據進行處理。

圖4 主程序流程圖

2.2 卡爾曼濾波器設計

被測對象是輪式機器人姿態系統，其內部采用的算法是卡爾曼濾波算法。設計的綜合測試平臺需要測試該算法在姿態系統中的處理效果，因此需要在測試平臺上，設計一個與被測系統內部相同的濾波器，便于測試算法的效果。系統狀態向量的選取是設計卡爾曼濾波器的關鍵[10]。因為機器人傾斜的角度與傾斜的角速度存在導數關系，因此可以選擇傾斜角作為狀態向量的一個元素。由于傾斜的角加速度導數無法給出，所以不能作為狀態向量的另外一個元素，采用加速度計來估計陀螺儀的零位偏差Gyro_bias，這樣就得到了狀態向量的另外一個元素[11]。機體傾斜的角度與傾斜的角速度之間存在以下關系:

Anglek=Anglek-1+(Gyrok-1-Gyro_biask-1).dt

(2)

式中，Anglek是估算出來的k時刻的傾斜角度；Anglek-1為加速度計解算出來的k-1時刻的傾斜角度；Gyrok-1為陀螺儀解算的到k-1時刻的傾斜角度；Gyro_biask-1為k-1時刻陀螺儀的位偏差；dt為采樣時間。

令：

Gyro_biask=Gyro_biask-1

(3)

綜合式(1)、式(2)兩式，將其改寫成狀態矩陣的形式，獲取離散卡爾曼狀態空間表達式：

(4)

過程噪聲協方差矩陣Q表達形式如下：

(5)

式中的Q_Gyro和Q_Angle分別是陀螺儀所測得的載體傾角過程噪聲協方差以及加速度計所測得的角速度過程噪聲協方差，不同的取值代表了不同的信任程度。機體處于高速變化運行狀態時，Q_Gyro設置的小一些，Q_Angle設置的大一些，使得濾波器更相信陀螺儀數據；機器人處于低速變化運行狀態時，可以將Q_Angle設置的小一些，使得濾波器更相信加速度計數據。

3 系統測試與實驗

3.1 濾波器性能測試

被測系統中采用卡爾曼濾波算法對姿態傳感器進行數據融合處理。因此需要測試姿態傳感器在算法處理前后的效果。選取的測試點是被測對象的輸入模塊，即姿態傳感器。此部分主要測試被測系統輸入模塊經過卡爾曼濾波算法處理的效果。

測試平臺在內部模擬了一個與被測系統相同的卡爾曼濾波器，利用采集到的測試點數據來測試濾波器的性能。靜態條件下，完成測試點加速度計傳感器數據的采集，并利用模擬的卡爾曼濾波器對原始數據進行濾波處理。然后利用測試平臺的上位機顯示模塊繪制原始信號和濾波處理后的信號曲線，便于對比濾波器處理前后的信號變化以及對傳感器靜態誤差進行分析。

圖5是靜態零輸入條件下加速度計的原始測試數據。可以看出，未做濾波處理時，MMA7361加速度計靜態信號毛刺嚴重，誤差區間在[-1 1]。圖6是經過濾波器處理后的加速度計數據?？梢钥闯觯涍^濾波處理后，MMA7361加速度計靜態數據的毛刺大大減少，靜態誤差保持在[-0.2 0.4]區間。

圖5 加速度計零輸入測試信號

圖6 加速度計濾波后數據

3.2 基于GUI的傳感器測試

為了實時直觀地顯示被測信號，利用MATLAB的M語言編寫串口通信的GUI，將被測點信號通過串口實時繪制信號曲線。在開啟GUI實時打印被測信號前，需要確保被測點模塊電路與STM32的正確連接?；贕UI的傳感器數據打印顯示提供了一種新的測試手段，其設計流程如圖7所示。

圖7 GUI設計流程圖

上位機檢測到的串口號為COM6，所以在GUI中要設置正確的串口號，否則就無法實時打印被測信號(圖8)。由于STM32內部程序設置的串口波特率為9 600，所以GUI中也要設置為9 600。

3.3 姿態角動態測試

1) MATLAB顯示

利用本文設計的綜合測試平臺對姿態系統的輸出信號、俯仰角和橫滾角進行測試并借助上位機顯示。

圖9、圖10是綜合測試平臺顯示的被測系統輸出信號。由于輪式機器人主要以俯仰角和橫滾角運動為主，因此本文設計的綜合測試平臺以測試橫滾角和俯仰角信號為主。為了更好地測試機器人姿態系統的性能，將輪式機器人繞俯仰軸、橫滾軸連續運動，分別產生俯仰角以及橫滾角信號，將姿態系統中的輸出信號傳送至上位機中，借助MATLAB通用性曲線打印方法，繪制姿態角信號。

圖9 動態俯仰角測試

圖10 動態橫滾角測試

2) 示波器信號實時檢測

將經過卡爾曼濾波算法融合后的姿態信號，通過主控制器STM32內部的DAC1轉換成模擬信號并接入示波器。借助示波器測試姿態角信息實時性強。示波器能實時檢測到微弱的信號變化，如圖11、圖12所示。

圖11 俯仰角模擬電壓信號

圖12 橫滾角模擬電壓信號

4 結語

以機器人姿態系統測試為例，以STM32控制器為核心設計了綜合測試平臺的硬件模塊和軟件模塊，利用所設計的測試平臺分別測試了姿態系統的輸入模塊信號、卡爾曼濾波算法性能以及被測系統的輸出信號。此外，還介紹了基于GUI的測試界面設計流程。為實時測試被測試點信號提供了界面化的方法。

[1] Salerno A, Angeles J. The control of semi-autonomous two-wheeled robots undergoing large payload-variations[J]. IEEE International Conference on Robotics & Automation,2004, 2(2):1740-1745.

[2] 劉魯. 基于PLC控制系統的電機測試平臺的設計與調試探究[J]. 電子測試, 2014(9):3-4.

[3] 李振全, 何波賢, 趙鳳全,等. 兩輪平衡車姿態檢測系統設計[J]. 電腦編程技巧與維護, 2015(21):7-9.

[4] 王巧花, 葉平, 黃民. 基于MATLAB的圖形用戶界面(GUI)設計[J]. 煤礦機械, 2005(3):60-62.

[5] 唐海玲, 趙春雨, 宋家友. 基于MMA7361加速度傳感器的重力感應遙控小車設計[J]. 傳感器世界, 2013, 19(6):9-12.

[6] 汪浩. 電容式微加速度計檢測原理與設計分析[J]. 巢湖學院學報, 2005, 7(3):87-89.

[7] 楊繼志, 郭敬. 基于MMA7260兩輪自平衡小車控制系統設計[J]. 機電產品開發與創新, 2011, 24(3):144-145.

[8] 劉海剛, 宋一標, 陳恒偉,等. 單軸兩輪自平衡小車姿態控制系統設計[J]. 工業控制計算機, 2013, 26(9):16-18.

[9] Han F, Liu T, Li L, et al. Design and Fabrication of a Differential Electrostatic Accelerometer for Space-Station Testing of the Equivalence Principle[J]. Sensors, 2016, 16(8)：20-23.

[10] 劉春陽, 徐軍領, 程洪濤,等. MPU9250傳感器的姿態檢測與數據融合[J]. 河南科技大學學報(自然科學版), 2015, 36(4):14-17.

[11] 李偉. 兩輪自平衡機器人的姿態檢測系統的設計[J]. 中國科技縱橫, 2011(15):213-213.