Path planning method for redundantly driven robotic arm based on DSAW offline reinforcement learning algorithm

Abstract

The present invention discloses a redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm, which belongs to the field of robot arm three-dimensional space path planning. In view of the collision detection problem in three-dimensional space, the collision relationship between the path node and the obstacle is simplified to the collision relationship between the sphere and the cuboid by the encirclement method to avoid the collision problem; in view of the design problem of the reinforcement learning path planning environment, the basic reward function, the collision penalty reward function, and the target reward function are designed from multiple aspects such as position, collision constraint, and step length limit to guide the redundant drive robot arm to learn the behavior strategy of reaching the target point. Then, the model predictive control algorithm is used to perform inverse solution calculation on the nodes on the path to obtain the corresponding joint angle. It is easy to use and has high calculation efficiency, which ensures the joint movement angle of the optimal solution of the redundant drive robot arm path.

Description

Technical Field

[0001] The present invention belongs to the field of three-dimensional space path planning of a robotic arm, and specifically is a redundant drive robotic arm path planning method based on a DSAW off-line reinforcement learning algorithm. Background Art

[0002] Path planning is a core task for the robot to carry out autonomous operations. Its goal is to find the optimal or feasible path for the end of the robot arm from the starting point to the end point in the environment. At the same time, there are infinite inverse solutions for redundant drive robot arms. In the process of redundant drive robot arm path planning, many factors need to be considered, such as avoiding obstacles, inverse solutions, minimizing path length or cost, and following specific path rules. Path planning is crucial to improving the efficiency and safety of robots and automation systems.

[0003] With the development of technology, although traditional path planning methods are effective in specific scenarios, they face challenges in complex, dynamic or unknown environments. These environments require the system to have higher adaptability and learning ability, and be able to make decisions based on environmental changes and historical experience. Reinforcement learning can adapt well to these problems due to its high degree of autonomous learning. Therefore, the combination of path planning and reinforcement learning provides a new perspective and method for solving navigation problems in complex environments.

[0004] However, in a more complex and uncertain high-dimensional environment, reinforcement learning is difficult to apply to high-dimensional multi-objective tasks due to its own low exploration performance and limited experience pool size. It is not only time-consuming to explore but also has slow convergence speed, which can easily lead to the curse of dimensionality and reduce planning ability. Offline reinforcement learning, also known as batch reinforcement learning, is a variant of reinforcement learning. Its main idea is to redundantly drive the robotic arm to learn from a fixed data set without exploration.

[0005] The path planning of the robotic arm is different from that of drones and cars. Therefore, in the path planning part, it is also necessary to consider how to calculate the accurate joint angles so that the end of the robotic arm can reach the specified position correctly. Summary of the invention

[0006] Purpose of the invention: In view of the above-mentioned prior art, a path planning method based on the DSAW (Dense State Advantage Weighting) offline reinforcement learning algorithm is proposed to improve the node traversal speed, reduce the number of iterations and the output path length in large-scale maps, and ensure the optimal joint solution angle.

[0007] Technical solution: To achieve the above technical objectives, the present invention discloses a redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm, comprising the following steps:

[0008] Step 1: Perform preliminary preparation for the reinforcement learning path planning of the redundant driving robot arm and initialize the global map: Model the environment of the redundant driving robot arm moving scene in the computer and establish a three-dimensional coordinate system. Use the encirclement method to simplify all obstacles in the redundant driving robot arm moving scene environment into cuboids and spheres to facilitate subsequent obstacle collision detection and prevent collision problems; let the number of obstacles be k, and the obstacle set be {obj i}, obstacles simplified to spheres include the radius and center coordinate information of the sphere, and obstacles simplified to rectangles include the minimum and maximum coordinate information on each direction axis, that is,

[0009] Step 2: Construct a reinforcement learning path planning environment for the redundant driving robot in the simulation simulator CoppeliaSim. The path planning environment includes the reinforcement learning state space, reinforcement learning action space, and reinforcement learning reward function to guide the redundant driving robot to learn the behavior strategy to reach the target point.

[0010] Step 2.1 Design the reinforcement learning state space; according to the actual needs of path planning and global map information, design the state space as state = {start, goal, current, dist obstacle ,dist goal ,step}, where start=(x init ,y init ,z init ) represents the starting position of the redundant driving robot in the path planning environment, goal = (x end ,y end ,z end ) indicates the end point, current=(x current ,y current ,z current ), indicating the current node position of the redundant drive robot, dist obstacle Indicates the distance between the current node and the obstacle, dist goal Indicates the distance between the current node and the end point, and step indicates the number of running steps;

[0011] Step 2.2: Design the reinforcement learning action space and define all possible actions that the redundant driving robot can perform. The most critical actions involve the direction and step length in the process of finding the point. Therefore, the action space is designed as action = {x, y, z, length}, where (x, y, z) represents the direction vector, the range is designed to be (-1, 1), and length represents the step length of a single movement of the redundant driving robot.

[0012] Step 2.3: Design a reward function for reinforcement learning; the reward function includes rewards for correct behavior of the redundantly driven robot arm, penalties for undesirable behavior, and a mechanism to guide the redundantly driven robot arm to avoid obstacles and achieve path planning in three-dimensional space. Learning and optimization;

[0013] Step 3: Build an offline reinforcement learning dataset in a reinforcement learning path planning environment The offline reinforcement learning dataset includes historical interaction records generated during the training process, data generated by the simulation simulator, and an artificial path planning expert database. Perform necessary normalization preprocessing to improve its usability;

[0014] Step 4: Use normalized preprocessed dataset The offline reinforcement learning algorithm DSAW is iterated to output the path nodes n0, n1, ..., n in the process of redundant driving robot end movement. n ;

[0015] The specific update process of the offline reinforcement learning algorithm DSAW algorithm is:

[0016] According to the reinforcement learning path planning environment, a dense state advantage weighting function w(s,s′) is proposed to assign a larger weight to the ideal strategy and a smaller weight to the poor strategy:

[0017] w(s,s')=[A(s,s')≥0]=[(Q θ (s,s')-V ψ (s))≥0]= 1 [Q θ (s,s')≥V ψ (s)]

[0018] Where A(s,s') represents the state advantage function, Q θ (s,s') and V ψ (s) represent the current state, the next state value function and the evaluation function of the current state respectively; 1 [Q θ (s,s')≥V ψ (s)] means that when Q(s,s′) is greater than V(s), the value of w(s,s′) is 1, and when Q(s,s′) is less than V(s), the value of w(s,s′) is 0;

[0019] According to the dense state advantage weighting function w(s,s′), the updating process of the offline reinforcement learning algorithm DSAW firstly updates the state evaluation function V δ (s) To update:

[0020]

[0021] In the formula, represents the indicator function, k represents the adjustment parameter, V δ (s) represents the state evaluation function, Represents the loss function when the state evaluation function is updated; weight distribution function To emphasize Q θ' (s,s') is greater than V δ (s) value, the weight of the high-quality state is increased when the DSAW algorithm updates the state evaluation function, and the weight of the low-quality state is reduced, while Q θ' (s, s') is generated by the target network θ' of the network parameter θ of the current state and the next state value function, and the state evaluation network V δ (s) is generated by the network parameter δ; then the state value function Q θ (s,s') is updated:

[0022]

[0023] In the formula, Represents the loss function when the current state and the next state value function are updated;

[0024] In progress θ (s,s') and V δ (s) when updating, only use To ensure that the data of state s and the next state s' are in the offline dataset when updating In this way, the interference of extrapolation error is avoided;

[0025] Generate the forward network f from the forward network parameters φ φ (s,a) and predict the next state through state s and action a, and train by minimizing:

[0026]

[0027] In the formula, Represents the loss function when the forward network function is updated;

[0028] Construct an inverse dynamics model I generated by the network parameter ω ω (s,s') to infer action a, inverse dynamics model I ω (s,s') is trained by the dense state advantage weighting function w(s,s'):

[0029]

[0030] In the formula, Iω represents the inverse dynamics network, s and s′ represent the current state and the next state, Represents the loss function when the inverse dynamics model function is updated;

[0031] Construct an additional prediction network s′=τ ψ (s), evaluate the next state s′, and perform training iterations in the following way to keep close to the dataset Distribution in:

[0032]

[0033] In the formula, Represents the loss function when the prediction network function is updated;

[0034] Step 5: Use the encirclement method to set the redundant drive robot arm joints and obstacles, simplify the robot arm connecting rods into cylinders, simplify the obstacles into spheres, calculate the distance between the robot arm joints and obstacles, and perform obstacle avoidance detection and collision prevention tests on the redundant drive robot arm joints;

[0035] Step 6: Establish the MPC state equation of the redundant drive robot model predictive control algorithm, and calculate the path nodes n0, n1, ..., n during the movement of the redundant drive robot end. n Perform inverse angle calculation to obtain the redundant drive joint angle value, and then the redundant drive robot arm can move according to the redundant drive joint angle value to complete the entire path planning.

[0036] Furthermore, the specific process of using the encirclement method to simplify obstacles is as follows:

[0037] The redundant drive robot arm is regarded as a small ball with a radius of r. For the spherical obstacle, under the premise that mutual collision is not allowed, the collision distance between the two is:

[0038]

[0039] In the formula, Represents the coordinates of the center of the spherical obstacle. The collision detection formula is as follows:

[0040]

[0041] In the formula, denote the radius of the redundant driving manipulator and the radius of the spherical obstacle respectively; if t collision If t is 1, it means that the redundant driving robot arm has collided with the i-th obstacle. collision If it is 0, it means that the redundant driving robot arm has no collision with the i-th obstacle;

[0042] For a rectangular obstacle, the judgment formula for detecting whether a collision occurs between the two is as follows:

[0043] The irregular obstacles are simplified by the AABB enclosing method to ensure that each face of the simplified cuboid is parallel to the direction axis. The smallest coordinate position on the cuboid is set as (x min ,y min ,z min ), the maximum coordinate is (x max ,y max ,z max ), the center coordinates of the simplified sphere of the redundant drive robot are (x current ,y current ,z current ), radius is r, and the closest point coordinates are (x, y, z): Calculate the closest point between the center of the redundant drive robot and the AABB cuboid if the following formula is satisfied:

[0044]

[0045] Then the closest point (x, y, z) is the center of the sphere of the redundant drive robot (x current ,y current ,z current ), a collision occurs. If the nearest point is not within the range of the cuboid, the center of the ball (x current ,y current ,z current ) to the cuboid (x min ,x max ),(y min ,y max ),(z min ,z max ) if there is a sphere center x current <x min The coordinates of the nearest point are x min , x current >x max Then x max , and similarly we can get the nearest coordinates y,z;

[0046] When the nearest point is not within the cuboid, use the Euclidean distance formula to calculate the distance from the nearest point to the center of the sphere:

[0047]

[0048] If dist exists obji <r, it is judged that the redundant drive robot arm collides with the obstacle.

[0049] Furthermore, the design of the reinforcement learning reward function follows the following principles:

[0050] (1) Penalty reward function: In order to safely plan a collision-free path, a penalty parameter R needs to be added when a collision occurs. collision =-λ collision , R collision where λ represents the collision penalty reward, collision Indicates that the penalty parameter is a constant. If a collision occurs, a reward is given to the redundant driving robot at this time. The penalty term is used to help the redundant driving robot learn how to avoid collisions.

[0051] (2) Guided reward function: Based on the distance between the redundant driving robot and the target position, the path planning task is decomposed into multiple stages from far away from the target to close to the target. Each stage uses a unified or independent learning goal and reward mechanism as needed. The specific formula for using an independent learning goal and reward mechanism in each stage is as follows:

[0052]

[0053] Wherein, d represents the actual Euclidean distance between the redundant driving manipulator position and the target position, d1 represents the first Euclidean distance between the preset manipulator position and the target position, d2 represents the second Euclidean distance between the preset manipulator position and the target position, λ1 represents the reward parameter when the distance d is less than d1, λ2 represents the reward parameter when the distance d is greater than or equal to d1 and less than or equal to d2, λ3 represents the reward parameter when the distance d is greater than d2, and R1 represents the stage reward function;

[0054] (3) Basic reward function: This function provides positive rewards for behaviors that approach the target state. It guides the redundant driving robot arm to obtain positive learning rewards even if it has not yet completed the entire task during the learning process. The formula is as follows:

[0055]

[0056] Where, d current represents the Euclidean distance between the redundant driving robot and the target position at the current moment, (x current ,y current ,z current ) represents the current node position of the redundant drive manipulator in three-dimensional space, (x goal ,y goal ,z goal ) represents the target position expected to be reached in the path planning task, α1 represents the adjustment parameter is a constant, and R2 represents the basic reward function; the penalty reward function, the guidance reward function and the basic reward function are added together to obtain the complete path planning reward function R sum =R collision +R1+R2.

[0057] Further, offline reinforcement learning dataset The specific collection steps are as follows: The sampling algorithms of the offline data set include: Twin Delayed Deep Deterministic Policy Gradient Algorithm TD3, Deep Deterministic Policy Gradient Algorithm DDPG and expert data set;

[0058] The TD3 algorithm is suitable for processing problems in continuous action spaces. Its improved policy update mechanism and value estimation method enable it to perform well in complex environments. The DDPG algorithm shows strong robustness and data efficiency when collecting data, and is suitable for application scenarios with restrictions on data quality and quantity. The TD3 algorithm and the DDPG algorithm are applied to the reinforcement learning path planning environment constructed in step 2, and training iterations are performed to collect data pairs (s, a, r, s') generated during the training process, where s represents the current state, a represents the action, r represents the reward, and s' represents the state at the next moment. Expert data sets are used as examples of correct behavior to accelerate the learning process.

[0059] The data (s, a, r, s') collected in the path planning environment are preprocessed to improve their usability, and the current state s is normalized. The formula is as follows:

[0060]

[0061] In the formula, λ represents the normalized state data s, α2 represents the adjustment parameter, Q(s i ,s i ′) represents the state value function, s′ represents the state at the next moment, and N represents Q(s i ,s i ′).

[0062] Furthermore, the process of simplifying the redundant driving robot joints and obstacles into cylinders and spheres by using the enclosing method is as follows: the collision problem between the robot and the obstacle is converted into the position relationship problem between the cylinder and the sphere by the enclosing box method. Before collision detection, it is necessary to determine the position relationship between the sphere and the cylinder, that is, the center P of the spherical obstacle. obj With cylinder The position relationship of the foot of the perpendicular axis D is used to calculate the center of the sphere through the dot product of the vectors. The axis of the cylinder The foot D position relationship is divided into the following steps:

[0063] (1) Calculate vector and vector;

[0064] (2) Calculate the projection length of the vector on the vector:

[0065] (3) Determine the position of the foot of the perpendicular D by the projection length z. If 0≤z≤1, the foot of the perpendicular D is on the line segment. Otherwise, the foot of the perpendicular D is on the line segment On the extension line of

[0066] If the foot of the perpendicular is on the axis of the cylinder, the collision distance calculation is as follows:

[0067]

[0068] If the foot of the perpendicular is on the extended line of the cylinder axis, the collision distance calculation is as follows:

[0069]

[0070] Where, d obj It represents the distance between the redundant drive robot arm joint and the spherical obstacle. Min represents the smaller number in the two formulas. Indicates the coordinate position of the center of the spherical obstacle. Represents the end position of the i-th joint; the collision detection formula is as follows:

[0071]

[0072] Where, d obj represents the distance between the redundant drive robot joint and the spherical obstacle, r obj Represents the radius of the spherical obstacle, r i Indicates the minimum cylindrical radius of the redundant drive robot joint; if t collision If t is 1, it means that the i-th joint of the redundant drive robot has collided with an obstacle. collision If it is 0, it means that the i-th joint of the redundant drive robot arm does not collide with the obstacle.

[0073] Furthermore, the process of calculating the path nodes of the MPC state equation of the redundant drive manipulator model predictive control algorithm is as follows:

[0074] First, the redundant drive manipulator model predictive control algorithm MPC is as follows:

[0075]

[0076] In the formula, (x k+1 ,y k+1 ,z k+1 ) represents the position of the redundant driving robot arm at time k+1, (x k ,y k ,z k ) represents the position of the redundant driving manipulator at time k, Δθ represents the angular velocity of each joint, J x ,Jy ,J z Represent the Jacobian linear velocity matrices in the x, y, and z directions respectively, so the state equation of the model predictive control algorithm of the redundant drive manipulator is:

[0077] x(k+1)=Ax(k)+Bu(k)=I[xyz] T +J[Δθ1 Δθ2 Δθ3 Δθ4 Δθ5 Δθ6 Δθ7] T

[0078] In the formula, A represents the state transfer matrix, B represents the control input matrix, I represents the 3×3 unit matrix, [x yz] represents the end position of the robot arm, Δθ i represents the increment of the joint angle, x(k) is the state variable at time k, x(k+1) is the state variable at time k+1, and u(k) is the control input variable at time k;

[0079] The model predictive control algorithm MPC predictive control optimizes control by performing performance in a certain period of time in the future, expressed in discrete state space, namely:

[0080] X k+1 =AX k +BU k

[0081] Let u(k+i|k) represent the input value at the k+1th moment predicted at the kth moment. Within the prediction interval N, the input control matrix is ​​as follows:

[0082]

[0083] Similarly, x(k+i|k) represents the system state predicted at time k as follows:

[0084]

[0085] Since we are solving the inverse solution of the redundant drive robot, the position error is E = x k -x c , where x c Represents the target position; the cost equation is also:

[0086]

[0087] Combined with Formula X k And the formula U k The formula X k+1 Written as:

[0088]

[0089] The cost matrix at time k can be expressed as:

[0090]

[0091] Finally, the formula X k+1 Substitute the cost matrix J k+1 Get the cost matrix you need to solve:

[0092]

[0093] In the formula, let in It is expressed as an augmented matrix, from which we can clearly see that the loss function J k With x k Initial conditions and input U k Finally, the formula for solving the inverse solution of redundant manipulators using the MPC algorithm was successfully obtained.

[0094] In order to prevent the robot joints from colliding with obstacles when the MPC algorithm is performing inverse calculations, the obstacle avoidance detection algorithm needs to be added as a constraint into the MPC equation. The specific steps of the obstacle avoidance constraint term are as follows:

[0095] The DH matrix can be used to obtain the current position of each joint point:

[0096]

[0097] In the formula, represents the DH matrix used to calculate the joint positions of the redundantly driven manipulator, θ j Represents the current robot arm joint angle value, where j represents the joint number, and the angle increment, i.e., the input value of the MPC formula, is u(k+i)=[Δθ1Δθ2Δθ3Δθ4Δθ5Δθ6Δθ7] T , o j =(x j ,y j ,z j ) represents the position of the end of the robot arm in three-dimensional space, by constraining the distance d between each rod of the robot arm and the obstacle obj (o j ) is greater than the collision distance, thus preventing collision:

[0098]

[0099] Where, d obj Represents the distance calculation formula; if the distance obtained is greater than r i +r obj If it is less than r, it is judged that no collision occurs. i +r obj It is determined that a collision occurs.

[0100] Beneficial effects: (1) The present invention discloses a method for performing path planning tasks of redundantly driven robotic arms in three-dimensional space, which can avoid collision problems. Based on the SAW algorithm and combined with the path planning environment, a dense state advantage weighted function is proposed to ensure that the strategy is biased towards high-quality data in the data set to reduce the negative impact of low-quality data and improve the efficiency of path planning.

[0101] (2) Aiming at the three design problems of the state space, action space, and reward function of the reinforcement learning path planning environment, a basic reward function, a collision penalty reward function, and a target reward function are designed from multiple aspects such as position, collision constraints, and step length restrictions to guide the redundant driving robot arm to learn the behavioral strategy to reach the target point, thereby improving the success rate of the redundant driving robot arm in planning paths in three-dimensional space with obstacles.

[0102] (3) By solving the redundant drive joint angles of the redundant drive robot arm and solving the joint movement path nodes of the redundant drive robot arm through the model predictive control algorithm, the optimal joint angles of the redundant drive robot arm are obtained while avoiding obstacles. BRIEF DESCRIPTION OF THE DRAWINGS

[0103] Figure 1 It is a flow chart of a redundant drive manipulator path planning method based on the combination of DSAW algorithm and MPC algorithm of the present invention;

[0104] Figure 2 It is a schematic diagram of simplifying the redundant driving robot arm connecting rod and obstacles into cylinders and spheres using the enclosure method theory in an embodiment of the present invention;

[0105] Figure 3 This is a schematic diagram of a robot arm connecting rod and an obstacle simplified as a cylinder and a sphere perpendicular to the axis in an embodiment of the present invention;

[0106] Figure 4 This is a schematic diagram of a robot arm connecting rod and an obstacle simplified as a cylinder and a sphere perpendicular to the extended line of the axis in an embodiment of the present invention;

[0107] Figure 5 A schematic diagram of a model of a redundant driving robot arm and obstacles built in a CoppeliaSim environment in an embodiment of the present invention;

[0108] Figure 6 Schematic diagram of path planning results in three-dimensional coordinates in an embodiment of the present invention;

[0109] Figure 7 A schematic diagram of a path planning result in an obstacle environment in an embodiment of the present invention;

[0110] Figure 8 This is a schematic diagram of the angles of the path nodes corresponding to the redundant drive manipulator joints calculated by the MPC algorithm in an embodiment of the present invention;

[0111] Fig. 9 It is a simulation schematic diagram of movement prediction in the CoppeliaSim environment according to an embodiment of the present invention. DETAILED DESCRIPTION

[0112] The embodiments of the present invention are further described below in conjunction with the accompanying drawings:

[0113] like Figure 1 As shown, the present invention discloses a redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm, and the specific steps are as follows:

[0114] Step 1: Perform preliminary preparation for the reinforcement learning path planning of the redundant driving robot arm and initialize the global map: Model the environment of the redundant driving robot arm moving scene in the computer and establish a three-dimensional coordinate system. Use the encirclement method to simplify all obstacles in the redundant driving robot arm moving scene environment into cuboids and spheres to facilitate subsequent obstacle collision detection and prevent collision problems; let the number of obstacles be k, and the obstacle set be {obj i}, obstacles simplified to spheres include the radius and center coordinate information of the sphere, and obstacles simplified to rectangles include the minimum and maximum coordinate information on each direction axis, that is,

[0115] The specific process of using the encirclement method to simplify obstacles is as follows: the redundant drive robot arm is regarded as a small ball with a radius of r. For spherical obstacles, under the premise that mutual collision is not allowed, the collision distance between the two is:

[0116]

[0117] In the formula, Represents the coordinates of the center of the spherical obstacle. The collision detection formula is as follows:

[0118]

[0119] In the formula, r, denote the radius of the redundant driving manipulator and the radius of the spherical obstacle respectively; if t collision If t is 1, it means that the redundant driving robot arm has collided with the i-th obstacle. collision If it is 0, it means that the redundant driving robot arm has no collision with the i-th obstacle;

[0120] For a rectangular obstacle, the judgment formula for detecting whether a collision occurs between the two is as follows:

[0121] The irregular obstacles are simplified by the AABB enclosing method to ensure that each face of the simplified cuboid is parallel to the direction axis. The smallest coordinate position on the cuboid is set as (x min ,y min ,z min ), the maximum coordinate is (x max ,y max ,z max ), the center coordinates of the simplified sphere of the redundant drive robot are (x current ,y current ,z current ), radius is r, and the closest point coordinates are (x, y, z): Calculate the closest point between the center of the redundant drive robot and the AABB cuboid if the following formula is satisfied:

[0122]

[0123] Then the closest point (x, y, z) is the center of the sphere of the redundant drive robot (x current ,y current ,z current ), a collision occurs. If the nearest point is not within the range of the cuboid, the center of the ball (x current ,y current ,z current ) to the cuboid (x min ,x max ),(y min ,y max ),(z min ,z max ) if there is a sphere center x current <x min The coordinates of the nearest point are x min , x current >x max Then x max , and similarly we can get the nearest coordinates y,z;

[0124] When the nearest point is not within the cuboid, use the Euclidean distance formula to calculate the distance from the nearest point to the center of the sphere:

[0125]

[0126] If exists It is determined that the redundant drive robot arm collides with the obstacle.

[0127] Step 2: Construct a reinforcement learning path planning environment for the redundant driving robot in the simulation simulator CoppeliaSim. The path planning environment includes the reinforcement learning state space, reinforcement learning action space, and reinforcement learning reward function to guide the redundant driving robot to learn the behavior strategy to reach the target point, such as Figure 5 , Figure 6 , Figure 7 and Fig. 9 As shown;

[0128] Step 2.1 Design the reinforcement learning state space; according to the actual needs of path planning and global map information, design the state space as state = {start, goal, current, dist obstacle ,dist goal ,step}, where start=(x init ,y init ,z init ) represents the starting position of the redundant driving robot in the path planning environment, goal = (x end ,y end ,z end ) indicates the end point, current=(x current ,y current ,z current ) indicates the current node position, dist obstacle Indicates the distance between the current node and the obstacle, dist goal Indicates the distance between the current node and the end point, and step indicates the number of running steps;

[0129] Step 2.2: Design the reinforcement learning action space and define all possible actions that the redundant driving robot can perform. The most critical actions involve the direction and step length in the process of finding the point. Therefore, the action space is designed as action = {x, y, z, length}, where (x, y, z) represents the direction vector, the range is designed to be (-1, 1), and length represents the step length of a single movement of the redundant driving robot.

[0130] Step 2.3: Design a reward function for reinforcement learning; the reward function includes rewards for correct behavior of the redundantly driven robot arm, penalties for undesirable behavior, and a mechanism to guide the redundantly driven robot arm to avoid obstacles and achieve path planning in three-dimensional space. Learning and optimization;

[0131] The design of the reinforcement learning reward function follows the following principles: (1) Penalty reward function: In order to safely plan a collision-free path, a penalty parameter R needs to be added when a collision occurs. collision =-λ collision , R collisionwhere λ represents the collision penalty reward, collision It means that the penalty parameter is a constant. If a collision occurs, a reward is given to the redundant driving robot for the action it is performing at this time. The penalty term is used to help the redundant driving robot learn how to avoid collisions.

[0132] (2) Guided reward function: Based on the distance between the redundant driving robot and the target position, the path planning task is decomposed into multiple stages from far away from the target to close to the target. Each stage uses a unified or independent learning goal and reward mechanism as needed. The specific formula for using an independent learning goal and reward mechanism in each stage is as follows:

[0133]

[0134] Where d represents the Euclidean distance between the redundant driving manipulator position and the target position, d1 represents the first Euclidean distance between the preset manipulator position and the target position, d2 represents the second Euclidean distance between the preset manipulator position and the target position, and λ i represents the reward parameter when the distance d is less than d1, λ2 represents the reward parameter when the distance d is greater than or equal to d1 and less than or equal to d2, λ3 represents the reward parameter when the distance d is greater than d2, and R1 represents the stage reward function;

[0135] (3) Basic reward function: This function provides positive rewards for behaviors that approach the target state. It guides the redundant driving robot arm to obtain positive learning rewards even if it has not yet completed the entire task during the learning process. The formula is as follows:

[0136]

[0137] Where, d current represents the Euclidean distance between the redundant driving robot and the target position at the current moment, (x current ,y current ,z current ) represents the position of the redundant drive manipulator in three-dimensional space, (x goal ,y goal ,z goal ) represents the target position expected to be reached in the path planning task, α1 represents the adjustment parameter is a constant, and R2 represents the basic reward function;

[0138] Adding the penalty reward function, the guided reward function, and the basic reward function, we get the complete path planning reward function R sum =R collision +R1+R2.

[0139] Step 3: Build an offline reinforcement learning dataset in a reinforcement learning path planning environment The offline reinforcement learning dataset includes historical interaction records generated during the training process, data generated by the simulation simulator, and an artificial path planning expert database. Perform necessary normalization preprocessing to improve its usability;

[0140] Step 4: Use normalized preprocessed dataset The offline reinforcement learning algorithm DSAW is iterated to output the path nodes n0, n1, ..., n in the process of redundant driving robot end movement. n ;

[0141] The specific update process of the offline reinforcement learning algorithm DSAW algorithm is as follows: Based on the reinforcement learning path planning environment, a dense state advantage weighting function w(s, s′) is proposed to assign a larger weight to the ideal strategy and a smaller weight to the poor strategy;

[0142] w(s,s')=[A(s,s')≥0]=[(Q θ (s,s')-V ψ (s))≥0]= 1 [Q θ (s,s')≥V ψ (s)]

[0143] Where A(s,s') represents the state advantage function, Q θ (s,s') and V ψ (s) represent the current state, the next state value function and the evaluation function of the current state respectively. 1 [Q θ (s,s')≥V ψ (s)] means that when Q(s,s′) is greater than V(s), the value of w(s,s′) is 1, and when Q(s,s′) is less than V(s), the value of w(s,s′) is 0;

[0144] According to the dense state advantage weighting function w(s,s′), the updating process of the offline reinforcement learning algorithm DSAW firstly updates the state evaluation function V δ (s) To update:

[0145]

[0146] In the formula, represents the indicator function, k represents the adjustment parameter, V δ (s) represents the state evaluation function, Represents the loss function when the state evaluation function is updated; weight distribution function To emphasize Q θ' (s,s') is greater than V δ(s) value, the weight of the high-quality state is increased when the DSAW algorithm updates the state evaluation function, and the weight of the low-quality state is reduced, while Q θ' (s, s') is generated by the target network θ' of the network parameter θ of the current state and the next state value function, and the state evaluation network V δ (s) is generated by the network parameter δ. Then the state value function Q θ (s,s') is updated:

[0147]

[0148] In the formula, E s,s′～D ? (What does it mean?) The loss expectation function in the state s and the next state s′, Represents the loss function when the current state and the next state value function are updated;

[0149] In progress θ (s,s') and V δ (s) when updating, only use To ensure that the data of state s and the next state s' are in the offline dataset when updating In this way, the interference of extrapolation error is avoided;

[0150] Generate the forward network f from the forward network parameters φ φ (s,a) and predict the next state through state s and action a, and train by minimizing:

[0151]

[0152] In the formula, Represents the loss function when the forward network function is updated;

[0153] Construct an inverse dynamics model I generated by the network parameter ω ω (s,s') to infer action a, inverse dynamics model I ω (s,s') is trained by the dense state advantage weighting function w(s,s'):

[0154]

[0155] In the formula, I ω represents the inverse dynamics network, s and s′ represent the current state and the next state, Represents the loss function when the inverse dynamics model function is updated;

[0156] Construct an additional prediction network s′=τ(s) to evaluate the next state s′ and perform training iterations in the following way to keep close to the dataset Distribution in:

[0157]

[0158] In the formula, Represents the loss function when the prediction network function is updated.

[0159] Step 5: Use the enclosure method to set the redundant drive robot arm joints and obstacles, simplify the robot arm connecting rods into cylinders, simplify the obstacles into spheres, and then calculate the distance between the robot arm joints and obstacles. Figure 3 , Figure 4 and Figure 5 As shown, obstacle avoidance detection and collision prevention test of redundantly driven robotic arm joints are performed;

[0160] The process of simplifying the redundant driving robot joints and obstacles into cylinders and spheres by using the enclosing method is as follows: the collision problem between the robot and the obstacle is converted into the position relationship problem between the cylinder and the sphere by the enclosing box method. Before collision detection, it is necessary to determine the position relationship between the sphere and the cylinder, that is, the center of the spherical obstacle P obj With cylinder The position relationship of the foot of the perpendicular axis D is used to calculate the center of the sphere through the dot product of the vectors. The axis of the cylinder The foot D position relationship is divided into the following steps:

[0161] (1) Calculate vector and vector;

[0162] (2) Calculate the projection length of the vector on the vector:

[0163] (3) Determine the position of the foot of the perpendicular D by the projection length z. If 0≤z≤1, the foot of the perpendicular D is on the line segment. Otherwise, the foot of the perpendicular D is on the line segment On the extension line of

[0164] If the foot of the perpendicular is on the axis of the cylinder, the collision distance calculation is as follows:

[0165]

[0166] If the foot of the perpendicular is on the extended line of the cylinder axis, the collision distance calculation is as follows:

[0167]

[0168] Where, d obj It represents the distance between the redundant drive robot arm joint and the spherical obstacle. Min represents the smaller number in the two formulas. Indicates the coordinate position of the center of the spherical obstacle. Represents the end position of the i-th joint; the collision detection formula is as follows:

[0169]

[0170] Where, d obj represents the distance between the redundant drive robot joint and the spherical obstacle, r obj Represents the radius of the spherical obstacle, r i Indicates the minimum cylindrical radius of the redundant drive robot joint; if t collision If t is 1, it means that the i-th joint of the redundant drive robot has collided with an obstacle. collision If it is 0, it means that the i-th joint of the redundant drive robot arm does not collide with the obstacle.

[0171] Offline reinforcement learning dataset The specific collection steps are as follows: The sampling algorithms of the offline data set include: Twin Delayed Deep Deterministic Policy Gradient Algorithm TD3, Deep Deterministic Policy Gradient Algorithm DDPG and expert data set;

[0172] The TD3 algorithm is suitable for dealing with problems in continuous action spaces. Its improved policy update mechanism and value estimation method enable it to perform well in complex environments. The DDPG algorithm shows strong robustness and data efficiency when collecting data, and is particularly suitable for application scenarios with restrictions on data quality and quantity. The TD3 algorithm and the DDPG algorithm are applied to the reinforcement learning path planning environment constructed in step 2, and training iterations are performed to collect data pairs (s, a, r, s') generated during the training process, where s represents the current state, a represents the action, r represents the reward, and s' represents the state at the next moment. Expert data sets are used as examples of correct behavior to accelerate the learning process.

[0173] The data (s, a, r, s') collected in the path planning environment are preprocessed to improve their usability, and the current state s is normalized. The formula is as follows:

[0174]

[0175] In the formula, λ represents the normalized state data s, α2 represents the adjustment parameter, Q(s i ,s i ′) represents the state value function, s′ represents the state at the next moment, and N represents Q(s i ,si ′).

[0176] Step 6: Establish the MPC state equation of the redundant drive robot model predictive control algorithm, and calculate the path nodes n0, n1, ..., n during the movement of the redundant drive robot end. n Perform inverse angle calculation to obtain the redundant drive joint angle value, and then the redundant drive robot arm can move according to the redundant drive joint angle value to complete the entire path planning.

[0177] The process of calculating the path nodes of the MPC state equation of the redundant drive manipulator model predictive control algorithm is as follows:

[0178] First, the redundant drive manipulator model predictive control algorithm MPC is as follows:

[0179]

[0180] In the formula, (x k+1 ,y k+1 ,z k+1 ) represents the position of the redundant driving robot arm at time k, (x k ,y k ,z k ) represents the position of the redundant driving manipulator at time k+1, Δθ represents the angular velocity of each joint, J x ,J y ,J z Represent the Jacobian linear velocity matrices in the x, y, and z directions respectively, so the state equation of the model predictive control algorithm of the redundant drive manipulator is:

[0181] x(k+1)=Ax(k)+Bu(k)=I[xyz] T +J[Δθ1 Δθ2 Δθ3 Δθ4 Δθ5 Δθ6 Δθ7] T

[0182] In the formula, A represents the state transfer matrix, B represents the control input matrix, I represents the 3×3 unit matrix, [x yz] represents the end position of the robot arm, Δθ i represents the increment of the joint angle, x(k) is the state variable at time k, x(k+1) is the state variable at time k+1, and u(k) is the input variable at time k;

[0183] The model predictive control algorithm MPC predictive control optimizes control by performing performance in a certain period of time in the future, expressed in discrete state space, namely:

[0184] X k+1 =AX k +BU k

[0185] Let u(k+i|k) represent the input value at the k+1th moment predicted at the kth moment. Within the prediction interval N, the input control matrix is ​​as follows:

[0186]

[0187] Similarly, x(k+i|k) represents the system state predicted at time k as follows:

[0188]

[0189] Since we are solving the inverse solution of the redundant drive robot, the position error is E = x k -x c , where x c Represents the target position; the cost equation is also:

[0190]

[0191] Combined with Formula X k And the formula U k The formula X k+1 Written as:

[0192]

[0193] The cost matrix at time k can be expressed as:

[0194]

[0195] Finally, the formula X k+1 Substitute the cost matrix J k+1 Get the cost matrix you need to solve:

[0196]

[0197] In the formula, let in It is expressed as an augmented matrix. From this we can clearly see that the loss function J k With x k Initial conditions and input U k Finally, the formula for solving the inverse solution of redundant manipulators using the MPC algorithm was successfully obtained, as shown in Figure 8 shown.

[0198] In order to prevent the robot joints from colliding with obstacles when the MPC algorithm is performing inverse calculations, the obstacle avoidance detection algorithm needs to be added as a constraint into the MPC equation. The specific steps of the obstacle avoidance constraint term are as follows:

[0199] The DH matrix can be used to obtain the current position of each joint point:

[0200]

[0201] In the formula, represents the DH matrix used to calculate the joint positions of the redundantly driven manipulator, θ j Represents the current robot arm joint angle value, where j represents the joint number, and the angle increment, i.e., the input value of the MPC formula, is u(k+i)=[Δθ1Δθ2Δθ3Δθ4Δθ5Δθ6Δθ7] T , o j =(x j ,y j ,z j ) represents the position of the end of the robot arm in three-dimensional space, by constraining the distance d between the end of each mechanical rod and the obstacle obj (o j ) is greater than the collision distance, thus preventing collision:

[0202]

[0203] Where, d obj Represents the distance calculation formula; if the distance obtained is greater than r i +r obj If it is less than r, it is judged that no collision occurs. i +r obj It is determined that a collision occurs.

[0204] The present invention discloses a redundant driven manipulator path planning method based on a dense state advantage weighting (DSAW) offline reinforcement learning algorithm. The DSAW algorithm applies the dense state advantage weighting function to the state advantage weighting (SAW) offline reinforcement learning algorithm through a weighted assignment constraint method, thereby increasing the probability that the strategy is biased toward high-quality data and reducing the negative impact of low-quality data; for the collision detection problem in three-dimensional space, the collision relationship between the path node and the obstacle is simplified to the collision relationship between the sphere and the cuboid through the encirclement method to avoid the collision problem; for the design problem of the reinforcement learning path planning environment, a basic reward function, a collision penalty reward function, and a target reward function are designed from multiple aspects such as position, collision constraints, and step size restrictions to guide the redundant driven manipulator to learn the behavior strategy of reaching the target point. Then, the Model Predicative Control (MPC) algorithm is used to perform inverse calculations on the nodes on the path to obtain the corresponding joint angles. Considering the existence of obstacles and the property that the redundant drive manipulator has infinite solutions, distance error constraints, angle increment constraints and obstacle avoidance constraints are added to the model predictive control algorithm to ensure that the optimal joint angle is obtained while the manipulator does not collide with obstacles. Compared with the classic offline reinforcement algorithm, the method of the present invention has significant improvements in the number of exploration steps, number of iterations and path length. At the same time, the use of the MPC algorithm to solve the joint angle improves the solution efficiency and ensures the optimal solution angle.

Claims

1. A redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm, characterized in that: The steps include: Step 1: Perform preliminary preparation for the reinforcement learning path planning of the redundant driving robot arm and initialize the global map: Model the environment of the redundant driving robot arm moving scene in the computer and establish a three-dimensional coordinate system. Use the encirclement method to simplify all obstacles in the redundant driving robot arm moving scene environment into cuboids and spheres to facilitate subsequent obstacle collision detection and prevent collision problems; Assume the number of obstacles is k, and the obstacle set is {obj i }, obstacles simplified to spheres include the radius and center coordinate information of the sphere, and obstacles simplified to rectangles include the minimum and maximum coordinate information on each direction axis, that is, Step 2: Construct a reinforcement learning path planning environment for the redundant driving robot in the simulation simulator CoppeliaSim. The path planning environment includes the reinforcement learning state space, reinforcement learning action space, and reinforcement learning reward function to guide the redundant driving robot to learn the behavior strategy to reach the target point. Step 2.1 Design reinforcement learning state space; According to the actual needs of path planning and global map information, the state space is designed as state = {start, goal, current, dist obstacle ,dist goal ,step}, where start=(x init ,y init ,z init ) represents the starting position of the redundant driving robot in the path planning environment, goal = (x end ,y end ,z end ) indicates the end point, current=(x current ,y current ,z current ), indicating the current node position of the redundant drive robot, dist obstacle Indicates the distance between the current node and the obstacle, dist goal Indicates the distance between the current node and the end point, and step indicates the number of running steps; Step 2.2: Design the reinforcement learning action space and define all possible actions that the redundant driving robot can perform. The most critical actions involve the direction and step length in the process of finding the point. Therefore, the action space is designed as action = {x, y, z, length}, where (x, y, z) represents the direction vector, the range is designed to be (-1, 1), and length represents the step length of a single movement of the redundant driving robot. Step 2.3: Design a reward function for reinforcement learning; the reward function includes rewards for correct behavior of the redundantly driven robot arm, penalties for undesirable behavior, and a mechanism to guide the redundantly driven robot arm to avoid obstacles and achieve path planning in three-dimensional space. Learning and optimization; Step 3: Build an offline reinforcement learning dataset in a reinforcement learning path planning environment The offline reinforcement learning dataset includes historical interaction records generated during the training process, data generated by the simulation simulator, and a manually designed path planning expert database. For the dataset Perform necessary normalization preprocessing to improve its usability; Step 4: Use normalized preprocessed dataset The offline reinforcement learning algorithm DSAW is iterated to output the path nodes n0, n1, ..., n in the process of redundant driving robot end movement. n ; The specific update process of the offline reinforcement learning algorithm DSAW algorithm is: According to the reinforcement learning path planning environment, a dense state advantage weighting function w(s,s′) is proposed to assign a larger weight to the ideal strategy and a smaller weight to the poor strategy: w(s,s')=[A(s,s')≥0]=[(Q θ (s,s')-V ψ (s))≥0]= 1 [Q θ (s,s')≥V ψ (s)] Where A(s,s') represents the state advantage function, Q θ (s,s') and V ψ (s) represent the current state, the next state value function and the evaluation function of the current state respectively; 1 [Q θ (s,s')≥V ψ (s)] means that when Q(s,s′) is greater than V(s), the value of w(s,s′) is 1, and when Q(s,s′) is less than V(s), the value of w(s,s′) is 0; According to the dense state advantage weighting function w(s,s′), the updating process of the offline reinforcement learning algorithm DSAW firstly updates the state evaluation function V δ (s) To update: In the formula, represents the indicator function, k represents the adjustment parameter, V δ (s) represents the state evaluation function, Represents the loss function when the state evaluation function is updated; weight distribution function To emphasize Q θ' (s,s') is greater than V δ (s) value, the weight of the high-quality state is increased when the DSAW algorithm updates the state evaluation function, and the weight of the low-quality state is reduced, while Q θ' (s, s') is generated by the target network θ' of the network parameter θ of the current state and the next state value function, and the state evaluation network V δ (s) is generated by the network parameter δ; then the state value function Q θ (s,s') is updated: In the formula, Represents the loss function when the current state and the next state value function are updated; In progress θ (s,s') and V δ (s) when updating, only use To ensure that the data of state s and the next state s' are in the offline dataset when updating In this way, the interference of extrapolation error is avoided; Generate the forward network f from the forward network parameters φ φ (s,a) and predict the next state through state s and action a, and train by minimizing: In the formula, Represents the loss function when the forward network function is updated; Construct an inverse dynamics model I generated by the network parameter ω ω (s,s') to infer action a, inverse dynamics model I ω (s,s') is trained by the dense state advantage weighting function w(s,s'): In the formula, I ω represents the inverse dynamics network, s and s′ represent the current state and the next state, Represents the loss function when the inverse dynamics model function is updated; Construct an additional prediction network s′=τ ψ (s), evaluate the next state s′, and perform training iterations in the following way to keep close to the dataset Distribution in: In the formula, Represents the loss function when the prediction network function is updated; Step 5: Use the encirclement method to set the redundant drive robot arm joints and obstacles, simplify the robot arm connecting rods into cylinders, simplify the obstacles into spheres, calculate the distance between the robot arm joints and obstacles, and perform obstacle avoidance detection and collision prevention tests on the redundant drive robot arm joints; Step 6: Establish the MPC state equation of the redundant drive robot model predictive control algorithm, and calculate the path nodes n0, n1, ..., n during the movement of the redundant drive robot end. n Perform inverse angle calculation to obtain the redundant drive joint angle value, and then the redundant drive robot arm can move according to the redundant drive joint angle value to complete the entire path planning.

2. The redundant drive robot arm path planning method based on the DSAW offline reinforcement learning algorithm according to claim 1 is characterized in that: The specific process of using the encirclement method to simplify obstacles is as follows: The redundant drive robot arm is regarded as a small ball with a radius of r. For the spherical obstacle, under the premise that mutual collision is not allowed, the collision distance between the two is: In the formula, Represents the coordinates of the center of the spherical obstacle. The collision detection formula is as follows: In the formula, r, denote the radius of the redundant driving manipulator and the radius of the spherical obstacle respectively; if t collision If t is 1, it means that the redundant driving robot arm has collided with the i-th obstacle. collision If it is 0, it means that the redundant driving robot arm has no collision with the i-th obstacle; For a rectangular obstacle, the judgment formula for detecting whether a collision occurs between the two is as follows: The irregular obstacles are simplified by the AABB enclosing method to ensure that each face of the simplified cuboid is parallel to the direction axis. The smallest coordinate position on the cuboid is set as (x min ,y min ,z min ), the maximum coordinate is (x max ,y max ,z max ), the center coordinates of the simplified sphere of the redundant drive robot are (x current ,y current ,z current ), radius is r, and the closest point coordinates are (x, y, z): Calculate the closest point between the center of the redundant drive robot and the AABB cuboid if the following formula is satisfied: Then the closest point (x, y, z) is the center of the sphere of the redundant drive robot (x current ,y current ,z current ), a collision occurs. If the nearest point is not within the range of the cuboid, the center of the ball (x current ,y current ,z current ) to the cuboid (x min ,x max ),(y min ,y max ),(z min ,z max ) if there is a sphere center x current <x min The coordinates of the nearest point are x min , x current >x max Then x max , and similarly we can get the nearest coordinates y,z; When the nearest point is not within the cuboid, use the Euclidean distance formula to calculate the distance from the nearest point to the center of the sphere: If exists It is determined that the redundant drive robot arm collides with the obstacle.

3. The redundant drive robot arm path planning method based on the DSAW offline reinforcement learning algorithm according to claim 1 is characterized in that: The design of the reinforcement learning reward function follows the following principles: (1) Penalty reward function: In order to safely plan a collision-free path, a penalty parameter R needs to be added when a collision occurs. collision =-λ collision , R collision where λ represents the collision penalty reward, collision Indicates that the penalty parameter is a constant. If a collision occurs, a reward is given to the redundant driving robot at this time. The penalty term is used to help the redundant driving robot learn how to avoid collisions. (2) Guided reward function: Based on the distance between the redundant driving robot and the target position, the path planning task is decomposed into multiple stages from far away from the target to close to the target. Each stage uses a unified or independent learning goal and reward mechanism as needed. The specific formula for using an independent learning goal and reward mechanism in each stage is as follows: Wherein, d represents the actual Euclidean distance between the redundant driving manipulator position and the target position, d1 represents the first Euclidean distance between the preset manipulator position and the target position, d2 represents the second Euclidean distance between the preset manipulator position and the target position, λ1 represents the reward parameter when the distance d is less than d1, λ2 represents the reward parameter when the distance d is greater than or equal to d1 and less than or equal to d2, λ3 represents the reward parameter when the distance d is greater than d2, and R1 represents the stage reward function; (3) Basic reward function: This function provides positive rewards for behaviors that approach the target state. It guides the redundant driving robot arm to obtain positive learning rewards even if it has not yet completed the entire task during the learning process. The formula is as follows: Where, d current represents the Euclidean distance between the redundant driving robot and the target position at the current moment, (x current ,y current ,z current ) represents the current node position of the redundant drive manipulator in three-dimensional space, (x goal ,y goal ,z goal ) represents the target position expected to be reached in the path planning task, α1 represents the adjustment parameter is a constant, and R2 represents the basic reward function; the penalty reward function, the guidance reward function and the basic reward function are added together to obtain the complete path planning reward function R sum =R collision +R1+R2.

4. The redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm according to claim 1 is characterized in that: Offline reinforcement learning dataset The specific collection steps are: The sampling algorithms for offline data sets include: Twin Delayed Deep Deterministic Policy Gradient Algorithm TD3, Deep Deterministic Policy Gradient Algorithm DDPG, and Expert Datasets; The TD3 algorithm is suitable for processing problems in continuous action spaces. Its improved policy update mechanism and value estimation method enable it to perform well in complex environments. The DDPG algorithm shows strong robustness and data efficiency when collecting data, and is suitable for application scenarios with restrictions on data quality and quantity. The TD3 algorithm and the DDPG algorithm are applied to the reinforcement learning path planning environment constructed in step 2, and training iterations are performed to collect data pairs (s, a, r, s') generated during the training process, where s represents the current state, a represents the action, r represents the reward, and s' represents the state at the next moment. Expert data sets are used as examples of correct behavior to accelerate the learning process. The data (s, a, r, s') collected in the path planning environment are preprocessed to improve their usability, and the current state s is normalized. The formula is as follows: In the formula, λ represents the normalized state data s, α2 represents the adjustment parameter, Q(s i ,s i ′) represents the state value function, s′ represents the state at the next moment, and N represents Q(s i ,s i ′).

5. The redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm according to claim 1 is characterized in that: The process of simplifying the redundant drive robot joints and obstacles into cylinders and spheres using the encirclement method is as follows: Through the bounding box method, the collision problem between the robot arm and the obstacle is converted into the position relationship problem between the cylinder and the sphere. Before collision detection, it is necessary to determine the position relationship between the sphere and the cylinder, that is, the center P of the spherical obstacle. obj With cylinder The position relationship of the foot of the perpendicular axis D is used to calculate the center of the sphere through the dot product of the vectors. The axis of the cylinder The foot D position relationship is divided into the following steps: (1) Calculate vector and vector; (2) Calculate the projection length of the vector on the vector: (3) Determine the position of the foot of the perpendicular D by the projection length z. If 0≤z≤1, the foot of the perpendicular D is on the line segment. On, otherwise the foot of the perpendicular D is on the line segment On the extension line of If the foot of the perpendicular is on the axis of the cylinder, the collision distance calculation is as follows: If the foot of the perpendicular is on the extended line of the cylinder axis, the collision distance calculation is as follows: Where, d obj It represents the distance between the redundant drive robot arm joint and the spherical obstacle. Min represents the smaller number in the two formulas. Indicates the coordinate position of the center of the spherical obstacle. Represents the end position of the i-th joint; the collision detection formula is as follows: Where, d obj represents the distance between the redundant drive robot joint and the spherical obstacle, r obj Represents the radius of the spherical obstacle, r i Indicates the minimum cylindrical radius of the redundant drive robot joint; if t collision If t is 1, it means that the i-th joint of the redundant drive robot has collided with an obstacle. collision If it is 0, it means that the i-th joint of the redundant drive robot arm does not collide with the obstacle.

6. The redundant drive robot arm path planning method based on DSAW offline reinforcement learning algorithm according to claim 5 is characterized in that: The process of calculating the path nodes of the MPC state equation of the redundant drive manipulator model predictive control algorithm is as follows: First, the redundant drive manipulator model predictive control algorithm MPC is as follows: In the formula, (x k+1 ,y k+1 ,z k+1 ) represents the position of the redundant driving robot arm at time k+1, (x k ,y k ,z k ) represents the position of the redundant driving manipulator at time k, Δθ represents the angular velocity of each joint, J x ,J y ,J z Represent the Jacobian linear velocity matrices in the x, y, and z directions respectively, so the state equation of the model predictive control algorithm of the redundant drive manipulator is: x(k+1)=Ax(k)+Bu(k)=I[xyz] T +J[Δθ1 Δθ2 Δθ3 Δθ4 Δθ5 Δθ6 Δθ7] T In the formula, A represents the state transfer matrix, B represents the control input matrix, I represents the 3×3 unit matrix, [xyz] represents the end position of the robot arm, Δθ i represents the increment of the joint angle, x(k) is the state variable at time k, x(k+1) is the state variable at time k+1, and u(k) is the control input variable at time k; The model predictive control algorithm MPC predictive control optimizes control by performing performance in a certain period of time in the future, expressed in discrete state space, namely: X k+1 =AX k +BU k Let u(k+i|k) represent the input value at the k+1th moment predicted at the kth moment. Within the prediction interval N, the input control matrix is ​​as follows: Similarly, x(k+i|k) represents the system state predicted at time k as follows: Since we are solving the inverse solution of the redundant drive robot, the position error is E = x k -x c , where x c Indicates the target location; cost The equation is also: Combined with Formula X k And the formula U k The formula X k+1 Written as: The cost matrix at time k can be expressed as: Finally, the formula X k+1 Substitute the cost matrix J k+1 Get the cost matrix you need to solve: In the formula, let in It is expressed as an augmented matrix, from which we can clearly see that the loss function J k With x k Initial conditions and input U k Finally, the formula for solving the inverse solution of redundant manipulators using the MPC algorithm was successfully obtained. In order to prevent the robot joints from colliding with obstacles when the MPC algorithm is performing inverse calculations, the obstacle avoidance detection algorithm needs to be added as a constraint into the MPC equation. The specific steps of the obstacle avoidance constraint term are as follows: The DH matrix can be used to obtain the current position of each joint point: In the formula, represents the DH matrix used to calculate the joint positions of the redundantly driven manipulator, θ j Represents the current robot arm joint angle value, where j represents the joint number, and the angle increment, i.e., the input value of the MPC formula, is u(k+i)=[Δθ1Δθ2Δθ3Δθ4Δθ5Δθ6Δθ7] T , o j =(x j ,y j ,z j ) represents the position of the end of the robot arm in three-dimensional space, by constraining the distance d between each rod of the robot arm and the obstacle obj (o j ) is greater than the collision distance, thus preventing collision: Where, d obj Represents the distance calculation formula; if the distance obtained is greater than r i +r obj If it is less than r, it is judged that no collision occurs. i +r obj It is determined that a collision occurs.

Images