Narrow channel passing method based on channel perpendicular bisector

By using vertical lines in the channel in AUV and combining polynomial trajectory planning, the smoothness and safety issues of path planning in narrow channels are solved, ensuring that the AUV can safely cross the channels and avoid concave traps in unknown environments.

CN118938967AActive Publication Date: 2024-11-12HARBIN ENG UNIV

Patent Information

Application Number
CN202410966498.0
Authority / Receiving Office
CN · China
Patent Type
Applications(China)
Current Assignee / Owner
Filing Date
2024-07-18
Publication Date
2024-11-12
Estimated Expiration
2044-07-18

AI Technical Summary

Technical Problem

The existing AUVs have problems with poor smoothness, poor safety and prone to falling into concave traps in narrow channels, especially in unknown environments, which are difficult to ensure the effectiveness and safety of the path.

Method used

A narrow channel travel method based on the perpendicular line in the channel is adopted to obtain obstacle information through sonar, combine the global path planning algorithm and polynomial trajectory planning, select local target points and plan smooth and safe paths to avoid AUVs falling into concave traps.

Benefits of technology

Improve the path smoothness and safety of AUV in narrow channels, ensure that AUV can effectively cross the channels and avoid concave traps, and enhance the reliability of path planning.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118938967A_ABST
    Figure CN118938967A_ABST
Patent Text Reader

Abstract

The invention discloses a narrow channel passing method based on a channel perpendicular bisector, and belongs to the technical field of AUV path planning. According to the method, the problems that the smoothness and the safety of the planned path are poor and the AUV is easy to sink into a concave trap in the existing method are solved. The local target point is selected by using the midperpendicular of the narrow channel, and then the path from the current position to the local target point is planned by using the polynomial trajectory planning method, so that the smoothness and safety of the path are ensured. And the AUV can observe the environment in the process of sailing to the local target point, so that the AUV is effectively prevented from sinking into a concave trap. Meanwhile, the position of the local target point is continuously updated according to the environment information to adjust the posture of the AUV, and the safety of the AUV passing through the narrow channel is further improved. The method can be applied to the technical field of AUV path planning.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The invention belongs to the technical field of AUV path planning, and in particular relates to a narrow channel traversal method based on a perpendicular line in the channel. Background Art

[0002] Autonomous Underwater Vehicle (AUV) is the main platform for exploring and developing marine resources. It has the ability to complete tasks such as marine monitoring, seabed survey, resource exploration, and maritime search in complex underwater environments. With the continuous improvement of mission requirements and complexity, the demand for exploring unknown waters is increasing. In addition, the marine environment is complex and uncertain. How to enable AUV to plan a safe and efficient path online in an unknown environment has become a current research hotspot. In online path planning, safe passage through narrow channels has always been a difficult problem in this field.

[0003] At present, there are few theoretical studies on AUVs passing through narrow channels. Most of the existing studies use probabilistic sampling algorithms, and the algorithms are specifically divided into two categories: algorithms based on probabilistic roadmaps (PRM) and rapidly exploring random trees (RRT). Since the configuration space of narrow channels is small, probabilistic sampling algorithms generally have the problem of being unable to effectively explore the configuration space and thus finding a path. Although deterministic algorithms, such as A*, can also find the optimal path solution, the computational complexity is large and time-consuming. In the paper "Rapidly-Exploring Random Vines (RRV) for Motion Planning in Configuration Spaces with Narrow Passages", Adnan Tahirovic et al. proposed a method based on bidirectional RRT to address the above problem, using image processing to mark narrow channels and their entrances and exits so that bidirectional RRT can quickly guide to these channels and combine deterministic algorithms to find paths through them. In the paper "RJ-RRT: Improved RRT for Path Planning in Narrow Passages", Cai Qisen et al. proposed a new method that combines particle swarm optimization (PSO) technology with PRM. By sharing free space information with sampling points initially deployed in the obstacle area, the connectivity of the undirected graph is increased without increasing the total sampling time, thereby improving the utilization of sampling points and the success rate of narrow channel path planning. Although these algorithms can efficiently find paths through narrow passages in complex environments, the smoothness of the final path is poor, and it may be very close to obstacles, making it impossible to ensure the safety of the path.

[0004] In summary, the existing path planning methods still have the following problems:

[0005] 1. In narrow channels, sampling points are often used to generate paths, ignoring the smoothness of the path, which does not conform to the kinematic characteristics of the AUV;

[0006] 2. Due to the randomness of the sampling points, the generated path may be close to obstacles, and the safety of the path cannot be ensured;

[0007] 3. In online path planning, due to the limited sonar detection capability, it is impossible to directly obtain complete environmental information. When obstacles are beyond the sonar perception range, the AUV often chooses to pass through narrow channels in order to improve speed. As the AUV moves forward, it may be discovered that the planned path is not a passable channel but a concave trap, which causes the AUV to fall into the concave trap. Summary of the invention

[0008] The purpose of the present invention is to solve the problems of poor smoothness and safety of the planned path in the existing methods and the AUV easily falling into a concave trap, and to propose a narrow channel traversal method based on the perpendicular line of the channel.

[0009] The technical solution adopted by the present invention to solve the above technical problems is: a method for passing through a narrow channel based on the vertical line of the channel, the method specifically comprising the following steps:

[0010] Step 1, initialization time t=1;

[0011] Step 2: Use the sonar deployed on the AUV to obtain obstacle information in the environment, and use the global path planning algorithm to plan the AUV path at time t;

[0012] Step 3: Determine whether there are gaps between obstacles in the local map based on the acquired obstacle information;

[0013] If there are no gaps between obstacles in the local map at time t, the AUV navigates along the path planned in step 2 at time t until it reaches time t+1, and then executes step 6;

[0014] If there are gaps between obstacles in the local map at time t, execute step 4;

[0015] Step 4: Calculate the minimum distance d between obstacles on both sides of the i-th gap i,min , and compare d i,min , as well as The size of is the minimum safe passage width, For the absolute safe passage width,

[0016] If there is a satisfying If there is a gap, continue to step 5;

[0017] If there is no satisfaction If there is a gap, the AUV will navigate along the path planned in step 2 at time t until it reaches time t+1, and then execute step 6;

[0018] Step 5: Use the narrow channel navigation method to perform local path planning, and select a navigation path based on the local path planning result and the global path planning result of step 2. The AUV navigates along the selected path until it reaches time t+1, and then executes step 6;

[0019] Step 6: Set t=t+1 and return to step 2.

[0020] Furthermore, the local path planning is performed by using the narrow channel traversal method, specifically:

[0021] Step 51: Let the obstacles on both sides of the i-th gap be A and B respectively, and then the minimum distance d i,min The corresponding point on obstacle A is denoted as A1, and the minimum distance d i,min The corresponding point on obstacle B is denoted as B1, and the midpoint of line segment A1B1 is denoted as P mid ;

[0022] Passing midpoint P mid Draw a perpendicular line and place the distance d between the perpendicular line and the midpoint. safe The point is denoted as P sub1 and P sub2 , and then from point P sub1 and P sub2 Select the point closer to the AUV and record the selected point as P sub , and the selected points are used as local target points;

[0023] Among them, d safe is the minimum safe distance;

[0024] Step 52: Combine the AUV kinematic model and the polynomial trajectory planning algorithm to plan a trajectory from the AUV current position to the local target point P sub The planned trajectory has a starting heading of the AUV’s current heading and a terminal heading from the local target point P sub Point to the channel midpoint P mid ;

[0025] Step 53: Similarly, for each satisfaction The gaps are processed separately.

[0026] Furthermore, the kinematic model of the AUV in a two-dimensional plane is:

[0027]

[0028] Where η = [x, y, ψ] T , is the first derivative of η, (x, y) is the position of the AUV, ψ is the Euler angle, the superscript T represents the transpose, υ = [u, v, r] T is the velocity vector, u is the velocity of the AUV along the x-axis of the carrier coordinate system, v is the velocity of the AUV along the y-axis of the carrier coordinate system, r is the angular velocity of the AUV rotating around the z-axis of the carrier coordinate system, and J Θ (η) is the rotation transformation matrix from the carrier coordinate system to the earth coordinate system, J Θ (η)∈R 3×3 , R is a real number.

[0029] Furthermore, the rotation transformation matrix J Θ (η) is specifically:

[0030]

[0031] Furthermore, the specific process of step 52 is as follows:

[0032] The AUV is defined to move in translation along the x-axis and y-axis of the carrier coordinate system and to rotate around the z-axis. The AUV has only a forward velocity u along the x-axis, and the velocity remains constant during navigation. The lateral velocity v along the y-axis is 0.

[0033] The initial time of AUV motion is recorded as t0, and the initial boundary condition is recorded as [x u ,y u ,ψ u ] T ,(x u ,y u ) is the position of the AUV at the initial time, ψ u is the Euler angle of the AUV at the initial time; the end time of the AUV motion is recorded as t f The termination boundary condition is recorded as [x sub ,y sub ,ψ sub ] T ,(x sub ,y sub ) is the position of the AUV at the end time, ψ sub is the Euler angle of the AUV at the termination time;

[0034] The planned distance from the AUV current position to the local target point P sub The trajectory equation is:

[0035]

[0036] Among them, a0, a1, b0, b1, b2 and b3 are the coefficients of the trajectory equation, and (x(t), y(x)) is the position of the AUV at time t.

[0037] Furthermore, the coefficients a0 and a1 are:

[0038]

[0039] Furthermore, the coefficients b0, b1, b2 and b3 are:

[0040] [b0 b1 b2 b3] T =B -1 Y (5)

[0041] Among them, B and Y are intermediate variable matrices, and the superscript -1 represents the inverse of the matrix.

[0042] Furthermore, the intermediate variable matrix B is:

[0043]

[0044] The intermediate variable matrix Y is:

[0045] Y=[y u tanψ u y sub tanψ sub ] T (7).

[0046] Furthermore, the navigation path is selected according to the local path planning result and the global path planning result, specifically:

[0047] For global path planning: take the end point of the planning result of the global path planning method at the current moment as the local target point of the global path planning method, calculate the path length l1 of the AUV to the local target point of the global path planning method, then calculate the Euclidean distance l2 from the local target point of the global path planning method to the end point, and then calculate the sum l of l1 and l2;

[0048] For the local path planning of the i-th gap: calculate the AUV to reach the local target point P mid The path length h 1i , and then calculate the local target point P mid Euclidean distance to the end point h 2i , and then calculate h 1i With h 2i The sum of h i , i=1,2,Ω,I;

[0049] From l, h i, i=1,2,Ω,I select the minimum value, and take the path corresponding to the minimum value as the selected navigation path.

[0050] Furthermore, the global path planning algorithm is an A* algorithm.

[0051] The beneficial effects of the present invention are:

[0052] The present invention uses the vertical line in the narrow channel to select the local target point, and then uses the polynomial trajectory planning method to plan the path from the current position to the local target point, ensuring the smoothness and safety of the path. In addition, the AUV can observe the environment during the navigation process to the local target point, effectively preventing the AUV from falling into a concave trap.

[0053] Moreover, the position of the local target point is continuously updated according to the environmental information to adjust the posture of the AUV, further improving the safety of the AUV passing through narrow channels. BRIEF DESCRIPTION OF THE DRAWINGS

[0054] Figure 1 A flow chart of a method for traversing a narrow channel based on a vertical line in the channel according to the present invention;

[0055] Figure 2 A schematic diagram of a narrow channel traversal method of the present invention;

[0056] The fan-shaped area is the sonar detection range, the dotted line represents the sonar beam, the dotted line is the narrow channel observed by the AUV, and the double-dotted line is the perpendicular midline of the channel;

[0057] Figure 3 This is a simulation schematic diagram of the first planning of a narrow passage path according to the present invention;

[0058] The dotted line is the entrance of the channel observed by the AUV, and the hollow circle is the position of the AUV after it sails from the current position along the initially planned polynomial trajectory planning path to the next moment;

[0059] Figure 4 This is a schematic diagram of the simulation of the second planning of the narrow channel passage path according to the present invention;

[0060] The hollow circle is the position after the AUV sails from the current position along the new polynomial trajectory planning path to the next moment;

[0061] Figure 5 A complete simulation schematic diagram of the present invention for planning a path through a narrow passage;

[0062] The hollow square indicates the actual location of the AUV at the end point;

[0063] Figure 6 Schematic diagram of path length estimation for the global path planning algorithm and the narrow channel navigation method. DETAILED DESCRIPTION

[0064] Specific implementation method 1: Combination Figure 1 This embodiment describes a method for traversing a narrow channel based on a perpendicular line in the channel, and the method specifically includes the following steps:

[0065] Step 1, initialization time t=1;

[0066] Step 2: Use the sonar deployed on the AUV to obtain obstacle information in the environment, and use the global path planning algorithm to plan the AUV path at time t;

[0067] Step 3: Determine whether there are gaps between obstacles in the local map (i.e., the local map of the area where the AUV is currently located) based on the acquired obstacle information;

[0068] If there are no gaps between obstacles in the local map at time t, the AUV navigates along the path planned in step 2 at time t until it reaches time t+1, and then executes step 6;

[0069] If there are gaps between obstacles in the local map at time t, execute step 4;

[0070] Step 4: Calculate the minimum distance d between obstacles on both sides of the i-th gap i,min , and compare d i,min , as well as The size of is the minimum safe passage width, For the absolute safe passage width,

[0071] For obstacles A and B on both sides of the i-th gap, for any point detected by the sonar on obstacle A, the distance between the point and each sonar data point on obstacle B can be calculated respectively. After traversing each data point on obstacle A, the minimum distance among all the calculated distances is obtained. The data points corresponding to the minimum distance are A1 and B1.

[0072] If there is a satisfying If there is a gap of (then it means that the i-th gap is a narrow channel), then continue to step 5;

[0073] If there is no satisfaction If there is a gap, the AUV will navigate along the path planned in step 2 at time t until it reaches time t+1, and then execute step 6;

[0074] Step 5: Use the narrow channel navigation method to perform local path planning, and select a navigation path based on the local path planning result and the global path planning result of step 2. The AUV navigates along the selected path until it reaches time t+1, and then executes step 6;

[0075] Step 6: Set t=t+1 and return to step 2.

[0076] It should be noted that: as the AUV approaches the local target point, the observation integrity of the narrow channel increases. If the narrow channel is determined to be passable, the AUV will pass along the narrow channel; if it is observed that the narrow channel ahead is not feasible, the AUV can promptly select other feasible paths to ensure the safety of the path.

[0077] like Figure 2 FIG. 1 is a schematic diagram of a narrow channel traversal method of the present invention. Figure 3 , Figure 4 and Figure 5 This is a result diagram of the narrow passage travel path planned by the present invention.

[0078] Specific implementation method 2: This implementation method is different from the specific implementation method 1 in that the narrow channel traversal method is used for local path planning, specifically:

[0079] Step 51: Let the obstacles on both sides of the i-th gap be A and B respectively, and then the minimum distance d i,min The corresponding point on obstacle A is denoted as A1, and the minimum distance d i,min The corresponding point on obstacle B is denoted as B1, and the midpoint of line segment A1B1 is denoted as P mid ;

[0080] Passing midpoint P mid Draw a perpendicular line and place the distance d between the perpendicular line and the midpoint. safe The point is denoted as P sub1 and P sub2 , and then from point P sub1 and P sub2 Select the point closer to the AUV and record the selected point as P sub , and the selected points are used as local target points;

[0081] Among them, d safe For the minimum safety distance, set d safe It can prevent the AUV from getting too close to obstacles and ensure the safety of the planned path;

[0082] Using the vertical line in the channel to select the local target point can ensure that the AUV navigates in the middle of the narrow channel;

[0083] Step 52: Combine the AUV kinematic model and the polynomial trajectory planning algorithm to plan a trajectory from the AUV current position to the local target point P sub The planned trajectory has a starting heading of the AUV’s current heading and a terminal heading from the local target point P sub Point to the channel midpoint P mid ;

[0084] Step 53: Similarly, for each satisfaction The gaps are processed separately.

[0085] The other steps and parameters are the same as those in the first embodiment.

[0086] Determining the terminal heading helps the AUV to observe the channel exit and facilitates the reasonable planning of the subsequent travel path.

[0087] Specific implementation method three: This implementation method is different from specific implementation methods one or two in that the kinematic model of the AUV in a two-dimensional plane is:

[0088]

[0089] Where η = [x, y, ψ] T , is the first derivative of η, (x, y) is the position of the AUV, ψ is the Euler angle, the superscript T represents the transpose, υ = [u, v, r] T is the velocity vector, u is the velocity of the AUV along the x-axis of the carrier coordinate system, v is the velocity of the AUV along the y-axis of the carrier coordinate system, r is the angular velocity of the AUV rotating around the z-axis of the carrier coordinate system, and J Θ (η) is the rotation transformation matrix from the carrier coordinate system to the earth coordinate system, J Θ (η)∈R 3×3 , R is a real number.

[0090] The other steps and parameters are the same as those in the first or second embodiment.

[0091] The carrier coordinate system takes the mass center of the AUV as the center of the circle, the forward direction as the positive direction of the x-axis, the right direction as the positive direction of the y-axis, and the downward direction as the positive direction of the z-axis.

[0092] Specific implementation method 4: This implementation method is different from any one of the specific implementation methods 1 to 3 in that the rotation transformation matrix J Θ (η) is specifically:

[0093]

[0094] The other steps and parameters are the same as those in Specific Embodiments 1 to 3.

[0095] Specific implementation method 5: This implementation method is different from specific implementation methods 1 to 4 in that the specific process of step 52 is as follows:

[0096] The AUV is defined to move in translation along the x-axis and y-axis of the carrier coordinate system and to rotate around the z-axis. The AUV has only a forward velocity u along the x-axis, and the velocity remains constant during navigation. The lateral velocity v along the y-axis is 0.

[0097] The initial time of AUV motion is recorded as t0, and the initial boundary condition is recorded as [x u ,y u ,ψ u ] T ,(x u ,y u ) is the position of the AUV at the initial time, ψ u is the Euler angle of the AUV at the initial time; the end time of the AUV motion is recorded as t f The termination boundary condition is recorded as [x sub ,y sub ,ψ sub ] T ,(x sub ,y sub ) is the position of the AUV at the end time, ψ sub is the Euler angle of the AUV at the termination time;

[0098] The planned distance from the AUV current position to the local target point P sub The trajectory equation is:

[0099]

[0100] Among them, a0, a1, b0, b1, b2 and b3 are the coefficients of the trajectory equation, and (x(t), y(x)) is the position of the AUV at time t.

[0101] The other steps and parameters are the same as those in Specific Embodiments 1 to 4.

[0102] Specific implementation method 6: This implementation method is different from any one of the specific implementation methods 1 to 5 in that the coefficients a0 and a1 are:

[0103]

[0104] The other steps and parameters are the same as those in Specific Implementation Methods 1 to 5.

[0105] Specific implementation method 7: This implementation method is different from any one of specific implementation methods 1 to 6 in that the coefficients b0, b1, b2 and b3 are:

[0106] [b0 b1 b2 b3] T =B-1 Y (5)

[0107] Among them, B and Y are intermediate variable matrices, and the superscript -1 represents the inverse of the matrix.

[0108] The other steps and parameters are the same as those in Specific Embodiments 1 to 6.

[0109] Specific implementation eight: This implementation differs from any one of specific implementations one to seven in that the intermediate variable matrix B is:

[0110]

[0111] The intermediate variable matrix Y is:

[0112] Y=[y u tanψ u y sub tanψ sub ] T (7)

[0113] The other steps and parameters are the same as those in Specific Embodiments 1 to 7.

[0114] Specific implementation method nine: Combination Figure 6 This embodiment is different from the first to eighth embodiments in that the navigation path is selected according to the local path planning result and the global path planning result, specifically:

[0115] For global path planning: take the end point of the planning result of the global path planning method at the current moment as the local target point of the global path planning method, calculate the path length l1 of the AUV to the local target point of the global path planning method, then calculate the Euclidean distance l2 from the local target point of the global path planning method to the end point, and then calculate the sum l of l1 and l2;

[0116] It should be noted that if the local target point of the global path planning result is the end point, the Euclidean distance l2 from the local target point to the end point is 0;

[0117] For the local path planning of the i-th gap: calculate the AUV to reach the local target point P mid The path length h 1i , and then calculate the local target point P mid Euclidean distance to the end point h 2i , and then calculate h 1i With h 2i The sum of h i , i=1,2,...,I;

[0118] From l, h i, select the minimum value among i=1,2,…,I, and take the path corresponding to the minimum value as the selected navigation path.

[0119] The other steps and parameters are the same as those in Specific Embodiments 1 to 8.

[0120] Specific implementation method ten: This implementation method is different from any one of specific implementation methods one to nine in that the global path planning algorithm is an A* algorithm.

[0121] The other steps and parameters are the same as those in Specific Embodiments 1 to 9.

[0122] The global path planning algorithm that can be used in this embodiment includes but is not limited to the A* algorithm.

[0123] The above calculation examples of the present invention are only used to explain the calculation model and calculation process of the present invention in detail, and are not intended to limit the implementation methods of the present invention. For ordinary technicians in the relevant field, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation methods here. All obvious changes or modifications derived from the technical solution of the present invention are still within the scope of protection of the present invention.

Claims

1. A method for traversing a narrow channel based on a perpendicular line in the channel, characterized in that: The method specifically comprises the following steps: Step 1, initialization time t=1; Step 2: Use the sonar deployed on the AUV to obtain obstacle information in the environment, and use the global path planning algorithm to plan the AUV path at time t; Step 3: Determine whether there are gaps between obstacles in the local map based on the acquired obstacle information; If there are no gaps between obstacles in the local map at time t, the AUV navigates along the path planned in step 2 at time t until it reaches time t+1, and then executes step 6; If there are gaps between obstacles in the local map at time t, execute step 4; Step 4: Calculate the minimum distance d between obstacles on both sides of the i-th gap i,min , and compare d i,min , as well as The size of is the minimum safe passage width, For the absolute safe passage width, If there is a satisfying If there is a gap, continue to step 5; If there is no satisfaction If there is a gap, the AUV will navigate along the path planned in step 2 at time t until it reaches time t+1, and then execute step 6; Step 5: Use the narrow channel navigation method to perform local path planning, and select a navigation path based on the local path planning result and the global path planning result of step 2. The AUV navigates along the selected path until it reaches time t+1, and then executes step 6; Step 6: Set t=t+1 and return to step 2.

2. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 1, characterized in that: The local path planning using the narrow channel traversal method is specifically as follows: Step 51: Let the obstacles on both sides of the i-th gap be A and B respectively, and then the minimum distance d i,min The corresponding point on obstacle A is denoted as A1, and the minimum distance d i,min The corresponding point on obstacle B is denoted as B1, and the midpoint of line segment A1B1 is denoted as P mid ; Passing midpoint P mid Draw a perpendicular line and place the distance d between the perpendicular line and the midpoint safe The point is denoted as P sub1 and P sub2 , and then from point P sub1 and P sub2 Select the point closer to the AUV and record the selected point as P sub , and the selected points are used as local target points; Among them, d safe is the minimum safe distance; Step 52: Combine the AUV kinematic model and the polynomial trajectory planning algorithm to plan a trajectory from the AUV current position to the local target point P sub The planned trajectory has a starting heading of the AUV’s current heading and a terminal heading from the local target point P sub Point to the channel midpoint P mid ; Step 53: Similarly, for each satisfaction The gaps are processed separately.

3. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 2, characterized in that: The kinematic model of the AUV in a two-dimensional plane is: Where η = [x, y, ψ] T , is the first derivative of η, (x,y) is the position of the AUV, ψ is the Euler angle, the superscript T represents the transpose, υ = [u, v, r] T is the velocity vector, u is the velocity of the AUV along the x-axis of the carrier coordinate system, v is the velocity of the AUV along the y-axis of the carrier coordinate system, r is the angular velocity of the AUV rotating around the z-axis of the carrier coordinate system, and J Θ (η) is the rotation transformation matrix from the carrier coordinate system to the earth coordinate system, J Θ (η)∈R 3×3 , R is a real number.

4. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 3, characterized in that: The rotation transformation matrix J Θ (η) is specifically:

5. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 2, characterized in that: The specific process of step 52 is as follows: The AUV is defined to move in translation along the x-axis and y-axis of the carrier coordinate system and to rotate around the z-axis. The AUV has only a forward velocity u along the x-axis, and the velocity remains constant during navigation. The lateral velocity v along the y-axis is 0. The initial time of AUV motion is recorded as t0, and the initial boundary condition is recorded as [x u ,y u ,ψ u ] T ,(x u ,y u ) is the position of the AUV at the initial time, ψ u is the Euler angle of the AUV at the initial time; the end time of the AUV motion is recorded as t f The termination boundary condition is recorded as [x sub ,y sub ,ψ sub ] T ,(x sub ,y sub ) is the position of the AUV at the end time, ψ sub is the Euler angle of the AUV at the termination time; The planned distance from the AUV current position to the local target point P sub The trajectory equation is: Among them, a0, a1, b0, b1, b2 and b3 are the coefficients of the trajectory equation, and (x(t), y(x)) is the position of the AUV at time t.

6. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 5, characterized in that: The coefficients a0 and a1 are:

7. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 5, characterized in that: The coefficients b0, b1, b2 and b3 are: <h2 style=";text-align:left;direction:ltr">[b0 b1 b2 b3]<h2 style=";text-align:left;direction:ltr"> T <h2 style=";text-align:left;direction:ltr"> =B<h2 style=";text-align:left;direction:ltr"> -1 <h2 style=";text-align:left;direction:ltr"> Y (5) Among them, B and Y are intermediate variable matrices, and the superscript -1 represents the inverse of the matrix.

8. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 7, characterized in that: The intermediate variable matrix B is: The intermediate variable matrix Y is: And=[and u tanψ u and sub tanψ sub ] T (7)。 9. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 1, characterized in that: The navigation path is selected according to the local path planning result and the global path planning result, specifically: For global path planning: take the end point of the planning result of the global path planning method at the current moment as the local target point of the global path planning method, calculate the path length l1 of the AUV to the local target point of the global path planning method, then calculate the Euclidean distance l2 from the local target point of the global path planning method to the end point, and then calculate the sum l of l1 and l2; For the local path planning of the i-th gap: calculate the AUV to reach the local target point P mid The path length h 1i , and then calculate the local target point P mid Euclidean distance to the end point h 2i , and then calculate h 1i With h 2i The sum of h i , i=1,2,Ω,I; From l, h i , i=1,2,Ω,I select the minimum value, and take the path corresponding to the minimum value as the selected navigation path.

10. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 1, characterized in that: The global path planning algorithm is the A* algorithm.

Citation Information

Patent Citations

  • Global path planning method based on physical size of fire-fighting robot

    CN113791617A

  • Narrow road passing method and device, robot and computer readable storage medium

    CN114326711A

  • Unmanned aerial vehicle moving platform target searching method and system

    CN115047917A

  • Collision-prevention local path planning method for unmanned ship in narrow water channel

    CN115373394A

  • Narrow door passing method and device of patrol robot, medium and equipment

    CN117420831A

Cited By

  • Mobile robot continuous space path planning method for narrow roadway

    CN119828702A