Trajectory tracking and force compliance control method for rocket solid fuel shaping robot

Abstract

The present application relates to a trajectory tracking and force compliance control method of a rocket solid fuel shaping robot, mainly applied to the field of rocket solid fuel shaping. The control method comprises a position control inner loop and a force control outer loop. Through the small interval linearization method, the second order Lagrange dynamics model of the six-axis robot arm is linearized and discretized. After obtaining the discretized and linearized robot arm model, in the control inner loop, an MPC controller based on the improved computed torque control principle and a PD controller are designed as the position controller. In the force control outer loop, an admittance controller is designed to realize local compliance control of force. The proposed control strategy can accurately track the given trajectory in the rocket solid fuel shaping work, and the contact with the solid fuel does not generate rigid force.

Description

Technical Field

[0001] This invention relates to the field of robotic arm control, and more specifically to a method for trajectory tracking and force control. Background Technology

[0002] Solid-fuel rocket propellants are primarily solidified in molds through casting. The solidified propellant grains often have burrs on their surface and cannot be used directly in the rocket; therefore, manual carving is required. Workers must remove these burrs and carve the propellant grains into specific shapes. Due to the flammable and explosive nature of solid fuel, the robotic arm cannot generate excessive rigid contact force with the working environment when the cutting tool contacts the propellant surface. Therefore, the robotic arm must not only cut along the desired trajectory but also maintain a certain degree of compliance upon contact. Manually carving propellants is fraught with difficulties and dangers, necessitating the use of robotic arms. However, current research on the tracking accuracy and local force compliance control of robotic arms is not yet fully sufficient to meet mission requirements.

[0003] Therefore, how to provide a method for trajectory tracking and force compliance control of robotic arms has become a problem that researchers need to solve. Summary of the Invention

[0004] Based on the two problems raised, this invention proposes a trajectory tracking and force compliance control method for a rocket solid fuel shaping robot. This method achieves local compliance control while ensuring the tracking accuracy of the robotic arm.

[0005] The technical solution adopted by this invention to achieve the above objectives is: a trajectory tracking and force compliant control method for a rocket solid fuel shaping robot, which uses a robotic arm as the controlled object, tracks the trajectory of the robotic arm based on a given reference trajectory, and performs local compliant control on the contact force between the robotic arm and the solid fuel, including the following steps:

[0006] Position control:

[0007] The reference joint angle information q0 of each joint of the robotic arm is obtained based on the reference trajectory; the control torque τ is obtained by using the model prediction controller to obtain the joint angle. m According to the control torque τ m Control the robotic arm to work and detect the actual joint angle q of the robotic arm. r Based on the actual joint angle q r The error e between the reference joint angle q0 and the PD controller is used to control the joint angle of the robotic arm in a closed loop.

[0008] Force control:

[0009] When the robotic arm comes into contact with the work surface, a force sensor located at the end of the robotic arm detects the contact force F; based on the contact force F, a corrected trajectory ΔX is obtained through an admittance controller; the corrected trajectory ΔX is then compared with the reference trajectory (x). r y r By subtracting Δx from Δx, a new reference trajectory is obtained. r Δy r ), return to the position control step.

[0010] The position control includes the following steps:

[0011] A dynamic model of the robotic arm is established using Lagrange dynamics and then linearized and discretized; the reference trajectory (x) in Cartesian space is used. r y r The joint angles are converted into the joint space of the robotic arm through inverse kinematics; the joint angles in the joint space are then sent to the model prediction controller.

[0012] The model predictive controller transforms the model predictive control problem into a quadratic optimization problem. Based on the received joint angles and the linearized and discretized dynamic model of the robot arm, it obtains the control torque τ of the robot arm over the next P cycles. m ;

[0013] Select the first set of torques to send to the robotic arm, causing the robotic arm to perform a movement;

[0014] Based on the actual joint angle q r The error between the reference joint angle q0 and the joint angle is controlled in a closed loop by the PD controller.

[0015] The model predictive controller recalculates the control torque τ over the next P cycles based on the newly detected actual joint angles. m .

[0016] The process of establishing a dynamic model of the robotic arm using Lagrange dynamics and then linearizing and discretizing it includes the following steps:

[0017] The dynamic model:

[0018] Linearized and discretized dynamic model: x(k+1)=Ax(k)+Bτ(k)+G t ;

[0019] Where, q, These are the angle vector, angular velocity vector, and angular acceleration vector of the robotic arm joint, respectively, M(q)∈R 6×6 It is the inertia matrix of the robotic arm. It is the centrifugal force and Coriolis force matrix, G(q)∈R 6×6 This is the gravity matrix of the robotic arm, τ∈R 6×1τ is the joint control torque vector, x(k) is the state variable at time k, x(k+1) is the state variable at time k+1, and the state variables represent the joint angles and angular velocities of each joint of the robotic arm; τ(k) is a set of control torques at time k, T is the sampling time, A and B are the coefficient matrices of the state variables, and G is the control torque vector. t It is a constant matrix, as shown below.

[0020]

[0021] The model predictive controller transforms the model predictive control problem into a quadratic optimization problem, as follows:

[0022] The cumulative error between the predicted and reference values ​​of the trajectory state variables is chosen as the cost function for model predictive control. Among them, U k It is a state control variable, X k It is the predicted state for P future control cycles, R k Q is the reference trajectory, W is the error weighting coefficient, and W is the control output weighting coefficient.

[0023]

[0024]

[0025] The model predictive control problem is transformed into a quadratic programming problem. in, This means finding U under the constraints. k The minimum value, U(k+i|k)≤U max These are constraints, indicating that the maximum control torque should not exceed the threshold Umax, Z = 2(Θ). T QΘ+W), f T =2(E T QΘ+Θ T QΘ), the error matrix E = Ψ between the predicted trajectory and the reference trajectory. x (k)-R(k), where x(k) is the state variable at time k, R(k) is the reference trajectory at time k; Ψ is the coefficient matrix of the state variable, and Θ is the coefficient matrix of the control quantity;

[0026]

[0027] Based on quadratic programming, the goal is to make J(U) effective within the next P control cycles. k The smallest U k Only select the control torque τ at the first time. m =U k (1) Send to the robotic arm.

[0028] The admittance controller is as follows:

[0029]

[0030] Among them, M d It is the inertia coefficient matrix, D d It is the damping coefficient matrix, K d It is the stiffness coefficient matrix, F ext It is the external force received; It is a correction acceleration. It is the correction speed, q e It corrects the joint angle for acceleration. By performing two integrations, the corrected trajectory ΔX of the robot arm under the action of external force can be obtained.

[0031] The trajectory tracking and force compliance control device for the rocket solid fuel shaping robot includes:

[0032] The position control module is used to obtain the reference joint angle information q0 of each joint of the robotic arm based on the reference trajectory; and to obtain the control torque τ from the joint angles through the model prediction controller. m According to the control torque τ m Control the robotic arm to work and detect the actual joint angle q of the robotic arm. r Based on the actual joint angle q r The error e between the reference joint angle q0 and the PD controller is used to control the joint angle of the robotic arm in a closed loop.

[0033] The force control module is used to detect the contact force F when the robotic arm comes into contact with the working surface. A force sensor located at the end of the robotic arm detects the contact force F; based on the contact force F, a corrected trajectory ΔX is obtained through an admittance controller; and the corrected trajectory ΔX is compared with the reference trajectory (x... r y r By subtracting Δx from Δx, a new reference trajectory is obtained. r Δy r ), then return to the position control module.

[0034] The present invention has the following beneficial effects and advantages:

[0035] 1. In the position control of the robotic arm, the present invention uses model predictive control to calculate the torque of the robotic arm, making full use of the model information of the robotic arm.

[0036] 2. In the position control section, a PD controller was added to the feedback loop to ensure the accuracy of trajectory tracking.

[0037] 3. This invention balances precise trajectory tracking and localized compliant control of the robotic arm, avoiding rigid contact forces between the robotic arm and the environment. Attached Figure Description

[0038] Figure 1 This is a schematic diagram of the method of the present invention;

[0039] Figure 2 This is the algorithm flowchart. Detailed Implementation

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

[0041] This invention discloses a trajectory tracking and local force compliance control method for a rocket solid fuel shaping robot, primarily applied in the field of rocket solid fuel shaping. It addresses the challenges of handling toxic, flammable, and explosive solid fuels, as well as the harsh and dangerous environment during manual operation, by controlling a robot to replace manual labor in solid fuel cutting. The control method comprises an inner position control loop and an outer force control loop. The second-order Lagrangian dynamics model of the six-axis robot arm is linearized and discretized using a small-interval linearization method. After obtaining the discretized and linearized robot arm model, an MPC controller and a PD controller based on an improved computational torque control principle are designed as position controllers in the inner control loop. An admittance controller is designed in the outer force control loop to achieve local force compliance control. The proposed control strategy can accurately track a given trajectory during rocket solid fuel shaping without generating rigid forces upon contact with the solid fuel.

[0042] like Figure 2 As shown, a trajectory tracking and force compliance control method for a rocket solid fuel shaping robot includes the following specific steps:

[0043] Using a robotic arm as the controlled object, based on a given Cartesian space (x... r y r Position control is performed based on the reference trajectory, and compliant control is performed based on the force F on the force sensor that interacts with the working environment.

[0044] The reference trajectory in Cartesian space is transformed into a reference angle q0 in joint space through inverse kinematics. This reference angle is then used to calculate the appropriate control torque τ for model predictive control. m .

[0045] The torque τ calculated by the model predictive controller m Given the controlled robotic arm, detect the actual joint angle q of the robotic arm. r The difference e between the actual joint angle and the reference joint angle is given to the PD controller, which is used to eliminate errors caused by inaccurate modeling.

[0046] The robotic arm receives the joint angle from the position controller, and then begins to work, interacting with the environment. The six-dimensional force sensor obtains the contact force F with the environment, and the force F is sent to the admittance controller.

[0047] The admittance controller converts the magnitude of the received force into a position ΔX. ΔX corrects the reference trajectory in Cartesian space, making the robotic arm exhibit local compliance.

[0048] The steps for achieving position control of the robotic arm are as follows:

[0049] A second-order dynamic model of a six-axis robotic arm is established using Lagrange dynamics to obtain the dynamic relationships of the robotic arm, thus linearizing and discretizing the highly nonlinear dynamic equations.

[0050] The reference trajectory (x) in Cartesian space is given by parametric equations. r y r );

[0051] The reference trajectory in Cartesian space is transformed into the joint angles in the joint space of the robotic arm through inverse kinematics.

[0052] The joint angles in the joint space are sent to the position controller;

[0053] The model predictive controller in the position controller transforms the model predictive control problem into a quadratic optimization problem. Based on the received joint angles and the discretized and linearized dynamic equations of the robotic arm, it calculates the control torque τ of the robotic arm over the next P cycles. m ;

[0054] Select the first set of torques to send to the robotic arm;

[0055] The robotic arm receives a torque and generates a movement, updating the system's joint angle q.

[0056] The PD controller corrects for the error between the new joint angle and the reference joint angle q0.

[0057] The model predictive controller recalculates the control torque τ for the next P cycles based on the new joint angles. m .

[0058] The steps for position correction of the admittance controller are as follows:

[0059] After the reference trajectory passes through the position controller, it drives the robotic arm, which then performs actions to shape the rocket's solid fuel.

[0060] The six-dimensional force sensor detects the force F between the robotic arm and the solid fuel surface and sends the force to the admittance controller;

[0061] The admittance controller derives a corrected trajectory ΔX based on the relationship between force and position, and corrects the reference trajectory in Cartesian space to make the end effector of the robotic arm locally compliant.

[0062] The specific steps for linearizing and discretizing the dynamic equations of the robotic arm are as follows:

[0063] A second-order dynamic model of the six-axis robotic arm is established based on Lagrange dynamics. choose As the state variable of the system, τ is chosen as the controlled variable. The linearized and discretized equation is: x(k+1)=Ax(k)+Bτ(k)+G t .

[0064] Where q, These are the angle vector, angular velocity vector, and angular acceleration vector of the robotic arm joints, respectively, M(q)∈R 6×6 It is the inertia matrix of the robotic arm. It is the centrifugal force and Coriolis force matrix, G(q)∈R 6×6 This is the gravity matrix of the robotic arm, τ∈R 6×1 It is the joint control torque vector, where

[0065]

[0066] The position controller performs trajectory tracking in the following manner:

[0067] The cumulative error between the predicted and reference values ​​of the trajectory state variables is chosen as the cost function for model predictive control. Among them, U k X is the control variable for the system state. k R is the predicted state of the system for P future control cycles. k Q is the reference trajectory, W is the error weighting coefficient, and W is the control output weighting coefficient.

[0068] The mathematical description of the model predictive control trajectory tracking problem is as follows:

[0069]

[0070]

[0071] The mathematical form of transforming the model predictive control problem into a quadratic optimization problem is: Where Z = 2(Θ) T QΘ+W), f T =2(E T QΘ+Θ T QΘ).

[0072] The mathematical form of the PD control law is: The control torque received by the robotic arm is: τ = τ m +τ f , where τ f It is a feedback term that compensates for errors in the system, τ mThe torque is calculated by the model predictive controller, e is the joint angle error, and K is the torque calculated by the model predictive controller. v It is the position gain matrix, K p It is a proportional gain matrix.

[0073] The force controller takes the following form:

[0074] Among them, M d It is the inertia coefficient matrix, D d It is the damping coefficient matrix, K d It is a stiffness coefficient matrix. Adjusting these parameters can change the compliance of the robotic arm in contact with the environment.

[0075] The acceleration generated by the robotic arm under external force: Integrating the acceleration twice yields the corrected trajectory ΔX of the robotic arm.

[0076] A trajectory tracking and force-compliant control method for a rocket solid fuel shaping robot, the specific steps of which are as follows:

[0077] like Figure 1 As shown, the design concept of this invention is as follows:

[0078] Using the robotic arm as the controlled object, the trajectory of the robotic arm is tracked based on a given reference trajectory, and the contact force between the robotic arm and the solid fuel is locally compliantly controlled.

[0079] The reference joint angle information q0 of the six joints of the robotic arm is obtained by inverse kinematics from the given reference trajectory in Cartesian space.

[0080] The joint angle is assigned to the position controller, and the model predictive controller within the position controller calculates the appropriate control torque τ. m The calculated torque controls the operation of the robotic arm, and the actual joint angle q of the robotic arm is detected. r ;

[0081] Based on the actual joint angle q r The PD controller corrects the control error based on the difference e between the reference joint angle q0 and the reference joint angle q0.

[0082] The robotic arm comes into contact with the work surface, and the six-dimensional force sensor detects the contact force F.

[0083] The corrected trajectory ΔX is calculated using the admittance controller based on the contact force F.

[0084] Compare the corrected trajectory ΔX with the reference trajectory (x) r y r By subtracting Δx from Δx, a new reference trajectory is obtained. r Δy r );

[0085] The new reference trajectory continues to be transformed into a reference joint angle in joint space through inverse kinematics, and then controls the movement of the robotic arm after passing through the position controller.

[0086] Optionally, in the above-mentioned trajectory tracking and force compliance control method for a rocket solid fuel shaping robot, the specific steps for the model predictive controller to calculate the required torque τ of the robotic arm are as follows:

[0087] Select according to the requirements of trajectory control. As state variables of the system, state variables represent the joint angles and joint angular velocities of each joint of the robotic arm.

[0088] By analyzing the dynamic equations of the robotic arm Linearization yields the linearized and discretized dynamic equations of the six-axis robotic arm: x(k+1)=Ax(k)+Bτ(k)+G t , where q, These are the angle vector, angular velocity vector, and angular acceleration vector of the robotic arm joints, respectively, M(q)∈R 6×6 It is the inertia matrix of the robotic arm. It is the centrifugal force and Coriolis force matrix, G(q)∈R 6×6 This is the gravity matrix of the robotic arm, τ∈R 6×1 Let τ be the joint control torque vector, x(k) be the system state variable at time k, x(k+1) be the system state variable at time k+1, τ(k) be a set of control torques of the system at time k, T be the system sampling time, A and B be the coefficient matrices of the state variables, and G be the control torque vector. t It is a constant matrix, as shown below.

[0089]

[0090] Based on the discretized and linearized dynamic equations of the robotic arm, the state equations of the system over the next P periods are as follows:

[0091] X k =Ψx(k)+ΘU k +Υ

[0092] Among them, X k X is the predicted state of the system for P future control cycles. k =[x(k+1|k) T x(k+2|k) T ...x(k+p|k) T ] T Let x(k) be the predicted state of the system at time k, and x(k+1|k) be the state of the system at time k. T U represents the predicted state of the system at time k+1, which is given by time k.k U is the control variable for the system state at P future time points. k =[τ m (k|k) T τ m (k+1|k) T ......τ m (k+p-1|k) T ] T , τ m (k+1|k) T This represents the control torque predicted at time k+1 from time k. Ψ is the coefficient matrix of the state variables, Θ is the coefficient matrix of the control input, and r is a matrix composed of constant values. Details are as follows:

[0093]

[0094] Define a cost function For the control torque τ m Apply constraints, where R k The trajectory that the system's state tracking is expected to follow is called the reference trajectory, R. k =[r(k+1)] T r(k+2) T ...r(k+p) T ] T , r(k+p) T This represents the reference value at time k+p, where Q is the error weighting coefficient and W is the control output weighting coefficient.

[0095] Describing the model predictive control problem mathematically.

[0096]

[0097] The model predictive control problem is transformed into a quadratic programming problem. in, This means finding U under the constraints. k The minimum value, U(k+i|k)≤U max These are constraints, indicating that the maximum control torque should not exceed 100N, Z = 2(Θ). T QΘ+W), f T =2(E T QΘ+Θ T QΘ), according to quadratic programming, can be calculated to make J(U) within the next P periods. k The smallest U k However, only the control torque τ at the first time point is selected. m =U k (1) Send to the robotic arm.

[0098] Optionally, in the above-mentioned trajectory tracking and force compliance control method for a rocket solid fuel shaping robot, the specific steps for the PD controller to perform position correction are as follows:

[0099] Based on the detected actual joint angle q r The difference e between the joint angle q0 and the reference joint angle is substituted into the PD controller. The control law of the PD controller is: in, It is the tracking error of the joint angular velocity of the robotic arm joint angle, K. p It is the position gain matrix, which is a diagonal matrix, K v It is the velocity gain matrix, which is also a diagonal matrix, τ f It is a feedback term that compensates for errors in the control system. Factors causing these errors include uncertainties in inertial parameters, unmodeled forces, and external disturbances.

[0100] The corrected control torque τ=τ m +τ f , where τ m The torque is calculated by the model predictive controller. It provides the joint force required for the desired state of the robotic arm. The control torque τ is sent to the robotic arm to control its movement.

[0101] Optionally, in the above-mentioned trajectory tracking and force compliance control method for a rocket solid fuel shaping robot, the specific steps of the admittance controller performing local compliance control are as follows:

[0102] When the robotic arm is shaping the solid fuel of the rocket, the six-dimensional force sensor obtains the contact force F with the working surface and sends the contact force F to the admittance controller.

[0103] The admittance controller takes the following form: Among them, M d It is the inertia coefficient matrix, D d It is the damping coefficient matrix, K d It is the stiffness coefficient matrix, F ext The admittance controller can be written as an external force. Form, among which, It is a correction acceleration. It is the correction speed, q e It corrects the joint angle for acceleration. By performing two integrations, the corrected trajectory ΔX of the robot arm under the action of external force can be obtained.

[0104] according to Figure 1 Taking the control block diagram shown as an example, given a reference trajectory (x) in Cartesian space... r y rAfter inverse kinematics transformation to joint space, the joint angles in joint space are transmitted to the robotic arm via a position controller, which sends the control torque τ. The robotic arm then begins shaping the rocket's solid fuel. A six-dimensional force sensor at the robotic arm's end effector transmits the force F detected by the end effector to the admittance controller. Based on the dynamic relationship between force and position, the admittance controller makes an appropriate position correction ΔX to prevent excessive end-effector contact force. The corrected trajectory is then retransmitted to the robotic arm to complete the next movement.

[0105] The inverse kinematics is achieved through the following steps:

[0106] First, establish a coordinate system using the base of the robotic arm as the base coordinate system, and then establish a task coordinate system using the geometric center point of the end effector of the robotic arm as the reference point.

[0107] Using the base coordinate system as the reference coordinate system, given the reference trajectory (x) in Cartesian space. r y r At time k, the reference trajectory is sampled to obtain two reference points. The translation transformation matrix T between the two reference points is then calculated. transl T transl The equation is given by the following formula:

[0108]

[0109] Translation transformation matrix T transl Through inverse kinematics, it is converted into the joint angle q0 in the joint space of the robotic arm.

[0110] The position controller includes a function responsible for calculating the control torque τ. m The system employs a model predictive controller and a PD controller responsible for correcting tracking errors. At time k, a given reference joint angle q0 is compared with the detected actual joint angle q of the robotic arm at the same time. r Together, they are sent to the model predictive controller, which calculates the control torque U between time k and time k+P based on the linearized model of the robotic arm and the reference angles for the next P cycles. k Then only the control torque τ at time k is selected. m After the robotic arm performs a movement, update the actual joint angle q of the robotic arm. r At this point, the updated joint angles and the reference joint angles at time k+1 are again fed to the model predictive controller. The robotic arm iteratively calculates the control torque for the next P cycles, ultimately outputting only the first torque to the robotic arm. Control torque τ m The calculation uses the following formula:

[0111]

[0112] Where Z = 2(Θ)T QΘ+W), f T =2(E T QΘ+Θ T QΘ), and find a set of solutions that satisfy J(U) according to the quadratic programming method. k The smallest U k Select the first control torque U k (1) Send to the robotic arm.

[0113] Control torque U k (1) Send the data to the robotic arm, which then performs the action. Detect the actual joint angle q of the robotic arm. r The actual joint angle q r The difference e between the joint angle and the reference joint angle q0 is sent to the PD controller, which calculates the correction torque τ based on the PD control law for the joint angle deviation. f The corrected torque is calculated using a PD controller as follows:

[0114]

[0115] in, It is the tracking error of the joint angular velocity of the robotic arm joint angle, K. p It is the position gain matrix, which is a 6x6 diagonal matrix for a six-axis robotic arm, K. v It is the velocity gain matrix, which is also a 6x6 diagonal matrix.

[0116] The admittance controller receives the force F from the six-dimensional force sensor at the end of the robotic arm. After passing through the admittance controller, a corrected position ΔX is generated. The equation for calculating the corrected position using the admittance controller is as follows:

[0117]

[0118] The difference in angular acceleration calculated By performing two integrations, the corrected position ΔX can be obtained.

[0119] The embodiments described above provide a detailed explanation of the technical solution of the present invention. It should be understood that the above descriptions are merely specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, additions, or similar substitutions made within the scope of the principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A trajectory tracking and force compliance control method for a rocket solid fuel shaping robot, characterized by, Using a robotic arm as the controlled object, the trajectory of the robotic arm is tracked based on a given reference trajectory, and the contact force between the robotic arm and the solid fuel is locally compliantly controlled, including the following steps: Position control: The reference joint angle information of each joint of the robotic arm is obtained based on the reference trajectory. The joint angles are used to obtain the control torque through a model predictive controller. According to the control torque Control the robotic arm to work and detect the actual joint angles of the robotic arm. Based on the actual joint angle and reference joint angle Error between The joint angles of the robotic arm are controlled in a closed loop using a PD controller. Force control: When the robotic arm comes into contact with the work surface, a force sensor located at the end of the robotic arm detects the contact force F; based on the contact force F, a corrected trajectory is obtained through an admittance controller. The trajectory will be corrected. Compared with reference trajectory By subtracting, a new reference trajectory is obtained. Return to the position control steps.

2. The trajectory tracking and force compliance control method of a rocket solid fuel shaping robot according to claim 1, characterized by, The position control includes the following steps: A dynamic model of the robotic arm is established using Lagrange dynamics and then linearized and discretized; the reference trajectory in Cartesian space is used. The inverse kinematics is transformed into the joint angles in the joint space of the robotic arm; the joint angles in the joint space are then sent to the model prediction controller. The model predictive controller transforms the model predictive control problem into a quadratic optimization problem, which obtains the control torque of the robot arm in the future P periods according to the received joint angle, the linearization and discretization of the dynamic model of the robot arm ; Select the first set of torques to send to the robotic arm, causing the robotic arm to perform a movement; According to the actual joint angle and the reference joint angle between the error, the joint angle is closed-loop controlled by the PD controller; The model predictive controller recalculates the control torque over the next P cycles based on the newly detected actual joint angles. .

3. The trajectory tracking and force compliance control method of a rocket solid fuel shaping robot according to claim 2, characterized by, The process of establishing a dynamic model of the robotic arm using Lagrange dynamics and then linearizing and discretizing it includes the following steps: The kinetic model: ; Linearized and discretized dynamics model: ; in, These are the angle vector, angular velocity vector, and angular acceleration vector of the robotic arm joints, respectively. It is the inertia matrix of the robotic arm. It is a matrix of centrifugal force and Coriolis force. It is the gravity matrix of the robotic arm. It is the joint control torque vector. It is the state variable at time k. These are the state variables at time k+1, representing the joint angles and angular velocities of each joint of the robotic arm. Let be a set of control torques at time k, T be the sampling time, and A and B be the coefficient matrices of the state variables, respectively. It is a constant matrix, as shown below. ， ， 。 4. The trajectory tracking and force compliance control method for the rocket solid fuel shaping robot according to claim 2, characterized in that, The model predictive controller transforms the model predictive control problem into a quadratic optimization problem, as follows: The cumulative error between the predicted and reference values ​​of the trajectory state variables is chosen as the cost function for model predictive control. ,in, It is a state control variable. It is the predicted state for P future control cycles. Q is the reference trajectory, W is the error weighting coefficient, and W is the control output weighting coefficient. ； The model predictive control problem is transformed into a quadratic programming problem. ,in, This indicates the search for the expression under constraints. The minimum value, This is a constraint condition, indicating that the maximum control torque should not exceed the threshold Umax. , Error matrix between predicted trajectory and reference trajectory , R(k) is the state variable at time k, and R(k) is the reference trajectory at time k. It is the coefficient matrix of the state variables. It is the coefficient matrix of the control quantity; ， ； Based on the quadratic programming problem, the control period of P future control cycles can be calculated to achieve the following: smallest Select only the control torque at the first time. = Send it to the robotic arm.

5. The trajectory tracking and force compliance control method for the rocket solid fuel shaping robot according to claim 1, characterized in that, The admittance controller is as follows: ； in, It is the inertia coefficient matrix. It is the damping coefficient matrix. It is the stiffness coefficient matrix. It is the external force received; It is a corrected acceleration. It is a speed correction. It corrects the joint angle for acceleration. By performing two integrations, the corrected trajectory of the robotic arm under the action of external force can be obtained. .

6. A trajectory tracking and force compliance control device for a rocket solid fuel shaping robot, characterized by, include: The position control module is used to obtain reference joint angle information for each joint of the robotic arm based on the reference trajectory. The joint angles are used to obtain the control torque through a model predictive controller. According to the control torque Control the robotic arm to work and detect the actual joint angles of the robotic arm. Based on the actual joint angle and reference joint angle Error between The joint angles of the robotic arm are controlled in a closed loop using a PD controller. The force control module is used to detect the contact force F when the robotic arm comes into contact with the working surface. A force sensor located at the end of the robotic arm detects the contact force F, and a corrected trajectory is obtained through an admittance controller based on the contact force F. The trajectory will be corrected. Compared with reference trajectory By subtracting, a new reference trajectory is obtained. Return to the position control module.

Images