本發(fā)明涉及仿河貍機(jī)器人智能控制，特別涉及一種多模態(tài)傳感器融合的仿河貍機(jī)器人航行路徑控制方法及系統(tǒng)。

背景技術(shù)：

1、水下仿河貍機(jī)器人在海洋探測(cè)、生態(tài)監(jiān)測(cè)、水下作業(yè)等領(lǐng)域應(yīng)用廣泛。仿河貍水下仿河貍機(jī)器人主要由控制系統(tǒng)、排水浮力系統(tǒng)、舵機(jī)驅(qū)動(dòng)系統(tǒng)、聲吶、傳感器等幾部分組成，主要用于研究海洋中生命現(xiàn)象、過程、規(guī)律及開展救援、打撈和軍事應(yīng)用等方面。

2、然而，現(xiàn)有的水下仿河貍機(jī)器人在復(fù)雜水下環(huán)境中面臨諸多挑戰(zhàn)。在環(huán)境感知方面，傳統(tǒng)水下仿河貍機(jī)器人常采用單一傳感器，如僅依賴聲吶或視覺傳感器，受水下渾濁、光照變化、水流擾動(dòng)等因素影響，無法全面準(zhǔn)確獲取水下障礙物、地形等環(huán)境信息以及仿河貍機(jī)器人自身姿態(tài)信息，導(dǎo)致環(huán)境感知存在盲區(qū)和誤差；在路徑規(guī)劃上，傳統(tǒng)算法未充分考慮水下動(dòng)態(tài)變化因素，在運(yùn)動(dòng)控制環(huán)節(jié)，傳統(tǒng)的控制器難以適應(yīng)水下復(fù)雜多變的水動(dòng)力環(huán)境，當(dāng)仿河貍機(jī)器人受到水流沖擊、姿態(tài)突變等情況時(shí)，無法快速準(zhǔn)確調(diào)整，易出現(xiàn)超調(diào)、振蕩或長(zhǎng)時(shí)間偏離預(yù)定路徑的問題，影響仿河貍機(jī)器人的航行精度和穩(wěn)定性。

技術(shù)實(shí)現(xiàn)思路

1、本發(fā)明的主要目的為提供一種多模態(tài)傳感器融合的仿河貍機(jī)器人航行路徑控制方法及系統(tǒng)，旨在解決上述背景技術(shù)中提出的技術(shù)問題。

2、本發(fā)明提出一種多模態(tài)傳感器融合的仿河貍機(jī)器人航行路徑控制方法，包括：

3、通過多模態(tài)傳感器組實(shí)時(shí)采集水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)，所述水下環(huán)境數(shù)據(jù)包括水下障礙物的距離及輪廓信息、水下三維點(diǎn)云數(shù)據(jù)和水壓信息，所述仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)信息包括加速度信息、角速度信息和姿態(tài)角信息；

4、對(duì)所述水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)進(jìn)行預(yù)處理，包括去噪、校正和零偏補(bǔ)償；

5、融合所述水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)，構(gòu)建水下環(huán)境動(dòng)態(tài)模型；

6、基于所述水下環(huán)境動(dòng)態(tài)模型，采用改進(jìn)路徑規(guī)劃算法生成全局航行路徑；

7、通過模糊比例積分微分控制器實(shí)時(shí)調(diào)整仿河貍機(jī)器人推力分配。

8、優(yōu)選的，所述通過多模態(tài)傳感器組實(shí)時(shí)采集水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)的步驟包括：

9、通過聲吶模塊獲取水下障礙物的距離及輪廓信息；

10、通過雙目視覺攝像頭獲取水下圖像信息，并生成水下三維點(diǎn)云數(shù)據(jù)；

11、通過深度傳感器獲取仿河貍機(jī)器人的所處水深的水壓信息；

12、通過慣性測(cè)量單元獲取仿河貍機(jī)器人的加速度信息、角速度信息及姿態(tài)角信息；

13、將所述水下障礙物的距離及輪廓信息、所述水下三維點(diǎn)云數(shù)據(jù)、所述水壓信息以及所述加速度信息、角速度信息和姿態(tài)角信息時(shí)間戳對(duì)齊。

14、優(yōu)選的，所述對(duì)所述水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)進(jìn)行預(yù)處理的步驟包括：

15、通過自適應(yīng)濾波算法對(duì)所述水下障礙物的距離及輪廓信息進(jìn)行消除噪聲；

16、對(duì)所述水下圖像信息進(jìn)行畸變校正及特征點(diǎn)匹配，并生成水下稠密三維點(diǎn)云數(shù)據(jù)；

17、對(duì)所述水壓信息進(jìn)行均值濾波，消除瞬時(shí)波動(dòng)；

18、對(duì)所述加速度信息、角速度信息和姿態(tài)角信息進(jìn)行零偏補(bǔ)償；

19、根據(jù)預(yù)處理后的所述水下障礙物的距離及輪廓信息、所述水下圖像信息及水下稠密三維點(diǎn)云數(shù)據(jù)、所述水壓信息和所述加速度信息、角速度信息和姿態(tài)角信息建立數(shù)據(jù)集。

20、優(yōu)選的，所述融合所述水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)，構(gòu)建水下環(huán)境動(dòng)態(tài)模型的步驟包括：

21、對(duì)所述水下障礙物的距離及輪廓信息和所述水下稠密三維點(diǎn)云數(shù)據(jù)進(jìn)行空間配準(zhǔn)，得到統(tǒng)一坐標(biāo)系下的障礙物三維模型；

22、基于障礙物三維模型，采用三維柵格法劃分水下環(huán)境空間，標(biāo)記每個(gè)柵格存儲(chǔ)障礙物的概率值，并隨傳感器數(shù)據(jù)更新動(dòng)態(tài)調(diào)整概率值，得到帶概率值的三維柵格地圖；

23、結(jié)合所述加速度信息、角速度信息和姿態(tài)角信息，對(duì)所述三維柵格地圖進(jìn)行坐標(biāo)系對(duì)齊，消除仿河貍機(jī)器人運(yùn)動(dòng)導(dǎo)致的坐標(biāo)偏移，得到同步仿河貍機(jī)器人實(shí)時(shí)位姿的環(huán)境地圖；

24、獲取所述三維柵格地圖中的移動(dòng)障礙物，根據(jù)所述移動(dòng)障礙物的速度和加速度獲取移動(dòng)障礙物軌跡預(yù)測(cè)信息；

25、根據(jù)所述同步仿河貍機(jī)器人實(shí)時(shí)位姿的環(huán)境地圖和移動(dòng)障礙物軌跡預(yù)測(cè)信息建立水下環(huán)境動(dòng)態(tài)模型。

26、優(yōu)選的，所述基于所述水下環(huán)境動(dòng)態(tài)模型，采用改進(jìn)路徑規(guī)劃算法生成全局航行路徑的步驟包括：

27、根據(jù)水下環(huán)境動(dòng)態(tài)模型獲取仿河貍機(jī)器人的起點(diǎn)位置和目標(biāo)位置；

28、采用雙向搜索機(jī)制，從所述起點(diǎn)位置和目標(biāo)位置同步擴(kuò)展路徑節(jié)點(diǎn)，當(dāng)兩搜索域交匯時(shí)終止計(jì)算，生成初步路徑；

29、通過跳點(diǎn)搜索算法跳過所述初步路徑中無路徑價(jià)值的中間節(jié)點(diǎn)，得到優(yōu)化路徑；

30、設(shè)計(jì)動(dòng)態(tài)啟發(fā)函數(shù)，結(jié)合水流速度矢量調(diào)整所述優(yōu)化路徑的路徑代價(jià)估計(jì)值，得到二次優(yōu)化路徑；

31、采用曲線擬合算法對(duì)所述二次優(yōu)化路徑進(jìn)行平滑處理，消除所述二次優(yōu)化路徑中的鋸齒狀軌跡，得到平滑無鋸齒的全局航行路徑。

32、優(yōu)選的，所述通過模糊比例積分微分控制器實(shí)時(shí)調(diào)整推進(jìn)器推力分配的步驟包括：

33、獲取仿河貍機(jī)器人與所述全局航行路徑的橫向偏差，并計(jì)算所述橫向偏差變化率；

34、定義模糊邏輯規(guī)則，其輸入變量為所述橫向偏差及所述橫向偏差變化率，輸出變量為比例、積分、微分增益的調(diào)整量；

35、建立模糊規(guī)則庫，根據(jù)所述橫向偏差的大小及所述橫向偏差變化率的變化趨勢(shì)動(dòng)態(tài)調(diào)整控制參數(shù)；

36、采用重心法解模糊，將所述模糊邏輯規(guī)則的輸出轉(zhuǎn)換為實(shí)際增益調(diào)整量；

37、基于所述實(shí)際增益調(diào)整量，根據(jù)推力分配策略調(diào)節(jié)仿河貍機(jī)器人的推進(jìn)器轉(zhuǎn)速和后腿運(yùn)動(dòng)速度，并引入抗飽和機(jī)制防止所述推進(jìn)器和所述后腿超限運(yùn)行。

38、本發(fā)明還公布了一種多模態(tài)傳感器融合的仿河貍機(jī)器人航行路徑控制系統(tǒng)，包括：

39、數(shù)據(jù)采集模塊，用于通過多模態(tài)傳感器組實(shí)時(shí)采集水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)；

40、預(yù)處理模塊，用于對(duì)所述水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)進(jìn)行預(yù)處理；

41、融合計(jì)算模塊，用于融合所述水下環(huán)境數(shù)據(jù)和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)，并構(gòu)建水下環(huán)境動(dòng)態(tài)模型；

42、路徑規(guī)劃模塊，用于基于所述水下環(huán)境動(dòng)態(tài)模型，采用改進(jìn)路徑規(guī)劃算法生成全局航行路徑；

43、運(yùn)動(dòng)控制模塊，通過模糊比例積分微分控制器實(shí)時(shí)調(diào)整仿河貍機(jī)器人推力分配。

44、優(yōu)選的，所述路徑規(guī)劃模塊包括：

45、環(huán)境感知定位單元，根據(jù)水下環(huán)境動(dòng)態(tài)模型獲取仿河貍機(jī)器人的當(dāng)前位置和目標(biāo)位置；

46、雙向路徑擴(kuò)展單元，采用雙向搜索機(jī)制，從所述當(dāng)前位置和目標(biāo)位置同步擴(kuò)展路徑節(jié)點(diǎn)，當(dāng)兩搜索域交匯時(shí)終止計(jì)算，生成初步路徑；

47、跳點(diǎn)路徑優(yōu)化單元，通過跳點(diǎn)搜索算法跳過所述初步路徑中無路徑價(jià)值的中間節(jié)點(diǎn)，得到優(yōu)化路徑；

48、二次優(yōu)化單元，設(shè)計(jì)動(dòng)態(tài)啟發(fā)函數(shù)，結(jié)合水流速度矢量調(diào)整所述優(yōu)化路徑的路徑代價(jià)估計(jì)值，得到二次優(yōu)化路徑；

49、路徑軌跡平滑單元，采用曲線擬合算法對(duì)所述二次優(yōu)化路徑進(jìn)行平滑處理，消除所述二次優(yōu)化路徑中的鋸齒狀軌跡，得到平滑無鋸齒的全局航行路徑。

50、本發(fā)明還公布了一種計(jì)算機(jī)設(shè)備，包括存儲(chǔ)器和處理器，所述存儲(chǔ)器存儲(chǔ)有計(jì)算機(jī)程序，其特征在于，所述處理器執(zhí)行所述計(jì)算機(jī)程序時(shí)實(shí)現(xiàn)一種多模態(tài)傳感器融合的仿河貍機(jī)器人航行路徑控制方法的步驟。

51、本發(fā)明還公布了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序，其特征在于，所述計(jì)算機(jī)程序被處理器執(zhí)行時(shí)實(shí)現(xiàn)一種多模態(tài)傳感器融合的仿河貍機(jī)器人航行路徑控制方法的步驟。

52、本發(fā)明的有益效果為：本發(fā)明通過多模態(tài)傳感器融合技術(shù)，集成聲吶、雙目視覺攝像頭、深度傳感器、慣性測(cè)量單元等多種傳感器，實(shí)現(xiàn)水下環(huán)境和仿河貍機(jī)器人運(yùn)動(dòng)姿態(tài)數(shù)據(jù)的全面采集，并通過數(shù)據(jù)預(yù)處理、時(shí)間戳對(duì)齊等手段，提升數(shù)據(jù)的準(zhǔn)確性和同步性，有效解決了傳統(tǒng)單一傳感器感知不全面的問題，使障礙物檢測(cè)率大幅提升，環(huán)境感知精度顯著提高。

53、在路徑規(guī)劃方面，采用雙向搜索、跳點(diǎn)搜索、動(dòng)態(tài)啟發(fā)函數(shù)以及曲線擬合等算法，充分考慮水流等動(dòng)態(tài)因素，能快速生成全局最優(yōu)且平滑的航行路徑，相比傳統(tǒng)算法，路徑規(guī)劃時(shí)間大幅縮短，路徑長(zhǎng)度顯著減小，同時(shí)有效降低了能耗，提高了仿河貍機(jī)器人在復(fù)雜水下環(huán)境中的航行效率和安全性。

54、在運(yùn)動(dòng)控制上，運(yùn)用模糊?pid?控制技術(shù)，依據(jù)仿河貍機(jī)器人與路徑的橫向偏差及偏差變化率，實(shí)時(shí)動(dòng)態(tài)調(diào)整?pid?控制參數(shù)，精準(zhǔn)調(diào)節(jié)推進(jìn)器推力分配和仿河貍機(jī)器人運(yùn)動(dòng)速度，并引入抗飽和機(jī)制保障設(shè)備安全。這使得仿河貍機(jī)器人能夠快速、穩(wěn)定地跟蹤規(guī)劃路徑，路徑跟蹤誤差大幅降低，有效提升了仿河貍機(jī)器人在復(fù)雜水下環(huán)境中的航行精度和穩(wěn)定性，增強(qiáng)了系統(tǒng)的可靠性。