Energy-optimal path planning algorithm for underwater swimming manipulator in complex environment

Abstract

The present application relates to underwater swimming mechanical arm path planning field, especially underwater swimming mechanical arm energy optimal path planning algorithm in complex environment, the present application is based on the double-layer RRT* path planning framework multi-joint underwater swimming mechanical arm path planning scheme includes two layers of path planning, the first layer path planning is obtained on the basis of Q-RRT* algorithm random sampling point by using the combination of Gauss and artificial potential field method, and the convergence speed is further accelerated by using the informed-RRT* node rejection strategy;The second layer path planning considers the whole body obstacle avoidance and energy consumption of underwater swimming mechanical arm on the basis of the first layer, and the energy optimal underwater swimming mechanical arm path planning is obtained after a large number of iterations.The present application significantly reduces the calculation amount of path planning, is suitable for most multi-joint robot path planning problems, and is easy to realize in actual engineering application.

Description

Technical Field

[0001] This invention relates to the field of underwater manipulator path planning, and more particularly to an energy-optimal path planning algorithm for underwater manipulators in complex environments. Background Technology

[0002] The underwater manipulator is a novel type of underwater robot composed of a multi-jointed chain-like bionic snake robot and multiple thrusters. The thrusters endow it with powerful maneuverability, and the multi-jointed articulated structure ensures that the underwater manipulator possesses operational capabilities similar to a robotic arm. Its maneuverability and slender body allow it to move in confined or narrow spaces that are inaccessible to traditional underwater robots such as remotely operated underwater vehicles (ROVs) or autonomous underwater vehicles (AUVs), enabling tasks such as inspecting and maintaining densely packed high piles at underwater docks and underwater oil pipeline structures. However, the path planning methods used in conventional robots are insufficient for the path planning problems of the variable-configuration underwater manipulator. Therefore, planning energy-efficient and collision-free paths for underwater manipulators in complex environments has become a pressing issue.

[0003] Chinese Patent Publication No. CN113074730B discloses an underwater path planning method and system. This invention fully considers the following factors: the influence of water flow in the underwater environment is significant; most underwater obstacles are not stationary and move with the water flow, and the movement of the underwater vehicle is also affected by the water flow; combining the kinematic characteristics of the underwater vehicle, the influence of water flow, the operating speed of the underwater vehicle, and the operating time, a cost function for the underwater environment is designed; when optimizing the trajectory, the BSpline algorithm is used to combine the speed of the water flow and the smoothness of the trajectory to generate a safe and dynamically feasible trajectory, which is more suitable for path planning in the underwater environment. It is also cheaper and easier to obtain than sonar, thus reducing costs. However, the underwater path planning method and system have the following problems: the algorithms are all based on the robot's center of mass motion for path planning, without considering the robot's actual configuration, dynamic characteristics, and environmental constraints. Therefore, in complex environments, it is often impossible to meet the requirements of optimal energy consumption and collision-free movement for multi-configuration robots. Secondly, sampling-based path planning algorithms have drawbacks such as high computational complexity, slow convergence speed, and long search time. In addition, when performing global path planning, the limited or narrow space movement that the robot cannot reach is not considered. Therefore, movement between dense obstacles, such as underwater dock piles or underwater oil pipelines, is difficult to become a path planning result, resulting in high cost of the planned route, which is not suitable for the energy consumption requirements of underwater robots in actual movement. Summary of the Invention

[0004] To address this, the present invention provides an energy-optimal path planning algorithm for underwater robotic arms in complex environments, which overcomes the problems of existing technologies that do not consider factors such as the actual configuration, dynamic characteristics and environmental constraints of the robot, and whose planned routes are too costly and unsuitable for the energy consumption requirements of underwater robots in actual operation.

[0005] To achieve the above objectives, on the one hand, this invention provides an energy-optimal path planning algorithm for underwater manipulators in complex environments, including: Step S1: Establish a forward kinematics model of the underwater manipulator. The model selects the northeast coordinate system to establish a reference coordinate system for the pose of the underwater manipulator. Step S2: Perform first-layer path planning on the underwater manipulator base. The first-layer path planning obtains random sampling points by combining Gaussian and artificial potential fields based on the Q-RRT* algorithm to reduce the probability of generating redundant nodes. Furthermore, the first-layer path planning uses the Informed-RRT* node rejection strategy to accelerate the convergence speed of the forward kinematics model of the underwater manipulator. Step S3: Under the first-level path planning conditions, perform second-level path planning for the base and the angles of each joint. The second-level path planning is based on the first-level path planning and iterates the model several times to reduce energy consumption and obtain the energy-optimal underwater manipulator base path planning so that the manipulator can accurately avoid obstacles. The base is the tail link of the underwater robotic arm. The condition for the first-level path planning is to complete the first-level path planning and generate the first-level path.

[0006] Further, in step S1, when calculating the pose of the links of the underwater manipulator in the world coordinate system, the links of the underwater manipulator are numbered sequentially from tail to head as link 1, link 2, ..., link n, wherein link i and link i+1 are connected by joint i, and the rotation direction of the i-th joint is defined with reference to the coordinate system of the i+1-th link.

[0007] Furthermore, in step S1, the attitude of link i... The rotation matrix of its coordinate system relative to the world coordinate system and position vector Defined as: ,in, Indicates a special European group; The pose of link i+1 under these conditions It can be represented as: ,in, Indicates from Defined coordinate system to Mapping of the defined coordinate system This represents the angle of the i-th joint; The pose of the link n in the northeast coordinate system can be represented as: , in, .

[0008] Furthermore, in step S1, the infinitesimal representation method of helical motion is used to describe the instantaneous linear velocity and angular velocity of the rigid body using screw quantities, and the screw quantity of joint i is... View as a skew-symmetric matrix The promotion of [the technology / mechanism] can be expressed by the following formula: , in, These represent the screw coordinates, linear velocity, and angular velocity vectors of joint i, respectively. express Oblique symmetric matrix; wedge-shaped operator Represents mapping So(3) represents the Lie algebra of a special Euclidean group; rigid bodies include the links and joints of an underwater robotic arm.

[0009] Furthermore, in step S1, the motion of the rigid body is... Viewed as a combination of rotation and translation along the spinor axis, it can be expressed by the following formula: , in, Let SO(3) be the mapping exponent, and SO(3) denote the three-dimensional special orthogonal group. This represents the rotation angle of the i-th joint.

[0010] Further, step S2 includes: Step S21: Load map space and environment information, initialize platform shape and joint constraints, and set the first path planning conditions, which include the number of iterations, iteration time, iteration step size, hypersphere radius, parent vertex depth, and Gaussian expectation. Gaussian standard deviation , starting point and ending point; Step S22: Establish a sampling space based on the starting point and ending point set in step S21, and obtain the first sampling point in the sampling space using a method combining Gaussian and artificial potential energy. ; Step S23: Search for the random tree node that is closest to the first sampling point on the random tree. Furthermore, new branch and leaf nodes are generated by expanding from the first sampling point along the direction of the random tree node. ; Step S24: Determine the nodes inside the hypersphere based on the leaf nodes. Take the nodes inside the hypersphere and all nodes traced upwards from each hypersphere node to the depth of the parent vertex as the candidate parent node range. Select the node with the minimum total cost of the path to the leaf node and no collision within the candidate parent node range as the parent node, so as to rewire the RRT tree. Step S25: Determine whether the parent node has reached the destination by judging whether the distance between the parent node and the preset endpoint meets the preset standard distance; If the distance between the parent node and the preset endpoint is less than or equal to the preset standard distance, then backtrack and update the path with the minimum cost; If the distance between the new node and the target node is greater than the preset standard distance, return to step S22; Step S26: Determine whether the first-level path planning is completed by judging whether the minimum cost path generated in step S25 satisfies the suboptimal cost condition. If so, the first-level path is generated. The suboptimal cost condition is that the cost of the path with the minimum cost is less than or equal to the suboptimal cost threshold, and the suboptimal cost threshold is 1.05 times the cost of the optimal path.

[0011] Furthermore, the first sampling point is obtained in step S22. The method combining Gaussian and artificial potential energy involves obtaining a sampling point in the sampling space based on the Gaussian distribution. The first sampling point was obtained by offsetting the sampling point using the artificial potential field method. .

[0012] Further, step S23 connects the first sampling point and the branch node. And it performs collision detection to determine whether the first sampling point should be obtained again. ; If a collision occurs, return to step S22 and reacquire the first sampling point. ; If no collision occurs, proceed to step S24.

[0013] Furthermore, in step S26, if the minimum cost path satisfies the suboptimal cost condition, the algorithm stops, the first-level path planning is completed, and the first-level path is generated. If the minimum cost path does not meet the suboptimal cost condition, then update the sampling space of a hyperellipsoid and return to step S22 to obtain the first sampling point again.

[0014] Further, step S3 includes: Step S31: Use the first layer path as the reference path for the underwater manipulator base, and set the second planning path conditions, which include the sampling step size. Number of iterations and joint constraints; Step S32: Discretize the portion of the underwater manipulator before and after each inflection point in the reference path, whose length is equal to the length of the manipulator, into a first series of nodes using a sampling step size. W node ; Step S33, the first series of nodes W node Each node in the model represents the position of the base. Furthermore, Gaussian sampling is used to sample the base's attitude and the angles of each joint. The Gaussian expectation of the base's attitude is... The vector direction of each node in the reference path, and the Gaussian expectation and Gaussian standard deviation of each joint angle are 0. Adjust according to the characteristics of the problem; Step S34: The pose of each joint of the underwater manipulator base is solved by forward kinematics based on the position of the base, the sampled base posture, and the angles of each joint. Step S35: Detect whether the underwater robotic arm is colliding; Step S36: If sampling is completed at each node and no collision is detected, then the whole-body motion planning of the underwater manipulator is completed once. Furthermore, the path with the lowest energy consumption is selected by calculating the total energy consumption of all joint paths after the second layer of planning. The total energy consumption is represented by the sum of mechanical work done by each joint displacement, and is approximately replaced by the sum of the displacement of each link.

[0015] Step S37: Determine whether to end the second-layer path planning by judging whether the energy consumption of the path meets the preset energy consumption conditions, so as to generate a full-body second-layer path including the base posture and the angles of each joint.

[0016] Further, step S35 determines whether the first series of nodes meet the second-layer path planning by detecting whether each joint of the underwater manipulator base collides with environmental obstacles and whether the displacement from the previous state to this state collides with environmental obstacles. If no collision occurs, then sample the next node. If a collision occurs, the node is resampled. If a preset threshold number of collisions occur consecutively, the process returns to step S32, where the sampling step size is changed to modify the first series of nodes to enable resampling. The previous state refers to the collision-free pose of each joint obtained in the previous step.

[0017] Further, step S37, determining whether the second-level path planning is complete, includes: If the energy consumption of the path meets the preset energy consumption condition, the second-layer path planning ends and a full-body second-layer path including the base posture and the angles of each joint is generated. If the energy consumption of the path does not meet the preset energy consumption condition, return to step S32 and change the sampling step size to change the first series of nodes to enable resampling.

[0018] Furthermore, the preset energy consumption condition includes that the total energy consumption of the path is less than or equal to a preset energy consumption standard value, wherein the preset energy consumption standard value = first-level planned path cost × number of joints × optimal parameters; The optimal parameter has a value range of [1.05, 1.5].

[0019] Compared with the prior art, the beneficial effects of the present invention are that the present invention adopts a two-layer path planning framework to study the path planning of multi-joint underwater manipulators, which solves the problems of ignoring the actual configuration of the robot, high computational complexity, slow convergence speed, and difficulty in handling narrow channels in the path planning of underwater manipulators.

[0020] Furthermore, the first-layer path planning obtains random sampling points by combining Gaussian and artificial potential fields on the basis of the Q-RRT* algorithm to reduce the probability of generating redundant nodes. In addition, the first-layer path planning also uses the Informed-RRT* node rejection strategy to accelerate the convergence speed of the forward kinematics model of the underwater manipulator.

[0021] Furthermore, the second-layer path planning, based on the first-layer path planning, iterates the model several times to reduce energy consumption and obtain the energy-optimal underwater manipulator base path planning so that the manipulator can accurately avoid obstacles. Attached Figure Description

[0022] Figure 1 This is a schematic diagram of an underwater robotic arm according to an embodiment of the present invention; Figure 2 This is a schematic diagram of the coordinate system establishment of the underwater manipulator in an embodiment of the present invention; Figure 3 This is a flowchart illustrating the energy-optimal path planning algorithm for an underwater manipulator in a complex environment according to an embodiment of the present invention. Figure 4 This is a random tree growth graph of the improved Q-RRT* algorithm in an embodiment of the present invention; Figure 5 The box plots show the time consumption of planning 50 times when using the improved Q-RRT* algorithm and the Q-RRT* algorithm respectively in the embodiments of the invention. Figure 6 This refers to the optimization speed of path cost with the number of iterations when the improved Q-RRT* algorithm and the Q-RRT* algorithm are used respectively in the embodiments of the present invention; Figure 7 This is a simulation diagram of the energy-efficient path planning algorithm for the underwater manipulator in an environment with dense obstacles, according to an embodiment of the present invention. Figure 8 This is a simulation diagram of the energy-efficient path planning algorithm for the underwater manipulator in a tortuous environment according to an embodiment of the present invention. In the diagram: 1, tail link; 2, ordinary link; 3, vector propulsion link; 4, head link; 5, rotatable shell; 6, northeast-east world coordinate system. Detailed Implementation

[0023] To make the objectives and advantages of the present invention clearer, the present invention will be further described below with reference to embodiments; it should be understood that the specific embodiments described herein are merely for explaining the present invention and are not intended to limit the present invention.

[0024] Preferred embodiments of the present invention will now be described with reference to the accompanying drawings. Those skilled in the art should understand that these embodiments are merely illustrative of the technical principles of the present invention and are not intended to limit the scope of protection of the present invention.

[0025] It should be noted that in the description of this invention, the terms "upper", "lower", "left", "right", "inner", "outer", etc., which indicate directions or positional relationships, are based on the directions or positional relationships shown in the accompanying drawings. This is only for the convenience of description and is not intended to indicate or imply that the device or element must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, it should not be construed as a limitation of this invention.

[0026] Furthermore, it should be noted that, in the description of this invention, unless otherwise explicitly specified and limited, the terms "installation," "connection," and "linking" should be interpreted broadly. For example, they can refer to a fixed connection, a detachable connection, or an integral connection; they can refer to a mechanical connection or an electrical connection; they can refer to a direct connection or an indirect connection through an intermediate medium; and they can refer to the internal connection of two components. Those skilled in the art can understand the specific meaning of the above terms in this invention according to the specific circumstances.

[0027] Please see Figure 1-2 As shown, these are schematic diagrams of an underwater manipulator arm according to an embodiment of the present invention and a schematic diagram of the coordinate system establishment of an underwater manipulator arm according to an embodiment of the present invention. The underwater manipulator arm structure used in this embodiment of the present invention includes: The swimming structure includes a head link 4, a tail link 1, and several intermediate links sequentially arranged between the head link 4 and the tail link 1. Each link is connected by a joint so that the relative positional relationship between the links can be changed by rotating the joint. A tail thruster is provided at the end of the tail link 1, and side thrusters are symmetrically arranged on both sides of each link so that the tail thruster and side thrusters provide power for the underwater swimming robotic arm. A wireless communication unit is provided on the head link 4 so that the controller can establish a connection with the host computer through the wireless communication unit. The sensor group includes a ranging unit, an inertial measurement unit, and a positioning unit installed on each link, so that the ranging unit can obtain the distance between each link and the pile foundation, the inertial measurement unit can obtain the three-axis attitude angles and accelerations of each link, and the sensor group can communicate with the host computer for positioning and navigation. The sampling module includes an image acquisition unit and an illumination unit disposed on the head link 4, so that the image acquisition unit can acquire a clear image of the area around the underwater manipulator arm; Servo motors, which are mounted on each joint of the underwater manipulator, are used to control the rotation of the joints so that the underwater manipulator can change its posture. The controller is mounted on the head link 4 and is connected to the servo motor, sampling module, tail thruster, side thruster and sensor group. It is used to control the movement of the servo motor, tail thruster and side thruster and to receive data acquired by the sampling module and sensor group. The host computer is wirelessly connected to the controller to receive data sent by the controller and send control commands to the controller so that the controller can control the servo motor, tail thruster and side thruster to move underwater, so that the underwater robotic arm moves underwater along a set path.

[0028] Please see Figure 3-4 As shown, they are respectively a flowchart of the energy-optimal path planning algorithm for underwater manipulators in complex environments according to an embodiment of the present invention and a random tree growth diagram of the improved Q-RRT* algorithm according to an embodiment of the present invention. Figure 4 The dashed elliptical region in the figure represents the improved Informed-RRT* restricted sampling point region. This invention provides an energy-optimal path planning algorithm for underwater manipulators in complex environments, including: Step S1: Establish a forward kinematics model of the underwater manipulator. The model selects the northeast coordinate system to establish a reference coordinate system for the pose of the underwater manipulator. Step S2: Perform first-layer path planning on the underwater manipulator base. The first-layer path planning obtains random sampling points by combining Gaussian and artificial potential fields based on the Q-RRT* algorithm to reduce the probability of generating redundant nodes. Furthermore, the first-layer path planning uses the Informed-RRT* node rejection strategy to accelerate the convergence speed of the forward kinematics model of the underwater manipulator. Step S3: Under the first-level path planning conditions, perform second-level path planning for the base and the angles of each joint. The second-level path planning is based on the first-level path planning and iterates the model several times to reduce energy consumption and obtain the energy-optimal underwater manipulator base path planning so that the manipulator can accurately avoid obstacles. The base is the tail link 1 of the underwater robotic arm. The condition for the first-level path planning is to complete the first-level path planning and generate the first-level path.

[0029] Specifically, in step S1, when calculating the pose of the links of the underwater manipulator in the world coordinate system, the links of the underwater manipulator are numbered sequentially from tail to head as link 1, link 2, ..., link n, where link i and link i+1 are connected by joint i, and the rotation direction of the i-th joint is defined with reference to the coordinate system of the i+1-th link.

[0030] Specifically, in step S1, the attitude of link i... The rotation matrix of its coordinate system relative to the world coordinate system and position vector Defined as: ,in, Indicates a special European group; The pose of link i+1 under these conditions It can be represented as: ,in, Indicates from Defined coordinate system to Mapping of the defined coordinate system This represents the angle of the i-th joint; The pose of the link n in the northeast coordinate system can be represented as: , in, .

[0031] Specifically, in step S1, the infinitesimal representation method in helical motion is used to describe the instantaneous linear velocity and angular velocity of the rigid body using spinor, and the spinor of joint i is... View as a skew-symmetric matrix The promotion of [the technology / mechanism] can be expressed by the following formula: , in, These represent the screw coordinates, linear velocity, and angular velocity vectors of joint i, respectively. express Oblique symmetric matrix; wedge-shaped operator Represents mapping So(3) represents the Lie algebra of a special Euclidean group; rigid bodies include the links and joints of an underwater robotic arm.

[0032] Specifically, in step S1, the motion of the rigid body is... Viewed as a combination of rotation and translation along the spinor axis, it can be expressed by the following formula: , in, Let SO(3) be the mapping exponent, and SO(3) denote the three-dimensional special orthogonal group. This represents the rotation angle of the i-th joint.

[0033] Step S2 of the energy-optimal path planning algorithm for an underwater manipulator in a complex environment according to an embodiment of the present invention includes: Step S21: Load map space and environment information, initialize platform shape and joint constraints, and set the first path planning conditions, which include the number of iterations, iteration time, iteration step size, hypersphere radius, parent vertex depth, and Gaussian expectation. Gaussian standard deviation , starting point and ending point; Step S22: Establish a sampling space based on the starting point and ending point set in step S21, and obtain the first sampling point in the sampling space using a method combining Gaussian and artificial potential energy. ; Step S23: Search for the random tree node that is closest to the first sampling point on the random tree. Furthermore, new branch and leaf nodes are generated by expanding from the first sampling point along the direction of the random tree node. ; Step S24: Determine the nodes inside the hypersphere based on the leaf nodes. Take the nodes inside the hypersphere and all nodes traced upwards from each hypersphere node to the depth of the parent vertex as the candidate parent node range. Select the node with the minimum total cost of the path to the leaf node and no collision within the candidate parent node range as the parent node, so as to rewire the RRT tree. Step S25, by determining the parent node With the preset endpoint To determine whether the distance meets the preset standard distance, the parent node is... Have we reached the destination? If the parent node With the preset endpoint If the distance is less than or equal to the preset standard distance, then backtrack and update the path with the minimum cost; If the distance between the new node and the target node is greater than the preset standard distance, return to step S22; Step S26: Determine whether the first-level path planning is completed by judging whether the minimum cost path generated in step S25 satisfies the suboptimal cost condition. If so, the first-level path is generated. The preset standard distance is adjusted according to the size of the map and the accuracy requirements of the plan in the actual scenario. For example, in a 10×10 meter environment, the destination can be considered to be within 20 centimeters, and in a 100×100 meter environment, the destination can be considered to be within 2 meters. In this embodiment, the default value is 0.5m. The suboptimal cost condition is that the cost of the path with the minimum cost is less than or equal to the suboptimal cost threshold, and the suboptimal cost threshold is 1.05 times the cost of the optimal path.

[0034] Specifically, step S22 forms a Gaussian distribution on both sides of the straight line L connecting the start and end points. The mean of the Gaussian sampling in the planar space is defined as the straight line L, and the standard deviation can be adjusted according to the characteristics of the problem. The Gaussian distribution function is: , A sampling point is obtained in the sampling space based on the Gaussian distribution. Furthermore, the sampling point was offset using an artificial potential field method. Obtain the first sampling point The specific process is as follows: , Where p represents the gravitational factor, h represents the repulsive factor, and d represents the sampling point. and the end point The distance, where n represents the number of obstacles near the sampling point. This represents the distance between the i-th obstacle and the sampling point.

[0035] Specifically, step S23 involves connecting the first sampling point. and the branch nodes Collision detection is performed to ensure that the RRT tree remains a collision-free feasible path after adding a new node, in order to determine whether the first sampling point should be obtained again. ; If a collision occurs, return to step S22 to obtain the first sampling point again. ; If no collision occurs, proceed to step S24.

[0036] In this embodiment, the branch and leaf nodes The calculation is as follows: , in, It is the distance from the random sampling point on the random tree. The nearest node of a node It's the step length.

[0037] Specifically, in step S26, if the minimum cost path satisfies the suboptimal cost condition, then the algorithm stops, the first-level path planning is completed, and the first-level path is generated. If the minimum cost path does not satisfy the suboptimal cost condition, then update the sampling space of a hyperellipsoid and return to step S22 to obtain the first sampling point again. Specifically, a focus is set as... and The hyperellipsoid limits the range of sampling points. For a two-dimensional planar environment, the sampling points... The equation must be satisfied: , in This represents the maximum distance from the two foci to a point on the path.

[0038] Specifically, step S3 includes: Step S31: Use the first layer path as the reference path for the underwater manipulator base, and set the second planning path conditions, which include the sampling step size. Number of iterations and joint constraints; Step S32: Discretize the portion of the underwater manipulator before and after each inflection point in the reference path, whose length is equal to the length of the manipulator, into a first series of nodes using a sampling step size. W node ; Step S33, the first series of nodes W node Each node in the model represents the position of the base. Furthermore, Gaussian sampling is used to sample the base's attitude and the angles of each joint. The Gaussian expectation of the base's attitude is... The vector direction of each node in the reference path, and the Gaussian expectation of each joint angle. Gaussian standard deviation Adjust according to the characteristics of the problem; Step S34: The pose of each joint of the underwater manipulator base is solved by forward kinematics based on the position of the base, the sampled base posture, and the angles of each joint. Step S35: Detect whether the underwater robotic arm is colliding; Step S36: If sampling is completed at each node and no collision is detected, then the whole-body motion planning of the underwater manipulator is completed once. Furthermore, the path with the lowest energy consumption is selected by calculating the total energy consumption of all joint paths after the second layer of planning. The total energy consumption is represented by the sum of mechanical work done by each joint displacement, and is approximately replaced by the sum of the displacement of each link.

[0039] Step S37: Determine whether to end the second-layer path planning by judging whether the energy consumption of the path meets the preset energy consumption conditions, so as to generate a full-body second-layer path including the base posture and the angles of each joint.

[0040] Specifically, the Gaussian distribution function for step S33 is: .

[0041] Specifically, step S35 determines whether the first series of nodes meet the second-layer path planning requirements by detecting whether each joint of the underwater manipulator base collides with environmental obstacles and whether the displacement from the previous state to this state collides with environmental obstacles. If no collision occurs, sample the next node. If a collision occurs, the node is resampled. If a preset threshold number of collisions occur consecutively, the process returns to step S32, where the sampling step size is changed to modify the first series of nodes to enable resampling. The previous state refers to the collision-free pose of each joint obtained in the previous step.

[0042] Specifically, in this embodiment, the number of collisions is preset to 20. The setting of 20 collisions is an empirical value, which combines the time cost of a single calculation and the probability of entering an infinite loop.

[0043] Specifically, step S37, which involves determining whether the second-level path planning is complete, includes: If the energy consumption of the path meets the preset energy consumption condition, the second-layer path planning ends and a full-body second-layer path including the base posture and the angles of each joint is generated. If the energy consumption of the path does not meet the preset energy consumption condition, return to step S32 and change the sampling step size to change the first series of nodes to enable resampling.

[0044] Specifically, the preset energy consumption condition includes the total energy consumption of the path being less than or equal to a preset energy consumption standard value, wherein the preset energy consumption standard value E = first-level planned path cost p × number of joints n × optimal parameter w; The optimal parameter w is set according to the actual scenario requirements, and its value range is w∈[1.05,1.5], generally set to 1.1 by default.

[0045] Please see Figure 5-6As shown, these are box plots of the time consumption for 50 planning iterations when using the improved Q-RRT* algorithm and the Q-RRT* algorithm respectively in the embodiments of the present invention, and the optimization speed of path cost with the number of iterations when using the improved Q-RRT* algorithm and the Q-RRT* algorithm respectively in the embodiments of the present invention.

[0046] To verify the effectiveness and feasibility of the path planning algorithm designed in this invention, a USM path planning simulation experiment was conducted using MATLAB as the simulation platform and a multi-joint underwater robotic arm as the object. All experiments were performed on a Dell 15-7570 computer with 8GB of Intel(R) Core(TM) i5-8250U memory.

[0047] The improved Q-RRT* algorithm was compared and analyzed with the original Q-RRT* algorithm. Experiments were conducted in an obstacle-filled scene with a map size of 50m x 40m and 20 obstacles. The starting position was (5, 5), the ending position was (45, 35), and the resolution was 0.1m. After 50 trials on both algorithms in a dense obstacle scene, it was found that the improved method significantly improved in terms of computation time and iterative optimization speed.

[0048] Please see Figure 7-8 As shown, these are simulation diagrams of the energy-efficient path planning algorithm for the underwater manipulator in an environment with dense obstacles and simulation diagrams of the energy-efficient path planning algorithm for the underwater manipulator in a tortuous environment, respectively.

[0049] To verify whether the proposed dual-layer path planning algorithm can plan a path for a multi-joint underwater manipulator in space, MATLAB was used as the simulation platform. The black broken line represents the underwater manipulator body, and the dashed line represents the base path. The results show that the proposed dual-layer path planning algorithm can plan an energy-optimal and collision-free path for the underwater manipulator.

[0050] The technical solution of the present invention has been described above with reference to the preferred embodiments shown in the accompanying drawings. However, it will be readily understood by those skilled in the art that the scope of protection of the present invention is obviously not limited to these specific embodiments. Without departing from the principles of the present invention, those skilled in the art can make equivalent changes or substitutions to the relevant technical features, and the technical solutions after these changes or substitutions will all fall within the scope of protection of the present invention.

[0051] The above description is merely a preferred embodiment of the present invention and is not intended to limit the invention. Various modifications and variations can be made to the present invention by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

Claims

1. An energy-optimal path planning algorithm for underwater manipulators in complex environments, characterized in that, Includes the following steps: Step S1: Establish a forward kinematics model of the underwater manipulator. The model selects the northeast coordinate system to establish a reference coordinate system for the pose of the underwater manipulator. Step S2: Perform first-layer path planning on the underwater manipulator base. The first-layer path planning obtains random sampling points by combining Gaussian and artificial potential fields based on the Q-RRT* algorithm to reduce the probability of generating redundant nodes. Furthermore, the first-layer path planning uses the Informed-RRT* node rejection strategy to accelerate the convergence speed of the forward kinematics model of the underwater manipulator. Step S3: Under the first-level path planning conditions, perform second-level path planning for the base and the angles of each joint. The second-level path planning is based on the first-level path planning and iterates the model several times to reduce energy consumption and obtain the energy-optimal underwater manipulator base path planning so that the manipulator can accurately avoid obstacles. The base is the tail link of the underwater robotic arm. The condition for the first-level path planning is to complete the first-level path planning and generate the first-level path; Step S3 includes: Step S31: Use the first layer path as the reference path for the underwater manipulator base, and set the second planning path conditions, which include the sampling step size. Number of iterations and joint constraints; Step S32: Discretize the portion of the underwater manipulator before and after each inflection point in the reference path, which is equal to the length of the manipulator, into a first series of nodes using a sampling step size. Step S33: Each node in the first series of nodes is taken as the position of the base. Gaussian sampling method is used to sample the base posture and the angle of each joint. The Gaussian expectation of the base posture is the vector direction of each node in the reference path. The Gaussian expectation of each joint angle is equal to 0. The Gaussian standard deviation is adjusted according to the characteristics of the problem. Step S34: The pose of each joint of the underwater manipulator base is solved by forward kinematics based on the position of the base, the sampled base posture, and the angles of each joint. Step S35: Detect whether the underwater robotic arm is colliding; Step S36: If sampling is completed at each node and no collision is detected, then the whole-body motion planning of the underwater manipulator is completed once. Furthermore, the path with the lowest energy consumption is selected by calculating the total energy consumption of all joint paths after the second layer of planning. The total energy consumption is represented by the sum of mechanical work done by each joint displacement, and is approximately replaced by the sum of the displacement of each link. Step S37: Determine whether to end the second-layer path planning by judging whether the energy consumption of the path meets the preset energy consumption conditions, so as to generate a full-body second-layer path including the base posture and the angles of each joint.

2. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 1, characterized in that, In step S1, when calculating the pose of the links of the underwater manipulator in the world coordinate system, the following settings are made: the links of the underwater manipulator are numbered sequentially from tail to head as link 1, link 2, ..., link n, where link i and link i+1 are connected by joint i, and the rotation direction of the i-th joint is defined with reference to the coordinate system of the (i+1)-th link.

3. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 1, characterized in that, Step S2 includes: Step S21: Load map space and environment information, initialize platform shape and joint constraints, and set the first path planning conditions, which include the number of iterations, iteration time, iteration step size, hypersphere radius, parent vertex depth, and Gaussian expectation. Gaussian standard deviation , starting point and ending point; Step S22: Establish a sampling space based on the starting point and ending point set in step S21, and obtain the first sampling point in the sampling space using a method combining Gaussian and artificial potential energy. Step S23: Search for the random tree node on the random tree that is closest to the first sampling point, and generate new branch and leaf nodes by expanding from the first sampling point along the direction of the random tree node; Step S24: Determine the nodes inside the hypersphere based on the leaf nodes. Take the nodes inside the hypersphere and all nodes traced upwards from each hypersphere node to the depth of the parent vertex as the candidate parent node range. Select the node with the minimum total cost of the path to the leaf node and no collision within the candidate parent node range as the parent node, so as to rewire the RRT tree. Step S25: Determine whether the parent node has reached the destination by judging whether the distance between the parent node and the preset endpoint meets the preset standard distance; If the distance between the parent node and the preset endpoint is less than or equal to the preset standard distance, then backtrack and update the path with the minimum cost; If the distance between the new node and the target node is greater than the preset standard distance, return to step S22; Step S26: Determine whether the first-level path planning is completed based on whether the minimum cost path generated in step S25 satisfies the suboptimal cost condition. If so, generate the first-level path. The suboptimal cost condition is that the cost of the path with the minimum cost is less than or equal to the suboptimal cost threshold, and the suboptimal cost threshold is 1.05 times the cost of the optimal path.

4. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 3, characterized in that, The method for obtaining the first sampling point by combining Gaussian distribution and artificial potential energy in step S22 includes obtaining a sampling point based on the Gaussian distribution in the sampling space, and using the artificial potential field method to offset the sampling point to obtain the first sampling point.

5. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 3, characterized in that, Step S23 connects the first sampling point and the branch node and performs collision detection to determine whether the first sampling point is obtained again. If a collision occurs, return to step S22 and obtain the first sampling point again; If no collision occurs, proceed to step S24.

6. The energy-efficient path planning algorithm for underwater manipulators in complex environments according to claim 3, characterized in that, In step S26, if the minimum cost path satisfies the suboptimal cost condition, the algorithm stops, the first-level path planning is completed, and the first-level path is generated. If the minimum cost path does not meet the suboptimal cost condition, then update the sampling space of a hyperellipsoid and return to step S22 to obtain the first sampling point again.

7. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 6, characterized in that, Step S35 determines whether the first series of nodes meet the second-layer path planning by detecting whether each joint of the underwater manipulator base collides with environmental obstacles and whether the displacement from the previous state to this state collides with environmental obstacles. If no collision occurs, then sample the next node. If a collision occurs, the node is resampled. If a preset threshold number of collisions occur consecutively, the process returns to step S32, where the sampling step size is changed to modify the first series of nodes to enable resampling. The previous state refers to the collision-free pose of each joint obtained in the previous step.

8. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 7, characterized in that, The determination in step S37 to complete the second-level path planning includes: If the energy consumption of the path meets the preset energy consumption condition, the second-layer path planning ends and a full-body second-layer path including the base posture and the angles of each joint is generated. If the energy consumption of the path does not meet the preset energy consumption condition, return to step S32 and change the sampling step size to change the first series of nodes to enable resampling.

9. The energy-optimal path planning algorithm for underwater manipulators in complex environments according to claim 8, characterized in that, The preset energy consumption condition includes that the total energy consumption of the path is less than or equal to the preset energy consumption standard value, wherein the preset energy consumption standard value = first-level planning path cost × number of joints × optimal parameters; The optimal parameter has a value range of [1.05, 1.5].

Images