基于北斗導(dǎo)航的機(jī)器人彈道曲線航道導(dǎo)航系統(tǒng)

李 峰,岳 云,張躍進(jìn),張雪松,朱文慧

(1.鄭州經(jīng)貿(mào)學(xué)院 智慧制造學(xué)院,河南 鄭州 450007;2.鄭州宇通集團(tuán)有限公司,河南 鄭州 450007;3.鄭州經(jīng)貿(mào)學(xué)院 公共教學(xué)部,河南 鄭州 450007;4.中原工學(xué)院 機(jī)電學(xué)院,河南 鄭州 450007)

現(xiàn)在戰(zhàn)爭(zhēng)的主要武器之一是導(dǎo)彈,對(duì)導(dǎo)彈的導(dǎo)航、控制,主要是通過對(duì)其彈道曲線進(jìn)行控制。彈道曲線是拋物體在空中飛行過程中所經(jīng)過的軌跡、路徑。在理想的情況下,可以用彈道曲線控制各種飛行器的有效運(yùn)動(dòng)和打擊范圍。通過控制彈道曲線,有效保證彈頭落點(diǎn)的密集度、空間的有效性,使導(dǎo)彈準(zhǔn)確可靠地完成空中飛行動(dòng)作,飛抵最終目標(biāo)。因此,在飛行器控制領(lǐng)域,彈道曲線研究是目前的研究熱點(diǎn)。相關(guān)研究者通過彈道曲線研究深入分析了機(jī)動(dòng)彈道及制導(dǎo)方法,為將彈道曲線應(yīng)用于飛行器控制打下了良好基礎(chǔ)[1-5]。

隨著科技的發(fā)展,機(jī)器人也越來越成為戰(zhàn)爭(zhēng)的主要武器。美國(guó)的機(jī)器人戰(zhàn)士”Atlas”,日本的機(jī)器人士兵“機(jī)動(dòng)戰(zhàn)士”,俄羅斯的軍用仿人機(jī)器人“FEDOR”都已具備一定的實(shí)戰(zhàn)能力[6-8]。很多國(guó)家現(xiàn)在在機(jī)器人士兵方面投入了大量的人力、物力和財(cái)力,機(jī)器人導(dǎo)航控制也越來越成為研究的重點(diǎn)。文獻(xiàn)[9-13]以不同方法和媒介研究了機(jī)器人的導(dǎo)航控制,但將彈道曲線應(yīng)用于機(jī)器人,特別是人形機(jī)器人的導(dǎo)航控制尚是空白,而將比較成熟的彈道曲線控制應(yīng)用于機(jī)器人控制,提高機(jī)器人的智能化程度,優(yōu)勢(shì)非常明顯。一般人形機(jī)器人到達(dá)預(yù)定區(qū)域主要通過激光、地磁、地圖等方法導(dǎo)航,多以直線航道方式,行動(dòng)方式比較單一,而人形機(jī)器人要成功到達(dá)預(yù)定區(qū)域完成任務(wù),并成功保護(hù)自己,躲避敵方打擊,就需要以不同的航道和導(dǎo)航方式來實(shí)現(xiàn),因此要提高人形機(jī)器人導(dǎo)航的智能化程度。本文以人形機(jī)器人彈道曲線航道導(dǎo)航為研究重點(diǎn),通過北斗系統(tǒng)國(guó)家2000坐標(biāo)下機(jī)器人導(dǎo)航實(shí)驗(yàn),成功地實(shí)現(xiàn)了人形機(jī)器人的彈道曲線航道導(dǎo)航控制,極大地提高了人形機(jī)器人導(dǎo)航控制的智能化程度。

1 人形機(jī)器人彈道曲線航道北斗導(dǎo)航模型

1.1 人形機(jī)器人運(yùn)動(dòng)學(xué)模型

圖1為機(jī)器人運(yùn)動(dòng)學(xué)模型。與人形機(jī)器人雙足中心連線垂直的中心線為yr,大地的正北方向?yàn)閥。人形機(jī)器人運(yùn)動(dòng)時(shí),雙足與縱軸yr之間α為機(jī)器人轉(zhuǎn)向角,機(jī)器人坐標(biāo)系中yr軸與大地坐標(biāo)系y軸之間的夾角ψ為機(jī)器人現(xiàn)在所處位置的姿態(tài)角。圖中,O0(x0,y0)為人形機(jī)器人初始坐標(biāo),Ot(xt,yt)為終點(diǎn)坐標(biāo),O0Ot與y之間的夾角ψh為人形機(jī)器人此位置的航向角。

圖1 機(jī)器人運(yùn)動(dòng)學(xué)模型

如果機(jī)器人達(dá)到(xt,yt)處后有姿態(tài)ψ′要求,機(jī)器人到達(dá)目的地后還需要進(jìn)行姿態(tài)調(diào)整。O0與Ot兩點(diǎn)距離為s,如果人形機(jī)器人行進(jìn)速度為v,轉(zhuǎn)動(dòng)角速度為ω,則有:

ψh=-arctan((xt-x0)/(yt-y0))

(1)

式中:

yt=y0+scosψh=y0+vΔtcosψh

(2)

xt=x0+ssinψh=x0+vΔtsinψh

(3)

ψh=ψ+α=ψ+ωΔt

(4)

1.2 彈道曲線航道模型

圖2為人形機(jī)器人彈道曲線航道變換模型。圖2中曲線1為普通彈道曲線,從圖中可以看出成拋物線樣式,左右對(duì)稱。但機(jī)器人遇到特殊環(huán)境和突發(fā)情況時(shí),人形機(jī)器人若需從所在點(diǎn)到任務(wù)目標(biāo)點(diǎn)確定一條基于彈道曲線的航道,安全完成任務(wù),則需對(duì)特定曲線1進(jìn)行變換。如圖2,可通過將彈道曲線先旋轉(zhuǎn),后平移,將曲線1旋轉(zhuǎn)為曲線2,后移動(dòng)至曲線3,使曲線3通過所在位置和終點(diǎn)位置。機(jī)器人彈道曲線航道及導(dǎo)航模型參數(shù)的確定需要大量的計(jì)算,具體見人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法設(shè)計(jì)。

圖2 人形機(jī)器人彈道曲線航道變換模型

1.3 人形機(jī)器人彈道曲線航道北斗導(dǎo)航模型

圖3 人形機(jī)器人彈道曲線航道北斗導(dǎo)航模型

(5)

式中:nl(i)為第i段內(nèi)可繼續(xù)差分nl(i)次,每次位移Δl。

從第i-1段到第i段需進(jìn)行航向調(diào)整,人形機(jī)器人轉(zhuǎn)向角為

Δα(i)=nα(i)Δθ

(6)

式中:nα(i)表示第i段轉(zhuǎn)向角可繼續(xù)差分nα(i)次,每次Δθ。

2 人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法設(shè)計(jì)

2.1 人形機(jī)器人彈道曲線方程確定

彈道曲線參數(shù)方程為

(7)

式中:vr,ξ為參數(shù)項(xiàng);g為常數(shù)項(xiàng);t(i)為自變量。則圖2中曲線1所示人形機(jī)器人彈道曲線方程為

(8)

令:

(9)

2.2 初始條件

(10)

(11)

rtx(n)=vr·t(n)·cosξ·rtcosζ-vr·t(n)·sinξ·
rtsinζ+[g·(t(n))2·rtsinζ/2]+rtχ

(12)

(13)

2.3 關(guān)鍵參數(shù)確定

(14)

所以:

(15)

則有:

(16)

因此:

(17)

(18)

將式(17)和式(18)代入式(10)和式(11):

(19)

(20)

彈道曲線N點(diǎn)差分,則:

t(i)=t(b)+i·[(t(n)-t(b))/(N-1)]

(21)

rtx(i)=vr·t(i)·cosξ·rtcosζ-vr·t(i)·sinξ·
rtsinζ+[(g·(t(i))2·rtsinζ)/2]+rtχ

(22)

(23)

式中:i=1,2,…,N-1。

結(jié)合式(1)～式(6),可完成人形機(jī)器人彈道曲線航道規(guī)劃過程。

3 系統(tǒng)組成與工作原理

根據(jù)圖4導(dǎo)航系統(tǒng)整體架構(gòu),人形機(jī)器人彈道曲線航道北斗導(dǎo)航系統(tǒng)主要由被導(dǎo)航人形機(jī)器人、彈道曲線航道規(guī)劃模塊、人形機(jī)器人位姿規(guī)劃模塊、CPU、控制柜、中間控制器等部分組成。通過北斗導(dǎo)航器得到人形機(jī)器人所在地國(guó)家2000坐標(biāo),通過彈道曲線航道規(guī)劃模塊完成彈道曲線設(shè)定,通過人形機(jī)器人位姿規(guī)劃模塊規(guī)劃人形機(jī)器人彈道曲線航道中各點(diǎn)位姿,通過控制柜、中間控制器及相關(guān)輔助設(shè)備具體完成人形機(jī)器人彈道曲線航道任務(wù)。

圖4 人形機(jī)器人彈道曲線航道北斗導(dǎo)航系統(tǒng)整體架構(gòu)

系統(tǒng)在確定機(jī)器人所在坐標(biāo)及目的坐標(biāo)后,通過彈道曲線航道規(guī)劃模塊自動(dòng)規(guī)劃生成彈道曲線,與普通機(jī)器人導(dǎo)航系統(tǒng)不同,該導(dǎo)航系統(tǒng)生成點(diǎn)對(duì)點(diǎn)的彈道曲線運(yùn)動(dòng)軌跡,而非點(diǎn)對(duì)點(diǎn)的直線運(yùn)動(dòng)軌跡,因此,智能程度更高。在得到彈道曲線后,由人形機(jī)器人位姿規(guī)劃模塊規(guī)劃彈道曲線航道人形機(jī)器人位姿變化,然后通過控制柜、中間控制器等設(shè)備向人形機(jī)器人發(fā)出動(dòng)作指令,控制人形機(jī)器人按照人形機(jī)器人位姿規(guī)劃動(dòng)作,沿彈道曲線到達(dá)目標(biāo)位置,完成任務(wù)。

4 人形機(jī)器人彈道曲線控制過程設(shè)計(jì)

4.1 控制過程設(shè)計(jì)

圖5為人形機(jī)器人彈道曲線航道北斗導(dǎo)航控制系統(tǒng)流程圖。

圖5 人形機(jī)器人彈道曲線航道北斗導(dǎo)航控制系統(tǒng)流程圖

按照?qǐng)D5,將系統(tǒng)初始化后,系統(tǒng)按照人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法設(shè)計(jì)解算彈道曲線主要參數(shù),并生成彈道曲線。彈道曲線生成后,系統(tǒng)結(jié)合圖1人形機(jī)器人運(yùn)動(dòng)學(xué)模型和圖3人形機(jī)器人彈道曲線航道北斗導(dǎo)航模型規(guī)劃人形機(jī)器人動(dòng)作航道和姿態(tài)。接著,人形機(jī)器人按照規(guī)劃從第1坐標(biāo)點(diǎn)(初始位置)調(diào)整姿態(tài),確定1到2點(diǎn)(第1段)航向,運(yùn)動(dòng),到達(dá)第2點(diǎn)后系統(tǒng)內(nèi)部參數(shù)更新,繼續(xù)按照規(guī)劃在第2坐標(biāo)點(diǎn)調(diào)整姿態(tài),確定2到3點(diǎn)(第2段)航向,運(yùn)動(dòng)……直到系統(tǒng)到達(dá)終點(diǎn)N,系統(tǒng)校核位置,自適應(yīng)調(diào)整位姿。系統(tǒng)通過差分彈道曲線,自動(dòng)規(guī)劃人形機(jī)器人動(dòng)作,實(shí)現(xiàn)人形機(jī)器人彈道曲線航道北斗導(dǎo)航,智能化水平高,自適應(yīng)能力強(qiáng)。

4.2 主要運(yùn)動(dòng)控制原理

根據(jù)控制流程,結(jié)合導(dǎo)航模型,采用stl語言將人形機(jī)器人彈道曲線導(dǎo)航模型及導(dǎo)航算法程序化,主要控制原理見圖6和圖7。從圖6彈道曲線規(guī)劃過程可以看出,控制系統(tǒng)在輸入彈道曲線相關(guān)參數(shù)后,按照人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法,對(duì)經(jīng)過初始點(diǎn)、終點(diǎn)的彈道曲線進(jìn)行結(jié)算,并調(diào)用相關(guān)子程序,自動(dòng)生成彈道曲線。

圖6 彈道曲線規(guī)劃過程

圖7 人形機(jī)器人彈道曲線航道北斗導(dǎo)航主程序

圖7為依據(jù)圖5人形機(jī)器人彈道曲線航道北斗導(dǎo)航控制系統(tǒng)流程圖而研發(fā)的人形機(jī)器人彈道曲線航道北斗導(dǎo)航主程序,因彈道曲線導(dǎo)航過程是一個(gè)復(fù)雜的過程,程序中略去了大量參數(shù)傳送和動(dòng)作程序。圖6和圖7結(jié)合圖4人形機(jī)器人彈道曲線航道北斗導(dǎo)航系統(tǒng),即可完成人形機(jī)器人彈道曲線航道北斗導(dǎo)航。

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

5.1 實(shí)驗(yàn)參數(shù)

人形機(jī)器人彈道曲線航道北斗導(dǎo)航控制過程比較復(fù)雜,整個(gè)過程包括彈道計(jì)算、航道規(guī)劃、任務(wù)實(shí)現(xiàn)等,通過實(shí)驗(yàn)一方面驗(yàn)證人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法的正確性,另一方面驗(yàn)證系統(tǒng)的可行性。實(shí)驗(yàn)主要參數(shù)見表1。

表1 實(shí)驗(yàn)主要參數(shù)

5.2 仿真分析與實(shí)驗(yàn)研究

為了檢驗(yàn)人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法的合理性、正確性,根據(jù)表1參數(shù),一方面利用METLAB軟件對(duì)人形機(jī)器人彈道曲線航道北斗導(dǎo)航模型進(jìn)行仿真,另一方面通過導(dǎo)航系統(tǒng)對(duì)模型進(jìn)行實(shí)驗(yàn)驗(yàn)證,仿真及實(shí)驗(yàn)結(jié)果如圖8和圖9所示。圖8中曲線4為彈道曲線仿真結(jié)果,曲線5為人形機(jī)器人彈道曲線導(dǎo)航航道仿真結(jié)果;圖9中曲線6為人形機(jī)器人彈道曲線航道北斗導(dǎo)航實(shí)驗(yàn)結(jié)果。

圖8 彈道曲線與導(dǎo)航航道仿真結(jié)果

圖9 人形機(jī)器人彈道曲線航道北斗導(dǎo)航實(shí)驗(yàn)結(jié)果

圖10為實(shí)驗(yàn)得到的導(dǎo)航軌跡與彈道曲線的誤差圖。

圖10 彈道曲線航道導(dǎo)航位移誤差圖

圖8中,曲線4彈道曲線和曲線5人形機(jī)器人彈道曲線航道接近,這說明導(dǎo)航算法達(dá)到了使人形機(jī)器人彈道曲線航道與彈道曲線接近的目的,可以實(shí)現(xiàn)人形機(jī)器人彈道曲線航道導(dǎo)航。在圖9的導(dǎo)航實(shí)驗(yàn)結(jié)果中,曲線4也逼近曲線6,說明所設(shè)計(jì)的人形機(jī)器人彈道曲線航道導(dǎo)航系統(tǒng)是合理的,可以實(shí)現(xiàn)人形機(jī)器人彈道曲線航道導(dǎo)航目標(biāo)。曲線4與曲線5之間的誤差主要是因?yàn)槿诵螜C(jī)器人本身航向調(diào)整誤差、位移調(diào)整誤差積累的結(jié)果,人形機(jī)器人每信號(hào)調(diào)整角為10°,調(diào)整位移為0.05 m(見表1),導(dǎo)航控制信號(hào)越多,誤差積累越大,后期可通過提高機(jī)器人本身的動(dòng)作精度降低誤差積累。曲線4與曲線6之間誤差一方面是因?yàn)榭刂葡到y(tǒng)誤差,另一方面導(dǎo)航軌跡各處摩擦系數(shù)不同,造成機(jī)器人在個(gè)別位置移動(dòng)結(jié)果差別,從圖10中可以看出導(dǎo)航軌跡與彈道曲線誤差在-0.05～0.1之間,后期可以通過完善實(shí)驗(yàn)環(huán)境,提高人形機(jī)器人腳部摩擦系數(shù)等方法,進(jìn)一步提高人形機(jī)器人導(dǎo)航效果。結(jié)合圖8～圖10可以看出,導(dǎo)航算法及導(dǎo)航系統(tǒng)可以實(shí)現(xiàn)人形機(jī)器人彈道曲線航道北斗導(dǎo)航,有效地提高人形機(jī)器人的智能化程度。

圖11～圖13所示為彈道曲線導(dǎo)航航道理想航向、仿真航向、實(shí)驗(yàn)航向、理想航向與實(shí)際航向誤差。隨著人形機(jī)器人的運(yùn)動(dòng),人形機(jī)器人的航向需要隨著航道及時(shí)變化,以滿足機(jī)器人導(dǎo)航航道要求。

圖11 彈道曲線理想航向與仿真航向變化曲線

圖12 彈道曲線理想航向與實(shí)驗(yàn)航向變化曲線

圖13 彈道曲線航道導(dǎo)航航向誤差圖

理論上,人形機(jī)器人在整個(gè)導(dǎo)航過程中,其航向圍繞理想航向上下波動(dòng),但波動(dòng)范圍在10°左右,這主要是由人形機(jī)器人本身因素造成,人形機(jī)器人每信號(hào)調(diào)整角為10°(見表1),此誤差是不可避免的,后期可以通過提高機(jī)器人每信號(hào)調(diào)整精度,但精度提高又增加了計(jì)算量,增加了導(dǎo)航控制信號(hào)數(shù),提高了控制難度,后期可以通過各種算法對(duì)此進(jìn)行優(yōu)化。從圖13可以看出,航向誤差也維持在10°內(nèi),在-7.5～8.5之間,部分區(qū)域因地面摩擦和規(guī)劃航向變化有較大變化,但整體在誤差范圍內(nèi)。結(jié)合圖11～圖13,再次驗(yàn)證了人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法的正確性和導(dǎo)航系統(tǒng)的可行性。

6 結(jié)束語

將彈道曲線應(yīng)用于人形機(jī)器人導(dǎo)航控制方面,建立了彈道曲線模型和人形機(jī)器人彈道曲線航道北斗導(dǎo)航模型,提出了人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法。根據(jù)導(dǎo)航模型和算法,研發(fā)了一種人形機(jī)器人彈道曲線航道北斗導(dǎo)航系統(tǒng),以stl語言為媒介,將人形機(jī)器人彈道曲線導(dǎo)航模型及導(dǎo)航算法程序化,介紹了機(jī)器人彈道曲線航道北斗導(dǎo)航控制原理。METLAB仿真分析和人形機(jī)器人彈道曲線航道北斗導(dǎo)航實(shí)驗(yàn)研究結(jié)果表明,人形機(jī)器人彈道曲線航道北斗導(dǎo)航算法和導(dǎo)航系統(tǒng)是合理有效的,這為提高人形機(jī)器人的智能化程度,增強(qiáng)人形機(jī)器人在特殊環(huán)境、突發(fā)情況下的自適應(yīng)能力打下了良好基礎(chǔ)。