二維運動目標姿態(tài)檢測和軌跡追蹤系統(tǒng)

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

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

關(guān)鍵詞： 姿態(tài)檢測;軌跡追蹤;MPU6050;STM32F103RCT6;無線通信

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

工業(yè)技術(shù)創(chuàng)新 URL： http：//gyjs.cbpt.cnki.net ? ?DOI： 10.14103/j.issn.2095-8412.2020.04.012

引言

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

張秀再等[2]提出了利用慣性元件進行軌跡追蹤的系統(tǒng)，其利用串口通信將數(shù)據(jù)傳輸?shù)絇C端，這種有線傳輸?shù)姆绞皆跍y試和實際使用中存在可移動性差等缺點，會受到一定限制。無線通信可移動性好，但在GPS信號不佳的環(huán)境下會受到實時性的限制[3]。

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

1 系統(tǒng)構(gòu)成與工作原理

1.1 ?系統(tǒng)構(gòu)成

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

1.2 ?工作原理

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

2 ?系統(tǒng)軟件設(shè)計

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

2.1 ?原始數(shù)據(jù)處理流程

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

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

2.2 ?姿態(tài)檢測判斷程序

姿態(tài)檢測判斷程序包括加減速判斷程序和轉(zhuǎn)彎判斷程序，其姿態(tài)判斷流程如圖4所示。

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

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

2.3 ?軌跡坐標計算程序

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

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

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

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

（1）

（2）

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

（3）

（4）

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

3 ?PC端程序設(shè)計及界面設(shè)計

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

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

4 ?系統(tǒng)測試和數(shù)據(jù)分析

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

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

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

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

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

5 ?結(jié)束語

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

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

系統(tǒng)具有體積小、操作簡單、實時性高、運行穩(wěn)定等特點，能為汽車行駛狀態(tài)判斷、車載GPS盲區(qū)導(dǎo)航、室內(nèi)機器人定位等研究提供一定的借鑒。

參考文獻

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

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

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

[4] 王建東， 王亞飛， 張晶. 基于卡爾曼濾波器的運動目標跟蹤算法[J]. 數(shù)字通信， 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] 陳義華. 基于加速度傳感器的定位系統(tǒng)研究[D]. 廈門： 廈門大學， 2006.

作者簡介：

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

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

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