[發明專利]一種結合RRT算法與VO算法的無人船路徑規劃方法

在審

• 申請號: 202211008564.0
• 申請日: 2022-08-22
• 公開（公告）號: CN115437373A
• 公開（公告）日: 2022-12-06
• 發明（設計）人: 王鴻東;孫世平;董磊;向金林;李鋒
• 申請（專利權）人: 上海交通大學
• 主分類號: G05D1/02
• 分類號: G05D1/02
• 代理公司: 北京律譜知識產權代理有限公司 11457
• 代理人: 黃云鐸

1.一種結合RRT算法與VO算法的無人船路徑規劃方法，其特征在于，所述方法的步驟包括：

S1、以無人船的中心點q_o出發，對第i個障礙物的體積圓作切線λ_Li和λ_Ri，以無人船中心點q_o為圓心，以無人船速度大小|V_u|為半徑作圓，將切線λ_Li和λ_Ri沿該第i個障礙物速度V_obi方向平移|V_obi|距離，平移后的切線與該圓相交于兩點，與圓心q_o圍成一扇形，該扇形區域為速度障礙區域VO_oui；

S2、計算切線λ_Li、λ_Ri與的方位角β_Li與β_Ri，另外，對第i個障礙物求解最小避讓幅度θ_mi，其中第j個障礙物數值有最大的最小避讓幅度θ_mj，定義其為最危險障礙物，取其最小避讓幅度θ_mj與該第j個障礙物的右切線方位角β_Rj比較，取數值較大者作為待定方向；

S3、以無人船艏向θ_u與無人船中心點q_o為基準，向左右兩邊每隔2°取一個航行方向，左右對稱共覆蓋260°的扇形范圍，記為方向角θ_k，k＝1，2，3…130，表示從順時針數第k個航向，從全部航行方向中，剔除步驟S2得到的待定方向開始順時針旋轉90°范圍以外的航行方向，再剔除落在障礙物的速度障礙區域VO_oui內的所有航行方向；

S4、將無人船中心點q_o與目標終點q_goal連線，該連線的方位角為β_goal，該方向角β_goal與此時剩余的所有航行方向的方向角θ_k形成夾角，稱為角度偏移量Δθ_k，其中數值最小的|Δθ_k|所對應的航行方向即為最優速度方向θ_best；

S5、從無人船中心點q_o出發，沿最優速度方向θ_best擴展步長距離D_vo得到新節點q_init，作為RRT算法的根節點，添加至節點列表，以根節點為起始，在搜索空間內生成隨機節點，在節點列表內尋找與該隨機節點距離最近的節點，從該距離最近的節點出發，向隨機節點方向擴展一個步長距離，得到一個新節點，判斷該新節點是否與障礙物發生碰撞，若未碰撞將該新節點添加至節點列表，若發生碰撞則重新生成；

S6、將目標終點q_goal設定為終點節點，判斷步驟S5生成的可用新節點與該終點節點的距離，若該距離大于預設判定距離，則返回步驟S5再次生成；若該距離小于預設判定距離，則路徑規劃結束。

2.如權利要求1所述的無人船路徑規劃方法，其特征在于，步驟S1中求解速度障礙區域VO_oui的障礙物i包括靜態障礙物與動態障礙物，而在步驟S2中僅需要對動態障礙物求解最小避讓幅度θ_mi。

3.如權利要求1所述的無人船路徑規劃方法，其特征在于，步驟S2中計算方位角β_Li與β_Ri的方法具體是：計算切線λ_Li、λ_Ri與無人船與障礙物中心點連線的夾角θ_obi，其中R_p為障礙物體積圓的半徑，D_oi為無人船中心點與障礙物中心點的距離，接著得到方位角β_Li與β_Ri：

4.如權利要求1所述的無人船路徑規劃方法，其特征在于，步驟S2中求解最小避讓幅度的方法具體是：取步驟S1中得到圓上與移動后切線的所有交點，分別與無人船中心點q_o連接形成連線，連線與無人船艏向θ_u方向之間形成一夾角，取其中最小的夾角即為最小避讓幅度θ_mi。

5.如權利要求4所述的無人船路徑規劃方法，其特征在于，求解最小避讓幅度θ_mi時，若該障礙物形成的切線與步驟S1中的圓沒有交點，則賦值此障礙物的最小避讓幅度θ_mi＝0。