，

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

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

1 加速度計(jì)數(shù)據(jù)采集

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

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

(1)

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

OffsetX=VolX-Vzerog

(2)

引入加速度計(jì)靈敏度S，設(shè)S= 478.5 mV/g，將偏移量轉(zhuǎn)化為物理量加速度單位。

根據(jù)AccelX=OffsetX/S計(jì)算加速度[7]。

(3)

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

圖1 單一加速度計(jì)和陀螺儀原始數(shù)據(jù)

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

2 數(shù)據(jù)融合與濾波

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

圖2 模擬陀螺儀原理示意

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

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

以轉(zhuǎn)動(dòng)參考系作為分析參考系，球從A到B′時(shí)間為

(4)

球偏離距離AB的距離為

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

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

(5)

當(dāng)Δt′很小時(shí)，可以認(rèn)為沿BB′的運(yùn)動(dòng)時(shí)勻加速運(yùn)動(dòng)而初速度為0，則

(6)

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

設(shè)已經(jīng)獲取的加速度計(jì)輸出慣性矢量為

Raccel=[Rxaccel,Ryaccel,Rzaccel]

(7)

式中：Rxaccel、Ryaccel、Rzaccel表示加速度向量在載體坐標(biāo)系空間內(nèi)的分量。這里若初始化加速度傳感器配置慣性矢量的長(zhǎng)度不是1g，需要對(duì)矢量進(jìn)行標(biāo)準(zhǔn)化處理。

(8)

式中：n為當(dāng)前時(shí)刻。

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

Rout=[Rxout,Ryout,Rzout]

(9)

數(shù)列初始化時(shí)，認(rèn)為加速度值正確并將加速度的值對(duì)應(yīng)賦值給輸出值，即

Rout(0)=Raccel(0)

(10)

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

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

Rgyro=[Rxgyro,Rygyro,Rzgyro]

(11)

(12)

以其中1個(gè)分量Rxgyro為例，見圖3。

圖3 空間加速度向量分解

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

(13)

在空間變換后新的角度：

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

(14)

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

(15)

因此：

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

(16)

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

(17)

由于

(18)

為了簡(jiǎn)化運(yùn)算，替換運(yùn)算符號(hào)，以方便在主控單元中計(jì)算，優(yōu)化程序。

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

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

(19)

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

(20)

將Rout進(jìn)行標(biāo)準(zhǔn)化處理。

(21)

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

圖4 傾角更新算法流程

3 算法仿真

固定權(quán)重需要根據(jù)實(shí)驗(yàn)進(jìn)行調(diào)節(jié)，采用二分法在[0,1]調(diào)節(jié)權(quán)重。

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

圖5 算法仿真圖形化顯示

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

4 結(jié)論

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