A method for passing through a narrow channel based on the perpendicular bisector of the channel

Abstract

A method for passing through a narrow channel based on the perpendicular bisector of the channel belongs to the technical field of AUV path planning. The present invention solves the problems of poor smoothness and safety of the planned path and the easy entrapment of the AUV in a concave trap in the existing methods. The present invention uses the perpendicular bisector of the narrow channel to select local target points, and then adopts a polynomial trajectory planning method to plan the path from the current position to the local target points, ensuring the smoothness and safety of the path. Moreover, the AUV can observe the environment during the navigation to the local target points, effectively preventing the AUV from falling into a concave trap. At the same time, the present invention continuously updates the position of the local target points according to the environmental information to adjust the attitude of the AUV, further improving the safety of the AUV passing through the narrow channel. The method of the present invention can be applied to the technical field of AUV path planning.

Description

Technical Field

[0001] The present invention belongs to the technical field of AUV path planning, and particularly relates to a method for passing through a narrow channel based on the perpendicular bisector of the channel. Background Technique

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

[0003] At present, there is little theoretical research on the AUV passing through narrow channels. Most of the existing research uses probability sampling algorithms, and the algorithms are specifically divided into two categories: algorithms based on probabilistic roadmap (PRM) and rapidly exploring random tree (RRT). Due to the small configuration space of narrow channels, the probability sampling algorithms generally have the problem that they cannot effectively explore the configuration space and thus it is difficult to find a path. Although deterministic algorithms, such as the A* algorithm, can also find the optimal path solution, the computational complexity is large and time-consuming. In the literature "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 for the above problems, using image processing to mark narrow channels and their entrances and exits so that bidirectional RRT can quickly guide to these channels, and combining with a deterministic algorithm to find a path through them. In the literature "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, thus increasing the connectivity of the undirected graph without increasing the total sampling time, improving the utilization rate of sampling points and the success rate of narrow channel path planning. Although these algorithms can efficiently find a path through narrow channels in a complex environment, the smoothness of the final path is very poor, and there may be a situation very close to obstacles, unable 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, paths are often generated using sampling points, ignoring the smoothness of the path, which does not conform to the kinematic characteristics of AUVs;

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

[0007] 3. In online path planning, due to the limited sonar detection ability, the complete environmental information cannot be directly obtained. And when obstacles are beyond the sonar perception range, AUVs often choose to pass through narrow channels to improve speed. It may be that as the AUV moves forward, it is found that the planned path is not a passable channel but a concave trap, which may cause 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, poor safety of the planned path and the easy falling of AUVs into concave traps in the existing methods, and a narrow channel passing method based on the perpendicular bisector of the channel is proposed.

[0009] The technical solution adopted by the present invention to solve the above technical problems is: a narrow channel passing method based on the perpendicular bisector of the channel, and the method specifically includes the following steps:

[0010] Step 1: Initialize the 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: Combine the obtained obstacle information to judge whether there are gaps between obstacles in the local map;

[0013] If there are no gaps between obstacles in the local map at time t, the AUV sails along the path planned in Step 2 at time t until it reaches time t + 1, and then execute 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 the sizes of d i,min , and . is the minimum safe passing width, is the absolute safe passing width,

[0016] If there is a gap that satisfies , then proceed to Step Five;

[0017] If there is no gap that satisfies , the AUV sails along the path planned in Step Two at time t until it reaches time t + 1, and then proceed to Step Six;

[0018] Step Five: Adopt the narrow - channel traversal method for local path planning, select the sailing path according to the local path planning result and the global path planning result of Step Two, and the AUV sails along the selected path until it reaches time t + 1, and then proceed to Step Six;

[0019] Step Six: Let t = t + 1, and return to execute Step Two.

[0020] Furthermore, the local path planning by adopting the narrow - channel traversal method is specifically as follows:

[0021] Step Five - One: Denote the obstacles on both sides of the i - th gap as A and B respectively, and then denote the point on obstacle A corresponding to the minimum distance d i,min as A 1 , and denote the point on obstacle B corresponding to the minimum distance d i,min as B 1 , and denote the mid - point of the line segment A 1 B 1 as P mid ;

[0022] Draw a perpendicular line through the mid - point P mid , and denote the points on the perpendicular line at a distance of d safe from the mid - point as P sub1 and P sub2 , then select the point closer to the AUV from points P sub1 and P sub2 , denote the selected point as P sub , and take the selected point as the local target point;

[0023] where d safe is the minimum safety distance;

[0024] Step Five - Two: Combine the AUV kinematic model and the polynomial trajectory planning algorithm to plan a trajectory from the current position of the AUV to the local target point P sub , and the starting heading of the planned trajectory is the current heading of the AUV, and the ending heading points from the local target point P sub to the mid - point P of the channel mid ;

[0025] Step Five - Three: Similarly, process each gap that satisfies .

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

[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, and υ = [u, v, r] T is the velocity vector, u is the velocity of the AUV along the x-axis of the body coordinate system, v is the velocity of the AUV along the y-axis of the body coordinate system, r is the angular velocity of the AUV rotating around the z-axis of the body coordinate system, and J Θ (η) is the rotation transformation matrix from the body coordinate system to the earth coordinate system, and J Θ (η) ∈ R 3×3 , and R is the set of real numbers.

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

[0030]

[0031] Furthermore, the specific process of Step 5-2 is as follows:

[0032] Define that the AUV makes translational motions along the x-axis and y-axis of the body coordinate system and a rotational motion around the z-axis; the AUV has only a forward velocity u in the x-axis direction and the magnitude of the velocity remains unchanged during navigation, and the lateral velocity v in the y-axis direction is 0;

[0033] Denote the initial time of the AUV's motion as t 0 , and the initial boundary conditions as [x u , y u , ψ u T , where (x u , y u ) is the position of the AUV at the initial time, and ψ u is the Euler angle of the AUV at the initial time; denote the termination time of the AUV's motion as t f , and the termination boundary conditions as [x sub , y sub , ψ sub T , where (x sub , y sub ) is the position of the AUV at the termination time, and ψ sub is the Euler angle of the AUV at the termination time;

[0034] Then, plan a path from the current position of the AUV to the local target point P​​sub The trajectory equation is:

[0035]

[0036] where a 0 and a 1 , b 0 , b 1 , b 2 and b 3 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 a 0 and a 1 are:

[0038]

[0039] Furthermore, the coefficients b 0 , b 1 , b 2 and b 3 are:

[0040] [b 0 b 1 b 2 b 3 T = B -1 Y (5)

[0041] where 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 selection of the navigation path according to the local path planning result and the global path planning result is specifically as follows:

[0047] For the 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, and calculate the path length l for the AUV to reach the local target point of the global path planning method 1 ​​, and then calculate the Euclidean distance l from the local target point to the end point of the global path planning method 2 , and then calculate l 1 With l 2 The sum of l;

[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 the simulation schematic diagram of the second planned narrow - channel traversal path for the present invention;

[0060] The hollow circles represent the positions of the AUV after sailing along the newly planned polynomial trajectory from the current position to the next moment;

[0061] Figure 5 This is the complete simulation schematic diagram of the planned path for the present invention to traverse a narrow channel;

[0062] The hollow squares represent the actual positions where the AUV reaches the end point;

[0063] Figure 6 This is the schematic diagram for estimating the path lengths of the global path - planning algorithm and the narrow - channel traversal method. Detailed implementation manners

[0064] Detailed implementation manner 1: Combined with Figure 1 Describe this implementation manner. A narrow - channel traversal method based on the perpendicular bisector of the channel in the present implementation manner specifically includes the following steps:

[0065] Step 1: Initialize the time t = 1;

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

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

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

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

[0070] Step 4: Calculate the minimum distance d between the obstacles on both sides of the i - th gap i,min , and compare the sizes of d i,min , and , is the minimum safe passing width, is the absolute safe passing width,

[0071] For the obstacles A and B on both sides of the i-th gap, for any point detected by sonar on obstacle A, the distances between this 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, and the data point corresponding to the minimum distance is A 1 and B 1 ;

[0072] If there exists a gap that satisfies (indicating that the i-th gap is a narrow channel), then continue to execute Step Five;

[0073] If there does not exist a gap that satisfies , the AUV sails along the path planned in Step Two at time t until it reaches time t + 1, and then executes Step Six;

[0074] Step Five: Adopt the narrow-channel traversal method for local path planning, and select the navigation path according to the local path planning result and the global path planning result of Step Two. The AUV sails along the selected path until it reaches time t + 1, and then executes Step Six;

[0075] Step Six: Let t = t + 1, and return to execute Step Two.

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

[0077] As Figure 2 shown is a schematic diagram of the narrow-channel traversal method of the present invention, Figure 3 , Figure 4 and Figure 5 are the result diagrams of planning the narrow-channel traversal path of the present invention.

[0078] Specific Embodiment Two: The difference between this embodiment and Specific Embodiment One is that the local path planning using the narrow-channel traversal method is specifically as follows:

[0079] Step Five One: Denote the obstacles on both sides of the i-th gap as A and B respectively, and then denote the point on obstacle A corresponding to the minimum distance d i,min as A 1 , denote the point on obstacle B corresponding to the minimum distance d i,min as B 1 , and denote the midpoint of the line segment A 1 B 1 as P mid ;

[0080] Pass through the midpoint Pmid Draw a perpendicular line, and mark the point on the perpendicular line at a distance d from the midpoint safe as point P sub1 and P sub2 , then select the point closer to the AUV from point P sub1 and P sub2 , and mark the selected point as P sub , and use the selected point as the local target point;

[0081] wherein, d safe is the minimum safety distance. By setting d safe , it can prevent the AUV from being too close to the obstacle and ensure the safety of the planned path;

[0082] Selecting the local target point using the perpendicular bisector of the channel can ensure that the AUV sails in the middle of the narrow channel;

[0083] Step Five Two: Combine the AUV kinematic model and the polynomial trajectory planning algorithm to plan a trajectory from the current position of the AUV to the local target point P sub , and the starting heading of the planned trajectory is the current heading of the AUV, and the ending heading points from the local target point P sub to the midpoint P of the channel mid ;

[0084] Step Five Three: Similarly, process each gap that satisfies .

[0085] Other steps and parameters are the same as those in the specific implementation method one.

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

[0087] Specific implementation method three: The difference between this implementation method and the specific implementation method one or two is that the kinematic model of the AUV in the two-dimensional plane is:

[0088]

[0089] In the formula, η = [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 direction of the body coordinate system, v is the velocity of the AUV along the y-axis direction of the body coordinate system, r is the angular velocity of the AUV rotating around the z-axis of the body coordinate system, J Θ (η) is the rotation transformation matrix from the body coordinate system to the earth coordinate system, J Θ (η) ∈ R3×3 , R is a real number.

[0090] Other steps and parameters are the same as those in the first or second specific implementation manner.

[0091] The body coordinate system has the centroid of the AUV as the center, the forward direction as the positive x-axis direction, the rightward direction as the positive y-axis direction, and the downward direction as the positive z-axis direction.

[0092] Specific implementation manner four: The difference between this implementation manner and one of the first to third specific implementation manners is that the rotation transformation matrix J Θ (η) is specifically:

[0093]

[0094] Other steps and parameters are the same as those in one of the first to third specific implementation manners.

[0095] Specific implementation manner five: The difference between this implementation manner and one of the first to fourth specific implementation manners is that the specific process of step five two is:

[0096] Define that the AUV makes translational motions along the x-axis and y-axis of the body coordinate system and a rotational motion around the z-axis; the AUV has only a forward velocity u in the x-axis direction and the magnitude of the velocity remains unchanged during navigation, and the lateral velocity v in the y-axis direction is 0;

[0097] Record the initial time of the AUV's motion as t 0 , and record the initial boundary conditions as [x u , y u , ψ u T , (x u , y u ) is the position of the AUV at the initial time, and ψ u is the Euler angle of the AUV at the initial time; record the termination time of the AUV's motion as t f , and record the termination boundary conditions as [x sub , y sub , ψ sub T , (x sub , y sub ) is the position of the AUV at the termination time, and ψ sub is the Euler angle of the AUV at the termination time;

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

[0099]

[0100] Where, a 0 , a 1 , b​​0 , b 1 , b 2 and b 3 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 any one of the first to fourth specific embodiments.

[0102] Specific Embodiment Six: The difference between this embodiment and any one of the first to fifth specific embodiments is that the coefficients a 0 and a 1 are:

[0103]

[0104] The other steps and parameters are the same as those in any one of the first to fifth specific embodiments.

[0105] Specific Embodiment Seven: The difference between this embodiment and any one of the first to sixth specific embodiments is that the coefficients b 0 , b 1 , b 2 and b 3 are:

[0106] [b 0 b 1 b 2 b 3 T = B -1 Y(5)

[0107] where 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 any one of the first to sixth specific embodiments.

[0109] Specific Embodiment Eight: The difference between this embodiment and any one of the first to seventh specific embodiments is 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 any one of the first to seventh specific embodiments.

[0114] Specific Embodiment Nine: In combination with​​Figure 6 Describe this embodiment. The difference between this embodiment and any one of the first to eighth specific embodiments is that when selecting a navigation path 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, and calculate the path length l of the AUV reaching the local target point of the global path planning method 1 , and then calculate the Euclidean distance l from the local target point of the global path planning method to the end point 2 , and then calculate l 1 and l 2 The sum l;

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

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

[0118] Select the minimum value from l, h i , i = 1, 2,..., I, and take the path corresponding to the minimum value as the selected navigation path.

[0119] Other steps and parameters are the same as any one of the first to eighth specific embodiments.

[0120] Specific embodiment ten: The difference between this embodiment and any one of the first to ninth specific embodiments is that the global path planning algorithm is the A* algorithm.

[0121] Other steps and parameters are the same as any one of the first to ninth specific embodiments.

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

[0123] The above calculation examples of the present invention are only for explaining in detail the calculation model and calculation process of the present invention, rather than limiting the implementation manners of the present invention. For those of ordinary skill in the art, other different forms of changes or modifications can be made based on the above description. It is impossible to list all the implementation manners here. Any obvious changes or modifications derived from the technical solutions of the present invention still fall within the protection scope 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; 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; 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 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.

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

4. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 1, 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.

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

6. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 4, characterized in that: The coefficients b0, b1, b2 and b3 are: [b0 b1 b2 b3] T =B -1 Y (5)Where B and Y are intermediate variable matrices, the superscript -1 represents the inverse of the matrix.

7. A method for traversing a narrow channel based on a perpendicular line in the channel according to claim 6, 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)。 8. 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 , select the minimum value among i=1,2,…,I, and take the path corresponding to the minimum value as the selected navigation path.

9. 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.

Images