Academic Journal of Science and Technology ISSN: 2771-3032 | Vol. 4, No. 3, 2022 30 Local Path Planning of USV Based on Millimeter Wave Radar Peilin Hu1, 2, 3, 4, Jinchao Xiao4, Junfeng Xiong4 and Jinqing Liu1, 2, 3, * 1College of Photonic and Electronic Engineering, Fujian Normal University, Fuzhou, 350007, China. 2Key Laboratory of Optoelectronic Science and Technology for Medicine, Ministry of Education, Fujian Normal University, Fuzhou, 350007, China. 3Fujian Provincial Key Laboratory of Photonic Technology, Fujian Normal University, Fuzhou, 350007, China. 4Guangzhou Institute of Industrial Intelligence, Guangzhou, 511458, China. *Corresponding author: Jinqing Liu Abstract: The USV (Unmanned Surface Vehicle) have unpredictable obstacles in the process of performing missions. To avoid obstacles smoothly, a local path planning method based on millimeter wave radar detection is proposed. An optimal path under the constraints of local path length and smoothness is screened out by improving the B-spline curve method, and the navigation safety and optimization of USV are improved through the actual verification of multiple launchings. Keywords: USV, Obstacle, Millimeter wave radar, Improved B-spline curve. 1. Introduction The USV refers to sailing in an autonomous or semi- autonomous manner on a known or unknown water surface environment with the help of internal sensors and external sensors without a crew and executes preset task robot. It can effectively solve the waste of human resources, reduce human misoperation, and optimize navigation routes which is widely used in hydrological monitoring, water quality sampling, sea rescue, reconnaissance, and other fields [1]. In the process of executing the mission, to ensure the safety of navigation, it is necessary to make a corresponding local path planning when facing obstacles. So, how to ensure the optimization of the local path is an urgent problem to be solved. In recent years, relevant scholars and researchers have done a lot of research. The literature [2] divides obstacle avoidance scenarios into four collision risk levels: high, medium, low, and zero, and adopts different coping strategies for obstacles with different collision risk levels. The rationality and safety of the USV avoiding obstacles in actual navigation are improved. The literature [3] designed a hybrid fuzzy and IAPF (improved artificial potential field) obstacle avoidance algorithm based on scanning sonar by combining the IAPF and fuzzy logic control, which can not only overcome the inherent defects of the TAPF (traditional artificial potential field), and can guide the USV to leave the unknown sea area with complex obstacle distribution. The literature [4] predicts the trajectory of obstacles and uses the evaluation function to select the optimal obstacle avoidance area and the corresponding speed to avoid obstacles in time. The literature [5] proposed a dynamic obstacle avoidance method considering international maritime rules, established a collision risk model based on DCPA （distance to closest point of approach）and TCPA （ time to closest point of approach）, and realize real-time obstacle avoidance by IAPF and obstacle connection strategy. The literature [6] uses the dynamic window method to sample the velocity space to simulate the trajectory of a period, based on the obstacle distance, azimuth, and velocity. The evaluation function selects the speed corresponding to an optimal trajectory. The literature [7] established an avoidance knowledge base, the factors that affect the navigation safety of the USV are decomposed and refined, and the obstacles avoidance of the USV in complex water environment are improved. The literature [8] proposes an improved heuristic function, which improves the optimization of the traditional A* star algorithm in global path planning. The literature [9] proposed a real- time path that combines a global improved A* algorithm that considers factors such as actual road conditions and local collision avoidance with adaptive obstacle distance, and the planning method improves the smoothness of the global path and the time of local obstacle avoidance. In the above studies, the output of the USV obstacle avoidance algorithm in the literatures [1-4] is the expected heading, which is a reactive collision avoidance method and is the optimal judgment for collision avoidance at the current moment, and its trajectory optimization is difficult to guarantee. The literatures [5-8] only considers simulation verification in simple scenarios, ignoring the shape and size of the USV and obstacles, and the actual reliability is difficult to guarantee. This study comprehensively considers the navigation safety and track optimization of the USV, and an optimal path under the constraints of local path length and smoothness is screened out by improving the B-spline curve method, which improves the navigation safety and reliability of the USV. 2. Millimeter Wave Radar Obstacle Detection For the USV, commonly used sensors for surface obstacle detection include lidar, millimeter wave radar, and visual cameras. In the study of L1-level single-sensor obstacle avoidance algorithm, the millimeter wave radar is the most cost-effective after considering comprehensive factors such as application scenarios and costs. The working band of the millimeter wave radar is the millimeter wave band, which has the advantages of microwave guidance and light wave guidance. During the working process, it can detect the distance, speed, angle, and other information of multiple obstacles at the same time. The 31 detection results are often mixed with invalid targets limited by the instability of radar work, the interference of metal obstacles, the unevenness of echo energy and other factors [10]. To improve the safety and reliability of the USV collision avoidance experiment, the effectiveness of radar detection targets is an important prerequisite. Facing the complex water environment, invalid targets are mainly noise points such as water wave reflection, but its duration is very short. By filtering the life cycle of the target, the noise on the water surface can be suppressed, the credibility of the target obstacle can be improved, and stable and reliable obstacle information can be output. The effects before and after filtering of targets with a radar detection lifetime longer than one second are shown as Figure 1 and Figure 2. Figure 1. Before filtering. Figure 2. After filtering. 3. Spline Local Path Planning The main methods of local path planning for the USV include APF (artificial potential field), Bezier curve and B- spline curve, but the APF has problems of local optimum and unreachable targets, and its track optimization is difficult to guarantee. In the Bezier curve method, the derivative degree of the curve will also be high when the curve degree is high, so the curve will have more peaks and valleys, and once the number of control points is determined, it cannot be locally modified. However, the B-spline curve can make up for the defects of the above two methods. B-spline curve is a linear combination of B-spline basis functions, and all spline functions on a given interval form a linear space. There is a total of 1n control points including 0 1 2 n, , , ,D D D D   , and these control points are used to define the trend and boundary range of the spline curve, then the definition of the k order B-spline curve is:   0,k 1,k 0 1 n i i,k 0 n,k ( ) ( ) ( ) ( ) ( ) n i B x B x D x D D D D B x B x               � � (1) In the formula, i,k ( )B x is the thi B-spline basis function of order k , which is corresponding to the control point iD , and x is the independent variable and 1k  . The basis functions have the following Deborah-Cox recursion: i i+1 i,k i i+k i,k-1 i+1,k-1 i+k-1 i i+k i+1 1, , 1 0,others ( ) ( ) ( ), 2 x x x k B x x x x x B x B x k x x x x             (2) We agree that 0 / 0 0 , where is a set of continuously changing values of a non-decreasing sequence called a node vector, the first and last values are generally defined as 0 and 1, and the sequence is as follows:  0 1 k k+1 n n+1 n+k, , , , , , , , ,x x x x x x x� � � (3) According to the recursive formula, when the order k is 1, the non-zero fields of different basis functions are shown as Figure 3: 32 Figure 3. The non-zero fields of different basis functions. From this we can deduce that the k order B-spline is a 1k  curve about x , and its curve is composed of several sections of Bezier curves, i,k ( )B x involves a total of 2n k  nodes, including i i+1 i+k, , ,x x x� , which has k intervals. Therefore, a total of 1n k  nodes are involved from 0,k ( )B x to n,k ( )B x .The domain of x is  k n+1,x x . The higher the order, the higher the derivative order of the curve, then there will be many zero points, and more derivative zero points will lead to more extreme values in the original curve, making the curve appear more peaks and valleys value. The lower the degree, the better the spline fits the control points. On the other hand, the cubic B-spline curve can realize the continuity of the second order derivative, so it is more appropriate to finally choose the cubic B-spline curve as the curve for trajectory planning. During the actual navigation of the USV, we set the local obstacle points detected by the millimeter wave radar as the main control points of the B-spline curve, so that a local collision avoidance trajectory limited by the obstacle space can be planned. But this can only guarantee avoidance of obstacles, and its path optimization is difficult to guarantee. Therefore, in addition to obstacle points, the selection of other control points will affect the optimization of collision avoidance path. We obtain a set of local path cluster by choosing different control points, and we use the path length and the average curvature of the path as the evaluation index of the local path planning of the USV, which are used to reflect the overall time of avoiding obstacles and the smoothness of the collision avoidance path respectively. According to the complexity of the problem of local path screening of the USV, it is more appropriate to choose the linear weighting method, as follows:     i1 2 2 1 1 length 2 curvature 1 i+1 i i+1 i 2 1 n n i i K x x y y n                    (4) 4. Experimental Results and Analysis This study has achieved a good result through simulation experiments and actual USV verification. As shown in the Figure 4, it is a local collision avoidance trajectory planned in real time by using the B-spline curve method, where 0D is the current position of the USV, 3D is the local target position of the USV, 2D is the obstacle, and 1D is the control point considering the collision avoidance for trajectory optimization, and the number of trajectory sampling points is 100. Figure 4. Local path in simulation experiments. 33 As shown in the Figure 5 is the trajectory and tracking effect of the actual collision avoidance process of the USV. Figure 5. Local path in actual navigation. As shown in the Figure 6, the planned heading angle and the actual heading angle change with time during the entire voyage of the USV. Figure 6. The change of heading. The distance between the USV and the obstacle is shown as Figure 7, and the change of the planned speed and the actual speed with time is shown as Figure 8. Figure 7. The change of distance between obstacle and USV. Figure 8. The change of speed of the USV. From the experimental results, the local collision avoidance path planned by the simulation is verified in practice. From the above experimental data, it can be seen that the change of heading angle of the USV in the process of collision avoidance is controlled within a reasonable range, and the error between the planned heading and the actual heading is small. The USV reduces the risk of collision as the distance between the USV and the obstacle decreases by decelerating, which improves the safety and rationality of the USV navigation. 5. Conclusion In this research, we can obtain the data of distance, speed, size, and other information of obstacles on water surface detected by millimeter wave radar. Then, a local collision avoidance path cluster is planned by B-spline curve method, and the length and smoothness of the local path are considered, which improved the safety and smoothness of the collision avoidance path in the navigation of the USV. Acknowledgment This work was supported by the Key-Area Research and Development Program of Guangdong Province (2020B1111010002). 34 References [1] Chen H, Zhang X Y, Jiang C F. A Review of Surface Unmanned Boat Path Planning Research. World Shipping. Vol. 38 (2015) No. 11, p. 30-33. [2] LIU S J: Research and Implementation of Intelligent Obstacle Avoidance Strategy for Unmanned Surface Vessel [D]. Huazhong University of Science and Technology, China, 2021. p. 60-61. [3] WANG Y T: Obstacles Avoidance Path Planning and Control System of Underactuated Surface Vehicles [D]. Dalian Maritime University, China, 2020. p. 62-64. [4] ZHU K C, XU Z P, ZHAO Z Q, et al. Dynamic collision avoidance method for unmanned surface vehicle based on track prediction. CHINA MEASUREMENT & TEST. Vol. 47 (2021) No. 11, p. 28-33+46. [5] WEI L K: Research on route planning technology of unmanned vessel based on collision avoidance rules [D]. Hubei University of Technology, China, 2020. p. 63-64. [6] CHENG Q: Research on Environmental Mapping and Obstacle Avoidance of Unmanned Surface Vehicle Based On Lidar [D]. Huazhong University of Science and Technology, China, 2021. p. 50-58. [7] JIANG N N, ZHANG Q, XIANG M. Construction of Intelligent Obstacle Avoidance Model for Unmanned Vessels Based on Avoidance Knowledge Base. Ship Electronic Engineering. Vol. 39 (2019) No. 6, p. 106-109. [8] YU L W, YE X, QIAN T H. Obstacle Avoidance Strategy of Autonomous Unmanned Watercraft on the Surface Using Heuristic Search Algorithm. Journal of Jianghan University Natural Science Edition. Vol. 50 (2022) No. 1, p. 79-86. [9] ZHAO J: Research on Track Re-planning Technology in AutonomousNavigation Process in Semi-structured Environment [D]. Shenyang Jianzhu University, China, 2019. p. 51-52. [10] GAO J D, JIAO X, LIU Q Z, et al. Research on vehicle detection based on data fusion of machine vision and millimeter wave radar. CHINA MEASUREMENT & TEST. Vol. 47 (2021) No. 10, p. 33-40.