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

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

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

中圖分類號(hào)：TP37 ? ?文獻(xiàn)標(biāo)識(shí)碼：A ? ?文章編號(hào)：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

引言

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

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

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

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

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

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

1.2 ?工作原理

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

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

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

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

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

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

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

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

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

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

2.3 ?軌跡坐標(biāo)計(jì)算程序

本系統(tǒng)以目標(biāo)在追蹤開始時(shí)刻的前進(jìn)方向?yàn)槎S平面x軸的正方向，以x軸正方向逆時(shí)針旋轉(zhuǎn)90°的方向?yàn)槎S平面y軸的正方向，即坐標(biāo)系是在系統(tǒng)開始追蹤目標(biāo)軌跡的第一時(shí)間確定的。

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

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

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

（1）

（2）

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

（3）

（4）

其中，為k時(shí)刻下的航偏角。軌跡坐標(biāo)計(jì)算流程如圖5所示。

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

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

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

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

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

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

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

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

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

5 ?結(jié)束語

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

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

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

參考文獻(xiàn)

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

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

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

[4] 王建東， 王亞飛， 張晶. 基于卡爾曼濾波器的運(yùn)動(dòng)目標(biāo)跟蹤算法[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]. 廈門： 廈門大學(xué)， 2006.

作者簡(jiǎn)介：

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

熊愛民（1970—），通信作者，男，江西吉安人，華南師范大學(xué)，碩士，副教授。主要研究方向：電子與通信技術(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