Multi-sensor fusion obstacle avoidance and course self-recovery system and method for water surface vehicle

Abstract

The invention discloses a multi-sensor fusion obstacle avoidance and course self-recovery system and method for a water surface vehicle. The method comprises the steps that multi-sensor data of the surrounding environment of the water surface vehicle are collected in real time through a millimeter wave radar and an active sonar detector; constructing an environment perception model by adopting multi-layer filtering and a space-time alignment algorithm based on space coordinate system conversion and time coordinate system alignment; judging the motion state of the obstacle according to the environment perception model, calculating obstacle avoidance course angle information when the relative speed of the motion state of the obstacle is determined to be backward, and resolving actual rudder angle information based on a control algorithm; a water surface vehicle steering control device or a power device is used for adjusting the course according to the actual rudder angle information; and after obstacle avoidance is completed, according to the stored preset course information, the course control module is utilized to automatically recover to the original course. According to the invention, the problems of insufficient sensing precision, route deviation after obstacle avoidance and difficulty in asynchronous fusion of multi-sensor data of the water surface vehicle in a complex environment are solved.

Description

Technical Field

[0001] The present invention relates to the technical field of intelligent navigation and automatic obstacle avoidance, and in particular to a multi-sensor fusion obstacle avoidance and heading self-recovery system and method for a surface vehicle. Background Art

[0002] Currently, obstacle avoidance systems for surface vehicles primarily rely on single sensors (such as millimeter-wave radar or active sonar) for environmental perception. However, due to limitations in sensor technology, existing systems suffer from insufficient obstacle recognition accuracy and detection range in complex surface environments (such as narrow waters and areas with dense underwater reefs). To improve obstacle recognition accuracy and system robustness, modern systems are increasingly adopting multi-sensor fusion technology, integrating data from multiple sensors such as GPS, AIS, sonar, and inertial navigation. This approach can overcome the shortcomings of a single sensor and improve the overall performance of the navigation system. However, multi-sensor fusion technology faces significant challenges in data processing and algorithm design, resulting in low data integration efficiency and difficulty in constructing high-precision environmental perception models.

[0003] In terms of data processing, the existing system lacks an effective filtering mechanism, resulting in significant noise interference in the raw data collected by sensors, which affects the real-time and accuracy of obstacle avoidance decisions. Furthermore, the lack of a heading recovery mechanism after obstacle avoidance means the vehicle often fails to automatically return to its intended route after completing an obstacle avoidance maneuver, increasing the risk of deviation. Furthermore, the existing system lacks sufficient redundancy. If a key sensor (such as the main radar or sonar) fails, the entire obstacle avoidance system may fail, seriously impacting navigation safety.

[0004] To address the above problems, there is an urgent need for a system that can combine multi-sensor fusion, efficient data filtering, intelligent obstacle avoidance decision-making and heading self-recovery functions to improve the autonomous obstacle avoidance capability and navigation reliability of surface vehicles in complex environments. Summary of the Invention

[0005] In response to the above-mentioned deficiencies in the existing technology, the present invention provides a multi-sensor fusion obstacle avoidance and heading self-recovery system and method for surface vehicles. Through the multi-sensor fusion of millimeter-wave radar and active sonar, combined with multi-layer filtering and spatiotemporal alignment processing, and utilizing dynamic obstacle avoidance heading calculation and heading self-recovery mechanism, the system solves the problems of insufficient perception accuracy of surface vehicles in complex environments, route deviation after obstacle avoidance, and difficulty in asynchronous fusion of multi-sensor data.

[0006] In order to solve the above technical problems, the present invention adopts the following technical solutions:

[0007] An intelligent obstacle avoidance and heading self-recovery system for a surface vehicle, comprising: a data perception module, a data fusion module, an obstacle avoidance algorithm module, a heading control module, and a heading self-recovery module;

[0008] The data perception module is used to collect the motion state information and three-dimensional data of the surface vehicle's surrounding environment in real time;

[0009] The data fusion module is used to pre-process and fuse the data output from the data perception module after filtering by the underlying algorithm to generate an environment perception model;

[0010] The obstacle avoidance algorithm module is used to judge the movement state of the obstacle based on the environmental perception model, and when it is determined that the obstacle has a collision risk, calculate the obstacle avoidance heading angle information, and solve the obstacle avoidance heading angle information based on the control algorithm to obtain the actual steering angle information;

[0011] The heading control module is used to receive actual rudder angle information and adjust the heading using the surface vehicle steering control device or power device to achieve obstacle recognition and avoidance;

[0012] The heading self-recovery module is used to store preset heading information, and read the preset heading information after obstacle avoidance is completed, and use the heading control module to automatically recover to the original route.

[0013] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles, as a preferred solution, the data perception module includes a millimeter-wave radar and an active sonar detector; the millimeter-wave radar is distributed on the front, left and right sides of the surface vehicle, and is used to obtain the relative longitudinal and lateral distance and speed between the obstacle and the vehicle in real time; the active sonar detector is installed on the front, left and right sides of the surface vehicle, and is used to monitor the distance from the front, left and right sides of the surface vehicle to the obstacle.

[0014] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles, as a preferred solution, the data fusion module includes a data preprocessing unit and a data synchronization unit; the data preprocessing unit is used to perform multi-layer filtering and noise removal preprocessing on the data output by the data perception module after filtering by the underlying algorithm; the data synchronization unit is used to perform target matching and fusion processing on the preprocessed data output by the data preprocessing unit to generate an environmental perception model.

[0015] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles is a preferred solution, wherein the processing process of the data pre-processing unit specifically includes:

[0016] The millimeter-wave radar data is filtered once using the millimeter-wave radar's underlying algorithm to obtain primary filtered radar data. The primary filtered radar data is compared with preset threshold data, and radar data exceeding the threshold is eliminated to obtain secondary filtered radar data. The secondary filtered radar data is clustered using the DBSCAN algorithm to construct a four-dimensional feature vector based on the relative longitudinal and lateral distances and speeds of the obstacles. The obstacle similarity is measured based on Euclidean distance to merge multiple detection results of the same obstacle, thereby obtaining tertiary filtered radar data.

[0017] For the data collected by the active sonar detector, a filtering is performed once through the underlying algorithm of the active sonar detector to obtain the primary filtered sonar data; the pulse noise in the said primary filtered sonar data is removed by median filtering to obtain the secondary filtered sonar data, and the obstacle information detected by the active sonar detector is obtained through the image processing algorithm.

[0018] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles is a preferred solution, and the processing process of the data synchronization unit specifically includes:

[0019] The preprocessed data output by the preprocessing unit is converted into a spatial coordinate system and aligned with a time coordinate system to perform spatiotemporal alignment. The intersection-over-union (IoU) algorithm is used to perform target matching on the multi-sensor filtered data after spatiotemporal alignment. When the IoU result is within a preset threshold range, it is determined that the detection results of the active sonar detector and the millimeter-wave radar detecting the same obstacle match. The successfully matched obstacle data are fused to generate an environmental perception model.

[0020] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles is, as a preferred solution, represented by the spatial coordinate system transformation:

[0021] q i =K[RT]p i ;

[0022] Where q i is the coordinate of the i-th data obtained by the millimeter wave radar in the sonar coordinate system, q i =[u i ,v i ,z i ,1] T ; K is the sonar internal parameter matrix; R is the rotation matrix for transforming the millimeter wave radar coordinate system to the sonar coordinate system; T is the translation matrix for transforming the millimeter wave radar coordinate system to the sonar coordinate system; p i is the coordinate of the i-th data obtained by the millimeter-wave radar in the millimeter-wave radar coordinate system, p i =[x i ,y i ,1] T;

[0023] The time coordinate system alignment is expressed as:

[0024]

[0025] Where, T sync is the synchronous sampling time of millimeter wave radar and active sonar; l cm is the lowest common multiple of the two sampling periods; f1 is the target update frequency of the millimeter wave radar; f2 is the target update frequency of the active sonar detector;

[0026] The intersection-over-union algorithm is expressed as:

[0027]

[0028] Where, IoU is the intersection over union algorithm; S S is the area of ​​the obstacle rectangle detected by the active sonar detector; S R The area of ​​the obstacle tracking box detected by the millimeter-wave radar.

[0029] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles, as a preferred solution, the obstacle avoidance algorithm module includes a path planning unit and a rudder angle control signal generation unit, the path planning unit is used to judge the movement state of the obstacle according to the environmental perception model, and calculate the obstacle avoidance heading angle information when it is determined that the obstacle has a collision risk; the rudder angle control information generation unit is used to receive the obstacle avoidance heading angle information obtained by the path planning unit, and solve the actual rudder angle information based on the control algorithm.

[0030] The above-mentioned intelligent obstacle avoidance and heading self-recovery system for surface vehicles, as a preferred solution, solves the obstacle avoidance heading angle information through the following equation:

[0031] N1=min(|δ|,|ε|);

[0032]

[0033] Wherein, N1 is the minimum steering reference angle; δ is the angle between the left edge of the obstacle and the original heading of the vehicle; ε is the angle between the right edge of the obstacle and the original heading of the vehicle; g is the safe obstacle avoidance distance; g1 is the minimum measurable distance of the active sonar detector; g2 is the half-width of the surface vehicle itself; D is the width of the surface vehicle; g3 is the distance between the edge of the obstacle and the vehicle; y is the relative longitudinal distance between the obstacle and the vehicle; γ is the additional steering angle; N2 is the obstacle avoidance heading angle. When the value of N2 is negative, it means that the obstacle avoidance heading angle is left relative to the original heading; when the value of N2 is positive, it means that the obstacle avoidance heading angle is right relative to the original heading; when the value of N2 is zero, it means that the original heading can be maintained to avoid the obstacle.

[0034] On the other hand, the present invention also provides a method for intelligent obstacle avoidance and heading self-recovery of a surface vehicle, which is performed using the above-mentioned intelligent obstacle avoidance and heading self-recovery system for a surface vehicle, and includes the following steps:

[0035] S1, collects the motion state information and three-dimensional data of the surface vehicle's surrounding environment in real time through the data perception module;

[0036] S2. Preprocessing and fusing the output data from the data perception module after filtering by an underlying algorithm to generate an environment perception model;

[0037] S3. Determine the motion state of the obstacle in the current segment based on the environment perception model. If the relative speed between the obstacle and the aircraft in the current segment remains unchanged or is positive, execute step S4; if the relative speed between the obstacle and the aircraft in the current segment is negative, execute step S5.

[0038] S4: Determine that there is no collision risk with obstacles in the current flight segment, set the next flight segment as the current flight segment, and return to step S1;

[0039] S5. If the obstacle in the current segment blocks the original course of the surface vehicle, execute step S6. If the obstacle in the current segment does not block the original course of the surface vehicle, but the relative lateral distance between the obstacle and the vehicle is less than the preset lateral safety distance, execute step S6. If the obstacle in the current segment does not block the original course of the surface vehicle, and the relative lateral distance between the obstacle and the vehicle is greater than or equal to the preset lateral safety distance, determine that there is no collision risk with the obstacle in the current segment, set the next segment as the current segment, and return to step S1.

[0040] S6. Determine that there is a collision risk with an obstacle in the current flight segment, calculate obstacle avoidance heading angle information using the obstacle avoidance algorithm module according to the environment perception model, and calculate the obstacle avoidance heading angle information based on the control algorithm to obtain actual rudder angle information;

[0041] S7, receiving actual rudder angle information and adjusting the course using the surface vehicle's steering control device or power unit to achieve obstacle recognition and avoidance;

[0042] S8. After the obstacle avoidance is completed, the preset heading information stored in the heading self-recovery module is read and preset rudder angle information is generated. The heading is adjusted by the surface vehicle steering control device or power unit to receive the preset rudder angle information, so as to automatically recover to the original route, and the next segment is used as the current segment. The process returns to step S1 and the next obstacle avoidance process is performed.

[0043] S9. Repeat steps S1-S8 to continuously monitor the environment surrounding the surface vehicle until the navigation mission is completed.

[0044] The above-mentioned intelligent obstacle avoidance and heading self-recovery method for surface vehicles is a preferred solution. In step S6, the obstacle avoidance heading angle information calculated according to the environment perception model is obtained by the following formula:

[0045] N1=min(|δ|,|ε|);

[0046]

[0047] Wherein, N1 is the minimum steering reference angle; δ is the angle between the left edge of the obstacle and the original heading of the vehicle; ε is the angle between the right edge of the obstacle and the original heading of the vehicle; g is the safe obstacle avoidance distance; g1 is the minimum measurable distance of the active sonar detector; g2 is the half-width of the surface vehicle itself; D is the width of the surface vehicle; g3 is the distance between the edge of the obstacle and the vehicle; y is the relative longitudinal distance between the obstacle and the vehicle; γ is the additional steering angle; N2 is the obstacle avoidance heading angle. When the value of N2 is negative, it means that the obstacle avoidance heading angle is left relative to the original heading; when the value of N2 is positive, it means that the obstacle avoidance heading angle is right relative to the original heading; when the value of N2 is zero, it means that the original heading can be maintained to avoid the obstacle.

[0048] Compared with the prior art, the present invention has the following technical effects:

[0049] 1. The present invention significantly improves the full-dimensional perception capability of surface vehicles of complex water surface environments through the multi-sensor fusion of millimeter-wave radar and active sonar detectors, overcomes the perception limitations of a single sensor in specific scenarios, and effectively eliminates noise interference and redundant data in multi-source sensor data through multi-layer filtering, thereby improving the accuracy and reliability of obstacle identification; at the same time, the spatiotemporal synchronization technology of coordinate transformation and time alignment is adopted to ensure the consistency of multi-sensor data, and combined with the intersection-over-union (IoU) matching algorithm, avoids the sudden threat of moving obstacles; in addition, the heading self-recovery module of the present invention automatically stores the preset route in the heading storage unit, reducing the heading regression error after obstacle avoidance, while reducing human intervention, and avoiding the problem of "getting lost after detouring" in traditional obstacle avoidance systems.

[0050] 2. The present invention enables surface vehicles to achieve complete closed-loop control from environmental perception to obstacle avoidance execution to heading recovery, significantly improving the autonomous navigation capability and safety of surface vehicles in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0051] In order to make the purpose, technical solutions and advantages of the invention more clear, the present invention will be further described in detail below with reference to the accompanying drawings, in which:

[0052] Figure 1 This is a system structure diagram of intelligent obstacle avoidance and heading self-recovery for a surface vehicle according to the present invention;

[0053] Figure 2 This is a schematic diagram of sonar coordinate system projection, taking an example of an obstacle detected in front of a surface vehicle according to an embodiment of the present invention;

[0054] Figure 3 This is a flow chart of a method for intelligent obstacle avoidance and heading self-recovery of a surface vehicle according to the present invention. DETAILED DESCRIPTION

[0055] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. The components of the embodiments of the present invention generally described and shown in the drawings here can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the invention claimed for protection, but only represents selected embodiments of the present invention. Based on the embodiments in the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0056] The present invention will be described in further detail below with reference to the accompanying drawings.

[0057] Due to its reliance on a single sensor technology, the existing system has obvious limitations in its perception capabilities in complex water environments. It is unable to fully cover the surrounding environment of the vehicle, and it is difficult to ensure the accuracy of obstacle identification. Secondly, when multiple sensors work together, there is a lack of an effective spatiotemporal synchronization mechanism, which makes it difficult to accurately match the data collected by different sensors in the time and space dimensions, seriously affecting the reliability of subsequent decision-making. Furthermore, the traditional obstacle avoidance system has imperfect functional design and cannot automatically restore the original route after completing the obstacle avoidance action, causing the vehicle to deviate from the predetermined mission path. Finally, the existing system architecture generally has the problem of insufficient redundancy. Once a key sensor or functional module fails, the entire obstacle avoidance system will fail, posing a major hidden danger to navigation safety.

[0058] In response to the above-mentioned problems and shortcomings, the present invention proposes a multi-sensor fusion obstacle avoidance and heading self-recovery system and method for surface vehicles. At the perception layer, the millimeter-wave radar and active sonar multi-sensor fusion technology is used to enhance the accurate recognition and understanding of the surrounding environment, and combined with multi-layer filtering processing, the environmental perception accuracy is significantly improved, thereby improving the accuracy and timeliness of obstacle avoidance decisions; at the data fusion layer, a spatiotemporal alignment algorithm based on spatial coordinate system conversion and time synchronization sampling is designed to ensure the consistency of multi-source data; at the decision control layer, the obstacle avoidance heading angle is dynamically calculated through the obstacle avoidance algorithm module, and the heading self-recovery module is used to realize the closed-loop control of "obstacle avoidance-resumption"; at the system architecture layer, the redundant design of sensors and systems is strengthened to ensure that the vehicle's obstacle avoidance system can still operate stably when the main sensors or components fail, thereby improving fault tolerance and ensuring navigation safety.

[0059] Specifically, the present invention proposes an intelligent obstacle avoidance and heading self-recovery system for surface vehicles, such as Figure 1 As shown, it includes: data perception module, data fusion module, obstacle avoidance algorithm module, heading control module and heading self-recovery module;

[0060] The data perception module is used to collect the motion state information and three-dimensional data of the surface vehicle's surrounding environment in real time;

[0061] The data fusion module is used to pre-process and fuse the output data from the data perception module after filtering by the underlying algorithm to generate an environment perception model;

[0062] The obstacle avoidance algorithm module is used to judge the movement state of the obstacle based on the environmental perception model, and when it is determined that the obstacle has a collision risk, calculate the obstacle avoidance heading angle information, and solve the obstacle avoidance heading angle information based on the control algorithm to obtain the actual steering angle information;

[0063] The heading control module is used to receive actual rudder angle information and adjust the heading using the surface vehicle steering control device or power device to achieve obstacle recognition and avoidance;

[0064] The heading self-recovery module is used to store preset heading information, and read the preset heading information after obstacle avoidance is completed, and automatically recover to the original route using the heading control module.

[0065] In the present invention, the environment around the surface vehicle is monitored in real time, and the movement state of the obstacle is analyzed to determine whether the obstacle will collide with the surface vehicle, so as to determine whether the heading angle of the surface vehicle needs to be adjusted. After completing the obstacle avoidance, the surface vehicle is autonomously restored to the original preset heading, thereby improving the accuracy of obstacle avoidance and effectively solving the problem of traditional obstacle avoidance systems getting lost after obstacle avoidance, resulting in the deviation of the final route.

[0066] In a specific implementation, the data perception module includes a millimeter-wave radar and an active sonar detector. In the specific application of this embodiment, in the data perception module, there are three millimeter-wave radars, which are respectively installed on the front, left and right sides of the surface vehicle, for obtaining the relative longitudinal distance y between the obstacle and the vehicle, the relative lateral distance x between the obstacle and the vehicle, and the relative longitudinal speed v between the obstacle and the vehicle in real time. x and the relative lateral velocity v between the obstacle and the vehicle y There are 9 active sonar detectors, which are divided into 3 groups and installed on the front, left and right sides of the surface vehicle respectively, for monitoring the distance from the front, left and right sides of the surface vehicle to obstacles. The quantitative formula is:

[0067]

[0068] Where x1 is the distance from the front of the surface vehicle to the obstacle; x2 is the distance from the left side of the surface vehicle to the obstacle; x3 is the distance from the right side of the surface vehicle to the obstacle; i1 to i9 represent the data obtained by the nine active sonar detectors respectively; α, β, and γ are all weight coefficients.

[0069] In specific implementation, the data fusion module includes a data preprocessing unit and a data synchronization unit; the data preprocessing unit is used to perform multi-layer filtering and noise removal preprocessing on the data output by the data perception module after filtering by the underlying algorithm; the data synchronization unit is used to perform target matching and fusion processing on the preprocessed data output by the data preprocessing unit to generate an environmental perception model;

[0070] Specifically, the processing process of the data pre-processing unit specifically includes:

[0071] When the present embodiment is specifically applied, the millimeter wave radar collected data D0 is filtered once by the underlying algorithm of the millimeter wave radar to obtain the primary filtered radar data D1; the primary filtered radar data D1 is compared with the preset threshold data, such as R min , R max , v min , v max ,θ min ,θ max By comparing and eliminating the over-limit radar data, we can obtain the secondary filtered radar data D2, whose quantization formula is: Among them, R max and R min are the upper and lower limits of the millimeter wave radar’s range respectively; R r is the relative distance of the obstacle, which can be obtained from the known x and y; v max and v minare the upper and lower speed limits of the millimeter wave radar respectively; v r is the relative speed of the obstacle, which can be obtained from v x 、v y Obtain; θ max and θ min are the upper and lower limits of the millimeter-wave radar’s angle measurement respectively; θ r is the relative azimuth of the obstacle, which can be obtained from the known x and y. The DBSCAN clustering algorithm is used to construct a four-dimensional feature vector based on the relative longitudinal and lateral distances and speeds of the obstacle. The obstacle similarity is measured based on the Euclidean distance to merge multiple detection results of the same obstacle, thereby obtaining the third-level filtered radar data D3.

[0072] The formula for measuring obstacle similarity based on Euclidean distance is expressed as:

[0073]

[0074] Where K(A,B) is the Euclidean distance between two vectors A and B; A and B are four-dimensional vectors established by the horizontal and vertical distances and the horizontal and vertical velocities; A i and B i are the values ​​of the i-th dimension in these two vectors respectively;

[0075] For the data collected by the active sonar detector, a filtering is performed once through the underlying algorithm of the active sonar detector to obtain the primary filtered sonar data; the impulse noise in the primary filtered sonar data is removed by median filtering to obtain the secondary filtered sonar data, and the obstacle information detected by the active sonar detector is obtained by the image processing algorithm [j i ,k i ,l i ,m i ], where [j i ,k i ] is the center of mass of the obstacle, l i is the width of the obstacle area, m i is the length of the obstacle area.

[0076] During specific implementation, the processing process of the data synchronization unit specifically includes: performing spatial coordinate system conversion and time coordinate system alignment processing on the preprocessed data output by the preprocessing unit to perform spatiotemporal alignment, and using the intersection-and-union algorithm to perform target matching on the multi-sensor filtered data after spatiotemporal alignment. When the intersection-and-union result is within a preset threshold range, it is determined that the detection results of the active sonar detector and the millimeter-wave radar detecting the same obstacle match, and the successfully matched obstacle data are fused to generate an environmental perception model.

[0077] Wherein, the spatial coordinate system transformation is expressed as:

[0078] q i =K[RT]p i ;

[0079] Where q i is the coordinate of the i-th data obtained by the millimeter wave radar in the sonar coordinate system, q i =[u i ,v i ,z i ,1] T ; K is the sonar internal parameter matrix; R is the rotation matrix for transforming the millimeter wave radar coordinate system to the sonar coordinate system; T is the translation matrix for transforming the millimeter wave radar coordinate system to the sonar coordinate system; p i is the coordinate of the i-th data obtained by the millimeter-wave radar in the millimeter-wave radar coordinate system, p i =[x i ,y i ,1] T ;

[0080] The time coordinate system alignment is expressed as:

[0081]

[0082] Where, T sync is the synchronous sampling time of millimeter wave radar and active sonar; l cm is the lowest common multiple of the two sampling periods; f1 is the target update frequency of the millimeter wave radar; f2 is the target update frequency of the active sonar detector;

[0083] The intersection-over-union algorithm is expressed as:

[0084]

[0085] Where, IoU is the intersection over union algorithm; S S is the area of ​​the obstacle rectangle detected by the active sonar detector; S R The area of ​​the obstacle tracking box detected by the millimeter-wave radar.

[0086] During specific implementation, the obstacle avoidance algorithm module includes a path planning unit and a rudder angle control signal generation unit. The path planning unit is used to judge the motion state of the obstacle based on the environmental perception model, and calculate the obstacle avoidance heading angle information when it is determined that the obstacle has a collision risk; the rudder angle control information generation unit is used to receive the obstacle avoidance heading angle information obtained by the path planning unit, and solve the actual rudder angle information based on the control algorithm.

[0087] In specific implementation, the obstacle motion state is judged according to the environment perception model. When the relative speed between the obstacle motion state and the aircraft is determined to be negative, that is, vr ≤0, it is necessary to change the heading to avoid the obstacle. In the specific application of this embodiment, taking the obstacle detected in front of the surface vehicle as an example, after the data synchronization unit performs time and space alignment, the following is established: Figure 2 The sonar coordinate system projection diagram is shown in the figure. Figure 2 In the equation, ST is the horizontal center axis (reference heading), S is the projection point of the millimeter-wave radar in the sonar coordinate system (i.e., the current position of the vehicle), SQ and SN are the left and right edge rays of the obstacle detected by the radar; ∠NSP is δ, and ∠PSQ is ε; the obstacle avoidance heading angle information is solved by the following equation:

[0088] N1=min(|δ|,|ε|);

[0089]

[0090] Wherein, N1 is the minimum steering reference angle; δ is the angle between the left edge of the obstacle and the original heading of the vehicle; ε is the angle between the right edge of the obstacle and the original heading of the vehicle; g is the safe obstacle avoidance distance; g1 is the minimum measurable distance of the active sonar detector; g2 is the half-width of the surface vehicle itself; D is the width of the surface vehicle; g3 is the distance between the edge of the obstacle and the vehicle; y is the relative longitudinal distance between the obstacle and the vehicle; γ is the additional steering angle; N2 is the obstacle avoidance heading angle. When the value of N2 is negative, it means that the obstacle avoidance heading angle is left relative to the original heading; when the value of N2 is positive, it means that the obstacle avoidance heading angle is right relative to the original heading; when the value of N2 is zero, it means that the original heading can be maintained to avoid the obstacle.

[0091] The above-mentioned obstacle avoidance heading angle calculation algorithm analyzes the angles of the left and right edges of the obstacle relative to the original heading and selects the side with the smaller angle for yaw avoidance. This algorithm is applicable to different situations: obstacles blocking the original heading (the obstacle's edges are to the left and right of the original heading respectively), obstacles located entirely to the left of the original heading (both obstacles' edges are to the left of the original heading), and obstacles located entirely to the right of the original heading (both obstacles' edges are to the right of the original heading). The calculated obstacle avoidance angle is determined by the magnitude of the obstacle avoidance heading angle N2, and the obstacle avoidance direction is determined by the positive or negative value of the obstacle avoidance heading angle N2. A negative N2 value indicates that the obstacle avoidance heading angle deviates to the left of the original heading; a positive N2 value indicates that the obstacle avoidance heading angle deviates to the right of the original heading; and a zero N2 value indicates that the original heading is sufficient for obstacle avoidance.

[0092] The present invention also proposes a method for intelligent obstacle avoidance and heading self-recovery of a surface vehicle, which is implemented by the above-mentioned intelligent obstacle avoidance and heading self-recovery system of the surface vehicle. Figure 3 As shown, the following steps are included:

[0093] S1, collects the motion state information and three-dimensional data of the surface vehicle's surrounding environment in real time through the data perception module;

[0094] S2. Preprocessing and fusing the output data from the data perception module after filtering by an underlying algorithm to generate an environment perception model;

[0095] S3. Determine the motion state of the obstacle in the current segment based on the environment perception model. If the relative speed between the obstacle and the aircraft in the current segment remains unchanged or is positive, execute step S4; if the relative speed between the obstacle and the aircraft in the current segment is negative, execute step S5.

[0096] S4: Determine that there is no collision risk with obstacles in the current flight segment, set the next flight segment as the current flight segment, and return to step S1;

[0097] S5. If the obstacle in the current segment blocks the original course of the surface vehicle, execute step S6. If the obstacle in the current segment does not block the original course of the surface vehicle, but the relative lateral distance between the obstacle and the vehicle is less than the preset lateral safety distance, execute step S6. If the obstacle in the current segment does not block the original course of the surface vehicle, and the relative lateral distance between the obstacle and the vehicle is greater than or equal to the preset lateral safety distance, determine that there is no collision risk with the obstacle in the current segment, set the next segment as the current segment, and return to step S1.

[0098] S6. Determine that there is a collision risk with an obstacle in the current flight segment, calculate obstacle avoidance heading angle information using the obstacle avoidance algorithm module according to the environment perception model, and calculate the obstacle avoidance heading angle information based on the control algorithm to obtain actual rudder angle information;

[0099] S7, receiving actual rudder angle information and adjusting the course using the surface vehicle's steering control device or power unit to achieve obstacle recognition and avoidance;

[0100] S8. After the obstacle avoidance is completed, the preset heading information stored in the heading self-recovery module is read and preset rudder angle information is generated. The heading is adjusted by the surface vehicle steering control device or power unit to receive the preset rudder angle information, so as to automatically recover to the original route, and the next segment is used as the current segment. The process returns to step S1 and the next obstacle avoidance process is performed.

[0101] S9. Repeat steps S1-S8 to continuously monitor the environment surrounding the surface vehicle until the navigation mission is completed.

[0102] In specific implementation, in step S5, the preset lateral safety distance can be a safety distance value determined according to the width of the surface vehicle and the minimum measurable distance g1 of the active sonar detector D. For example, the preset lateral safety distance can be set to be greater than or equal to In step S6, the obstacle avoidance heading angle information is calculated according to the environment perception model, which can be calculated and determined in the aforementioned manner.

[0103] In summary, the present invention has the following technical effects:

[0104] 1. The present invention significantly improves the full-dimensional perception capability of surface vehicles of complex water surface environments through the multi-sensor fusion of millimeter-wave radar and active sonar detectors, overcomes the perception limitations of a single sensor in specific scenarios, and effectively eliminates noise interference and redundant data in multi-source sensor data through multi-layer filtering, thereby improving the accuracy and reliability of obstacle identification; at the same time, the spatiotemporal synchronization technology of coordinate transformation and time alignment is adopted to ensure the consistency of multi-sensor data, and combined with the intersection-over-union (IoU) matching algorithm, avoids the sudden threat of moving obstacles; in addition, the heading self-recovery module of the present invention automatically stores the preset route in the heading storage unit, reducing the heading regression error after obstacle avoidance, while reducing human intervention, and avoiding the problem of "getting lost after detouring" in traditional obstacle avoidance systems.

[0105] 2. The present invention enables surface vehicles to achieve complete closed-loop control from environmental perception to obstacle avoidance execution to heading recovery, significantly improving the autonomous navigation capability and safety of surface vehicles in complex environments.

[0106] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not limiting. Although the present invention has been described with reference to the preferred embodiments of the present invention, it should be understood by those skilled in the art that various changes can be made in form and details without departing from the spirit and scope of the present invention as defined in the appended claims.

Claims

1. An intelligent obstacle avoidance and heading recovery system for a surface vehicle, characterized in that: include: Data perception module, data fusion module, obstacle avoidance algorithm module, heading control module and heading self-recovery module; The data perception module is used to collect the motion state information and three-dimensional data of the surface vehicle's surrounding environment in real time; The data fusion module is used to pre-process and fuse the data output from the data perception module after filtering by the underlying algorithm to generate an environment perception model; The obstacle avoidance algorithm module is used to judge the movement state of the obstacle based on the environmental perception model, and when it is determined that the obstacle has a collision risk, calculate the obstacle avoidance heading angle information, and solve the obstacle avoidance heading angle information based on the control algorithm to obtain the actual steering angle information; The heading control module is used to receive actual rudder angle information and adjust the heading using the surface vehicle steering control device or power device to achieve obstacle recognition and avoidance; The heading self-recovery module is used to store preset heading information, and read the preset heading information after obstacle avoidance is completed, and use the heading control module to automatically recover to the original route.

2. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 1, characterized in that: The data perception module includes a millimeter-wave radar and an active sonar detector; the millimeter-wave radar is distributed on the front, left and right sides of the surface vehicle, and is used to obtain the relative longitudinal and lateral distance and speed between the obstacle and the vehicle in real time; the active sonar detector is installed on the front, left and right sides of the surface vehicle, and is used to monitor the distance from the front, left and right sides of the surface vehicle to the obstacle.

3. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 2, characterized in that: The data fusion module includes a data preprocessing unit and a data synchronization unit; the data preprocessing unit is used to perform multi-layer filtering and noise removal preprocessing on the data output by the data perception module after filtering by the underlying algorithm; The data synchronization unit is used to perform target matching and fusion processing on the preprocessed data output by the data preprocessing unit to generate an environment perception model.

4. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 3, characterized in that: The processing process of the data preprocessing unit specifically includes: The millimeter-wave radar data is filtered once using the millimeter-wave radar's underlying algorithm to obtain primary filtered radar data. The primary filtered radar data is compared with preset threshold data, and radar data exceeding the threshold is eliminated to obtain secondary filtered radar data. The secondary filtered radar data is clustered using the DBSCAN algorithm to construct a four-dimensional feature vector based on the relative longitudinal and lateral distances and speeds of the obstacles. The obstacle similarity is measured based on Euclidean distance to merge multiple detection results of the same obstacle, thereby obtaining tertiary filtered radar data. For the data collected by the active sonar detector, a filtering is performed once through the underlying algorithm of the active sonar detector to obtain the primary filtered sonar data; the pulse noise in the said primary filtered sonar data is removed by median filtering to obtain the secondary filtered sonar data, and the obstacle information detected by the active sonar detector is obtained through the image processing algorithm.

5. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 3, characterized in that: The processing process of the data synchronization unit specifically includes: The preprocessed data output by the preprocessing unit is converted into a spatial coordinate system and aligned with a time coordinate system to perform spatiotemporal alignment. The intersection-over-union (IoU) algorithm is used to perform target matching on the multi-sensor filtered data after spatiotemporal alignment. When the IoU result is within a preset threshold range, it is determined that the detection results of the active sonar detector and the millimeter-wave radar detecting the same obstacle match. The successfully matched obstacle data are fused to generate an environmental perception model.

6. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 5, characterized in that: The spatial coordinate system transformation is expressed as: q i =K[R T]p i ; Where q i is the coordinate of the i-th data obtained by the millimeter wave radar in the sonar coordinate system, q i =[u i ,v i ,z i ,1] T ; K is the sonar internal parameter matrix; R is the rotation matrix for transforming the millimeter wave radar coordinate system to the sonar coordinate system; T is the translation matrix for transforming the millimeter wave radar coordinate system to the sonar coordinate system; p i is the coordinate of the i-th data obtained by the millimeter-wave radar in the millimeter-wave radar coordinate system, p i =[x i ,y i ,1] T ; The time coordinate system alignment is expressed as: Where, T sync is the synchronous sampling time of millimeter wave radar and active sonar; l cm is the lowest common multiple of the two sampling periods; f1 is the target update frequency of the millimeter wave radar; f2 is the target update frequency of the active sonar detector; The intersection-over-union algorithm is expressed as: Where IoU is the intersection over union algorithm; S S is the area of ​​the obstacle rectangle detected by the active sonar detector; S R The area of ​​the obstacle tracking box detected by the millimeter-wave radar.

7. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 1, characterized in that: The obstacle avoidance algorithm module includes a path planning unit and a rudder angle control signal generation unit. The path planning unit is used to judge the motion state of the obstacle based on the environmental perception model, and calculate the obstacle avoidance heading angle information when it is determined that the obstacle has a collision risk; the rudder angle control information generation unit is used to receive the obstacle avoidance heading angle information obtained by the path planning unit and solve the actual rudder angle information based on the control algorithm.

8. The intelligent obstacle avoidance and heading self-recovery system for surface vehicles according to claim 1, characterized in that: Obstacle avoidance heading angle information is solved by the following equation: N1=min(|δ|,|ε|); Wherein, N1 is the minimum steering reference angle; δ is the angle between the left edge of the obstacle and the original heading of the vehicle; ε is the angle between the right edge of the obstacle and the original heading of the vehicle; g is the safe obstacle avoidance distance; g1 is the minimum measurable distance of the active sonar detector; g2 is the half-width of the surface vehicle itself; D is the width of the surface vehicle; g3 is the distance between the edge of the obstacle and the vehicle; y is the relative longitudinal distance between the obstacle and the vehicle; γ is the additional steering angle; N2 is the obstacle avoidance heading angle. When the value of N2 is negative, it means that the obstacle avoidance heading angle is left relative to the original heading; when the value of N2 is positive, it means that the obstacle avoidance heading angle is right relative to the original heading; when the value of N2 is zero, it means that the original heading can be maintained to avoid the obstacle.

9. A method for intelligent obstacle avoidance and heading self-recovery of a surface vehicle, characterized in that: The intelligent obstacle avoidance and heading self-recovery system for a surface vehicle according to claim 1 is used for execution, comprising the following steps: S1, collects the motion state information and three-dimensional data of the surface vehicle's surrounding environment in real time through the data perception module; S2. Preprocessing and fusing the output data from the data perception module after filtering by an underlying algorithm to generate an environment perception model; S3. Determine the motion state of the obstacle in the current segment based on the environment perception model. If the relative speed between the obstacle and the aircraft in the current segment remains unchanged or is positive, execute step S4; if the relative speed between the obstacle and the aircraft in the current segment is negative, execute step S5. S4: Determine that there is no collision risk with obstacles in the current flight segment, set the next flight segment as the current flight segment, and return to step S1; S5. If the obstacle in the current segment blocks the original course of the surface vehicle, execute step S6. If the obstacle in the current segment does not block the original course of the surface vehicle, but the relative lateral distance between the obstacle and the vehicle is less than the preset lateral safety distance, execute step S6. If the obstacle in the current segment does not block the original course of the surface vehicle, and the relative lateral distance between the obstacle and the vehicle is greater than or equal to the preset lateral safety distance, determine that there is no collision risk with the obstacle in the current segment, set the next segment as the current segment, and return to step S1. S6. Determine that there is a collision risk with an obstacle in the current flight segment, calculate obstacle avoidance heading angle information using the obstacle avoidance algorithm module according to the environment perception model, and calculate the obstacle avoidance heading angle information based on the control algorithm to obtain actual rudder angle information; S7, receiving actual rudder angle information and adjusting the course using the surface vehicle's steering control device or power unit to achieve obstacle recognition and avoidance; S8. After the obstacle avoidance is completed, the preset heading information stored in the heading self-recovery module is read and preset rudder angle information is generated. The heading is adjusted by the surface vehicle steering control device or power unit to receive the preset rudder angle information, so as to automatically recover to the original route, and the next segment is used as the current segment. The process returns to step S1 and the next obstacle avoidance process is performed. S9. Repeat steps S1-S8 to continuously monitor the environment surrounding the surface vehicle until the navigation mission is completed.

10. The method for intelligent obstacle avoidance and heading self-recovery of a surface vehicle according to claim 9, characterized in that: In step S6, the obstacle avoidance heading angle information is calculated based on the environment perception model using the following formula: N1=min(|δ|,|ε|); Wherein, N1 is the minimum steering reference angle; δ is the angle between the left edge of the obstacle and the original heading of the vehicle; ε is the angle between the right edge of the obstacle and the original heading of the vehicle; g is the safe obstacle avoidance distance; g1 is the minimum measurable distance of the active sonar detector; g2 is the half-width of the surface vehicle itself; D is the width of the surface vehicle; g3 is the distance between the edge of the obstacle and the vehicle; y is the relative longitudinal distance between the obstacle and the vehicle; γ is the additional steering angle; N2 is the obstacle avoidance heading angle. When the value of N2 is negative, it means that the obstacle avoidance heading angle is left relative to the original heading; when the value of N2 is positive, it means that the obstacle avoidance heading angle is right relative to the original heading; when the value of N2 is zero, it means that the original heading can be maintained to avoid the obstacle.

Images