A space robot safe motion planning method based on constraint reinforcement learning

Abstract

The application provides a space robot safe motion planning method based on constraint reinforcement learning, comprising the following steps: constructing a space robot constraint reinforcement learning framework; constructing a space robot safe motion planner according to the space robot constraint reinforcement learning framework; constructing a space robot safe motion constraint equation according to the space robot constraint reinforcement learning framework; constructing a space robot safe motion training strategy according to the space robot safe motion planner and the space robot safe motion constraint equation, and obtaining a trained space robot safe motion planning strategy network. According to the technical scheme provided by the embodiment of the application, the space robot can realize safe and rapid obstacle avoidance in an unstructured environment, without interrupting the current task as much as possible and under the premise of meeting various constraint conditions.

Description

TECHNICAL FIELD

[0001] The application belongs to the field of space manipulator motion planning, and relates to a space manipulator safe motion planning method based on constraint reinforcement learning. BACKGROUND

[0002] The space manipulator can perform various on-orbit tasks such as carrying and repairing on the surface of a spacecraft cabin, thereby replacing the extravehicular operation of astronauts and reducing the safety risk of astronauts. However, various loads are usually installed on the surface of the spacecraft cabin, and the shapes and sizes of the loads are various, which leads to a very complex working environment with strong unstructured characteristics, seriously affecting the motion safety and task execution success rate of the space manipulator, and therefore the obstacle avoidance problem in the unstructured environment needs to be considered in motion planning.

[0003] Conventional manipulator motion planning methods such as RRT and A* can realize obstacle avoidance of the manipulator during motion. However, for the space manipulator, if the current operation task is interrupted only for the purpose of avoiding obstacles, the valuable task execution window period will be wasted. The GPM method realizes obstacle avoidance without interrupting the current task by using the null space characteristics of the redundant degrees of freedom of the space manipulator; however, this method depends on the accurate modeling of the space manipulator and environmental objects such as obstacles, and in actual engineering applications, the constraints such as working space, joint motion limit and collision danger distance need to be considered, which leads to the fact that the space manipulator needs to perform iterative optimization operation of a complex nonlinear problem in each control period to obtain a real-time collision-free path, and the calculation efficiency is low, and it is difficult to realize real-time obstacle avoidance.

[0004] In recent years, the success of reinforcement learning has shown its prospects in solving real-time dynamic problems. However, the current motion planning method based on reinforcement learning does not consider obstacle avoidance in complex environments, and the motion constraints of the space manipulator are also not considered. Therefore, how to use the excellent characteristics of reinforcement learning to plan the motion of the space manipulator under multiple constraints and realize safe and fast obstacle avoidance without interrupting the current task as much as possible is still a key technical problem to be solved. SUMMARY

[0005] Therefore, the application provides a space manipulator safe motion planning method based on constraint reinforcement learning, which constructs a constraint reinforcement learning framework to ensure the safety of the space manipulator during reinforcement learning training and after the training is completed and the motion task is executed.

[0006] The application provides a space manipulator safe motion planning method based on constraint reinforcement learning, which includes:

[0007] Step S1 constructs a space manipulator constraint reinforcement learning framework;

[0008] Step S2 constructs a space manipulator safety motion planner according to the space manipulator constraint reinforcement learning framework;

[0009] Step S3 constructs a space manipulator safety motion constraint equation according to the space manipulator constraint reinforcement learning framework;

[0010] Step S4 constructs a space manipulator safety motion training strategy according to the space manipulator safety motion planner and the space manipulator safety motion constraint equation, and obtains a trained space manipulator safety motion planning strategy network.

[0011] In the above method, the step S1 comprises:

[0012] Step S1.1 defines a state vector s and an action vector a of the space manipulator;

[0013] Step S1.2 divides the constraints of the space manipulator into two categories of equality constraints g(a)=0 and inequality constraints h(a)≤0, and denotes j and k as the number of equality constraints and inequality constraints, respectively;

[0014] Step S1.3 constructs an equalization vector u and defines a joint action vector a u =[a T ,u T ] T , and derives a joint constraint function:

[0015]

[0016] Wherein, c g (a)=g(a),

[0017] Step S1.4 differentiates the joint constraint function c(a u ) with respect to time t to obtain:

[0018]

[0019] Wherein, is the differentiation of the joint action vector a u with respect to time t; is the differentiation of the action vector a with respect to time t; is the differentiation of the equalization vector u with respect to time t; J c (a u ) is a constraint Jacobian matrix, and its expression is:

[0020]

[0021] Wherein, O is a zero matrix, and diag(u) is a diagonal matrix with elements in u as diagonals.

[0022] Step S1.5 involves differentiating the joint constraint function from step S1.4 with respect to time t. By rearranging the terms, we get:

[0023]

[0024] in, It is J c Moore-Penrose pseudo-inverse matrix; It is J c The null space mapping matrix; I is the identity matrix; The derivative of the null space motion vector is generated by the spatial robotic arm safety motion planning strategy network;

[0025] Step S1.6 Construct the joint constraint function differential as shown below. The control law, thereby enabling the joint constraint function c(a) to... u The condition ) = 0 is always true:

[0026]

[0027] Where, k c c is the proportionality coefficient. d =0 represents the desired value of the joint constraint function.

[0028] In the above method, step S2 includes:

[0029] Step S2.1 Define the actual angle of the space robot arm joint as q θ The actual angular velocity of the joint is The actual pose of the end effector is x e The actual velocity at the end is The desired pose of the end effector is x ed The expected velocity at the end is

[0030] Step S2.2: Calculate the actual speed of the end effector of the space robotic arm. Decomposed into:

[0031]

[0032] in, The terminal target velocity is obtained according to the following control law:

[0033]

[0034] This refers to the end-of-course obstacle avoidance speed;

[0035] Step S2.3 Based on the actual angular velocity of the space robotic arm joints and the end actual velocity The mapping relationship is derived as follows:

[0036]

[0037] wherein, is the Moore-Penrose pseudo-inverse matrix of the spatial manipulator joint angular velocity and end velocity Jacobian matrix J f ; N f is the null space mapping matrix of J f ; is the joint null space angular velocity;

[0038] Step S2.4 constructs a spatial manipulator action vector a as shown below:

[0039]

[0040] The differential of the above formula with respect to time t is:

[0041]

[0042] Step S2.5 constructs a spatial manipulator state vector s as shown below, thereby serving as an input of the spatial manipulator safe motion planning strategy network:

[0043]

[0044] wherein n represents the number of joints of the spatial manipulator, and p0, p1~pn respectively represent the relative position vectors between the spatial manipulator base, link 1 to link n and the obstacle; n

[0045] Step S2.6 constructs the network structure of the spatial manipulator safe motion planner, which includes a spatial manipulator safe motion planning strategy network π θ , two main Q networks Q φ1 and Q φ2 , and two target Q networks and wherein the strategy network π θ takes the spatial manipulator state vector s as an input, and takes the differential of the null space action vector as an output; the main Q networks Q φ1 and Q φ2 and the target Q networks and all take the spatial manipulator state vector s and the differential of the null space action vector as inputs, and take the Q values corresponding to s and as outputs.

[0046] ​In the above method, the step S3 comprises:

[0047] Step S3.1 constructs the joint motion limit constraint equation as follows:

[0048] Define the minimum and maximum values of the joint angle of the spatial manipulator as q θmin and q θmax , and obtain the joint angle constraint condition of the spatial manipulator as follows:

[0049]

[0050] According to the above formula, the joint motion limit inequality constraint equation is constructed as follows:

[0051] h q_min (a) = -q θ + q θmin

[0052] h q_max (a) = q θ -q θmax

[0053] Take the partial derivative of the above formula with respect to a to obtain

[0054]

[0055] Step S3.2 constructs the end working space limit constraint equation as follows:

[0056] Define the upper and lower bounds of the end working space of the spatial manipulator as F emax and F emin , and obtain the end constraint condition of the spatial manipulator as follows:

[0057] F emin ≤ x e ≤ F emax

[0058] According to the above formula, the end working space limit constraint equation is constructed as follows:

[0059] h e_min (a) = -f forward (q θ ) + F emin

[0060] h e_max (a) = f forward (q θ ) - F emax

[0061] Wherein, f forward (·) is the forward kinematics model of the spatial manipulator;

[0062] Taking partial derivative of the above formula with respect to a, we obtain

[0063]

[0064] Step S3.3 constructs the end desired trajectory constraint equation as follows:

[0065] The end desired trajectory constraint equation is constructed as shown below:

[0066] g traj (a)=x ed -f forward (q θ )-x eδ

[0067] wherein the above formula is an equality constraint equation, f forward (·) is a spatial manipulator forward kinematics model, x eδ is an end obstacle avoidance pose;

[0068] Taking partial derivative of the above formula with respect to a, we obtain

[0069]

[0070] Step S3.4 obtains the constraint Jacobian matrix as shown below according to step S1.4 and steps S3.1, S3.2 and S3.3:

[0071]

[0072] wherein,

[0073]

[0074] In the above method, the step S4 comprises:

[0075] Step S4.1 constructs the reward function as shown below:

[0076] r=λ o r o +λ s r s +λ c r c

[0077] wherein, r o is a link obstacle avoidance feedback term, and its expression is:

[0078]

[0079] wherein, p i is a relative position vector between the spatial manipulator link i and the obstacle in step S2.5, d sa safety distance between the redundant space manipulator and the obstacle set by human; s a feedback term for end obstacle avoidance, whose expression is:

[0080]

[0081] wherein, and are the end obstacle avoidance velocities in x, y and z axis directions, and are the end obstacle avoidance velocities in x, y and z axis directions, respectively; c a collision penalty term, whose expression is:

[0082]

[0083] λ o , λ s and λ c are the weight coefficients of r o , r s and r c set by human, respectively;

[0084] Step S4.2 uses the reinforcement learning SAC algorithm to train the space manipulator safety motion planning strategy network π θ , the main Q network Q φ1 and Q φ2 , the target Q network and until the network converges, obtaining the trained space manipulator safety motion planning strategy network π θ .

[0085] Step S4.3 takes the space manipulator state vector s constructed in step S2.5 as the input of the space manipulator safety motion planning strategy network π θ , obtaining the differential of the zero space action vector According to step S1.5, the differential of the joint action vector a u with respect to time t is obtained. According to step S1.4, the differential of the joint action vector a is obtained from According to steps S2.4 and S2.3, the actual joint angular velocity ω of the space manipulator is calculated according to Actual driving instructions of the space mechanical arm. BRIEF DESCRIPTION OF DRAWINGS

[0086] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings needed in the embodiments will be briefly introduced as follows. Obviously, the drawings in the following description are only some embodiments of the present application, and other drawings can be obtained by those skilled in the art without creative and laborious work on the basis of these drawings.

[0087] Figure 1 is a flowchart of a space mechanical arm safety motion planning method based on constraint reinforcement learning provided by an embodiment of the present application;

[0088] Figure 2 is a space mechanical arm schematic diagram with a floating base and a four-degree-of-freedom mechanical arm used by an embodiment of the present application;

[0089] Figure 3 is a space mechanical arm simulation environment schematic diagram used by an embodiment of the present application;

[0090] Figure 4 is a cumulative reward average value curve diagram obtained by an embodiment of the present application in the space mechanical arm training process;

[0091] Figure 5 is a space mechanical arm motion trajectory diagram in a simulation environment when a policy network trained in an embodiment of the present application is verified. DETAILED DESCRIPTION

[0092] In order to better understand the technical solutions of the present application, the embodiments of the present application will be described in detail below with reference to the drawings.

[0093] It should be clear that the described embodiments are only some of the embodiments of the present application, not all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0094] An embodiment of the present application provides a space mechanical arm safety motion planning method based on constraint reinforcement learning, as shown in Figure 1 is a flowchart of a space mechanical arm safety motion planning method based on constraint reinforcement learning provided by an embodiment of the present application, which includes the following steps:

[0095] Step A1 constructs a space mechanical arm constraint reinforcement learning framework, which specifically includes the following steps:

[0096] Step A1.1 defines the state vector of the space mechanical arm as s and the action vector as a;

[0097] Step A1.2 divides the constraints of the space manipulator into two categories: equality constraints g(a) = 0 and inequality constraints h(a) < 0, and let j and k represent the number of equality constraints and inequality constraints, respectively;

[0098] Step A1.3 constructs the equalization vector u and defines the joint action vector a u = [a T , u T ] T , and derives the joint constraint function:

[0099]

[0100] where c g (a) = g(a),

[0101] Step A1.4 differentiates the joint constraint function c(a u ) with respect to time t to obtain:

[0102]

[0103] where is the differentiation of the joint action vector a u with respect to time t; is the differentiation of the action vector a with respect to time t; is the differentiation of the equalization vector u with respect to time t; J c (a u ) is the constraint Jacobian matrix, and its expression is:

[0104]

[0105] where O is a zero matrix, and diag(u) is a diagonal matrix with the elements of u as the diagonals;

[0106] Step A1.5 moves the term of the differentiation of the joint constraint function with respect to time t in Step A1.4 to obtain:

[0107]

[0108] where is the Moore-Penrose pseudo-inverse matrix of J c ; is the null space mapping matrix of J c ; I is the identity matrix; is the differentiation of the null space action vector, the value of which is generated by the space manipulator safety motion planning strategy network;

[0109] ​Step A1.6. Construct the differential of the joint constraint function as shown below Step A1.7. Construct the control law as shown below to make the joint constraint function c(a u ) = 0 always hold:

[0110]

[0111] where k c is the proportional coefficient, and c d = 0 is the desired joint constraint function value.

[0112] Step A2. Construct the spatial manipulator safety motion planner according to the spatial manipulator constraint reinforcement learning framework, which specifically includes the following steps:

[0113] Step A2.1. Define the spatial manipulator joint actual angle as q θ , the joint actual angular velocity as , the end actual pose as x e , the end actual velocity as , the end desired pose as x ed , and the end desired velocity as

[0114] Step A2.2. Decompose the spatial manipulator end actual velocity as follows:

[0115]

[0116] where is the end target velocity, which is obtained according to the following control law:

[0117]

[0118] is the end obstacle avoidance velocity;

[0119] Step A2.3. According to the mapping relationship between the spatial manipulator joint actual angular velocity and the end actual velocity , the following is derived:

[0120]

[0121] where is the Moore-Penrose pseudo-inverse matrix of the spatial manipulator joint angular velocity and end velocity Jacobian matrix J f ; N f is the null space mapping matrix of J f ; is the joint null space angular velocity;

[0122] Step A2.4 constructs a spatial manipulator action vector a as shown below:

[0123]

[0124] The differential of the above formula with respect to time t is:

[0125]

[0126] Step A2.5 constructs a spatial manipulator state vector s as shown below, so as to serve as an input of a spatial manipulator safe motion planning strategy network:

[0127]

[0128] wherein n represents the number of joints of the spatial manipulator, p0, p1~pn represent the relative position vectors between the base of the spatial manipulator, the first link to the nth link and the obstacle respectively. n

[0129] Step A2.6 constructs a network structure of a spatial manipulator safe motion planner, which includes a spatial manipulator safe motion planning strategy network π θ , two main Q networks Q φ1 and Q φ2 , and two target Q networks Q and Q wherein the strategy network π θ takes the spatial manipulator state vector s as an input and takes the differential of the null space action vector as an output; the main Q networks Q φ1 and Q φ2 and the target Q networks Q and Q all take the spatial manipulator state vector s and the differential of the null space action vector as inputs and take the Q values corresponding to s and as outputs.

[0130] Step A3 constructs a spatial manipulator safe motion constraint equation according to the spatial manipulator constraint reinforcement learning framework, which specifically includes the following steps:

[0131] Step A3.1 constructs a joint motion limit constraint equation as follows:

[0132] Define the minimum and maximum values of the joint angle of the spatial manipulator as q θmin and q θmax , and obtain the joint angle constraint condition of the spatial manipulator as shown below:

[0133]

[0134] ​According to the above formula, the joint motion limit inequality constraint equation is constructed as follows:

[0135] h q_min (a)=-q θ +q θmin

[0136] h q_max (a)=q θ -q θmax

[0137] Taking the partial derivative of the above formula with respect to a, we obtain

[0138]

[0139] Step A3.2 constructs the end workspace limit constraint equation as follows

[0140] Define the upper and lower bounds of the spatial manipulator end workspace as F emax and F emin , respectively, to obtain the spatial manipulator end constraint condition as follows:

[0141] F emin ≤x e ≤F emax

[0142] According to the above formula, the end workspace limit constraint equation is constructed as follows:

[0143] h e_min (a)=-f forward (q θ )+F emin

[0144] h e_max (a)=f forward (q θ )-F emax

[0145] Where f forward (·) is the spatial manipulator forward kinematics model;

[0146] Taking the partial derivative of the above formula with respect to a, we obtain

[0147]

[0148] Step A3.3 constructs the end desired trajectory constraint equation as follows:

[0149] The end desired trajectory constraint equation is constructed as follows:

[0150] g traj (a)=x ed -f forward (qθ )-x eδ

[0151] wherein the above formula is an equality constraint equation, f forward (·) is a spatial robot forward kinematics model, x eδ is an end obstacle avoidance pose;

[0152] Take the partial derivative of the above formula with respect to a to obtain

[0153]

[0154] Step A3.4 obtains the constraint Jacobian matrix as shown below according to step A1.4 and steps A3.1, A3.2, A3.3:

[0155]

[0156] wherein,

[0157]

[0158] Step A4 constructs a spatial robot safe motion training strategy according to the spatial robot safe motion planner and the spatial robot safe motion constraint equation, and obtains a trained spatial robot safe motion planning strategy network, which specifically includes the following steps:

[0159] Step A4.1 constructs a reward function as shown below:

[0160] r = λ o r o + λ s r s + λ c r c

[0161] wherein, r o is a link obstacle avoidance feedback term, and its expression is:

[0162]

[0163] wherein, p i is the relative position vector between the spatial robot link i and the obstacle as described in step A2.5, d s is a safety distance artificially set between the redundant spatial robot and the obstacle; r s is an end obstacle avoidance feedback term, and its expression is:

[0164]

[0165] wherein, and are the end obstacle avoidance velocity components in x, y and z axis directions, are the end-avoiding velocities component maximums in x, y and z axis directions; r c is a collision penalty term, whose expression is:

[0166]

[0167] λ o , λ s and λ c are the weight coefficients of r o , r s and r c respectively;

[0168] Step A4.2 uses the network structure of the spatial manipulator safety motion planner constructed in step A2.6, the spatial manipulator safety motion constraint equation constructed in step A3, and the reward function constructed in step A4.1 to train the spatial manipulator safety motion planning strategy network π θ , the main Q network Q φ1 and Q φ2 , the target Q network until the network converges, obtaining the trained spatial manipulator safety motion planning strategy network π θ , the specific steps of which are as follows:

[0169] Step A4.2.1 specifies the initial configuration of the spatial manipulator the safety distance d s of the spatial manipulator and the obstacle, the maximum number of rounds k num_ep in the training process, and the longest time t ep_max of each round;

[0170] Step A4.2.2 creates a data resource pool and initializes the strategy network π θ , the main Q network and the target Q network

[0171] Step A4.2.3 sets the current round number k = 1;

[0172] Step A4.2.4 generates a random joint angle Δq θ , sets and calculates the initial end pose

[0173] ​​​​Step A4.2.5 select the position p of the obstacle o , so as to satisfy the condition 0 < ||p0||, ||p1||, …, ||pn|| < d n s ;

[0174] Step A4.2.6 set the current time t = 0;

[0175] Step A4.2.7 set the cumulative reward R t = 0, and obtain the state vector s of the space manipulator according to the environment feedback;

[0176] Step A4.2.8 input the state vector s of the space manipulator at the current time into the policy network π θ , to obtain the differential of the null space action vector

[0177] Step A4.2.9 calculate the differential value of the joint constraint function according to step A1.6 Calculate the constraint Jacobian matrix J c according to step A3.4; based on and J c , calculate the differential of the joint action vector according to step A1.5 Take out the component about the differential of the action vector According to steps A2.4 and A2.3, calculate the actual angular velocity of the joints of the space manipulator

[0178] Step A4.2.10 take the actual angular velocity of the joints of the space manipulator as the control instruction to drive the space manipulator to move, obtain the state variable s' at time t+1, and calculate the obstacle avoidance reward function r at time t, which is added to the cumulative reward R t ;

[0179] Step A4.2.11 according to the reinforcement learning SAC algorithm, sample training data from the data resource pool, update the network parameters of the main Q network and the target Q network and and the policy network π θ ;

[0180] Step A4.2.12 if ||p i || > d s or ||p i || = 0 (i = 0, 1, …, n), exit the current round, otherwise skip this step;

[0181] ​​Step A4.2.13 Let t = t + 1, s = s';

[0182] Step A4.2.14 If t < t ep_max , go back to Step A4.2.8, otherwise skip this step;

[0183] Step A4.2.16 Let k = k + 1;

[0184] Step A4.2.16 If k < k num_ep , go back to Step A4.2.4, otherwise skip this step;

[0185] Step A4.2.17 End the training, obtain the trained spatial robot safe motion planning policy network

[0186] Step A4.3 Use the trained spatial robot safe motion planning policy network θ to perform spatial robot motion planning, the specific steps are as follows:

[0187] Step A4.3.1 Deploy the spatial robot safe motion planning policy network

[0188] Step A4.3.2 Set the end effector desired trajectory tracking controller parameters K p , K d , set the maximum time t ep_max for each round;

[0189] Step A4.3.3 Use the traditional offline trajectory generator to generate a spatial robot end effector desired trajectory sequence about time t

[0190] Step A4.3.4 Let the current time t = 0;

[0191] Step A4.3.5 Obtain the state vector s of the spatial robot according to the environment feedback;

[0192] Step A4.3.6 According to Step A2.2, obtain the spatial robot end effector target velocity

[0193] Step A4.3.7 Input the state vector s of the spatial robot at the current time into the policy network θ , obtain the differential of the null space action vector

[0194] Step A4.3.8 Calculate the joint constraint function differential value according to Step A1.6, calculate the constraint Jacobian matrix J c according to Step A3.4; based on and J c , the differential of the joint action vector is calculated according to step A1.5 The component about the differential of the action vector is taken out According to steps A2.4 and A2.3, the joint target angular velocity of the spatial manipulator is calculated according to

[0195] Step A4.3.9 If , the spatial manipulator is driven using , otherwise, the joint target angular velocity of the spatial manipulator is calculated according to the target velocity of the end of the spatial manipulator to drive the spatial manipulator;

[0196] Step A4.3.10 The state vector s' of the spatial manipulator at time t+1 is obtained;

[0197] Step A4.3.11 Let t=t+1 and s=s';

[0198] Step A4.3.12 If t≤t ep_max , return to step A4.3.6, otherwise, skip this step;

[0199] Step A4.3.13 End the above steps to complete the safe motion of the spatial manipulator.

[0200] According to the above method provided by the embodiment of the present application, a simulation experiment is performed on a planar four-degree-of-freedom spatial manipulator with a floating base. The planar four-degree-of-freedom spatial manipulator with a floating base used in the embodiment of the present application is shown in Figure 2 . The D-H parameters of the spatial manipulator are shown in Table 1, and the dynamics parameters are shown in Table 2.

[0201] Table 1 D-H parameters of the spatial manipulator

[0202]

[0203] Table 2 Dynamics parameters of the spatial manipulator

[0204]

[0205] Table 3 Hyperparameters used in the training process

[0206]

[0207] In the PyGame module based on the OpenAI Gym framework, a simulation environment is built, as shown in Figure 3 , and a radius d​​o = 0.04m circular obstacle with safety distance d s = 0.1m, in Figure 3 are respectively shown as dark thick line small circles and dark thick line large circles; in the simulation scene, a light thin line small circle representing the target position of the motion of the end of the space manipulator is also randomly placed, and the position is represented as x goal , so the desired trajectory of the end is a straight line from the initial position to the target position; at the same time, the motion range of the end of the manipulator is also limited within the rectangular region in the figure. The space manipulator is trained under the constraint conditions set as follows:

[0208] (1) joint position constraint:

[0209] q θmin = [-80°, -160°, -160°, -90°]

[0210] q θmax = [80°, 160°, 160°, 90°]

[0211] (2) end position constraint:

[0212] F emin = [0.3m, -0.7m, -180°]

[0213] F emax = [1.7m, 0.7m, 180°]

[0214] (3) end trajectory constraint:

[0215] x ed = 0.4(x goal -x e )

[0216] The hyperparameters used in the training are shown in Table 3, and the training is performed using the technical solution of the embodiment of the application. Once the distance between the obstacle and the space manipulator is greater than the safety distance, or the space manipulator collides with the obstacle, the round will be terminated and the next round will be entered. The training process undergoes 3e6 time points, and the average value of the cumulative reward obtained during the training process is shown in Figure 4 . As can be seen from the figure, after the training is performed for 2e6 time points, the cumulative reward gradually converges, the average value of the cumulative reward changes gently, the difference between the maximum value and the minimum value is decreasing, indicating that the space manipulator safe motion planning strategy network has gradually learned to complete. At the same time, it can be observed that during the entire training process, the space manipulator does not have the phenomenon of exceeding the constraint condition, and can meet the requirements of the set constraint condition.

[0217] The strategy network Verify it, such as Figure 5 As shown. The end effector of the space robotic arm needs to reach the designated target position (the small circle with light-colored thin lines) while avoiding collisions with obstacles (the small circles with dark-colored thick lines).

[0218] from Figure 5 It can be seen that during the movement of the end effector of the space robotic arm towards the target position, link 2 entered the safe distance of the obstacle. Therefore, according to step A4.3.9, the strategy network will be used. The generated Drive the space robotic arm. According to... Figure 5 From intermediate states 1 and 2, we can see that the actual distance between link 2 and the obstacle remains at a safe distance, without colliding with the obstacle. Simultaneously, the end effector of the robotic arm continues to move along the desired trajectory as far as possible to reach the target position. Only when it can no longer track the desired trajectory under the current configuration does the policy network... Increased end-of-course obstacle avoidance speed To ensure that link 2 does not collide with the obstacle, this increases the error between the actual and desired end-effector positions. The error gradually decreases until link 2 stops encroaching on the obstacle's safe distance, eventually reaching the target position. Furthermore, during the above verification process, the robotic arm did not exceed the constraints and consistently met the requirements of the set constraints, thus proving the effectiveness of this method.

[0219] The technical solutions of the embodiments of the present invention have the following beneficial effects:

[0220] A constraint reinforcement learning framework for a space manipulator is constructed, and based on this framework, a safe motion planner for the space manipulator is designed. This planner can ensure that the space manipulator is trained without violating a variety of given constraints, and ultimately achieve safe and fast obstacle avoidance of the space manipulator while satisfying the constraints and minimizing interruption of the current task.

[0221] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the scope of protection of the present invention.

[0222] The contents not described in detail in this specification are common knowledge to those skilled in the art.

Claims

1. A method for safe motion planning of a spatial robotic arm based on constraint reinforcement learning, characterized in that, The method includes: Step S1: Construct a constraint reinforcement learning framework for the spatial robotic arm; Step S2: Based on the aforementioned constraint reinforcement learning framework for the space manipulator, construct a safe motion planner for the space manipulator. Step S3: Based on the aforementioned spatial manipulator constraint reinforcement learning framework, construct the spatial manipulator safety motion constraint equations; Step S4: Based on the space manipulator safety motion planner and the space manipulator safety motion constraint equation, construct the space manipulator safety motion training strategy and obtain the trained space manipulator safety motion planning strategy network. Step S1 includes: Step S1.1 Define the state vector of the spatial robotic arm as follows: The action vector is ; Step S1.2: Divide the constraints of the space robot into equality constraints. and inequality constraints Two categories, and remember and These represent the number of equality constraints and inequality constraints, respectively. Step S1.3 Constructing the equalization vector and define the joint action vector. The joint constraint function is derived as follows: in, , ; Step S1.4 Combine the constraint functions Regarding time Differentiation yields: in, Joint action vector Regarding time The differential; Action vector Regarding time The differential; For equalization vectors Regarding time The differential; To constrain the Jacobian matrix, its expression is: in, It is a zero matrix. For The elements in the matrix are diagonal matrices. Step S1.5: Adjust the joint constraint function from step S1.4 for time. Differential result By rearranging the terms, we get: in, yes Moore-Penrose pseudo-inverse matrix; yes The null space mapping matrix; It is the identity matrix; The derivative of the null space motion vector is generated by the spatial robotic arm safety motion planning strategy network; Step S1.6 Construct the joint constraint function differential as shown below. The control law, thereby enabling the joint constraint function Always true: in, This is the proportionality coefficient. Let be the desired numerical value of the joint constraint function.

2. The method according to claim 1, characterized in that, Step S2 includes: Step S2.1 Define the actual angle of the space robotic arm joint as... The actual angular velocity of the joint is The actual pose of the end effector is The actual velocity at the end is The desired pose of the end effector is The expected velocity at the end is ; Step S2.2 Calculate the actual speed of the end effector of the space robotic arm. Decomposed into: in, The terminal target velocity is obtained according to the following control law: ； This refers to the end-of-course obstacle avoidance speed; Step S2.3 Based on the actual angular velocity of the space robotic arm joints and actual terminal velocity The mapping relationship is derived as follows: in, Jacobian matrix of joint angular velocity and end effector velocity of a space robotic arm Moore-Penrose pseudo-inverse matrix; for The null space mapping matrix; The joint zero-space angular velocity; Step S2.4 Construct the spatial robotic arm motion vector as shown below. : ； The above formula relates to time The differential is: ； Step S2.5 Construct the spatial robotic arm state vector as shown below. This serves as the input to the space robotic arm's safe motion planning strategy network: in, This indicates the number of joints in a space robotic arm. These represent the space robotic arm base, link 1 to link 2, respectively. The relative position vector between the object and the obstacle; Step S2.6 Construct the network structure of the space robotic arm safety motion planner, which includes a space robotic arm safety motion planning strategy network. Two main Q networks and Two target Q-networks and Among them, policy network Using the state vector of the space robotic arm As input, the differential of the null space action vector As output; Main Q network and and the target Q network and Both use the state vector of the space robotic arm Differential of null space action vector As input, with and The corresponding Q value is output.

3. The method according to claim 1, characterized in that, Step S3 includes: Step S3.1 Construct the joint motion limit constraint equations as follows: Define the minimum and maximum joint angles of the spatial robotic arm as follows: and The joint angle constraints of the space robotic arm are obtained as follows: Based on the above equation, the constraint equation for the joint motion limitation inequality is constructed as follows: Adjust the above equation Taking the partial derivative, we get ； Step S3.2 Construct the end-effector workspace constraint equations as follows: Define the upper and lower bounds of the workspace of the end effector of the spatial robotic arm as follows: and The end effector constraints of the space robotic arm are obtained as follows: Based on the above equation, the end-effector workspace constraint equation is constructed as follows: in, Forward kinematics model of a space robotic arm; Adjust the above equation Taking the partial derivative, we get ； Step S3.3 Construct the terminal desired trajectory constraint equations as follows: Construct the terminal desired trajectory constraint equations as shown below: The above equation is an equality constraint equation. Forward kinematics model of a space robotic arm This is the end-effector obstacle avoidance posture; Adjust the above equation Taking the partial derivative, we get Step S3.4 Based on steps S1.4 and S3.1, S3.2, and S3.3, the constrained Jacobian matrix is ​​obtained as follows: in, ， 。 4. The method according to claim 1, characterized in that, Step S4 includes: Step S4.1 Construct the reward function as shown below: in, The link obstacle avoidance feedback term is expressed as follows: in, The space robotic arm linkage described in step S2.5 The relative position vector between the object and the obstacle. A safe distance between the manually set redundant space robotic arm and obstacles; The end-point obstacle avoidance feedback term is expressed as follows: in, , and These are the end-of-line obstacle avoidance speeds. Components in the x, y, and z axes, , and These are the end-of-line obstacle avoidance speeds. The maximum value of the components in the x, y, and z axes; The collision penalty term is expressed as follows: ； , and each is manually designated , and Weighting coefficients; Step S4.2 Based on the network structure of the space robot safe motion planner constructed in step S2.6, the space robot safe motion constraint equation constructed in step S3, and the reward function constructed in step S4.1, the space robot safe motion planning policy network is trained using the reinforcement learning SAC algorithm. Main Q network and Target Q network and The training continues until the network converges, yielding a completed network for safe motion planning strategies for the space robotic arm. ; Step S4.3 The spatial robotic arm state vector constructed in step S2.5 is... As a network for safe motion planning strategies of space robotic arms The input yields the differential of the null space action vector. Based on step S1.5, the joint action vector is obtained. Regarding time Differential According to step S1.4, from To obtain the differential of the action vector Based on steps S2.4 and S2.3, according to The actual angular velocity of the space robotic arm joints was calculated. , serving as the actual driving commands for the space robotic arm.

Images