運(yùn)動障礙環(huán)境中uuv三維航路規(guī)劃方法
【專利摘要】本發(fā)明涉及一種運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于：步驟一：通過計(jì)算UUV遭遇固定障礙與運(yùn)動障礙的時(shí)間，建立UUV三維避障代價(jià)函數(shù)；步驟二：通過計(jì)算UUV艏向與目標(biāo)方向角度關(guān)系，建立UUV三維趨向目標(biāo)代價(jià)函數(shù)；步驟三：結(jié)合UUV三維避障代價(jià)函數(shù)和三維趨向目標(biāo)代價(jià)函數(shù)，建立UUV三維航路規(guī)劃代價(jià)函數(shù)。
【專利說明】運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種運(yùn)動障礙環(huán)境中無人水下航行器(Unmannedunderwatervehicle, UUV)三維航路規(guī)劃方法。
【背景技術(shù)】
[0002]隨著海洋經(jīng)濟(jì)的發(fā)展,科學(xué)技術(shù)的進(jìn)步，UUV (Unmanned underwater vehicle)在海洋環(huán)境探測、資源開采、軍事干擾與偵察等方面的作用越來越重要，逐漸成為研究熱點(diǎn)。海洋環(huán)境十分復(fù)雜，既有海底山脈、焦巖、沉船、暗樁等固定障礙，又有艦艇等運(yùn)動障礙威脅。如何尋找一條從起點(diǎn)到目標(biāo)點(diǎn)優(yōu)化的航路，既能避開障礙與威脅又能高效的完成勘測與偵察任務(wù)，成為UUV安全、高效作業(yè)的首要問題。
[0003]國內(nèi)外眾多學(xué)者對航路規(guī)劃問題進(jìn)行了深入的研究，提出了許多有效的航路規(guī)劃方法，比如A*算法、D*算法、人工勢場算法等，獲得了較好的應(yīng)用效果。大多數(shù)的傳統(tǒng)航路規(guī)劃算法，需要對地圖進(jìn)行柵格化，柵格大小的選擇會影響到解的質(zhì)量，隨著地圖復(fù)雜度的增加，需要較大的存儲空間。傳統(tǒng)的人工勢場算法容易陷入局部極小值，當(dāng)機(jī)器人試圖穿過兩相鄰很近的障礙物時(shí)，兩障礙物產(chǎn)生的斥力分別為F' ^和!^ r2，斥力的合力Fr方向?yàn)楸畴x障礙物方向。如果此時(shí)引力小于斥力，機(jī)器人將遠(yuǎn)離目標(biāo)點(diǎn)運(yùn)動；另一種情況，當(dāng)引力大于或者近似等于斥力時(shí)，當(dāng)機(jī)器人靠近障礙物時(shí)，機(jī)器人與障礙物可能發(fā)生碰撞或者停滯不前。機(jī)器人無法到達(dá)目標(biāo)點(diǎn)。當(dāng)目標(biāo)點(diǎn)在障礙物產(chǎn)生的斥力作用范圍內(nèi)時(shí)，如果斥力大于引力時(shí)，合力方向會背離目標(biāo)點(diǎn)，機(jī)器人遠(yuǎn)離目標(biāo)點(diǎn)運(yùn)動。在這種情況下，目標(biāo)點(diǎn)不是總勢場最小的點(diǎn)，即使機(jī)器人和目標(biāo)點(diǎn)之間沒有障礙物，機(jī)器人也無法運(yùn)動到目標(biāo)點(diǎn)，只會在實(shí)際勢場最小的點(diǎn)位置附近振蕩。
【發(fā)明內(nèi)容】
[0004]本發(fā)明目的在于提供一種運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，能夠有效避開障礙，實(shí)現(xiàn)到達(dá)目標(biāo)點(diǎn)的優(yōu)化航路規(guī)劃。
[0005]實(shí)現(xiàn)本發(fā)明目的技術(shù)方案:
[0006]一種運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于:
[0007]步驟一:通過計(jì)算UUV遭遇固定障礙與運(yùn)動障礙的時(shí)間，建立UUV三維避障代價(jià)函數(shù)；
[0008]步驟二:通過計(jì)算UUV艏向與目標(biāo)方向角度關(guān)系，建立UUV三維趨向目標(biāo)代價(jià)函數(shù)；
[0009]步驟三:結(jié)合UUV三維避障代價(jià)函數(shù)和三維趨向目標(biāo)代價(jià)函數(shù)，建立UUV三維航路規(guī)劃代價(jià)函數(shù)。
[0010]步驟一中，避碰固定障礙代價(jià)函數(shù)為:
【權(quán)利要求】
1.一種運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于: 步驟一:通過計(jì)算UUV遭遇固定障礙與運(yùn)動障礙的時(shí)間，建立UUV三維避障代價(jià)函數(shù)； 步驟二:通過計(jì)算UUV艏向與目標(biāo)方向角度關(guān)系，建立UUV三維趨向目標(biāo)代價(jià)函數(shù)； 步驟三:結(jié)合UUV三維避障代價(jià)函數(shù)和三維趨向目標(biāo)代價(jià)函數(shù)，建立UUV三維航路規(guī)劃代價(jià)函數(shù)。
2.根據(jù)權(quán)利要求1所述的運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于:步驟一中，避碰固定障礙代價(jià)函數(shù)為:
3.根據(jù)權(quán)利要求2所述的運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于:步驟一中，假設(shè)，當(dāng)前點(diǎn)Pi在候選點(diǎn)P' i+1方向延長線與運(yùn)動障礙方向延長線的最近交互點(diǎn)為M，運(yùn)動障礙前行延長線上的最近交互點(diǎn)為M'，h*M與M'點(diǎn)的距離，則
4.根據(jù)權(quán)利要求3所述的運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于:步驟二中,三維趨向目標(biāo)代價(jià)函數(shù)為，
5.根據(jù)權(quán)利要求4所述的運(yùn)動障礙環(huán)境中UUV三維航路規(guī)劃方法，其特征在于:步驟三中，三維航路規(guī)劃代價(jià)函數(shù)為，
f=k1f1+k2f2-k3f3 其中h為權(quán)重系數(shù)(kpo)。
【文檔編號】G05D1/10GK103777639SQ201410011297
【公開日】2014年5月7日 申請日期:2014年1月10日 優(yōu)先權(quán)日:2014年1月10日
【發(fā)明者】嚴(yán)浙平, 鄧超, 李聰聰, 趙玉飛, 徐健, 陳濤 申請人:哈爾濱工程大學(xué)