二維運動目標姿態檢測和軌跡追蹤系統

邱鴻鑫　佘淑儀　林玲璇　梁梓豪　徐福仁　熊愛民

摘 ? 要： 傳統運動目標定位系統存在可移動性差、實時性受限、對目標姿態檢測不適用等缺陷。提出一種基于六軸傳感器MPU6050的二維運動目標姿態檢測和軌跡追蹤系統。系統采用MPU6050傳感器獲取目標運動數據，將數據傳送到STM32F103RCT6主控芯片進行處理;利用無線通信模塊將數據處理結果傳輸至PC端;在PC端實時顯示目標的運動姿態和運動軌跡。通過靜止、加速、轉彎等運動對系統進行測試，總體路線偏差控制在6.25%以內，達到了設計要求?？梢栽谄囆旭偁顟B判斷、車載GPS盲區導航、室內機器人定位等領域推廣使用。

關鍵詞： 姿態檢測;軌跡追蹤;MPU6050;STM32F103RCT6;無線通信

中圖分類號：TP37 ? ?文獻標識碼：A ? ?文章編號：2095-8412 （2020） 04-063-06

工業技術創新 URL： http：//gyjs.cbpt.cnki.net ? ?DOI： 10.14103/j.issn.2095-8412.2020.04.012

引言

為了檢測運動目標的姿態和軌跡，進而判斷運動目標是否按照預定模式正常工作，已經開發了各種定位技術和定位系統。傳統的室內外定位系統具有較高的測量精度，但它們往往對使用環境有特定要求，也存在目標定位受外界參考系限定、測量量程受基準參考系制約、對目標姿態檢測不適用等缺陷[1]。

張秀再等[2]提出了利用慣性元件進行軌跡追蹤的系統，其利用串口通信將數據傳輸到PC端，這種有線傳輸的方式在測試和實際使用中存在可移動性差等缺點，會受到一定限制。無線通信可移動性好，但在GPS信號不佳的環境下會受到實時性的限制[3]。

為了解決上述問題，本文設計一種基于六軸傳感器MPU6050的二維運動目標姿態檢測和軌跡追蹤系統。系統通過MPU6050傳感器獲取運動目標的加速度、姿態角等參數，進行數據處理，進而將處理結果無線傳輸至PC端;PC端將實時顯示目標的運動姿態并描繪其運動軌跡，具有設計簡單、準確率高、實時性強等優點，可以在室內環境及GPS信號不好的環境下實現運動物體的檢測、定位和追蹤。

1 系統構成與工作原理

1.1 ?系統構成

系統結構框架如圖1所示。系統主要包括四個模塊：運動感測模塊、主控模塊、無線通信模塊、PC端處理模塊。運動感測模塊采用六軸傳感器MPU6050，通過IIC通信向主控模塊提供加速度和姿態角信息。主控模塊以STM32F103RCT6作為主控芯片，主要負責處理和計算運動感測模塊傳送的信息。無線通信模塊采用ESP8266模塊，將處理的結果數據傳輸至PC端。PC端處理模塊令界面顯示姿態檢測的結果，并在二維坐標軸上描繪目標運動軌跡。

1.2 ?工作原理

本系統主要涉及室內定位，所以在無GPS信號或GPS信號不好的情況下，由運動感測模塊向主控模塊提供運動目標的各軸加速度和姿態角信息。主控模塊在對原始的運動數據進行濾波及積分等處理后，結合不同姿態的數據變化特征，便可以推斷運動目標的姿態并計算其軌跡坐標。最后，利用無線通信模塊將各項結果數據傳輸至PC端，使用者便可在現有的界面上實時獲取運動目標的姿態信息、追蹤其在二維平面上的軌跡了。工作原理圖亦如圖1所示。

2 ?系統軟件設計

系統STM32F103RCT6芯片運行程序用C語言編寫，包括MPU6050原始數據處理程序、姿態檢測判斷程序、軌跡坐標計算程序。PC端執行用Python編寫的界面顯示程序，即接收無線通信模塊發送的各項結果數據。姿態檢測結果將在界面中予以顯示。

2.1 ?原始數據處理流程

首先，MPU6050傳感器采集得到運動目標的三軸加速度和三軸旋轉角速度，其中三軸旋轉角速度經過DMP庫的計算處理被轉換為姿態角。三軸加速度包括了空間直角坐標系下x、y、z三軸方向上的加速度數據;姿態角包括物體繞空間直角坐標系下x、y、z三軸方向旋轉的角度，它們分別稱為橫滾角、俯仰角、航偏角。接著，由于采集到的原始數據存在較大的噪聲干擾，且受干擾程度是隨機的，因此難以測得精確值。為了保證加速度數據的實時性和準確性，需要再對加速度進行卡爾曼濾波和加權移動平均濾波。原始數據處理流程如圖2所示。

卡爾曼濾波通過建立目標的狀態模型并估計目標的運動速度和加速度，對目標的未來點進行預測[4]，使估計值盡可能地接近真實值。盡管卡爾曼濾波得到的是一個最優估計，但加速度作為一個模擬量，在短時間內不會出現極劇烈的波動變化。為經過卡爾曼濾波后的數據選擇合適的幀長度進行加權移動平均濾波，權衡好平均濾波的滯后性和響應度，可進一步減小最優估計值的誤差，提高對數據變化的響應能力[5]。數據處理結果對比如圖3所示。

2.2 ?姿態檢測判斷程序

姿態檢測判斷程序包括加減速判斷程序和轉彎判斷程序，其姿態判斷流程如圖4所示。

對于加減速判斷，首先將最初的一部分加速度樣本數據取平均值，作為初始狀態的基準值，接著讓后續的加速度都減去該基準值，得到實際偏差值。根據一段時間內實際偏差值的變化趨勢及程度，利用閾值法判斷目標處于加速還是減速的姿態。

對于轉彎判斷，為防止航偏角在一定時間內變化比較小而被誤判為不變，首先將航偏角數據每20個分為一組，然后將每組數據逐個與前一組對應位置的數據作差，對差值大于和小于閾值的值分別進行計數。當某段時間內的計數達到一定數量時，則判定目標處于左轉或右轉的姿態，否則認為目標直線運動。

2.3 ?軌跡坐標計算程序

本系統以目標在追蹤開始時刻的前進方向為二維平面x軸的正方向，以x軸正方向逆時針旋轉90°的方向為二維平面y軸的正方向，即坐標系是在系統開始追蹤目標軌跡的第一時間確定的。

由于系統無法保證處于完全水平狀態，因此重力加速度會在x軸、y軸上產生分量，造成由位移計算的加速度數據不準確，因此需要對加速度數據進行偏移修正。具體做法是在開啟MPU6050傳感器后，前150個加速度樣本保持不變，計算這部分樣本的平均值，后續獲取的加速度都減去該平均值，即得到偏移修正的加速度。

在靜止狀態下，經偏移修正后的加速度仍會在零點周圍小范圍波動，這種零點漂移是由MPU6050傳感器的抖動等產生的，可視為非零均值噪聲，該噪聲在速度和位移的積分計算中會引起累積誤差，因此考慮使用閾值法減小該噪聲帶來的影響。設定一個合適的閾值，在該閾值以下，任何加速度值都將被設置為零，從而保證了靜止狀態下代入積分計算的加速度值為零，確保后續計算得到的速度、位移數據的準確性。

以牛頓第二定律為基礎建立運動學模型，加速度、速度、位移之間的關系為

（1）

（2）

其中，a是運動載體的加速度，v是運動載體的速度，s是運動載體的位移。式（1）和式（2）表明，對加速度進行一次時間積分可以得到運動速度，對運動速度再進行一次時間積分得到位移長度[6]。航偏角對位移長度進行偏轉，即可計算得出實際位移矢量。若k-1時刻下運動目標的軌跡坐標已知，則可以計算出k時刻下目標的軌跡坐標。即

（3）

（4）

其中，為k時刻下的航偏角。軌跡坐標計算流程如圖5所示。

3 ?PC端程序設計及界面設計

首先，在PC端配置好PyCharm +QTDesigner+PyUIC環境，使用QTdesigner布局設計用戶界面，并生成.ui文件;然后，利用PyUIC將.ui文件轉換為.py文件，并導入Pycharm;第三，實例化socket對象并綁定與無線通信模塊對應的網絡IP和端口;第四，開辟新的線程用于監聽和接收端口數據，對不同類型數據進行分類篩選和臨時存放;第五，編寫界面文本顯示函數，將姿態判斷結果等數據實時顯示于界面對應的文本框;第六，編寫二維坐標軸軌跡函數，將軌跡坐標描繪成二維坐標軸上實時變化的曲線;最后，編寫按鈕點擊事件的處理函數和主函數。

PC端界面效果展示如圖6所示。

4 ?系統測試和數據分析

系統測試開始前，將MPU6050傳感器水平放置，z軸方向垂直于水平面，x軸和y軸與水平面平行。電路板通過無線通信模塊與PC端進行TCP數據透傳，把姿態檢測與軌跡追蹤分開，單獨進行測試。

在姿態檢測測試階段，為確保系統可在不同類型的運動目標上適用，分別在室內靜止環境、室外電瓶車騎行環境、室外四輪玩具小車行駛環境三個條件進行了相應的測試，測試結果如表1所示。

在軌跡追蹤測試階段，控制室外四輪玩具小車在預定的路徑上分別按直線行駛和左轉彎行駛，在PC端界面的坐標軸上得到行駛軌跡，如圖7所示，描繪的追蹤軌跡和目標實際運動軌跡基本一致。

其余軌跡追蹤的測試結果如表2所示。

由以上測試數據和結果可以得出：在誤差允許范圍內，系統對運動目標的靜止、加速、轉彎等姿態判斷有較高的準確率，對目標在二維平面運動所描繪的軌跡與實際的運動軌跡基本一致，總體路線偏差控制在6.25%以內，達到了設計要求，可以推廣使用。

5 ?結束語

本文設計了一種基于六軸傳感器MPU6050的二維運動目標姿態檢測和軌跡追蹤系統。系統利用了MPU6050傳感器模塊、無線傳輸模塊等，在室內環境及GPS信號不好的環境下實現運動物體的檢測、定位和追蹤。

測試表明：系統不僅能較為準確地判斷目標的運動姿態，也可以進行軌跡的追蹤，并實時描繪軌跡曲線，總體路線偏差控制在6.25%以內。

系統具有體積小、操作簡單、實時性高、運行穩定等特點，能為汽車行駛狀態判斷、車載GPS盲區導航、室內機器人定位等研究提供一定的借鑒。

參考文獻

[1] 胡三慶. 基于MEMS加速度傳感器的空間運動軌跡追蹤系統設計與實現[D]. 武漢： 華中科技大學， 2009.

[2] 張秀再， 王躍飛， 王錫寧， 等. 基于MPU6050的運動目標軌跡追蹤系統[EB/OL]. 北京： 中國科技論文在線 [2018-09-30]. http：//www.paper.edu.cn/releasepaper/content/201809-73.

[3] 陳培才， 施文振， 田敬北， 等. 熱水器遠程智能控制系統設計[J]. 科技風， 2018（20）： 88.

[4] 王建東， 王亞飛， 張晶. 基于卡爾曼濾波器的運動目標跟蹤算法[J]. 數字通信， 2009， 36（6）： 53-57.

[5] Pitale A N， Bendre A A， Jadhav P M， et al. Signal conditioning algorithms on accelerometers in an Inertial Navigation System （INS）[C]// International Conference on Signal & Information Processing. IEEE， 2016.

[6] 陳義華. 基于加速度傳感器的定位系統研究[D]. 廈門： 廈門大學， 2006.

作者簡介：

邱鴻鑫（1999—），男，廣東汕頭人，華南師范大學，本科生。主要研究方向：電子信息。

熊愛民（1970—），通信作者，男，江西吉安人，華南師范大學，碩士，副教授。主要研究方向：電子與通信技術。

E-mail： 77057181@qq.com

（收稿日期：2020-05-12）

Two-dimensional Motion Target Gesture Detection and Trajectory Tracking System

QIU Hong-xin， SHE Shu-yi， LIN Ling-xuan， LIANG Zi-hao， XU Fu-ren， XIONG Ai-min

（College of Physics and Telecommunications Engineering， South China Normal University， Guangzhou 510006， China）

Abstract： The traditional motion target positioning system has some defects such as poor mobility， limited real-time performance and inapplicability to the target gesture detection. A two-dimensional motion target gesture detection and trajectory tracking system based on six-axis sensor MPU6050 is proposed. The system uses MPU6050 sensor to obtain the motion data of the target， transmits the data to STM32F103RCT6 main control chip for processing， adopts the wireless communication module to transmit the data processing results to the PC terminal， and displays the motion gesture and movement trajectory of the target in real-time at the PC terminal. The system is tested through motions including static， acceleration and turning， and the overall deviation of the route is controlled within 6.25%， which meets the design requirements. It can be promoted and used in the fields including vehicle driving state judgment， onboard GPS blind zone navigation and indoor robot positioning.

Key words： Gesture Detection; Trajectory Tracking; MPU6050; STM32F103RCT6; Wireless Communication