[發(fā)明專利]無人船測繪航行系統(tǒng)及其控制方法

在審

• 申請?zhí)枺?/td>: 202111401223.5
• 申請日: 2021-11-19
• 公開（公告）號: CN114115264A
• 公開（公告）日: 2022-03-01
• 發(fā)明（設(shè)計）人: 邵宇平;張呈;桂小宇;劉淳;楊詠林
• 申請（專利權(quán)）人: 四方智能（武漢）控制技術(shù)有限公司;北京四方繼保自動化股份有限公司
• 主分類號: G05D1/02
• 分類號: G05D1/02;G01S13/86;G01S13/89;G01S17/89;G01S19/41;G01S19/47
• 代理公司: 北京易捷勝知識產(chǎn)權(quán)代理事務(wù)所(普通合伙) 11613
• 代理人: 齊勝杰

1.一種無人船測繪航行系統(tǒng)，其特征在于，包括定位模塊、決策控制單元、運(yùn)動控制單元、環(huán)境感知單元、海事雷達(dá)、激光雷達(dá)和毫米波雷達(dá)；

所述定位模塊接收導(dǎo)航設(shè)備數(shù)據(jù)，獲取無人船的位姿狀態(tài)，位姿狀態(tài)包括所述無人船的經(jīng)緯度、高度、航速、航向和轉(zhuǎn)向率；

所述決策控制單元根據(jù)計劃線、定位信息以及感知的水岸分界線信息，規(guī)劃目標(biāo)軌跡，并傳輸至所述運(yùn)動控制單元；

所述運(yùn)動控制單元接收所述決策控制單元發(fā)送的無人船的目標(biāo)軌跡，結(jié)合所述定位模塊計算的所述位姿信息，實(shí)時計算無人船所需動力分配并執(zhí)行反饋，控制執(zhí)行機(jī)構(gòu)輸出合適的無人船前進(jìn)推力及轉(zhuǎn)向力，驅(qū)動無人船按目標(biāo)軌跡運(yùn)動；

所述環(huán)境感知單元接收的傳感器數(shù)據(jù)包括：海事雷達(dá)回波圖像信息、激光雷達(dá)三維點(diǎn)云信息和毫米波雷達(dá)回波分布，所述環(huán)境感知單元結(jié)合定位模塊位姿數(shù)據(jù)，計算無人船附近水岸線分界線在大地坐標(biāo)系中二維投影，并生成多點(diǎn)線段包絡(luò)水岸分界線。

2.根據(jù)權(quán)利要求1所述的無人船測繪航行系統(tǒng)，其特征在于，所述定位模塊接收多系統(tǒng)衛(wèi)星導(dǎo)航信號，并采用定點(diǎn)RTK或網(wǎng)絡(luò)差分實(shí)時修正定位精度，并結(jié)合慣性傳感器采集的多軸加速度、角速度的信息，深度耦合后輸出無人船的位置、航向、速度、轉(zhuǎn)向率。

3.一種利用權(quán)利要求1所述的無人船測繪航行系統(tǒng)的控制方法，其特征在于，包括如下步驟：

S1、所述決策控制單元實(shí)時計算無人船的當(dāng)前位置與附近所有水岸分界線的最近距離；

S2、所述決策控制單元實(shí)時計算無人船的目標(biāo)航線與附近水岸分界線的穿越關(guān)系；

S3、所述決策控制單元判斷無人船距水岸分界線距離小于安全距離，且目標(biāo)航線穿越水岸分界線，決策所述控制單元放棄當(dāng)前目標(biāo)航點(diǎn)，執(zhí)行后續(xù)任務(wù)；

S4、所述決策控制單元判斷無人船距水岸分界線距離小于安全距離，且目標(biāo)航線不穿越水岸分界線，所述決策控制單元規(guī)劃遠(yuǎn)離岸線軌跡，驅(qū)動無人船向目標(biāo)位置航行。

4.根據(jù)權(quán)利要求3所述的無人船測繪航行系統(tǒng)的控制方法，其特征在于，所述步驟S1具體為：

S101、所述決策控制單元獲取無人船實(shí)時位置；

S102、計算無人船位置到水岸分界線各線段距離；

S103、當(dāng)無人船的當(dāng)前位置到線段投影落在線段內(nèi)，將投影距離記為到水岸分界線各線段距離；

S104、當(dāng)無人船的當(dāng)前位置到線段投影落在線段外，將位置到線段兩端點(diǎn)距離的較小值記為到此線段距離；

S105、所述決策控制單元尋找無人船到各線段距離的最小值，記為無人船到水岸分界線距離。

5.根據(jù)權(quán)利要求3所述的無人船測繪航行系統(tǒng)的控制方法，其特征在于，所述步驟S2具體為：

S201、所述決策控制單元獲取無人船的目標(biāo)位置，與當(dāng)前位置連線形成目標(biāo)航線；

S202、計算目標(biāo)航線與水岸分界線各線段穿越關(guān)系，存在至少一條線段與目標(biāo)航線為穿越關(guān)系，判定目標(biāo)航線與水岸分界線存在穿越。

6.根據(jù)權(quán)利要求3所述的無人船測繪航行系統(tǒng)的控制方法，其特征在于，所述步驟S3具體為：

S301、所述決策控制單元識別無人船的當(dāng)前位置與水岸分界線距離小于設(shè)定的安全閾值時，啟動水岸分界線保護(hù)子程序，其中安全閾值依據(jù)無人船回轉(zhuǎn)半徑設(shè)定；

S302、目標(biāo)航線與水岸分界線存在穿越關(guān)系，判定當(dāng)前目標(biāo)位置無人船不可到達(dá)，放棄當(dāng)前目標(biāo)航線，繼續(xù)執(zhí)行后續(xù)目標(biāo)位置導(dǎo)航控制。

7.根據(jù)權(quán)利要求3所述的無人船測繪航行系統(tǒng)的控制方法，其特征在于，所述步驟S4具體為：

S401、所述決策控制單元識別無人船的當(dāng)前位置與水岸分界線距離小于設(shè)定的安全閾值時，啟動水岸分界線保護(hù)子程序；

S402、目標(biāo)航線與水岸分界線不存在穿越關(guān)系，無人船的當(dāng)前目標(biāo)位置可以到達(dá)，保護(hù)子程序?qū)崟r規(guī)劃運(yùn)動方向，向目標(biāo)航點(diǎn)運(yùn)動，同時無人船保持與水岸分界線安全距離；

S403、計算目標(biāo)軌跡航向，判定目標(biāo)軌跡與水岸分界線距離小于安全閾值，調(diào)整目標(biāo)軌跡，使無人船與水岸分界線距離始終大于安全閾值。