Lunar surface robotic arm excavation control system and method

Abstract

The present application relates to a kind of lunar surface mechanical arm excavation control system and method, comprising: excavation parameter setting module, dynamics solving module, kinematics positive solution module, lunar soil characteristic identification module, target trajectory planning module, kinematics inverse solution module.The present application realizes the motion control of the end sampling device of mechanical arm by the motion control of the joint of mechanical arm, and the structural length of mechanical arm itself is fully utilized, so that the excavation depth is greatly improved.Meanwhile, the lunar soil characteristic parameters can be identified in the process of excavation, the motor overcurrent state is automatically judged, the excavation parameters such as cutting depth and bulldozing distance are optimized, and the adaptability of mechanical arm to lunar soil and the excavation efficiency are improved.Compared with the traditional lunar surface sampling mechanical arm, the system mass and power consumption can be significantly reduced.

Description

Technical Field

[0001] This invention relates to a control system and method for lunar surface robotic arm excavation, belonging to the field of extraterrestrial body sampling and exploration. Background Technology

[0002] As the closest celestial body to Earth, the Moon has always been a key research focus in the field of deep space exploration. Water ice resources, as an important material and energy source, play a crucial role in future long-term manned lunar exploration activities. Currently, domestic and international probes mainly sample the lunar surface by digging or drilling at fixed locations. These methods involve a stationary robotic arm, with the end effector using its own mechanism to excavate and sample the lunar surface. Because the robotic arm's structure is not fully utilized, the excavation depth achieved by these probes is generally shallow relative to the arm's geometry, with the ratio of excavation depth to arm extension length typically less than 0.05. This significantly increases the mass and power consumption of the robotic arm system for in-situ lunar sampling missions. Furthermore, current lunar sampling systems are poorly adapted to complex lunar soil conditions, lacking the ability to identify the characteristics of lunar soils with varying densities and automatically adjust excavation parameters.

[0003] The robotic arm mentioned in the patent "A Lunar Surface Sampling Robotic Arm" (ZL201810557136.0) samples by moving above the lunar surface and maintaining its current configuration, using an end-effector sampling device to perform sampling. The sampling depth of this robotic arm can only be guaranteed by the end-effector sampling device, without utilizing the robotic arm's own geometry.

[0004] The patent "A sampling and detection tool for extraterrestrial objects" (ZL202110206580.X) proposes a sampling device that can be applied to the lunar surface. The device is installed at the end of a robotic arm, and its maximum sampling depth is determined by the device's own geometric dimensions, making it impossible to utilize the geometric configuration of the robotic arm.

[0005] Compared to the aforementioned patents, the lunar surface robotic arm excavation method proposed in this patent makes full use of the robotic arm's own structural length, thereby greatly increasing the excavation depth. Summary of the Invention

[0006] The technical problem solved by the present invention is to overcome the shortcomings of the prior art and provide a lunar surface robotic arm excavation control system. By controlling the motion of the robotic arm joints, the motion control of the sampling device at the end of the robotic arm is realized, making full use of the structural length of the robotic arm itself, thereby greatly improving the excavation depth.

[0007] The technical solution of this invention is:

[0008] A lunar surface robotic arm excavation control system includes: an excavation parameter setting module, a dynamics calculation module, a forward kinematics module, a lunar soil characteristic identification module, a target trajectory planning module, and an inverse kinematics module;

[0009] The excavation parameter setting module receives the excavation command from the ground, extracts the geometric parameters that characterize the shape of the excavation pit from the command, including the initial excavation depth, the horizontal pushing distance, and the bulldozing distance, and outputs them to the target trajectory planning module.

[0010] The forward kinematics module collects joint angle information and sends it to the dynamics calculation module. At the same time, the forward kinematics module also calculates the end-effector pose information based on the robot arm's DH parameters and outputs the end-effector pose information to the inverse kinematics module and the lunar soil characteristic identification module.

[0011] The dynamics calculation module collects joint current and determines the motor overcurrent state; it calculates the end effector environmental force based on the motor torque coefficient and the robot arm dynamic parameters, and inputs the end effector environmental force information into the lunar soil characteristic identification module and the target trajectory planning module; at the same time, it sends the motor overcurrent state to the target trajectory planning module.

[0012] The lunar soil characteristic identification module identifies the lunar soil characteristic parameter matrix based on the end-effector environmental force information and end-effector pose information, and calculates the optimal digging depth of the robotic arm accordingly, and inputs the calculation results into the target trajectory planning module.

[0013] The target trajectory planning module generates a sequence of target points for multi-layered excavation trajectories based on the input optimal excavation depth, pushing distance, bulldozing distance, and flow status information, and inputs this sequence into the joint kinematics inverse solution module; and updates the excavation parameters based on the flow status of the joint motors during the movement.

[0014] The inverse kinematics module calculates the desired end-effector velocity based on the target point sequence of the excavation trajectory and the current end-effector pose information, converts it into the desired joint velocity, and outputs it to the joint motion controller. The joint motion controller performs closed-loop control on the joint velocity, so that the end-effector of the robotic arm moves along the desired trajectory.

[0015] Furthermore, the working process of the kinematic forward solving module is as follows:

[0016] The angles of each joint of the lunar surface robotic arm are measured as θ = [θ1θ2θ3θ4], where θ1 to θ4 are the angles of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint, respectively.

[0017] Calculate the transfer matrix T0 between the shoulder yaw joint coordinate system and the base coordinate system. 1 The transition matrix T1 between the shoulder pitch joint coordinate system and the shoulder yaw joint coordinate system 2 The transition matrix T2 between the elbow joint coordinate system and the shoulder pitch joint coordinate system3 The wrist joint coordinate system to elbow joint coordinate system transition matrix T3 4 Transition matrix T4 between the end-effector coordinate system and the wrist joint coordinate system 5 ;

[0018] Calculate the coordinates P of the robotic arm's end effector. e =T0 1 ·T1 2 ·T2 3 ·T3 4 •P0, where P0 is the coordinate of the end-effector coordinate system in the wrist joint coordinate system. P e The coordinates of the robotic arm's end effector in the base coordinate system.

[0019] Furthermore, the working process of the dynamics solution module is as follows:

[0020] The lunar surface robotic arm receives angles θ = [θ1θ2θ3θ4] from the kinematics forward solution module, where θ1 to θ4 are the angles of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint, respectively.

[0021] The motor current of each joint of the lunar surface robotic arm is measured as I = [i1i2i3i4], where i1 to i4 are the motor currents of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint, respectively.

[0022] Calculate the joint output torque: τ i =i i ·k i ·r i i = 1, 2, 3, 4, k i r is the torque coefficient of the joint motor. i This refers to the joint reduction ratio;

[0023] Calculate the Jacobian matrix J of the robotic arm;

[0024] Calculate the environmental force at the end point: F = J T ·[τ1τ2τ3τ4] T

[0025] Determine the overcurrent status of each joint motor. When the current i of any motor is... j ≥i_lim j Set the overcurrent flag flag_over_cur = 1, otherwise set flag_over_cur = 0; where j = 1, 2, 3, 4, i_ lim This is the overcurrent threshold.

[0026] Furthermore, the specific working process of the lunar soil characteristic identification module is as follows:

[0027] The lunar soil characteristic identification module identifies end-effector environmental forces and end-effector pose information based on P...e Identify the lunar soil characteristic parameter matrix and calculate the optimal digging depth of the robotic arm accordingly;

[0028] Identify parameter initialization, let P0 = I 3×3 identity matrix λ is the lunar soil characteristic parameter matrix; λ is the forgetting factor in the identification process, λ = 0.9; k is the number of iterations. When the set upper limit of the number of calculations is reached, the loop calculation stops and the value of k is initialized to 1.

[0029] Update parameter matrix y k =F, where F is the end-effect force calculated by the dynamics solution module;

[0030] calculate Let x1 = sinα·h; x2 = l·h; x3 = l·(cosφ) where x1 = sinα·h; x2 = l·h; x3 = l·(cosφ) 2 ·h; where α is the angle between the excavation surface of the robotic arm's end effector and the soil, which is a preset value; l is the current pushing distance, determined by the current end effector pose information P e The calculation formula is as follows:

[0031] l = ||P e -P initial ||

[0032] Where P initial To mine the end pose P at the start time e h is the current digging depth; φ is the angle between the digging blade and the soil, determined by α and the geometry of the digging blade.

[0033] calculate

[0034] K k It is the gain matrix, derived from the state matrix. Operator matrix P k-1 The forgetting factor λ is calculated and continuously updated in the loop, with its subscript k indicating the current loop number;

[0035] calculate

[0036] P k It is an operator matrix, given by P k-1 Gain matrix K k State matrix The calculated value is given by the subscript k, which represents the current loop count and is obtained by recursively iterating from P0.

[0037] calculate

[0038] Let k = k + 1, and repeat the calculation. When the number of calculations reaches the set value, proceed to the next step;

[0039] Calculate the optimal excavation depth Where F max This is the maximum force that the robotic arm's end effector can provide, and it is a preset value.

[0040] Furthermore, the working process of the target trajectory planning module is as follows:

[0041] Update the shovel depth d h =Δ m ·j, Δ at the initial time m The starting excavation depth is input to the excavation parameter setting module; j is the number of layers.

[0042] Calculate the coordinates P of the descent point at level j. j_1 =P0+C·[d h L' 0], L' is the descent distance, which is a preset value, P0 is the starting and ending coordinates, and C is the ending attitude transfer matrix;

[0043] If flag_over_cur = 1, then update the push distance L = L - Δl, where Δl is the adjustment amount of the push distance after the overcurrent, which is a preset value;

[0044] Calculate the coordinates P of the j-th layer push point. j_2 =P0+C·[d h L+L' 0];

[0045] Calculate the coordinates P of the ascending point of the j-th layer. j_3 =P0+C·[0 L+L'+d h 0];

[0046] Calculate the coordinates P of the bulldozing point on layer j. j_4 =P0+C·[0 L p +L'+d h 0], where L p This represents the bulldozing distance, a preset value.

[0047] Calculate the coordinates P of the return point at level j. j_5 =P0+[0 0 h'], where h' is the height above the return point, which is a preset value;

[0048] Update the cumulative layer count j = j + 1;

[0049] Calculate the current excavation depth he = Δ m +he, if he≥h r Stop planning.

[0050] Furthermore, the working process of the inverse kinematics module is as follows:

[0051] Calculate target point P des and the current point P int Positional deviation:

[0052]

[0053] Calculate the acceleration and deceleration times during motion: acceleration time t s =v m a v v m For the preset linear velocity, a v The preset acceleration; total time t f =disv m +t s Deceleration time t z =t f -t s ;

[0054] The timer's time accumulation is t = t + Δt, where Δt is the path planning cycle and is a preset value;

[0055] Calculate the expected running distance f d The calculation method is as follows:

[0056]

[0057] Calculate the desired end linear velocity v e :

[0058] Where P e This is the current end-effector pose;

[0059] Calculate the desired joint angular velocity ω = J -1 ·v e .

[0060] Secondly, the present invention also proposes a method for controlling lunar surface robotic arm excavation, comprising the following steps:

[0061] (1) Set excavation parameters: Receive the excavation command from the ground and extract the geometric parameters that characterize the shape of the excavation pit from the command, including the initial excavation depth, the horizontal pushing distance, and the bulldozing distance.

[0062] (2) Perform kinematics solution: Collect joint angle information and calculate end-effector posture information based on the robot arm DH parameters;

[0063] (3) Perform dynamic calculations: collect joint currents and determine the motor overcurrent state; calculate the end-effector environmental forces based on the motor torque coefficient and the robot arm dynamic parameters.

[0064] (4) Identify lunar soil characteristics: Identify the lunar soil characteristic parameter matrix based on the end-effector environmental force information and end-effector pose information, and calculate the optimal excavation depth of the robotic arm accordingly;

[0065] (5) Perform target trajectory planning: Based on the input optimal excavation depth, horizontal pushing distance, bulldozing distance, and overcurrent status information, generate a target point sequence for multi-layer excavation trajectory, and update the excavation parameters according to the overcurrent status of the joint motor during the movement.

[0066] (6) Perform inverse kinematics: Based on the target point sequence of the excavation trajectory and the current end pose information of the robotic arm, calculate the desired end motion velocity and convert it into the desired joint motion velocity, output to the joint motion controller, and the joint motion controller performs closed-loop control on the joint velocity so that the end of the robotic arm moves along the desired trajectory.

[0067] The beneficial effects of this invention compared to the prior art are:

[0068] (1) The lunar surface robotic arm excavation control method proposed in this invention realizes the motion control of the sampling device at the end of the robotic arm by controlling the motion of the robotic arm joints, making full use of the structural length of the robotic arm itself, thereby greatly improving the excavation depth.

[0069] (2) During the excavation process, this invention can identify lunar soil characteristic parameters, automatically determine the motor overcurrent state, and optimize excavation parameters such as cutting depth and pushing distance, thereby improving the robotic arm's adaptability to lunar soil and its excavation efficiency. Compared with traditional lunar surface sampling robotic arms, it can significantly reduce system mass and power consumption.

[0070] (3) During the excavation process of this invention, the lunar soil characteristic parameter matrix is ​​obtained by combining the motor current, joint angle, and real-time calculation of end force with the excavation parameters through parameter identification method. The optimal excavation depth is calculated based on the preset maximum end force, the excavation parameters are updated, and the excavation trajectory is adjusted autonomously. Attached Figure Description

[0071] Figure 1 A schematic diagram of the components of the lunar surface robotic arm;

[0072] Figure 2 Block diagram of the control system for a lunar surface robotic arm excavation;

[0073] Figure 3 Here is the flowchart for the dynamics solution module.

[0074] Figure 4 Flowchart for the target trajectory planning module execution;

[0075] Figure 5 Flowchart for the lunar soil characteristic identification module;

[0076] Figure 6 Flowchart for the inverse kinematics module;

[0077] Figure 7 Flowchart for the execution of the forward kinematics solution module;

[0078] Figure 8 This is a schematic diagram of the excavation trajectory. Detailed Implementation

[0079] The specific embodiments of the present invention will now be described in further detail with reference to the accompanying drawings.

[0080] This invention realizes the motion control of a lunar surface robotic arm for excavation. Based on excavation parameters such as excavation depth and horizontal pushing distance calculated from ground data and autonomous calculations, the sequence of target points for the next layer of excavation trajectory is calculated. The trajectory consists of a descent point, a horizontal pushing point, an ascent point, a bulldozing point, and a return point. The current end-effector pose is calculated using forward kinematics based on the robotic arm's geometric parameters. The desired end-effector velocity is calculated using inverse kinematics based on the planned target points, and converted into desired joint velocity for closed-loop joint motion control. During the robotic arm's movement, the motor overcurrent status is monitored in real time. The environmental force at the end-effector is calculated based on joint current, motor torque coefficient, and robotic arm dynamic parameters. Lunar soil resistance model parameters are calculated using a recursive least squares method to optimize the cutting depth. The bulldozing distance is adjusted based on the overcurrent status. The next layer of target point sequence is planned based on the updated excavation parameters. Planning stops when the actual excavation depth exceeds the desired excavation depth. Because the end-effector movement is driven by joint motion, utilizing the robotic arm's own structure, the excavation depth is significantly increased. Simultaneously, optimizing the excavation depth based on lunar soil parameters during excavation improves the robotic arm's excavation efficiency and reduces the system's mass.

[0081] The lunar surface robotic arm corresponding to this invention is as follows: Figure 1 As shown, the device includes a shoulder yaw joint 1, a shoulder pitch joint 2, an elbow joint 3, a wrist joint 4, an end effector 5, a boom 6, and a forearm 7. The shoulder pitch joint 2 is fixed to the output shaft of the shoulder yaw joint 1. One end of the boom 6 is fixed to the output shaft of the shoulder pitch joint 2, and the other end is fixed to the elbow joint 3. One end of the forearm 7 is fixed to the output shaft of the elbow joint 3, and the other end is fixed to the wrist joint 4. The end effector 5 is fixed to the output shaft of the wrist joint 4.

[0082] The components of the excavation trajectory control system are as follows: Figure 2 As shown. The system includes: a mining parameter setting module, a robotic arm dynamics calculation module, a robotic arm forward kinematics solution module, a lunar soil characteristic identification module, a target trajectory planning module, and a robotic arm inverse kinematics solution module.

[0083] The excavation parameter setting module receives the excavation command from the ground, extracts the geometric parameters that characterize the shape of the excavation pit from the command, including the initial excavation depth, the horizontal pushing distance, and the bulldozing distance, and outputs them to the target trajectory planning module.

[0084] The forward kinematics module collects joint angle information and sends it to the dynamics calculation module. At the same time, the forward kinematics module also calculates the end-effector pose information based on the robot arm's DH parameters and outputs the end-effector pose information to the inverse kinematics module and the lunar soil characteristic identification module.

[0085] The dynamics calculation module collects joint current and determines the motor overcurrent state; it calculates the end effector environmental force based on the motor torque coefficient and the robot arm dynamic parameters, and inputs the end effector environmental force information into the lunar soil characteristic identification module and the target trajectory planning module; at the same time, it sends the motor overcurrent state to the target trajectory planning module.

[0086] The lunar soil characteristic identification module identifies the lunar soil characteristic parameter matrix based on the end-effector environmental force information and end-effector pose information, and calculates the optimal digging depth of the robotic arm accordingly, and inputs the calculation results into the target trajectory planning module.

[0087] The target trajectory planning module generates a sequence of target points for multi-layered excavation trajectories based on the input optimal excavation depth, pushing distance, bulldozing distance, and flow status information, and inputs this sequence into the joint kinematics inverse solution module; and updates the excavation parameters based on the flow status of the joint motors during the movement.

[0088] The inverse kinematics module calculates the desired end-effector velocity based on the target point sequence of the excavation trajectory and the current end-effector pose information, converts it into the desired joint velocity, and outputs it to the joint motion controller. The joint motion controller performs closed-loop control on the joint velocity, so that the end-effector of the robotic arm moves along the desired trajectory.

[0089] The process of the kinematics forward solution module is as follows: Figure 7 As shown.

[0090] Step (1): Measure the angles of each joint of the lunar surface robotic arm, θ = [θ1 θ2 θ3 θ4], which are the angles of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint, respectively.

[0091] Step (2): Calculate the transfer matrix T0 of the shoulder yaw joint coordinate system relative to the base. 1 The transition matrix T1 between the shoulder pitch joint coordinate system and the shoulder yaw joint coordinate system 2 The transition matrix T2 between the elbow joint coordinate system and the shoulder pitch joint coordinate system 3 The wrist joint coordinate system to elbow joint coordinate system transition matrix T3 4 Transition matrix T4 between the end-effector coordinate system and the wrist joint coordinate system 5 .

[0092] Step (3): Calculate the coordinates of the end point P. e =T0 1 ·T1 2 ·T2 3 ·T3 4 • P0. Where P0 is the coordinate of the end-effector coordinate system in the wrist joint coordinate system. P e The coordinates of the robotic arm's end effector in the base coordinate system.

[0093] The dynamics solution module process is as follows: Figure 3 As shown.

[0094] Step (1): Measure the angles of each joint of the lunar surface robotic arm, θ = [θ1 θ2 θ3 θ4], which are the angles of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint, respectively.

[0095] Step (2): Measure the motor current I = [i1 i2 i3 i4] of each joint of the lunar surface robotic arm, which are the motor currents of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint, respectively.

[0096] Step (3), calculate the joint output torque: τ i =i i ·k i ·r i i = 1 / 2 / 3 / 4, k i r is the torque coefficient of the joint motor. i This refers to the joint reduction ratio;

[0097] Step (4): Calculate the Jacobian matrix J of the robotic arm;

[0098] Step (5): Calculate the end-effect environmental force: F = J T ·[τ1τ2τ3τ4] T

[0099] Step (6): Determine the overcurrent status of each joint motor. When the current i of any motor is... j ≥i_lim j j = 1 / 2 / 3 / 4 ( If i_lim is the overcurrent threshold, set the overcurrent flag flag_over_cur = 1; otherwise, set flag_over_cur = 0.

[0100] The lunar soil characteristic identification module process is as follows: Figure 5 As shown.

[0101] Step (1): Initialize the identification parameters, let P0 = I 3×3 The identity matrix, λ = 0.9. λ is the lunar soil characteristic parameter matrix; λ is the forgetting factor in the identification process, λ = 0.9; k is the number of iterations. When the set upper limit of the number of calculations is reached, the loop calculation stops and the value of k is initialized to 1.

[0102] Step (2): Update the parameter matrix y k =F, where F is the end-effect force calculated by the dynamics solution module;

[0103] Step (3), Calculation Let x1 = sinα·h; x2 = l·h; x3 = l·(cosφ) be the state matrix for the identification process. 2 ·h. Where α is the angle between the digging surface of the robotic arm's end effector and the soil, which is a preset value; l is the current pushing distance, determined by the current end effector pose information P. e The calculation formula is as follows:

[0104] l = ||P e -P initial ||

[0105] Where P initial To mine the end pose P at the start time e h is the current digging depth; φ is the angle between the digging blade and the soil, determined by α and the geometry of the digging blade.

[0106] Step (4), Calculation K k It is the gain matrix, derived from the state matrix. Operator matrix P k-1 The forgetting factor λ is calculated and continuously updated in the loop, with its subscript k indicating the current loop number;

[0107] Step (5), Calculation P k It is an operator matrix, given by P k-1 Gain matrix K k State matrix The calculated value is given by the subscript k, which represents the current loop count and is obtained by recursively iterating from P0.

[0108] Step (6) Calculation

[0109] Step (7): Let k = k + 1, and repeat the calculation. When the number of calculations reaches the set value, proceed to the next step;

[0110] Step (8): Calculate the optimal excavation depth Where F max This is the maximum force that the robotic arm's end effector can provide, and it is a preset value.

[0111] The target trajectory planning module process is as follows: Figure 4 As shown.

[0112] Step (1) Update the shovel depth d h =Δ m ·j, Δ at the initial time m The starting excavation depth is input into the excavation parameter setting module;

[0113] Step (2): Calculate the coordinates P of the descent point of the j-th layer. j_1 =P0+C·[d h L' 0], L' is the descent distance, which is a preset value, P0 is the starting and ending coordinates, and C is the ending attitude transfer matrix;

[0114] Step (3): If flag_over_cur = 1, then update the horizontal push distance L = L - Δl, where Δl is the adjustment amount of the horizontal push distance after the overcurrent, which is a preset value;

[0115] Step (4): Calculate the coordinates P of the j-th layer horizontal push point. j_2 =P0+C·[d h L+L' 0];

[0116] Step (5): Calculate the coordinates P of the rising point of the j-th layer. j_3 =P0+C·[0 L+L'+d h 0];

[0117] Step (6): Calculate the coordinates P of the bulldozing point in layer j. j_4 =P0+C·[0 L p +L'+d h 0], where L p This represents the bulldozing distance, a preset value.

[0118] Step (7): Calculate the coordinates P of the return point at level j. j_5 =P0+[0 0 h'], where h' is the height above the return point, which is a preset value;

[0119] Step (8): Update the cumulative layer number j = j + 1;

[0120] Step (9): Calculate the current excavation depth he = Δ m +he, if he≥h r Stop planning.

[0121] The inverse kinematics module process is as follows: Figure 6 As shown.

[0122] Step (1): Calculate the target point P des and the current point P int Positional deviation:

[0123]

[0124] Step (2): Calculate the acceleration and deceleration times during the motion:

[0125] Calculate acceleration time t s =v m a v v m For the preset linear velocity, a v The preset acceleration is used; the total time t is calculated. f =disv m +t s ; Calculate the deceleration time t z =t f -t s ;

[0126] Step (3): Accumulate time using the timer: t = t + Δt

[0127] Step (4): Calculate the expected running distance f d The calculation method is as follows:

[0128]

[0129] Step (5): Calculate the desired end linear velocity

[0130] Where P e This is the current end pose.

[0131] Step (6): Calculate the desired joint angular velocity ω = J -1 ·v e

[0132] Example:

[0133] This invention also proposes a method for controlling lunar surface robotic arm excavation, comprising:

[0134] Set excavation parameters: Receive excavation instructions from the ground and extract the geometric parameters that characterize the shape of the excavation pit from the instructions, including the initial excavation depth, horizontal pushing distance, and bulldozing distance;

[0135] Perform forward kinematics: Collect joint angle information and calculate the end-effector posture information based on the robot arm's DH parameters;

[0136] Perform dynamic calculations: collect joint currents to determine motor overcurrent status; calculate the end effector environmental forces based on motor torque coefficients and robotic arm dynamic parameters;

[0137] Lunar soil characteristic identification: Identify the lunar soil characteristic parameter matrix based on the end-effector environmental force information and end-effector pose information, and calculate the optimal digging depth of the robotic arm accordingly;

[0138] Target trajectory planning: Based on the input optimal excavation depth, pushing distance, bulldozing distance, and flow status information, a target point sequence for multi-layer excavation trajectories is generated, and the excavation parameters are updated according to the flow status of the joint motor during the movement.

[0139] Perform inverse kinematics: Based on the target point sequence of the excavation trajectory and the current end-effector pose information, calculate the desired end-effector velocity and convert it into the desired joint velocity, which is then output to the joint motion controller. The joint motion controller performs closed-loop control on the joint velocity, so that the end-effector of the robotic arm moves along the desired trajectory.

[0140] This invention relates to a lunar surface robotic arm excavation control method. By controlling the motion of the robotic arm joints, it achieves motion control of the sampling device at the end of the robotic arm, fully utilizing the structural length of the robotic arm itself to significantly increase the excavation depth. For example... Figure 8 As shown, the excavation trajectory includes a penetration section, a pusher section, an ascending section, and a leveling section, forming a smooth profile shape that facilitates in-situ lunar surface sampling. During the penetration section, the end of the shovel tip is perpendicular to the lunar surface. The squeezing action of the bucket-shaped shovel increases the density of the lunar regolith on the pit wall, preventing pit wall collapse. During the ascending section, maintaining a slope angle of less than 45° prevents loose lunar regolith generated during excavation from flowing back into the pit. The leveling section continues to push the lunar regolith away from the excavation pit, providing space for in-situ sampling. Furthermore, lunar regolith parameters can be identified during excavation, automatically determining the motor's overcurrent status and adjusting the excavation trajectory parameters accordingly. This optimizes excavation parameters such as excavation depth and pushing distance, significantly improving the robotic arm's excavation efficiency and adaptability to lunar regolith of varying densities.

[0141] The parts of this invention not described in detail are common knowledge to those skilled in the art.

Claims

1. A lunar surface robotic arm excavation control system, characterized in that, include: The system includes modules for setting parameters, dynamics calculation, forward kinematics, lunar soil characteristics identification, target trajectory planning, and inverse kinematics. The excavation parameter setting module receives the excavation command from the ground, extracts the geometric parameters that characterize the shape of the excavation pit from the command, including the initial excavation depth, the horizontal pushing distance, and the bulldozing distance, and outputs them to the target trajectory planning module. The forward kinematics module collects joint angle information and sends it to the dynamics calculation module. At the same time, the forward kinematics module also calculates the end-effector pose information based on the robot arm's DH parameters and outputs the end-effector pose information to the inverse kinematics module and the lunar soil characteristic identification module. The dynamics calculation module collects joint currents to determine the motor overcurrent status. The end effector force is calculated based on the motor torque coefficient and the dynamic parameters of the robotic arm, and the end effector force information is input into the lunar soil characteristic identification module and the target trajectory planning module. At the same time, the motor overcurrent status is also sent to the target trajectory planning module; The lunar soil characteristic identification module identifies the lunar soil characteristic parameter matrix based on the end-effector environmental force information and end-effector pose information, and calculates the optimal digging depth of the robotic arm accordingly, and inputs the calculation results into the target trajectory planning module. The target trajectory planning module generates a sequence of target points for multi-layered excavation trajectories based on the input optimal excavation depth, pushing distance, bulldozing distance, and flow state information, and inputs it into the joint kinematics inverse solution module. And during the movement, the excavation parameters are updated according to the overcurrent status of the joint motor; The inverse kinematics module calculates the desired end-effector velocity based on the target point sequence of the excavation trajectory and the current end-effector pose information of the robotic arm, converts it into the desired joint velocity, and outputs it to the joint motion controller. The joint motion controller performs closed-loop control on the joint velocity, so that the end-effector of the robotic arm moves along the desired trajectory. The specific working process of the lunar soil characteristic identification module is as follows: The lunar soil characteristic identification module identifies end-effector environmental forces and end-effector pose information. Identify the lunar soil characteristic parameter matrix and calculate the optimal digging depth of the robotic arm accordingly; Identify parameter initialization, let identity matrix This is a matrix of lunar soil characteristic parameters; Forgetting factor in the recognition process ; The loop count is set to the maximum number of iterations. The loop will stop when the maximum number of iterations is reached. The value is initialized to 1; Update parameter matrix , The end-effect forces calculated by the dynamics solution module; calculate , Let be the state matrix of the identification process, where ; ; ;in, The angle between the excavation blade of the robotic arm's end effector and the soil is a preset value. The current translation distance is determined by the current end-effector pose information. The calculation formula is as follows: in To mine the end pose at the start time ; This is the current excavation depth; To dig the angle between the shovel blade and the soil, by The geometry of the excavator shovel was determined; calculate ; It is the gain matrix, derived from the state matrix. Operator matrix Forgetting factors The calculated value is continuously updated in the loop, and its index is... Indicates the current loop count; calculate ; It is an operator matrix, by Gain matrix State matrix The calculated result, its subscript Indicates the current loop count, by Obtained through iterative recursion; calculate ; make Repeated calculation When the number of calculations reaches the set value, proceed to the next step; Calculate the optimal excavation depth ,in This is the maximum force that the robotic arm's end effector can provide, and it is a preset value.

2. The lunar surface robotic arm excavation control system according to claim 1, characterized in that: The working process of the kinematic forward solution module is as follows: Measuring the angles of each joint of the lunar surface robotic arm , ~ These are the angles of the shoulder lateral joint, shoulder flexion / extension joint, elbow joint, and wrist joint; Calculate the transfer matrix between the shoulder yaw joint coordinate system and the base coordinate system. The transition matrix between the shoulder pitch joint coordinate system and the shoulder yaw joint coordinate system Elbow joint coordinate system to shoulder pitch joint coordinate system transfer matrix Transition matrix between wrist coordinate system and elbow coordinate system Transition matrix between end-effector coordinate system and wrist joint coordinate system ; Calculate the coordinates of the robotic arm's end effector ,in, These are the coordinates of the end-effector coordinate system in the wrist joint coordinate system; The coordinates of the robotic arm's end effector in the base coordinate system.

3. The lunar surface robotic arm excavation control system according to claim 1, characterized in that: The working process of the dynamics solution module is as follows: Receive the joint angles of the lunar surface robotic arm from the forward kinematics module. , ~ These are the angles of the shoulder lateral joint, shoulder flexion / extension joint, elbow joint, and wrist joint; Measuring the motor current of each joint of the lunar surface robotic arm , These are the motor currents for the shoulder lateral joint, shoulder flexion / extension joint, elbow joint, and wrist joint, respectively. Calculate the joint output torque: , , This refers to the torque coefficient of the joint motor. This refers to the joint reduction ratio; Calculate the Jacobian matrix of the robotic arm ; Calculate the environmental forces at the end point: Determine the overcurrent status of each joint motor; when the current of any motor... Overcurrent indicator Otherwise ;in, , This is the overcurrent threshold.

4. The lunar surface robotic arm excavation control system according to claim 1, characterized in that: The working process of the target trajectory planning module is as follows: Update shovel depth The starting time The starting excavation depth is input into the excavation parameter setting module; It refers to the number of floors; Calculate the first Layer descent point coordinates , This is the distance of the descent segment, a preset value. The starting and ending coordinates are... This is the end-effector attitude transfer matrix; if Then update the pushing distance. , This is the adjustment amount for the horizontal push distance after the flow has passed; it is a preset value. Calculate the first Layer-by-layer push point coordinates ; Calculate the first Layer Ascent Point Coordinates ; calculate Coordinates of bulldozing points ,in This represents the bulldozing distance, a preset value. Calculate the first Layer return point coordinates , The height above the return point is a preset value; Update cumulative floor count ; Calculate the current excavation depth , like Stop planning.

5. A lunar surface robotic arm excavation control system according to claim 4, characterized in that: The working process of the inverse kinematics module is as follows: Calculate the target point and current point Positional deviation: Calculate the acceleration and deceleration time during motion: acceleration time , The preset linear velocity, The preset acceleration; Total Time Deceleration time ; Timer time accumulation , This represents the path planning cycle, which is a preset value. Calculate the expected running distance The calculation method is as follows: Calculate the desired end linear velocity : ,in This is the current end-effector pose; Calculate the desired joint angular velocity .

6. A method for controlling lunar surface robotic arm excavation, characterized in that, include: Set excavation parameters: Receive excavation instructions from the ground and extract the geometric parameters that characterize the shape of the excavation pit from the instructions, including the initial excavation depth, horizontal pushing distance, and bulldozing distance; Perform forward kinematics: Collect joint angle information and calculate the end-effector posture information based on the robot arm's DH parameters; Perform dynamic calculations: collect joint currents and determine the motor overcurrent condition; Calculate the environmental forces acting on the end effector based on the motor torque coefficient and the dynamic parameters of the robotic arm; Lunar soil characteristic identification: Identify the lunar soil characteristic parameter matrix based on the end-effector environmental force information and end-effector pose information, and calculate the optimal digging depth of the robotic arm accordingly; Target trajectory planning: Based on the input optimal excavation depth, pushing distance, bulldozing distance, and flow status information, a target point sequence for multi-layer excavation trajectories is generated, and the excavation parameters are updated according to the flow status of the joint motor during the movement. Perform inverse kinematics: Based on the target point sequence of the excavation trajectory and the current end-effector pose information, calculate the desired end-effector velocity and convert it into the desired joint velocity, output to the joint motion controller, and perform closed-loop control of the joint velocity to make the end-effector of the robotic arm move along the desired trajectory. The specific process for identifying lunar soil characteristics is as follows: Lunar soil characteristic identification is based on end-effector environmental force information and end-effector pose information. Identify the lunar soil characteristic parameter matrix and calculate the optimal digging depth of the robotic arm accordingly; Identify parameter initialization, let identity matrix This is a matrix of lunar soil characteristic parameters; Forgetting factor in the recognition process ; The loop count is set to the maximum number of iterations. The loop will stop when the maximum number of iterations is reached. The value is initialized to 1; Update parameter matrix , The end-effect forces obtained from dynamic calculations; calculate , Let be the state matrix of the identification process, where ; ; ;in, The angle between the excavation blade of the robotic arm's end effector and the soil is a preset value. The current translation distance is determined by the current end-effector pose information. The calculation formula is as follows: in To mine the end pose at the start time ; This is the current excavation depth; To dig the angle between the shovel blade and the soil, by The geometry of the excavator shovel was determined; calculate ; It is the gain matrix, derived from the state matrix. Operator matrix Forgetting factors The calculated value is continuously updated in the loop, and its index is... Indicates the current loop count; calculate ; It is an operator matrix, by Gain matrix State matrix The calculated result, its subscript Indicates the current loop count, by Obtained through iterative recursion; calculate ; make Repeated calculation When the number of calculations reaches the set value, proceed to the next step; Calculate the optimal excavation depth ,in This is the maximum force that the robotic arm's end effector can provide, and it is a preset value.

7. The lunar surface robotic arm excavation control method according to claim 6, characterized in that: The working process of the correct kinematic solution is as follows: Measuring the angles of each joint of the lunar surface robotic arm , ~ These are the angles of the shoulder lateral joint, shoulder flexion / extension joint, elbow joint, and wrist joint; Calculate the transfer matrix between the shoulder yaw joint coordinate system and the base coordinate system. The transition matrix between the shoulder pitch joint coordinate system and the shoulder yaw joint coordinate system Elbow joint coordinate system to shoulder pitch joint coordinate system transfer matrix Transition matrix between wrist coordinate system and elbow coordinate system Transition matrix between end-effector coordinate system and wrist joint coordinate system ; Calculate the coordinates of the robotic arm's end effector ,in, These are the coordinates of the end-effector coordinate system in the wrist joint coordinate system; The coordinates of the robotic arm's end effector in the base coordinate system; The specific working process of dynamic solution is as follows: Receive the joint angles of the lunar surface robotic arm from the forward kinematics module. , ~ These are the angles of the shoulder lateral joint, shoulder flexion / extension joint, elbow joint, and wrist joint; Measuring the motor current of each joint of the lunar surface robotic arm , These are the motor currents for the shoulder lateral joint, shoulder flexion / extension joint, elbow joint, and wrist joint, respectively. Calculate the joint output torque: , , This refers to the torque coefficient of the joint motor. This refers to the joint reduction ratio; Calculate the Jacobian matrix of the robotic arm ; Calculate the environmental forces at the end point: Determine the overcurrent status of each joint motor; when the current of any motor... Overcurrent indicator Otherwise ;in, , This is the overcurrent threshold.

8. The lunar surface robotic arm excavation control method according to claim 7, characterized in that: The specific process of target trajectory planning is as follows: Update shovel depth The starting time The starting excavation depth is input into the excavation parameter setting module; It refers to the number of floors; Calculate the first Layer descent point coordinates , This is the distance of the descent segment, a preset value. The starting and ending coordinates are... This is the end-effector attitude transfer matrix; if Then update the pushing distance. , This is the adjustment amount for the horizontal push distance after the flow has passed; it is a preset value. Calculate the first Layer-by-layer push point coordinates ; Calculate the first Layer Ascent Point Coordinates ; calculate Coordinates of bulldozing points ,in This represents the bulldozing distance, a preset value. Calculate the first Layer return point coordinates , The height above the return point is a preset value; Update cumulative floor count ; Calculate the current excavation depth , like Stop planning; The working process of inverse kinematics is as follows: Calculate the target point and current point Positional deviation: Calculate the acceleration and deceleration time during motion: acceleration time , The preset linear velocity, Preset acceleration; total time Deceleration time ; Timer time accumulation , This represents the path planning cycle, which is a preset value. Calculate the expected running distance The calculation method is as follows: Calculate the desired end linear velocity : ,in This is the current end-effector pose; Calculate the desired joint angular velocity .

Images