Robot adaptive motion planning method and system based on compliant control

Abstract

The invention provides a robot adaptive motion planning method and system based on compliance control, the method adopts admittance control as a compliance algorithm, the compliance is stronger compared with the traditional rigid control, and different compliance degrees can be selected according to the environment; combining inverse kinematics, trajectory interpolation and environmental parameters to construct an optimal reset trajectory under the constraint of a joint space safety region; by sensing external acting force and dynamically adjusting control parameters, the flexibility of the robot in a complex working environment is improved; a chassis-mechanical arm joint kinematics model is provided, and the problem that the working space of a traditional mechanical arm is limited is solved.

Description

Technical Field

[0001] The invention belongs to the field of robot technology control, and in particular relates to an adaptive motion planning method and system based on a compliant control robot. Background Art

[0002] Robotics has become a key force in promoting intelligent manufacturing and automated production, and has also been widely used in industrial production, medical rehabilitation, home services and other scenarios. However, traditional robot motion planning and control methods often face many challenges in complex and changing working environments. For example, in tasks such as industrial assembly, medical surgery or human-machine collaboration, robots need to have high precision, high flexibility and adaptability to cope with environmental changes, external interference or safe interaction with humans.

[0003] At present, robot motion planning and control mainly rely on rigid control methods. Although such methods can achieve high-precision trajectory tracking, they are prone to impact, vibration, and even damage to equipment in contact tasks or dynamic environments, and are difficult to meet the requirements of compliance and safety. In addition, existing adaptive motion planning technologies are mostly based on preset models or fixed rules, lacking the ability to perceive and respond to dynamic environments in real time, resulting in insufficient adaptability of robots in complex scenarios. Summary of the invention

[0004] In view of the above problems, the present invention proposes an adaptive motion planning method and system based on a compliant control robot. The idea is to dynamically calculate the expected position of the robot arm and generate a motion trajectory based on the perceived external force in combination with a compliant control and path planning method, so that the robot arm can move smoothly and better complete the target task. The present invention aims to solve the following technical problems: (1) Traditional robots use rigid control and have poor compliance; (2) The path planning of the existing technology has insufficient adaptability and lacks real-time perception of the dynamic environment; (3) The lack of coordination between the motion planning and control modules leads to limited overall system performance.

[0005] In a first aspect, the present invention provides an adaptive motion planning method based on a compliant control robot, comprising the following steps:

[0006] S1: Analyze the collected data information, adjust the compliance control parameters and calculate the expected position;

[0007] S2: Generate a preliminary motion trajectory through trajectory interpolation, and use inverse kinematics to calculate the specific joint angles; when there is no solution, use a method of coordinated operation of the robot arm and the chassis to re-solve. Interpolate the final trajectory points to generate the total trajectory.

[0008] S3: Establish an error feedback compensation system: Calculate the required compensation amount in advance based on the system input signal and the known characteristics of the external disturbance.

[0009] S4: Complete the robot arm adaptive path planning task based on the total trajectory.

[0010] S5: Collect historical operation data of the robot arm, calculate important evaluation indicators, and optimize trajectory planning algorithms and compliant control.

[0011] Preferably, step S1 specifically includes:

[0012] S11: Admittance control is used as the compliant control algorithm. The basic formula of the manipulator admittance control is as follows:

[0013]

[0014] Where M d , B d , K d represent the inertia matrix, damping matrix and stiffness matrix respectively; They represent the expected acceleration and the actual acceleration respectively; They represent the expected speed and the actual speed respectively; p d 、p e represent the expected position and the actual position respectively; h d 、h e They represent the expected elastic force screw and the actual elastic force screw in contact with the environment, respectively;

[0015] S12: Select a compliant strategy based on the perceived environmental information:

[0016] When |F e |≥F max When high compliance mode is activated: reduce K d , increase B d , which is manifested as greater resistance;

[0017] When |F e | <F max When low smoothness mode is activated: Increase K d , reduce B d , which is manifested as less resistance;

[0018] Among them, F e F is the value of the force applied by the human on the end handle of the robotic arm detected by the high-precision six-dimensional force sensor. max The threshold of the contact force of the robot arm is set.

[0019] S13: Calculate the expected acceleration, the first time and p d Use the initial value, and then use the updated and p d Perform iterative calculations:

[0020]

[0021] The initial value is the sampling value of the sensor in a static state.

[0022] S14: Set the control period Δt, use the expected acceleration to calculate and update the expected speed:

[0023]

[0024] S15: Calculate and update the expected position using the expected speed:

[0025]

[0026] S16: In each subsequent control cycle Perform integration and quadratic integration to update the expected speed respectively and the desired position p d ,make and p d Gradually converge until the admittance control equation is satisfied.

[0027] Preferably, step S2 specifically includes:

[0028] S21: According to the desired position obtained in the previous step, the trajectory from the known starting point and the target point is planned, and the joint trajectory is generated by using quintic polynomial interpolation for the joint position, velocity and acceleration of the starting point and the end point.

[0029] S22: For each trajectory point, use inverse kinematics to solve the corresponding joint angle:

[0030] For a robotic arm with n degrees of freedom, each joint of the robotic arm is a revolute joint, and the motion equation of the robotic arm is:

[0031] 0 T n = 0 T1(θ1) 1 T2(θ2)… n-1 T n (θ n ) (29)

[0032] in 0 T n Represents the rotation matrix from the 0th to the nth joint;

[0033] According to the motion equation, solve the variables θ1,…θ of each axis joint n .

[0034] S23: The number of generated solutions is M, and each solution is recorded as:

[0035]

[0036] S24: Filter valid solutions based on the range of joint motion angles:

[0037]

[0038] If any joint is out of bounds, then the solution θ is excluded (k) ;

[0039] S25: Define collision function C(θ):

[0040]

[0041] If C(θ (k) )=1, then the solution θ is excluded (k) ;

[0042] S26: Use the weighted Euclidean distance as the objective function to calculate the minimum total energy consumption of each joint movement:

[0043]

[0044] Among them, w i is the joint weight;

[0045] From the remaining valid solution set S = {θ (k)}, select the solution that minimizes the objective function as the optimal solution θ * :

[0046]

[0047] S27: The optimal solution θ for each trajectory point * , a continuous and smooth joint trajectory is generated by quintic polynomial interpolation:

[0048] θ i (t) = a i t 5 +b i t 4 +c i t 3 +d i t 2 +e i t+f i (15)

[0049] Among them, a i 、b i 、c i d i 、e i 、f i are the polynomial coefficients;

[0050] When there is no solution, the target is determined to be unreachable and the chassis is moved:

[0051] Define the radius of the workspace sphere as r and the end position deviation vector Δp:

[0052] Δp=p d -p0 (16)

[0053] Among them, p0 is the initial position.

[0054] When |Δp|>r, the chassis coordinated motion is triggered, generating the desired chassis acceleration:

[0055]

[0056] Among them, K p1 is the proportional gain matrix;

[0057] S28: Convert acceleration to the global coordinate system and integrate the acceleration to obtain the speed command:

[0058]

[0059] When the speed threshold is reached, maintain a constant speed and decelerate when approaching the target point;

[0060] S29: Update the coordinate system and target position coordinates p after the chassis stops moving d , re-plan the trajectory.

[0061] Preferably, the degree of freedom of the robotic arm is 6, and step S22 specifically includes:

[0062] For each trajectory point, use inverse kinematics to solve the corresponding joint angle:

[0063] For a robot with 6 degrees of freedom and each joint is a rotational joint, the motion equation of the robot is:

[0064]

[0065] Where n, o and a are the unit vectors of the new coordinate system, n x 、n y and n z is the projection vector of the n vector in the XYZ coordinate system, o x , o y and z is the projection vector of the o vector in the XYZ coordinate system, a x 、a y and a z is the projection vector of vector a in the XYZ coordinate system; p x , p y and p zis the position coordinate of the origin of the new coordinate system in the XYZ coordinate system; solve the first three axis joint variables:

[0066]

[0067]

[0068] in, θ i is the rotation angle of the i-th joint, a i is the connecting rod length of the ith joint, d i is the offset of the i-th joint;

[0069] Since the joint angles of the rear three axes only affect the orientation of the robot arm coordinate system, the joint variables of the rear three axes can be calculated using the Euler angle solution.

[0070] use The rotation sequence represents the rotation sequence around the x-axis, y-axis, and z-axis.

[0071] For a known attitude rotation matrix R:

[0072]

[0073] Solve for the corresponding Euler angles:

[0074]

[0075] The relationship between the joint variables of the rear three axes is:

[0076]

[0077] Preferably, step S3 specifically includes:

[0078] S31: adjusting the control signal based on feedforward compensation;

[0079] S32: Adjust the control input u in real time according to the current error e(t) PID (t):

[0080]

[0081] S33: Calculate the required compensation amount in advance based on the system input signal r(t) and the known characteristics of the external disturbance d(t):

[0082] u FF (t) = G FF (r(t),d(t)) (20)

[0083] S34: Output according to the compensation amount:

[0084] u(t)=uPID (t)+u FF (t) (21)

[0085] Preferably, step S5 specifically includes:

[0086] S51: Evaluate the robot's performance in the current task from the perspective of task completion rate, execution efficiency, and safety:

[0087] Task completion rate:

[0088]

[0089] Execution efficiency:

[0090]

[0091] Where T is the task duration, τ(t) is the joint torque vector, and α, β are weight coefficients;

[0092] Safety scoring based on force and collision detection:

[0093]

[0094] Among them, F k is the kth contact force, F safe is the safety threshold, λ is the penalty coefficient, and N is the total number of task executions;

[0095] S52: Evaluate the robot's compliant control performance in the current task:

[0096] Contact force tracking error:

[0097]

[0098] F actual is the actual force, F desired It is the power of expectation;

[0099] Dynamic impedance matching:

[0100]

[0101] Where Z(s) is the transfer function of the impedance model, Z actual is the actual impedance transfer function, Z desired is the desired impedance transfer function; is the norm measuring the frequency domain error;

[0102] S53: Based on the feedback data, the compliant control is optimized through adaptive impedance parameter learning, and the adaptive motion planning algorithm is optimized through reinforcement learning to improve the performance of the system;

[0103] S54: Update control parameters, apply new parameters and models, and prepare for the next task.

[0104] In a second aspect, the present invention proposes an adaptive motion planning system based on a compliant control robot, characterized in that it includes:

[0105] The compliance control module adopts an admittance control as a compliance algorithm and iteratively calculates the desired position based on the force information obtained from the human on the robot arm.

[0106] The adaptive path planning module uses a polynomial interpolation method to generate joint trajectories based on the calculated desired position, and then uses an inverse kinematics solution method to calculate and output the specific joint angles. When there is no solution, a method of coordinated operation of the robot arm and chassis is used to approach the target point and complete the task.

[0107] The feedback and error compensation module uses a controller based on feedforward compensation to adjust the control signal. The required compensation amount is calculated in advance based on the known characteristics of the system input signal and external disturbance.

[0108] The optimization learning module is responsible for evaluating the robot's performance in the current task and optimizing the path planning module and the compliant control module.

[0109] The beneficial effects of the present invention are:

[0110] The present invention adopts admittance control as the compliance algorithm, which is more compliant than traditional rigid control, and can select different degrees of compliance according to the different forces applied by people, reflecting better human-computer interaction; an adaptive trajectory planning method is proposed, which combines inverse kinematics, trajectory interpolation and environmental parameters to construct the optimal reset trajectory under the constraints of the joint space safety area; by sensing the external force and dynamically adjusting the control parameters, the flexibility of the robot in a complex working environment is improved; a chassis-manipulator joint kinematic model is proposed to solve the problem of limited working space of traditional manipulators. BRIEF DESCRIPTION OF THE DRAWINGS

[0111] Figure 1 It is a flow chart of an adaptive motion planning method for a compliant control robot.

[0112] Figure 2 It is a block diagram of an adaptive motion planning system based on a compliant control robot. DETAILED DESCRIPTION

[0113] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0114] In a real case, consider a robot that consists of a 6-DOF arm and a chassis. A force sensor is installed at the end of the arm. The robot system is initialized and the sensor collects data.

[0115] As attached Figure 1 As shown, a method for adaptive motion planning based on a compliant control robot includes the following steps:

[0116] S1: Analyze the collected data information, adjust the compliance control parameters and calculate the expected position;

[0117] Furthermore, step S1 specifically includes:

[0118] S11: Admittance control is used as the compliant control algorithm. The basic formula of the manipulator admittance control is as follows:

[0119]

[0120] Where M d , B d , K d represent the inertia matrix, damping matrix and stiffness matrix respectively; They represent the expected acceleration and the actual acceleration respectively; They represent the expected speed and the actual speed respectively; p d 、p e represent the expected position and the actual position respectively; h d 、h e They represent the expected elastic force screw and the actual elastic force screw in contact with the environment, respectively;

[0121] S12: Select a compliant strategy based on the perceived environmental information:

[0122] When |F e |≥F max When high compliance mode is activated: reduce K d , increase B d , which is manifested as greater resistance;

[0123] When |F e | <F max When low smoothness mode is activated: Increase K d , reduce B d , which is manifested as less resistance;

[0124] Among them, F e F is the value of the force applied by the human on the end handle of the robotic arm detected by the high-precision six-dimensional force sensor. max The threshold of the contact force of the robot arm is set.

[0125] S13: Calculate the expected acceleration, the first time and p d Use the initial value, and then use the updated and p d Perform iterative calculations:

[0126]

[0127] The initial value is the sampling value of the sensor in a static state.

[0128] S14: Set the control period Δt, use the expected acceleration to calculate and update the expected speed:

[0129]

[0130] S15: Calculate and update the expected position using the expected speed:

[0131]

[0132] S16: In each subsequent control cycle Perform integration and quadratic integration to update the expected speed respectively and the desired position p d ,make and p d Gradually converge until the admittance control equation is satisfied.

[0133] S2: Generate a preliminary motion trajectory through trajectory interpolation, and use inverse kinematics to calculate the specific joint angles; when there is no solution, use a method of coordinated operation of the robot arm and the chassis to re-solve. Interpolate the final trajectory points to generate the total trajectory.

[0134] Furthermore, step S2 specifically includes:

[0135] S21: According to the desired position obtained in the previous step, the trajectory from the known starting point and the target point is planned, and the joint position, velocity and acceleration of the starting point and the end point are interpolated by quintic polynomial to generate the preliminary joint trajectory.

[0136] S22: For each trajectory point, use inverse kinematics to solve the corresponding joint angle:

[0137] For the 6-DOF robotic arm in this example, each joint of the robotic arm is a revolute joint, and the motion equation of the robotic arm is:

[0138]

[0139] Solve for the first three axis joint variables:

[0140]

[0141] in, θ i is the rotation angle of the i-th joint, a i is the connecting rod length of the ith joint, d i is the offset of the i-th joint.

[0142] Since the joint angles of the rear three axes only affect the orientation of the robot arm coordinate system, the joint variables of the rear three axes can be calculated using the Euler angle solution.

[0143] use The rotation sequence represents the rotation sequence around the x-axis, y-axis, and z-axis.

[0144] For a known attitude rotation matrix R:

[0145]

[0146] The corresponding Euler angles can be solved:

[0147]

[0148] The relationship between the joint variables of the rear three axes is:

[0149]

[0150] S23: The number of generated solutions is M, and each solution is recorded as:

[0151]

[0152] S24: Filter valid solutions based on the range of joint motion angles:

[0153]

[0154] in, and They are the minimum and maximum values ​​of the set movement angle respectively.

[0155] If any joint is out of bounds, then the solution θ is excluded (k) ;

[0156] S25: Define collision function C(θ):

[0157]

[0158] If C(θ(k) )=1, then the solution θ is excluded (k) ;

[0159] S26: Use the weighted Euclidean distance as the objective function to calculate the minimum total energy consumption of each joint movement:

[0160]

[0161] Among them, w i is the joint weight, is the current joint angle value of the robot arm;

[0162] From the remaining valid solution set S = {θ (k)}, select the solution that minimizes the objective function as the optimal solution θ * :

[0163]

[0164] S27: The optimal solution θ for each trajectory point * , a continuous and smooth joint trajectory is generated by quintic polynomial interpolation:

[0165] θ i (t) = a i t 5 +b i t 4 +c i t 3 +d i t 2 +e i t+f i (15)

[0166] Among them, a i 、b i 、c i ,d i 、e i 、f i is the polynomial coefficient of the i-th joint;

[0167] When there is no solution, the target is determined to be unreachable and the chassis is moved:

[0168] Define the radius of the workspace sphere as r and the end position deviation vector Δp:

[0169] Δp=p d -p0 (16)

[0170] Among them, p0 is the initial position.

[0171] When |Δp|>r, the chassis coordinated motion is triggered to generate the desired chassis acceleration

[0172]

[0173] Among them, K p1 is the proportional gain matrix;

[0174] S28: Convert acceleration to the global coordinate system and integrate the acceleration to obtain the speed command:

[0175]

[0176] When the speed threshold is reached, maintain a constant speed Slow down when approaching the target point;

[0177] S29: Update the coordinate system and target position coordinates p after the chassis stops moving d , re-plan the trajectory.

[0178] S3: Establish an error feedback compensation system: Calculate the required compensation amount in advance based on the system input signal and the known characteristics of the external disturbance.

[0179] Furthermore, step S3 specifically includes:

[0180] S31: using a PID controller based on feedforward compensation to adjust the control signal;

[0181] S32: PID controller adjusts the control input u in real time according to the current error e(t) PID (t):

[0182]

[0183] S33: The feedforward controller calculates the required compensation amount in advance based on the system input signal r(t) and the known characteristics of the external disturbance d(t):

[0184] u FF (t) = G FF (r(t),d(t)) (20)

[0185] S34: The controller outputs according to the compensation amount:

[0186] u(t)=u PID (t)+u FF (t) (21)

[0187] S4: Complete the robot arm adaptive path planning task based on the total trajectory.

[0188] S41: Drive motors to execute joint angles and chassis movement.

[0189] S42: When the following conditions are met, the system enters the task end state.

[0190] The robot reaches the specified position:

[0191] |p e -p d |≤1mm (22)

[0192] time out:

[0193] t current -t start >60s (23)

[0194] If a collision or other abnormal situation occurs, the system will stop immediately and enter the mission end state.

[0195] S43: Reset the control system to the initial state and prepare for the next task.

[0196] S5: Collect historical operation data of the robot arm, calculate important evaluation indicators, and optimize trajectory planning algorithms and compliant control.

[0197] Furthermore, step S5 specifically includes:

[0198] S51: Evaluate the robot's performance in the current task from the perspective of task completion rate, execution efficiency, and safety:

[0199] Task completion rate:

[0200]

[0201] Execution efficiency:

[0202]

[0203] Where T is the task duration, τ(t) is the joint torque vector, and α, β are weight coefficients;

[0204] Safety scoring based on force and collision detection:

[0205]

[0206] Among them, F k is the kth contact force, F safe is the safety threshold, λ is the penalty coefficient;

[0207] S52: Evaluate the robot's compliant control performance in the current task:

[0208] Contact force tracking error:

[0209]

[0210] Dynamic impedance matching:

[0211]

[0212] Where Z(s) is the transfer function of the impedance model, is the norm measuring the frequency domain error;

[0213] S53: Based on the feedback data, the compliant control is optimized through adaptive impedance parameter learning, and the adaptive motion planning algorithm is optimized through reinforcement learning to improve the performance of the system;

[0214] S54: Update control parameters, apply new parameters and models, and prepare for the next task.

[0215] As attached Figure 2 As shown, an adaptive motion planning system based on a compliant control robot is characterized by comprising:

[0216] The compliance control module adopts an admittance control as a compliance algorithm and iteratively calculates the desired position based on the force information obtained from the human on the robot arm.

[0217] The adaptive path planning module uses a polynomial interpolation method to generate joint trajectories based on the calculated desired position, and then uses an inverse kinematics solution method to calculate and output the specific joint angles. When there is no solution, a method of coordinated operation of the robot arm and chassis is used to approach the target point and complete the task.

[0218] The feedback and error compensation module uses a controller based on feedforward compensation to adjust the control signal. The required compensation amount is calculated in advance based on the known characteristics of the system input signal and external disturbance.

[0219] The optimization learning module is responsible for evaluating the robot's performance in the current task and optimizing the path planning module and the compliant control module.

[0220] The above is only a preferred embodiment of the present invention, and the protection scope of the present invention is not limited to the above embodiments. All technical solutions under the concept of the present invention belong to the protection scope of the present invention. It should be pointed out that for ordinary technicians in this technical field, some improvements and modifications without departing from the principle of the present invention should also be regarded as the protection scope of the present invention.

Claims

1. A method for adaptive motion planning based on a compliant control robot, comprising the following steps: S1: Analyze the collected data information, adjust the compliance control parameters and calculate the expected position; S2: Generate a preliminary motion trajectory through trajectory interpolation, and use inverse kinematics to calculate the specific joint angles; when there is no solution, use a method of coordinated operation of the robot arm and the chassis to re-solve. Interpolate the final trajectory points to generate the total trajectory. S3: Establish an error feedback compensation system: Calculate the required compensation amount in advance based on the system input signal and the known characteristics of the external disturbance. S4: Complete the robot arm adaptive path planning task based on the total trajectory. S5: Collect historical operation data of the robot arm, calculate important evaluation indicators, and optimize trajectory planning algorithms and compliant control.

2. The method for adaptive motion planning based on a compliant control robot according to claim 1, characterized in that: The S1 step specifically includes: S11: Admittance control is used as the compliant control algorithm. The basic formula of the manipulator admittance control is as follows: Where M d , B d , K d represent the inertia matrix, damping matrix and stiffness matrix respectively; They represent the expected acceleration and the actual acceleration respectively; They represent the expected speed and the actual speed respectively; p d 、p e They represent the expected position and the actual position respectively; h d 、h e They represent the expected elastic force screw and the actual elastic force screw in contact with the environment, respectively; S12: Select a compliant strategy based on the perceived environmental information: When |F e |≥F max When high compliance mode is activated: reduce K d , increase B d , which is manifested as greater resistance; When |F e | <F max When low smoothness mode is activated: Increase K d , reduce B d , which is manifested as less resistance; Among them, F e F is the value of the force applied by the human on the end handle of the robotic arm detected by the high-precision six-dimensional force sensor. max The threshold of the contact force of the robot arm is set. S13: Calculate the expected acceleration, the first time and p d Use the initial value, and then use the updated and p d Perform iterative calculations: S14: Set the control period Δt, use the expected acceleration to calculate and update the expected speed: S15: Calculate and update the expected position using the expected speed: S16: In each subsequent control cycle Perform integration and quadratic integration to update the expected speed respectively and the desired position p d ,make and p d Gradually converge until the admittance control equation is satisfied.

3. The method for adaptive motion planning based on a compliant control robot according to claim 1, characterized in that: The S2 step specifically includes: S21: According to the desired position obtained in the previous step, the trajectory from the known starting point and the target point is planned, and the joint trajectory is generated by using quintic polynomial interpolation for the joint position, velocity and acceleration of the starting point and the end point. S22: For each trajectory point, use inverse kinematics to solve the corresponding joint angle: For a robotic arm with n degrees of freedom, each joint of the robotic arm is a revolute joint, and the motion equation of the robotic arm is: 0 T n = 0 T1(θ1) 1 T2(θ2)… n-1 T n (i n ) (29) in 0 T n Represents the rotation matrix from the 0th to the nth joint; According to the motion equation, solve the variables θ1,…θ of each axis joint n ; S23: The number of generated solutions is M, and each solution is recorded as: S24: Filter valid solutions based on the range of joint motion angles: If any joint is out of bounds, then the solution θ is excluded (k) ; S25: Define collision function C(θ): If C(θ (k) )=1, then the solution θ is excluded (k) ; S26: Use the weighted Euclidean distance as the objective function to calculate the minimum total energy consumption of each joint movement: Among them, w i is the joint weight; From the remaining valid solution set S = {θ (k) }, select the solution that minimizes the objective function as the optimal solution θ * : S27: The optimal solution θ for each trajectory point * , a continuous and smooth joint trajectory is generated by quintic polynomial interpolation: θ i (t)=a i 5 +b i t 4 +c i t 3 +d i t 2 +e i t+f i (15) Among them, a i , b i 、c i d i 、e i 、f i are the polynomial coefficients; When there is no solution, the target is determined to be unreachable and the chassis is moved: Define the radius of the workspace sphere as r and the end position deviation vector Δp: Δp=p d -p0 (16) Among them, p0 is the initial position; When |Δp|>r, the chassis coordinated motion is triggered, generating the desired chassis acceleration: Among them, K p1 is the proportional gain matrix; S28: Convert acceleration to the global coordinate system and integrate the acceleration to obtain the speed command: When the speed threshold is reached, maintain a constant speed and decelerate when approaching the target point; S29: Update the coordinate system and target position coordinates p after the chassis stops moving d , re-plan the trajectory.

4. The method for adaptive motion planning based on a compliant control robot according to claim 3, characterized in that: The robot has 6 degrees of freedom, and step S22 specifically includes: For each trajectory point, use inverse kinematics to solve the corresponding joint angle: Each joint of the robotic arm is a revolute joint, and the motion equation of the robotic arm is: Where n, o and a are the unit vectors of the new coordinate system, n x 、n y and n z is the projection vector of the n vector in the XYZ coordinate system, o x , o y and z is the projection vector of the o vector in the XYZ coordinate system, a x 、a y and a z is the projection vector of vector a in the XYZ coordinate system; p x , p y and p z is the position coordinate of the origin of the new coordinate system in the XYZ coordinate system; solve the first three axis joint variables: in, θ i is the rotation angle of the i-th joint, a i is the connecting rod length of the ith joint, d i is the offset of the i-th joint; use The rotation sequence represents the rotation sequence around the x-axis, y-axis, and z-axis; For a known attitude rotation matrix R: Solve for the corresponding Euler angles: The relationship between the joint variables of the rear three axes is:

5. The method for adaptive motion planning based on a compliant control robot according to claim 1, characterized in that: The S3 step specifically includes: S31: adjusting the control signal based on feedforward compensation; S32: Adjust the control input u in real time according to the current error e(t) PID (t): S33: Calculate the required compensation amount in advance based on the system input signal r(t) and the known characteristics of the external disturbance d(t): u FF (t)=G FF (r(t),d(t)) (20) S34: Output according to the compensation amount: u(t)=u PID (t)+u FF (t) (21)。 6. The method for adaptive motion planning based on a compliant control robot according to claim 1, characterized in that: Step S5 specifically includes: S51: Evaluate the robot's performance in the current task from the perspective of task completion rate, execution efficiency, and safety: Task completion rate: Execution efficiency: Where T is the task duration, τ(t) is the joint torque vector, and α, β are weight coefficients; Safety scoring based on force and collision detection: Among them, F k is the kth contact force, F safe is the safety threshold, λ is the penalty coefficient, and N is the total number of task executions; S52: Evaluate the robot's compliant control performance in the current task: Contact force tracking error: F actual is the actual force, F desired It is the power of expectation; Dynamic impedance matching: Where Z(s) is the transfer function of the impedance model, Z actual is the actual impedance transfer function, Z desired is the desired impedance transfer function; is the norm measuring the frequency domain error; S53: Based on the feedback data, the compliant control is optimized through adaptive impedance parameter learning, and the adaptive motion planning algorithm is optimized through reinforcement learning to improve the performance of the system; S54: Update control parameters, apply new parameters and models, and prepare for the next task.

7. An adaptive motion planning system based on a compliant control robot, characterized in that: include: The compliance control module adopts an admittance control as a compliance algorithm and iteratively calculates the desired position based on the force information obtained from the human on the robot arm. The adaptive path planning module uses a polynomial interpolation method to generate joint trajectories based on the calculated desired position, and then uses an inverse kinematics solution method to calculate and output the specific joint angles. When there is no solution, a method of coordinated operation of the robot arm and chassis is used to approach the target point and complete the task. The feedback and error compensation module uses a controller based on feedforward compensation to adjust the control signal. The required compensation amount is calculated in advance based on the known characteristics of the system input signal and external disturbance. The optimization learning module is responsible for evaluating the robot's performance in the current task and optimizing the path planning module and the compliant control module.

Images