基于6自由度平臺(tái)的體感模擬算法研究

王軍政，蔣鑠，汪首坤，沈偉

(1.北京理工大學(xué) 自動(dòng)化學(xué)院，復(fù)雜系統(tǒng)智能控制與決策國(guó)家重點(diǎn)實(shí)驗(yàn)室，北京 100081； 2.北京理工大學(xué) 自動(dòng)化學(xué)院，伺服運(yùn)動(dòng)系統(tǒng)驅(qū)動(dòng)與控制工信部重點(diǎn)實(shí)驗(yàn)室，北京 100081)

提 要： 針對(duì)模擬駕駛器對(duì)動(dòng)感逼真度和平臺(tái)工作空間的需求，將基于洗出算法的體感模擬控制策略應(yīng)用于6自由度并聯(lián)式運(yùn)動(dòng)模擬器.首先對(duì)系統(tǒng)進(jìn)行了整體設(shè)計(jì)及數(shù)學(xué)建模，然后通過(guò)分析人體感知機(jī)理，提出了基于加速度控制的體感模擬洗出算法；隨后具體研究了模擬駕駛過(guò)程中可能存在的多種復(fù)雜工況及對(duì)應(yīng)的加速度控制模式；最后，通過(guò)聯(lián)合仿真與實(shí)驗(yàn)驗(yàn)證了所提出控制算法的可行性及控制效果，結(jié)果顯示平臺(tái)在有限運(yùn)動(dòng)范圍內(nèi)動(dòng)感逼真度得到了顯著提升.

運(yùn)動(dòng)模擬器在航天、航海及車輛駕駛等領(lǐng)域中得到了廣泛應(yīng)用[1]，早期的運(yùn)動(dòng)模擬器工作空間小、承載能力低、驅(qū)動(dòng)方式多為液壓式且缺乏能夠模擬復(fù)雜環(huán)境的視景系統(tǒng)，隨著近年來(lái)虛擬現(xiàn)實(shí)技術(shù)的興起，運(yùn)動(dòng)模擬器可用于模擬駕駛系統(tǒng)研發(fā)、作戰(zhàn)飛行員輔助訓(xùn)練、促進(jìn)VR動(dòng)感娛樂(lè)裝置及汽車測(cè)試樣機(jī)等產(chǎn)品開發(fā)[2]. 除對(duì)系統(tǒng)控制精度、平臺(tái)負(fù)載能力及結(jié)構(gòu)穩(wěn)定性的要求外，模擬車輛駕駛過(guò)程中能否給與操作者真實(shí)的動(dòng)感逼真度及不同工況下模擬駕駛運(yùn)動(dòng)平臺(tái)的控制性能成為了運(yùn)動(dòng)模擬器研究領(lǐng)域中的熱點(diǎn)[3-5].

體感模擬洗出算法作為運(yùn)動(dòng)模擬控制的核心技術(shù)之一，有效地提升了并聯(lián)式平臺(tái)運(yùn)動(dòng)過(guò)程中的動(dòng)感模擬逼真度. 上世紀(jì)90年代，KIM等[6]采用簡(jiǎn)單的洗出濾波器針對(duì)Stewart平臺(tái)進(jìn)行有限范圍內(nèi)的運(yùn)動(dòng)模擬，但無(wú)法有效量化評(píng)估人體感知并進(jìn)行參數(shù)設(shè)計(jì)，本世紀(jì)初BARBAGLI等[7]將經(jīng)典洗出算法應(yīng)用于電動(dòng)車模擬系統(tǒng)的研發(fā)，通過(guò)不斷調(diào)整和優(yōu)化濾波器參數(shù)打破了并聯(lián)式平臺(tái)工作空間的限制，實(shí)現(xiàn)了一定程度上的動(dòng)感模擬，但在非極限工況下系統(tǒng)響應(yīng)還不夠明顯. 近年來(lái)，BEGHI等[8]設(shè)計(jì)出用于航空飛行器的體感洗出算法，基于模型預(yù)測(cè)控制的方式，在線進(jìn)行濾波器及控制器參數(shù)預(yù)測(cè)試圖實(shí)現(xiàn)復(fù)雜工況下的體感模擬，但可變參數(shù)較多，無(wú)法保證通訊實(shí)時(shí)性，仍欠缺系統(tǒng)可靠性.

為解決以上問(wèn)題，設(shè)計(jì)了一款Stewart型6自由度動(dòng)感模擬器，創(chuàng)新采用改進(jìn)形式的體感模擬洗出算法. 搭建Linux交叉編譯環(huán)境并建立5G路由器基站，將Unity3D城市道路視景系統(tǒng)中的路況信息轉(zhuǎn)化為搖擺臺(tái)的6自由度運(yùn)動(dòng)指令進(jìn)而驗(yàn)證算法的有效性. 實(shí)驗(yàn)結(jié)果表明，平臺(tái)在有限工作空間的運(yùn)動(dòng)提升了操作者的動(dòng)感體驗(yàn).

1 系統(tǒng)組成與原理

本文研究的動(dòng)感模擬駕駛器實(shí)驗(yàn)樣機(jī)如圖1所示，由并聯(lián)式6自由度平臺(tái)、座椅、視景系統(tǒng)、嵌入式主控制器、PMSM電機(jī)驅(qū)動(dòng)器、通訊基站、傳感器測(cè)量裝置以及慣性負(fù)載組成.

6自由度平臺(tái)是結(jié)構(gòu)基礎(chǔ)，其主要執(zhí)行機(jī)構(gòu)為6根力位復(fù)合一體化電動(dòng)缸. 電動(dòng)缸通過(guò)十字萬(wàn)向節(jié)鉸接上下平臺(tái)，內(nèi)嵌PMSM電機(jī)、增量式光電編碼器、應(yīng)變片式拉壓力傳感器等，將伺服電機(jī)轉(zhuǎn)子與傳動(dòng)絲桿一體化，在實(shí)現(xiàn)力位復(fù)合控制的同時(shí)可滿足疲勞試驗(yàn)要求，提高了傳動(dòng)效率及可靠性.

主控選用Linux多任務(wù)交互式實(shí)時(shí)操作系統(tǒng)實(shí)現(xiàn)交叉編譯的同時(shí)提升了平臺(tái)的動(dòng)態(tài)響應(yīng)速度，利用5G路由器基站可提升視景系統(tǒng)、手機(jī)移動(dòng)終端、

圖1 動(dòng)感模擬器樣機(jī)Fig.1 Dynamic simulator prototype

上位機(jī)與主控間的數(shù)據(jù)傳輸效率. 編碼器脈沖經(jīng)差分輸入、光耦隔離、加減計(jì)數(shù)將位移信息送入主控；力傳感器經(jīng)標(biāo)定校準(zhǔn)、放大調(diào)理將力信號(hào)送入主控，主控利用SPI串行模塊結(jié)合DA向驅(qū)動(dòng)器發(fā)送指令驅(qū)使電動(dòng)缸伸縮. 視景系統(tǒng)在Unity3D2019環(huán)境下配合C#腳本語(yǔ)言完成場(chǎng)景繪制、車輛模型搭建，多傳感器數(shù)據(jù)的采集、濾波與融合，通過(guò)UDP協(xié)議以固定格式指令傳輸虛擬運(yùn)動(dòng)體的姿態(tài)信息給主控，進(jìn)而實(shí)現(xiàn)與6自由度平臺(tái)的聯(lián)動(dòng).

2 系統(tǒng)建模

2.1 一體化電動(dòng)缸傳動(dòng)環(huán)節(jié)

PMSM電機(jī)作為一體化電動(dòng)缸的重要傳動(dòng)環(huán)節(jié)，由定子、轉(zhuǎn)子、電樞鐵芯等構(gòu)成，魯棒性強(qiáng)，具有較硬機(jī)械特性，建立永磁同步電機(jī)在d-q軸的方程如下

(1)

式中：ud和uq分別為定子d、q軸定子電壓；id和iq分別為d、q軸的定子電流；Ld和Lq分別為定子繞組自感；P為微分算子；Ra為定子繞組電阻；φf(shuō)為定子磁鏈幅值；ωn為磁場(chǎng)旋轉(zhuǎn)角速度. 根據(jù)系統(tǒng)運(yùn)動(dòng)特性有

(2)

式中：pn為極對(duì)數(shù)；J為系統(tǒng)的轉(zhuǎn)動(dòng)慣量；Te為電磁轉(zhuǎn)矩；TL為負(fù)載轉(zhuǎn)矩；ωr m為電機(jī)輸出軸機(jī)械角速度.id=0時(shí)，可聯(lián)立各式得電機(jī)簡(jiǎn)化數(shù)學(xué)傳函[9]

(3)

式中：KT為伺服電機(jī)轉(zhuǎn)矩系數(shù)；Teq為交叉角頻率的倒數(shù)；G(s)為二階滯后環(huán)節(jié)，由于電機(jī)內(nèi)部結(jié)構(gòu)復(fù)雜及負(fù)載受諸多干擾因素等原因需利用參數(shù)辨識(shí)法估計(jì).

滾珠絲杠啟動(dòng)力矩小，無(wú)爬行，能實(shí)現(xiàn)精確微給進(jìn)，若僅考慮黏性摩擦，與電機(jī)間聯(lián)軸器視為剛性連接，不考慮回程間隙，根據(jù)文獻(xiàn)[10]，可建立傳動(dòng)方程如下

(4)

式中：Jm為總轉(zhuǎn)動(dòng)慣量；cb，ct分別為旋轉(zhuǎn)部件與軸向黏性摩擦系數(shù)；ls為絲杠導(dǎo)程；K為剛度系數(shù)；xl為缸桿位移；θm為絲杠轉(zhuǎn)角.

2.2 6自由度平臺(tái)數(shù)學(xué)建模

圖2 6自由度平臺(tái)模型Fig.2 Six-degree-of-freedom platform

T=T(x,θx)Τ(y,θy)T(z,θz)

(5)

(6)

式中Lmin為電動(dòng)缸伸出中位時(shí)的長(zhǎng)度[11].

體坐標(biāo)系下用角速度ω描述姿態(tài)變化，在慣性坐標(biāo)系下則用歐拉角變化率描述，需將體坐標(biāo)系下運(yùn)載工具角速度信號(hào)轉(zhuǎn)變?yōu)閼T性坐標(biāo)系下歐拉角變化率，即

(7)

為進(jìn)行6自由度位置正解以實(shí)時(shí)計(jì)算平臺(tái)位姿，采取Levenberg-Marquardt法求解非線性方程組F(x)=0

F(x)=f(θx,θy,θz,x,y,z,ui)-li，i=1,2,…6

(8)

式中：ui為驅(qū)動(dòng)器控制量；li為初始缸長(zhǎng).

首先取u0=‖F(xiàn)(x0)‖,x0為初始迭代點(diǎn)，按下式求解搜索方向d

(JTJ+u0I)d=-JTF

(9)

式中J為雅可比矩陣，通過(guò)泰勒展開獲得.

隨后，采用0.618一維搜索法搜索最優(yōu)迭代步長(zhǎng)λ，求取φ(λ)局部極小值,其中

φ(λ)=F(x0+λd)

(10)

0.618黃金分割法計(jì)算過(guò)程中可實(shí)時(shí)更新步長(zhǎng)，Levenberg-Marquardt法可有效改善Gauss-Newton迭代法對(duì)雅可比矩陣列滿秩的限制條件，且能夠提高計(jì)算效率. 最后計(jì)算增量并更新x0的值，并按特定規(guī)律更新u0進(jìn)行迭代，可得平臺(tái)6自由度參數(shù).

3 人體感知器

人體感知系統(tǒng)主要由視覺、前庭系統(tǒng)和觸覺感受器組成. 前庭器官包括半規(guī)管與耳石器，其中半規(guī)管用來(lái)感知角速度變化，例如車輛前傾與轉(zhuǎn)彎，耳石器用來(lái)感知加速度，例如車輛啟動(dòng)、制動(dòng)及在崎嶇路面上的顛簸，耳石器對(duì)加速度感覺以比力形式呈現(xiàn)，即線加速度與重力加速度的矢量和. 加速度與角速度低于某閾值時(shí)人體不會(huì)察覺到運(yùn)動(dòng)，利用這一特點(diǎn)可設(shè)計(jì)運(yùn)動(dòng)模擬器回歸中位而不被模擬器駕駛員察覺的方式[12].

耳石與半規(guī)管數(shù)學(xué)模型對(duì)人體感知進(jìn)行量化，有助于為體感模擬算法相關(guān)參數(shù)的設(shè)計(jì)提供參考，耳石器傳遞函數(shù)表達(dá)式如下

(11)

表1 耳石模型參數(shù)Tab.1 Otolith model parameters

半規(guī)管傳遞函數(shù)表達(dá)式如下

(12)

表2 半規(guī)管模型參數(shù)Tab.2 Parameters of semicircular canal model

4 體感模擬洗出算法

基于加速度控制的體感模擬洗出算法控制系統(tǒng)原理如圖3所示，圖中f為視景系統(tǒng)慣性坐標(biāo)系下的三軸比力輸入，ω為實(shí)際駕駛器的角速度輸入，θin為模擬駕駛路面傾角，T為歐拉角旋轉(zhuǎn)矩陣，L為角速度變換矩陣，F(xiàn)HP1,2為二階高通濾波器，F(xiàn)LP1為二階低通濾波器，G(s)為加速度控制器，其確定了加速度與目標(biāo)位移之間的聯(lián)系. 洗出算法的作用是利用搖擺臺(tái)有限的運(yùn)動(dòng)工作空間還原真實(shí)運(yùn)動(dòng)過(guò)程中的動(dòng)感，系統(tǒng)最終輸出為6自由度平臺(tái)的位移l與角度θ.

圖3 體感模擬算法控制原理Fig.3 Control principle of somatosensory simulation algorithm

4.1 輸入信號(hào)縮比

首先需對(duì)輸入的模擬駕駛系統(tǒng)加速度和角速度信號(hào)進(jìn)行縮比以調(diào)節(jié)平移量，采用非線性縮比策略可保證整個(gè)區(qū)域信號(hào)連續(xù)性的同時(shí)，對(duì)不同幅值段信號(hào)實(shí)現(xiàn)不同幅度縮放，美國(guó)NASA蘭利探究中心的航天飛行模擬器也曾采用這種方法，通常為三階多項(xiàng)式法，其形式為

y=a3x3+a2x2+a1x+a0

(13)

式中：x為輸入信號(hào)；y為輸出信號(hào)；a0～a3為各階次系數(shù). 非線性縮比策略的作用是防止6自由度平臺(tái)超出運(yùn)動(dòng)范圍，隨后將縮比信號(hào)經(jīng)矩陣變換到體坐標(biāo)系下.

4.2 加速度分解策略

當(dāng)操作者模擬駕駛于平滑路面或崎嶇不平道路，需分別設(shè)計(jì)對(duì)應(yīng)的控制策略，因此要將體坐標(biāo)系中加速度信號(hào)進(jìn)行高低頻分解. 在振動(dòng)工況及角速度過(guò)濾環(huán)節(jié)選用二階高通濾波器，表達(dá)式如下

(14)

式中：ξ為阻尼系數(shù)；ωs為震蕩頻率；k′為常數(shù)量綱一. 過(guò)濾出的高頻信息輸入到加速度控制器中. 在模擬平滑路面行駛情況時(shí)，加速度呈現(xiàn)低頻特性，利用二階低通濾波環(huán)節(jié)將其濾出，表達(dá)式如下

(15)

4.3 傾斜補(bǔ)償策略

模擬持續(xù)加速、減速及連續(xù)轉(zhuǎn)彎過(guò)程時(shí)，需將平臺(tái)沿各自軸向傾斜角度以獲取重力加速度分量. 比力信號(hào)經(jīng)低通濾波得到的加速度低頻穩(wěn)態(tài)分量與重力的矢量夾角可補(bǔ)償人體耳石感受到的持續(xù)加速度. 補(bǔ)償傾角為

(16)

(17)

4.4 加速度控制器實(shí)現(xiàn)

崎嶇不平路面駕駛過(guò)程中加速度信號(hào)呈現(xiàn)高頻抖動(dòng)，6自由度平臺(tái)垂直向等效為彈簧阻尼器，列出線性時(shí)不變單自由度振動(dòng)運(yùn)動(dòng)力平衡方程

(18)

式中：m為慣性負(fù)載質(zhì)量；b為阻尼系數(shù)；k為彈簧常數(shù)；ag為垂直向加速度. 電動(dòng)缸阻尼力與彈性恢復(fù)力與其相對(duì)速度和位移有關(guān)[13].

在墜落撞擊的過(guò)程中，等效為單位脈沖激振力作用，表現(xiàn)出寬頻帶特性，其通解為

式中：ω0為系統(tǒng)固有頻率；ε為臨界阻尼比.

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

為驗(yàn)證所提出控制算法的可行性及控制效果，在Solidworks、Adams中依據(jù)實(shí)驗(yàn)樣機(jī)建立圖4所示的機(jī)械模型并進(jìn)行動(dòng)力學(xué)分析，結(jié)合Matlab實(shí)現(xiàn)體感模擬算法，進(jìn)行聯(lián)合仿真，并在圖1所示的實(shí)驗(yàn)樣機(jī)中進(jìn)行測(cè)試. 機(jī)械模型如圖4所示，系統(tǒng)參數(shù)如表3所示.

圖4 動(dòng)感模擬器機(jī)械模型Fig.4 Mechanical model of motion simulator

表3 平臺(tái)結(jié)構(gòu)參數(shù)Tab.3 Parameters of platform’s structure

圖5為操作者模擬車輛連續(xù)快速地經(jīng)過(guò)5組10°的標(biāo)準(zhǔn)化斜坡，給定為俯仰向周期性正弦運(yùn)動(dòng)，利用上位機(jī)采集驅(qū)動(dòng)器控制量與電動(dòng)缸伸縮速度進(jìn)而得出實(shí)時(shí)姿態(tài)，通過(guò)前饋PID控制以提升電動(dòng)缸的伺服控制精度. 圖5為運(yùn)動(dòng)時(shí)的動(dòng)態(tài)跟蹤曲線，軌跡方程為

θy(t)=10.0sin(4πt)

(20)

圖5 斜坡工況跟隨測(cè)試Fig.5 Slope condition following test

通過(guò)最小二乘參數(shù)辨識(shí)法近似得出電動(dòng)缸傳動(dòng)環(huán)節(jié)傳遞函數(shù)為Gc(s),表達(dá)式為

(21)

圖6為模擬崎嶇不平路面振動(dòng)工況的6自由度平臺(tái)位移與單缸速度曲線. 改變加速度高通通道二階震蕩頻率ωs，對(duì)比ωs為0.5，1.0，2.5時(shí)的實(shí)驗(yàn)結(jié)果，可以看出，當(dāng)ωs較小時(shí)，6自由度搖擺臺(tái)超出限制，但運(yùn)動(dòng)速度過(guò)快，容易引發(fā)電動(dòng)缸失控，如圖7所示. 經(jīng)反復(fù)調(diào)試，ωs取2.5時(shí)，隨著ωs的逐漸增大，平臺(tái)能夠?qū)崿F(xiàn)有限運(yùn)動(dòng)范圍內(nèi)振動(dòng)模擬，且能夠在模擬器運(yùn)動(dòng)結(jié)束后回歸0位.

圖6 崎嶇路面振動(dòng)工況測(cè)試Fig.6 Vibration test of rough road

圖7 振動(dòng)工況下電動(dòng)缸速度Fig.7 Electric cylinder’s speed under vibration condition

圖8為傾斜補(bǔ)償實(shí)驗(yàn)測(cè)試曲線，在視景環(huán)境下模擬貨車的啟動(dòng)過(guò)程，最大加速度約為2 m/s2,從實(shí)驗(yàn)曲線看出，在第4 s時(shí)，加速度達(dá)到最大，搖擺臺(tái)做出俯仰向后傾約12°的傾角，符合預(yù)期，而未采用傾斜補(bǔ)償?shù)母┭鼋莾H來(lái)自于模擬駕駛器自身的傾角，無(wú)法還原真實(shí)環(huán)境下的動(dòng)感. 在10 s時(shí)，速度達(dá)到最大，駕駛器進(jìn)入勻速運(yùn)動(dòng)階段，傾角歸0.

圖8 傾斜補(bǔ)償測(cè)試Fig.8 Tilt compensation test condition

6 結(jié) 論

為滿足運(yùn)動(dòng)模擬器在有限運(yùn)動(dòng)空間下盡可能真實(shí)的還原駕駛操作者在模擬駕駛過(guò)程中的動(dòng)感模擬需求，搭建了基于一體化電動(dòng)缸執(zhí)行單元的6自由度運(yùn)動(dòng)模擬器實(shí)驗(yàn)平臺(tái)，并設(shè)計(jì)了基于人體感知器的體感模擬洗出算法，對(duì)視景環(huán)境下的振動(dòng)工況進(jìn)行了策略分析，針對(duì)車輛啟動(dòng)加速工況采取了傾斜補(bǔ)償策略，通過(guò)聯(lián)合仿真與對(duì)比實(shí)驗(yàn)，驗(yàn)證了系統(tǒng)的可行性及體感模擬控制效果. 下一步工作將深入研究復(fù)雜工況下的并聯(lián)式穩(wěn)定平臺(tái)伺服控制方法.