基于多項(xiàng)式的uuv空間軌跡規(guī)劃方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種能夠規(guī)劃出三維空間曲線的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法。包括以下幾個(gè)步驟：UUV利用傳感器獲得當(dāng)前自身位姿、速度信息作為軌跡規(guī)劃的起點(diǎn)信息，接收軌跡規(guī)劃的終點(diǎn)信息，接收障礙物信息；利用多項(xiàng)式參數(shù)化空間軌跡方程；求解空間軌跡方程的固定系數(shù)；求解空間軌跡方程的可調(diào)系數(shù)的最優(yōu)值；判斷空間軌跡是否滿足無(wú)碰條件，如果滿足無(wú)碰條件，將可調(diào)系數(shù)的最優(yōu)值代入空間軌跡方程，并輸出空間軌跡，如果不滿足無(wú)碰條件，求解可調(diào)系數(shù)的次優(yōu)值，并將可調(diào)系數(shù)的次優(yōu)值代入空間軌跡方程，并輸出空間軌跡。本發(fā)明通過(guò)可調(diào)系數(shù)的選取來(lái)獲得一條空間無(wú)碰的、最優(yōu)的空間曲線軌跡。
【專(zhuān)利說(shuō)明】基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明屬于一種軌跡規(guī)劃方法，尤其涉及一種基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法。
【背景技術(shù)】
[0002]隨著科學(xué)技術(shù)的發(fā)展，海洋資源開(kāi)發(fā)的變化，海洋利用區(qū)域不斷擴(kuò)大，并不斷向深海、遠(yuǎn)海延伸，認(rèn)識(shí)海洋、開(kāi)發(fā)海洋需要各種高技術(shù)手段。作為探索海洋空間的最重要手段之一的無(wú)人水下航行器(UUV)技術(shù)與探索外空間的運(yùn)載火箭技術(shù)有同等重要的意義，它具有活動(dòng)范圍大、潛水深、機(jī)動(dòng)性好、安全、智能化、運(yùn)行和維護(hù)費(fèi)用低等優(yōu)點(diǎn)。但UUV自身攜帶能源有限，工作時(shí)間和距離都受到了限制，需要進(jìn)行水下能源補(bǔ)給和數(shù)據(jù)交換，此時(shí)就必須要求UUV能夠進(jìn)行回收。
[0003]運(yùn)動(dòng)母船水下自主回收UUV的過(guò)程分為三個(gè)典型的階段:集合階段(Rendezvous)、回瑪階段(Homing)和對(duì)接階段(Docking)。集合階段的任務(wù)是空間軌跡規(guī)劃和空間軌跡跟蹤。空間軌跡規(guī)劃是UUV自主回收中的第一步，且軌跡規(guī)劃的好壞直接影響回收過(guò)程的安全性和準(zhǔn)確性。
[0004]目前，國(guó)內(nèi)對(duì)無(wú)人水下航行器軌跡規(guī)劃問(wèn)題做了大量的研究。冉紅閣提出了一種基于FMM法的路徑規(guī)劃算法，F(xiàn)MM法屬于柵格模型規(guī)劃方法，最后得到了不同地圖模型下AUV的軌跡(冉紅閣.自主式水下機(jī)器人的路徑規(guī)劃與路徑跟蹤方法研究[M].中國(guó)海洋大學(xué)，2009)。曹江麗博士提出了一種基于Q學(xué)習(xí)的避碰路徑規(guī)劃方法，采用Q學(xué)習(xí)對(duì)機(jī)器人反應(yīng)式行為進(jìn)行自主學(xué)習(xí)，以產(chǎn)生優(yōu)化的行為，這種方法充分提高了系統(tǒng)運(yùn)行速度(曹江麗.水下機(jī)器人路徑規(guī)劃問(wèn)題的關(guān)鍵技術(shù)研究[D].哈爾濱工程大學(xué)，2009)。吳小平、馮正平等人采用蟻群算法結(jié)合TSP問(wèn)題解決了多AUV軌跡規(guī)劃問(wèn)題，并仿真驗(yàn)證了該方法具有耗時(shí)短、效率高的特點(diǎn)(吳小平，馮正平等.基于蟻群算法的多AUV路徑規(guī)劃仿真研究[J].計(jì)算機(jī)仿真，2009, 26(1):150-153)。李欣、朱大奇在傳統(tǒng)人工勢(shì)場(chǎng)法的基礎(chǔ)上提出了一種改進(jìn)勢(shì)場(chǎng)函數(shù)的AUV軌跡規(guī)劃方法，能夠?qū)崿F(xiàn)AUV對(duì)靜態(tài)、動(dòng)態(tài)障礙物進(jìn)行很好避碰(李欣，朱大奇.基于人工勢(shì)場(chǎng)法的自制水下機(jī)器人路徑規(guī)劃[J].上海海事大學(xué)學(xué)報(bào).2010，31 (2):35-39)。

【發(fā)明內(nèi)容】

[0005]本發(fā)明的目的是提供一種能夠規(guī)劃出三維空間曲線的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法。
[0006]本發(fā)明是通過(guò)以下技術(shù)方案實(shí)現(xiàn)的:
[0007]基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，包括以下幾個(gè)步驟:
[0008]步驟一:UUV利用傳感器獲得當(dāng)前自身位姿、速度信息作為軌跡規(guī)劃的起點(diǎn)信息，起點(diǎn)信息包括初始時(shí)間tQ、初始速度V。、初始位姿qQ = (x0, y0, ζ0, Φ0, θ 0，ψ0),UUV接收軌跡規(guī)劃的終點(diǎn)信息，終點(diǎn)信息包括終點(diǎn)時(shí)間tf、終點(diǎn)速度Vf、終點(diǎn)位姿qf =(xf) yf, zf, φ?; Θ f，Ψ?)，UUV 接收障礙物信息；
[0009]步驟二:利用多項(xiàng)式參數(shù)化空間軌跡方程，得到空間軌跡方程的固定系數(shù)與可調(diào)系數(shù)；
[0010]步驟三:利用軌跡規(guī)劃的起點(diǎn)信息和終點(diǎn)信息求解空間軌跡方程的固定系數(shù)；
[0011]步驟四:建立軌跡優(yōu)化的性能指標(biāo)，求解空間軌跡方程的可調(diào)系數(shù)的最優(yōu)值；
[0012]步驟五:判斷空間軌跡是否滿足無(wú)碰條件，如果滿足無(wú)碰條件，將可調(diào)系數(shù)的最優(yōu)值代入空間軌跡方程，并輸出空間軌跡，如果不滿足無(wú)碰條件，求解可調(diào)系數(shù)的次優(yōu)值，并將可調(diào)系數(shù)的次優(yōu)值代入空間軌跡方程，并輸出空間軌跡。
[0013]本發(fā)明基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法還可以包括:
[0014]1、利用多項(xiàng)式參數(shù)化的空間軌跡方程為:
[0015]X (t) = ao+a^+aat^agt3 [0016]y(x) = b0+b1x+b2x2+b3x3+b4x4
[0017]z (t) = c0+c1t+c2t2+c3t3+c4t4
[0018]其中，x(t)、y(X)、z (t)為空間軌跡的狀態(tài)量，a0, a” a2, a3 ；b0, b” b2, b3 ；c0, C1, c2, C3為空間軌跡方程的固定系數(shù)，b4，C4為空間軌跡方程的可調(diào)系數(shù)。
[0019]2、空間軌跡方程的固定系數(shù)為:
[0020][a0 Sl1 a2 a3] = (B1) Y1
[0021 ] [b0 Id1 b2 b3]T = (B2) (Y2_A2b4)
[0022][c0 C1 C2 c3]T = (B3) (Y3-A3C4)
[0023]其中各向量為:
【權(quán)利要求】
1.基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，其特征在于，包括以下幾個(gè)步驟: 步驟一:uuv利用傳感器獲得當(dāng)前自身位姿、速度信息作為軌跡規(guī)劃的起點(diǎn)信息，起點(diǎn)信息包括初始時(shí)間td、初始速度V(!、初始位姿q(! = (x0, y0) Z0, Φ0, θ 0, ψ0),UUV接收軌跡規(guī)劃的終點(diǎn)信息，終點(diǎn)信息包括終點(diǎn)時(shí)間tf、終點(diǎn)速度Vf、終點(diǎn)位姿qf =(xf, yf, Zf, Φr, θ f，Vf)，UUV 接收障礙物信息； 步驟二:利用多項(xiàng)式參數(shù)化空間軌跡方程，得到空間軌跡方程的固定系數(shù)與可調(diào)系數(shù)； 步驟三:利用軌跡規(guī)劃的起點(diǎn)信息和終點(diǎn)信息求解空間軌跡方程的固定系數(shù)； 步驟四:建立軌跡優(yōu)化的性能指標(biāo)，求解空間軌跡方程的可調(diào)系數(shù)的最優(yōu)值； 步驟五:判斷空間軌跡是否滿足無(wú)碰條件，如果滿足無(wú)碰條件，將可調(diào)系數(shù)的最優(yōu)值代入空間軌跡方程，并輸出空間軌跡，如果不滿足無(wú)碰條件，求解可調(diào)系數(shù)的次優(yōu)值，并將可調(diào)系數(shù)的次優(yōu)值代入空間軌跡方程，并輸出空間軌跡。
2.根據(jù)權(quán)利要求1所述的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，其特征在于:所述的利用多項(xiàng)式參數(shù)化的空間軌跡方程為:
X (t) = a0+a1t+a2t2+a:1t:}
y (X) = b0+b1x+b2x2+b3x3+b4x4
Z (t) = c0+c1t+c2t2+c3t3+c4t4
其中，x(t)、y (X)、z (t)為空間軌跡的狀態(tài)量，aQ，a” a2, a3 ；b0, b” b2, b3 ；c0, C1, c2, C3 為空間軌跡方程的固定系數(shù)，b4，C4為空間軌跡方程的可調(diào)系數(shù)。
3.根據(jù)權(quán)利要求2所述的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，其特征在于:所述的空間軌跡方程的固定系數(shù)為:
4.根據(jù)權(quán)利要求3所述的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，其特征在于:所述的軌跡優(yōu)化的性能指標(biāo)為:
5.根據(jù)權(quán)利要求4所述的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，其特征在于:所述的軌跡優(yōu)化的性能指標(biāo)簡(jiǎn)化為:
6.根據(jù)權(quán)利要求4或5所述的基于多項(xiàng)式的UUV空間軌跡規(guī)劃方法，其特征在于:所述的空間軌跡的無(wú)碰條件為:
【文檔編號(hào)】G05D1/02GK104020770SQ201410264172
【公開(kāi)日】2014年9月3日 申請(qǐng)日期:2014年6月13日 優(yōu)先權(quán)日:2014年6月13日
【發(fā)明者】張偉, 陳海田, 徐達(dá), 張明臣, 嚴(yán)浙平 申請(qǐng)人:哈爾濱工程大學(xué)