張?zhí)欤?靳舒馨， 王強(qiáng)， 段曉波， 劉鐵成， 牛海濤， 侯曾， 楊毅， 劉彤

(1.中國(guó)兵器工業(yè)計(jì)算機(jī)應(yīng)用技術(shù)研究所， 北京 100089； 2.北京衛(wèi)星導(dǎo)航中心， 北京 100094； 3.北京理工大學(xué) 自動(dòng)化學(xué)院， 北京 100081)

0 引言

隨著現(xiàn)代軍事裝備的智能化升級(jí)，武器裝備對(duì)感知認(rèn)知與智能控制提出了更高需求[1-2]。隨動(dòng)裝備作為一種典型的智能化裝備，通過(guò)多種技術(shù)手段把受控對(duì)象與操控人員的肢體動(dòng)作直接結(jié)合，實(shí)現(xiàn)自然的人機(jī)交互方式，提升作戰(zhàn)效率[3]。在隨動(dòng)控制裝備中，人員頭部運(yùn)動(dòng)由慣性傳感器、光電系統(tǒng)采集，經(jīng)融合解算得到人員頭部相對(duì)載具艙體的位置姿態(tài)變化量，生成控制信息[4]。該控制信息經(jīng)載具控制總線傳遞給武器站、觀察鏡、雷達(dá)等執(zhí)行設(shè)備，控制其跟隨載具乘員頭部實(shí)時(shí)準(zhǔn)確運(yùn)動(dòng)[5]。隨動(dòng)控制系統(tǒng)的低延遲與高精度需求使得隨動(dòng)裝備對(duì)信息采集到控制執(zhí)行的實(shí)時(shí)性、準(zhǔn)確性與穩(wěn)定性有極強(qiáng)依賴[6]。

在大部分機(jī)電系統(tǒng)的設(shè)計(jì)中，因機(jī)電常數(shù)相對(duì)固定，精度指標(biāo)與實(shí)時(shí)性指標(biāo)通常難以兼顧，在機(jī)電系統(tǒng)對(duì)精度需求較高時(shí)，采取延長(zhǎng)響應(yīng)時(shí)間換取控制精度的方案；在實(shí)時(shí)性需求更高時(shí)，采用犧牲精度以縮短響應(yīng)時(shí)間的方案[7-8]。不同的控制策略需要針對(duì)應(yīng)用場(chǎng)景進(jìn)行綜合考慮。在隨動(dòng)控制系統(tǒng)中，隨動(dòng)精度與隨動(dòng)時(shí)延均為關(guān)鍵性指標(biāo)，二者共同決定了隨動(dòng)控制裝備的人機(jī)交互體驗(yàn)，需要機(jī)電系統(tǒng)即能滿足實(shí)時(shí)性需求，又能滿足精度指標(biāo)。因此兼顧實(shí)時(shí)性與準(zhǔn)確性的系統(tǒng)方案也成為了隨動(dòng)系統(tǒng)的設(shè)計(jì)難點(diǎn)。

目前低成本的隨動(dòng)控制方案通常采用微慣性器件[9-10]與光電系統(tǒng)配合完成測(cè)量，并將機(jī)電設(shè)備作為控制的最終執(zhí)行器。對(duì)該系統(tǒng)而言，隨動(dòng)控制精度受到姿態(tài)采集精度與機(jī)電執(zhí)行器精度的共同影響，隨動(dòng)控制的實(shí)時(shí)性受到姿態(tài)采集與解算延遲、機(jī)電控制與響應(yīng)延遲的共同影響，需要進(jìn)行針對(duì)性改進(jìn)。

本文選取微慣性傳感器與小型云臺(tái)構(gòu)成的隨動(dòng)系統(tǒng)作為研究對(duì)象，分別對(duì)實(shí)時(shí)姿態(tài)采集算法與云臺(tái)控制方案進(jìn)行設(shè)計(jì)，提升隨動(dòng)系統(tǒng)實(shí)時(shí)性與準(zhǔn)確性。在姿態(tài)解算方面，本文選用低成本小型六軸微機(jī)電系統(tǒng)慣性測(cè)量單元(MEMS IMU)作為傳感單元，通過(guò)姿態(tài)解算與互補(bǔ)濾波獲取實(shí)時(shí)準(zhǔn)確的姿態(tài)信息。在云臺(tái)控制方面，本文使用模糊控制PID高頻位置閉環(huán)算法改善現(xiàn)有雙軸云臺(tái)的機(jī)電控制系統(tǒng)，兼顧非線性輸入下機(jī)電響應(yīng)的實(shí)時(shí)性與準(zhǔn)確性[11-12]。針對(duì)體感高動(dòng)態(tài)隨動(dòng)系統(tǒng)的準(zhǔn)確性與實(shí)時(shí)性指標(biāo)，研究設(shè)計(jì)了姿態(tài)測(cè)量精度實(shí)驗(yàn)、頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)與位置隨動(dòng)響應(yīng)實(shí)驗(yàn)，進(jìn)行了驗(yàn)證分析。

1 隨動(dòng)信息獲取

以微慣性傳感器為基礎(chǔ)的微慣性測(cè)量技術(shù)是一種有效的低成本姿態(tài)測(cè)量手段。在姿態(tài)測(cè)量中，MEMS IMU輸出為載體坐標(biāo)系3個(gè)軸向的加速度(比力)與角速度信息[13]。需要通過(guò)姿態(tài)算法，根據(jù)上一時(shí)刻姿態(tài)信息與三軸陀螺儀輸出，更新載體在導(dǎo)航坐標(biāo)系下的姿態(tài)信息，完成隨動(dòng)信息獲取。同時(shí)，考慮到慣性積分的累積誤差，需要通過(guò)其他觀測(cè)手段修正測(cè)量結(jié)果，提升姿態(tài)測(cè)量精度。

1.1 隨動(dòng)信息采集

考慮到傳感器增重對(duì)人體頭部造成的不適感，需要在合理體積質(zhì)量下選型精度較高的MEMS慣性器件。綜合比較下，本文選用六軸微機(jī)電慣性測(cè)量單元ADIS16475-2作為姿態(tài)測(cè)量傳感器。該傳感器常溫漂移量小于5°/h、量程大于300°/s，接近戰(zhàn)術(shù)級(jí)精度，且體積小、質(zhì)量輕、功耗低，通過(guò)工裝固定在頭部不會(huì)產(chǎn)生不適感[14]。ADIS16475對(duì)外數(shù)據(jù)接口為SPI總線，最高采樣頻率為2 kHz，具備同步觸發(fā)功能，可實(shí)現(xiàn)慣性數(shù)據(jù)的高頻實(shí)時(shí)采集。使用DSP28335完成數(shù)據(jù)采集與解算，并通過(guò)串口實(shí)時(shí)發(fā)送至機(jī)電系統(tǒng)，如圖1所示。

圖1 搭載ADIS16475的DSP28335最小系統(tǒng)Fig.1 DSP28335 minimum system with ADIS16475

1.2 實(shí)時(shí)姿態(tài)更新

姿態(tài)解算將IMU的三軸角速度信息解算成頭部姿態(tài)信息。推導(dǎo)姿態(tài)更新公式，三維空間中任意3個(gè)坐標(biāo)系的角位置關(guān)系可通過(guò)3次姿態(tài)角基本旋轉(zhuǎn)(歐拉旋轉(zhuǎn))或1次四元數(shù)旋轉(zhuǎn)實(shí)現(xiàn)對(duì)齊，兩種旋轉(zhuǎn)方式的轉(zhuǎn)換矩陣存在等價(jià)關(guān)系[15](見(jiàn)圖2)。圖2中，Oxnynzn為導(dǎo)航坐標(biāo)系(東北天)，Oxbybzb載體坐標(biāo)系中，yb軸向載體正前方，xb軸水平向右，zb軸與xb軸、yb軸呈右手螺旋準(zhǔn)則，以滾動(dòng)角γ、俯仰角θ、偏航角ψ表示3個(gè)姿態(tài)角。

圖2 坐標(biāo)變換矩陣的3次變換Fig.2 Three transformations of the coordinate transformation matrix

(1)

基于四元數(shù)的坐標(biāo)變換矩陣

(2)

式中：q0、q1、q2、q3為旋轉(zhuǎn)四元數(shù)。

比照式(1)、式(2)可以求解姿態(tài)角

(3)

為實(shí)現(xiàn)四元數(shù)實(shí)時(shí)更新，還需要建立四元數(shù)微分方程：

(4)

(5)

式中：ωx、ωy、ωz分別為IMU角速度測(cè)量值。由于MEMS IMU精度限制，本文中可近似忽略位置速率和地球自轉(zhuǎn)速率對(duì)姿態(tài)角解算的影響。

由式(5)可見(jiàn)，在算法實(shí)際使用中，只要初始四元數(shù)已知，便可根據(jù)捷聯(lián)慣導(dǎo)陀螺輸出(采樣間隔已知)實(shí)時(shí)更新四元數(shù)。

(6)

式中：k為離散采樣序號(hào)；Δt為采樣時(shí)間間隔。

更新四元數(shù)后，再通過(guò)式(3)求解姿態(tài)角，完成姿態(tài)解算。

1.3 互補(bǔ)濾波

由于陀螺儀誤差會(huì)隨著時(shí)間的增加不斷積累，而加速度計(jì)不會(huì)隨著時(shí)間的增加產(chǎn)生誤差積累，尤其在低頻狀態(tài)測(cè)傾角會(huì)有理想的效果。利用這兩種傳感器在頻域上的互補(bǔ)特性，設(shè)計(jì)互補(bǔ)濾波器融合兩種傳感器的數(shù)據(jù)，可以達(dá)到提高測(cè)量精度和系統(tǒng)的動(dòng)態(tài)的目的。算法具體步驟實(shí)現(xiàn)如下：

1)將測(cè)量得到的加速度歸一化，即

(7)

式中：ax、ay、az為IMU測(cè)量的加速度三軸分量。

2)通過(guò)姿態(tài)矩陣求解重力加速度由導(dǎo)航坐標(biāo)系投影到載體坐標(biāo)系的分量：

(8)

(9)

式中：δax、δay、δaz分別為三軸誤差角。

4)利用PI控制環(huán)節(jié)求解陀螺儀漂移補(bǔ)償向量：

(10)

(11)

式中：wx、wy、wz分別為陀螺原三軸漂移；x、y、z分別為三軸漂移補(bǔ)償量；為修正后的三軸漂移；Kp、Ki分別為PI控制器的控制參數(shù)。

2) 跨移動(dòng)平臺(tái)適配能力，基于React Native，通過(guò)封裝或引入基礎(chǔ)組件形成基礎(chǔ)組件庫(kù)，實(shí)現(xiàn)“一次編寫(xiě)，分別編譯，多端運(yùn)行”的跨平臺(tái)目標(biāo)；

5)代入四元數(shù)更新方程式(6)中，求解角速度微分方程。

6)對(duì)四元數(shù)進(jìn)行規(guī)范化處理，由式(3)求解姿態(tài)角的精確值。

根據(jù)應(yīng)用場(chǎng)景不同，調(diào)整濾波器開(kāi)啟條件以及PI的控制參數(shù)可以有效抑制除航向角外的積分漂移。

按照上述算法，通過(guò)隨動(dòng)信息采集、姿態(tài)解算與互補(bǔ)濾波器3個(gè)環(huán)節(jié)完成控制器程序設(shè)計(jì)，可以準(zhǔn)確獲取人體頭部姿態(tài)。此部分延遲主要存在于采集、解算與傳輸延遲，異步通訊延遲。經(jīng)DSP內(nèi)部時(shí)鐘測(cè)量，該部分采集與傳輸總延遲相對(duì)于毫秒級(jí)的機(jī)電響應(yīng)時(shí)間可以忽略不記。

2 云臺(tái)控制設(shè)計(jì)

2.1 云臺(tái)電氣改造

在隨動(dòng)控制應(yīng)用中，云臺(tái)需要對(duì)外部控制命令做出準(zhǔn)確且快速的響應(yīng)，要求云臺(tái)有較高的位置控制實(shí)時(shí)性與準(zhǔn)確性[16]。但傳統(tǒng)小型云臺(tái)因轉(zhuǎn)動(dòng)慣量較大，電機(jī)驅(qū)動(dòng)力矩不足，無(wú)法滿足此應(yīng)用的動(dòng)態(tài)特性需求。同時(shí)，較大的驅(qū)動(dòng)力矩會(huì)增大電機(jī)運(yùn)動(dòng)慣量，增大控制難度。需要根據(jù)機(jī)電控制對(duì)象設(shè)計(jì)電氣方案來(lái)提升整體性能。

本文選用一種帶有直流力矩電機(jī)、18位絕對(duì)位置編碼器的小型云臺(tái)進(jìn)行算法改造，如圖3所示。該云臺(tái)可作為車載觀察鏡使用，并且云臺(tái)的旋轉(zhuǎn)部分體積與人體頭部大致相當(dāng)，選用其作為機(jī)電執(zhí)行機(jī)構(gòu)具有代表性。該云臺(tái)外框繞方位軸z旋轉(zhuǎn)，內(nèi)框繞俯仰軸x旋轉(zhuǎn)，外框軸系配有導(dǎo)電滑環(huán)，可連續(xù)無(wú)限旋轉(zhuǎn)，內(nèi)框軸系配置機(jī)械限位機(jī)構(gòu)。云臺(tái)內(nèi)框軸系與外框軸系均采用高精密交叉滾子軸承，以保證該軸的回轉(zhuǎn)精度和軸向承載及抗傾覆力矩。小型云臺(tái)電氣原理如圖4所示。

圖3 小型云臺(tái)結(jié)構(gòu)示意圖Fig.3 Schematic diagram of the small turntable structure

圖4 小型云臺(tái)電氣原理圖Fig.4 Electrical schematic diagram of the two-axis turntable

雙軸云臺(tái)的電氣系統(tǒng)通過(guò)DSP實(shí)現(xiàn)編碼器解碼、電機(jī)控制功能。

1)編碼器解碼

編碼器選用的18位絕對(duì)位置編碼器，精度可達(dá)到±20″。在解碼過(guò)程中，將DSP的兩路多通道緩沖串行口(McBSP)設(shè)置為SPI模式，發(fā)送固定時(shí)鐘信號(hào)至編碼器，觸發(fā)編碼器返回編碼信息。編碼器返回的SSI二進(jìn)制編碼信息至DSP，完成數(shù)據(jù)接收過(guò)程。

2)電機(jī)控制

DSP通過(guò)異步串口(SCI)外設(shè)接收外部控制命令，并結(jié)合從編碼器獲取的云臺(tái)位置信系解算出控制量(數(shù)字電壓)。該控制量通過(guò)同步串口(SPI)發(fā)送，經(jīng)DA轉(zhuǎn)換為模擬信號(hào)發(fā)送至電機(jī)驅(qū)動(dòng)器。電機(jī)驅(qū)動(dòng)采用美國(guó)AMC公司的模塊化驅(qū)動(dòng)器以簡(jiǎn)化電氣設(shè)計(jì)。該驅(qū)動(dòng)器以開(kāi)關(guān)速度快、壓降小的IGBT功率模塊構(gòu)成雙路四象限全橋電路，可根據(jù)±5 V以內(nèi)的模擬電壓生成PWM驅(qū)動(dòng)電壓，對(duì)電機(jī)進(jìn)行驅(qū)動(dòng)。采用成熟驅(qū)動(dòng)模塊作為電機(jī)驅(qū)動(dòng)器，其成熟度高、可靠性好，降低了機(jī)電控制難度。

2.2 控制流程

本文使用高頻位置閉環(huán)控制取代了速度閉環(huán)，用2 kHz的實(shí)時(shí)高頻位置控制頻率，具有更強(qiáng)的控制實(shí)時(shí)性，更適用于位置隨動(dòng)控制。云臺(tái)的控制程序按照以下10個(gè)步驟進(jìn)行(見(jiàn)圖5)[17-18]：

步驟1系統(tǒng)、外設(shè)、中斷初始化。

步驟2開(kāi)啟0.5 ms周期控制定時(shí)器。

步驟3使用DSP的McBSP收取編碼器數(shù)據(jù)。

步驟4絕對(duì)位置編碼器在0°與360°位置會(huì)發(fā)生位置跳變，進(jìn)行圈數(shù)判斷，以形成螺旋結(jié)構(gòu)；從360°至0°時(shí)圈數(shù)加1(角度加360°)；從0°～360°時(shí)圈數(shù)減1(角度減360°)。

步驟5使用FIR濾波器對(duì)角度值濾波后儲(chǔ)存，以減少角度測(cè)量的噪聲干擾或臺(tái)階突跳。

步驟6從寄存器(緩存RS422中斷數(shù)據(jù))讀取上位機(jī)發(fā)出的目標(biāo)位置，因控制頻率高于目標(biāo)位置發(fā)送頻率，若目標(biāo)位置未更新則按照寄存器現(xiàn)有數(shù)值進(jìn)行后續(xù)計(jì)算。

步驟7對(duì)角度值進(jìn)行窗口采樣后計(jì)算角速度值，以減少速度值的噪聲，然后根據(jù)實(shí)際位置、目標(biāo)位置計(jì)算角度偏差量、速度偏差量。

步驟8由模糊PID控制器根據(jù)角度偏差量以及速度偏差量生成Kp、Ki、Kd取值。

步驟9由Kp、Ki、Kd角度偏差量以及速度偏差量生成控制電壓V，并進(jìn)行飽和控制。

步驟10清空寄存器，進(jìn)入下個(gè)控制循環(huán)。

在控制流程中，步驟6保證了云臺(tái)能夠在0.5 ms內(nèi)收到位置指令。步驟8、步驟9決定了控制的穩(wěn)定性、準(zhǔn)確性、快速性?？紤]到本文系統(tǒng)動(dòng)靜態(tài)要求，系統(tǒng)采用帶有模糊PID控制的復(fù)合控制策略提升系統(tǒng)性能。

圖5 以DSP28335為處理器的小型云臺(tái)控制框圖Fig.5 Control block diagram of the turntable with DSP28335

2.3 模糊PID控制

數(shù)字PID控制器通過(guò)微分增益提供系統(tǒng)穩(wěn)定所需的阻尼，積分增益來(lái)消除穩(wěn)態(tài)誤差，速度前饋增益來(lái)減少阻尼引起的跟隨誤差(該項(xiàng)誤差與速度呈正比)，如圖6所示。圖6中，Kvff為速度前饋增益，En為第n個(gè)采樣時(shí)刻的位置偏差，P、I、D分別為為比例項(xiàng)、積分項(xiàng)、微分項(xiàng)產(chǎn)生的控制電壓，S為累積誤差。

圖6 數(shù)字PID控制器設(shè)計(jì)Fig.6 Design of the digital PID controller

數(shù)字PID控制器輸出計(jì)算公式為

En=Pt，n-Pa，n

(12)

(13)

為提升高動(dòng)態(tài)輸入下機(jī)電系統(tǒng)的控制效率，采用模糊PID自整定控制，使控制器能夠根據(jù)位置偏差En和速度Va調(diào)整Kp、Ki、Kd取值，以達(dá)到最優(yōu)控制[19]。本文中響應(yīng)時(shí)間與執(zhí)行精度是重點(diǎn)關(guān)注指標(biāo)，考慮到系統(tǒng)在目標(biāo)位置距離不同時(shí)，應(yīng)采用不同的PID控制參數(shù)，傳統(tǒng)PID控制難以勝任，因此需要設(shè)計(jì)模糊控制器，按照不同階段機(jī)電運(yùn)動(dòng)特性對(duì)PID取值進(jìn)行優(yōu)化[20]。

除上述控制策略外，約定云臺(tái)的極限位置精度，以避免云臺(tái)在目標(biāo)位置的微小抖動(dòng)。該現(xiàn)象是由于云臺(tái)運(yùn)動(dòng)到目標(biāo)位置后存在微小誤差，此微小偏差經(jīng)控制算法產(chǎn)生的驅(qū)動(dòng)力矩已經(jīng)小于云臺(tái)軸系的靜摩擦力，轉(zhuǎn)臺(tái)雖然不再運(yùn)動(dòng)，但此時(shí)控制器PID的積分項(xiàng)仍緩慢累積，直到再次驅(qū)動(dòng)轉(zhuǎn)臺(tái)。當(dāng)云臺(tái)再次運(yùn)動(dòng)時(shí)云臺(tái)內(nèi)部靜摩擦變?yōu)榛瑒?dòng)摩擦，摩擦力瞬間減小，同時(shí)控制策略產(chǎn)生的制動(dòng)力將云臺(tái)制動(dòng)，直至云臺(tái)運(yùn)動(dòng)停止。該過(guò)程不斷重復(fù)，形成云臺(tái)在目標(biāo)位置的微小抖動(dòng)現(xiàn)象。因此，考慮到本文研究隨動(dòng)控制的精度極限，設(shè)定云臺(tái)運(yùn)行偏差在±0.01°以內(nèi)時(shí)不再進(jìn)行積分輸出，以消除該穩(wěn)態(tài)微小抖動(dòng)現(xiàn)象，增強(qiáng)了控制穩(wěn)定性。

3 實(shí)驗(yàn)驗(yàn)證

為了對(duì)基于微慣性器件與模糊控制云臺(tái)的隨動(dòng)系統(tǒng)控制實(shí)時(shí)性與準(zhǔn)確性進(jìn)行驗(yàn)證，設(shè)計(jì)三部分實(shí)驗(yàn)內(nèi)容。首先設(shè)計(jì)姿態(tài)測(cè)量精度實(shí)驗(yàn)，使用高精度雙軸轉(zhuǎn)臺(tái)對(duì)慣性器件的短時(shí)測(cè)量精度進(jìn)行驗(yàn)證，驗(yàn)證隨動(dòng)系統(tǒng)的姿態(tài)測(cè)量精度。隨后設(shè)計(jì)頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)，利用姿態(tài)測(cè)量精度實(shí)驗(yàn)中的姿態(tài)測(cè)量系統(tǒng)對(duì)人體頭部運(yùn)動(dòng)進(jìn)行測(cè)量，獲取人體頭部運(yùn)動(dòng)的典型參數(shù)。最后結(jié)合頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)中的人體頭部運(yùn)動(dòng)典型參數(shù)，設(shè)計(jì)位置隨動(dòng)控制實(shí)驗(yàn)，對(duì)微慣性器件與模糊控制云臺(tái)隨動(dòng)系統(tǒng)性能進(jìn)行綜合表征，驗(yàn)證隨動(dòng)系統(tǒng)的實(shí)時(shí)性與準(zhǔn)確性。

3.1 姿態(tài)測(cè)量精度實(shí)驗(yàn)

首先進(jìn)行姿態(tài)測(cè)量精度實(shí)驗(yàn)，以驗(yàn)證IMU姿態(tài)解算得準(zhǔn)確性。在姿態(tài)測(cè)量精度驗(yàn)證實(shí)驗(yàn)中將IMU與紅外全局相機(jī)固定在902E-1型高精度雙軸轉(zhuǎn)臺(tái)上，如圖7所示。

圖7 姿態(tài)精度實(shí)驗(yàn)設(shè)備圖Fig.7 Attitude accuracy test equipment

在測(cè)試開(kāi)始前對(duì)IMU進(jìn)行30 min的上電初始化，使IMU達(dá)到恒定溫度以減小零偏變化。在實(shí)驗(yàn)過(guò)程中鎖定雙軸轉(zhuǎn)臺(tái)的俯仰軸，使轉(zhuǎn)臺(tái)航向角從0°起，每次遞增10°并保持30 s直至40°，再依次遞減至0°。在運(yùn)動(dòng)過(guò)程中，采集計(jì)算機(jī)分別以10 Hz記錄轉(zhuǎn)臺(tái)航向角信息，以1 kHz獲取DSP解算的姿態(tài)測(cè)量信息，分別對(duì)采集到得轉(zhuǎn)臺(tái)真值與IMU測(cè)量數(shù)據(jù)標(biāo)記時(shí)間戳，以便于后續(xù)數(shù)據(jù)處理。

數(shù)據(jù)采集后將數(shù)據(jù)對(duì)比如圖8所示，從中可以看出與轉(zhuǎn)臺(tái)實(shí)際讀取數(shù)據(jù)比較，IMU姿態(tài)測(cè)量結(jié)果基本準(zhǔn)確。選取轉(zhuǎn)臺(tái)航向角40°與返程時(shí)航向角10°兩個(gè)典型位置放大分析，發(fā)現(xiàn)IMU解算結(jié)果存在0.2°以內(nèi)的測(cè)量誤差。

圖8 姿態(tài)測(cè)量精度實(shí)驗(yàn)Fig.8 Attitude measurement accuracy experiment

為更清晰地觀察IMU解算數(shù)據(jù)與真值相比的誤差，將IMU解算值與真值作差繪制誤差隨時(shí)間變化圖，如圖9所示?？傮w上看，因測(cè)量時(shí)間較短，陀螺零偏影響可忽略。在實(shí)驗(yàn)過(guò)程300 s內(nèi)，IMU解算精度較高，均方根誤差(RMSE)為0.065 2°，可以滿足短時(shí)姿態(tài)測(cè)量精度需求。誤差主要原因在于轉(zhuǎn)臺(tái)運(yùn)動(dòng)狀態(tài)下由于真值采樣頻率較低，且轉(zhuǎn)臺(tái)內(nèi)部采集存在時(shí)間延遲波動(dòng)，影響了運(yùn)動(dòng)狀態(tài)下求得誤差的精確度。

圖9 姿態(tài)測(cè)量精度實(shí)驗(yàn)航向角誤差隨時(shí)間變化關(guān)系Fig.9 Relationship between heading angle error andtime

3.2 頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)

將IMU固定在人的前額上，如圖10所示。使用實(shí)時(shí)姿態(tài)更新與互補(bǔ)濾波算法獲取頭部運(yùn)動(dòng)姿態(tài)的角度信息。設(shè)計(jì)頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)以模擬頭部對(duì)外部階躍信號(hào)的姿態(tài)響應(yīng)，獲取頭部轉(zhuǎn)動(dòng)時(shí)的靜態(tài)信息。在實(shí)驗(yàn)中，測(cè)試者頭部從起始位置依次快速轉(zhuǎn)頭到A、B、C、D、E5個(gè)位置，并在每個(gè)位置停留一小段時(shí)間，如圖11所示。為避免超出人體轉(zhuǎn)動(dòng)范圍，A、B、C、D、E5個(gè)位置交錯(cuò)分布在正前方±60°范圍內(nèi)。該測(cè)試重復(fù)10次，測(cè)得數(shù)據(jù)如圖12所示。

圖10 頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)設(shè)備Fig.10 Head movement measurement equipment

圖11 頭部位置測(cè)量實(shí)驗(yàn)運(yùn)動(dòng)示意圖Fig.11 Schematic diagram of the head position measurement test

圖12 頭部位置測(cè)量實(shí)驗(yàn)10次測(cè)量數(shù)據(jù)Fig.12 Data of the ten measurement experiments

圖13 頭部位置測(cè)量實(shí)驗(yàn)單次測(cè)量數(shù)據(jù)Fig.13 Single-time measurement data

由10次測(cè)試數(shù)據(jù)可以看出，頭部運(yùn)動(dòng)周期約為5 s左右，運(yùn)動(dòng)范圍在±60°范圍內(nèi)，與位置點(diǎn)分布間隔相同。因眼部運(yùn)動(dòng)影響，多次運(yùn)動(dòng)中頭部再次到達(dá)固定位置時(shí)存在位置偏差，該偏差在4°以內(nèi)。雖然不同次測(cè)量的重復(fù)精度有限，但角度變化趨勢(shì)大體相同，通過(guò)差分計(jì)算得到頭部角速度極限值接近600°/s。

就單次測(cè)試數(shù)據(jù)來(lái)看，如圖13所示，頭部對(duì)單點(diǎn)階躍信號(hào)的運(yùn)動(dòng)響應(yīng)更近似多階系統(tǒng)，體現(xiàn)在頭部在到達(dá)指定位置點(diǎn)時(shí)，測(cè)得角度存在小幅超調(diào)。相鄰點(diǎn)之間頭部運(yùn)動(dòng)時(shí)間約為300 ms，因生理結(jié)構(gòu)限制，極限角速度持續(xù)時(shí)間十分短暫，僅為100 ms左右。另外，從靜止時(shí)的數(shù)據(jù)可以看出，即使在頭部穩(wěn)定時(shí)，姿態(tài)輸出也有一定范圍的波動(dòng)，選取32.5～33.1 s這一段穩(wěn)定時(shí)間觀察角度變化范圍，其中角度輸出最大值43.54°，最小值41.71°，角度變化范圍在1.83°以內(nèi)，RMSE為0.51°。可以看出姿態(tài)輸出波動(dòng)范圍高于“姿態(tài)測(cè)量精度實(shí)驗(yàn)”中IMU測(cè)量精度(RMSE為0.065 2°)一個(gè)數(shù)量級(jí)，其余頭部穩(wěn)定時(shí)的姿態(tài)輸出也基本遵循這一精度特征?？梢越七x取1°作為高動(dòng)態(tài)體感隨動(dòng)控制的精度設(shè)計(jì)指標(biāo)。

由人體頭部運(yùn)動(dòng)的角速度與角加速度實(shí)測(cè)結(jié)果人可以看出，人體頭部運(yùn)動(dòng)的極限角速度與角加速度很高，但維持時(shí)間較短，在設(shè)計(jì)隨動(dòng)機(jī)電系統(tǒng)時(shí)可根據(jù)此數(shù)據(jù)提出響應(yīng)特性需求，并且為減少機(jī)電系統(tǒng)延遲，應(yīng)盡可能匹配頭部運(yùn)動(dòng)的角速度與角加速度指標(biāo)。

3.3 位置隨動(dòng)響應(yīng)實(shí)驗(yàn)

為驗(yàn)證整套體感隨動(dòng)系統(tǒng)運(yùn)行精度與延遲情況，設(shè)計(jì)兩個(gè)小型雙軸云臺(tái)的運(yùn)動(dòng)響應(yīng)實(shí)驗(yàn)。在實(shí)驗(yàn)過(guò)程中，手動(dòng)控制搭載IMU的1號(hào)傳感器云臺(tái)運(yùn)動(dòng)作為隨動(dòng)控制輸入，解算的姿態(tài)數(shù)據(jù)作為隨動(dòng)控制命令發(fā)給2號(hào)執(zhí)行器云臺(tái)，并由2號(hào)云臺(tái)執(zhí)行運(yùn)動(dòng)。在運(yùn)行過(guò)程中使用上位機(jī)同時(shí)記錄1號(hào)云臺(tái)與2號(hào)云臺(tái)的角度數(shù)據(jù)，實(shí)驗(yàn)原理如圖14所示。

圖14 云臺(tái)運(yùn)動(dòng)響應(yīng)實(shí)驗(yàn)原理圖Fig.14 Schematic diagram of the gimbal motion response experiment

測(cè)量過(guò)程中兩個(gè)云臺(tái)姿態(tài)數(shù)據(jù)直接從云臺(tái)內(nèi)部的18位光電編碼器獲取，編碼器角度測(cè)量精度優(yōu)于0.005°，可近似認(rèn)為該測(cè)量值為真值。2號(hào)云臺(tái)已采用模糊PID控制方法，根據(jù)機(jī)電系統(tǒng)做出了最優(yōu)化控制參數(shù)已設(shè)計(jì)，以保證隨動(dòng)控制的最快響應(yīng)與執(zhí)行精度。此外，為減小數(shù)據(jù)采集延遲帶來(lái)的干擾，兩個(gè)云臺(tái)角度均通過(guò)同樣的軟硬件采集，傳輸至上位機(jī)時(shí)打上時(shí)間戳。

云臺(tái)運(yùn)動(dòng)響應(yīng)的具體實(shí)驗(yàn)設(shè)備布置如圖15所示。為便于后續(xù)隨動(dòng)控制的性能評(píng)估，首先進(jìn)行系統(tǒng)傳感器到執(zhí)行的隨動(dòng)延遲時(shí)間估算。在估算過(guò)程中迭代計(jì)算延遲時(shí)間t，尋找使得1號(hào)云臺(tái)與2號(hào)云臺(tái)角度差的標(biāo)準(zhǔn)差最小值σ。將使σ最小的時(shí)間t作為估算的隨動(dòng)延遲時(shí)間，如圖16所示。

圖15 云臺(tái)運(yùn)動(dòng)響應(yīng)實(shí)驗(yàn)設(shè)備Fig.15 Experimental equipment for PTZ motion response

圖16 隨動(dòng)響應(yīng)延遲時(shí)間計(jì)算原理Fig.16 Schematic diagram for the calculation of follow-up response delay

實(shí)驗(yàn)過(guò)程中，為保證數(shù)據(jù)的準(zhǔn)確性進(jìn)行5組2 min以上測(cè)量，運(yùn)動(dòng)類型涵蓋了低速運(yùn)動(dòng)、高速運(yùn)動(dòng)、往復(fù)運(yùn)動(dòng)、間歇運(yùn)動(dòng)等，因不同運(yùn)動(dòng)形式產(chǎn)生的隨動(dòng)效果存在一定差別，求得的多組隨動(dòng)延遲均值更具代表意義(見(jiàn)表1)。計(jì)算得到隨動(dòng)系統(tǒng)平均延遲為88.17 ms。延遲主要源自于云臺(tái)機(jī)電系統(tǒng)執(zhí)行過(guò)程中由轉(zhuǎn)動(dòng)慣量、轉(zhuǎn)動(dòng)阻尼等帶來(lái)的響應(yīng)延遲，這部分延遲難以通過(guò)算法改進(jìn)來(lái)避免。

表1 隨動(dòng)延遲時(shí)間估算Table 1 Follow-up delay time estimation

為詳細(xì)探討隨動(dòng)精度與體感隨動(dòng)延遲之間關(guān)系，結(jié)合3.2節(jié)頭部運(yùn)動(dòng)測(cè)量實(shí)驗(yàn)中人體頭部的典型運(yùn)動(dòng)數(shù)據(jù)選取，考慮人員對(duì)目標(biāo)視覺(jué)追蹤、大范圍目標(biāo)切換、突然啟動(dòng)、突然停止等運(yùn)動(dòng)的極限工況，總結(jié)出了低速、間歇、大范圍、高速、搖擺等典型運(yùn)動(dòng)形式，形成一組綜合運(yùn)動(dòng)條件進(jìn)行隨動(dòng)系統(tǒng)的精度特性分析。

采集一組綜合運(yùn)動(dòng)條件下的數(shù)據(jù)進(jìn)行比對(duì)分析。在實(shí)驗(yàn)中手動(dòng)控制1號(hào)傳感器云臺(tái)在低速、間歇、高速、搖擺的混合運(yùn)動(dòng)中工作300 s，記錄得到修正延遲后的1號(hào)云臺(tái)與2號(hào)云臺(tái)航向角隨時(shí)間變化關(guān)系如圖17(a)所示，并將高速運(yùn)動(dòng)部分與搖擺運(yùn)動(dòng)部分放大展示為圖17(b)與圖17(c)。1號(hào)云臺(tái)與2號(hào)云臺(tái)航向角差分求取的角速度隨時(shí)間變化記錄結(jié)果如圖18所示。隨動(dòng)誤差與時(shí)間關(guān)系記錄結(jié)果如圖19所示。

圖17 位置隨動(dòng)響應(yīng)實(shí)驗(yàn)Fig.17 Experiments of location follow-up response

圖18 航向角速度隨時(shí)間變化Fig.18 Relationship between course angle velocity and time

圖19 航向角偏差隨時(shí)間變化Fig.19 Relationship between course angle deviation and time

可以看出：1)在低速運(yùn)動(dòng)(速度峰均值約30°/s)情況下，隨動(dòng)控制效果較好，執(zhí)行誤差較小，即使運(yùn)動(dòng)范圍較大也能保持較高執(zhí)行精度；2)在間歇情況下，角度誤差基本穩(wěn)定在固定值不動(dòng)；3)在高速運(yùn)動(dòng)(峰值約300°/s)狀況下，執(zhí)行云臺(tái)以最大功率向目標(biāo)位置運(yùn)動(dòng)，但受限于機(jī)電常數(shù)，運(yùn)動(dòng)過(guò)程無(wú)法達(dá)到目標(biāo)速度，因此運(yùn)動(dòng)過(guò)程中角度偏差較大，運(yùn)動(dòng)結(jié)束后偏差得以快速消除，與位置跟隨的控制邏輯相符；4)在快速搖擺(峰均值約50°/s)狀態(tài)下，由于機(jī)電延遲，執(zhí)行云臺(tái)在未達(dá)到傳感器最遠(yuǎn)運(yùn)動(dòng)位置時(shí)即收到了新的控制指令，開(kāi)始返回，與機(jī)電控制邏輯相符。

總體上看，在實(shí)驗(yàn)過(guò)程300 s內(nèi)，綜合低速、間歇、大范圍、高速、搖擺等運(yùn)動(dòng)形式，隨動(dòng)系統(tǒng)精度較高，能夠在88.17 ms內(nèi)實(shí)現(xiàn)RMSE 1.478°的隨動(dòng)運(yùn)動(dòng)精度，可以滿足位置隨動(dòng)控制精度需求。該精度主要受限于機(jī)電控制環(huán)節(jié)，提升響應(yīng)速度與精度一方面可以通過(guò)載荷、編碼器、機(jī)械結(jié)構(gòu)、線纜減重以降低轉(zhuǎn)動(dòng)慣量實(shí)現(xiàn)；另一方面，提升電機(jī)效率，能在單位轉(zhuǎn)動(dòng)慣量下實(shí)現(xiàn)更高功率輸出，也能改善隨動(dòng)系統(tǒng)的機(jī)電特性。

4 結(jié)論

本文提出了一種基于微慣性器件與模糊控制云臺(tái)的低延遲體感隨動(dòng)控制技術(shù)，通過(guò)低成本高采樣頻率的姿態(tài)算法與高實(shí)時(shí)性與響應(yīng)精度的小型雙軸云臺(tái)模糊控制算法改進(jìn)，實(shí)現(xiàn)了88.17 ms內(nèi)實(shí)現(xiàn)RMSE 1.478°的隨動(dòng)控制。該隨動(dòng)控制系統(tǒng)能夠滿足無(wú)人機(jī)控制與觀察、車載雷達(dá)與武器站控制等領(lǐng)域，可大幅降低設(shè)備操控復(fù)雜度，提升控制特性。后續(xù)優(yōu)化一方面可圍繞慣性系統(tǒng)濾波修正，引入觀察量提升隨動(dòng)控制的長(zhǎng)期穩(wěn)定性；另一方面繼續(xù)提升執(zhí)行器機(jī)電性能，提升隨動(dòng)控制的響應(yīng)速度與精度。