基于Mahony算法的游泳運動姿態測量快速對準算法

于向懷，向 政，孟祥濤

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

0 引言

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

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

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

1 游泳運動姿態特性分析

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

游泳運動具有以下特性：

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

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

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

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

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

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

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

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

2 MEMS-SINS初始對準原理

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

式(2)中，fb＝[fxfyfz]T為加速度計的輸出，gn＝[0 0g]T為當地重力矢量，g為當地水平重力加速度。由式(1)和式(2)可以得到

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

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

式(4)中，Mb為載體坐標系下的三維磁場強度，Mn為導航坐標系下的三維磁場強度，H0為當地地磁場水平強度，α為當地磁偏角，β為當地磁傾角。

由于

所以可以確定偏航角ψ。

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

3 Mahony互補濾波方法

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

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

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

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

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

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

4 實驗與實測結果及分析

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

圖3 慣導系統設計框架Fig.3 Diagram of inertial navigation system design framework

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

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

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

圖5 系統整體展示Fig.5 Overalldisplay of the system

4.1 實驗結果與分析

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

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

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

圖7展示了在水平基準上進行測試的過程。因為測試時是按照東北天的方位角進行放置的，所以水平角對準結果的真實值為0°，若計算初始值也為0°，則無法展示收斂過程，故將初始值設置為一個偏差較大的值從而展示收斂過程。可以看出，該算法在0.2s時進入了收斂環節，即5%誤差帶；在0.4s時收斂完畢，達到了快速性的要求。采用3s的數據作為對準結果，發現其水平角度對準結果為-0.042°和0.077°，精度為角分級別；偏航角精度受磁力計以及磁場建模精度限制僅為度級別，但也符合游泳運動的需求。

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

4.2 實測結果與分析

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

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

實測數據如圖9所示。可以看出，在跳水前運動員在12s～20s保持了8s的靜止狀態，提供了初始對準的時間窗口期，在20s～45s跳水并進行了前進過程，在45s～50s進行了翻身動作，在50s～85s進行了返回，其余時間的數據為運動數據之外的佩戴與取下設備的時間。

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

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

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

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

圖11 實際姿態測量結果圖Fig.11 Diagram of realattitude measurement results

5 結論

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