Paper reading (58):research and implementation of global path planning for unmanned surface vehicle based

1 summary

1.1 subject

  2022： Research and implementation of global path planning for unmanned surface craft based on electronic chart (Research and implementation of global path planning for unmanned surface vehicle based on electronic chart)

1.2 Abstract

   Unmanned watercraft (Unmanned surface vehicle, USV) It is a new type of intelligent surface craft , The key technology is Global path planning . In order to solve the global path planning problem of unmanned craft , An improved Navigation cost optimization based on electronic chart A$\ast$ Algorithm .S-57 electronic chart Realize the establishment of octree grid environment model , A method based on navigation safety weight is proposed 、 Improvement of pilotage number and path curve smoothing A$\ast$ Algorithm , Ensure route safety , Reduce planning Time , And improve the smoothness of the path . The simulation results show that , Environment model construction method and improved A$\ast$ The algorithm can generate safe and reasonable global paths .

1.3 Code

  Python：https://github.com/wylloong/Global_path_planning_for_USV

1.4 Bib

@inproceedings{
    Wang:2017:534539,
author		=	{
    Yan Long Wang and Xu Liang and Bao An Li and Xue Min },
title		=	{
    Research and implementation of global path planning for unmanned surface vehicle based on electronic chart},
booktitle	=	{
    International Conference on Mechatronics and Intelligent Robotics},
pages		=	{
    534--539},
year		=	{
    2017},
doi			=	{
    10.1007/978-3-319-65978-7_80},
url			=	{
    https://link.springer.com/chapter/10.1007/978-3-319-65978-7_80}
}

2 Establishment and representation of environment model

  USA When sailing in a large area of sea , radar 、 Sensors such as cameras cannot provide information about the global marine environment , Therefore, it can provide detailed and accurate global marine environment information S-57 electronic chart Become a necessary input for the global route planning of unmanned craft , To ensure the safety of navigation .

2.1 Electronic chart data extraction

   Electronic chart is an important component of sea elements , The details include seabed topography 、 Navigation obstacles 、 Navigation marks , And port facilities .S-57 The format of electronic chart standard package is ISO/IEC 8211 international standard , Therefore, this paper adopts ISO8211 Open source library and according to S-57 The package structure of electronic chart is used to analyze all electronic chart information . chart 1 It shows S-57 Package format of electronic chart .

chart 1：S-57 Package format of electronic chart

2.2 Establishment of environmental model

  USA The establishment of the environment model can be simplified as ：
  1）USV At sea level Finite and arbitrary convex regions OS In the mobile ;
  2）OS A limited number of static obstacles in $O_i(i=1,2,\dots ,n)$;
  3） The uncertainty of the shape and position of obstacles .
   Rasterization of environment model It's by putting OS Fill as a rectangle , Use the filled area as an obstacle area ,OS It is also divided into multiple grids of equal size . According to the information extracted in turn, this paper judges whether there is land in the grid 、 Islands and other obstacles , Thus, navigable and non navigable regions are established in grid units . chart 2 (a) (b) The original state and navigation state of the environment model are displayed , The shaded part indicates that it is not passable .

chart 2： Environmental models in different states

   Octree As an extension strategy of path search , Such as chart 2 ($\text{c}$) Shown , Every node $i$ There are eight domain nodes .USV May deviate from the planned path during obstacle avoidance and temporary tasks , There may be diagonal routes near non navigable areas in octree structure , Therefore, this paper believes that there are potential risks in the navigable area near the non navigable area . This setting Navigation safety weight To guide path planning without falling into potential risk areas , That's grid $(C_i,R_i)$ The weight of is affected by the number of non navigable grids in the domain ：$1+2^{n-2}$.

3 The improved A* Description and implementation of the algorithm

3.1 The improved A* Algorithm

  A$\ast$ The algorithm is a classical heuristic path planning algorithm , The basic idea is to use the preset cost function to calculate the value of each domain child node that the current node may reach , Select the node with the least cost to join the search space and expand , And so on , Until you reach the target point . Under safety constraints ,USA Path planning Optimization objectives Is to minimize the cost of the voyage . The cost of sailing $D(p_i,p_{(i+1)})$ Is the linear distance between two points multiplied by the navigation safety weight ：
$$D(p_i,p_{i+1})=\sqrt{(x_{p_i}-x_{p_{(i+1)}}{)}^2+(y_{p_i}-y_{p_{(i+1)}}{)}^2}\ast w(C_{(i+1)},R_{i+1}).\text{\hspace{1em}(1)}$$   Sometimes there is more than one route with the lowest sailing cost , Although only one is needed , But they have been explored , Sometimes because of the straight line between the starting point and the target point $L_{sg}$ There is a deviation , At this time, the automatic route planning is not reasonable . therefore , Pilot quality $P$ By introducing , To ensure that the planned path is close to $L_{sg}$. Make $(S.x,S.y)$ The starting point 、$(G.x,G.y)$ End point , as well as $(C.x,C.y)$ Represents the current node $i$, be start - End And $i$- The goal is The angle of the vector ${\theta }_i$ Calculated as ：
$$\theta i=\arcsin \left(\frac{\sqrt{(C.x-G.x)\ast (S.y-G.y)-(S.x-G.x)\ast (C.y-G.y)}}{\sqrt{(C.x-G.x{)}^2+(C.y-G.y{)}^2}\ast \sqrt{(S.x-G.x{)}^2+(S.y-G.y{)}^2}}\right).\text{\hspace{1em}(2)}$$   node $i$ The number of pilots is calculated as ：
$$p_{(i)}=3/\left(4-\sin \left({\theta }_i\right)\right).\text{\hspace{1em}(3)}$$
   node $i$ The cost function of is ：
$$f_{(i)}=g_{(i)}+h_{(i)},\text{\hspace{1em}(4)}$$ among $g_{(i)}$ It's starting point to node $i$ Sailing cost function ,$h_{(i)}$ Is the node $i$ Heuristics to the end . In this paper, the heuristic function is changed to the ride of distance and pilot quality , It is used to ensure that the range is optimized . This is because $h_{(i)}$ No greater than the slave node $i$ The minimum cost of sailing to the destination ：
$$h_{(i)}=\sqrt{{\left(x_{pi}-x_{pG}\right)}^2+{\left(y_{pi}-y_{pG}\right)}^2}\ast p(i)\le \sqrt{{\left(x_{pi}-x_{pG}\right)}^2+{\left(y_{pi}-y_{pG}\right)}^2}\le \min \left(\sum _{j=i}^G{D}_{(j,j+1)}\right).\text{\hspace{1em}(5)}$$

3.2 Path curve smoothing

   In Grid Environment , If it will be improved A$\ast$ The nodes obtained by the algorithm are connected in turn as USV Planning path for , Ladder or zigzag lines sometimes appear on the path , It is easy to know that the planned path does not meet the requirements . So a kind of Path curve smoothing Method to remove redundant nodes . This method needs to traverse all nodes , If node $i$ and $i+2$ There is no barrier between , Then remove the redundant nodes $i+1$. Repeat the above steps until $i$ and $j$ The wires of go through obstacles , And then at the node $i$ Then take it out 3 Consecutive nodes $P_{(j-1)}$、$P_j$ and $P_{(j+1)}$, Continue these steps until you have traversed all the nodes in the path .

4 Simulation results

   To illustrate environment modeling and improvement A$\ast$ The effectiveness of the algorithm , The above algorithm is simulated . Electronic chart analysis uses C++、 Environment modeling and path planning python .
   For a certain area in the South China Sea ( Regional scope 18.10°N~18.40°N,109.35°E~109.85°E), The simulation results of the given path planning are as follows chart 3 Shown . chart (a) To improve A$\ast$ Planning results of the algorithm 、 chart (b) yes A$\ast$ Planning results of the algorithm , as well as chart ($\text{c}$) yes Dijkstra Planning results of the algorithm , The solid line is the final planned path , Dashed lines are paths without path smoothing .

chart 3： Simulation results

   surface 1 The simulation results of the above path planning algorithm are listed , The number of potential hazards is the total number of non navigable areas close to the final route within the grid . Results show A$\ast$ The number of nodes traversed by the algorithm Dijkstra Fewer algorithms , because Dijkstra The algorithm directly searches the global space without considering the target information , therefore A$\ast$ The path planning efficiency of the algorithm is much higher than Dijkstra Algorithm . Improved A$\ast$ The algorithm is better than the unimproved A$\ast$ The algorithm is improving the route safety 、 Reduce redundant nodes 、 Improve path smoothness 、 Shorten the sailing distance, etc , The layout of navigation nodes is richer and more reasonable .