Motion planning method of underwater hyper-redundant dexterous robot under dynamic constraint condition

Abstract

The invention relates to a motion planning method for an underwater hyper-redundant dexterous robot under a dynamic constraint condition, and the method comprises the following steps: obtaining an underwater three-dimensional environment map, and constructing a scale-adaptive grid map; on the basis of the grid map, according to motion constraints, an improved Q-learning reinforcement learning method is adopted to carry out global planning, and an initial moving track sequence of the robot is obtained; on the basis of the initial moving trajectory sequence, according to dynamic constraints in the actual moving process, an improved D * algorithm is adopted for local re-planning, and an overall trajectory sequence of the robot after local re-planning is obtained; and based on the initial moving trajectory sequence and the overall trajectory sequence after local re-planning, performing dynamic configuration planning by adopting an improved genetic algorithm to obtain a motion configuration dynamic sequence, and completing a motion planning process. Compared with the prior art, the method has the advantages of realizing efficient and accurate motion planning and the like.

Description

Technical Field

[0001] The present invention relates to the technical field of robot control, and particularly to a motion planning method for an underwater hyper-redundant dexterous robot under dynamic constraint conditions. Background Art

[0002] Underwater robots are important tools for exploring and developing the ocean. Hyper-redundant multi-joint robots have broad application prospects in complex underwater environments due to their advantages such as high flexibility and high adaptability. However, the underwater environment is complex and changeable, with various dynamic constraints, such as water flow changes, moving obstacles, etc. A motion planning method is required to process real-time dynamic environmental information. At the same time, the hyper-redundant characteristics of hyper-redundant multi-joint robots themselves also pose higher requirements for the motion planning method. Hyper-redundant multi-joint robots have multiple redundant control variables, and there is a problem of multiple solutions for motion, that is, for a given task objective and planned path points, there are multiple possible combinations of joint configurations to achieve this motion. How to plan the optimal motion trajectory from numerous possible configurations is the key to motion planning.

[0003] Patent CN112148030B discloses a path planning method for an underwater glider based on a heuristic algorithm, patent application CN119536320A discloses a multi-task point path planning method for an underwater robot considering the ocean environment, and patent application CN117555353A discloses a path planning method for an underwater robot based on a hybrid motion sparrow search algorithm. Traditional motion planning methods, such as the A* algorithm, Dijkstra algorithm, etc., generally only plan the motion trajectory of the centroid point and cannot provide guidance for the configuration during motion, making it difficult to meet the high-efficiency and accurate motion planning requirements of hyper-redundant multi-joint robots in underwater dynamic environments. Existing methods are often not flexible enough in dealing with dynamic constraints, or do not fully consider the kinematic characteristics of the robot and the diversity of task objectives during the planning process, resulting in an unsatisfactory planned motion trajectory. Due to the complexity of the underwater environment and the constraints of robot motion, path planning faces many challenges, such as complex underwater terrain, obstacles, water flow, and the dynamic and kinematic constraints of the robot itself. Summary of the Invention

[0004] The purpose of the present invention is to provide a motion planning method for an underwater hyper-redundant dexterous robot under dynamic constraint conditions to solve the problem of control redundancy of underwater multi-joint robots.

[0005] The purpose of the present invention can be achieved by the following technical solutions:

[0006] A motion planning method for an underwater hyper-redundant dexterous robot under dynamic constraint conditions includes the following steps:

[0007] Obtain an underwater three-dimensional environmental map and construct a scale-adaptive grid map;

[0008] Based on the grid map, according to the motion constraints, use an improved Q-learning reinforcement learning method for global planning to obtain the initial running trajectory sequence of the robot;

[0009] Based on the initial running trajectory sequence, according to the dynamic constraints in the actual motion process, use an improved D* algorithm for local replanning to obtain the overall trajectory sequence after local replanning of the robot;

[0010] Based on the initial running trajectory sequence and the overall trajectory sequence after local replanning, use an improved genetic algorithm for dynamic configuration planning to obtain a dynamic sequence of motion configurations and complete the motion planning process.

[0011] Further, the steps of constructing the scale-adaptive grid map include:

[0012] Determine the initial grid scale according to the size data of the robot, and perform initial gridification on the underwater three-dimensional environmental map to obtain a plurality of initial map grids with the same size, where the underwater three-dimensional environmental map includes a task area, an obstacle area, a passable area, and an impassable area for inspection operations;

[0013] Determine the obstacle density and task information density included in each initial map grid, where the expressions for the obstacle density and task information density are respectively:

[0014]

[0015] In the formula, ρ i is the ratio of the obstacle volume of the i-th initial map grid to the total grid volume, V obstacle-i is the obstacle volume of the i-th initial map grid, V grid-i is the total volume of the i-th initial map grid, t i is the ratio of the task area volume of the i-th initial map grid to the total grid volume, V task-i is the task area volume of the i-th initial map grid;

[0016] According to the obstacle density and task information density of each initial map grid, determine the number of subdivided grids, where the calculation expression for the number of subdivided grids is:

[0017]

[0018] In the formula, n i is the number of subdivided grids of the i-th initial map grid, ρ j is the ratio of the obstacle volume of the j-th initial map grid to the total grid volume, tj is the ratio of the task area volume of the j-th initial map grid to the total grid volume, and N is the preset maximum number of subdivided grids;

[0019] Subdivide each initial map grid according to the number of subdivided grids of each initial map grid, and finally generate a scale-adaptive grid map C map , where the scale-adaptive grid map C map is expressed as:

[0020] C map = [C map0 , C map1 , …, C mapi , C mapm

[0021] C mapi = [C mapi0 , …, C mapij , …, C mapin

[0022] where C mapi is the i-th initial map grid. Determine the 3D position of C mapi in the underwater three-dimensional environment map. C mapi stores n i sub-grid structures internally. Each sub-grid structure C mapij saves the lower left corner coordinates, length, width, height, and passability attribute of the grid, representing one of the refined variable-scale sub-grids.

[0023] Furthermore, the step of subdividing each initial map grid includes:

[0024] According to the obstacle density and task information density of each initial map grid, determine the number of passable grids and non-passable grids in the subdivided grid based on the ratio of the obstacle volume to the actual initial map grid volume and the ratio of the task area volume to the actual initial map grid volume;

[0025] Determine the spatial range of the obstacles, and use the non-passable grids for filling to determine the scale of the non-passable grids under the condition of completely covering the obstacle spatial range;

[0026] Determine the spatial ranges of the task area and the passable area, and use the passable grids for filling to determine the scale of the passable grids under the condition of completely covering the passable area spatial range and not conflicting with the non-passable grids, thus completing the subdivision process.

[0027] Further, the motion constraints include hard constraints and soft constraints. The hard constraints include static obstacle constraints and kinematic constraints, and the soft constraints include the energy consumption, path length, and low-priority task information of the current state.

[0028] Further, the steps for obtaining the initial operating trajectory sequence include:

[0029] Generate an initial Q-value table, where the Q-value table includes different position states of the robot and corresponding actions in different position states;

[0030] Determine whether the maximum number of iterations has been reached. If so, end and save the Q-value table. If not, generate an adaptive normalized action factor A, where the expression for the adaptive normalized action factor A is:

[0031]

[0032] In the formula, A0 is the initial action factor, A min is the minimum action factor, G is the maximum number of iterations, and g is the current number of iterations;

[0033] Determine whether the adaptive normalized action factor A is greater than the action reference. If so, take a random action. If not, take the maximum Q-value action;

[0034] Update the position state according to the taken action, and determine whether the current position state satisfies the hard constraints. If so, execute the next step. If not, give a penalty, update the Q value, and start a new iteration;

[0035] Determine whether the end point has been reached. If the end point has been reached, give the highest reward Q max , set the end flag bit to 1, otherwise execute the next step;

[0036] Update the Q value according to the soft constraints. The calculation expression for the Q value is:

[0037] Q(s,a) = Q(s,a) + α[R(s,a) + γmaxQ(s′,a′) - Q(s,a)]

[0038] In the formula, Q(s,a) is the value of taking action a in state s, α is the learning rate used to control the learning speed, R(s,a) is the reward and punishment obtained by taking action a in state s, γ is the discount factor used to balance the importance of the current reward and future rewards, and maxQ(s′,a′) is the maximum value of all possible actions in the new state s′;

[0039] Determine whether the end flag bit is set to 1. If it is set to 1, output the planned path, save the Q-value table, end the iteration. If not, continue the iteration;

[0040] Furthermore, when the hard constraint is not met, the process of giving a penalty includes:

[0041] Based on the size data of the robot, a collision monitoring area is defined as the center of the refined grid;

[0042] Retrieve the traffic attributes of all refined grids of different scales contained in the collision monitoring area to determine whether there is an impassable area. If so, it is considered that a collision has occurred and the maximum penalty Q is given. punish , update the Q value of the state-action pair in the Q value table and place the robot in the initial position; if not, it is considered that no collision has occurred.

[0043] Furthermore, the step of obtaining the overall trajectory sequence after the local replanning includes:

[0044] Acquire sensor data of the robot during actual motion, obtain dynamic obstacles and unmodeled obstacles according to the sensor data, and redivide the initial map grid in the area and define the traffic attributes according to the area where the robot is located, so as to dynamically modify the initial grid map;

[0045] Determine whether the robot's current position to the next path point violates the underwater environment or the body's motion constraints. If it moves to an impassable grid, retrieve the affected path range based on the area of the entire impassable grid and determine the replanning range. If not, continue to move;

[0046] The starting grid and the ending grid of the replanning segment are determined according to the replanning range, and the improved D* algorithm is used to perform local replanning to obtain the overall trajectory sequence after local replanning.

[0047] Furthermore, the step of using the improved D* algorithm to perform local replanning includes:

[0048] 1) Initialize the bidirectional edge map, treat each subdivided grid as a node, and establish bidirectional edges between adjacent grids that are passable: connect the center points of adjacent grid cells, represent a feasible path from one grid cell to another, and set the weight of the edge;

[0049] 2) Initialize the cost sequence h, and set the cost h(s) of the end grid goal ) is set to 0, the cost h(s) of all remaining grids is set to infinity, and the priority sequence k is initialized, k(s) = h(s);

[0050] 3) Set the starting grid s goal Insert into open list T;

[0051] 4) Start the iteration, extract the node s with the highest priority from the open list T, where when the priority of node s is the highest, k(s) is the smallest;

[0052] 5) Perform operations according to the node state. If k(s) < h(s), set the node state to 0. If k(s) = h(s), set the node to state 1;

[0053] 6) If the node state is 0, traverse all neighbor nodes s of node s i , calculate the path cost c(s i , s) from all neighbor nodes s to node s i , and further determine whether h(s i ) > h(s) + c(s i , s) holds. If so, update h(s i ) = h(s) + c(s i , s), set k(s i ) = h(s i ), and place s i into the open list T. If not, do not perform any operations and continue to traverse the next neighbor node;

[0054] 7) If the node state is 1, traverse all neighbor nodes s of node s i , calculate the path cost c(s, s i ) from all nodes s to node s i , and further determine whether h(s) > h(s i ) + h(s, s i ) holds. If so, update h(s) = h(s i ) + h(s, s i ), set k(s) = h(s), and place s into the open list T, set the state to 0. If not, keep h(s) unchanged, forcefully propagate state 1 to all neighbor nodes, and update the priority of node s;

[0055] 8) If the h(s) of the starting grid is stable, or the open list T is empty, end and output the overall trajectory sequence after local replanning. If h(s) is still infinite, it means there is no feasible path. Repeat steps 4) - 8) for iteration until the end.

[0056] Further, the steps for obtaining the dynamic sequence of motion configurations include:

[0057] (1) Generate the chromosome encoding of the initial configuration sequence based on the obstacle density and the overall trajectory sequence after local replanning, where the generation method of the chromosome encoding includes:

[0058] (2) Take the initial running trajectory sequence as the reference gene sample. On the reference gene sample, by setting random change amounts, accumulate the angular factor and the position factor, and set the position segment of the last gene as the position of the next trajectory point. Repeat this operation multiple times to finally obtain the chromosome encoding. For each chromosome encoding, each of its genes consists of multiple joint angle gene segments and multiple centroid position gene segments, respectively representing the configuration information and centroid position of the robot at the i-th sampling moment;

[0059] (3) Based on the chromosome encoding, construct a fitness function fitness based on obstacles and energy consumption, and the expression is:

[0060]

[0061] In the formula, C collision is the constraint conflict risk of the configuration sequence represented by the chromosome, α is the energy consumption weight of the joint angle change, m is the number of robot joints, n is the number of sampling moments in the motion configuration sequence, A ij is the angular change amount of the j-th joint at the i-th sampling moment, β is the energy consumption weight of the centroid position change, x, y, z are the three coordinate axes in the three-dimensional space, and S ik is the position of the robot centroid on the k-th coordinate axis at the i-th sampling moment;

[0062] (4) Perform conventional genetic operator operations of single-point crossover and K-point crossover and special genetic operator operations of non-uniform mutation on the chromosomes to obtain an alternative chromosome population;

[0063] (5) Calculate the individual fitness according to the fitness function;

[0064] (6) Generate a roulette wheel according to the individual fitness, and determine the next-generation population by roulette wheel gambling. The specific steps include:

[0065] ① Calculate the probability P(x i ) that each individual is inherited into the next-generation population:

[0066]

[0067] In the formula, x i is the i-th individual, fitness(x i ) is the fitness of the individual x i , fitness(x j ) is the fitness of the individual x j , and M is the population size;

[0068] ② Calculate the cumulative probability q i :

[0069]

[0070] In the formula, P(x j ) is the probability that individual x j is inherited into the next-generation population;

[0071] ③ Generate a pseudo-random number r uniformly distributed in the interval [0, 1];

[0072] ④ Select the individual i that satisfies the cumulative probability q i-1 < r < q i ;

[0073] ⑤ Repeat steps 3)-4) until a sufficient number of individuals are selected as the next-generation population;

[0074] (7) Determine whether the stop iteration condition is satisfied. If the maximum iteration number is reached or the optimal fitness approaches convergence, it is considered that the iteration is completed and the dynamic sequence of the motion configuration is output. If not, execute steps (4)-(8) until the stop iteration condition is satisfied.

[0075] Furthermore, the steps of the special genetic operator operation of the non-uniform mutation include:

[0076] Matching crossover: When performing the crossover operation, compare a gene before the selected crossover segment of two chromosomes. If the two match, perform crossover on the selected segment. If they do not match, do not perform crossover;

[0077] Non-uniform mutation: Adaptively adjust the mutation probability of genes. Among them, the more the number of iterations, the closer the gene position is to the specified position, and the higher the mutation probability. The mutation probability P mutation The formula is:

[0078]

[0079] In the formula, i is the index position of the chromosome gene in the chromosome sequence, u is the index position of the chromosome gene with the maximum expected mutation probability, σ is the mutation standard deviation, representing the diffusion degree of the mutation probability, P final is the final expected mutation probability at the end of the iteration, P init is the initial expected mutation probability at the initial iteration, i is the number of iterations, n gen is the maximum number of iterations, τ is the attenuation coefficient, controlling the attenuation speed of the mutation probability with the iteration, α is the scaling factor, overall adjusting the size of the mutation probability, and j is the index variable, used to traverse the positions on the chromosome sequence.

[0080] Compared with the prior art, the present invention has the following beneficial effects:

[0081] (1) The method of the present invention improves the traditional path planning method. Based on a scale-adaptive grid map, it can improve the planning accuracy in dangerous areas, increase the success rate of planning, and reduce risks. In addition, this application uses an improved Q-learning reinforcement learning method for global planning and an improved D* algorithm for real-time local replanning, which can adjust the fineness of the planned path. On this basis, a configuration sequence planning is carried out for the planned path, further improving the planning accuracy, solving the control redundancy problem of underwater multi-joint robots, and improving the planning accuracy in complex underwater dynamic environments.

[0082] (2) During the trajectory planning process of the present invention, motion constraints and dynamic constraints in the actual motion process are respectively considered. The improved D* algorithm is used for real-time adjustment and replanning of part of the trajectory, which can perform real-time correction on the local part of the initial operation trajectory sequence and improve the accuracy of path planning.

[0083] (3) During the dynamic configuration dynamic planning process of the present invention, chromosome coding is combined with the genetic algorithm, which has an efficient global search ability and improves the accuracy of the configuration sequence. BRIEF DESCRIPTION OF THE DRAWINGS

[0084] Figure 1 is a schematic flow chart of the method of the present invention;

[0085] Figure 2 is an embodiment schematic diagram of the adaptive scale grid map of the present invention;

[0086] Figure 3 is a schematic flow chart of the Q-learning global trajectory planning of the present invention;

[0087] Figure 4 is a schematic diagram of the executable actions of the present invention;

[0088] Figure 5 is the improved D* replanning algorithm flow of the present invention;

[0089] Figure 6 is a schematic flow chart of the genetic algorithm configuration planning of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

[0090] The present invention will be described in detail below with reference to the drawings and specific embodiments. This embodiment is implemented on the premise of the technical solution of the present invention, and the detailed implementation manners and specific operation processes are given. However, the protection scope of the present invention is not limited to the following embodiments.

[0091] This embodiment provides a motion planning method for an underwater ultra - redundant dexterous robot under dynamic constraint conditions. Based on an improved algorithm of traditional trajectory planning and combined with the characteristics of the underwater ultra - redundant dexterous robot itself, it realizes dynamic configuration sequence planning, and solves the control redundancy problem of the ultra - redundant dexterous robot from the planning layer. First, according to the environmental map, a scale - adaptive grid map is constructed, and the grid number and scale of different regions are determined according to the obstacle size and task information; secondly, an improved reinforcement learning method is used to obtain the motion trajectory of the centroid of the underwater ultra - redundant dexterous robot, and during the actual motion process, based on the dynamic environmental constraints sensed by the sensor, the D* algorithm is used to adjust and replan part of the trajectory in real - time; finally, an improved genetic algorithm is used to plan the expected configuration sequence between trajectory points. Next, this embodiment takes a multi - joint robot with 5 joints as the application object of the algorithm. The length of a single section of this robot is 0.6m, and the maximum diameter is 0.45m. As Figure 1 shown, this method includes the following steps:

[0092] Step S1: Obtain a three - dimensional static map, analyze information such as the task target area, obstacles, and terrain. In this embodiment, it includes multiple environmental obstacles and a target area for inspection operations. For this map, perform adaptive grid processing to obtain a map C with an adaptive grid scale map , which specifically includes the following sub - steps:

[0093] Step S11: Perform initial grid processing on the environmental map according to the initial grid scale. Specifically:

[0094] 1) According to the size data of the underwater ultra - redundant dexterous robot, determine that the size of the initial grid is 2m×2m×2m, and perform a preliminary grid division on the environmental map, dividing the map into multiple grids of the same scale;

[0095] 2) For each initial map grid, determine the obstacle density ρ and task information density t contained in the grid. The expressions are:

[0096]

[0097] where ρ i is the ratio of the obstacle volume to the total volume of the grid for the i - th grid, and t i is the ratio of the task area volume to the total volume of the grid for the i - th grid.

[0098] 3) According to the obstacle density ρ and task information density t of each map grid, determine the number n of sub - grids of the grid. The expression is:

[0099]

[0100] According to this expression, the high-density obstacle area and the inspection task area are subdivided into 8 sub-grids, and the open area remains at the original scale.

[0101] 4) Subdivide the grid according to the number of sub-grids in each grid, and finally generate a grid map C with an adaptive scale map . The specific process of grid subdivision is as follows:

[0102] According to the obstacle density ρ and task information density t of the corresponding initial grid, determine the number of feasible grids and infeasible grids in the refined grid according to the ratio of the obstacle volume to the actual grid volume and the ratio of the task area volume to the actual grid volume;

[0103] Determine the spatial range of the obstacle, and fill the grids with the calculated number of infeasible grids. When the obstacle space can be completely covered, determine the scale of the infeasible grids;

[0104] Determine the spatial ranges of the task area and the feasible area, and fill the grids with the calculated number of feasible grids. When the feasible space can be completely covered and there is no conflict with the infeasible grids, determine the scale of the feasible grids.

[0105] 5) Finally, generate an adaptive-scale grid map as shown in Figure 2 . In the figure, the black area represents the obstacle area, the bottom right area on the right represents the operation task area. Based on the variable-scale division method, it is subdivided into grids with a smaller scale. The remaining gray areas are passable areas and remain at the original grid scale. The grid map is represented as:

[0106] C map = [C map0 , C map1 , …, C mapi , C mapm

[0107] C mapi = [C mapi0 , …, C mapij , …, C mapin

[0108] Among them, C map stores the initial grids of the entire map, indexes each grid through the grid serial number, and indexes in the order of row, column, and height from the upper left corner; each item C mapi represents an initial grid. According to the index and the scale of the initial grid, its three-dimensional position in the map can be determined, and it internally stores n i sub-grid structures. Each structure C mapij saves the lower left corner coordinates, length, width, height, and passability attribute of the grid, representing one of the refined variable-scale sub-grids.

[0109] Step S2. Based on the motion constraints and the improved Q-learning reinforcement learning method, plan the motion trajectory sequence P of the overall centroid position of the hyper-redundant multi-joint robot t , as Figure 3 shown. The specific process is as follows:

[0110] Step S21. Generate the initial Q-value table, as shown in Table 1:

[0111] Table 1 Q-value table

[0112]

[0113] Step S22. Check whether the maximum number of iterations is reached. If the maximum number of iterations is not reached, generate the adaptive normalized action factor A. If the maximum number of iterations is reached, the algorithm stops and saves the trained Q-value table. The expression of the adaptive normalized action factor A is:

[0114]

[0115] where A0 is the initial action factor, A min is the minimum action factor, G is the maximum number of iterations, and g is the current number of iterations.

[0116] Step S23. Compare the action factor A with the action reference G. If A > G, take a random action; otherwise, take the maximum Q-value action. As Figure 4 shown, the available action is to move to the surrounding grids of the current three-dimensional grid where it is located;

[0117] Step S24. Update the position state according to the action taken, and check whether the current position state meets the hard constraints such as static obstacles and kinematic constraints. If not, give a penalty, update the Q value, and start a new iteration. The specific process is as follows:

[0118] 1) Based on the size parameters of the hyper-redundant robot, delimit the collision monitoring area centered on the grid where it is located. For this embodiment, the collision detection range is 1 grid in each direction of the grid where it is located;

[0119] 2) Retrieve the passage attributes of all refined grids with different scales included in the collision monitoring area. If there is an impassable area, it is regarded as a collision, and the maximum penalty Q punish .

[0120] 3) If a collision occurs, update the Q value of the state-action pair in the Q-value table and place the robot at the initial grid position.

[0121] Step S25. Update the Q value according to the soft constraints of the energy consumption, path length, and low-priority task information of the current state. The expression for calculating the Q value is:

[0122] Q(s,a) = Q(s,a) + α[R(s,a) + γ max Q(s′,a′) - Q(s,a)]

[0123] Among them, Q(s,a) is the value of taking action a in state s; α is the learning rate, which is used to control the learning speed; R(s,a) is the reward and punishment obtained by taking action a in state s; γ is the discount factor, which is used to balance the importance of current rewards and future rewards; s′ is the new state reached after executing action a; max Q(s′,a′) is the maximum value of all possible actions in the new state s′.

[0124] Step S26: Determine whether the end point is reached. If the end point is reached, give the highest reward Q max and update the Q value, output the planned path, and start a new iteration round.

[0125] In this embodiment, the parameters of the reinforcement learning path planning algorithm are set as shown in Table 2.

[0126] Table 2 Algorithm Parameter Settings of the Embodiment

[0127] Parameter A0 <![CDATA[A min > G α β <![CDATA[Q punish > <![CDATA[P max > Value 0.9 0.1 1000 0.8 0.95 -1000 1000

[0128] Step S3: Based on the initial motion trajectory sequence, during the actual motion process, evaluate the dynamic constraints according to the information collected by the sensor, and replan part of the trajectory P according to the improved D* Part as Figure 5 shown, and the specific process is as follows:

[0129] Step S31: During actual operation, dynamically modify the initial grid map according to the sensor data. When dynamic obstacles and unmodeled obstacles are found, re-divide the grid in the area where they are located and define the passage attributes;

[0130] Step S32: Determine whether the current position to the next path point violates the environmental and body motion constraints. If the grid attribute where the movement arrives is not passable, retrieve the affected path range according to the area of the overall non-passable grid, and determine the replanning range. The body motion constraint refers to the structural limitation of the robot itself. For example, in the original planned path, it is necessary to turn 180 degrees to avoid obstacles, but the structure of this robot only supports a 90-degree turn. Therefore, it is necessary to replan the motion path to find other feasible solutions;

[0131] Step S33: Determine the start point and end point of the replanning segment according to the replanning range, and use the D* algorithm to replan the trajectory of the replanning segment. The specific process is as follows:

[0132] 1) Initialize the bidirectional edge map, connect the center points of adjacent grid cells, indicating a feasible path from one grid cell to another. Set the weight of the edge to 1, and non-passable grid points are not connected to other grids.

[0133] 2) Initialize the cost sequence h, set the cost h(s goal ) of the end point to 0, and set the cost h(s) of all other grid points to infinity. Initialize the priority sequence k, k(s) = h(s);

[0134] 3) Insert the end point s goal into the open list T;

[0135] 4) Start iteration, extract the node s with the highest priority (the smallest k(s)) from the open list T;

[0136] 5) Perform operations according to the node status. If k(s) < h(s), set the node status to 0. If k(s) = h(s), set the node to status 1.

[0137] 6) If the node status is 0, traverse all neighbor nodes s i of the node s, calculate the path cost c(s i , s) from all neighbor nodes s i to the node s. If h(s i ) > h(s) + c(s i , s), update h(s i ) = h(s) + c(s i , s), set k(s i ) = h(s i ), and insert s i into the open list T.

[0138] 7) If the node status is 1, traverse all neighbor nodes s i of the node s, calculate the path cost c(s, s i ) from all nodes s to the node s i . If h(s) > h(s i ) + h(s, s i ), update h(s) = h(s j ) + h(s, s i ), set k(s) = h(s), and insert s into the open list T, set its status to 0. Otherwise, keep h(s) unchanged, forcefully propagate status 1 to all neighbor nodes, and update their priorities.

[0139] 8) If the h(s) of the start point is stable, or the open list T is empty, the algorithm ends. If h(s) is still infinity, it means there is no feasible path.

[0140] Step S4. Based on the centroid motion trajectory, the dynamic configuration sequence S of the hyper-redundant multi-joint robot is dynamically planned based on the improved genetic algorithm t , as Figure 6 shown, the specific process is as follows:

[0141] Step S41. Based on the environmental obstacle density and the overall trajectory sequence after local replanning, generate the chromosome encoding of the initial configuration sequence. Here, the obstacle density determines the number of genes of each chromosome, and then the configuration sequence should be planned according to the overall trajectory after replanning. The specific encoding method is as follows:

[0142] [10A 11 A 12 A 13 A 14 A 15 S 1x S 1y S 1z …A i1 A i2 …A ij …A im S ix S iy S iz …A 101 A 102 A 103 A 104 A 105 S 1x S 10y S 10z

[0143] For a coding, each of its genes consists of 5 joint angle gene segments and three centroid position gene segments, representing the centroid position and configuration information of the robot at the i-th sampling moment respectively.

[0144] The initial chromosome coding generation method is as follows:

[0145] Based on the configuration during planning as the reference gene sample of the initial sequence, on the basis of this gene, by setting a random change amount, the angle factor and position factor are accumulated on the initial gene sample, and the position segment of the last gene is set as the position of the next trajectory point. Repeat this operation multiple times until a sufficient number of individuals are generated.

[0146] Step S42. Construct a fitness function fitness based on environmental obstacles and energy consumption, and its expression is:

[0147]

[0148] Among them, C collision ​For the risk of constraint conflict of the configuration sequence represented by this chromosome, α is the energy consumption weight for the change in joint angle, and β is the energy consumption weight for the change in the position of the center of mass.

[0149] Step S43: Obtain a group of alternative chromosomes by performing conventional genetic operator operations such as single-point crossover and K-point crossover on the chromosomes, and special genetic operator operations such as matching crossover and non-uniform mutation. Specifically:

[0150] The special genetic operator operations include:

[0151] Matching crossover: When performing the crossover operation, compare a gene before the selected crossover segment of two chromosomes. If the two match, perform crossover on the selected segment.

[0152] Non-uniform mutation: Adaptively adjust the mutation probability of genes. The more iterations, the closer the gene position is to the specified position, and the higher the mutation probability. The mutation probability formula is:

[0153]

[0154] where i is the index position of the chromosome gene in the chromosome sequence, u is the index position of the chromosome gene with the maximum expected mutation probability in the chromosome sequence, σ is the mutation standard deviation, representing the diffusion degree of the mutation probability, P final is the final expected mutation probability at the end of the iteration, P init is the initial expected mutation probability at the initial iteration, k is the number of iterations, n gen is the maximum number of iterations, and τ is the attenuation coefficient, controlling the attenuation speed of the mutation probability with iterations.

[0155] The mutation probability of this non-uniform mutation takes into account two factors: the number of iterations and the gene position. As the number of iterations increases, the mutation probability decreases, enabling the chromosome group to obtain a larger mutation probability in the initial stage, reducing the possibility of falling into a local optimum, and being able to reduce the mutation probability in the later stage of iteration to improve the convergence speed. For a chromosome, the genes at the initial segment need to consider the current position and configuration of the robot, while the genes at the end segment need to consider the planned initial trajectory. Therefore, the mutation probability of genes near the initial and end segments is lower to conform to the trajectory planning results and current state constraints, and the mutation probability of genes near u (usually taken as n / 2) is higher.

[0156] Step S44: Calculate the individual fitness according to the fitness function;

[0157] Step S45: Generate a roulette wheel according to the fitness, and determine the next-generation population by roulette wheel gambling. The specific process is as follows:

[0158] 1) Calculate the probability that each individual is inherited into the next-generation population:

[0159]

[0160] Among them, M is the population size;

[0161] 2) Calculate the cumulative probability of each individual:

[0162]

[0163] 3) Generate a pseudo-random number r uniformly distributed in the interval [0, 1].

[0164] 4) Select the individual i that satisfies the cumulative probability q i-1 < t < q i of.

[0165] 5) Repeat steps 3)-4) until a sufficient number of individuals are selected as the next-generation population.

[0166] Step S46: Determine whether the stop iteration condition is satisfied. When the maximum number of iterations is reached, or the optimal fitness is close to convergence, it is considered that the algorithm has completed the iteration and outputs the best configuration sequence.

[0167] This configuration sequence includes the positions x t , y t , z t of the robot at different times t and the configuration parameters (joint angles) A 11 A 12 … A 1j … A 1m , guiding the robot to follow the planned motion trajectory according to the given configuration.

[0168] In this embodiment, the parameter settings of the genetic algorithm configuration planning algorithm are shown in Table 3.

[0169] Table 3 Algorithm Parameter Settings of the Embodiment

[0170] Parameter u σ <![CDATA[P init > <![CDATA[P final > α β Value 3 1.2 0.15 0.01 0.2 0.8

[0171] This embodiment can realize the motion planning of a multi-joint robot with 5 joints, establish an adaptive-scale grid map based on obstacles and task density, use reinforcement learning to plan the motion trajectory of the centroid, use the D* algorithm to replan the infeasible trajectory, and use the genetic algorithm to plan the configuration during motion.

[0172] If the above functions are implemented in the form of software function units and sold or used as independent products, they can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art, or a part of this technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) to execute all or part of the steps of the methods described in various embodiments of the present invention. The foregoing storage medium includes: various media that can store program codes, such as USB flash drives, mobile hard disks, read-only memories (ROM, Read-Only Memory), random access memories (RAM, Random Access Memory), magnetic disks, or optical discs.

[0173] Those skilled in the art should understand that the embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a complete hardware embodiment, a complete software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present invention can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk memories, CD-ROMs, optical memories, etc.) containing computer-usable program codes. The solutions in the embodiments of the present invention can be implemented in various computer languages. For example, object-oriented programming languages such as Java and interpreted scripting languages such as JavaScript.

[0174] The present invention is described with reference to the flowcharts and / or block diagrams of methods, apparatuses (systems), and computer program products according to embodiments of the present invention. It should be understood that each flow and / or block in the flowchart and / or block diagram, and the combination of flows and / or blocks in the flowchart and / or block diagram, can be implemented by computer program instructions. These computer program instructions can be provided to the processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing devices to generate a machine, so that the instructions executed by the processor of the computer or other programmable data processing devices generate a device for implementing the specified functions in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0175] These computer program instructions can also be stored in a computer-readable memory that can direct a computer or other programmable data processing device to work in a specific manner, so that the instructions stored in the computer-readable memory generate a manufactured article including an instruction device, and the instruction device implements the specified functions in Figure 1 one flow or multiple flows and / or blocks Figure 1 one block or multiple blocks.

[0176] These computer program instructions may also be loaded onto a computer or other programmable data processing apparatus, causing a series of operational steps to be performed on the computer or other programmable apparatus to produce a computer-implemented process, so that the instructions executed on the computer or other programmable apparatus provide steps for implementing the functions specified in one process or a plurality of processes and / or blocks Figure 1 one process or a plurality of processes and / or blocks Figure 1 or steps for implementing the functions specified in a plurality of blocks or a plurality of blocks.

[0177] Although the preferred embodiments of the present invention have been described, additional changes and modifications can be made by those skilled in the art once they learn of the basic creative concept. Therefore, the appended claims are intended to be construed to cover the preferred embodiments as well as all changes and modifications falling within the scope of the present invention.

[0178] Obviously, those skilled in the art can make various changes and modifications to the present invention without departing from the spirit and scope of the present invention. Thus, if these modifications and variations of the present invention fall within the scope of the claims of the present invention and their equivalent technologies, the present invention is also intended to include these modifications and variations.

Claims

1. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions, characterized in that The steps include: Obtain an underwater three-dimensional environment map and construct a scale-adaptive grid map; Based on the grid map, according to the motion constraints, adopt an improved Q-learning reinforcement learning method for global planning to obtain the initial running trajectory sequence of the robot; Based on the initial running trajectory sequence, according to the dynamic constraints in the actual motion process, adopt an improved D* algorithm for local replanning to obtain the overall trajectory sequence after local replanning of the robot; Based on the initial running trajectory sequence and the overall trajectory sequence after local replanning, adopt an improved genetic algorithm for dynamic configuration planning to obtain a dynamic configuration sequence and complete the motion planning process.

2. The motion planning method of an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 1, characterized in that The steps of constructing the scale-adaptive grid map include: Determine the initial grid scale according to the size data of the robot, and perform initial gridification on the underwater three-dimensional environment map to obtain a plurality of initial map grids with the same size, where the underwater three-dimensional environment map includes a task area, an obstacle area, a passable area, and an impassable area for inspection operations; Determine the obstacle density and task information density contained in each initial map grid, where the expressions of the obstacle density and task information density are respectively: where ρ i is the ratio of the obstacle volume to the total volume of the i-th initial map grid, V obstacle-i is the obstacle volume of the i-th initial map grid, V grid-i is the total volume of the i-th initial map grid, t i is the ratio of the task area volume to the total volume of the i-th initial map grid, V task-i is the task area volume of the i-th initial map grid; According to the obstacle density and task information density of each initial map grid, determine the number of subdivided grids, where the calculation expression of the number of subdivided grids is: where n i is the number of subdivided grids of the i-th initial map grid, ρ j is the ratio of the volume of the obstacle in the j-th initial map grid to the total volume of the grid, t j is the ratio of the volume of the task area in the j-th initial map grid to the total volume of the grid, and N is the preset maximum number of subdivided grids; Subdivide each initial map grid according to the number of subdivided grids of each initial map grid, and finally generate a scale-adaptive grid map C map , where the scale-adaptive grid map C map is expressed as: C map = [C map0 , C map1 , …, C mapi , C mapm ​ C mapi = [C mapi0 , …, C mapij , …, C mapin ​ Among them, C mapi is the i-th initial map grid. According to the index and the scale of the initial map grid, C mapi the three-dimensional position in the underwater three-dimensional environment map is determined. C mapi stores n i sub-grid structure bodies internally. Each sub-grid structure body C mapij saves the coordinates of the lower left corner, length, width, height, and passability attribute of the grid, representing one of the refined variable-scale sub-grids.

3. The motion planning method of an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 2, characterized in that, The steps of subdividing each initial map grid include: According to the obstacle density and task information density of each initial map grid, and according to the ratio of the obstacle volume to the actual initial map grid volume and the ratio of the task area volume to the actual initial map grid volume, determine the number of passable grids and impassable grids in the subdivided grids; Determine the spatial range of the obstacle, and use the impassable grids for filling to determine the scale of the impassable grids when completely covering the obstacle spatial range; Determine the spatial ranges of the task area and the passable area, and use the passable grids for filling to determine the scale of the passable grids when completely covering the passable area spatial range and not conflicting with the impassable grids, and complete the subdivision process.

4. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 1, characterized in that, The motion constraints include hard constraints and soft constraints. The hard constraints include static obstacle constraints and kinematic constraints, and the soft constraints include the energy consumption, path length, and low-priority task information of the current state.

5. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 4, characterized in that The steps for obtaining the initial running trajectory sequence include: Generate an initial Q-value table, where the Q-value table includes different position states of the robot and corresponding actions in different position states; Judge whether the maximum number of iterations is reached. If so, end and save the Q-value table. If not, generate an adaptive normalized action factor A, where the expression of the adaptive normalized action factor A is: wherein, A0 is the initial action factor, A min is the minimum action factor, G is the maximum number of iterations, and g is the current number of iterations; Judge whether the adaptive normalized action factor A is greater than the action reference. If so, take a random action. If not, take the maximum Q-value action; Update the position state according to the taken action, and judge whether the current position state meets the hard constraints. If so, execute the next step. If not, give a penalty, update the Q value, and start a new iteration; Determine whether the end point is reached. If the end point is reached, give the highest reward Q max , set the end flag to 1, otherwise execute the next step; Update the Q value according to the soft constraint, and the calculation expression of the Q value is: Q(s,a) = Q(s,a) + α[R(s,a) + γmaxQ(s′,a′) - Q(s,a)] In the formula, Q(s,a) is the value of taking action a in state s, α is the learning rate used to control the learning speed, R(s,a) is the reward and punishment obtained by taking action a in state s, γ is the discount factor used to balance the importance of the current reward and future rewards, and maxQ(s′,a′) is the maximum value of all possible actions in the new state s′; Judge whether the end flag bit is set to 1. If it is set to 1, output the planned path, save the Q value table, and end the iteration. If not, continue the iteration.

6. The motion planning method of an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 5, wherein In the case of not meeting the hard constraint, the process of giving punishment includes: Based on the size data of the robot, delimit a collision monitoring area centered on the refined grid where it is located; Retrieve the passage attributes of all refined grids of different scales included in the collision monitoring area, and determine whether there is an impassable area. If so, it is regarded as a collision, and the maximum penalty Q is given. punish , update the Q value of the state-action pair in the Q value table, and place the robot at the initial position; if not, it is regarded as no collision.

7. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 1, characterized in that, The steps for obtaining the overall trajectory sequence after local replanning include: Obtain the sensor data of the robot during the actual movement process, obtain dynamic obstacles and unmodeled obstacles according to the sensor data, and re-divide and define the passage attributes of the initial map grid in the area according to the regional position where it is located, so as to dynamically modify the initial grid map; Judge whether the current position of the robot to the next path point violates the underwater environment or the body movement constraint. If it moves to an impassable grid, retrieve the affected path range according to the area of the overall impassable grid to determine the replanning range. If not, continue to move; Determine the starting grid and ending grid of the replanning segment according to the replanning range, and use the improved D* algorithm to perform local replanning to obtain the overall trajectory sequence after local replanning.

8. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 7, characterized in that, The steps of performing local replanning using the improved D* algorithm include: 1) Initialize the two-way edge map, take each refined grid as a node, and establish two-way edges between adjacent passable grids: connect the center points of adjacent grid cells, indicating a feasible path from one grid cell to another grid cell, and set the weight of the edge; 2) Initialize the cost sequence h, and set the cost h(s goal ) of the end grid to 0, and set the cost h(s) of all the remaining grids to infinity. Initialize the priority sequence k, where k(s) = h(s); 3) Insert the starting grid s goal into the open list T; 4) Start the iteration, extract the node s with the highest priority from the open list T, where when the priority of the node s is the highest, k(s) is the smallest; 5) Perform operations according to the node state. If k(s) < h(s), set the node state to 0. If k(s) = h(s), set the node to state 1; 6) If the node status is 0, traverse all neighbor nodes s of node s i , calculate all neighbor nodes s i to the path cost c(s i , s) of node s, and further judge whether h(s i ) > h(s) + c(s i , s) holds. If so, update h(s i ) = h(s) + c(s i , s), set k(s i ) = h(s i ), and place s i into the open list T. If not, do not perform any operation and continue to traverse the next neighbor node; 7) If the node state is 1, traverse all neighbor nodes s of node s i , calculate the path cost c(s, s i ) from all nodes s to node s i ), and further determine whether h(s) > h(s i ) + h(s, s i ) holds. If so, update h(s) = h(s i ) + h(s, s i ), set k(s) = h(s), and place s into the open list T, set the state to 0. If not, keep h(s) unchanged, forcefully propagate state 1 to all neighbor nodes, and update the priority of node s; 8) If the h(s) of the starting grid is stable, or the open list T is empty, end, and output the overall trajectory sequence after local replanning. If h(s) is still infinite, it means there is no feasible path, and repeat steps 4)-8) for iteration until the end.

9. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 1, characterized in that The steps for obtaining the dynamic sequence of motion configurations include: (1) Generate the chromosome encoding of the initial configuration sequence based on the obstacle density and the overall trajectory sequence after local replanning, where the generation method of the chromosome encoding includes: (2) Take the initial running trajectory sequence as a reference gene sample. On the reference gene sample, by setting random change amounts, accumulate the angular factor and the position factor, and set the position segment of the last gene as the position of the next trajectory point. Repeat this operation multiple times. Finally, obtain the chromosome encoding. For each chromosome encoding, each of its genes consists of multiple joint angle gene segments and multiple centroid position gene segments, respectively representing the configuration information and the centroid position of the robot at the i-th sampling moment; (3) Based on the chromosome encoding, construct a fitness function fitness based on obstacles and energy consumption, and the expression is: where C collision is the constraint conflict risk of the configuration sequence represented by the chromosome, α is the energy consumption weight of the joint angle change, m is the number of robot joints, n is the number of sampling moments in the motion configuration sequence, A ij is the angle change of the j-th joint at the i-th sampling moment, β is the energy consumption weight of the centroid position change, x, y, z are the three coordinate axes in the three-dimensional space, S ik is the position of the robot centroid on the k-th coordinate axis at the i-th sampling moment; (4) Perform conventional genetic operator operations of single-point crossover and K-point crossover and special genetic operator operations of non-uniform mutation on the chromosomes to obtain an alternative chromosome population; (5) Calculate the individual fitness according to the fitness function; (6) Generate a roulette wheel according to the individual fitness, and determine the next-generation population by roulette wheel gambling. The specific steps include: ① Calculate the probability P(x i ) that each individual is inherited into the next generation population: where x i is the i-th individual, fitness(x i ) is the fitness of the individual x i , fitness(x j ) is the fitness of the individual x j , and M is the population size; ② Calculate the cumulative probability q of each individual i : where, P(x j ) is the probability that individual x j is inherited into the next generation population; ③ Generate a uniformly distributed pseudo-random number r in the interval [0, 1]; ④ Select an individual i that satisfies the cumulative probability q i-1 <r < q i ; ⑤ Repeat steps 3)-4) until a sufficient number of individuals are selected as the next-generation population; (7) Determine whether the stop iteration condition is satisfied. If the maximum iteration number is reached, or the optimal fitness is close to convergence, it is considered that the iteration is completed, and the dynamic sequence of the motion configuration is output. If not, execute steps (4)-(8) until the stop iteration condition is satisfied.

10. A motion planning method for an underwater ultra-redundant dexterous robot under dynamic constraint conditions according to claim 9, characterized in that, The steps of the special genetic operator operation of non-uniform mutation include: Matching crossover: When performing the crossover operation, compare a gene before the selected crossover segment of two chromosomes. If the two match, perform crossover on the selected segment. If the two do not match, do not perform crossover; Non-uniform mutation: adaptively adjust the mutation probability of genes. Among them, the more iterations are performed, the closer the gene position is to the specified position, and the higher the mutation probability. The mutation probability P mutation The formula is: where i is the index position of the chromosome gene in the chromosome sequence, u is the index position of the chromosome gene with the maximum expected mutation probability in the chromosome sequence, σ is the mutation standard deviation, representing the diffusion degree of the mutation probability, P final is the final expected mutation probability at the end of the iteration, P init is the initial expected mutation probability at the initial iteration, k is the number of iterations, n gen is the maximum number of iterations, τ is the attenuation coefficient, controlling the attenuation speed of the mutation probability with the iteration, α is the scaling factor, overall adjusting the magnitude of the mutation probability, and j is the index variable, used to traverse the positions on the chromosome sequence.

Images