A compliant control system and method based on terminal force estimation

Abstract

The present invention discloses a compliant control system based on terminal force estimation, which relates to the field of robot control technology and includes a terminal force estimation module, a trajectory regression module, a compliant trajectory adjustment module, an inverse kinematics solution module, a radial basis neural network control module, and an anti-interruption module. The present invention also discloses a compliant control method based on terminal force estimation, which includes S100, preparation; S200, calculation of the estimated amount of terminal force; S300, generation of the compliant trajectory speed; S400, calculation of the ideal joint angle and the ideal joint speed; S500, generation of the robot joint active force signal; S600, anti-interruption processing. The present invention ensures that the robot reaches the equilibrium point when encountering interference during the execution of the robot task.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a compliant control system and method based on terminal force estimation. Background Art

[0002] The new generation of robots should be able to interact safely and stably with their environment. This ability to achieve stable interaction is often referred to as compliance. Compliance is typically implemented in two ways: active compliance through the robot's internal algorithms, and passive compliance through the robot's mechanical mechanisms (springs, variable impedance mechanisms, etc.). Compliance control methods combine the concepts of active compliance with control and have been widely used. However, conventional compliance control methods have some practical problems:

[0003] 1. Compliant control methods struggle to maintain task execution even under strong external disturbances (such as those from human interference). For example, if a robot is performing a window-wiping task 1 meter above the ground and the operator lowers it to 0.5 meters, without additional algorithmic compensation, the robot will perform a meaningless window-wiping maneuver between 0.5 and 1.0 meters above the ground. The fundamental reason for this is that classic active compliance algorithms at the control level are designed based on control errors. This tolerance of control errors prevents the robot from accurately tracking large errors.

[0004] 2. Compliant control methods usually require the use of end-of-line force sensors. However, for tasks such as remote minimally invasive robotic surgery, large end-of-line force sensors and other components cannot be installed on the end of the robot, making compliant control methods unsuitable.

[0005] 3. The compliant control method requires obtaining the environmental stiffness parameters. The essence of compliant control is to simulate specific physical properties through algorithms in the process of controlling the robot's interaction with the external environment. This virtual physical property can only work well when it is consistent with the physical properties of the actual object being contacted. However, in the actual operating environment, there are a large number of situations where the physical properties are unknown. In particular, in the robot grinding scenario, due to the existence of certain deformation errors in the processed object and the fact that the processed object is an irregular object with a difficult-to-describe shape, the robot is required to adjust the contact strength in real time while following the trajectory. However, conventional compliant control methods can only be used to grind workpieces of a certain fixed material. They cannot process workpieces composed of composite materials or some unknown materials, and there are considerable limitations.

[0006] The patent "Robot Admittance Compliant Control System and Method" (application number: CN201911006791.8) discloses a robot admittance compliance control system and method. This system detects the end force applied to the robot, as well as the robot's motion speed and acceleration. These end force, motion speed, and acceleration are then input into a trained neural network system model to output admittance parameters, thereby achieving compliant control. However, this patent requires prior training to obtain the admittance parameters (similar parameters are often only applicable when operating on the same object as the training situation). Furthermore, it cannot effectively replan tasks for large disturbances. Therefore, its versatility needs to be improved.

[0007] Therefore, those skilled in the art are committed to developing a compliant control system and method based on terminal force estimation. Summary of the Invention

[0008] In view of the above-mentioned defects of the prior art, the technical problem to be solved by the present invention is how to ensure the execution of the operation task and improve the compliance of the robot in the case of large disturbances and without an end force sensor.

[0009] The inventors analyzed that while traditional compliant control can handle small disturbances during task execution, such as collisions, improving interaction safety and giving the robot a certain degree of end-force control capability, it cannot achieve ideal results when dealing with subjective disturbances from humans or when there is no end-force sensor.

[0010] The inventors investigated the difficulty of applying this control method to the robot due to large errors in end-force estimation. They replaced the end-force sensor with an end-force estimation module and used a taught trajectory regression method to generate an original trajectory velocity with an equilibrium point. This original trajectory velocity was then adjusted to generate a compliant trajectory velocity. The compliant trajectory was then resolved into joint angles and joint velocities. A neural network control module was then used to control the robot's motion, achieving both compliance and convergence of the trajectory to an equilibrium point. The inventors defined an equilibrium point as a stable point to which one can return after being disturbed.

[0011] In one embodiment of the present invention, a compliant control system based on terminal force estimation is provided, comprising:

[0012] The end force estimation module calculates and obtains an estimate of the end force in response to the actual joint angle and the actual joint velocity from the robot joint encoder;

[0013] Trajectory regression module collects relevant teaching trajectories and generates original trajectory speed;

[0014] A compliant trajectory adjustment module, in response to the estimated amount of the end force and the original trajectory velocity, adds an adjustment term for achieving trajectory compliant replanning to generate a compliant trajectory velocity;

[0015] The inverse kinematics solver module generates compliant trajectory path points by integration in response to the compliant trajectory velocity, and calculates the ideal joint angles and ideal joint velocities of the robot joints.

[0016] A radial basis function neural network control module, in response to the ideal joint angle and the ideal joint velocity, the actual joint angle and the actual joint velocity, uses a radial basis function neural network to fit the model parameters, generates the robot joint active force signal, and sends it to the robot drive module to control the robot movement;

[0017] The anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the equilibrium point of the original trajectory;

[0018] The terminal force estimation module, the compliant trajectory adjustment module, the inverse kinematics solution module, and the radial basis neural network control module are connected in series and communicatively in sequence, and the trajectory regression module and the anti-interruption module are respectively communicatively connected to the compliant trajectory adjustment module; in response to the actual joint angle and the actual joint speed from the robot joint encoder, the terminal force estimation module calculates the estimated amount of the terminal force, and the trajectory regression module generates the original trajectory speed according to the relevant teaching trajectory; in response to the estimated amount of the terminal force and the original trajectory speed, the compliant trajectory adjustment module generates the compliant trajectory speed; the inverse kinematics solution module calculates the ideal joint angle and the ideal joint speed of the robot joint movement according to the compliant trajectory speed; the radial basis neural network control module generates the robot joint active force signal according to the ideal joint angle and the ideal joint speed, the actual joint angle and the actual joint speed, and sends it to the robot's drive module to control the robot movement; the anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the balance point of the original trajectory.

[0019] Optionally, in the compliant control system based on end force estimation in the above embodiment, the end force estimation module uses generalized momentum to describe the end force signal, multiplies the inertia matrix by the joint angle as the generalized momentum, constructs a state observer in combination with the classical robot motion model, and uses Kalman filtering to reduce the error to obtain an estimate of the end force signal.

[0020] Optionally, in the compliant control system based on terminal force estimation in any of the above embodiments, the original trajectory velocity is generated according to the teaching trajectory regression method, and the teaching trajectory data is fitted using a Gaussian mixture model and obtained through Bayesian reasoning.

[0021] Optionally, in the compliant control system based on terminal force estimation in any of the above embodiments, the number of layers of the Gaussian mixture model is preferably 4.

[0022] Based on the compliant control system based on end force estimation, the inventors studied the compliant control method. In response to the actual joint angle and actual joint velocity from the robot joint encoder, the end force estimation module calculates the estimated amount of the end force, and the trajectory regression module generates the original trajectory velocity according to the relevant teaching trajectory; in response to the estimated amount of the end force and the original trajectory velocity, the compliant trajectory adjustment module generates the compliant trajectory velocity; the inverse kinematics solution module calculates the ideal joint angle and ideal joint velocity of the robot joint movement according to the compliant trajectory velocity; the radial basis neural network control module outputs the active joint force according to the ideal joint angle and ideal joint velocity, the actual joint angle and actual joint velocity, and controls the robot movement; the anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the equilibrium point of the original trajectory.

[0023] Based on any of the above embodiments, another embodiment of the present invention provides a compliance control method based on terminal force estimation, comprising the following steps:

[0024] S100, preparation work, the trajectory regression module collects the teaching trajectory and obtains the original trajectory speed;

[0025] S200, calculate the estimated amount of end force In response to the actual joint angle and actual joint velocity from the robot joint encoder, the end force estimation module calculates the estimated end force ;

[0026] S300, generate smooth trajectory speed Using the estimated value of the terminal force output by the terminal force estimation module and the original trajectory speed output by the trajectory regression module, the flexible trajectory adjustment module adds an adjustment item for achieving trajectory flexible replanning to generate a flexible trajectory speed ;

[0027] S400, calculating the ideal joint angle and the ideal joint velocity. In response to the compliant trajectory velocity, the inverse kinematics solution module generates compliant trajectory path points through integration, and calculates the ideal joint angle and the ideal joint velocity of the robot joint movement.

[0028] S500, generating active force signals for robot joints. In response to the ideal joint angles and ideal joint velocities, the actual joint angles and actual joint velocities, the radial basis function neural network control module uses the radial basis function neural network to fit model parameters, generates active force signals for robot joints, and sends the signals to the robot drive module to control the robot motion.

[0029] S600, anti-interruption processing: During the robot movement, when the balance point of the original trajectory is not reached, the anti-interruption module uses the anti-interruption algorithm to ensure that the robot reaches the balance point of the original trajectory.

[0030] Optionally, in the compliance control method based on terminal force estimation in the above embodiment, step S100 includes:

[0031] S110, specify the starting point and end point of the teaching trajectory;

[0032] S120, control the robot end to move to the end point, use the teaching function, and manually control the robot end to move to the end point;

[0033] S130, generating original trajectory speed , using the Gaussian mixture model regression algorithm to extract the original trajectory velocity from the teaching trajectory .

[0034] Furthermore, in the compliant control method based on terminal force estimation in the above embodiment, the number of layers of the Gaussian mixture model is preferably 4.

[0035] Optionally, in the compliance control method based on terminal force estimation in any of the above embodiments, step S300 includes:

[0036] S310, setting the maximum force threshold , maximum force threshold It is the upper limit of the end force during the movement or the tracking value of the end force during the movement;

[0037] S320, generating an adjustment ratio using the maximum force threshold , which reflects the ratio of the estimated end force to the maximum force threshold, is used to modify the components of the original trajectory velocity in each direction. The formula is as follows:

[0038] (1)

[0039] in, Indicates the dimension of the vector, which is 3 in two-dimensional space and 6 in three-dimensional space; i is a positive integer from 1 to D;

[0040] S330, define the feature space, the formula is as follows:

[0041] (2) (3)

[0042] in, is the unit vector in the direction of the estimated force at the end, For the unit vector values, For The feature space generated for the first basis;

[0043] S340, define the projection value adjustment method, the formula is as follows:

[0044] (4)

[0045] Among them, the eigenvalue matrix is the adjustment method of the projection of the original trajectory velocity in different direction vectors, is the degree of adjustment in the i-th direction, and the formula is as follows:

[0046] (5)

[0047] in, is the effect switching item, the formula is as follows:

[0048] (6)

[0049] in, , , For effect switching parameters, obtained through debugging in actual use. and When it is 0, the maximum force threshold is the upper limit of the estimated end force. and , the estimated amount of end force can be realized to track the maximum force threshold;

[0050] S350. Generate a smooth trajectory speed using an adjustment item. The adjustment item formula is as follows:

[0051] (7)

[0052] Adjustment Multiplying the original trajectory speed to get the smooth trajectory speed In order to compensate for the error in the estimated end force, When the adjustment item takes effect, when it is less than the minimum force threshold hour, Equal to the identity matrix , and finally returns to the smooth trajectory speed, the formula is as follows:

[0053] (8)

[0054] Because of mathematical calculations, the adjustment Unexpected equilibrium points may be introduced. The mathematical description of unexpected equilibrium points is as follows:

[0055] (9)

[0056] The original trajectory speed is Project upward and modify the size of each projection value according to the eigenvalue matrix. The formula is as follows:

[0057] (10)

[0058] in, is the projection value of the compliant trajectory velocity in the force direction, which is relative to the projection value of the original trajectory velocity. Added tuning parameters.

[0059] Optionally, in the compliance control method based on terminal force estimation in any of the above embodiments, step S400 includes:

[0060] S410, in response to the compliant trajectory speed, the inverse kinematics solution module generates compliant trajectory path points by integration;

[0061] S420: Generate ideal joint angles and ideal joint velocities by inverse kinematics calculation of the compliant trajectory path points and the compliant trajectory velocities.

[0062] Optionally, in the compliance control method based on terminal force estimation in any of the above embodiments, step S600 includes:

[0063] S610, determine whether the equilibrium point of the original trajectory is reached. , it is considered that the equilibrium point of the original trajectory has been reached, and step S630 is executed; otherwise, step S620 is executed;

[0064] S620, determine whether to reach the unexpected equilibrium point, when and When it is considered that an unexpected equilibrium point has been reached, the anti-interruption module uses the anti-interruption algorithm to add disturbances to the smooth trajectory speed. , generating a smooth trajectory velocity with disturbance , the formula is as follows:

[0065] (11)

[0066] Wherein, j is a positive integer from 2 to D; return to step S610;

[0067] S630: The compliant control process is completed and the robot reaches the equilibrium point of the original trajectory.

[0068] Furthermore, in the compliant control method based on terminal force estimation in the above embodiment, the disturbance The preferred value is 0.1.

[0069] This invention addresses three shortcomings of traditional compliance control methods: 1. Inability to withstand large human disturbances; 2. Inability to be applied to robots without end-point force sensors; and 3. Ineffectiveness when the properties of the contacting object are unknown. This invention can be applied to different scenarios to achieve different compliance effects. For example, in polishing scenarios, the contact strength can be set to track a fixed intensity, or in remote minimally invasive surgery scenarios, the contact strength can be limited to an upper limit. This ensures that the robot reaches equilibrium even when encountering disturbances during task execution.

[0070] The concept, specific structure and technical effects of the present invention will be further described below in conjunction with the accompanying drawings to fully understand the purpose, characteristics and effects of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0071] Figure 1 is a schematic structural diagram illustrating a compliant control system based on terminal force estimation according to an exemplary embodiment;

[0072] Figure 2 is a flow chart illustrating a compliance control method based on terminal force estimation according to an exemplary embodiment. DETAILED DESCRIPTION

[0073] The following describes several preferred embodiments of the present invention with reference to the accompanying drawings to make its technical content clearer and easier to understand. The present invention can be embodied in many different forms of embodiments, and the scope of protection of the present invention is not limited to the embodiments mentioned herein.

[0074] In the drawings, components with identical structures are denoted by the same reference numerals, and components with similar structures or functions are denoted by similar reference numerals. The size and thickness of each component shown in the drawings are arbitrary and are not limited by the present invention. To enhance clarity, the thickness of components in some places in the drawings is schematically exaggerated.

[0075] The inventors designed a compliant control system based on terminal force estimation, such as Figure 1 As shown, including:

[0076] The end-force estimation module responds to the actual joint angles and actual joint velocities from the robot's joint encoders. It uses generalized momentum to describe the end-force signal, multiplies the inertia matrix by the joint angle as the generalized momentum, builds a state observer based on the classic robot motion model, and uses Kalman filtering to reduce errors to obtain an estimate of the end-force signal.

[0077] The trajectory regression module collects relevant teaching trajectories and generates the original trajectory speed. Based on the teaching trajectory regression method, the teaching trajectory data is fitted using a Gaussian mixture model and obtained through Bayesian inference. The number of layers of the Gaussian mixture model is preferably 4.

[0078] A compliant trajectory adjustment module, in response to the estimated amount of the end force and the original trajectory velocity, adds an adjustment term for achieving trajectory compliant replanning to generate a compliant trajectory velocity;

[0079] The inverse kinematics solver module generates compliant trajectory path points by integration in response to the compliant trajectory velocity, and calculates the ideal joint angles and ideal joint velocities of the robot joints.

[0080] A radial basis function neural network control module, in response to the ideal joint angle and the ideal joint velocity, the actual joint angle and the actual joint velocity, uses a radial basis function neural network to fit the model parameters, generates the robot joint active force signal, and sends it to the robot drive module to control the robot movement;

[0081] The anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the equilibrium point of the original trajectory;

[0082] The terminal force estimation module, the compliant trajectory adjustment module, the inverse kinematics solution module, and the radial basis neural network control module are connected in series and communicatively in sequence, and the trajectory regression module and the anti-interruption module are respectively communicatively connected to the compliant trajectory adjustment module; in response to the actual joint angle and the actual joint speed from the robot joint encoder, the terminal force estimation module calculates the estimated amount of the terminal force, and the trajectory regression module generates the original trajectory speed according to the relevant teaching trajectory; in response to the estimated amount of the terminal force and the original trajectory speed, the compliant trajectory adjustment module generates the compliant trajectory speed; the inverse kinematics solution module calculates the ideal joint angle and the ideal joint speed of the robot joint movement according to the compliant trajectory speed; the radial basis neural network control module generates the robot joint active force signal according to the ideal joint angle and the ideal joint speed, the actual joint angle and the actual joint speed, and sends it to the robot's drive module to control the robot movement; the anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the balance point of the original trajectory.

[0083] Based on the above embodiments, the inventors provide a compliant control method based on terminal force estimation, such as Figure 2 As shown, the following steps are included:

[0084] S100, preparation work, the trajectory regression module collects the teaching trajectory and obtains the original trajectory speed; specifically includes:

[0085] S110, specify the starting point and end point of the teaching trajectory;

[0086] S120, control the robot end to move to the end point, use the teaching function, and manually control the robot end to move to the end point;

[0087] S130, generating original trajectory speed , using the Gaussian mixture model regression algorithm to extract the original trajectory velocity from the teaching trajectory , the number of layers of the Gaussian mixture model is preferably 4.

[0088] S200, calculate the estimated amount of end force In response to the actual joint angle and actual joint velocity from the robot joint encoder, the end force estimation module calculates the estimated end force ;

[0089] S300, generate smooth trajectory speed Using the estimated value of the terminal force output by the terminal force estimation module and the original trajectory speed output by the trajectory regression module, the flexible trajectory adjustment module adds an adjustment item for achieving trajectory flexible replanning to generate a flexible trajectory speed ; Specifically include:

[0090] S310, setting the maximum force threshold , maximum force threshold It is the upper limit of the end force during the movement or the tracking value of the end force during the movement;

[0091] S320, generating an adjustment ratio using the maximum force threshold , which reflects the ratio of the estimated end force to the maximum force threshold, is used to modify the components of the original trajectory velocity in each direction. The formula is as follows:

[0092] (1)

[0093] in, Indicates the dimension of the vector, which is 3 in two-dimensional space and 6 in three-dimensional space; i is a positive integer from 1 to D;

[0094] S330, define the feature space, the formula is as follows:

[0095] (2) (3)

[0096] in, is the unit vector in the direction of the estimated force at the end, For the unit vector values, For The feature space generated for the first basis;

[0097] S340, define the projection value adjustment method, the formula is as follows:

[0098] (4)

[0099] Among them, the eigenvalue matrix is the adjustment method of the projection of the original trajectory velocity in different direction vectors, is the degree of adjustment in the i-th direction, and the formula is as follows:

[0100] (5)

[0101] in, is the effect switching item, the formula is as follows:

[0102] (6)

[0103] in, , , For effect switching parameters, obtained through debugging in actual use. and When it is 0, the maximum force threshold is the upper limit of the estimated end force. and , the estimated amount of end force can be realized to track the maximum force threshold;

[0104] S350. Generate a smooth trajectory speed using an adjustment item. The adjustment item formula is as follows:

[0105] (7)

[0106] Adjustment Multiplying the original trajectory speed to get the smooth trajectory speed In order to compensate for the error in the estimated end force, When the adjustment item takes effect, when it is less than the minimum force threshold hour, Equal to the identity matrix , and finally returns to the smooth trajectory speed, the formula is as follows:

[0107] (8)

[0108] Because of mathematical calculations, the adjustment Unexpected equilibrium points may be introduced. The mathematical description of unexpected equilibrium points is as follows:

[0109] (9)

[0110] The original trajectory speed is Project upward and modify the size of each projection value according to the eigenvalue matrix. The formula is as follows:

[0111] (10)

[0112] in, is the projection value of the compliant trajectory velocity in the force direction, which is relative to the projection value of the original trajectory velocity. Added tuning parameters.

[0113] S400, calculating the ideal joint angle and the ideal joint velocity. In response to the compliant trajectory velocity, the inverse kinematics solution module generates compliant trajectory path points through integration, and calculates the ideal joint angle and the ideal joint velocity of the robot joint movement. Specifically, the process includes:

[0114] S410, in response to the compliant trajectory speed, the inverse kinematics solution module generates compliant trajectory path points by integration;

[0115] S420: Generate ideal joint angles and ideal joint velocities by inverse kinematics calculation of the compliant trajectory path points and the compliant trajectory velocities.

[0116] S500, generating active force signals for robot joints. In response to the ideal joint angles and ideal joint velocities, the actual joint angles and actual joint velocities, the radial basis function neural network control module uses the radial basis function neural network to fit model parameters, generates active force signals for robot joints, and sends the signals to the robot drive module to control the robot motion.

[0117] S600, anti-interruption processing: During the robot's motion, if it has not reached the balance point of the original trajectory, the anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the balance point of the original trajectory; specifically, the following steps are involved:

[0118] S610, determine whether the equilibrium point of the original trajectory is reached. , it is considered that the equilibrium point of the original trajectory has been reached, and step S630 is executed; otherwise, step S620 is executed;

[0119] S620, determine whether to reach the unexpected equilibrium point, when and When it is considered that an unexpected equilibrium point has been reached, the anti-interruption module uses the anti-interruption algorithm to add disturbances to the smooth trajectory speed. , disturbance The preferred value is 0.1, which generates a smooth trajectory speed with disturbance , the formula is as follows:

[0120] (11)

[0121] Wherein, j is a positive integer from 2 to D; return to step S610;

[0122] S630: The compliant control process is completed and the robot reaches the equilibrium point of the original trajectory.

[0123] The above describes in detail the preferred embodiments of the present invention. It should be understood that those skilled in the art can make numerous modifications and variations based on the concepts of the present invention without inventive effort. Therefore, any technical solutions that can be derived by those skilled in the art through logical analysis, reasoning, or limited experimentation based on the concepts of the present invention and the prior art should be within the scope of protection defined by the claims.

Claims

1. A compliant control system based on terminal force estimation, characterized in that: include: The end force estimation module calculates the estimated amount of end force in response to the actual joint angle and actual joint velocity from the robot joint encoder ; Trajectory regression module collects relevant teaching trajectories and generates original trajectory speed The original trajectory speed is generated according to the teaching trajectory regression method, and the teaching trajectory data is fitted using a Gaussian mixture model and obtained through Bayesian inference. The number of layers of the Gaussian mixture model is 4; The compliant trajectory adjustment module, in response to the estimated amount of the end force and the original trajectory velocity, adds an adjustment item for achieving trajectory compliant replanning to generate a compliant trajectory velocity, including: S310, setting the maximum force threshold , maximum force threshold It is the upper limit of the end force during the movement or the tracking value of the end force during the movement; S320, generating an adjustment ratio using the maximum force threshold , which reflects the ratio of the estimated end force to the maximum force threshold, is used to modify the components of the original trajectory velocity in each direction. The formula is as follows: ； in, Indicates the dimension of the vector, which is 3 in two-dimensional space and 6 in three-dimensional space; i is a positive integer from 1 to D; S330, define the feature space, the formula is as follows: ； in, is the unit vector in the direction of the estimated force at the end, For the unit vector values, For The feature space generated for the first basis; S340, define the projection value adjustment method, the formula is as follows: ； Among them, the eigenvalue matrix is the adjustment method of the projection of the original trajectory velocity in different direction vectors, is the degree of adjustment in the i-th direction, and the formula is as follows: in, is the effect switching item, the formula is as follows: in, , , For effect switching parameters, obtained through debugging in actual use; when and When it is 0, the maximum force threshold is the upper limit of the estimated end force. and , the estimated amount of end force can be realized to track the maximum force threshold; S350. Generate a smooth trajectory speed using an adjustment item. The adjustment item formula is as follows: Adjustment Multiplying the original trajectory speed to get the smooth trajectory speed In order to compensate for the error in the estimated end force, When the adjustment item takes effect, when it is less than the minimum force threshold hour, Equal to the identity matrix , and finally returns to the smooth trajectory speed, the formula is as follows: ； an inverse kinematics solving module, responsive to the compliant trajectory velocity, generating compliant trajectory path points by integration, and calculating ideal joint angles and ideal joint velocities of the robot joint movement; a radial basis function neural network control module, in response to the ideal joint angle and the ideal joint velocity, the actual joint angle and the actual joint velocity, using a radial basis function neural network to fit model parameters, generate a robot joint active force signal, and send the signal to the robot drive module to control the robot movement; The anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the equilibrium point of the original trajectory, including: S610, determine whether the equilibrium point of the original trajectory is reached, when the original trajectory speed , it is considered that the equilibrium point of the original trajectory has been reached, and step S630 is executed; otherwise, step S620 is executed; S620, determine whether to reach the unexpected equilibrium point, when and When the unexpected equilibrium point is reached, the anti-interruption module uses the anti-interruption algorithm to add a disturbance to the smooth trajectory speed. , generating a smooth trajectory velocity with disturbance , the formula is as follows: ； Wherein, j is a positive integer from 2 to D; Return to step S610; S630: The compliant control process is completed, and the robot reaches the equilibrium point of the original trajectory; The end force estimation module, the compliant trajectory adjustment module, the inverse kinematics solution module, and the radial basis function neural network control module are sequentially connected in series and communicatively coupled, and the trajectory regression module and the anti-interruption module are respectively communicatively coupled to the compliant trajectory adjustment module. In response to the actual joint angle and the actual joint velocity from the robot joint encoder, the end force estimation module calculates an estimate of the end force, and the trajectory regression module generates an original trajectory velocity based on the relevant teaching trajectory. In response to the estimate of the end force and the original trajectory velocity, the compliant trajectory adjustment module generates the compliant trajectory velocity. The inverse kinematics solution module calculates the ideal joint angle and the ideal joint velocity for robot joint movement based on the compliant trajectory velocity. The radial basis function neural network control module generates a robot joint active force signal based on the ideal joint angle and the ideal joint velocity, the actual joint angle and the actual joint velocity, and sends the signal to the robot drive module to control the robot movement. The anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the equilibrium point of the original trajectory.

2. The compliant control system based on terminal force estimation according to claim 1, characterized in that: The end force estimation module uses generalized momentum to describe the end force signal, multiplies the inertia matrix by the joint angle as the generalized momentum, builds a state observer based on the classical robot motion model, and uses Kalman filtering to reduce the error to obtain an estimate of the end force signal.

3. A compliant control method based on terminal force estimation, using the compliant control system based on terminal force estimation according to any one of claims 1-2, characterized in that: The steps include: S100, preparation work, the trajectory regression module collects the teaching trajectory and obtains the original trajectory speed; specifically including: S110, specify the starting point and end point of the teaching trajectory; S120, controlling the robot end to move to the end point, using the teaching function to manually control the robot end to move to the end point; S130, generating original trajectory speed , using the Gaussian mixture model regression algorithm to extract the original trajectory velocity from the teaching trajectory , the number of layers of the Gaussian mixture model is 4; S200, calculate the estimated amount of end force In response to the actual joint angle and the actual joint velocity from the robot joint encoder, the end force estimation module calculates an estimate of the end force; S300: Generate a compliant trajectory velocity. Utilizing the estimated terminal force output by the terminal force estimation module and the original trajectory velocity output by the trajectory regression module, the compliant trajectory adjustment module adds an adjustment item for achieving trajectory compliant replanning to generate the compliant trajectory velocity. Specifically, the process includes: S310, setting the maximum force threshold ; S320: Generate an adjustment ratio using the maximum force threshold , which reflects the ratio of the estimated end force to the maximum force threshold, is used to modify the components of the original trajectory velocity in each direction. The formula is as follows: ； in, Indicates the dimension of the vector, which is 3 in two-dimensional space and 6 in three-dimensional space; i is a positive integer from 1 to D; S330, define the feature space, the formula is as follows: ； in, is the unit vector in the direction of the estimated force at the end, For the unit vector values, For The feature space generated for the first basis; S340, define the projection value adjustment method, the formula is as follows: Among them, the eigenvalue matrix is the adjustment method of the projection of the original trajectory velocity in different direction vectors, is the degree of adjustment in the i-th direction, and the formula is as follows: in, is the effect switching item, the formula is as follows: in, , , For effect switching parameters, obtained through debugging in actual use; when and When it is 0, the maximum force threshold is the upper limit of the estimated end force. and , the estimated amount of end force can be realized to track the maximum force threshold; S350: Generate a smooth trajectory speed using an adjustment item, wherein the adjustment item formula is as follows: The formula for the compliant trajectory speed is as follows: ； S400, calculating an ideal joint angle and an ideal joint velocity. In response to the compliant trajectory velocity, the inverse kinematics solution module generates compliant trajectory path points by integration, and calculates the ideal joint angle and ideal joint velocity of the robot joint movement. S500, generating active force signals for robot joints. In response to the ideal joint angles and the ideal joint velocities, the actual joint angles and the actual joint velocities, the radial basis function neural network control module uses a radial basis function neural network to fit model parameters, generates active force signals for robot joints, and sends the signals to the driving module of the robot to control the movement of the robot. S600, anti-interruption processing: During the movement of the robot, when the robot has not reached the balance point of the original trajectory, the anti-interruption module uses an anti-interruption algorithm to ensure that the robot reaches the balance point of the original trajectory, specifically including: S610, determine whether the equilibrium point of the original trajectory is reached, when the original trajectory speed , it is considered that the equilibrium point of the original trajectory has been reached, and step S630 is executed; otherwise, step S620 is executed; S620, determine whether to reach the unexpected equilibrium point, when and When the unexpected equilibrium point is reached, the anti-interruption module uses the anti-interruption algorithm to add a disturbance to the smooth trajectory speed. , disturbance A value of 0.1 generates a smooth trajectory speed with disturbance , the formula is as follows: ； Wherein, j is a positive integer from 2 to D; Return to step S610; S630: The compliant control process is completed, and the robot reaches the equilibrium point of the original trajectory.

4. The compliant control method based on terminal force estimation according to claim 3, characterized in that: The step S400 includes: S410, in response to the compliant trajectory speed, the inverse kinematics solution module generates the compliant trajectory path points by integration; S420: Generate the ideal joint angle and the ideal joint speed by inverse kinematics back-calculation of the compliant trajectory path points and the compliant trajectory speed.

Images