趙修平，齊嘉興

(海軍航空大學(xué)， 山東 煙臺 264001)

導(dǎo)彈裝填裝置可以克服傳統(tǒng)裝填手段的不足，有利于提高導(dǎo)彈裝填的工作效率，減少人員工作量，充分發(fā)揮人員工作效能，但如何實(shí)現(xiàn)裝填時導(dǎo)彈與裝填位置的精確對準(zhǔn)是目前亟待解決的問題。

并聯(lián)機(jī)構(gòu)由于具有控制精度高、工作空間小、負(fù)載能力強(qiáng)等特點(diǎn)[1]，作為微動機(jī)器人在精密工程領(lǐng)域中得到了廣泛應(yīng)用，因此用并聯(lián)機(jī)構(gòu)作為導(dǎo)彈裝填平臺實(shí)現(xiàn)導(dǎo)彈姿態(tài)的精確調(diào)整具有一定優(yōu)勢。

較其他傳感器相比，視覺傳感器因其信息量大、適用范圍廣、非接觸性等特點(diǎn)已成為最重要的傳感器之一，因此視覺伺服與基于傳統(tǒng)傳感器的控制相比也具有更高的靈活性，更高的精度等優(yōu)勢[2]。而視覺伺服中基于無標(biāo)定的圖像視覺伺服技術(shù)由于不需要對攝像機(jī)進(jìn)行標(biāo)定和目標(biāo)模型的位姿進(jìn)行檢測，使其較其他視覺伺服方法擁有了更高的魯棒性，目前已成為無標(biāo)定視覺伺服控制中的主流技術(shù)[3,4]。

本文提出了基于視覺伺服的導(dǎo)彈裝填平臺自動對準(zhǔn)方法，結(jié)合圖像視覺伺服技術(shù)和并聯(lián)平臺在控制精度上的優(yōu)勢，在裝填平臺運(yùn)動學(xué)模型的基礎(chǔ)上利用視覺伺服控制實(shí)現(xiàn)六自由度并聯(lián)機(jī)構(gòu)的姿態(tài)調(diào)整和自動對準(zhǔn)。

1 裝填平臺及坐標(biāo)系介紹

整個裝填裝置由提升機(jī)構(gòu)、并聯(lián)裝填平臺以及推彈機(jī)構(gòu)組成，攝像機(jī)與推彈機(jī)構(gòu)固連，其結(jié)構(gòu)如圖1所示。其中裝填平臺為六自由度的并聯(lián)機(jī)構(gòu)，由動平臺A1A2A3A4A5A6和靜平臺B1B2B3B4B5B6組成，采用半正六邊形的形狀，上下平臺通過液壓缸和虎克鉸相連接，該平臺主要通過視覺伺服技導(dǎo)彈空間六自由度位姿進(jìn)行調(diào)整以實(shí)現(xiàn)與發(fā)射裝置裝填位置的對準(zhǔn)功能。

圖1 裝填裝置結(jié)構(gòu)示意圖

為了滿足自動對準(zhǔn)的需要，裝填平臺和視覺伺服系統(tǒng)共使用了六種坐標(biāo)系，分別為世界坐標(biāo)系OWxWyWzW，動平臺坐標(biāo)系OPxPyPzP，靜平臺坐標(biāo)系OBxByBzB，相機(jī)坐標(biāo)系OCxCyCzC，圖像平面坐標(biāo)系Ocxyz以及像素坐標(biāo)系Ouv。

動平臺坐標(biāo)系和靜平臺坐標(biāo)系建立在上下平臺中心，見圖1，符合右手坐標(biāo)系規(guī)則。世界坐標(biāo)系與靜平臺坐標(biāo)系重合，相機(jī)坐標(biāo)系建立在攝像機(jī)中心，如圖2所示，其x軸正向與動平臺y軸負(fù)方向相同，z軸正向與動平臺x軸正向相同。圖像平面坐標(biāo)系與像素坐標(biāo)系建立在攝像機(jī)內(nèi)部，用于相機(jī)的小孔成像模型。設(shè)空間中一點(diǎn)P在相機(jī)坐標(biāo)系中的坐標(biāo)為(XC,YC,ZC)，則空間中一點(diǎn)的坐標(biāo)由相機(jī)坐標(biāo)系到像素坐標(biāo)的轉(zhuǎn)換關(guān)系為[5]

(1)

圖2 相機(jī)坐標(biāo)系示意圖

2 裝填平臺運(yùn)動學(xué)模型建立

2.1 運(yùn)動學(xué)逆解

運(yùn)動學(xué)逆解即通過動平臺位姿求得六個液壓支腿的長度[6]。動平臺相對靜平臺的位置用向量t=[x,y,z]T表示，即OP在靜平臺中的坐標(biāo)。動平臺相對靜平臺的姿態(tài)通常使用Roll-Pitch-Yaw旋轉(zhuǎn)表示，假定偏航角為α，俯仰角為β，橫滾角為γ，則動平臺相對靜平臺的姿態(tài)為

BRP=R(z,γ)R(y,β)R(z,α)=

(2)

其中，s(·)=sin(·)，c(·)=cos(·)。則動平臺相對靜平臺的位姿可以表示為

p=[x,y,z,α,β,γ]T

(3)

動、靜平臺上的鉸點(diǎn)坐標(biāo)分別用ai和bi表示，因此液壓支腿的空間向量可描述為

li=t+BRPai-bi

(4)

故液壓支腿的長度為

(5)

(6)

為了得到液壓缸速度，將上式得到的上鉸點(diǎn)速度向液壓缸矢量方向投影，即

(7)

整理可得到裝填平臺空間度到液壓支腿伸縮速度的雅克比矩陣及其傳遞關(guān)系

(8)

2.2 運(yùn)動學(xué)正解

運(yùn)動學(xué)正解即通過六個液壓支腿的長度求得動平臺位姿。目前關(guān)于六自由度并聯(lián)機(jī)構(gòu)的運(yùn)動學(xué)正向求解方法主要分為解析解法和數(shù)值解法兩大類，解析法主要致力于獲得全部解，而數(shù)值解致力于提高求解效率，更適用于實(shí)際應(yīng)用[7]。數(shù)值解法中Newton-Raphson方法是一種傳統(tǒng)且被廣泛使用的迭代求解方法，有較好的實(shí)時性和通用性[8]。Newton法的迭代式為

pk+1=pk-(F′(pk))-1F(pk)

(9)

從迭代式可以看出Newton法的關(guān)鍵是構(gòu)造動平臺位姿的函數(shù)F(pk)，文獻(xiàn)[9]提出了一種F(pk)的構(gòu)造方法，在此基礎(chǔ)上定義了改進(jìn)的Jacobian矩陣[9]，并給出了解析表達(dá)式。文獻(xiàn)[7]經(jīng)過仿真計(jì)算表明了改進(jìn)的Jacobian矩陣方法較其他數(shù)值解法具有更好的計(jì)算效率。由于視覺伺服系統(tǒng)每個采樣周期均需要正向運(yùn)動學(xué)的解算，對算法的實(shí)時性要求較高，因此在裝填平臺的正向運(yùn)動學(xué)中應(yīng)用改進(jìn)的Jacobian矩陣方法，結(jié)合式(9)，該算法的流程框圖如圖3所示。

圖3 Newton法求解運(yùn)動學(xué)正解流程框圖

3 基于圖像視覺伺服的自動對準(zhǔn)方法

3.1 基于圖像的視覺伺服

基于圖像的視覺伺服有兩個關(guān)鍵問題需要解決，一是圖像特征的選取，通常分為局部特征和全局特征?？紤]到實(shí)際情況中相機(jī)視場的限制，同時點(diǎn)特征作為局部特征容易提取，其圖像雅克比矩陣也易于獲得[2]，因此可以選取發(fā)射裝置上的點(diǎn)特征為圖像特征。

第二個關(guān)鍵問題是圖像雅克比矩陣的求取。圖像雅克比矩陣描述了執(zhí)行機(jī)構(gòu)的空間速度到圖像空間速度的映射關(guān)系，即

(10)

其中vC=[vx,vy,vz,ωx,ωy,ωz]T，表示了攝像機(jī)在自身坐標(biāo)系中的空間速度。一個點(diǎn)特征的圖像雅克比矩陣為[10]

(11)

3.2 自動對準(zhǔn)流程

裝填裝置在執(zhí)行裝填任務(wù)時主要分為3個階段，其中前兩個階段實(shí)現(xiàn)導(dǎo)彈的自動對準(zhǔn)過程：

1) 由于裝填時裝填裝置和發(fā)射裝置的位置相對固定，因此先由提升機(jī)構(gòu)搭載裝填平臺使裝填平臺上的推彈機(jī)構(gòu)運(yùn)動至裝填位置下方附近；

2) 考慮到發(fā)射裝置復(fù)位誤差、艦面不平等因素，裝填平臺應(yīng)用圖像視覺伺服技術(shù)實(shí)現(xiàn)推彈機(jī)構(gòu)的位姿調(diào)整以使導(dǎo)彈與裝填位置位姿一致?；谘b填平臺的運(yùn)動學(xué)原理，該階段的自動對準(zhǔn)流程如圖4所示。首先，攝像機(jī)在當(dāng)前位姿下通過圖像處理過程獲得圖像特征的像素坐標(biāo)f，然后與期望的像素坐標(biāo)值fd相比對形成負(fù)反饋控制產(chǎn)生控制誤差e。視覺控制器可采用比例控制產(chǎn)生攝像機(jī)所需速度，即

(12)

控制誤差經(jīng)過視覺控制器并結(jié)合圖像雅克比矩陣形成攝像機(jī)的控制速度，該速度可進(jìn)一步轉(zhuǎn)換為裝填平臺的控制速度。最后，裝填平臺的動平臺根據(jù)控制速度進(jìn)行運(yùn)動并使攝像機(jī)到達(dá)新的位姿，至此完成了攝像機(jī)一個采樣周期內(nèi)裝填平臺位姿的調(diào)整過程。當(dāng)圖像特征的像素坐標(biāo)達(dá)到期望的坐標(biāo)值或誤差滿足精度要求時，裝填平臺搭載導(dǎo)彈運(yùn)動到指定位置并完成了與裝填位置的對準(zhǔn)。

圖4 裝填平臺自動對準(zhǔn)流程框圖

3) 在完成前兩個階段的對準(zhǔn)過程后，推彈機(jī)構(gòu)將導(dǎo)彈送入裝填位置，至此完成了一次裝填。

需要指出的是，發(fā)射裝置上有多個裝填位置，每次裝填時都要先執(zhí)行上述第一階段的過程，使導(dǎo)彈與裝填位置粗略對準(zhǔn)，再進(jìn)一步由裝填平臺實(shí)現(xiàn)導(dǎo)彈位姿的精確調(diào)整。

3.2 裝填平臺對準(zhǔn)過程中的速度轉(zhuǎn)換關(guān)系

在裝填平臺的對準(zhǔn)流程中，存在圖像特征的運(yùn)動速度，攝像機(jī)的運(yùn)動速度，動平臺的運(yùn)動速度、液壓支腿的伸縮速度以及速度之間的雅克比矩陣，但不同的速度是在不同的坐標(biāo)系中表示的，為了實(shí)現(xiàn)對裝填平臺的控制，應(yīng)使上述速度之間形成閉環(huán)的轉(zhuǎn)換關(guān)系。

記相機(jī)坐標(biāo)系與動平臺坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為PRC，則相機(jī)坐標(biāo)系與靜平臺坐標(biāo)系即世界坐標(biāo)系之間的轉(zhuǎn)換關(guān)系為

(13)

通過視覺伺服得到的相機(jī)控制速度是在相機(jī)坐標(biāo)系中表示的，由于動平臺與推彈機(jī)構(gòu)、攝像機(jī)固連，因此相機(jī)的控制速度即是動平臺的控制速度，將其轉(zhuǎn)換到世界坐標(biāo)系中

(14)

在裝填平臺的運(yùn)動學(xué)分析中，動平臺的轉(zhuǎn)動速度是用Roll-Pitch-Yaw表示的，所以應(yīng)進(jìn)一步將式(14)得到的速度中的轉(zhuǎn)動速度分量轉(zhuǎn)換成角速度分量，轉(zhuǎn)換關(guān)系為

(15)

其中[11]

(16)

至此綜合式(8)、式(12)、式(14)、式(15)可以得到由圖像特征速度和液壓支腿伸縮速度的轉(zhuǎn)換關(guān)系

(17)

4 仿真計(jì)算

根據(jù)自動對準(zhǔn)流程以及裝填平臺的運(yùn)動學(xué)模型，建立了裝填平臺對準(zhǔn)流程的數(shù)學(xué)模型，如圖5所示。

圖5 對準(zhǔn)流程仿真計(jì)算模型簡圖

仿真計(jì)算時假設(shè)推彈機(jī)構(gòu)已運(yùn)動至某一裝填位置附近，以靜坐標(biāo)系為世界坐標(biāo)系，液壓支腿的初始長度均為0.65 m，其最大長度為1 m，并給定上、下鉸點(diǎn)在各自平臺坐標(biāo)系中的坐標(biāo)以及攝像機(jī)相對動平臺的坐標(biāo)位置。在發(fā)射裝置上選取4個特征點(diǎn)作為圖像特征且特征點(diǎn)在一個平面內(nèi)，模擬給出了實(shí)際情況下相機(jī)的初始位姿和圖像特征的空間位置，如圖6所示。在初始姿態(tài)下，可以得到當(dāng)前圖像特征的像素坐標(biāo)為

圖6 攝像機(jī)及特征點(diǎn)初始位姿

當(dāng)裝填平臺完成對準(zhǔn)時，攝像機(jī)獲取的期望的圖像特征像素坐標(biāo)為

令λ=3，經(jīng)仿真計(jì)算可以得到6個液壓支腿的運(yùn)動的速度如圖7所示，支腿長度變化如圖8所示。圖9給出了攝像機(jī)成像平面下點(diǎn)特征的運(yùn)動軌跡，其中圓形表示特征點(diǎn)初始位置，五角形表示特征點(diǎn)期望位置。

圖7 液壓支腿速度變化曲線

圖8 液壓支腿長度變化曲線

圖9 特征點(diǎn)運(yùn)動軌跡

仿真結(jié)果分析：

1) 液壓支腿運(yùn)動速度隨仿真時間逐漸趨于零，表明裝填平臺可以按照設(shè)定好的比例控制規(guī)律運(yùn)動。

2) 從攝像機(jī)成像平面可以看出特征點(diǎn)在向期望的位置運(yùn)動，表明裝填平臺可以實(shí)現(xiàn)對準(zhǔn)過程。

3) 理想裝填情況下，特征點(diǎn)所在平面應(yīng)與相機(jī)坐標(biāo)系的Z軸垂直，但實(shí)際情況中存在發(fā)射裝置復(fù)位誤差以及艦面不平等因素，因此仿真時使發(fā)射裝置上特征點(diǎn)所在的平面位置與理想位置具有大于實(shí)際的位姿差異，這從初始姿態(tài)下圖像特征的像素坐標(biāo)可以看出。仿真結(jié)果表明在此時的裝填位置下液壓支腿的長度變化符合預(yù)期，且沒有超過其最大長度，結(jié)合自動對準(zhǔn)流程可以認(rèn)為在其他裝填位置也可以實(shí)現(xiàn)對準(zhǔn)過程。

5 結(jié)論

將無標(biāo)定的圖像視覺伺服技術(shù)應(yīng)用于六自由度并聯(lián)結(jié)構(gòu)形式的導(dǎo)彈裝填平臺上，結(jié)合裝填平臺的運(yùn)動學(xué)模型，根據(jù)圖像特征偏差建立了伺服控制方法，仿真計(jì)算表明，該方法能夠?qū)崿F(xiàn)裝填平臺的姿態(tài)調(diào)整和自動對準(zhǔn)過程。