基于STM32F767IGT6 的AGV 姿態解算系統設計

陳世強，劉明

（云南民族大學，云南昆明 650500）

近年來，隨著自動導引車AGV（Automatic Guided Vehicle）在工廠、碼頭等高動態復雜場景中的應用普及，迫切需要能夠滿足室內和室外環境的低成本、高精度、小體積的導航定位系統[1]。利用磁傳感器、慣性測量單元IMU（Inertial Measurement Unit）與外部信號如GPS 和紅外反射組合構成的導航定位系統成為主要的解決方案。慣性測量單元（IMU）是廣泛用于姿態檢測中的導航器件，通過解算九軸數據可以得到載體的姿態信息。但由于慣性測量元件隨溫度發生漂移、交叉軸存在耦合誤差，嚴重影響了系統的輸出精度[2]，且IMU 往往成本較高，無法在低成本的AGV 車輛中使用。目前低成本解決方案中常用于姿態解算的芯片有博世(BOSCH)公司的BMX055 和InvenSense 公司的MPU6050、MPU9250、ICM-20948等傳感器芯片。文獻[3]運用ICM-20948 九軸傳感器對風的三維姿態角進行解算，設計手持式多信息風速風向定位儀，幫助消防員進行救援工作。文獻[4]使用BMX055 芯片，并結合四元數解算轉歐拉角的算法實現小車的自平衡。文獻[5]采用FPGA+MPU9250的組合實現四軸無人機的姿態解算，FPGA 軟硬件結合提高了姿態解算效率。但是FPGA 需要使用硬件描述語言來實現，且較為復雜。而STM32F767IGT6使用最基本的C 語言就能實現，操作簡單，內部集成的雙精度硬件浮點計算單元（DPFPU）和DSP 指令，能夠滿足姿態解算對數據處理能力的要求。另外，價格方面STM32 相比FPGA 也較便宜。文獻[6]采用FPGA+DSP 雙處理器的方案實現對某彈載控制器的姿態解算，輸出精度能達到±0.5°。但在低成本的AGV 中，使用雙處理器解算姿態成本較高，且STM32F767IGT6 具有DSP 單元，能加速數字信號的處理。文獻[7]中采用6 軸處理組件MPU6050 和磁阻傳感器HMC5883L 進行電路設計和編程，以實現姿態解算。文獻[8]中使用MPU9250 輔助控制無人機的飛行姿態，飛行試驗表明，該系統可以較好地控制無人機飛行姿態。MPU9250 運用于低成本、小體積、高精度的姿態解算系統中有較好的效果。

該文采用MPU9250 來完成AGV 的傳感器數據采集和姿態解算，MPU9250 整合了九軸運動組件[9]，避免了組合陀螺儀與加速度器的軸間差問題，免除了慣性測量單元進行姿態解算前的校準工作，姿態解算效率高。相較其他的組合方案，采用STM32F767 IGT6+MPU9250 具有小體積、低功耗、高精度、易于控制等特點。

1 AGV姿態描述與更新

1.1 AGV姿態描述

首先建立坐標系，取“東-北-天”地理坐標系作為導航參考坐標系，取On點為坐標系原點；載體坐標系是b系，為笛卡爾坐標系，即“右-前-上”，其坐標原點Ob與導航坐標系的On點重合[10]，載體的姿態信息用偏航角、橫滾角和俯仰角描述，載體坐標系和參考坐標系如圖1 所示。

圖1 載體坐標系和參考坐標系

在AGV 運動之后，通過坐標轉換，可以得到新坐標系下的坐標。用方向余弦矩陣表示的坐標變換為：

由式（1）結合四輪AGV 運動模型，在行駛過程中給定AGV 一個控制量u=[v ω]T，則通過MCU 給定控制量就可控制AGV 的運動。

1.2 姿態更新的四元數表示

四元數（quaternion）的概念最早于1843 年由數學家哈密頓（W.R.Hamilton）提出，它可用于描述剛體運動或姿態變換[11]。與方向余弦陣相比，四元數表示方法雖然比較抽象，但卻十分簡潔。四元數可以表示為：

在姿態解算系統中，方向余弦矩陣運算量大，歐拉角法存在萬向節死鎖的現象，而四元數法計算簡潔、精度高，不存在歐拉角“萬向節死鎖”現象，可以完美的彌補二者不足[12]。因此，為了方便、準確地描述AGV 的運動，大多使用四元數法。四元數與方向余弦矩陣之間的關系如下：

在這里四元數q能夠顯示轉動的全部信息，包括旋轉角度、旋轉方向以及旋轉軸的信息。因此姿態矩陣的四元數表示關系式為：

2 系統硬件設計

該系統采用STM32F767IGT6的兩個GPIO（PH4、PH5）連接MPU9250 的IIC 接口23 腳IIC-SCL、24 腳IIC-SDA。通過電路集成總線IIC，實時將九軸傳感器采集到的原始數據和解算后的歐拉角信息上報給MCU，再通過串口將數據上報給上位機，可以實時地顯示MPU9250 的傳感器狀態曲線，同時在LCD 模塊上顯示溫度和歐拉角等信息。通過PA8、PA9、PA10、PA11、PB6、PB7、PB8、PB9 八個GPIO 輸出PWM 控制信號，以控制AGV 的前進和后退。系統框圖如圖2所示。

圖2 系統框圖

2.1 主控芯片

系統主控芯片采用ST 公司的STM32F767IGT6，該芯片采用六級流水線，自帶指令和數據Cache、集成JPEG 編碼器、集成雙精度硬件浮點計算單元（DPFPU）和DSP 指令，并具有512 kB SARM、1 024 kB Flash、四個IIC、八個串口以及140 個通用I/O 口等[13]，芯片主頻率高達216 MHz 可以輕松應對系統的需求。STM32F767IGT6 最小系統如圖3 所示。

圖3 STM32F767IGT6最小系統

2.2 MPU9250

MPU9250 是InvenSense 公司推出的九軸運動跟蹤傳感器，內部集成一個MPU6050 和一個磁力計AK8963[14]。廣泛運用于手勢控制、體感游戲控制器、可穿戴智能設備、游戲手柄、位置查找服務、姿態解算等領域。3×3×1 mm3的超小封裝使其能裝在各種載體上，能承受住10 000 g 的震動沖擊。九軸三個16 位分辨率的傳感器以數字形式輸出融合演算數據，能極大地減少MCU 處理任務的工作量。具有靈活的時鐘方案，適用于各種內部時鐘源同步電路，可以對信號進行調理和數模轉換，片上PLL 允許各種輸入的時鐘信號。工作條件：在40～85 ℃時帶高精度的1%的時鐘漂移，嵌入溫度傳感器，并帶可編程中斷。其完美的IIC 方案，可以直接以數字的形式輸出九軸融合演算數據，具有可編程數字濾波器，自帶512 字節FIFO 緩沖區，支持SPI 和IIC 兩種通信方式。

2.2.1 MPU9250與STM32硬件連接

MPU9250 的SCL、SDA 與STM32F767IGT6 的PH4、PH5 連接，與PCF8574T 共用IIC 總線。圖4 中，AD0 引腳接GND，AD0 引腳是IIC 從機LSB 的地址，MPU9250 器件IIC 地址其實是內部集成的MPU6050的地址b01101000，即0X68。9D_INT 信號線接在PCF8574T 的P5 腳上，在讀取9D_INT 時，需要先初始化PCF8574T，然后通過IIC 總線讀取數據。MPU9250 通過IIC 總線進行單字節或多字節的配置，加速度值、陀螺儀值等存放在FIFO 數據緩存器中，隨時等待讀取。STM32F767IGT6 與MPU9250 的硬件連接電路如圖4 所示。

圖4 STM32F767IGT6與MPU9250的硬件連接電路

2.2.2 IIC總線運行模式

IIC 是一個雙線通信方案，屬于半雙工同步通信方式，通過SDA 數據線和SCL 時鐘線這兩根線電平的變化，分別傳輸數據和時鐘信號[15]。通常這兩個接口是雙向的開漏極接口，在連接設備時可以做主機或者從機。從機在通信時，通過地址即可匹配，MPU9250 通常和控制芯片連接時作為從機，SDA 和SCL 需要接阻值為1.2 kΩ的上拉電阻到VDD，其最快通信速度達400 kHz。

MPU9250 有輔助IIC 通信芯片通信的功能，這個功能有兩個模式：1）IIC 主機模式：MPU9250 作為主機直接與從機通過IIC 進行通信。允許MPU9250 直接讀傳感器的寄存器，比如加速度計。在這個模式下，MPU9250 可以不需要主控芯片而直接讀取輔助芯片的寄存器。2）通行模式：主控芯片直接通過IIC控制MPU9250 和外設芯片。該系統采用IIC 主機模式，IIC 口的SDA 和SCL 是復用口，采用主機模式MPU9250 對輔助芯片進行配置，復用功能不能再用，只能通過輔助芯片讀取STM32F767IGT6的數據，中斷腳必須連接系統的GPIO，從而喚醒系統。其中MPU9250 的AUX_DA 和AUX_CL 不連接，懸空。IIC時序邏輯如圖5 所示。

圖5 IIC時序邏輯

2.2.3 DMP使用

數字運動處理（Digital Motion Processor，DMP）內置于MPU9250 中，可以直接處理九軸傳感器采集到的原始數據[16]。先將原始數據轉化為四元數，再融合解算為姿態角，大大節約了姿態解算時間，使用DMP 進行數據融合演算，能有效地釋放主芯片STM32F767IGT6 的工作任務，使得MCU 能處理更多的任務。MPU9252 從寄存器中就能直接讀取到DMP計算好的演算數據，或者直接把算好的數據存入FIFO緩沖器中等待需要時調用。DMP 有個中斷引腳，可用來喚醒主控芯片。為了保證數據融合解算的高速率和高精度，使用DMP 可以節能，也可以節約程序運行時間，對于設計AGV 是一個很好的選擇。在軟件設計中，設置兩個函數：mpu_dmp_init 函數，用于初始化MPU9250 DMP；mpu_dmp_get_data 函數，用于得到DMP 姿態解算后的俯仰角、橫滾角和航向角，以及溫度等信息。

當STM32F767IGT6 讀取MPU9250 的加速度和角速度傳感器數據時（非中斷方式），需要先初始化相關的配置參數等，初始化完成后就可以讀取相關的寄存器，首先將加速度，陀螺儀、磁場值傳給DMP，融合演算之后，再從寄存器中將數據傳給MCU。數據讀取流程圖如圖6 所示。

圖6 數據讀取流程圖

3 系統軟件設計

AGV 姿態解算系統軟件是在硬件的基礎上進行設計的，STM32F767IGT6 控制程序采用C 語言在keil 5 中編寫完成，主要包括MPU9250 初始化、MPU9250 數據采集、IIC 驅動、MPL 驅動、上報數據給上位機等程序設計。

MPU_Init()函數按照圖6 所示流程進行初始化，函數執行完成之后，便可讀取傳感器數據。陀螺儀滿量程設為±2 000°/s，加速度傳感器滿量程設置為±2g，陀螺儀采樣頻率設置為50 Hz。通過電源管理寄存器1 設置系統時鐘源，將CLKSEL 置為001。通過MPU_Get_Magnetometer、MPU_Get_Accelerometer、MPU_Get_Gyroscope 三個函數獲取九軸（加速度、陀螺儀、磁力計）數據。Void usart1_report_imu()函數通過串口1 上報解算后的姿態數據（roll、pitch、yaw）給上位機，通過狀態幀（01H）進行發送。代碼編譯成功下載到STM32F767IGT6 后，從LCD 屏幕可以實時查看當前的MPU9250 溫度、俯仰角（pitch）、橫滾角（roll）和航向角（yaw）的數值，AGV 姿態信息如圖7所示。

圖7 AGV 姿態信息

上位機采用ANO_TC 匿名科創地面站v4.exe 軟件，MPU9250_send_data 函數用于上報加速度和陀螺儀的原始數據，通過傳感器幀（02H）進行數據發送，串口COM3 進行數據接收(共6 條線:ACC_X、ACC_Y、ACC_Z、GYRO_X、GYRO_Y、GYRO_Z)，串口波特率設置為500 kbps，上位機數據校驗設置為OFF，從而以圖形的形式顯示傳感器數據。

4 結論

該文設計的AGV 姿態解算系統，綜合利用了STM32F76IGT6、MPU9250 強大的控制能力和傳感解算能力，傳感器輸出頻率達到1 000 Hz，姿態解算頻率達到320 Hz，姿態角精度為±0.2°，并能實時顯示。在AGV 運動中實現了高精度的姿態解算，實時掌控AGV 的姿態信息。在陌生復雜的環境中，對于AGV 自身姿態的把握控制以及后續運動控制的執行都是至關重要的。該系統運用于無人車、無人機和水下控制器的控制中都能為控制中心提供高精度的載體姿態信息，給后臺控制人員提供了便利。在STM32F767IGT6 中運用姿態解算結果控制AGV 運動的算法還有待提高，AGV 運動與實際預期還有一定差距，后期將從算法層面改進AGV 運動控制，優化控制效果。