于向懷，向 政，孟祥濤

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

0 引言

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

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

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

1 游泳運(yùn)動(dòng)姿態(tài)特性分析

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

游泳運(yùn)動(dòng)具有以下特性：

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

2)游泳運(yùn)動(dòng)過程中的速度維持在1m／s～2m／s；

3)游泳運(yùn)動(dòng)過程中的橫滾角變化范圍為±60°；

4)游泳運(yùn)動(dòng)過程中的轉(zhuǎn)動(dòng)角速度范圍為±720(°)／s；

5)游泳運(yùn)動(dòng)過程中不存在較長的靜止時(shí)間；

6)游泳運(yùn)動(dòng)需要盡可能地降低水的阻力。

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

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

2 MEMS-SINS初始對準(zhǔn)原理

靜基座初始對準(zhǔn)時(shí)，其輸出與東北天導(dǎo)航坐標(biāo)系中比力的關(guān)系為

式(2)中，fb＝[fxfyfz]T為加速度計(jì)的輸出，gn＝[0 0g]T為當(dāng)?shù)刂亓κ噶浚琯為當(dāng)?shù)厮街亓铀俣取Ｓ墒?1)和式(2)可以得到

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

用校準(zhǔn)后的三軸磁強(qiáng)計(jì)載體坐標(biāo)系的磁場強(qiáng)度[5-7]來確定導(dǎo)航坐標(biāo)系與載體坐標(biāo)系的關(guān)系

式(4)中，Mb為載體坐標(biāo)系下的三維磁場強(qiáng)度，Mn為導(dǎo)航坐標(biāo)系下的三維磁場強(qiáng)度，H0為當(dāng)?shù)氐卮艌鏊綇?qiáng)度，α為當(dāng)?shù)卮牌牵聻楫?dāng)?shù)卮艃A角。

由于

所以可以確定偏航角ψ。

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

3 Mahony互補(bǔ)濾波方法

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

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

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

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

更新完畢后的四元數(shù)會因?yàn)楦鱾€(gè)步驟中的計(jì)算誤差等因素逐漸失去單位化特性，再進(jìn)行單位化處理，通過四元數(shù)與姿態(tài)角對應(yīng)關(guān)系計(jì)算得到姿態(tài)角

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

4 實(shí)驗(yàn)與實(shí)測結(jié)果及分析

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

圖3 慣導(dǎo)系統(tǒng)設(shè)計(jì)框架Fig.3 Diagram of inertial navigation system design framework

圖4 MTI-3實(shí)物圖Fig.4 Physicaldrawing of MTI-3

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

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

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

4.1 實(shí)驗(yàn)結(jié)果與分析

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

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

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

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

圖7 水平基準(zhǔn)下的對準(zhǔn)結(jié)果Fig.7 Diagram of alignment results under horizontalreference

4.2 實(shí)測結(jié)果與分析

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

圖8 運(yùn)動(dòng)員佩戴設(shè)備示意圖Fig.8 Schematic diagram of equipment wearing by swimmers

實(shí)測數(shù)據(jù)如圖9所示?？梢钥闯觯谔斑\(yùn)動(dòng)員在12s～20s保持了8s的靜止?fàn)顟B(tài)，提供了初始對準(zhǔn)的時(shí)間窗口期，在20s～45s跳水并進(jìn)行了前進(jìn)過程，在45s～50s進(jìn)行了翻身動(dòng)作，在50s～85s進(jìn)行了返回，其余時(shí)間的數(shù)據(jù)為運(yùn)動(dòng)數(shù)據(jù)之外的佩戴與取下設(shè)備的時(shí)間。

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

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

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

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

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

5 結(jié)論

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