基于視覺伺服的并聯(lián)裝填平臺(tái)自動(dòng)對(duì)準(zhǔn)方法

趙修平，齊嘉興

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

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

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

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

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

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

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

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

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

動(dòng)平臺(tái)坐標(biāo)系和靜平臺(tái)坐標(biāo)系建立在上下平臺(tái)中心，見圖1，符合右手坐標(biāo)系規(guī)則。世界坐標(biāo)系與靜平臺(tái)坐標(biāo)系重合，相機(jī)坐標(biāo)系建立在攝像機(jī)中心，如圖2所示，其x軸正向與動(dòng)平臺(tái)y軸負(fù)方向相同，z軸正向與動(dòng)平臺(tái)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 裝填平臺(tái)運(yùn)動(dòng)學(xué)模型建立

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

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

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

(2)

其中，s(·)=sin(·)，c(·)=cos(·)。則動(dòng)平臺(tái)相對(duì)靜平臺(tái)的位姿可以表示為

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

(3)

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

li=t+BRPai-bi

(4)

故液壓支腿的長(zhǎng)度為

(5)

(6)

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

(7)

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

(8)

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

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

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

(9)

從迭代式可以看出Newton法的關(guān)鍵是構(gòu)造動(dòng)平臺(tái)位姿的函數(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)每個(gè)采樣周期均需要正向運(yùn)動(dòng)學(xué)的解算，對(duì)算法的實(shí)時(shí)性要求較高，因此在裝填平臺(tái)的正向運(yùn)動(dòng)學(xué)中應(yīng)用改進(jìn)的Jacobian矩陣方法，結(jié)合式(9)，該算法的流程框圖如圖3所示。

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

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

3.1 基于圖像的視覺伺服

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

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

(10)

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

(11)

3.2 自動(dòng)對(duì)準(zhǔn)流程

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

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

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

(12)

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

圖4 裝填平臺(tái)自動(dòng)對(duì)準(zhǔn)流程框圖

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

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

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

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

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

(13)

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

(14)

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

(15)

其中[11]

(16)

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

(17)

4 仿真計(jì)算

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

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

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

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

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

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

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

圖8 液壓支腿長(zhǎng)度變化曲線

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

仿真結(jié)果分析：

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

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

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

5 結(jié)論

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