于向懷，向 政，孟祥濤

(北京航天時代光電科技有限公司，北京 100094)

0 引言

近年來，隨著“科技助力體育”口號的提出，越來越多的技術應用到了人體運動測量領域[1-2]。目前，常見的姿態(tài)測量方法有基于光學標記點的定位測姿方法、基于圖像分析的姿態(tài)識別方法以及慣性技術姿態(tài)測量?；诠鈱W標記點的定位測姿方法和基于圖像分析的姿態(tài)識別方法均為基于光學的姿態(tài)測量方法，在水面之上進行姿態(tài)測量時因為水的折射、水花和人體對光線的遮擋等問題，無法實現(xiàn)較好的姿態(tài)測量效果；水下光學測量則需要在狹窄環(huán)境下對設備進行布置，需要較多攝像頭才能夠針對全泳段進行姿態(tài)測量，成本較高且實現(xiàn)難度較大。目前，在國家體育總局訓練局已經安裝布置了一套15m泳段的光學姿態(tài)測量設備，但其僅能提供側向圖像的展示，不能給出定量姿態(tài)指標，同時價格也極為昂貴。隨著慣性技術中具有輕量化和成本低兩個特性的MEMS慣性器件的發(fā)展，慣性技術作為一種不依賴外部信息、多場景應用的技術，也逐漸引入人體運動測量中。

慣性姿態(tài)測量需要通過初始對準技術提供初值從而不斷積分計算得到連續(xù)的運動姿態(tài)信息。最傳統(tǒng)的初始對準方法是抗晃動粗對準[3]，其算法核心是假設載體處于靜止狀態(tài)，測量得到的角速度和加速度信息等于地球自轉角速度和重力加速度，兩者相比從而計算得到初始姿態(tài)角，但該算法忽略了晃動和慣性器件測量誤差的影響。另外，因為游泳運動佩戴傳感器輕質化、小型化的要求，必須使用MEMS器件進行測量，而目前的MEMS器件仍然不能很好地感應地速，所以抗晃動粗對準方法無法應用于游泳運動中。目前，慣性領域中常用的晃動基座下初始對準方法[4]則需要較長的時間才能完成相關工作，對運動員的使用體驗影響很大，所以一個快速且精度滿足要求的初始對準方法成為了亟待解決的問題。

本文提出了一種新的基于Mahony互補濾波算法的初始對準方法，基于MEMS-SINS初始對準原理，使用加速度計和磁力計對陀螺估計的角度進行修正，修正過程采用Mahony算法，很好地結合了陀螺的低頻性能和加速度計的高頻性能，可以快速提供一個精度符合游泳運動需求且具有一定抗干擾能力的初始對準結果。

1 游泳運動姿態(tài)特性分析

游泳運動是運動幅度較大、速度較快、全身肢體協(xié)調性要求較高的一項運動。對于競技游泳運動選手來說，提供最為全面和準確的反饋定量分析十分重要，游泳運動測量設備的設計需要與游泳運動過程的運動特性進行結合。

游泳運動具有以下特性：

1)游泳運動過程中的姿態(tài)變化具有一定的周期性；

2)游泳運動過程中的速度維持在1m／s～2m／s；

3)游泳運動過程中的橫滾角變化范圍為±60°；

4)游泳運動過程中的轉動角速度范圍為±720(°)／s；

5)游泳運動過程中不存在較長的靜止時間；

6)游泳運動需要盡可能地降低水的阻力。

綜上，游泳運動過程極為復雜，且對姿態(tài)測量設備的體積、質量以及使用體驗等有極高的要求，所以采用MEMS慣性器件組合作為姿態(tài)測量設備，并研究快速初始對準算法。因為慣性技術測量的是固定點位的姿態(tài)信息，所以選擇一個合適的測量點對反饋游泳運動過程極為重要。研究表明，髖部在游泳過程中可以集中體現(xiàn)運動員軀干扭轉幅度、扭轉頻率等關鍵信息，所以以其作為慣性姿態(tài)測量點反映運動員整體姿態(tài)運動較為合適，設備佩戴示意圖如圖1所示。

圖1 設備佩戴背部示意圖Fig.1 Schematic diagram of equipment wearing back

2 MEMS-SINS初始對準原理

靜基座初始對準時，其輸出與東北天導航坐標系中比力的關系為

式(2)中，fb＝[fxfyfz]T為加速度計的輸出，gn＝[0 0g]T為當?shù)刂亓κ噶?，g為當?shù)厮街亓铀俣?。由?1)和式(2)可以得到

從而計算得到兩個水平姿態(tài)角θ和γ，但無法獲取偏航角ψ， 故引入磁強計提供偏航角的參考信息。

用校準后的三軸磁強計載體坐標系的磁場強度[5-7]來確定導航坐標系與載體坐標系的關系

式(4)中，Mb為載體坐標系下的三維磁場強度，Mn為導航坐標系下的三維磁場強度，H0為當?shù)氐卮艌鏊綇姸龋翞楫數(shù)卮牌?，β為當?shù)卮艃A角。

由于

所以可以確定偏航角ψ。

綜上，由式(3)和式(8)可以從加速度信息和磁場強度信息計算得到姿態(tài)角，可以實現(xiàn)利用MEMS設備的游泳運動姿態(tài)測量初始姿態(tài)角的計算。

3 Mahony互補濾波方法

MEMS捷聯(lián)慣性導航系統(tǒng)計算得到的初始姿態(tài)角需要進行快速的修正從而獲得較高精度的結果。Mahony于2008年提出了一種非線性互補濾波進行姿態(tài)估計的算法適合應用于此過程[8-10]，其主要思想是利用地理系下的重力進行三軸角速度的修正，計算量小，且有著良好的跟隨能力和魯棒性，可以快速地得到姿態(tài)信息，但受限于重力矢量，只能夠對兩個水平姿態(tài)角進行估計。本文中引入了地磁場進行補充，從而能夠在偏航角方向提供參考，并進行估計，具體如圖2所示。

圖2 Mahony互補濾波算法流程圖Fig.2 Flowchart of Mahony complementary filtering algorithm

將誤差姿態(tài)角進行比例和積分處理，最終將PI運算結果加到當前陀螺所測得的三軸角速度上，Kp和Ki參數(shù)大小關聯(lián)修正陀螺角速度的速度，Kp和Ki越大，姿態(tài)收斂的速度就越快。

最后，將已經被加速度修正后的角速度代入式(11)中進行四元數(shù)的更新

更新完畢后的四元數(shù)會因為各個步驟中的計算誤差等因素逐漸失去單位化特性，再進行單位化處理，通過四元數(shù)與姿態(tài)角對應關系計算得到姿態(tài)角

由此可以快速地對初始姿態(tài)角度進行修正，從而獲得一個精度較高的初始姿態(tài)角，為游泳運動的姿態(tài)測量過程提供一個精度較高的初始值。

4 實驗與實測結果及分析

為了驗證算法的可行性，采用實驗室自研的MEMS慣性姿態(tài)測量系統(tǒng)對算法進行測試，設計框架如圖3所示。這是一款使用CC3200作為主控制器、MTI-3作為主傳感器、使用WIFI作為數(shù)據(jù)傳輸手段、使用FLASH作為數(shù)據(jù)存儲單元的慣性姿態(tài)測量系統(tǒng)，實物圖如圖4所示。按照游泳運動中傳感器對人體干擾盡可能小的要求，其質量僅為16g，尺寸僅為50mm×26mm×14mm，盡可能滿足了游泳運動佩戴的要求。

圖3 慣導系統(tǒng)設計框架Fig.3 Diagram of inertial navigation system design framework

圖4 MTI-3實物圖Fig.4 Physicaldrawing of MTI-3

MTI-3的選型是因為其體積小、質量小且角速度測量范圍大，MTI-3的最大測量可以達到2000(°)／s。游泳運動不同于航空常見的姿態(tài)測量應用中飛機的運動場景，游泳運動相比于飛機運動來講，姿態(tài)變化更加快速，轉動角速度極大，實測場景下存在720(°)／s級別的角速度，這個是在MTI-3的2000(°)／s包絡內，所以采用了此IMU作為數(shù)據(jù)采集單元。

IMU設備通過電池供電運行，數(shù)據(jù)臨時存儲在板載FLASH中，整體結構采用了IP68等級的防水設計，數(shù)據(jù)采集完畢后通過WIFI與上位機軟件進行數(shù)據(jù)通訊，將數(shù)據(jù)上傳并進行分析，系統(tǒng)整體運行框架如圖5所示。

圖5 系統(tǒng)整體展示Fig.5 Overalldisplay of the system

4.1 實驗結果與分析

為了驗證快速初始對準算法的可行性，在實驗室的大理石平板上進行精度評估測試，該平板每年一次校準，要求水平誤差不大于10″，北向基準誤差不大于1′，實際測試結果如圖6所示。

圖6 水平基準下的實際測試數(shù)據(jù)Fig.6 Diagram of actualtest data under horizontalreference

由圖6可知，x軸、y軸的加速度計存在0.15m／s2的零偏誤差。z軸為重力加速度測量軸，測量所在地為北京，北京的重力加速度值認定為9.8015m／s2，所以其也存在零偏誤差。因為地球自轉角速度為7.292×10－5rad／s，而圖示數(shù)據(jù)的穩(wěn)定性都在10－3量級，故陀螺無法感應到地球自轉角速度。

圖7展示了在水平基準上進行測試的過程。因為測試時是按照東北天的方位角進行放置的，所以水平角對準結果的真實值為0°，若計算初始值也為0°，則無法展示收斂過程，故將初始值設置為一個偏差較大的值從而展示收斂過程?？梢钥闯觯撍惴ㄔ?.2s時進入了收斂環(huán)節(jié)，即5%誤差帶；在0.4s時收斂完畢，達到了快速性的要求。采用3s的數(shù)據(jù)作為對準結果，發(fā)現(xiàn)其水平角度對準結果為-0.042°和0.077°，精度為角分級別；偏航角精度受磁力計以及磁場建模精度限制僅為度級別，但也符合游泳運動的需求。

圖7 水平基準下的對準結果Fig.7 Diagram of alignment results under horizontalreference

4.2 實測結果與分析

為了驗證快速初始對準算法的實際應用性能，在浙江省水上運動管理中心請國家游泳隊運動員進行了實際的測試。設備按照圖8的方式進行了佩戴，其x軸朝右，y軸朝前，z軸朝天，傳感器y軸與人體脊骨對齊， 并經過嚴格調整，因為綁帶的限制，此處的安裝誤差僅對橫滾角造成小角度的固定偏差影響，本文關注于對準方法以及對準誤差，不再對此處進行探討。實際測試請專業(yè)運動員按照實際運動過程做了準備工作，其中包含了跳前的跳臺預備跳水的靜止過程。

圖8 運動員佩戴設備示意圖Fig.8 Schematic diagram of equipment wearing by swimmers

實測數(shù)據(jù)如圖9所示?？梢钥闯?，在跳水前運動員在12s～20s保持了8s的靜止狀態(tài)，提供了初始對準的時間窗口期，在20s～45s跳水并進行了前進過程，在45s～50s進行了翻身動作，在50s～85s進行了返回，其余時間的數(shù)據(jù)為運動數(shù)據(jù)之外的佩戴與取下設備的時間。

圖9 運動員游泳實際采集數(shù)據(jù)Fig.9 Diagram of realdata collected by swimmers

對準窗口期的對準結果如圖10所示，因為無法保證運動員何時跳水，所以算法設定為與實驗測試一致的3s對準時間?？梢钥闯觯瑢崪y數(shù)據(jù)上算法在0.2s時進入了收斂環(huán)節(jié)，由于運動員本身不是絕對的靜止狀態(tài)，水平姿態(tài)角存在一些波動，無法衡量是否收斂完成，但0.3s后圍繞定值上下波動。偏航角因為磁力計的精度僅為度級別，所以近乎不波動。同時也可以看出，運動員對于自身靜止的保持極為優(yōu)秀。

圖10 實測數(shù)據(jù)算法對準結果Fig.10 Alignment results of real measured data

圖11是圖9中數(shù)據(jù)以圖10的初始對準結果作為初始值進行姿態(tài)測量的結果。其中，為了方便運動員與教練員理解，特意將偏航角設置為0°，并讓角度范圍從-180°～180°變成了0°～360°。26.85s前運動員從起點游動到終點，26.85s～28.15s為運動員終點轉身動作，28.15s后游回起點?？梢钥闯觯\動員各個運動階段的時間與上邊根據(jù)原始數(shù)據(jù)進行推算的時間保持了一致，很好地還原了整個運動過程。

圖11 實際姿態(tài)測量結果圖Fig.11 Diagram of realattitude measurement results

5 結論

本文針對游泳運動場景下慣性姿態(tài)測量初始對準時間短、精度高的要求，提出了采用Mahony算法進行快速對準的方法。通過角速度信息計算姿態(tài)角，加速度信息修正水平姿態(tài)角，磁場強度信息修正偏航角，使用互補濾波方法對多源信息進行融合，從而快速得到慣性姿態(tài)測量的初值。在實驗室場景和實際游泳運動場景對該算法進行了驗證，本算法可以在3s內達到姿態(tài)水平角角分級別、偏航角度級別的對準精度，滿足了游泳運動姿態(tài)測量的要求。