VR機艙人機交互姿態(tài)追蹤器的算法設計

，

(大連海事大學 輪機工程學院，遼寧 大連 116026)

運動載體的傾角檢測技術在眾多領域均有重要應用，如飛行器航姿、導航系統(tǒng)等，VR技術的出現使得其在人機交互的位置追蹤、姿態(tài)識別方面得到重視[1]。虛擬三維空間通過大尺寸顯示環(huán)境如弧幕或者球幕等進行顯示，人機交互的位置追蹤方式有超聲波跟蹤器、GPS跟蹤器、光學跟蹤器等[2]。以此為基礎考慮設計精度適用于虛擬現實機艙的追蹤器算法，可用于機械跟蹤器或慣性跟蹤器等穿戴嵌入式設備中。

1 加速度計數據采集

單一的加速度計的輸出在動態(tài)時精度差，在載體運動時存在動作檢測輸出延遲，但靜止時輸出不會隨時間漂移；另一方面陀螺儀精度高，但存在零點漂移[3]。借鑒卡爾曼濾波思想[4-6]，采用固定權值的方法融合加速度值與陀螺儀值，屬于前饋型數據調節(jié)系統(tǒng)。

采用16位ADC加速度檢測模塊，模塊輸出值范圍是0～655 35，Vref=5 V。由加速度計傳感器手冊查知零加速度Vzerog，將得到的三軸加速度數據轉換為電壓值。

(1)

AdcX是實時采集到的加速度計的值，由此得到電壓值，該電壓值相對于0電壓的偏移量為

OffsetX=VolX-Vzerog

(2)

引入加速度計靈敏度S，設S= 478.5 mV/g，將偏移量轉化為物理量加速度單位。

根據AccelX=OffsetX/S計算加速度[7]。

(3)

由此求出夾角,由于載體的重力方向與東北天坐標系的天地軸是平行重合的，通過測量載體坐標系各軸與重力方向的夾角以求得其與地理坐標系的角度旋轉關系，進一步獲知姿態(tài)。經過解算角速度計輸出的加速度信息，得到載體與地平面的傾角，但是根據加速度計的工作原理可知其測量是慣性力，即便是載體處于相對平穩(wěn)的狀態(tài)，它對振動和機械噪聲比較敏感。相對靜止狀態(tài)下加速度計的輸出原始數據見圖1。

圖1 單一加速度計和陀螺儀原始數據

由圖1可見，加速度計的輸出在動態(tài)時精度差，但不會隨時間漂移；而陀螺儀精度高，但存在零點漂移。因此考慮將雙方的優(yōu)點進行融合，用卡爾曼濾波思想做數據融合，通過計算加速度計和陀螺儀值得到更準確的當前載體姿態(tài)的傾角值。

2 數據融合與濾波

陀螺儀輸出的是三軸角速率，原理見圖2。

圖2 模擬陀螺儀原理示意

在表面光滑的圓盤模型上面有一個小球在A點，當小球有一個初速度V指向圓心運動時，經過一段時間t后運動到B點(可見A、B以及O在同一直線上)。當圓盤以角速率時，那么經過相同的時間t則運動到B點的前一個位置B′點，由于圓盤在轉動，因此小球離開A時，除了具有徑向速度V′外，還有切向速度V1；在B點的切向速度為V2，易知V1

如果以圓盤為參考系，當圓盤轉動時，將小球從A點以初速度V′向圓心拋去，對于圓盤上的B點來說，小球并未沿直線達到B點，而是向B點的右前方偏離，對這一結果作分析發(fā)現球具有徑向速度V′的同時，還具有垂直于這一方向向右的加速度a′。根據牛頓第二定律知，既然球出手后在該方向上沒有受到“真實力”的作用，那一定是垂直于V′向右的慣性力Fc。這種在轉動參考系中觀察到的運動物體的加速現象即科里奧利效應，Fc即科里奧利力。

以轉動參考系作為分析參考系，球從A到B′時間為

(4)

球偏離距離AB的距離為

BB′=(V1-V2)· Δt′=

ω·(OA-OB)·Δt′=ω·V′Δt′2

(5)

當Δt′很小時，可以認為沿BB′的運動時勻加速運動而初速度為0，則

(6)

由公式(6)反解出a′，該結果是在Δt′很小時得到的。真實的設計是在陀螺儀內部的1個以一定頻率做水平震蕩的質量塊，當該質量塊水平振蕩時的頻率和位移唯一確定時，此時該質量塊的速度可以確定；當陀螺儀旋轉時，垂直于該質量塊的水平方向會有1個科里奧利力的作用，在該力的作用下會有1個加速度，結合加速度計的工作原理求出該加速度，根據a′=2ωV′求出陀螺儀旋轉時的角速率。

設已經獲取的加速度計輸出慣性矢量為

Raccel=[Rxaccel,Ryaccel,Rzaccel]

(7)

式中：Rxaccel、Ryaccel、Rzaccel表示加速度向量在載體坐標系空間內的分量。這里若初始化加速度傳感器配置慣性矢量的長度不是1g，需要對矢量進行標準化處理。

(8)

式中：n為當前時刻。

參考卡爾曼濾波思想[8]，引入數據融合后估算的輸出值向量Rout。

Rout=[Rxout,Ryout,Rzout]

(9)

數列初始化時，認為加速度值正確并將加速度的值對應賦值給輸出值，即

Rout(0)=Raccel(0)

(10)

Rout(0)為初始的輸出估算值。

在嵌入式主控單元內部有時間片Tick，每經過Tick時間做一次測量值接收,于是經過nTick后，便有了前一時刻的Rout(n-1)估算值，初始值Rout(0)=Raccel(0)以及當前時刻加速度的測量值Raccel(n)，前述用陀螺儀數據修正加速度值，因此在估算當前時刻的最佳值Rout(n)之前，引入由陀螺儀當前值和前一個估算值得到的修正中間變量Rgyro。

Rgyro=[Rxgyro,Rygyro,Rzgyro]

(11)

(12)

以其中1個分量Rxgyro為例，見圖3。

圖3 空間加速度向量分解

φ即為橫滾角，θ即為俯仰角。在Rzaccel和Rxz組成的直角三角形中，由tanφ=Rxaccel/Rzaccel反解夾角φ=arctan(Rxaccel/Rzaccel)，根據已經得到的(n-1)時刻的Rout(n-1)，可以得到

(13)

在空間變換后新的角度：

φ(n)=φ(n-1) +Rateφ(n) ×T

(14)

Rateφ表示陀螺儀讀取到的橫滾角的角速率，為了使數值更準確可以采用平均值法或者提高采用頻率，這里采用平均值法

(15)

因此：

φ(n)=φ(n-1)+RateφAvg×T

(16)

θ(n)=θ(n-1) +RateθAvg×T

(17)

由于

(18)

為了簡化運算，替換運算符號，以方便在主控單元中計算，優(yōu)化程序。

Raccel(n)：加速度計讀取的當前值；Rgyro(n)：根據 (n-1)和陀螺儀當前值所得。

當前時刻的角度Rout(n)信息更新由以上2項共同完成，Rout(n)信息更新采用固定權值—加權平均值的方法。

(19)

wgyro是信賴程度，可以看出該出使用的權值不同于卡爾曼濾波權重隨著加速度計的噪聲而改變，這里采用固定權重的方法足夠使用，其值的確定在軟件設計部分，由實驗產生。Rout(n)為

(20)

將Rout進行標準化處理。

(21)

得到Rxout后，進行下一輪ADC數據采集與融合循環(huán)。算法流程見圖4。

圖4 傾角更新算法流程

3 算法仿真

固定權重需要根據實驗進行調節(jié)，采用二分法在[0,1]調節(jié)權重。

采用Matlab仿真的方式，將設計的算法編寫成m函數，取值范圍為[0,360]。模擬載體坐標系繞原點位置旋轉一圈，將其做生成單軸折線圖查看噪聲與濾波后情況，見圖5。最下方曲線表示高斯白噪聲干擾，范圍為(1，360)之間的隨機數，這里權重wgyro初始化為0.25，曲線顯示其中的一個傾角偏航角。

圖5 算法仿真圖形化顯示

通過分析步伐姿態(tài)識別在機艙虛擬現實系統(tǒng)的應用可行性。按照算法解算出來的數據在數據調試端口看不出任何差別，將其做成折線圖5實時顯示則可以看出：(1)在最開始一段時間時載體是靜止的，實時顯示的折線圖抖動較大原因是此時算法的作用并不明顯，正如前文敘述的此時認為加速度計的輸出是可靠的并作為估算值輸出，陀螺儀對加速度值的平滑處理作用還未完全開始。(2)隨著時間和接收數據的增多，將載體原位置旋轉，可以看出其偏航角實時平滑變化。對比完整的卡爾曼濾波變權重的方法，計算出當前的姿態(tài)估算值，可以看出本文設計的固定權重wgyro在嵌入式設備中比較合適。

4 結論

本算法可直接用于機械師跟蹤器、慣性跟蹤器的下位機姿態(tài)采集穿戴設備中，尤其適用于嵌入式系統(tǒng)等處理器性能不足的低成本設備中，算法的不足之處在于權重不便于調節(jié)，需要根據傳感器的安裝位置、初始坐標系設定進行確定，但運行足夠穩(wěn)定，可以做到低功耗實時獲取當前姿態(tài)傾角狀態(tài)。