基于無動力外骨骼的無基礎設施三維空間定位

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

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

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

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

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

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

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

(1)

(2)

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

1.1　位置關系構建與剛體三維空間矢量算法

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

1.1.1接觸式測量的解算推導

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

圖1　三維空間測量模塊

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

初始狀態為：

其中：Eo、No和So分別表示正東方向、正北方向和正天方向的單位矢量；Lo表示原始姿態剛體帶正東方向、正北方向和正天方向的起止點所構成的矢量矩陣。

根據機體坐標系的定義可得，該坐標系是依據歐拉角變換得到的[16]，故即時狀態如式(3)所示：

(3)

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

(4)

1.1.2非接觸式測量的解算推導

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

圖2　人體模型抽象示意圖

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

(5)

MDT(AB×CD)=0

(6)

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

1.2　運動力學特征分析與數據采集

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

圖3　膝部和腳跟單步態周期加速度數據

首先使用軟件對被測人員做圖像追蹤分析，發現步態中最穩定的2個位置分別是膝蓋和腳后跟，所以在這兩點安放加速度計并按照上述方法做進一步測試。

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

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

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

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

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

1.3　兩點和四點相對位置測量方案

為了簡化系統復雜度，首先提出兩點相對位置測量方案和4點相對位置測量方案。兩個方案的具體實施模型如圖6所示。

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

1.4　六點相對位置測量方案

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

圖6　2點和4點相對位置測量方案

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

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

(7)

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

2　試驗結果分析

表1　3.0 km/h狀態下的單步即時性誤差　ms

圖8　三維誤差模型截面圖

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

2.1　儀器誤差對方案的影響

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

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

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

(8)

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

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

(9)

圖9　α的誤差概率

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

(10)

(11)

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

2.2　異常步態對方案的影響

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

2.3　外界環境對方案的影響

對6點相對位置測量方案而言，外界環境影響主要包括強磁和溫度，但在該方面并未對儀器做進一步預測、分析和處理。

2.4　正常步態下的綜合測量數據

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

在4 m×4 m的水平面上進行回歸原點測試，結果如圖12所示。

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

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

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

上樓造成的高度誤差相比下樓時較小，上樓單步高度誤差區間為(-0.01 m,0.01 m)，下樓單步高度誤差區間為(-0.02 m,0.02 m)。總體來說，目前方案的精確度可達到分米級別，誤差同行徑距離正相關，單步平均誤差為3 cm左右，建議同GPS進行數據融合使用。

3　結束語

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

參考文獻：

[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組合系統模型精化及載波相位定位測姿[J].測繪學報,2015,44(11):1295-1295.

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

[12] 張佳帆,陳鷹,楊燦軍.柔性外骨骼人機智能系統[M].北京：科學出版社,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] 武漢大學測繪學院測量平差學科組.誤差理論與測量平差基礎[M].武漢:武漢大學出版社,2014.

[21] 楊元喜.抗差貝葉斯估計及應用[J].測繪學報,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.