張顥齡，張金榮，黃熙瑞，梁浩天

(1.重慶理工大學(xué), 重慶　400054； 2.四川大學(xué)， 成都　610065)

測繪的核心在于通過人或物采集現(xiàn)實(shí)環(huán)境與所需地理信息之間的關(guān)系進(jìn)行精確的映射，因而精確且即時(shí)的定位成為測繪的首要問題之一。很多即時(shí)精確測繪研究是從全球衛(wèi)星導(dǎo)航定位系統(tǒng)(global navigation satellite system，GNSS)展開的，其中比較常用的GNSS測量方法是實(shí)時(shí)動態(tài)定位技術(shù)(real-time kinematic，RTK)。在2016年的測試中，RTK的即時(shí)誤差為94.58 cm，靜止10 s后可縮減至4.39 cm[1]，靜止?fàn)顟B(tài)的誤差在大多數(shù)測繪中可忽略不計(jì)。由于與GNSS相關(guān)的技術(shù)容易受到環(huán)境限制和信號干擾[2]，單純使用低成本GNSS定位很難滿足測繪所需精確性與即時(shí)性，因此衍生出眾多的定位技術(shù)。其中較主流的定位技術(shù)有超寬帶[3-4]、射頻識別[4-5]、藍(lán)牙[6]、無線局域網(wǎng)[7]技術(shù)等。在實(shí)際測量中通常會對多種定位技術(shù)進(jìn)行數(shù)據(jù)融合，但無法避免基站范圍的約束。航空遙感測繪同樣受到天氣、地表覆蓋等問題的影響[8]。

目前地球仍有很多陸地未被人類所探索，部分地域缺乏基礎(chǔ)設(shè)施。針對特定無基礎(chǔ)設(shè)施地區(qū)的定位局限，有必要進(jìn)行無基礎(chǔ)設(shè)施定位服務(wù)的探索。慣性導(dǎo)航系統(tǒng)(inertial navigation system，INS)與需要預(yù)先架設(shè)定位基站的定位方案不同，其融合加速度傳感器、陀螺儀、地磁傳感器等傳感器數(shù)據(jù)完成相對位置轉(zhuǎn)換的位移測量[9]。INS最主要的缺點(diǎn)是誤差會不斷累積放大，所以INS通常和GNSS數(shù)據(jù)融合后使用[10]。

相比現(xiàn)代測繪，國內(nèi)針對傳統(tǒng)測繪的研究相對較少。目前在傳統(tǒng)測繪中使用較多的是行走測繪，即采用平均步距乘以步數(shù)的方式計(jì)算單程長度，并使用指北針確定方法，得出單次測量的兩點(diǎn)之間相對二維位置。行走測繪存在對人的身體要求較高(肌肉記憶)、無法獲取高度變量以及測量精度較低等問題，因而在精確測繪領(lǐng)域使用較少。文獻(xiàn)[11]對相關(guān)下肢狀態(tài)估計(jì)研究構(gòu)建了一種基于低成本慣性傳感器的位置和步態(tài)跟蹤的解決方案，但其只針對單側(cè)下肢的狀態(tài)進(jìn)行跟蹤，并非針對測繪領(lǐng)域。本文使用無動力外骨骼[12-13]作為提升人下肢測量精度的工具，提出一種基于行走測繪的單步移動相對空間位置測量方案。該方案減小了設(shè)備的工作負(fù)荷，增強(qiáng)了實(shí)際測繪應(yīng)用的可行性。

1　基于無動力外骨骼的測量方案分析

人在正常行進(jìn)過程中相對位置的改變實(shí)質(zhì)上是下肢相對位置做出的交替性改變[14]。本文的思路是使用基于無動力外骨骼的設(shè)備對交替性改變對應(yīng)相對位置的矢量做可靠性測量。用式(1)(2)表示為：

(1)

(2)

其中:Pn表示通過n次半個(gè)步態(tài)周期，人的重心距初始點(diǎn)PI所在地理坐標(biāo)系的三維矢量，所以每步后人的重心都會新增RD(i-1,i)的相對位移矩陣。通常PI設(shè)置為零矩陣或依據(jù)GNSS初始數(shù)據(jù)設(shè)置。

1.1　位置關(guān)系構(gòu)建與剛體三維空間矢量算法

空間矢量測量可分為接觸式和非接觸式。

1.1.1接觸式測量的解算推導(dǎo)

本文的接觸式測量部分相關(guān)試驗(yàn)采用MPU9250作為姿態(tài)角獲取的傳感器,MIRAN-KTC 150作為瞬時(shí)位移獲取的傳感器，將從2個(gè)傳感器中即時(shí)采集到的數(shù)據(jù)輸入到STM32中進(jìn)行數(shù)據(jù)融合和分析。三維空間測量模塊如圖1所示。

圖1　三維空間測量模塊

試驗(yàn)中的數(shù)據(jù)融合與卡爾曼濾波主要參考文獻(xiàn)[15]，并進(jìn)行了參數(shù)調(diào)整，姿態(tài)角的融合與解算本文不做贅述，下文直接使用融合和濾波后得到的數(shù)據(jù)。在公式中的正東方向、正北方向和朝天方向分別使用E、N和S表示，仰俯角、航偏角和橫滾角分別使用Pitch、Yaw和Roll表示。

初始狀態(tài)為：

其中：Eo、No和So分別表示正東方向、正北方向和正天方向的單位矢量；Lo表示原始姿態(tài)剛體帶正東方向、正北方向和正天方向的起止點(diǎn)所構(gòu)成的矢量矩陣。

根據(jù)機(jī)體坐標(biāo)系的定義可得，該坐標(biāo)系是依據(jù)歐拉角變換得到的[16]，故即時(shí)狀態(tài)如式(3)所示：

(3)

其中：Ec、Nc和Sc分別為在觀測者進(jìn)行姿態(tài)變換后，當(dāng)前機(jī)體坐標(biāo)系軸方向相對于地理坐標(biāo)系的矢量矩陣。采用式(4)得出當(dāng)前模塊在當(dāng)前狀態(tài)下對應(yīng)的地理坐標(biāo)系的三維空間矢量MD為

(4)

1.1.2非接觸式測量的解算推導(dǎo)

非接觸式測量通常出現(xiàn)在2個(gè)三維空間測量模塊之間，即已知2個(gè)模塊的天向量。通過附加的非接觸位移傳感器及高度傳感器可以求出三維空間測量模塊之間的三維空間矢量。

圖2　人體模型抽象示意圖

因此，已知兩模塊頂端相對高度差為Δh，相對位移為L，則人體模型的抽象立體幾何如圖2所示。其中線段AB為后模塊(右)，線段CD為前模塊(左)，BC為兩模塊頂端相對位移，CE為兩模塊頂端相對高度。已知式(5)(6)：

(5)

MDT(AB×CD)=0

(6)

則求得X后，可得矢量MD。遺憾的是，目前已知的高度傳感器無法達(dá)到測量所需的毫米級精度，所以使用非接觸式測量的方案并不成熟。

1.2　運(yùn)動力學(xué)特征分析與數(shù)據(jù)采集

通過檢索，獲取到在舒適的步態(tài)下的最大速度為4.5 km/h左右[17]。但由于測量過程需穿戴設(shè)備，相對速度較低，所以選用1.5～3.5 km/h區(qū)間，以0.5 km/h為單位變量對2組20～40歲區(qū)間的成年正常男性進(jìn)行測試。穩(wěn)定且有代表性的實(shí)際數(shù)據(jù)如圖3所示。

圖3　膝部和腳跟單步態(tài)周期加速度數(shù)據(jù)

首先使用軟件對被測人員做圖像追蹤分析，發(fā)現(xiàn)步態(tài)中最穩(wěn)定的2個(gè)位置分別是膝蓋和腳后跟，所以在這兩點(diǎn)安放加速度計(jì)并按照上述方法做進(jìn)一步測試。

圖4　M型腳跟加速度物理模型

在進(jìn)行卡爾曼濾波后，通過對加速度的統(tǒng)計(jì)和切片分析發(fā)現(xiàn)膝關(guān)節(jié)加速度在步態(tài)周期中的周期形態(tài)不固定。當(dāng)人的速度在1.5 km/h及以下時(shí)，由于人步幅較短，所以膝關(guān)節(jié)的變化不明顯。因而，從單側(cè)膝關(guān)節(jié)相對地面最高點(diǎn)到第二次達(dá)到相對地面最高點(diǎn)時(shí)，整個(gè)周期性變化如同一個(gè)加粗的“N”型，稱之為“N”型膝部加速度變化周期。由2.0～3.5 km/h的加速度圖像可以得出：從單側(cè)膝關(guān)節(jié)相對地面最高點(diǎn)到第二次達(dá)到相對地面最高點(diǎn)時(shí)，周期性變化呈現(xiàn)為加粗的“M”型，稱之為“M”型膝部加速度變化周期。相比之下，腳跟加速度在整個(gè)測量區(qū)間內(nèi)比較穩(wěn)定且易甄別，稱之為“M型腳跟加速度物理模型”。M型腳跟加速度物理模型如圖4所示。

由于人的正常步態(tài)是按周期性進(jìn)行變化的[18]，所以在正常步態(tài)中，人體下肢運(yùn)動力學(xué)的特征也是周期性變化的，所以采用有窮自動機(jī)進(jìn)行研究。整個(gè)步態(tài)周期中，M型腳跟加速度物理模型共分為9個(gè)狀態(tài)，由于會有異常步態(tài)的情況，故試驗(yàn)中通過有窮自動機(jī)進(jìn)行異常狀態(tài)過濾和正常狀態(tài)捕獲，并提供對應(yīng)的流程(如圖5所示)。

圖5中，狀態(tài)0為開機(jī)狀態(tài)，通過匹配進(jìn)入狀態(tài)1，狀態(tài)1至狀態(tài)3、狀態(tài)7至狀態(tài)9為擺動相，從狀態(tài)3瞬時(shí)替換成狀態(tài)4正好是支撐相首次觸地的瞬時(shí)時(shí)刻，狀態(tài)4至狀態(tài)6為支撐相承重反應(yīng)期，狀態(tài)6瞬時(shí)替換成狀態(tài)7為支撐相末期足跟離地的瞬時(shí)時(shí)刻。在試驗(yàn)中，狀態(tài)3瞬時(shí)替換成狀態(tài)4的瞬時(shí)時(shí)間記錄肢體狀態(tài)，狀態(tài)6瞬時(shí)替換成狀態(tài)7的瞬時(shí)時(shí)間對系統(tǒng)單次測量做歸零和初始化。

圖5　M型腳跟加速度有窮自動機(jī)模型流程

1.3　兩點(diǎn)和四點(diǎn)相對位置測量方案

為了簡化系統(tǒng)復(fù)雜度，首先提出兩點(diǎn)相對位置測量方案和4點(diǎn)相對位置測量方案。兩個(gè)方案的具體實(shí)施模型如圖6所示。

考慮到本文1.1.2節(jié)所述的非接觸式測量的精度問題，以及2點(diǎn)和4點(diǎn)相對位置測量方案對環(huán)境的要求較高，只可在無遮擋且較平緩的地面進(jìn)行測量(比如上下樓梯、大于30度的下坡等)，故舍棄該測量方案。

1.4　六點(diǎn)相對位置測量方案

在2點(diǎn)和4點(diǎn)相對位置測量方案的基礎(chǔ)上，考慮實(shí)際情況做進(jìn)一步分析，得到了較為成熟、實(shí)際可行的方案，稱之為6點(diǎn)相對位置測量方案。其測量模型如圖7所示。

圖6　2點(diǎn)和4點(diǎn)相對位置測量方案

該方案參考人體下肢6自由度的D-H模型[19]，采用M型腳跟加速度有窮自動機(jī)觸發(fā)設(shè)備對人的空間狀態(tài)進(jìn)行記錄并基于剛體三維空間矢量算法對單位模塊進(jìn)行空間解算。

整個(gè)方案共有7個(gè)三維空間可測量單位模塊，分別在腰間、兩大腿、兩小腿及兩腳掌。故每步三維空間矢量可用式(7)表示。

(7)

其中：RD表示單步三維空間矢量；i表示對應(yīng)的模塊編號，在實(shí)際測量中，i的位置從后腳模塊開始依據(jù)連接順序編排到前腳模塊。

2　試驗(yàn)結(jié)果分析

表1　3.0 km/h狀態(tài)下的單步即時(shí)性誤差　ms

圖8　三維誤差模型截面圖

對本文的觀測誤差從測量儀器、觀測者和外界條件3個(gè)方面進(jìn)行討論[20]。

2.1　儀器誤差對方案的影響

六點(diǎn)相對位置測量方案的儀器誤差主要源自姿態(tài)傳感器、位移測量傳感器的所得數(shù)據(jù)的即時(shí)性與精度。對測量即時(shí)性進(jìn)行分析，采用壓力傳感器數(shù)據(jù)收集時(shí)刻與M型腳跟加速度有窮自動機(jī)模型數(shù)據(jù)收集時(shí)刻作差，得到如表1的數(shù)據(jù)。

共進(jìn)行5組試驗(yàn)，其中正值表示M型腳跟加速度有窮自動機(jī)模型步態(tài)采集時(shí)刻比壓力傳感器滯后，反之提前。由表1可知：單次采集的誤差在區(qū)間(-6 ms,10 ms)，平均滯后2.64 ms，方差為4.619。故整體模塊測量采用提前2.64 ms的數(shù)據(jù)進(jìn)行運(yùn)算。

對三維空間可測量單位模塊單次測量的精度如圖8所示，對角度和位移進(jìn)行整合運(yùn)算與評估。其中：α為最大誤差角度；d為最大誤差長度；l為單次長度測量準(zhǔn)確值；斜交叉線表示理論正確值及其完整誤差三維鏡面區(qū)域?？赏ㄟ^誤差終點(diǎn)和正確終點(diǎn)的相對距離計(jì)算出誤差情況。對三維誤差評估的表達(dá)見式(8)。

(8)

其中：δ表示實(shí)際相誤差，下文使用全稱表達(dá)式δ(l,α,d)；MDt表示三維測量真值，MDr表示三維測量實(shí)際值；lc表示瞬時(shí)長度真值；αc表示當(dāng)前角度誤差且αc∈(0,α)；dc表示當(dāng)前誤差且dc∈(0,d)。根據(jù)實(shí)際誤差情況求得誤差長度和誤差角度的概率，結(jié)果如圖9、10所示。已知兩種誤差的后驗(yàn)概率，計(jì)算對應(yīng)誤差的可能性，對應(yīng)α、d和l所在瞬時(shí)狀態(tài)下的概率見式(9)。

P(l,d,α)=P(l)×P(d)×P(α)

(9)

圖9　α的誤差概率

其中α和d的誤差概率函數(shù)由式(10)(11)表示。

(10)

(11)

由于P(l)受人腿長的影響，故本文以被測者的小腿長(l=43 cm)為例。采用抗差貝葉斯估計(jì)方法[21]求得當(dāng)前情況下的理論誤差及其概率分布如圖11所示。

2.2　異常步態(tài)對方案的影響

目前，6點(diǎn)相對位置測量方案僅對簡單的瞬時(shí)異常步態(tài)做處理。從系統(tǒng)制作的角度考慮，將小腿與腳踝部分的連接由球面連接更改為滾動連接以去除腳踝的內(nèi)外翻動作。雖然舒適度有所下降，但提升了測量的可靠性，降低了異常步態(tài)的發(fā)生率[22]。

2.3　外界環(huán)境對方案的影響

對6點(diǎn)相對位置測量方案而言，外界環(huán)境影響主要包括強(qiáng)磁和溫度，但在該方面并未對儀器做進(jìn)一步預(yù)測、分析和處理。

2.4　正常步態(tài)下的綜合測量數(shù)據(jù)

由于上下坡本身的相對位置難以衡量，故本方案只測量了水平面行走及上下樓梯的情況。

在4 m×4 m的水平面上進(jìn)行回歸原點(diǎn)測試，結(jié)果如圖12所示。

圖11　定長43 cm下的理論誤差及其概率分布

圍繞該平面行走一圈，共計(jì)17步，西東方向誤差為0.17 m，南北方向誤差為-0.19 m，相對誤差位移為0.25 m。

選用單梯高為15 cm、層間梯數(shù)為2×11的樓梯進(jìn)行高度測試。在實(shí)際測量中，測量數(shù)據(jù)如圖13、14所示。

上樓造成的高度誤差相比下樓時(shí)較小，上樓單步高度誤差區(qū)間為(-0.01 m,0.01 m)，下樓單步高度誤差區(qū)間為(-0.02 m,0.02 m)?？傮w來說，目前方案的精確度可達(dá)到分米級別，誤差同行徑距離正相關(guān)，單步平均誤差為3 cm左右，建議同GPS進(jìn)行數(shù)據(jù)融合使用。

3　結(jié)束語

為了解決無基礎(chǔ)設(shè)施的定位問題，本文提出了多套相對位置測量方案。在方案實(shí)施前，建立了用于設(shè)備模塊化并可進(jìn)行剛體三維空間矢量算法解算的三維空間測量模塊以及用于步態(tài)捕獲的M型腳跟加速度有窮自動機(jī)模型。本文通過分步分析和抗差貝葉斯估計(jì)方法驗(yàn)證所提出方案的可行性和有效性。試驗(yàn)結(jié)果表明：兩點(diǎn)和四點(diǎn)相對位置測量方案由于受傳感器精度和環(huán)境限制的影響暫不能進(jìn)行有效的測量，六點(diǎn)相對位置測量方案的單步實(shí)際空間平均誤差在3 cm左右、單程(50步)誤差可達(dá)分米級別。下一步工作中，考慮將六點(diǎn)相對位置測量方案作為基礎(chǔ)，研究在跌倒等異常情況下的數(shù)據(jù)校準(zhǔn)并在植被密度較大的山區(qū)進(jìn)行試驗(yàn)，驗(yàn)證方案的有效性。

參考文獻(xiàn)：

[1]SPETH T,KAMANN A,BRANDMEIER T,et al.Precise relative ego-positioning by stand-alone RTK-GPS[C]//Positioning,Navigation and Communications (WPNC),2016 13th Workshop on.USA:IEEE,2016:1-6.

[2]ABD R M,EL-RABBANY A.Performance analysis of precise point positioning using multi-constellation GNSS:GPS,GLONASS,Galileo and BeiDou[J].Survey Review,2017,49(352):39-50.

[3]PROMWONG S,SOUTHISOMBAT P.UWB transmission measurement and modeling for indoor localization[C]//2016 International Conference (ICSEC) on Computer Science and Engineering.USA:IEEE,2016:1-4.

[4]ZHAI C,ZOU Z,ZHOU Q,et al.A 2.4-GHz ISM RF and UWB hybrid RFID real-time locating system for industrial enterprise Internet of Things[J].Enterprise Information Systems,2017,11(6):909-926.

[5]VARGO R,ZOSCHG R,BORGOYN E C,et al.Method and System For Correctly Identifying Specific Rfid Tags[P].U.S.Patent 20,170,004,334.2017-1-5.

[6]CONTI M.Real Time Localization Using Bluetooth Low Energy[C]//International Conference on Bioinformatics and Biomedical Engineering.USA:Springer,2017:584-595.

[8]LILLESAND T,KIEFER R W,CHIPMAN J.Remote sensing and image interpretation[M].USA:John Wiley & Sons,2014.

[9]GAO P,LI K,WANG L,et al.A Self-Calibration Method for Non-Orthogonal Angles of Gimbals in Tri-Axis Rotational Inertial Navigation System[J].IEEE Sensors Journal,2016 (99):1-1.

[10] 甘雨.GNSS/INS組合系統(tǒng)模型精化及載波相位定位測姿[J].測繪學(xué)報(bào),2015,44(11):1295-1295.

[11] ZHI R.A Drift Eliminated Attitude & Position Estimation Algorithm In 3D[Z].2016.

[12] 張佳帆,陳鷹,楊燦軍.柔性外骨骼人機(jī)智能系統(tǒng)[M].北京：科學(xué)出版社,2011.

[13] COLLINS S H,WIGGIN M B,SAWICKI G S.Reducing the energy cost of human walking using an unpowered exoskeleton[J].Nature， 2015,522(7555):212-215.

[14] LEVINE D,RICHARDS J,WHITTLE M W.Whittle’s gait analysis[M].USA:Elsevier Health Sciences,2012.

[15] CHUNYANG L,FAN C,XIN S,et al.Gesture detection and data fusion based on MPU9250 sensor[C]//2015 12th IEEE International Conference on Electronic Measurement & Instruments (ICEMI).USA:IEEE,2015,3:1612-1615.

[16] ROSKAM J.Airplane flight dynamics and automatic flight controls[M].USA:DARcorporation,1995.

[17] BOHANNON R W.Comfortable and maximum walking speed of adults aged 20—79 years:reference values and determinants[J].Age and ageing,1997,26(1):15-19.

[18] GAGE J R,DELUCA P A,RENSHAW T S.Gait analysis:principle and applications with emphasis on its use in cerebral palsy[J].Instructional Course Lectures,1996,45(4):491.

[19] PONS J L.Wearable robots:biomechatronic exoskeletons[M].USA:John Wiley & Sons,2008.

[20] 武漢大學(xué)測繪學(xué)院測量平差學(xué)科組.誤差理論與測量平差基礎(chǔ)[M].武漢:武漢大學(xué)出版社,2014.

[21] 楊元喜.抗差貝葉斯估計(jì)及應(yīng)用[J].測繪學(xué)報(bào),1992(1):42-49.

[22] PARK Y,CHO W,QUAN C,et al.A gait imbalance evaluation system using multi-sensors[C]//2016 IEEE Conference on Computational Intelligence in Bioinformatics and Computational Biology (CIBCB).USA:IEEE,2016:1- 4.