Unmanned boat navigation method based on millimeter wave radar

Abstract

The present invention relates to an unmanned boat navigation method based on millimeter wave radar, comprising the following steps: S1, radar point cloud data processing: using millimeter wave radar to sense obstacles around the unmanned boat, and publishing relatively stable obstacle list data; S2, local map real-time update: subscribing to the obstacle list and putting it into the obstacle database to form a stable local map; S3, lateral path planning: subscribing to positioning and attitude data, local map and local target waypoints, and calculating the safe and feasible path within the prediction time by performing two consecutive sampling evaluations of the safety margin on the heading space; S4, longitudinal speed planning: subscribing to positioning and attitude data and using nonlinear smoothing functions to give the safe speed within the prediction time; S5, track tracking: publishing the calculated target waypoints and target speed to the control unit for track tracking. The present invention has the advantages of real-time effectiveness, versatility, practicality, stability and safety.

Description

Technical Field

[0001] The present invention relates to the technical field of unmanned boat navigation, and more specifically, to an unmanned boat navigation method based on millimeter wave radar. Background Art

[0002] The application of unmanned boats in water quality monitoring, water surveying and mapping, security and rescue, and shipping is becoming more and more common and urgent. However, the operating environment of unmanned boats is often complex and diverse, including buoys, aerators, small islands, large and medium-sized floating objects, bridge holes, and passing ships. Therefore, unmanned boats need effective navigation methods to ensure that the platform can complete user tasks while maximizing the safety and stability of the platform.

[0003] Among the existing navigation methods, the cost of laser radar is too high, and the visual solution is easily affected by external lighting; millimeter wave radar avoids these shortcomings, and the reasonable use of millimeter wave radar's sparse point cloud data combined with planning algorithms can achieve good navigation and obstacle avoidance effects. Not only can it smoothly avoid static and dynamic obstacles on the water, but it can also effectively identify the shore and track along the river channel or shore.

[0004] The patent with the publication date of May 7, 2019, publication number CN10925332A, and invention name "A method for avoiding obstacles in unmanned boats based on laser radar" is a solution for local avoidance using laser radar point cloud data. The solution first clusters the laser radar point cloud data to obtain the boundary of the area for safe passage, and then uses an algorithm similar to VFH to obtain the heading at the next moment for obstacle avoidance. This method cannot cope with the sudden appearance of obstacles, such as dynamic obstacles or defects in radar acquisition and processing; in addition, the stability of the ship's heading change and the regression of the route are not considered, and because there is no secondary detection process, the safety of the selected heading is reduced. Furthermore, laser radar is easily affected by weather, and the high cost also limits the application of this method. Summary of the invention

[0005] The technical problem to be solved by the present invention is to provide an unmanned boat navigation method based on millimeter wave radar, which achieves cost control while taking into account lateral static obstacle avoidance and longitudinal dynamic obstacle avoidance, so as to be conveniently applied to actual unmanned boat autonomous navigation tasks.

[0006] The technical solution adopted by the present invention to solve the technical problem is: constructing an unmanned boat navigation method based on millimeter wave radar, comprising the following steps:

[0007] S1. Radar point cloud data processing: Use millimeter-wave radar to sense obstacles around the unmanned boat, cluster and denoise the returned obstacle points, and publish relatively stable obstacle list data;

[0008] S2, real-time update of local map: subscribe to the obstacle list and put it into the obstacle database, and process the obstacle life cycle to form a stable local map;

[0009] S3, lateral path planning: subscribe to positioning and attitude data, local maps and local target waypoints, and calculate the safe and feasible path within the prediction time by performing two consecutive sampling evaluations of the safety margin in the heading space;

[0010] S4, longitudinal speed planning: Subscribe to positioning and attitude data, local maps and local target waypoints, calculate the danger distance in a certain area of ​​the current heading, and use a nonlinear smoothing function to give a safe speed within the prediction time;

[0011] S5, track tracking: the calculated target waypoint and target speed are sent to the control unit for track tracking.

[0012] According to the above scheme, the obstacle points are returned in step S1, aggregated through reasonable clustering parameters, the cluster center of the obstacle points is calculated, and the information of distant clutter points is filtered out to obtain the relative coordinates relative to the radar coordinate system. Then, the absolute coordinates of the cluster obstacle center point are calculated through the acquired hull coordinate information and heading information, and the obstacle information is sent to the control end for use.

[0013] According to the above scheme, in step S2, obstacles that meet the distance limit are pushed into the buffer queue from the subscribed obstacle list and are time-stamped.

[0014] According to the above scheme, elements with similar element comparison positions are taken out from the buffer queue in turn. If there are any, they are merged and pressed into the obstacle database according to the time difference; if there are no elements, they are directly updated and pressed into the obstacle database.

[0015] According to the above scheme, it is checked whether the survival time of each obstacle in the obstacle database reaches the preset life cycle. If it has timed out, it is deleted from the obstacle database; if not, no operation is performed.

[0016] According to the above scheme, the specific content of step S3 is: subscribing to the current unmanned boat position and posture data, obstacle database and mission setting waypoints, and by carefully dividing the heading space and calculating the safety margin, determining the angle closest to the target route direction as the planned waypoint at the next moment.

[0017] According to the above scheme, the following steps are specifically included:

[0018] S401, subscribing to obstacle database, ship positioning and attitude data and mission set speed;

[0019] S402: Given the radius r and angle α of the investigation area, The most dangerous distance d is calculated for obstacles, where The current speed direction of the unmanned boat;

[0020] S403. Calculate the expected speed according to the basic principle of square root algorithm Where a is the set deceleration, t c To arrange the transition time for critical collision;

[0021] S404. If d ≥ r, navigate at the designed speed.

[0022] According to the above scheme, according to the target waypoint and target speed, they are published to the lateral track tracking and longitudinal speed tracking modules, and then the throttle and steering are output to the execution unit for operation.

[0023] The implementation of the unmanned boat navigation method based on millimeter wave radar of the present invention has the following beneficial effects:

[0024] 1. The present invention uses millimeter wave radar to reduce costs, is not affected by weather conditions, and improves practical value. Secondary detection and heading suppression smoothing processing are used in the lateral direction to improve the safety and lateral stability of temporary desired waypoints.

[0025] 2. The present invention provides life cycle management for obstacle data, which largely solves the problem of too small acquisition angle of millimeter wave radar, and also enhances the stability of navigation algorithm. It uses nonlinear smooth function for speed planning, and achieves the goal of slowing down the speed when approaching obstacle area to facilitate radar perception and lateral avoidance, and speeding up the speed when away from obstacle area to escape as quickly as possible.

[0026] 3. The present invention can be applied indiscriminately to laser radars to achieve automatic switching of infeasible waypoints and precise navigation along the river bank, which should be verified through experiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] The present invention will be further described below with reference to the accompanying drawings and embodiments, in which:

[0028] Figure 1 It is a flow chart of the unmanned boat navigation method based on millimeter wave radar of the present invention. DETAILED DESCRIPTION

[0029] In order to have a clearer understanding of the technical features, purposes and effects of the present invention, specific embodiments of the present invention are now described in detail with reference to the accompanying drawings.

[0030] like Figure 1 As shown, the unmanned boat navigation method based on millimeter wave radar of the present invention comprises the following steps:

[0031] S1. Radar point cloud data processing: Use millimeter-wave radar to sense obstacles around the unmanned boat, cluster and denoise the returned obstacle points, and publish relatively stable obstacle list data;

[0032] Specifically, the returned obstacle points are aggregated through reasonable clustering parameters, the cluster center of the obstacle points is calculated, and the information of distant clutter points is filtered out to obtain the relative coordinates relative to the radar coordinate system. Then, the absolute coordinates of the cluster obstacle center point are calculated through the obtained hull coordinate information and heading information, and the obstacle information is sent to the control end for use.

[0033] S2, real-time update of local map: subscribe to the obstacle list and put it into the obstacle database, and process the obstacle life cycle to form a stable local map;

[0034] Specifically, from the subscribed obstacle list, obstacles that meet the distance limit are pushed into the buffer queue and marked with a time stamp. Elements with similar element comparison positions are taken out from the buffer queue one by one. If there are any, they are merged according to the time difference and pushed into the obstacle database; if not, they are directly updated and pushed into the obstacle database. At the same time, check whether the life span of each obstacle in the obstacle database has reached the preset life cycle. If it has timed out, it is deleted from the obstacle database; if not, no operation is performed.

[0035] S3, lateral path planning: subscribe to positioning and attitude data, local maps and local target waypoints, and calculate the safe and feasible path within the prediction time by performing two consecutive sampling evaluations of the safety margin in the heading space;

[0036] Specifically: subscribe to the current unmanned boat position and posture data, obstacle database and mission setting waypoints, and through detailed division of the heading space and safety margin calculation, determine the angle closest to the target route direction as the planned waypoint for the next moment.

[0037] S4, longitudinal speed planning: Subscribe to positioning and attitude data, local maps and local target waypoints, calculate the danger distance in a certain area of ​​the current heading, and use a nonlinear smoothing function to give a safe speed within the prediction time;

[0038] S4 specifically includes the following steps:

[0039] S401, subscribing to obstacle database, ship positioning and attitude data and mission set speed;

[0040] S402: Given the radius r and angle α of the investigation area, The most dangerous distance d is calculated for obstacles, where The current speed direction of the unmanned boat;

[0041] S403. Calculate the expected speed according to the basic principle of square root algorithm Where a is the set deceleration, t c To arrange the transition time for critical collision;

[0042] S404. If d ≥ r, navigate at the designed speed.

[0043] S5, track tracking: the calculated target waypoint and target speed are sent to the control unit for track tracking.

[0044] Specifically: according to the target waypoint and target speed, it is published to the lateral track tracking and longitudinal speed tracking modules, and then the throttle and steering are output to the execution unit for operation.

[0045] The preferred embodiments of the present invention are as follows:

[0046] Step 1: Radar point cloud data processing: Aggregate through reasonable clustering parameters, calculate the cluster center of the obstacle points, and filter out the information of distant clutter points to obtain the relative coordinates relative to the radar coordinate system. Then, calculate the absolute coordinates of the cluster obstacle center point through the acquired hull coordinate information and heading information, and send the obstacle information to the control end for use.

[0047] Step 2: Real-time update of local map: Subscribe to the obstacle list, push the obstacles that meet the distance limit into the buffer queue and mark them with timestamps. Take out the elements with similar positions from the buffer queue one by one. If there are any, merge them according to the time difference and push them into the obstacle database; if not, directly update and push them into the obstacle database. On the other hand, check whether the survival time of each obstacle in the obstacle database has reached the preset life cycle. If it has timed out, delete it from the obstacle database; if not, do nothing.

[0048] Step 3: Lateral path planning: Subscribe to the current position and attitude data of the unmanned boat, the obstacle database, and the mission setting waypoints. By carefully dividing the heading space and calculating the safety margin, the angle closest to the target route direction is determined as the planned waypoint at the next moment.

[0049] Step 4: Longitudinal speed planning: Subscribe to the obstacle database, ship positioning and attitude data, and the speed set by the mission. Given the radius r and angle α of the inspection area, The most dangerous distance d is calculated for obstacles, where is the current speed direction of the unmanned boat. According to the basic principle of the square root algorithm, calculate the expected speed Where a is the set deceleration, t c To arrange the transition time of critical collision. If d ≥ r, sail at the design speed.

[0050] Step 5: Track tracking: According to the expected waypoint and speed, it is published to the lateral track tracking and longitudinal speed tracking modules, and then the throttle and steering are output to the execution unit for operation.

[0051] The embodiments of the present invention are described above in conjunction with the accompanying drawings, but the present invention is not limited to the above-mentioned specific implementation modes, which are merely illustrative rather than restrictive. Under the guidance of the present invention, ordinary technicians in this field can also make many forms without departing from the scope of protection of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. A navigation method for an unmanned boat based on millimeter wave radar, characterized in that: The following steps are involved: S1. Radar point cloud data processing: Use millimeter wave radar to sense obstacles around the unmanned boat, cluster and denoise the returned obstacle points, and publish obstacle list data; S2, real-time update of local map: subscribe to the obstacle list and put it into the obstacle database, and process the obstacle life cycle to form a local map; S3, lateral path planning: subscribe to positioning and attitude data, local maps and local target waypoints, and calculate the safe and feasible path within the prediction time by performing two consecutive sampling evaluations of the safety margin in the heading space; S4, longitudinal speed planning: subscribe to positioning and attitude data, local maps and local target waypoints, calculate the danger distance in a certain area of ​​the current heading, and use a nonlinear smoothing function to give a safe speed within the prediction time; specifically, the following steps are included: S401, subscribing to obstacle database, ship positioning and attitude data and mission set speed; S402: Given the radius r and angle α of the investigation area, The most dangerous distance d is calculated for obstacles, where The current speed direction of the unmanned boat; S403. Calculate the expected speed according to the basic principle of square root algorithm Where a is the set deceleration, t c To arrange the transition time for critical collision; S404. If d ≥ r, sail at the designed speed; S5, track tracking: the calculated target waypoint and target speed are sent to the control unit for track tracking.

2. The unmanned boat navigation method based on millimeter wave radar according to claim 1 is characterized in that: In the step S1, the obstacle points are returned, clustered by clustering parameters, the cluster center of the obstacle points is calculated, and the clutter point information is filtered out to obtain the relative coordinates relative to the radar coordinate system. Then, the absolute coordinates of the cluster obstacle center point are calculated by obtaining the hull coordinate information and heading information, and the obstacle information is sent to the control end for use.

3. The unmanned boat navigation method based on millimeter wave radar according to claim 1 is characterized in that: In step S2, referring to the subscribed obstacle list, obstacles meeting the distance limit are pushed into the buffer queue and time-stamped.

4. The unmanned boat navigation method based on millimeter wave radar according to claim 3 is characterized in that: In step S2, elements with similar element comparison positions are taken out from the buffer queue in sequence. If there are any, they are merged and pushed into the obstacle database according to the time difference; if not, they are directly updated and pushed into the obstacle database.

5. The unmanned boat navigation method based on millimeter wave radar according to claim 3 is characterized in that: In step S2, it is checked whether the life span of each obstacle in the obstacle database reaches the preset life span. If it has timed out, it is deleted from the obstacle database; if not, no operation is performed.

6. The unmanned boat navigation method based on millimeter wave radar according to claim 1 is characterized in that: In step S3, the current unmanned boat position and posture data, obstacle database and mission setting waypoints are subscribed, and the heading space is carefully divided and the safety margin is calculated to determine the angle closest to the target route direction as the planned waypoint at the next moment.

7. The unmanned boat navigation method based on millimeter wave radar according to claim 1 is characterized in that: In step S5, according to the target waypoint and target speed, they are published to the lateral track tracking and longitudinal speed tracking modules, and then the throttle and steering are output to the execution unit for operation.

Images