Obstacle updating and avoiding method for underwater unmanned vehicle
By carrying forward-looking sonar equipment on an underwater unmanned vehicle and building an obstacle library, the problem of low obstacle detection accuracy in traditional methods is solved, and more accurate obstacle detection and effective obstacle avoidance navigation are achieved.
Patent Information
- Application Number
- CN202510093737.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-01-21
- Publication Date
- 2025-05-06
AI Technical Summary
The obstacle detection method of traditional underwater unmanned aircraft has low measurement accuracy and poor accuracy of obstacle detection, which leads to the inability to effectively avoid obstacles and affects the normality of navigation.
By carrying forward-view sonar equipment, we can obtain obstacle detection results in real time, and combine the vehicle attitude information of the underwater unmanned aircraft to build an obstacle library, perform deduplication and filter out obstacle attribute information, update the obstacle library, and then control the unmanned aircraft to avoid obstacles.
It improves the accuracy of obstacle detection, ensures that underwater unmanned vehicles can effectively avoid obstacles, ensure the safety and stability of navigation, and improves the real-time nature of obstacle avoidance navigation.
Smart Images
Figure CN119937562A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of underwater unmanned vehicle control, and in particular to an obstacle updating and avoiding method for an underwater unmanned vehicle. Background Art
[0002] Underwater unmanned vehicles are an important type of unmanned equipment that can replace humans in marine environment detection. They are widely used in various military and civilian scenarios such as underwater search and rescue, surveying, and marine life monitoring. Due to the complexity and variability of the underwater environment, underwater unmanned vehicles will be disturbed by various obstacles during navigation, which seriously affects the normal operation of underwater unmanned vehicles. Therefore, accurate obstacle avoidance is the key to underwater unmanned vehicles completing various tasks.
[0003] Accurately detecting obstacles is the most fundamental prerequisite for ensuring precise obstacle avoidance. Traditional methods usually use a forward-looking sonar system to collect images in the direction of the underwater unmanned vehicle's advance and detect obstacles ahead through the images. However, this method has low measurement accuracy and poor obstacle detection accuracy. Summary of the invention
[0004] In view of the above problems and technical requirements, this application proposes an obstacle update and avoidance method for an underwater unmanned vehicle. The technical solution of this application is as follows:
[0005] An obstacle updating and avoiding method for an underwater unmanned vehicle comprises the following steps:
[0006] When the underwater unmanned vehicle is navigating along the target path, the obstacle detection results within the detection range in front of the underwater unmanned vehicle are obtained in real time through the forward-looking sonar device carried by the vehicle, and the obstacle attribute information of each obstacle within the detection range of the forward-looking sonar device is obtained in combination with the real-time vehicle attitude information of the underwater unmanned vehicle in the geodetic coordinate system;
[0007] The obstacle attribute information of each detected obstacle is added to the obstacle library, and the obstacle attribute information belonging to the same obstacle is deduplicated, and the obstacle attribute information of obstacles in the obstacle library that are beyond the affected range in the navigation direction of the underwater unmanned vehicle is filtered out to obtain an updated obstacle library; the obstacle library stores the obstacle attribute information of all obstacles within the affected range in the navigation direction of the underwater unmanned vehicle detected during the historical navigation process of the underwater unmanned vehicle along the target path;
[0008] The underwater unmanned vehicle is controlled to move and avoid obstacles according to the obstacle attribute information of all obstacles in the obstacle library.
[0009] A further technical solution is to perform deduplication processing on obstacle attribute information belonging to the same obstacle, including:
[0010] Determine the similarity between the detected obstacle o and the obstacle i in the obstacle library according to the obstacle attribute information of any obstacle o detected and the obstacle attribute information of any obstacle i in the obstacle library;
[0011] When the similarity between obstacle i in the obstacle library and the detected obstacle o meets the similarity requirement, it is determined that obstacle o and obstacle i correspond to the same obstacle, and the obstacle attribute information of obstacle o and the obstacle attribute information of obstacle i in the obstacle library are deduplicated.
[0012] A further technical solution is that the obstacle attribute information of obstacle o and the obstacle attribute information of obstacle i in the obstacle library are deduplicated, including:
[0013] The obstacle attribute information of the detected obstacle o and the obstacle attribute information of the obstacle i in the obstacle database are fused as the updated obstacle attribute information of the obstacle i.
[0014] A further technical solution is that the obstacle attribute information includes the obstacle position and obstacle size of the obstacle in the geodetic coordinate system, and determining the similarity between the detected obstacle o and the obstacle i in the obstacle library includes:
[0015] Calculate the distance d between the detected obstacle o and the obstacle position of obstacle i in the obstacle library o , and calculate the obstacle size difference between the detected obstacle o and the obstacle i in the obstacle library, according to the distance d o The difference between the detected obstacle o and the obstacle i in the obstacle library is determined by the obstacle size difference, and the distance d o The closer, the smaller the obstacle size difference, and the higher the similarity between the detected obstacle o and the obstacle i in the obstacle library.
[0016] A further technical solution is that the vehicle attitude information of the underwater unmanned vehicle includes the vehicle position (U x ,U y ), the obstacle detection result includes the obstacle size of the obstacle within the detection range in front of the underwater unmanned vehicle and the relative position of the obstacle in the follow-up coordinate system of the underwater unmanned vehicle; the obstacle attribute information of each obstacle within the detection range of the forward-looking sonar device includes:
[0017] According to the real-time position of the underwater unmanned vehicle in the geodetic coordinate system (U x ,U y) The relative position of the obstacle in the tracking coordinate system of the underwater unmanned vehicle is transformed to obtain the obstacle position of the obstacle in the earth coordinate system, and the obstacle attribute information of the obstacle is obtained in combination with the obstacle size of the obstacle.
[0018] A further technical solution is that the obstacle attribute information includes the obstacle position of the obstacle in the geodetic coordinate system, and the obstacle attribute information of the obstacles in the obstacle library that are beyond the affected range in the navigation direction of the underwater unmanned vehicle along the target path is filtered out, including:
[0019] For any obstacle i in the obstacle library, when the distance d between obstacle i and the underwater unmanned vehicle uo Greater than the distance threshold d oy , and when obstacle i is located behind the underwater unmanned vehicle in the navigation direction of the target path, it is determined that the affected range exceeds the navigation direction of the underwater unmanned vehicle along the target path and the obstacle attribute information of obstacle i is filtered out.
[0020] A further technical solution is that determining whether the obstacle i is located behind the underwater unmanned vehicle in the navigation direction along the target path includes:
[0021] Calculate the position of the underwater unmanned vehicle in the geodetic coordinate system (U x ,U y ) and the projection distance of obstacle i on the target path in the obstacle library. The projection distance represents the distance between the projection point on the target path and the starting point of the target path along the target path. When the projection distance LP of obstacle i on the target path ox Less than the projection distance LP of the underwater unmanned vehicle on the target path ux When , it is determined that obstacle i is located behind the underwater unmanned vehicle in the navigation direction along the target path.
[0022] A further technical solution is that the target path is a straight line path;
[0023] The vehicle attitude information of the underwater unmanned vehicle includes the vehicle position (U x ,U y ), calculate the projection distance LP of the underwater unmanned vehicle on the target path ux =(U x -P1 x )cos(θ)+(U y -P1 y )sin(θ);
[0024] The obstacle attribute information of obstacle i includes the obstacle position in the geodetic coordinate system (G xi ,G yi), calculate the projection distance LP of obstacle i on the target path ox =(G xi -P1 x )cos(θ)+(G yi -P1 y )sin(θ),(P1 x ,P1 y ) is the position of the starting point of the target path in the geodetic coordinate system, and θ is the angle between the straight line where the target path is located and the x-axis of the geodetic coordinate system.
[0025] A further technical solution is that obtaining updated obstacle attribute information of obstacle i includes:
[0026] The obstacle attribute information of the detected obstacle o and the obstacle attribute information of the obstacle i in the obstacle database are averaged to obtain the updated obstacle attribute information of the obstacle i.
[0027] The beneficial technical effects of this application are:
[0028] The obstacle update and obstacle avoidance method for underwater unmanned vehicles proposed in this application builds an obstacle library to store the obstacle attribute information obtained from detection at each moment and performs deduplication processing. By associating the historical detection results in the obstacle library, it can effectively solve the problem that the traditional sonar detection method can only detect obstacles in front of it. The problem of inaccurate detection results caused by identifying the same obstacle as different obstacles is solved by continuously updating the obstacle library through deduplication processing, which effectively improves the accuracy of obstacle detection, lays a solid foundation for the obstacle avoidance navigation of underwater unmanned vehicles, and ensures the safety and stability of underwater unmanned vehicles.
[0029] The obstacle library is updated by filtering out obstacles that are beyond the affected range of the underwater unmanned vehicle's navigation direction, ensuring that only valid obstacle attribute information is stored in the obstacle library to improve the search efficiency when the obstacle library is subsequently updated, which helps to improve the real-time performance of the underwater unmanned vehicle's obstacle avoidance navigation. BRIEF DESCRIPTION OF THE DRAWINGS
[0030] Figure 1 It is a structural diagram of an obstacle update and avoidance system for an underwater unmanned vehicle.
[0031] Figure 2 It is a flow chart of an obstacle updating and avoiding method for an underwater unmanned vehicle.
[0032] Figure 3 It is a schematic diagram of obstacle avoidance navigation for underwater unmanned vehicles with obstacle updates. DETAILED DESCRIPTION
[0033] The specific implementation of the present application is further described below in conjunction with the accompanying drawings.
[0034] The obstacle updating and avoiding method of underwater unmanned vehicle proposed in this application can be applied to Figure 1 The obstacle update and avoidance system of the underwater unmanned vehicle shown is composed of a detector, a navigation controller, a solver and an obstacle library.
[0035] The detector is mainly composed of a forward-looking sonar device, which is installed on the head of the underwater unmanned vehicle and is used to detect obstacles in front of the underwater unmanned vehicle. The forward-looking sonar device collects images containing obstacles to identify the relative distance, relative angle and obstacle size of the obstacles relative to the underwater unmanned vehicle.
[0036] The navigation controller is the core control unit of the underwater unmanned vehicle. It is used to control the attitude of the underwater unmanned vehicle. It consists of a navigation computer and a fiber-optic inertial navigation device. The fiber-optic inertial navigation device is used to obtain the vehicle attitude information of the underwater unmanned vehicle in the geodetic coordinate system. The geodetic coordinate system takes the north direction as the positive direction of the x-axis and the east direction as the positive direction of the y-axis.
[0037] The solver is a logic control unit running in the underwater unmanned vehicle. It can perform matrix arithmetic operations. After the detector obtains the relative distance and relative angle relative to the obstacle, it is converted into the relative position of the obstacle in the follow-up coordinate system of the underwater unmanned vehicle. The follow-up coordinate system of the underwater unmanned vehicle takes the positive direction of the x-axis directly in front of the underwater unmanned vehicle and the positive direction of the y-axis pointing to the starboard side. The relative angle is negative when the obstacle is on the left side of the x-axis and positive when the obstacle is on the right side of the x-axis. After obtaining the relative position, the obstacle position in the earth coordinate system is solved using the vehicle attitude information obtained by the navigation controller.
[0038] The obstacle library is an E2PROM chip with storage function. The obstacle library stores the obstacle attribute information of all obstacles detected in the historical navigation process of the underwater unmanned vehicle along the target path and located within the affected range of the navigation direction of the underwater unmanned vehicle, and can be continuously updated.
[0039] Based on this system, an obstacle update and obstacle avoidance method for underwater unmanned vehicles proposed in this application can be found in Figure 2 The specific steps in the flowchart are as follows:
[0040] Step 1: When the underwater unmanned vehicle is navigating along the target path, the obstacle detection results within the detection range in front of the underwater unmanned vehicle are obtained in real time through the forward-looking sonar device carried by the vehicle, and the obstacle attribute information of each obstacle within the detection range of the forward-looking sonar device is obtained in combination with the real-time vehicle attitude information of the underwater unmanned vehicle in the geodetic coordinate system.
[0041] In one embodiment, the vehicle attitude information of the underwater unmanned vehicle includes the vehicle position (U x ,U y ) and the direction of the vehicle Au. The obstacle detection results include the obstacle size of the obstacles in the detection range in front of the underwater unmanned vehicle and the relative position of the obstacles in the follow-up coordinate system of the underwater unmanned vehicle. The obstacle size is expressed as the maximum circumscribed circle radius of the obstacle. The relative distance L and relative angle A obtained by the forward-looking sonar device relative to any detected obstacle o are calculated using a solver. o Solved as the relative position of the underwater unmanned vehicle in the following coordinate system (L x ,L y )for:
[0042]
[0043] Then, according to the real-time position of the underwater unmanned vehicle in the earth coordinate system (U x ,U y ) and the spacecraft direction A u The relative position of the obstacle o in the following coordinate system of the underwater unmanned vehicle (L x ,L y ) to obtain the obstacle position (G xo ,G yo ), and combined with the obstacle size R of obstacle o o Obtain the obstacle attribute information of obstacle o. The direction of the aircraft is 0 degrees in the north direction and increases clockwise. For any obstacle position (G xo ,G yo ) The specific conversion formula is as follows:
[0044]
[0045] Step 2: Add the obstacle attribute information of each obstacle detected to the obstacle library and deduplicate the obstacle attribute information belonging to the same obstacle, and filter out the obstacle attribute information of obstacles in the obstacle library that are beyond the affected range in the navigation direction of the underwater unmanned vehicle to obtain an updated obstacle library.
[0046] The process of underwater unmanned vehicle navigating along the target path is as follows Figure 3As shown, the fan-shaped area of the underwater unmanned vehicle head represents the detection range in front of the underwater unmanned vehicle. When the underwater unmanned vehicle 1 is at point A, there is no obstacle in the detection range, and no obstacle attribute information is stored in the obstacle library for this detection. When the underwater unmanned vehicle 1 arrives at point B, it detects obstacle 2 for the first time and stores the obstacle attribute information in the obstacle library. When the underwater unmanned vehicle 1 arrives at point C, it repeatedly detects obstacle 2. However, due to the measurement accuracy of the forward-looking sonar equipment, the detection results for the same obstacle are different from the previous time. In order to ensure the uniqueness of each obstacle in the obstacle library, the obstacle attribute information of the same obstacle needs to be deduplicated.
[0047] In one embodiment, the obstacle database is traversed to compare the obstacle attribute information of the detected obstacle with all the obstacle attribute information in the obstacle database one by one to determine whether they belong to the same obstacle. The specific process is:
[0048] (1) Determine the similarity between the detected obstacle o and the obstacle i in the obstacle library based on the obstacle attribute information of any detected obstacle o and the obstacle attribute information of any obstacle i in the obstacle library.
[0049] The obstacle attribute information includes the obstacle position and obstacle size in the geodetic coordinate system. The similarity between the detected obstacle o and the obstacle i in the obstacle library is determined from the two aspects of position and shape, including:
[0050] Calculate the obstacle position (G xo ,G yo ) and the obstacle position of obstacle i in the obstacle library in the geodetic coordinate system (G xi ,G yi ) o , any distance calculation formula can be used to calculate the distance. This application uses the Euclidean distance calculation formula to calculate
[0051]
[0052] On the other hand, the obstacle size R of the detected obstacle o is calculated o The obstacle size R of obstacle i in the obstacle library i The difference rd=|R o -R i |, according to the distance d o The obstacle size difference rd determines the similarity between the detected obstacle o and the obstacle i in the obstacle library, and the distance d o The closer, the smaller the obstacle size difference, and the higher the similarity between the detected obstacle o and the obstacle i in the obstacle library.
[0053] (2) When the similarity between obstacle i in the obstacle library and the detected obstacle o meets the similarity requirement, it is determined that obstacle o and obstacle i correspond to the same obstacle, and the obstacle attribute information of obstacle o and the obstacle attribute information of obstacle i in the obstacle library are deduplicated.
[0054] When the obstacle position (G xo ,G yo ) and the obstacle position of obstacle i (G xi ,G yi ) o Less than the distance threshold d s , and the obstacle size difference rd is less than the size threshold rd s When the distance threshold d is , it is considered that the similarity between the detected obstacle o and the obstacle i in the obstacle library meets the similarity requirement of the same obstacle. At this time, the obstacle attribute information of the obstacle i belonging to the same obstacle needs to be updated according to the attribute information of the obstacle o. s and size threshold rd s The setting can be customized according to the actual application. In one embodiment, the obstacle attribute information of the detected obstacle o and the obstacle attribute information of the obstacle i in the obstacle library are fused as the updated obstacle attribute information of the obstacle i.
[0055] The obstacle attribute information of obstacle o can be used to overwrite the obstacle attribute information of obstacle i in the obstacle library, or a weighted sum of part of the obstacle attribute information of obstacle o and part of the obstacle attribute information of obstacle i can be taken as the updated obstacle attribute information of obstacle i. The present application takes the average of the obstacle attribute information of obstacle o detected and the obstacle attribute information of obstacle i in the obstacle library to obtain the updated obstacle attribute information of obstacle i. The obstacle position (G xi ,G yi ) and obstacle size R i The calculation formula is:
[0056]
[0057] (3) When all obstacles in the obstacle library are traversed and no obstacle is found to be the same as the detected obstacle o, the obstacle attribute information of obstacle o is added to the obstacle library to update the obstacle library. The newly added obstacle attribute information can be added to any position in the obstacle library. For convenience, the obstacle attribute information can be directly added to the end of the obstacle library.
[0058] like Figure 3As shown, the underwater unmanned vehicle 1 continues to navigate along the target path. When the underwater unmanned vehicle 1 reaches point D, Figure 3 The dotted circle area represents the influence range of obstacle 2. At this time, the underwater unmanned vehicle 1 has exceeded the influence range of obstacle 2 detected previously, and obstacle 2 will no longer affect the underwater unmanned vehicle 1. Therefore, it is necessary to remove the corresponding obstacle attribute information from the obstacle library to ensure that only necessary information is stored in the obstacle library to prevent unnecessary calculations from being added to subsequent calculations.
[0059] The obstacle library is updated by traversing the obstacle library and determining the positional relationship between the obstacles in the obstacle library and the underwater unmanned vehicle. In one embodiment, it is determined from the two aspects of distance and orientation whether the obstacles in the obstacle library exceed the affected range of the underwater unmanned vehicle in the navigation direction along the target path.
[0060] For any obstacle i in the obstacle library, when the distance d between obstacle i and the underwater unmanned vehicle uo Greater than the distance threshold d oy , and when obstacle i is located behind the underwater unmanned vehicle in the navigation direction of the target path, it is determined that the affected range exceeds the navigation direction of the underwater unmanned vehicle along the target path and the obstacle attribute information of obstacle i is filtered out.
[0061] Among them, the distance d between obstacle i and the underwater unmanned vehicle is uo Calculated by Euclidean distance calculation formula Distance threshold d oy The settings can be customized according to the actual application.
[0062] According to the positional relationship between obstacle i and the projection point of the underwater unmanned vehicle on the target path, it is determined whether obstacle i is located behind the underwater unmanned vehicle in the navigation direction of the target path. The specific method is: calculate the vehicle position (U x ,U y ) and the projection distance of obstacle i on the target path in the obstacle library. The projection distance represents the distance between the projection point on the target path and the starting point of the target path along the target path. When the projection distance LP of obstacle i on the target path ox Less than the projection distance LP of the underwater unmanned vehicle on the target path ux When , it is determined that obstacle i is located behind the underwater unmanned vehicle in the navigation direction along the target path.
[0063] by Figure 3 Taking the case where the target path is a straight path as an example, the straight-line distance between the starting point a of the target path and the projection point ox of the obstacle path is the projection distance LP of obstacle 2 on the target path.ox The straight-line distance between the starting point a of the target path and the projection point ux of the vehicle path is the projection distance LP of the underwater unmanned vehicle 1 on the target path. ux At this time, the obstacle path projection point ox of obstacle 2 on the target path lags behind the vehicle path projection point ux of underwater unmanned vehicle 1 on the target path, and the projection distance LP of obstacle 2 on the target path is ox Less than the projection distance LP of the underwater unmanned vehicle 1 on the target path ux It should be noted that, when the target path is a curved path, the curve distance between the starting point a of the target path and the projection point ox of the obstacle path is the projection distance LP of obstacle 2 on the target path. ox The curve distance between the starting point a of the target path and the projection point ux of the vehicle path is the projection distance LP of the underwater unmanned vehicle 1 on the target path. ux .
[0064] In one embodiment, when the target path is a straight path, the projection distance LP of the underwater unmanned vehicle on the target path is calculated. ux =(U x -P1 x )cos(θ)+(U y -P1 y )sin(θ); Calculate the projection distance LP of any obstacle i in the obstacle library on the target path ox =(G xi -P1 x )cos(θ)+(G yi -P1 y )sin(θ),(P1 x ,P1 y ) is the position of the starting point a of the target path in the geodetic coordinate system, θ is the angle between the straight line where the target path is located and the x-axis of the geodetic coordinate system, and (P2 x ,P2 y ) is the position of the target path end point b in the geodetic coordinate system.
[0065] Step 3, according to the detection results at each moment, the obstacle attribute information of all obstacles in the obstacle library is determined and the navigation controller is used to control the underwater unmanned vehicle to move and avoid obstacles. The specific obstacle avoidance method can adopt any obstacle avoidance method in the prior art, such as fuzzy control algorithm, particle swarm algorithm and artificial potential field algorithm.
[0066] Since the obstacles that need to be avoided in the process of controlling the movement of the underwater unmanned vehicle are all obstacles in the obstacle library, rather than just the obstacles detected by the forward-looking sonar device at the current moment, the obstacles around the underwater unmanned vehicle that affect it can be avoided more accurately and effectively, thereby improving the safety of the underwater unmanned vehicle's navigation.
[0067] The above is only a preferred embodiment of the present application, and the present application is not limited to the above embodiments. It is understood that other improvements and changes directly derived or associated by those skilled in the art without departing from the spirit and concept of the present application should be considered to be included in the protection scope of the present application.
Claims
1. An obstacle updating and avoiding method for an underwater unmanned vehicle, characterized in that: The obstacle updating and obstacle avoidance method comprises: When the underwater unmanned vehicle is navigating along the target path, the obstacle detection result within the detection range in front of the underwater unmanned vehicle is obtained in real time by the forward-looking sonar device carried by the vehicle, and the obstacle attribute information of each obstacle within the detection range of the forward-looking sonar device is obtained in combination with the real-time vehicle attitude information of the underwater unmanned vehicle in the geodetic coordinate system; Adding obstacle attribute information of each detected obstacle to an obstacle library, performing deduplication processing on obstacle attribute information belonging to the same obstacle, and filtering out obstacle attribute information of obstacles in the obstacle library that are beyond the affected range in the navigation direction of the underwater unmanned vehicle, to obtain an updated obstacle library; the obstacle library stores obstacle attribute information of all obstacles within the affected range in the navigation direction of the underwater unmanned vehicle detected during the historical navigation process of the underwater unmanned vehicle along the target path; The underwater unmanned vehicle is controlled to move and avoid obstacles according to the obstacle attribute information of all obstacles in the obstacle library.
2. The obstacle updating and avoiding method according to claim 1, characterized in that: Deduplication of obstacle attribute information belonging to the same obstacle includes: Determine the similarity between the detected obstacle o and the obstacle i in the obstacle library according to the obstacle attribute information of any obstacle o detected and the obstacle attribute information of any obstacle i in the obstacle library; When the similarity between obstacle i in the obstacle library and the detected obstacle o meets the similarity requirement, it is determined that obstacle o and obstacle i correspond to the same obstacle, and the obstacle attribute information of obstacle o and the obstacle attribute information of obstacle i in the obstacle library are deduplicated.
3. The obstacle updating and avoiding method according to claim 2, characterized in that: Deduplication processing of obstacle attribute information of obstacle o and obstacle attribute information of obstacle i in the obstacle database includes: The obstacle attribute information of the detected obstacle o and the obstacle attribute information of the obstacle i in the obstacle database are fused as the updated obstacle attribute information of the obstacle i.
4. The obstacle updating and avoiding method according to claim 2, characterized in that: The obstacle attribute information includes the obstacle position and obstacle size of the obstacle in the geodetic coordinate system. Determining the similarity between the detected obstacle o and the obstacle i in the obstacle library includes: Calculate the distance d between the detected obstacle o and the obstacle position of obstacle i in the obstacle library o , and calculate the obstacle size difference between the detected obstacle o and the obstacle i in the obstacle library, according to the distance d o The difference between the detected obstacle o and the obstacle i in the obstacle library is determined by the obstacle size difference, and the distance d o The closer, the smaller the obstacle size difference, and the higher the similarity between the detected obstacle o and the obstacle i in the obstacle library.
5. The obstacle updating and avoiding method according to claim 4, characterized in that: The vehicle attitude information of the underwater unmanned vehicle includes the vehicle position (U x ,U y ), the obstacle detection result includes the obstacle size of the obstacle within the detection range in front of the underwater unmanned vehicle and the relative position of the obstacle in the tracking coordinate system of the underwater unmanned vehicle; Obtaining obstacle attribute information of each obstacle within the detection range of the forward-looking sonar device includes: According to the real-time position of the underwater unmanned vehicle in the geodetic coordinate system (U x ,U y ) The relative position of the obstacle in the follow-up coordinate system of the underwater unmanned vehicle is converted to obtain the obstacle position of the obstacle in the earth coordinate system, and the obstacle attribute information of the obstacle is obtained in combination with the obstacle size of the obstacle.
6. The obstacle updating and avoiding method according to claim 1, characterized in that: The obstacle attribute information includes the obstacle position of the obstacle in the geodetic coordinate system, and the obstacle attribute information of the obstacles in the obstacle library that are beyond the affected range in the navigation direction of the underwater unmanned vehicle along the target path is filtered out, including: For any obstacle i in the obstacle library, when the distance d between obstacle i and the underwater unmanned vehicle uo Greater than the distance threshold d oy , and when obstacle i is located behind the underwater unmanned vehicle in the navigation direction of the target path, it is determined that the affected range exceeds the navigation direction of the underwater unmanned vehicle along the target path and the obstacle attribute information of obstacle i is filtered out.
7. The obstacle updating and avoiding method according to claim 6, characterized in that: Determining whether obstacle i is located behind the underwater unmanned vehicle in the navigation direction along the target path includes: Calculate the position of the underwater unmanned vehicle in the geodetic coordinate system (U x ,U y ) and the projection distance of obstacle i on the target path in the obstacle library. The projection distance represents the distance between the projection point on the target path and the starting point of the target path along the target path. When the projection distance LP of obstacle i on the target path ox Less than the projection distance LP of the underwater unmanned vehicle on the target path ux When , it is determined that obstacle i is located behind the underwater unmanned vehicle in the navigation direction along the target path.
8. The obstacle updating and avoiding method according to claim 7, characterized in that: The target path is a straight line path; The vehicle attitude information of the underwater unmanned vehicle includes the vehicle position (U x ,U y ), calculate the projection distance LP of the underwater unmanned vehicle on the target path ux =(U x -P1 x )cos(θ)+(U y -P1 y )sin(θ); The obstacle attribute information of obstacle i includes the obstacle position in the geodetic coordinate system (G xi ,G yi ), calculate the projection distance LP of obstacle i on the target path ox =(G xi -P1 x )cos(θ)+(G yi -P1 y )sin(θ),(P1 x ,P1 y ) is the position of the starting point of the target path in the geodetic coordinate system, and θ is the angle between the straight line where the target path is located and the x-axis of the geodetic coordinate system.
9. The obstacle updating and avoiding method according to claim 3, characterized in that: The updated obstacle attribute information of obstacle i includes: The obstacle attribute information of the detected obstacle o and the obstacle attribute information of the obstacle i in the obstacle database are averaged to obtain the updated obstacle attribute information of the obstacle i.