Space path planning method for underwater flexible mechanical arm

Abstract

The invention discloses a spatial path planning method for an underwater flexible mechanical arm, and aims to solve a solution meeting requirements randomly by utilizing a geometrical relationship under a local coordinate system through multiple coordinate transformation according to coordinates and direction vectors of a to-be-grabbed target in order to solve the problems that an existing mechanical arm is complex in control and the precision is easy to influence. And finally, converting to a world coordinate to obtain a path of the center of the underwater flexible mechanical arm, further obtaining paths of four ropes of the underwater flexible mechanical arm according to the path of the center of the underwater flexible mechanical arm, and completing rapid path planning of the underwater flexible mechanical arm. According to the method, through simple mathematical calculation and logic judgment, the calculation amount is greatly reduced, and on the premise that the control precision is guaranteed, the time of grabbing of the underwater flexible mechanical arm every time can be effectively shortened.

Description

Technical Field

[0001] The present invention belongs to the technical field of flexible manipulator path planning, and in particular relates to the design of a spatial path planning method for an underwater flexible manipulator. Background Art

[0002] Nearly 70% of the Earth is covered by water, and the resources contained therein account for a significant proportion. As humanity continues to develop, it is inevitable that it will seek ways to exploit these underwater resources. Given the complexities of the underwater world, the use of mechanical equipment to replace humans in this task is currently a popular approach, with the development of underwater robots and robotic arms.

[0003] Traditional underwater robotic arms, similar to human limbs, use multiple single-dimensional joints to achieve multiple degrees of freedom. This inevitably results in the arm being too large and complex to control. Certain environments may require specific bending, which traditional robotic arms cannot achieve. Furthermore, traditional robotic arms require control of each joint during the control process, requiring calculation of the position and movement of each joint, making control more complex. A deviation in the control accuracy of one joint will inevitably affect the overall movement, preventing the end point from reaching the designated position. Furthermore, obstacle avoidance in the path of traditional robotic arms is complex, and obstacle coordinates cannot be intuitively determined. Multiple joints must be adjusted to avoid obstacles. Summary of the Invention

[0004] The purpose of this invention is to solve the problem that the existing robotic arm control is complex and the accuracy is easily affected. A method for spatial path planning of an underwater flexible robotic arm is proposed, which avoids obstacles in the path and quickly and effectively plans the underwater flexible robotic arm path to complete the grasping task.

[0005] The technical solution of the present invention is: a method for spatial path planning of an underwater flexible manipulator, comprising the following steps: S1. Obtain the length, world coordinate system, world coordinates of the target point, and direction vector of the underwater flexible manipulator.

[0006] S2. Obtain the origin of the local coordinate system according to the length of the underwater flexible manipulator and construct the local coordinate system.

[0007] S3. Determine a first plane where the first arc segment is located in the world coordinate system, and determine a second plane where the second arc segment is located in the local coordinate system.

[0008] S4. Calculate a rotation coordinate transformation matrix from the first plane to the second plane according to the world coordinates of the target point and the direction vector.

[0009] S5. Convert the world coordinates of the second plane into local coordinates according to the rotation coordinate transformation matrix, and then calculate the local coordinates of each point on the second arc.

[0010] S6. Convert the local coordinates of the intersection of the first arc and the second arc into world coordinates according to the rotation coordinate transformation matrix, and then calculate the world coordinates of each point on the first arc.

[0011] S7. Convert the local coordinates of each point on the second arc into world coordinates according to the rotation coordinate transformation matrix.

[0012] S8. Combine the world coordinates of each point on the first arc, the world coordinates of each point on the second arc, and the world coordinates of the initial straight line segment to obtain the path of the center of the underwater flexible robotic arm.

[0013] S9. Calculate the paths of the four ropes of the underwater flexible manipulator based on the path of the center of the underwater flexible manipulator.

[0014] Furthermore, step S2 includes the following sub-steps: S21. In the world coordinate system O-xyz of y A node is obtained on the axis so that the path length of the center of the underwater flexible manipulator is greater than the length of the underwater flexible manipulator.

[0015] S22. Align the node with the origin of the world coordinate system O Any point between them is taken as the origin of the local coordinate system M .

[0016] S23. Get the intersection of the first arc and the second arc B .

[0017] S24, the origin of the local coordinate system M Intersection B The direction of the connection is x 'Axis negative direction, and make the target point C lie in Mx ' y 'In the plane, build a local coordinate system Mx ' y ' z '.

[0018] Furthermore, step S3 is specifically as follows: Oxy The plane is taken as the first plane, and the local coordinate system Mx ' y ' plane as the second plane.

[0019] Furthermore, step S4 includes the following sub-steps: S41, according to the target point C The world coordinates and direction vector S Get the normal vector of the second plane and parallel toz The axis vector is used as the normal vector of the first plane.

[0020] S42. Calculate the angle between the first plane and the second plane according to the normal vector of the first plane and the normal vector of the second plane.

[0021] S43. Obtain a rotation coordinate transformation matrix from the first plane to the second plane according to the angle between the first plane and the second plane: in Indicates the direction from the first plane to the second plane x The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane x The rotation angle of the axis, Indicates the direction from the first plane to the second plane y The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane y The rotation angle of the axis, Indicates the direction from the first plane to the second plane z The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane z The rotation angle of the axis.

[0022] Furthermore, step S5 includes the following sub-steps: S51. Multiply the world coordinates of the second plane by the rotation coordinate transformation matrix to convert them into local coordinates of the second plane.

[0023] S52. Calculate the local coordinates of each point on the second arc according to the local coordinates of the second plane: in represents the number of solution steps, The angle representing the center angle of the second arc segment, Indicates from 0 to The fixed angles increase successively, represents the equal division calculation function, represents the absolute value function, represents the local horizontal coordinate of each point on the second arc, represents the local ordinate of each point on the second arc, Indicates the intersection of the first arc and the second arc B The local abscissa of Indicates the local coordinates of the center point of the second arc segment.

[0024] Furthermore, step S6 includes the following sub-steps: S61, the intersection of the first arc and the second arc B The local coordinates are divided by the rotation coordinate transformation matrix to convert to the intersection point B The world coordinates of .

[0025] S62, according to the intersection B The world coordinates of each point on the first arc are calculated by: in represents the number of solution steps, The angle representing the center angle of the first arc segment, Indicates from 0 to The fixed angles increase successively, represents the equal division calculation function, represents the absolute value function, Represents the world horizontal coordinate of each point on the first arc, Represents the world ordinate of each point on the first arc, Indicates the intersection of the first arc and the second arc B The world horizontal coordinate, Indicates the intersection of the first arc segment and the initial straight line segment A The world vertical coordinate.

[0026] Furthermore, step S7 specifically includes: dividing the local coordinates of each point on the second arc by the rotation coordinate transformation matrix to convert them into the world coordinates of each point on the second arc.

[0027] Furthermore, step S9 includes the following sub-steps: S91. Calculate the world coordinates of the first path of the four ropes of the underwater flexible manipulator based on the world coordinates of each point on the first arc and the world coordinates of the initial straight line segment: in Represents the world coordinates of each point in the first path of the first rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first path of the second rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first segment of the path of the third rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first segment of the fourth rope of the underwater flexible manipulator, Represents the initial straight line segment OA and the first arc AEB The world horizontal coordinate of each point on Represents the initial straight line segment OA and the first arc AEB The world ordinate of each point on the Represents the width of the underwater flexible manipulator.

[0028] S92, take the end point of the first path of the four ropes of the underwater flexible manipulator as the starting point of the second path, multiply the world coordinates of the starting point of the second path by the rotation coordinate transformation matrix, convert it into the local coordinates of the starting point of the second path, and project it to y 'Axis, get the projection distance .

[0029] S93, according to the projection distance Get the radius of the second path of the four ropes of the underwater flexible manipulator : in Indicates the initial design radius of the second path of the four ropes of the underwater flexible manipulator.

[0030] S94, according to the radius of the second path of the four ropes of the underwater flexible manipulator Get the local coordinates of the second path of the four ropes of the underwater flexible manipulator : in represents the arc angle of the second path of the four ropes of the underwater flexible manipulator, Indicates that the points on the second path are in the local coordinate system z 'Axis coordinate values.

[0031] S95. Divide the local coordinates of the second path segment of the four ropes of the underwater flexible manipulator by the rotation coordinate transformation matrix to obtain the world coordinates of the second path segment of the four ropes of the underwater flexible manipulator.

[0032] S96. Obtain the paths of the four ropes of the underwater flexible manipulator according to the world coordinates of the first path segment and the world coordinates of the second path segment of the four ropes of the underwater flexible manipulator.

[0033] The beneficial effects of the present invention are: (1) The present invention only needs to input information such as the target point coordinates and the grasping direction vector of the robotic arm to directly plan the paths of the four ropes of the underwater flexible robotic arm, and then integrate the path of the entire underwater flexible robotic arm. Its lightweight algorithm based on mathematical calculation and logical judgment greatly reduces the calculation complexity. Under the premise of ensuring control accuracy, it can effectively reduce the time of each grasping of the underwater flexible robotic arm.

[0034] (2) The present invention plans the paths of the four ropes of the underwater flexible manipulator, and the ropes drive the manipulator. The flexible transmission characteristics of the ropes enable the manipulator to flexibly bypass complex obstacles and achieve bionic winding motion, which is particularly suitable for narrow or unstructured underwater environments. The efficient transmission of the ropes (efficiency exceeds 90%) and the nearly silent operation characteristics give it unique advantages in noise-sensitive tasks.

[0035] (3) The present invention adopts four-rope redundant drive control, and its fault tolerance further enhances its practicality in scenarios such as deep-sea exploration, underwater rescue, and marine ranching. In the future, the performance can be further improved by combining intelligent algorithm optimization and advanced material application. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] Figure 1 Shown is a flow chart of a spatial path planning method for an underwater flexible robotic arm provided by an embodiment of the present invention.

[0037] Figure 2 Shown is a schematic diagram of spatial path planning of an underwater flexible robotic arm provided by an embodiment of the present invention.

[0038] Figure 3 Shown is a schematic diagram of path planning for the center of an underwater flexible robotic arm provided by an embodiment of the present invention.

[0039] Figure 4 Shown is a schematic diagram of the path planning of the four ropes of the underwater flexible robotic arm provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0040] The exemplary embodiments of the present invention will now be described in detail with reference to the accompanying drawings. It should be understood that the embodiments shown and described in the accompanying drawings are merely exemplary and are intended to illustrate the principles and spirit of the present invention, rather than to limit the scope of the present invention.

[0041] The embodiment of the present invention provides a method for spatial path planning of an underwater flexible manipulator. Figure 1 As shown, the following steps S1 to S9 are included: S1. Obtain the length, world coordinate system, world coordinates of the target point, and direction vector of the underwater flexible manipulator.

[0042] like Figure 2 As shown, in the embodiment of the present invention, the world coordinate system is O-xyz , the target point is C , the direction vector of the target point, that is, the direction vector of the underwater flexible manipulator grasping is S .

[0043] S2. Obtain the origin of the local coordinate system according to the length of the underwater flexible manipulator and construct the local coordinate system.

[0044] Step S2 includes the following sub-steps S21 to S24: S21. In the world coordinate system O-xyz of y The path length of the center of the underwater flexible manipulator is obtained on the axis (i.e. the first arc BEA , the second arc CDB With the initial straight line segment AO The total length of the underwater flexible manipulator is greater than the length of the underwater flexible manipulator.

[0045] S22. Align the node with the origin of the world coordinate system O Any point between them is taken as the origin of the local coordinate system M .

[0046] In the embodiment of the present invention, the node and the origin of the world coordinate system O The points between them are all the local coordinate system origins that meet the conditions, so any point is selected as the local coordinate system origin M .

[0047] S23. Get the intersection of the first arc and the second arc B .

[0048] S24, such as Figure 2 As shown, the origin of the local coordinate system is M Intersection B The direction of the connection is x 'Axis negative direction, and make the target point C lie in Mx ' y 'In the plane, build a local coordinate system Mx ' y ' z '.

[0049] S3. Determine a first plane where the first arc segment is located in the world coordinate system, and determine a second plane where the second arc segment is located in the local coordinate system.

[0050] In the embodiment of the present invention, the world coordinate system Oxy plane as the first plane (i.e. Figure 2 in ), the local coordinate system Mx ' y ' plane as the second plane (i.e. Figure 2 in ).

[0051] S4. Calculate a rotation coordinate transformation matrix from the first plane to the second plane according to the world coordinates of the target point and the direction vector.

[0052] Step S4 includes the following sub-steps S41 to S43: S41, according to the target point C The world coordinates and direction vector S Get the normal vector of the second plane and parallel to z The axis vector is used as the normal vector of the first plane.

[0053] S42. Calculate the angle between the first plane and the second plane according to the normal vector of the first plane and the normal vector of the second plane.

[0054] S43. Obtain a rotation coordinate transformation matrix from the first plane to the second plane according to the angle between the first plane and the second plane: in Indicates the direction from the first plane to the second plane x The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane x The rotation angle of the axis, Indicates the direction from the first plane to the second plane y The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane y The rotation angle of the axis, Indicates the direction from the first plane to the second plane z The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane z The rotation angle of the axis.

[0055] S5. Convert the world coordinates of the second plane into local coordinates according to the rotation coordinate transformation matrix, and then calculate the local coordinates of each point on the second arc.

[0056] Step S5 includes the following sub-steps S51-S52: S51. Multiply the world coordinates of the second plane by the rotation coordinate transformation matrix to convert them into local coordinates of the second plane.

[0057] S52. Calculate the local coordinates of each point on the second arc according to the local coordinates of the second plane.

[0058] In the embodiment of the present invention, Figure 2 As shown, first get the target point C Direction vector S and x 'Intersection of the axes N , then the second plane C 、 D 、 B 、 N 、 M The local vertical coordinate of the point ( z The 'axis coordinates) are all 0, so the three-dimensional problem can be transformed into a two-dimensional problem. In the local coordinate system, the center of the circle is solved through geometric relationships. O 2 and the local coordinates of the intersection point B (the second arc is at C Point and B Points and line segments CN 、 BN Tangent, O 2 is the center of the second arc, then the triangle O 2 NC and O 2 NB are congruent triangles. BN = CN , B The local coordinates of the point can be obtained, ∠ O 2 NB ∠ CNB Half of tan∠ O 2 NB You can solve BO 2 length, center of circle O 2), and then the local coordinates of each point on the second arc can be calculated using the following formula: in represents the number of solution steps, The central angle of the second arc is ∠ CO 2 B Angle (i.e. Figure 2 in ), Indicates from 0 to The fixed angles increase successively, represents the equal division calculation function, represents the absolute value function, represents the local horizontal coordinate of each point on the second arc, represents the local ordinate of each point on the second arc, Indicates the intersection of the first arc and the second arc B The local abscissa of Indicates the center of the second arc O 2. The local coordinates of each point on the second arc .

[0059] S6. Convert the local coordinates of the intersection of the first arc and the second arc into world coordinates according to the rotation coordinate transformation matrix, and then calculate the world coordinates of each point on the first arc.

[0060] Step S6 includes the following sub-steps S61-S62: S61, the intersection of the first arc and the second arc B The local coordinates are divided by the rotation coordinate transformation matrix to convert to the intersection point B The world coordinates of .

[0061] S62, according to the intersection B The world coordinates of each point on the first arc are calculated.

[0062] In the embodiment of the present invention, Figure 2 As shown, the first arc AEB exist A Point and y Axis tangent, at the same time B Point and x 'Axis tangent, then the first arc AEB The center of the circle O The world coordinates of 1 and the first arc AEB Radius Then, the world coordinates of each point on the first arc can be calculated using the following formula: in represents the number of solution steps, Indicates the central angle of the first arc ∠ AO 1 B Angle (i.e. Figure 2 in ), Indicates from 0 to The fixed angles increase successively, represents the equal division calculation function, represents the absolute value function, Represents the world horizontal coordinate of each point on the first arc, Represents the world ordinate of each point on the first arc, Indicates the intersection of the first arc and the second arc B The world horizontal coordinate, Indicates the intersection of the first arc segment and the initial straight line segment A The world vertical coordinate.

[0063] S7. Convert the local coordinates of each point on the second arc into world coordinates according to the rotation coordinate transformation matrix.

[0064] In the embodiment of the present invention, the local coordinates of each point on the second arc are divided by the rotation coordinate transformation matrix to be converted into the world coordinates of each point on the second arc.

[0065] S8. Combine the world coordinates of each point on the first arc, the world coordinates of each point on the second arc, and the world coordinates of the initial straight line segment to obtain the path of the center of the underwater flexible robotic arm.

[0066] S9. Calculate the paths of the four ropes of the underwater flexible manipulator based on the path of the center of the underwater flexible manipulator.

[0067] The underwater flexible manipulator is composed of four ropes. In the embodiment of the present invention, the trajectories of the central axes of the four ropes are first determined, and then the trajectories of the four ropes are obtained from the trajectories of the central axes.

[0068] Step S9 includes the following sub-steps S91 to S93: S91. Calculate the world coordinates of the first path of the four ropes of the underwater flexible manipulator based on the world coordinates of each point on the first arc and the world coordinates of the initial straight line segment: in Represents the world coordinates of each point in the first path of the first rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first path of the second rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first segment of the path of the third rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first segment of the fourth rope of the underwater flexible manipulator, Represents the initial straight line segment OA and the first arc AEB The world horizontal coordinate of each point on Represents the initial straight line segment OA and the first arc AEB The world ordinate of each point on the It represents the width of the underwater flexible manipulator, that is, the shortest distance between two non-adjacent ropes.

[0069] S92, take the end point of the first path of the four ropes of the underwater flexible manipulator as the starting point of the second path, multiply the world coordinates of the starting point of the second path by the rotation coordinate transformation matrix, convert it into the local coordinates of the starting point of the second path, and project it to y 'Axis, get the projection distance .

[0070] S93, according to the projection distance Get the radius of the second path of the four ropes of the underwater flexible manipulator : in Indicates the initial design radius of the second path of the four ropes of the underwater flexible manipulator. In this embodiment of the present invention, if the target point C The starting point of the second path is y 'Axis on the same side, then , if the target point C The starting point of the second path is y'Axis opposite side, then .

[0071] S94, according to the radius of the second path of the four ropes of the underwater flexible manipulator Get the local coordinates of the second path of the four ropes of the underwater flexible manipulator : in represents the arc angle of the second path of the four ropes of the underwater flexible manipulator, Indicates that the points on the second path are in the local coordinate system z 'Axis coordinate values.

[0072] S95. Divide the local coordinates of the second path segment of the four ropes of the underwater flexible manipulator by the rotation coordinate transformation matrix to obtain the world coordinates of the second path segment of the four ropes of the underwater flexible manipulator.

[0073] S96. Obtain the paths of the four ropes of the underwater flexible manipulator according to the world coordinates of the first path segment and the world coordinates of the second path segment of the four ropes of the underwater flexible manipulator.

[0074] In the embodiment of the present invention, the image of the center of the underwater flexible manipulator is drawn into a three-dimensional image in the world coordinate system, such as Figure 3 As shown, the three-dimensional image of the four ropes of the underwater flexible manipulator is drawn in another image, as shown in Figure 4 As shown, the results of path planning can be seen intuitively.

[0075] Those skilled in the art will appreciate that the embodiments described herein are intended to help readers understand the principles of the present invention, and it should be understood that the scope of protection of the present invention is not limited to such specific descriptions and embodiments. Those skilled in the art can make various other specific variations and combinations based on the technical teachings disclosed in the present invention without departing from the essence of the present invention, and such variations and combinations are still within the scope of protection of the present invention.

Claims

1. A spatial path planning method for an underwater flexible manipulator, characterized in that: The following steps are involved: S1, obtain the length, world coordinate system, world coordinates of the target point and direction vector of the underwater flexible manipulator; S2. Obtain the origin of the local coordinate system according to the length of the underwater flexible manipulator and construct the local coordinate system; S3. Determine a first plane where the first arc segment is located in the world coordinate system, and determine a second plane where the second arc segment is located in the local coordinate system; S4, calculating the rotation coordinate transformation matrix from the first plane to the second plane according to the world coordinates of the target point and the direction vector; S5. Convert the world coordinates of the second plane into local coordinates according to the rotation coordinate transformation matrix, and then calculate the local coordinates of each point on the second arc; S6. Convert the local coordinates of the intersection of the first arc and the second arc into world coordinates according to the rotation coordinate transformation matrix, and then calculate the world coordinates of each point on the first arc; S7. Convert the local coordinates of each point on the second arc segment into world coordinates according to the rotation coordinate transformation matrix; S8. Combining the world coordinates of each point on the first arc, the world coordinates of each point on the second arc, and the world coordinates of the initial straight line segment to obtain a path of the center of the underwater flexible manipulator; S9. Calculate the paths of the four ropes of the underwater flexible manipulator based on the path of the center of the underwater flexible manipulator.

2. The underwater flexible manipulator space path planning method according to claim 1, characterized in that: The step S2 comprises the following sub-steps: S21. In the world coordinate system O-xyz of y Obtaining a node on the axis that makes the path length of the center of the underwater flexible manipulator greater than the length of the underwater flexible manipulator; S22. Align the node with the origin of the world coordinate system O Any point between them is taken as the origin of the local coordinate system M ; S23. Get the intersection of the first arc and the second arc B ; S24, the origin of the local coordinate system M Intersection B The direction of the connection is x 'Axis negative direction, and make the target point C lie in Mx ' y 'In the plane, build a local coordinate system Mx ' y ' z '.

3. The underwater flexible manipulator space path planning method according to claim 2, characterized in that: The step S3 is specifically as follows: Oxy The plane is taken as the first plane, and the local coordinate system Mx ' y ' plane as the second plane.

4. The underwater flexible manipulator space path planning method according to claim 1, characterized in that: The step S4 comprises the following sub-steps: S41, according to the target point C The world coordinates and direction vector S Get the normal vector of the second plane and parallel to z The axis vector is used as the normal vector of the first plane; S42. Calculate the angle between the first plane and the second plane according to the normal vector of the first plane and the normal vector of the second plane; S43. Obtain a rotation coordinate transformation matrix from the first plane to the second plane according to the angle between the first plane and the second plane: in Indicates the direction from the first plane to the second plane x The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane x The rotation angle of the axis, Indicates the direction from the first plane to the second plane y The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane y The rotation angle of the axis, Indicates the direction from the first plane to the second plane z The axis rotation coordinate transformation matrix, Indicates the direction from the first plane to the second plane z The rotation angle of the axis.

5. The underwater flexible manipulator space path planning method according to claim 1, characterized in that: The step S5 comprises the following sub-steps: S51, multiplying the world coordinates of the second plane by the rotation coordinate transformation matrix to convert them into local coordinates of the second plane; S52. Calculate the local coordinates of each point on the second arc according to the local coordinates of the second plane: in represents the number of solution steps, The angle representing the center angle of the second arc segment, Indicates from 0 to The fixed angles increase successively, represents the equal division calculation function, represents the absolute value function, represents the local horizontal coordinate of each point on the second arc, represents the local ordinate of each point on the second arc, Indicates the intersection of the first arc and the second arc B The local abscissa of Indicates the local coordinates of the center point of the second arc segment.

6. The underwater flexible manipulator space path planning method according to claim 1, characterized in that: The step S6 includes the following sub-steps: S61, the intersection of the first arc and the second arc B The local coordinates are divided by the rotation coordinate transformation matrix to convert to the intersection point B The world coordinates of S62, according to the intersection B The world coordinates of each point on the first arc are calculated by: in represents the number of solution steps, The angle representing the center angle of the first arc segment, Indicates from 0 to The fixed angles increase successively, represents the equal division calculation function, represents the absolute value function, Represents the world horizontal coordinate of each point on the first arc, Represents the world ordinate of each point on the first arc, Indicates the intersection of the first arc and the second arc B The world horizontal coordinate, Indicates the intersection of the first arc segment and the initial straight line segment A The world vertical coordinate.

7. The method for spatial path planning of an underwater flexible manipulator according to claim 1, characterized in that: The step S7 specifically includes: dividing the local coordinates of each point on the second arc by the rotation coordinate transformation matrix to convert the local coordinates of each point on the second arc into the world coordinates.

8. The underwater flexible manipulator space path planning method according to claim 1, characterized in that: The step S9 includes the following sub-steps: S91. Calculate the world coordinates of the first path of the four ropes of the underwater flexible manipulator based on the world coordinates of each point on the first arc and the world coordinates of the initial straight line segment: in Represents the world coordinates of each point in the first path of the first rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first path of the second rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first segment of the path of the third rope of the underwater flexible manipulator, Represents the world coordinates of each point in the first segment of the fourth rope of the underwater flexible manipulator, Represents the initial straight line segment OA and the first arc AEB The world horizontal coordinate of each point on Represents the initial straight line segment OA and the first arc AEB The world ordinate of each point on the represents the width of the underwater flexible manipulator; S92, take the end point of the first path of the four ropes of the underwater flexible manipulator as the starting point of the second path, multiply the world coordinates of the starting point of the second path by the rotation coordinate transformation matrix, convert it into the local coordinates of the starting point of the second path, and project it to y 'Axis, get the projection distance ; S93, according to the projection distance Get the radius of the second path of the four ropes of the underwater flexible manipulator : in represents the initial design radius of the second path of the four ropes of the underwater flexible manipulator; S94, according to the radius of the second path of the four ropes of the underwater flexible manipulator Get the local coordinates of the second path of the four ropes of the underwater flexible manipulator : in represents the arc angle of the second path of the four ropes of the underwater flexible manipulator, Indicates that the points on the second path are in the local coordinate system z 'Axis coordinate values; S95. Divide the local coordinates of the second path segment of the four ropes of the underwater flexible manipulator by the rotation coordinate transformation matrix to obtain the world coordinates of the second path segment of the four ropes of the underwater flexible manipulator; S96. Obtain the paths of the four ropes of the underwater flexible manipulator according to the world coordinates of the first path segment and the world coordinates of the second path segment of the four ropes of the underwater flexible manipulator.

Images