Moon surface mechanical arm excavation control system and method

Abstract

The invention relates to a lunar surface mechanical arm excavation control system and method. The lunar surface mechanical arm excavation control system comprises an excavation parameter setting module, a dynamics resolving module, a kinematics forward solution module, a lunar soil characteristic identification module, a target trajectory planning module and a kinematics inverse solution module. Through motion control over the mechanical arm joints, motion control over the sampling device at the tail end of the mechanical arm is achieved, the structural length of the mechanical arm is fully utilized, and therefore the digging depth is greatly increased. Meanwhile, in the excavating process, lunar soil characteristic parameters can be recognized, the motor overcurrent state is automatically judged, excavating parameters such as the soil cutting depth and the bulldozing distance are optimized, and the adaptive capacity of the mechanical arm to lunar soil and the excavating efficiency are improved. Compared with a traditional lunar surface sampling mechanical arm, the system mass and power consumption can be remarkably reduced.

Description

TECHNICAL FIELD

[0001] The application relates to a moon surface mechanical arm excavation control system and method, and belongs to the field of extraterrestrial celestial body sampling and detection. BACKGROUND

[0002] The moon is the closest celestial body to the earth and has been the focus of deep space exploration. Water ice resources, as important materials and energy sources, play a key role in future long-term manned exploration activities on the moon. At present, the main way for the moon surface sampling of domestic and foreign probes is fixed position shovel or drill sampling, which is characterized in that a mechanical arm remains stationary, and an end sampling device realizes moon surface excavation sampling through its own mechanism. Since the mechanical arm itself is not fully utilized, the excavation depth realized by these probes is generally shallow compared with the geometric size of the mechanical arm, and the ratio of the excavation depth to the mechanical arm extension length is less than 0.05, which greatly increases the mass and power consumption of the mechanical arm system for the moon surface in-situ sampling and detection task. In addition, the current moon surface sampling system has poor adaptability to complex lunar soil and lacks the ability to identify the characteristics of lunar soil with different compactness and automatically adjust the excavation parameters.

[0003] The mechanical arm mentioned in the patent "Moon surface sampling mechanical arm" (ZL201810557136.0) moves above the moon surface, keeps the current configuration of the mechanical arm, and realizes sampling through the movement mode of the end sampling device. The sampling depth of the mechanical arm can only be ensured by the end sampling device, and the geometric configuration of the mechanical arm itself is not utilized.

[0004] The patent "Extraterrestrial celestial body sampling and detection tool" (ZL202110206580.X) proposes a sampling device applicable to the moon surface, which is installed at the end of the mechanical arm, and the maximum sampling depth is determined by the geometric size of the device itself, and the geometric configuration of the mechanical arm cannot be utilized.

[0005] Compared with the above-mentioned patents, the moon surface mechanical arm excavation method proposed in the present application fully utilizes the structural length of the mechanical arm itself to greatly improve the excavation depth. SUMMARY

[0006] The technical problem of the present application is to overcome the shortcomings of the prior art and provide a moon surface mechanical arm excavation control system, which realizes the movement control of the end sampling device of the mechanical arm through the movement control of the joints of the mechanical arm, fully utilizes the structural length of the mechanical arm itself, and greatly improves the excavation depth.

[0007] The technical solution of the present application is:

[0008] A lunar surface mechanical arm excavation control system, comprising: an excavation parameter setting module, a dynamics solving module, a kinematics forward solution module, a lunar soil property identification module, a target trajectory planning module, a kinematics inverse solution module;

[0009] The excavation parameter setting module receives a ground surface excavation instruction, extracts geometric parameters representing the shape of the excavation pit in the instruction, including the initial excavation depth, the flat pushing distance, and the bulldozing distance, and outputs them to the target trajectory planning module;

[0010] The kinematics forward solution module collects joint angle information and sends it to the dynamics solving module. Meanwhile, the kinematics forward solution module also calculates the end position and attitude information according to the DH parameters of the mechanical arm, and outputs the end position and attitude information to the kinematics inverse solution module and the lunar soil property identification module;

[0011] The dynamics solving module collects joint current and judges the motor overcurrent state. According to the motor torque coefficient and the mechanical arm dynamics parameters, the end environmental force is calculated, and the end environmental force information is input to the lunar soil property identification module and the target trajectory planning module. At the same time, the motor overcurrent state is also sent to the target trajectory planning module;

[0012] The lunar soil property identification module identifies the lunar soil property parameter matrix according to the end environmental force information and the end position and attitude information, and calculates the optimal excavation depth of the mechanical arm according to the matrix, and inputs the calculation result to the target trajectory planning module;

[0013] The target trajectory planning module generates a target point sequence of a multi-layer excavation trajectory according to the input optimal excavation depth, flat pushing distance, bulldozing distance, and overcurrent state information, and inputs it to the joint kinematics inverse solution module. During the movement, the excavation parameters are updated according to the joint motor overcurrent state;

[0014] The kinematics inverse solution module calculates the end desired motion speed according to the target point sequence of the excavation trajectory and the current end position and attitude information of the mechanical arm, and converts it into the joint desired motion speed, which is output to the joint motion controller. The joint motion controller performs closed-loop control on the joint speed, so that the mechanical arm end moves along the desired trajectory.

[0015] Further, the working process of the kinematics forward solution module is as follows:

[0016] The joint angles of the lunar surface mechanical arm are measured, θ = [θ1θ2θ3θ4], θ1-θ4 are the shoulder yaw joint angle, the shoulder pitch joint angle, the elbow joint angle, and the wrist joint angle, respectively;

[0017] The shoulder yaw joint coordinate system relative to the base coordinate system transfer matrix T0 1 , the shoulder pitch joint coordinate system relative to the shoulder yaw joint coordinate system transfer matrix T1 2 , the elbow joint coordinate system relative to the shoulder pitch joint coordinate system transfer matrix T23 , wrist coordinate system relative to the elbow joint coordinate system transfer matrix T3 4 , end coordinate system relative to the wrist coordinate system transfer matrix T4 5 ;

[0018] Calculate the end of the robot arm coordinates P e = T0 1 ·T1 2 ·T2 3 ·T3 4 ·P0, wherein P0 is the coordinate of the end coordinate system in the wrist coordinate system. P e is the coordinate of the end of the robot arm in the base coordinate system.

[0019] Further, the working process of the dynamics calculation module is specifically:

[0020] Receive the lunar surface robot joint angle θ = [θ1θ2θ3θ4] provided by the kinematics forward solution module, θ1- θ4 are shoulder yaw joint angle, shoulder pitch joint angle, elbow joint angle, wrist joint angle respectively;

[0021] Measure the motor current of each joint of the lunar surface robot I = [i1i2i3i4], i1- i4 are the motor currents of the shoulder yaw joint, the shoulder pitch joint, the elbow joint and the wrist joint respectively;

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

[0023] Calculate the robot Jacobian matrix J;

[0024] Calculate the end of the environment force: F = J T ·[τ1τ2τ3τ4] T

[0025] Determine the overcurrent state of each joint motor, when any motor current i j ≥ i_lim j , let the overcurrent flag flag_over_cur = 1, otherwise let flag_over_cur = 0; wherein, j = 1, 2, 3, 4, i_ lim is the overcurrent threshold.

[0026] Further, the working process of the lunar soil property identification module is specifically:

[0027] The lunar soil property identification module identifies the lunar soil property according to the end of the environment force information and the end of the pose information Pe Recognize the lunar soil characteristic parameter matrix, and calculate the optimal digging depth of the manipulator accordingly;

[0028] Initialize the parameters for recognition, let P0 = I 3×3 Unit matrix, is the lunar soil characteristic parameter matrix; λ is the forgetting factor of the recognition process, λ = 0.9; k is the number of cycles, when the upper limit of the set calculation number is reached, the cycle calculation is stopped, and the value of k is initialized to 1;

[0029] Update the parameter matrix y k = F, F is the end environmental force calculated by the dynamics calculation module;

[0030] Calculate is the state matrix of the recognition process, where x1 = sinα·h; x2 = l·h; x3 = l·(cosφ) 2 ·h; where α is the angle between the digging blade of the manipulator end effector and the soil, which is a preset value; l is the current pushing distance, which is calculated from the current end pose information P e , the calculation formula is:

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

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

[0033] Calculate

[0034] K k is the gain matrix, which is calculated from the state matrix operator matrix P k-1 , forgetting factor λ, which is updated in the loop, and its subscript k represents the current cycle number;

[0035] Calculate

[0036] P k is the operator matrix, which is calculated from P k-1 , gain matrix K k , and state matrix , whose subscript k represents the current cycle number, which is obtained by cyclic recursion from P0;

[0037] Calculate

[0038] Let k = k + 1, repeat the calculation When the number of calculations reaches the set value, the next step is performed;

[0039] Calculate the optimal digging depth Where F max is the maximum force that can be provided by the end of the mechanical arm, and is a preset value.

[0040] Further, the working process of the target trajectory planning module is specifically:

[0041] Update the lower shovel depth d h = Δ m · j, where Δ m is the initial digging depth input by the digging parameter setting module, and j is the layer number;

[0042] Calculate the jth layer drop point coordinate P j_1 = P0+C·[d h L' 0] where L' is the drop distance, which is a preset value, P0 is the initial end coordinate, and C is the end pose transfer matrix;

[0043] If flag_over_cur = 1, update the flat pushing distance L = L - Δl, where Δl is the flat pushing distance adjustment amount after flowing, which is a preset value;

[0044] Calculate the jth layer flat pushing point coordinate P j_2 = P0+C·[d h L+L' 0];

[0045] Calculate the jth layer rise point coordinate P j_3 = P0+C·[0 L+L'+d h 0];

[0046] Calculate the jth layer bulldozing point coordinate P j_4 = P0+C·[0 L p +L'+d h 0] where L p is the bulldozing distance, which is a preset value;

[0047] Calculate the jth layer return point coordinate P 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 number j = j + 1;

[0049] Calculate the current digging depth he = Δ m + he, and if he ≥ h r stop planning.

[0050] Further, the working process of the kinematics inverse solution module is specifically:

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

[0052]

[0053] Calculate the acceleration and deceleration time in the motion process: acceleration time t s = v m a v , v m is the preset straight line speed, a v is the preset acceleration; total time t f = disv m + t s ; deceleration time t z = t f -t s ;

[0054] Timer time accumulation t = t + Δt, Δt is the path planning period, which is a preset value;

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

[0056]

[0057] Calculate the expected end line speed v e :

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

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

[0060] In the second aspect, the application further provides a lunar surface mechanical arm excavation control method, comprising the following steps:

[0061] (1) Perform excavation parameter setting: receive the excavation instruction input on the ground, extract the geometric parameters representing the excavation pit shape in the instruction, including the initial excavation depth, the pushing distance, and the bulldozing distance;

[0062] (2) Perform kinematics forward solution: collect joint angle information, and calculate end pose information according to the DH parameters of the mechanical arm;

[0063] (3) Perform dynamics calculation: collect joint current, judge motor overcurrent state; calculate the end environmental force according to the motor torque coefficient and the dynamics parameters of the mechanical arm;

[0064] (4) lunar soil characteristic identification: according to the end environmental force information and the end pose information, the lunar soil characteristic parameter matrix is identified, and the optimal digging depth of the manipulator is calculated;

[0065] (5) target trajectory planning: according to the input optimal digging depth, flat pushing distance, bulldozing distance and flow state information, the target point sequence of the multi-layer digging trajectory is generated, and in the movement process, the digging parameters are updated according to the joint motor overcurrent state;

[0066] (6) kinematics inverse solution: according to the target point sequence of the digging trajectory and the current end pose information of the manipulator, the end desired movement speed is calculated, and is converted into the joint desired movement speed, and is output to the joint movement controller, the joint movement controller performs closed-loop control on the joint speed, so that the manipulator end moves along the desired trajectory.

[0067] The beneficial effects of the present application compared with the prior art are:

[0068] (1) the lunar surface manipulator digging control method provided by the present application realizes the movement control of the sampling device at the end of the manipulator by controlling the movement of the joints of the manipulator, and fully utilizes the structural length of the manipulator itself, thereby greatly improving the digging depth.

[0069] (2) the present application can identify the lunar soil characteristic parameters during the digging process, automatically judge the motor overcurrent state, optimize the digging parameters such as cutting depth and bulldozing distance, and improve the adaptability of the manipulator to the lunar soil and the digging efficiency. Compared with the traditional lunar surface sampling manipulator, the system mass and power consumption can be significantly reduced.

[0070] (3) in the digging process of the present application, the motor current, joint angle and real-time calculation of the end force are combined with the digging parameters to obtain the lunar soil characteristic parameter matrix by parameter identification method, and the optimal digging depth is calculated according to the preset maximum end force, the digging parameters are updated, and the digging trajectory is adjusted automatically. BRIEF DESCRIPTION OF DRAWINGS

[0071] Figure 1 is a schematic diagram of a lunar surface manipulator;

[0072] Figure 2 is a block diagram of the lunar surface manipulator digging control system;

[0073] Figure 3 is an execution flowchart of the dynamics solving module;

[0074] Figure 4 is an execution flowchart of the target trajectory planning module;

[0075] Figure 5 is an execution flowchart of the lunar soil characteristic identification module;

[0076] Figure 6 execute a flow chart for kinematics inverse solution module;

[0077] Figure 7 execute a flow chart for kinematics inverse solution module;

[0078] Figure 8 execute a flow chart for kinematics inverse solution module; DETAILED DESCRIPTION

[0079] The specific embodiments of the present application are described in further detail below in conjunction with the accompanying drawings.

[0080] The present application realizes the lunar surface manipulator excavation motion control. According to the ground injection and autonomous calculation of the excavation parameters such as excavation depth, flat push distance, the target point sequence of the next layer excavation trajectory is calculated, and the trajectory is composed of the descending point, the flat push point, the ascending point, the bulldozing point and the return point. According to the manipulator geometric parameters, the current end pose of the manipulator is calculated by kinematics forward solution, and the end desired motion speed is calculated by kinematics inverse solution according to the target point obtained by planning, and is converted into joint desired motion speed for joint motion closed loop control. In the process of manipulator motion, the motor overcurrent state is monitored in real time, the end environmental force is calculated according to the joint current, motor torque coefficient and manipulator dynamics parameters, the lunar soil resistance model parameters are calculated by recursive least squares method, the soil cutting depth is optimized, and the bulldozing distance is adjusted according to the overcurrent state, the next layer target point sequence is planned according to the updated excavation parameters, and when the actual excavation depth is greater than the expected excavation depth, the planning is stopped. Since the end motion is driven by joint motion, the manipulator structure itself is utilized, so that the excavation depth is greatly improved. At the same time, the excavation depth is optimized according to the lunar soil parameters in the process of excavation, the manipulator excavation efficiency is improved, and the mass of the manipulator system is reduced.

[0081] The lunar surface manipulator corresponding to the present application is shown as Figure 1 The shoulder yaw joint 1, the shoulder pitch joint 2, the elbow joint 3, the wrist joint 4, the end effector 5, the large arm rod 6 and the small arm rod 7. The shoulder pitch joint 2 is fixedly connected to the output shaft of the shoulder yaw joint 1. One end of the large arm rod 6 is fixedly connected to the output shaft of the shoulder pitch joint 2, and the other end is fixedly connected to the elbow joint 3. One end of the small arm rod 7 is fixedly connected to the output shaft of the elbow joint 3, and the other end is fixedly connected to the wrist joint 4. The end effector 5 is fixedly connected to the output shaft of the wrist joint 4.

[0082] The composition of the excavation trajectory control system is shown as Figure 2 The system comprises: an excavation parameter setting module, a manipulator dynamics calculation module, a manipulator kinematics forward solution module, a lunar soil property identification module, a target trajectory planning module and a manipulator kinematics inverse solution module.

[0083] The excavation parameter setting module receives an excavation instruction input on the ground, extracts geometric parameters representing a shape of an excavation pit in the instruction, including a starting excavation depth, a push distance, and a bulldozing distance, and outputs to a target trajectory planning module;

[0084] The kinematics forward solution module collects joint angle information and sends it to the dynamics calculation module, and the kinematics forward solution module also calculates end position and attitude information according to DH parameters of the robotic arm, and outputs the end position and attitude information to the kinematics inverse solution module and the lunar soil property identification module;

[0085] The dynamics calculation module collects joint current and judges a motor overcurrent state, calculates an end environment force according to a motor torque coefficient and the dynamics parameters of the robotic arm, and inputs the end environment force information to the lunar soil property identification module and the target trajectory planning module, and also sends the motor overcurrent state to the target trajectory planning module;

[0086] The lunar soil property identification module identifies a lunar soil property parameter matrix according to the end environment force information and the end position and attitude information, and calculates an optimal excavation depth of the robotic arm according to the lunar soil property parameter matrix, and inputs the calculation result to the target trajectory planning module;

[0087] The target trajectory planning module generates a target point sequence of a multi-layer excavation trajectory according to the input optimal excavation depth, push distance, bulldozing distance, and overcurrent state information, and inputs the target point sequence to the joint kinematics inverse solution module, and updates the excavation parameters according to the joint motor overcurrent state during the motion process;

[0088] The kinematics inverse solution module calculates an end desired motion speed according to the target point sequence of the excavation trajectory and the current end position and attitude information of the robotic arm, converts the end desired motion speed into a joint desired motion speed, and outputs the joint desired motion speed to the joint motion controller, and the joint motion controller performs closed-loop control on the joint speed to make the robotic arm end move along the desired trajectory.

[0089] The kinematics forward solution module flow is shown in Figure 7 .

[0090] Step (1), measure the angles of each joint of the lunar surface robotic arm θ=[θ1 θ2 θ3 θ4] respectively as the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint angle;

[0091] Step (2), calculate the shoulder yaw joint coordinate system relative to the base transfer matrix T0 1 , the shoulder pitch joint coordinate system relative to the shoulder yaw joint coordinate system transfer matrix T1 2 , the elbow joint coordinate system relative to the shoulder pitch joint coordinate system transfer matrix T2 3 , the wrist joint coordinate system relative to the elbow joint coordinate system transfer matrix T3 4 , the end coordinate system relative to the wrist joint coordinate system transfer matrix T4 5 .

[0092] Step (3), calculate the end coordinate P e = T0 1 · T1 2 · T2 3 · T3 4 · P0. Where P0 is the coordinate of the end coordinate system in the wrist coordinate system. P e is the coordinate of the end of the robot arm in the base coordinate system.

[0093] The flow of the dynamics solving module is shown in Figure 3 .

[0094] Step (1), measure the angles of each joint of the lunar surface robot arm θ = [θ1 θ2 θ3 θ4] respectively for the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint angle;

[0095] Step (2), measure the motor currents of each joint of the lunar surface robot arm I = [i1 i2 i3 i4] respectively for the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint motor current;

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

[0097] Step (4), calculate the Jacobian matrix J of the robot arm;

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

[0099] Step (6), judge the overcurrent state of each joint motor, when any motor current i j ≥ i_lim j , j = 1 / 2 / 3 / 4 ( i_lim is the overcurrent threshold), let the overcurrent flag flag_over_cur = 1, otherwise let flag_over_cur = 0.

[0100] The flow of the lunar soil property identification module is shown in Figure 5 .

[0101] Step (1), initialize the parameters, let P0 = I 3×3 unit matrix, λ = 0.9, is the matrix of lunar soil characteristic parameters; λ is the forgetting factor of the identification process, λ = 0.9; k is the cycle number, and the cycle calculation is stopped when the upper limit of the set calculation number is reached, and the value of k is initialized to 1;

[0102] Step (2), updating the parameter matrix y k = F, F is the end environmental force calculated by the dynamics calculation module;

[0103] Step (3), calculation is the state matrix of the identification process, x1 = sin α · h; x2 = l · h; x3 = l · (cos φ) · h. Wherein, α is the angle between the mechanical arm end effector digging spade surface and the soil, which is a preset value; l is the current flat pushing distance, which is calculated by the current end pose information P 2 e , the calculation formula is:

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

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

[0106] Step (4), calculation K k is the gain matrix, which is calculated by the state matrix operator matrix P k-1 and forgetting factor λ, and is updated in the loop, and the subscript k represents the current cycle number;

[0107] Step (5), calculation P k is the operator matrix, which is calculated by P k-1 , gain matrix K k , and state matrix , and the subscript k represents the current cycle number, which is obtained by cyclic recursion from P0;

[0108] Step (6), calculation

[0109] Step (7), let k = k + 1, repeat the calculation When the calculation number reaches the set value, the next step is performed;

[0110] Step (8), calculation of the optimal digging depth Wherein F max is the maximum force that can be provided by the mechanical arm end, which is a preset value.

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

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

[0113] Step (2), calculate the coordinates P of the jth layer drop point j_1 =P0+C·[d h L' 0], L' is the distance of the descending segment, which is the preset value, P0 is the starting terminal coordinate, and C is the terminal posture transfer matrix;

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

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

[0116] Step (5), calculate the coordinates P of the jth layer rising point j_3 =P0+C·[0 L+L'+d h 0];

[0117] Step (6): Calculate the j-layer bulldozing point coordinates P j_4 =P0+C·[0 L p +L'+d h 0], where L p is the bulldozing distance, which is the preset value;

[0118] Step (7), calculate the coordinates of the return point P on the jth layer j_5 =P0+[0 0 h'], h' is the height above the return point, which is the preset value;

[0119] Step (8), update the cumulative number of layers j = j + 1;

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

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

[0122] Step (1), calculate the target point P des and the current point P int Position deviation:

[0123]

[0124] Step (2), calculate the acceleration and deceleration time during the movement:

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

[0126] Step (3), timer time accumulation 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 expected end linear velocity

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

[0131] Step (6), calculate the expected joint angular velocity ω = J -1 · v e

[0132] Embodiment:

[0133] The application also provides a lunar surface mechanical arm excavation control method, comprising:

[0134] Performing excavation parameter setting: receiving the excavation instruction input on the ground, extracting the geometric parameters representing the excavation pit shape in the instruction, including the initial excavation depth, the flat pushing distance, and the bulldozing distance;

[0135] Performing forward kinematics solution: collecting joint angle information, and calculating end pose information according to the DH parameters of the mechanical arm;

[0136] Performing dynamics solution: collecting joint current, judging the motor overcurrent state; calculating the end environmental force according to the motor torque coefficient and the dynamics parameters of the mechanical arm;

[0137] Performing lunar soil characteristic identification: identifying the lunar soil characteristic parameter matrix according to the end environmental force information and the end pose information, and calculating the optimal excavation depth of the mechanical arm accordingly;

[0138] Target trajectory planning is performed: according to the input optimal excavation depth, bulldozing distance, bulldozing distance, overcurrent state information, the target point sequence of the multi-layer excavation trajectory is generated, and in the movement process, the excavation parameters are updated according to the joint motor overcurrent state;

[0139] Kinematics inverse solution is performed: according to the target point sequence of the excavation trajectory, the current end pose information of the manipulator, the end desired motion speed is calculated, and the joint desired motion speed is converted, and output to the joint motion controller, the joint motion controller is closed loop controlled to the joint speed, and the manipulator end moves along the desired trajectory.

[0140] The moon surface manipulator excavation control method realizes the motion control of the sampling device at the end of the manipulator by controlling the motion of the joints of the manipulator, and fully utilizes the structural length of the manipulator itself, so that the excavation depth is greatly improved. Figure 8 As shown in the figure, the excavation trajectory includes the penetration section, the push shovel section, the rising section and the flattening section, which can form a flat cross-sectional shape, and is convenient for in-situ sampling on the moon surface. During the penetration section, the end shovel tip is perpendicular to the moon surface, and under the extrusion action of the bucket shovel, the compaction degree of the pit wall lunar soil in the penetration section can be increased, and the pit wall collapse can be avoided. During the rising section, the slope angle is kept less than 45°, which can avoid the loose lunar soil generated in the excavation process from flowing back into the pit. In the flattening section, the lunar soil is continuously pushed away from the excavation pit, providing a motion space for in-situ sampling. In addition, the lunar soil parameters can be identified during the excavation process, the motor overcurrent state is automatically judged, and the excavation trajectory parameters are automatically adjusted according to the motor overcurrent state, so as to optimize the excavation depth, bulldozing distance and other excavation parameters, thereby significantly improving the excavation efficiency of the manipulator and the adaptability to lunar soil with different compaction degrees.

[0141] The part not described in detail in the present application is common knowledge to those skilled in the art.

Claims

1. A lunar surface manipulator excavation control system, characterized in that: include: Mining parameter setting module, dynamics solution module, kinematics forward solution module, lunar soil characteristics identification module, target trajectory planning module, and kinematics inverse solution module; The excavation parameter setting module receives the excavation instructions from the ground, extracts the geometric parameters representing the shape of the excavation pit from the instructions, including the starting excavation depth, horizontal push distance, and bulldozing distance, and outputs them to the target trajectory planning module; The kinematics forward solution module collects joint angle information and sends it to the dynamics solution module. At the same time, the kinematics forward solution module also calculates the end-position posture information based on the manipulator DH parameters and outputs the end-position posture information to the kinematics inverse solution module and the lunar soil characteristics identification module. The dynamics solution module collects joint currents and determines the motor overcurrent status; The terminal environmental force is calculated based on the motor torque coefficient and the dynamic parameters of the manipulator, and the terminal environmental 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 terminal environmental force information and terminal posture information, and calculates the optimal excavation depth of the robotic arm based on this, and inputs the calculation results into the target trajectory planning module; The target trajectory planning module generates a target point sequence of a multi-layer excavation trajectory based on the input optimal excavation depth, horizontal pushing distance, bulldozing distance, and flow state information, and inputs it into the joint kinematics inverse solution module; During the movement, the mining parameters are updated according to the overcurrent status of the joint motor; The kinematic inverse module calculates the desired end motion speed based on the target point sequence of the excavation trajectory and the current end position information of the robotic arm, converts it into the desired joint motion speed, and outputs it to the joint motion controller. The joint motion controller performs closed-loop control of the joint speed to make the end of the robotic arm move along the desired trajectory.

2. The lunar surface manipulator excavation control system according to claim 1, characterized in that: The working process of the kinematics forward solution module is as follows: Measure the joint angles of the lunar manipulator θ = [θ1 θ2 θ3 θ4], where θ1 to θ4 are the shoulder yaw joint angle, shoulder pitch joint angle, elbow joint angle, and wrist joint angle, respectively; Calculate the transfer matrix of the shoulder yaw joint coordinate system relative to the base coordinate system Transfer matrix T1 of shoulder pitch joint coordinate system relative to shoulder yaw joint coordinate system 2 , transfer matrix of elbow joint coordinate system relative to shoulder pitch joint coordinate system Transfer matrix of wrist joint coordinate system relative to elbow joint coordinate system Transfer matrix of the end coordinate system relative to the wrist joint coordinate system Calculate the coordinates of the end of the robot arm Among them, P0 is the coordinate of the end coordinate system in the wrist joint coordinate system. e is the coordinate of the end of the robot arm in the base coordinate system.

3. The lunar surface manipulator 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 θ = [θ1θ2θ3θ4] of the lunar manipulator provided by the kinematics forward solution module, where θ1 to θ4 are the shoulder yaw joint angle, shoulder pitch joint angle, elbow joint angle, and wrist joint angle, respectively; Measure the motor current of each joint of the lunar surface manipulator I = [i1 i2 i3 i4], where i1 to i4 are the motor currents of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint respectively; Calculate the joint output torque: τ i =i i ·k i ·r i , i=1,2,3,4,k i is the joint motor torque coefficient, r i is the joint reduction ratio; Calculate the Jacobian matrix J of the robotic arm; Calculate the end environmental force: F = J T ·[τ1τ2τ3τ4] T Determine the overcurrent status of each joint motor. When any motor current i 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 is the overcurrent threshold.

4. The lunar surface manipulator excavation control system according to claim 3, characterized in that: The working process of the lunar soil characteristics identification module is as follows: The lunar soil characteristics identification module is based on the terminal environment force information and terminal posture information P e Identify the lunar soil characteristic parameter matrix and calculate the optimal excavation depth of the robotic arm based on it; Initialize the identification parameters, let P0 = I 3×3 The identity matrix, is the lunar soil characteristic parameter matrix; λ is the forgetting factor of the identification process, λ = 0.9; k is the number of cycles. When the set upper limit of the number of calculations is reached, the cycle calculation is stopped and the k value is initialized to 1; Update the parameter matrix y k =F, F is the terminal environmental force calculated by the dynamics solution module; calculate is the state matrix of the identification process, where x1 = sinα·h; x2 = l·h; x3 = l·(cosφ) 2 h; where α is the angle between the excavation shovel face of the end effector and the soil, which is a preset value; l is the current horizontal push distance, which is obtained from the current end pose information P e The calculation formula is: l=||P e -P initial || Among them, P initial is the terminal position P at the start of excavation e h is the current excavation depth; φ is the angle between the excavation blade and the soil, which is determined by α and the excavation blade geometry; calculate K k is the gain matrix, which is given by the state matrix Operator matrix P k-1 , the forgetting factor λ is calculated and continuously updated in the loop, and its subscript k represents the number of current loops; calculate P k is the operator matrix, P k-1 , gain matrix K k , the state matrix The calculated value, where the subscript k represents the current loop number, is obtained by recursive calculation of the P0 loop; calculate Let k = k + 1 and repeat the calculation When the number of calculations reaches the set value, proceed to the next step; Calculate the optimal excavation depth Among them F max It is the maximum force that the end of the robot arm can provide, which is the preset value.

5. The lunar surface manipulator excavation control system according to claim 4, characterized in that: The working process of the target trajectory planning module is specifically as follows: Update the shovel depth d h =Δ m j, Δ at the start time m The starting mining depth is input to the mining parameter setting module; j is the number of layers; Calculate the coordinates P of the j-th layer descent point j_1 =P0+C·[d h L' 0], L' is the distance of the descending segment, which is the preset value, P0 is the starting terminal coordinate, and C is the terminal posture transfer matrix; If flag_over_cur=1, then update the push distance L=L-Δl, where Δl is the push distance adjustment after overcurrent, which is a preset value; Calculate the coordinates P of the j-th layer push point j_2 =P0+C·[d h L+L' 0]; Calculate the coordinates P of the jth layer rising point j_3 =P0+C·[0 L+L'+d h 0]; Calculate the coordinates P of the push point on layer j j_4 =P0+C·[0 L p +L'+d h 0], where L p is the bulldozing distance, which is the preset value; Calculate the coordinates P of the return point on the jth layer j_5 =P0+[0 0 h'], h' is the height above the return point, which is the preset value; Update the cumulative number of layers j = j + 1; Calculate the current excavation depth he = Δ m +he, if he ≥ h r Stop planning.

6. The lunar surface manipulator excavation control system according to claim 5, characterized in that: The working process of the kinematic inverse solution module is as follows: Calculate the target point P des and the current point P int Position deviation: Calculate the acceleration and deceleration time during the motion process: acceleration time t s =v m / a v , v m is the preset linear speed, a v is the preset acceleration; the total time t f =dis / v m +t s ;Deceleration time t z =t f -t s ; The timer accumulates t = t + Δt, where Δt is the path planning period and is a preset value; Calculate the expected running distance f d , the calculation method is as follows: Calculate the expected terminal linear velocity v e : Among them, P e is the current end pose; Calculate the expected joint angular velocity ω = J -1 ·v e .

7. A lunar surface robotic arm excavation control method, characterized in that: include: Set excavation parameters: receive excavation instructions from the ground, and extract geometric parameters representing the shape of the excavation pit in the instructions, including starting excavation depth, horizontal push distance, and bulldozing distance; Perform kinematics forward solution: collect joint angle information and calculate end-position posture information based on the robot arm DH parameters; Perform dynamics calculations: collect joint currents and determine motor overcurrent status; Calculate the end environment force based on the motor torque coefficient and the robot arm dynamic parameters; Identify lunar soil characteristics: Identify the lunar soil characteristic parameter matrix based on the terminal environmental force information and terminal posture information, and calculate the optimal excavation depth of the robotic arm based on this information; Perform target trajectory planning: Generate a target point sequence for a multi-layer excavation trajectory based on the input optimal excavation depth, horizontal pushing distance, bulldozing distance, and overcurrent status information. During the motion process, the excavation parameters are updated according to the overcurrent status of the joint motors. Perform kinematic inverse analysis: Based on the target point sequence of the excavation trajectory and the current end position information of the robotic arm, calculate the desired end motion speed, convert it into the desired joint motion speed, and output it to the joint motion controller. The joint motion controller performs closed-loop control of the joint speed to make the end of the robotic arm move along the desired trajectory.

8. The lunar surface manipulator excavation control method according to claim 7, characterized in that: The working process of kinematics positive solution is as follows: Measure the joint angles of the lunar manipulator θ = [θ1θ2θ3θ4], where θ1 to θ4 are the shoulder yaw joint angle, shoulder pitch joint angle, elbow joint angle, and wrist joint angle, respectively; Calculate the transfer matrix of the shoulder yaw joint coordinate system relative to the base coordinate system Transfer matrix T1 of shoulder pitch joint coordinate system relative to shoulder yaw joint coordinate system 2 , transfer matrix of elbow joint coordinate system relative to shoulder pitch joint coordinate system Transfer matrix of wrist joint coordinate system relative to elbow joint coordinate system Transfer matrix of the end coordinate system relative to the wrist joint coordinate system Calculate the coordinates of the end of the robot arm Among them, P0 is the coordinate of the end coordinate system in the wrist joint coordinate system. e is the coordinate of the end of the robot arm in the base coordinate system; The working process of dynamic solution is as follows: Receive the joint angles θ = [θ1θ2θ3θ4] of the lunar manipulator provided by the kinematics forward solution module, where θ1 to θ4 are the shoulder yaw joint angle, shoulder pitch joint angle, elbow joint angle, and wrist joint angle, respectively; Measure the motor current of each joint of the lunar surface manipulator I = [i1 i2 i3 i4], where i1 to i4 are the motor currents of the shoulder yaw joint, shoulder pitch joint, elbow joint, and wrist joint respectively; Calculate the joint output torque: τ i =i i ·k i ·r i , i=1,2,3,4,k i is the joint motor torque coefficient, r i is the joint reduction ratio; Calculate the Jacobian matrix J of the robotic arm; Calculate the end environmental force: F = J T ·[τ1τ2τ3τ4] T Determine the overcurrent status of each joint motor. When any motor current i 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 is the overcurrent threshold.

9. The lunar surface manipulator excavation control method according to claim 8, characterized in that: The specific working process of lunar soil characteristics identification is as follows: Lunar soil characteristics identification is based on the terminal environment force information and terminal posture information P e Identify the lunar soil characteristic parameter matrix and calculate the optimal excavation depth of the robotic arm based on it; Initialize the identification parameters, let P0 = I 3×3 The identity matrix, is the lunar soil characteristic parameter matrix; λ is the forgetting factor of the identification process, λ = 0.9; k is the number of cycles. When the set upper limit of the number of calculations is reached, the cycle calculation is stopped and the k value is initialized to 1; Update the parameter matrix y k =F, F is the terminal environmental force calculated by dynamic solution; calculate is the state matrix of the identification process, where x1 = sinα·h; x2 = l·h; x3 = l·(cosφ) 2 h; where α is the angle between the excavation shovel face of the end effector and the soil, which is a preset value; l is the current horizontal push distance, which is obtained from the current end pose information P e The calculation formula is: l=||P e -P initial || Among them, P initial is the terminal position P at the start of excavation e h is the current excavation depth; φ is the angle between the excavation blade and the soil, which is determined by α and the excavation blade geometry; calculate K k is the gain matrix, which is given by the state matrix Operator matrix P k-1 , the forgetting factor λ is calculated and continuously updated in the loop, and its subscript k represents the number of current loops; calculate P k is the operator matrix, P k-1 , gain matrix K k , the state matrix The calculated value, where the subscript k represents the current loop number, is obtained by recursive calculation of the P0 loop; calculate Let k = k + 1 and repeat the calculation When the number of calculations reaches the set value, proceed to the next step; Calculate the optimal excavation depth Among them F max It is the maximum force that the end of the robot arm can provide, which is the preset value.

10. The lunar surface manipulator excavation control method according to claim 9, characterized in that: The working process of target trajectory planning is as follows: Update the shovel depth d h =Δ m j, Δ at the start time m The starting mining depth is input to the mining parameter setting module; j is the number of layers; Calculate the coordinates P of the j-th layer descent point j_1 =P0+C·[d h L' 0], L' is the distance of the descending segment, which is the preset value, P0 is the starting terminal coordinate, and C is the terminal posture transfer matrix; If flag_over_cur=1, then update the push distance L=L-Δl, where Δl is the push distance adjustment after overcurrent, which is a preset value; Calculate the coordinates P of the j-th layer push point j_2 =P0+C·[d h L+L' 0]; Calculate the coordinates P of the jth layer rising point j_3 =P0+C·[0 L+L'+d h 0]; Calculate the coordinates P of the push point on layer j j_4 =P0+C·[0 L p +L'+d h 0], where L p is the bulldozing distance, which is the preset value; Calculate the coordinates P of the return point on the jth layer j_5 =P0+[0 0 h'], h' is the height above the return point, which is the preset value; Update the cumulative number of layers j = j + 1; Calculate the current excavation depth he = Δ m +he, if he ≥ h r Stop planning; The working process of kinematic inverse solution is as follows: Calculate the target point P des and the current point P int Position deviation: Calculate the acceleration and deceleration time during the motion process: acceleration time t s =v m / a v , v m is the preset linear speed, a v is the preset acceleration; the total time t f =dis / v m +t s ;Deceleration time t z =t f -t s ; The timer accumulates t = t + Δt, where Δt is the path planning period and is a preset value; Calculate the expected running distance f d , the calculation method is as follows: Calculate the expected terminal linear velocity v e : Among them, P e is the current end pose; Calculate the expected joint angular velocity ω = J -1 ·v e .

Images