Mechanical arm compliance tracking system and control method

Abstract

The invention relates to a mechanical arm compliance tracking system and a control method. The system comprises a mechanical arm module, a mechanical arm pose control module, a force acquisition module and a dynamic self-adaptive hybrid impedance controller. In the dynamic self-adaptive hybrid impedance controller, through a dynamic self-adaptive hybrid impedance control algorithm, dynamic contact force tracking in an uncertain environment is completed, force overshoot in a contact stage is avoided, and meanwhile, a force tracking error in a dynamic tracking stage is kept. The controller is capable of adjusting its update rate parameters online in order to track the reference force in an uncertain environment.

Description

Technical Field

[0001] The invention belongs to the technical field of industrial robots, and in particular relates to a compliance force tracking system and a control method for a robotic arm. Background Art

[0002] With the development of robot technology, more advanced production needs have prompted robots to develop in the direction of intelligence and diversification. In advanced production, there are more and more scenarios for robots to interact with the external environment, such as the production and manufacturing of high-precision parts, grinding and polishing and flexible assembly in industrial scenarios, and robot teaching human-machine interaction. When performing these interactive tasks where the robot comes into contact with the environment, interaction forces will be generated between the contact surfaces. When there is a deviation between the end effector and the planned trajectory, it may cause the actuator to detach from the contact surface or generate strong pressure, which will not only have a huge impact on the accuracy of the robot control system, but also have some destructive consequences due to the high rigidity structure of the robot. Therefore, the robot must have a certain degree of flexibility to the environment so that it can work in some unknown environments.

[0003] In order to solve the contact problem, a robot flexible control method is proposed to solve the position control and force control at the same time. Compliant force control is mainly divided into active compliance control and passive compliance control. Passive compliance control mainly uses some mechanical devices that are flexible to the environment, such as shock absorbers, springs, etc., and uses their elastic deformation to absorb vibration and impact energy, so that the robot shows compliance characteristics to the environment. Active compliance control relies on intelligent control algorithms to process force / position information, so that the end produces the required stiffness, damping or force to achieve the purpose of compliance. Since active compliance control has a high feedback ability to force, it has a higher control accuracy, so active compliance control is selected for research.

[0004] According to the relationship between position and force, active compliance control can be further divided into impedance control and position / force hybrid control. Position / force hybrid control divides the task space into position subspace and force subspace based on the formal model of the manipulator and the task geometry. Since it is impossible to control position and force simultaneously along the same degree of freedom, it tracks force and position in different directions, which is the main disadvantage of this strategy.

[0005] To solve this dynamic problem, a control method based on the relationship between position and force is proposed: classical impedance control. The principle of impedance control is to adjust the mechanical impedance of the manipulator. Classical impedance control deals with the dynamic relationship between force and motion, so motion and force can be controlled simultaneously in one direction. In many cases, impedance control is superior to position / force hybrid control in controlling the dynamic contact between the manipulator and the environment, and exhibits stronger robustness in unknown stiffness environments. However, since position / force hybrid control is easy to implement and has good dynamic force tracking performance, while impedance control can also achieve precise force control and position control, it requires adjusting impedance parameters, which is very cumbersome and unintuitive. Therefore, various studies have combined the concept of force / position control with the impedance control scheme, namely the hybrid impedance control scheme, to obtain better performance.

[0006] However, when there are large uncertainties in both the position and stiffness of the contact dynamic environment, the hybrid impedance control has large errors in force tracking accuracy. At this time, the stability region of the impedance controller can be expanded by using an adaptive law that estimates the impedance parameters in real time based on environmental parameters. On this basis, the improved adaptive hybrid impedance control strategy can not only track the required force, but also compensate for the uncertainty of the environmental position and stiffness. In addition, since most application robots are industrial robots and can only be controlled in position mode, a position-based adaptive hybrid impedance controller is proposed to improve the force tracking accuracy in unknown environments.

[0007] However, for a certain type of processing, the parameters of the workpiece (such as ultra-thin parts, aircraft wings, and patient hearts) are time-varying during the operation process (such as polishing and cutting). Although the adaptive hybrid impedance control scheme has superior performance in compensating for force tracking errors, it is easy to cause large force overshoot. Relying on existing control strategies, it is difficult to maintain processing accuracy without damaging parts, and to maintain a small force tracking error while avoiding force overshoot. In view of the above defects, in current industrial applications, a simple and practical solution has not yet been proposed for production practice.

[0008] Therefore, the robot arm compliant contact algorithm in the prior art mainly has the following defects:

[0009] (1) The position / force hybrid control strategy requires the design of separate control loops for force and position according to requirements. It is impossible to track force and position in the same direction, and it is necessary to obtain information about the interactive environment. However, in practical applications, it is difficult to obtain an accurate environmental model, resulting in many errors in the control effect.

[0010] (2) Although the hybrid impedance control scheme can achieve precise force control and position control simultaneously when the environmental model is known, it has large errors in force tracking accuracy when the environmental uncertainty is large, and cannot achieve effective tracking;

[0011] (3) The adaptive hybrid impedance control strategy has greatly improved the force tracking accuracy, but compared with the hybrid impedance control strategy, the force overshoot is also greatly increased and the stability is deteriorated. When the robot transitions from the free motion space to the task space, it is easy to generate a very short and large contact force with the environment. The controller cannot respond in time, which may cause damage to the robot itself or the contact environment.

[0012] In order to solve the above problems and improve the quality of compliant contact control of the robot arm, a force tracking strategy that can accurately track the contact force and has high stability is urgently needed. This force strategy can avoid force overshoot in the contact phase, while maintaining the force error in the dynamic tracking phase, and has high adaptability to time-varying dynamic environments, so as to solve problems that traditional control algorithms cannot solve. Summary of the invention

[0013] In order to overcome the above problems existing in the prior art, the present invention provides a robot arm compliance force tracking system and control method, which are used to solve the above problems existing in the prior art.

[0014] A mechanical arm compliance force tracking system, the system comprising: a mechanical arm module, a mechanical arm posture control module, a force acquisition module and a dynamic adaptive hybrid impedance controller;

[0015] The robot arm posture control module is connected to the robot arm module and is used to send control signals to the robot arm module in real time and receive motion information of the robot arm module in real time;

[0016] The force acquisition module is mounted on the mechanical arm module and connected to the dynamic adaptive hybrid impedance controller, and is used to transmit the acquired force and torque of the mechanical arm module to the dynamic adaptive hybrid impedance controller;

[0017] The dynamic adaptive hybrid impedance controller is connected to the robotic arm posture control module and the force acquisition module, and is used to receive the motion information of the robotic arm module and the force data of the force acquisition module in real time, and perform kinematic calculations based on the dynamic adaptive hybrid impedance control strategy, and output the calculated desired posture control information to the robotic arm posture control module. The robotic arm posture control module outputs the calculated coordinate information to the robotic arm module according to the desired posture control information.

[0018] According to the aspects described above and any possible implementation method, an implementation method is further provided, which also includes a power supply module connected to the robotic arm module, the robotic arm posture control module, the force acquisition module and the dynamic adaptive hybrid impedance controller.

[0019] According to the aspects described above and any possible implementation, an implementation is further provided, wherein the robot arm module includes a robot arm, a servo motor and an end effector, wherein the end effector is disposed on the robot arm and connected to the servo motor.

[0020] According to the aspects described above and any possible implementation, an implementation is further provided, wherein the force acquisition module is implemented by a force sensor, and the force sensor is disposed at the end of the robotic arm to acquire the force and torque exerted on the end actuator.

[0021] As described above and any possible implementation method, an implementation method is further provided, wherein the dynamic adaptive hybrid impedance controller includes a system and parameter initialization module, a robotic arm information processing module, a force information processing module, a dynamic update rate calculation module, an impedance function calculation module, an adaptive compensation rate calculation module and an expected end posture calculation module.

[0022] The robot arm information processing module is used to process the robot arm posture information transmitted by the robot arm posture control module, and transmit the processed data information to the dynamic update rate calculation module;

[0023] The system and parameter initialization module is used to initialize the impedance parameters and the expected contact force information, and transmit the initialized data information to the dynamic update rate calculation module;

[0024] The force information processing module is used to unpack the force data collected by the force collection module, convert the data format, and transmit the converted data information to the dynamic update rate calculation module;

[0025] The dynamic update rate calculation module is used to calculate the real-time update rate σ according to the processed force data information and the converted data information, and transmit the real-time update rate σ to the adaptive compensation rate calculation module;

[0026] The adaptive compensation rate calculation module is used to calculate the real-time adaptive compensation rate ρ according to the converted six-dimensional force information, the impedance parameter and the dynamic update rate σ, and transmit the real-time updated dynamic adaptive impedance parameter to the impedance function calculation module;

[0027] The impedance function calculation module is used to calculate the posture of the robotic arm at the next moment according to the converted six-dimensional force information, the expected contact force information, the updated adaptive impedance parameters and the posture information of the robotic arm at the previous moment, and transmit the calculated posture information to the expected end posture calculation module;

[0028] The desired end position and posture calculation module is used to output the Cartesian coordinates of the next moment position and posture of the robot arm according to the position and posture information, and transmit it to the robot arm position and posture control module.

[0029] According to the aspects described above and any possible implementation, there is further provided an implementation, wherein the robot arm posture control module includes an inverse solution module, and the inverse solution module converts the received Cartesian coordinates into joint coordinates of the robot arm.

[0030] According to the aspects described above and any possible implementation, there is further provided an implementation, wherein the robotic arm is a joint coordinate of a six-degree-of-freedom robotic arm.

[0031] According to the aspects and any possible implementations described above, an implementation is further provided, wherein the dynamic adaptive hybrid impedance controller is implemented on a PC side, and communication connection and data processing are performed in a C++ environment.

[0032] The present invention also provides a control method for a robot arm compliance tracking system, which is implemented by the system and comprises the following steps:

[0033] S1. Start the system, start the robotic arm and the robotic arm posture control module in the robotic arm module, and the end effector on the robotic arm moves under the drive of the servo motor;

[0034] S2. The force acquisition module acquires the force and torque information of the end effector during movement at the current moment, and transmits the information to the dynamic adaptive hybrid impedance controller;

[0035] S3. The dynamic adaptive hybrid impedance controller calculates the Cartesian coordinates of the next moment posture of the robot arm according to the force and torque information, the robot arm posture information, the impedance parameters and the expected contact force information, and transmits it to the robot arm posture control module;

[0036] S4. The robotic arm posture control module converts the Cartesian coordinates into the joint coordinates of the robotic arm and sends them to the robotic arm, and the robotic arm moves according to the received joint coordinates.

[0037] According to the above aspects and any possible implementation, an implementation is further provided, wherein the dynamic adaptive hybrid impedance controller adopts a dynamic adaptive hybrid impedance control strategy to perform calculations, and the strategy includes: the dynamic adaptive hybrid impedance controller adopts a dynamic adaptive hybrid impedance control strategy to perform calculations, and the strategy includes:

[0038] H1. Establish an adaptive hybrid impedance control algorithm formula and calculate the real-time position error e(t) based on the force tracking error ΔF:

[0039]

[0040] in,

[0041] m d : The desired inertia parameters of the controller;

[0042] b d : The desired damping parameter of the controller;

[0043] ΔF: The error between the actual contact force and the expected force;

[0044] e(t): The current position tracking error of the end of the robot arm, correspondingly, is the current speed tracking error, Then it is the current acceleration tracking error;

[0045] ρ: Adaptive compensation law of force tracking error, which can be dynamically adjusted through force tracking error and is calculated by the following formula:

[0046]

[0047] The meanings of each variable are as follows:

[0048] T: sampling period;

[0049] b: initial value of environmental stiffness;

[0050] σ: update rate;

[0051] f d (tT): the expected power at the previous moment;

[0052] f e (tT): actual contact force at the last moment;

[0053] ρ(tT): adaptive compensation rate at the previous moment;

[0054] ρ(t): Adaptive compensation rate at the current moment.

[0055] H2. Use the following formula to calculate the position command at the next moment:

[0056] P c (t) = P d (t)+e(t).

[0057] in:

[0058] e(t): current position tracking error of the end of the robotic arm;

[0059] P d (t): expected position;

[0060] P c (t): The command position output at the current moment.

[0061] According to the position error e(t), the expected position P d (t) Real-time update to obtain the command position P c (t), which is tracked by the position-based robot servo control system, which adjusts the position of the robot end so that the contact force continuously tracks the desired force.

[0062] Beneficial effects of the present invention

[0063] In the compliant contact force tracking system of the robot arm of the present invention, a dynamic adaptive hybrid impedance controller is used to complete dynamic contact force tracking in an uncertain environment (such as a series of production scenarios such as grinding, polishing, and assembly). Force overshoot in the contact phase is avoided while maintaining the force tracking error in the dynamic tracking phase. The controller can adjust its update rate parameters online to track the reference force in an uncertain environment.

[0064] Aiming at the problem that the traditional robot compliance control cannot simultaneously consider the transient contact force overshoot and steady-state force tracking error, the present invention designs a dynamic adaptive hybrid impedance controller to handle the dynamic contact force tracking in an uncertain environment. The transient response and steady-state error of the traditional impedance control and the improved adaptive hybrid impedance control are analyzed, and experiments and performance comparisons are carried out in the Simulink simulation environment and the industrial robot experimental environment respectively. Improvements are made on the basis of the traditional impedance algorithm to improve the stability and accuracy of the compliance control. BRIEF DESCRIPTION OF THE DRAWINGS

[0065] Figure 1 It is a schematic diagram of the principle of the compliance force tracking system of the robot arm of the present invention;

[0066] Figure 2 is a schematic diagram of an algorithm implementation of a dynamic adaptive hybrid impedance control method in an embodiment of the present invention;

[0067] Figure 3 It is a working flow chart of the compliance force tracking system of the robot arm in the present invention. DETAILED DESCRIPTION

[0068] In order to better understand the technical solution of the present invention, the content of the present invention includes but is not limited to the specific implementation methods described below, and similar technologies and methods should be considered to be within the scope of protection of the present invention. In order to make the technical problems, technical solutions and advantages to be solved by the present invention clearer, the following will be described in detail with reference to the accompanying drawings and specific embodiments.

[0069] It should be clear that the embodiments described in the present invention are only part of the embodiments of the present invention, rather than all 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.

[0070] The terms used in the embodiments of the present invention are only for the purpose of describing specific embodiments, and are not intended to limit the present invention. The singular forms "a", "said" and "the" used in the embodiments of the present invention and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0071] The present invention provides a mechanical arm compliance force tracking system, the system comprising: a mechanical arm module, a mechanical arm posture control module, a force acquisition module and a dynamic adaptive hybrid impedance controller;

[0072] The robot arm posture control module is connected to the robot arm module and is used to send control signals to the robot arm module in real time and receive motion information of the robot arm module in real time;

[0073] The force acquisition module is mounted on the mechanical arm module and connected to the dynamic adaptive hybrid impedance controller, and is used to transmit the acquired force and torque of the mechanical arm module to the dynamic adaptive hybrid impedance controller;

[0074] The dynamic adaptive hybrid impedance controller is connected to the robotic arm posture control module and the force acquisition module, and is used to receive the motion information of the robotic arm module and the force data of the force acquisition module in real time, and perform kinematic calculations based on the dynamic adaptive hybrid impedance control strategy, and output the calculated desired posture control information to the robotic arm posture control module. The robotic arm posture control module outputs the calculated coordinate information to the robotic arm module according to the desired posture control information.

[0075] Furthermore, it also includes a power supply module connected to the robotic arm module, the robotic arm posture control module, the force acquisition module and the dynamic adaptive hybrid impedance controller.

[0076] Furthermore, the robotic arm module includes a robotic arm, a servo motor and an end effector, wherein the end effector is arranged on the robotic arm and connected to the servo motor.

[0077] Furthermore, the force acquisition module is implemented by a force sensor, and the force sensor is arranged at the end of the mechanical arm to collect the force and torque applied to the end effector.

[0078] Furthermore, the dynamic adaptive hybrid impedance controller includes a system and parameter initialization module, a robotic arm information processing module, a force information processing module, a dynamic update rate calculation module, an impedance function calculation module, an adaptive compensation rate calculation module and an expected end posture calculation module.

[0079] The robot arm information processing module is used to process the robot arm posture information transmitted by the robot arm posture control module, and transmit the processed data information to the dynamic update rate calculation module;

[0080] The system and parameter initialization module is used to initialize the impedance parameters and the expected contact force information, and transmit the initialized data information to the dynamic update rate calculation module;

[0081] The force information processing module is used to unpack the force data collected by the force collection module, convert the data format, and transmit the converted data information to the dynamic update rate calculation module;

[0082] The dynamic update rate calculation module is used to calculate the real-time update rate σ according to the processed force data information and the converted data information, and transmit the real-time update rate σ to the adaptive compensation rate calculation module;

[0083] The adaptive compensation rate calculation module is used to calculate the real-time adaptive compensation rate ρ according to the converted six-dimensional force information, the impedance parameter and the dynamic update rate σ, and transmit the real-time updated dynamic adaptive impedance parameter to the impedance function calculation module;

[0084] The impedance function calculation module is used to calculate the posture of the robotic arm at the next moment according to the converted six-dimensional force information, the expected contact force information, the updated adaptive impedance parameters and the posture information of the robotic arm at the previous moment, and transmit the calculated posture information to the expected end posture calculation module;

[0085] The desired end position and posture calculation module is used to output the Cartesian coordinates of the next moment position and posture of the robot arm according to the position and posture information, and transmit it to the robot arm position and posture control module.

[0086] Furthermore, the robotic arm posture control module includes an inverse solution module, which converts the received Cartesian coordinates into joint coordinates of the robotic arm.

[0087] Furthermore, the robotic arm is the joint coordinates of a six-degree-of-freedom robotic arm.

[0088] Furthermore, the dynamic adaptive hybrid impedance controller is implemented on a PC, and communication connection and data processing are performed in a C++ environment.

[0089] Furthermore, the dynamic adaptive hybrid impedance controller adopts a dynamic adaptive hybrid impedance control strategy to perform calculations, and the strategy includes:

[0090] H1. Establish an adaptive hybrid impedance control algorithm formula, which can calculate the real-time position error e(t) based on the force tracking error ΔF:

[0091]

[0092] The meanings of each variable are as follows:

[0093] m d : The desired inertia parameters of the controller;

[0094] b d : The desired damping parameter of the controller;

[0095] ΔF: The error between the actual contact force and the expected force;

[0096] e(t): The current position tracking error of the end of the robot arm, correspondingly, is the current speed tracking error, Then it is the current acceleration tracking error;

[0097] ρ: Adaptive compensation law of force tracking error, which can be dynamically adjusted through force tracking error and is calculated by the following formula:

[0098]

[0099] The meanings of each variable are as follows:

[0100] T: sampling period of the system;

[0101] b: initial value of environmental stiffness;

[0102] σ: update rate;

[0103] f d (tT): expected force at the previous moment, a known quantity;

[0104] f e (tT): actual contact force at the previous moment, a known quantity;

[0105] ρ(tT): adaptive compensation rate at the previous moment, a known quantity;

[0106] ρ(t): Adaptive compensation rate at the current moment.

[0107] H2. Use the following formula to calculate the position command at the next moment:

[0108] P c (t) = Pd (t)+e(t).

[0109] The meanings of each variable are as follows:

[0110] e(t): current position tracking error of the end of the robotic arm;

[0111] P d (t): expected position;

[0112] P c (t): The command position output at the current moment.

[0113] The above formula can be used to calculate the expected position P according to the position error e(t). d Real-time update can be performed to obtain the command position P c , which is tracked by the position-based robot servo control system. The contact force is made to continuously track the desired force by adjusting the position of the end of the robot arm.

[0114] Specifically, Figure 1 The present invention shows a flexible force tracking system for a robotic arm in an example, which is used to detect dynamic contact force in an uncertain environment, and designs a dynamic adaptive hybrid impedance controller to achieve constant force compliant tracking of the contact force. It solves the force tracking error caused by position uncertainty and environmental stiffness uncertainty in an unknown environment without generating a large overshoot, and realizes the deployment of a dynamic adaptive hybrid impedance control algorithm on a six-degree-of-freedom robotic arm.

[0115] The robot arm compliance force tracking system in the embodiment of the present invention includes a power module, a robot arm posture control module, a robot arm, a servo motor, an end effector, a force acquisition module and a dynamic adaptive hybrid impedance controller. The power module provides power to the system. The robot arm is arranged on the robot and is implemented in a six-degree-of-freedom form. It moves under the control of the robot arm posture control module and feeds back the motion information to the robot arm posture control module. The robot arm posture control module is used to send control signals to the six-degree-of-freedom robot arm in real time and receive the motion information of the robot arm in real time. The servo motor is installed at the end of the robot arm and moves with the robot arm to a specified position to drive the end effector at the end of the robot arm to complete a specific task. The end effector is used to complete a series of production tasks such as grinding, polishing, and assembly under the drive of the servo motor.

[0116] The force acquisition module is mounted at the end of the six-degree-of-freedom robotic arm and is used to acquire the six-dimensional force and six-dimensional torque exerted on the end effector.

[0117] The power module can be in the form of common wired, battery, etc., and there is no specific limitation here, as long as it can realize the power supply function.

[0118] The dynamic adaptive hybrid impedance controller is connected to the manipulator posture control module and the force acquisition module, and is used to receive the motion information of the manipulator posture control module and the force data of the force acquisition module in real time, and perform robot kinematic calculation based on a dynamic adaptive hybrid impedance control strategy to obtain the desired posture control information of the manipulator, and output it to the robot inverse solution module built into the manipulator posture control module. The robot inverse solution module outputs the calculated manipulator joint coordinate information to the six-degree-of-freedom manipulator, and updates the motion state of the manipulator in real time. The manipulator drives the end effector to move to the calculated desired position, and adjusts the end contact force to keep it constant by controlling the position of the end effector.

[0119] The manipulator posture control module is connected to the power module. The present invention adopts a manipulator control cabinet supporting an industrial manipulator, which is used to send control signals to the six-degree-of-freedom manipulator in real time, and receive the motion information of the manipulator in real time, and connect to the dynamic adaptive hybrid impedance controller mounted on the PC host computer and complete the transmission of control signals and feedback signals. The system uses a wired method to complete the data interaction between the manipulator posture control module and the impedance controller, based on the Ethernet communication protocol defined by the IEEE 802.3 standard. Connect one end of the network cable to the LAN port of the manipulator control cabinet, and the other end to the PC end, i.e., the dynamic adaptive hybrid impedance controller, and modify the IP address of the PC end so that it is in the same network segment as the IP address of the control cabinet. It can be understood that the data transmission method can also be in the form of serial port, Ethernet, SPI, etc., which is not specifically limited here, as long as the function of data transmission can be realized.

[0120] Combination Figure 1 As shown, the six-degree-of-freedom robot is connected to the robot posture control module, receives the control signal of the control module and feeds back the motion information to the control module. It can be understood that the robot can be a common industrial six-degree-of-freedom robot, which is not specifically limited here, as long as it can achieve the function.

[0121] For example Figure 1 As shown, the servo motor is installed at the end of the robot arm and moves to the specified position with the robot arm. By accurately controlling the position, speed and torque of the end effector, high-precision grinding, assembly and other operations can be achieved. It can be understood that the servo motor can be a stepping servo motor, an AC servo motor, a linear servo motor, etc., and there is no specific limitation here, as long as the function can be achieved.

[0122] The end effector is connected to the servo motor and completes a series of production tasks such as grinding, polishing, and assembly under the drive of the motor. It is understandable that different actuators can be selected according to specific uses. For example, a grinding head needs to be installed to complete a grinding task, and a servo clamp needs to be installed to complete an assembly task. There is no specific limitation here, as long as the function can be achieved.

[0123] The force acquisition module is loaded at the end of the six-degree-of-freedom robotic arm, and is used to collect the six-dimensional force and six-dimensional torque exerted on the end effector. The module can adopt a six-dimensional force sensor based on the principle of resistive strain gauge, the electrical and physical layers adopt the RS-485 communication protocol, and the application layer adopts the Modbus-RTU protocol. The six-dimensional force sensor is installed at the end of the robotic arm and connected to the dynamic adaptive hybrid impedance controller, i.e., the PC end, via a network cable. In the dynamic adaptive hybrid impedance controller, the force data received by the six-dimensional force sensor is unpacked according to the Modbus-RTU protocol. The data format is the 32-bit floating point format of the IEEE-754 standard, and it needs to be converted into decimal data. It can be understood that the force acquisition module can adopt force sensors with other measurement principles and transmission protocols, and there is no specific restriction here, as long as the functions of six-dimensional force measurement and data transmission can be realized.

[0124] The dynamic adaptive hybrid impedance controller is connected to the manipulator posture control module and the force acquisition module, and is used to receive the manipulator motion information and the force data of the force acquisition module in real time, and perform robot kinematic calculations based on a dynamic adaptive hybrid impedance control strategy, and output the desired posture control information of the manipulator. This module is implemented on the PC side, and communication connection and data processing are performed in the C++ environment. It can be understood that other different programming languages ​​such as C and C# can be used for control implementation, and there is no specific limitation here, as long as the function can be realized.

[0125] The transfer function of the interaction model of the entire compliance tracking system is shown below:

[0126]

[0127] The above block diagram is a simple closed-loop control circuit of the controller, robot and environment system. The controller is implemented on a computer, the robot motion controller is implemented using the robot's built-in position-based servo control system, the environment is the force space at the end of the robot arm, and the contact force acting on the end effector is measured using a force sensor. c is the position command issued by the impedance controller, P e is the actual motion position of the robot arm, F d is the expected contact force, F e is the actual contact force measured by the force sensor. G(s) and H(s) are the transfer functions of the controller and feedback link, respectively. G(s) = C(s) / E(s), E(s) = F d -F e Since the robot motion controller has excellent position tracking ability, the uncertainty of the model (including inertia, friction, etc.) and some external interference can be suppressed through PID control or some compensation techniques, which means that P c =Pe (position tracking capability), so the gain of the motion controller can be simplified to 1.

[0128] The dynamic adaptive hybrid impedance control strategy first selects the impedance model of the linear second-order system, and expresses the dynamic relationship between the force tracking error ΔF and the position error e(t), that is, the impedance function relationship, as follows:

[0129]

[0130] Where, e(t) = P d -P e , P e is the actual end position of the workpiece, P d is the desired end position, that is, e(t) is the position tracking error, and accordingly, is the speed tracking error, is the acceleration tracking error; F e is the actual contact force on the workpiece, F d is the expected force, that is, ΔF is the force tracking error. m d 、b d , k d The parameters are the inertia, damping and stiffness gains expected by the controller. In formula (1), when the force tracking error ΔF is known, the position error e(t) can be calculated by the second-order differential equation. d Real-time update can be performed to obtain the command position P c (t) = P d (t)+e(t), is tracked by the position-based robot servo control system. The contact force is made to continuously track the desired force by adjusting the position of the end of the robot arm.

[0131] By analyzing the steady-state force tracking error of ΔF, it can be seen that when the stiffness gain k d When k is zero, the system can meet the ideal steady-state condition, that is, the steady-state force error is zero. d =0, the impedance function in equation (1) is rewritten as:

[0132]

[0133] In actual systems, the environmental stiffness is a continuous time-varying variable. Due to the change of environmental stiffness, even if k dEven if it is set to zero, the force tracking error cannot be avoided. Moreover, the robot servo system or force sensor has transmission delay, measurement noise and model uncertainty, and there will be an infinite steady-state force tracking error. Therefore, the above ordinary hybrid impedance control is not competent for accurate force tracking control. In order to reduce the force tracking error, an improvement is made on the basis of formula (2), and the improved adaptive hybrid impedance control algorithm is shown in formula (3):

[0134]

[0135] Where ρ is the adaptive compensation law of the force tracking error, which can be adjusted online according to the force error and can be calculated by formula (4):

[0136]

[0137] ρ(t) is the adaptive compensation rate at the current moment, ρ(tT) is the adaptive compensation rate at the previous moment, which is a known quantity. In the initial state, ρ(0) = 0, T is the sampling period of the system, b is the initial value of the environmental stiffness, and f d (tT) is the expected force at the previous moment, and is the known quantity f e (tT) is the actual contact force at the previous moment, which is a known quantity; σ is the update rate, which is a time-varying parameter related to the stability of the system.

[0138] According to the impedance control law in equation (3), By performing double integration, the real-time position error e(t) to be compensated can be calculated, as shown in formula (5):

[0139]

[0140] By correcting this error based on the position at the previous moment, the position command at the next moment can be obtained, as shown in formula (6):

[0141] P c (t) = P d (t)+e(t) (6)

[0142] Where P d (t) is the expected position at the previous moment, and the position command P is obtained c After (t), the expected position at this time is updated and P is set d (t) = P c (t). And the position command P c (t) is sent to the robot servo controller to drive the robot to move to the command position.

[0143] Through the above robust adaptive control scheme, the impedance parameters can be adjusted in real time according to the environmental contact force to compensate for the uncertainty in the environment. Based on the error of the desired force and the adaptive law, the adaptive hybrid impedance control can keep the desired force constant.

[0144] At this point, analysis of the compensation rule shows that when the update rate σ is large, the compensation effect is more obvious (better force tracking capability), but the contact force jitter range is larger (larger force overshoot). On the contrary, when the update rate σ is small, the compensation effect is not obvious (poorer force tracking capability), and the contact force change trend is slower (smaller force overshoot). That is, if σ is too large, oscillation will occur; if σ is too small, the force error will be relatively large. Therefore, in order to simultaneously reduce the force tracking error and avoid force overshoot, and maintain a stable desired contact force, it is inappropriate to keep the update rate σ unchanged throughout the control process. In order to improve the performance of the controller, the update rate σ needs to be adjusted online. Therefore, an algorithm for dynamically calculating the update rate is added to the controller, and the feedback information of the force error Δf and the force error change Δf' is used to correct the update rate σ online:

[0145]

[0146] Among them, α and β are gains used to adjust the force error ratio and force error change. limt It is the upper limit to ensure that the system is in a stable state. In practical applications, in order to ensure the asymptotic stability of the system, the update rate σ needs to be set within a small range, and the upper limit value must be large enough, thus sacrificing a certain force tracking accuracy.

[0147] The update rate can be dynamically adjusted according to the force error through equation (7). When the robot transitions from the free motion space to the task space, the contact force error is large and the update rate σ is relatively small; when the robot enters the stable contact stage, the force error is small, and the update rate σ gradually increases as the force error decreases. By dynamically adjusting σ, the controller can achieve a small force overshoot and a high-precision force tracking effect.

[0148] Deploying the dynamic adaptive hybrid control strategy to the dynamic adaptive hybrid impedance controller can compensate for the force tracking error caused by the change of environmental stiffness, and ensure that there is no large force overshoot in the system by adjusting σ. Small steps are used to avoid overshoot in the contact state, and large steps are used to achieve force tracking in the stable state;

[0149] The dynamic adaptive hybrid impedance controller includes a mechanical arm information processing module, which is used to process the mechanical arm posture information transmitted by the mechanical arm posture control module and transmit the data information to the dynamic update rate calculation module;

[0150] The dynamic adaptive hybrid impedance controller includes a system and parameter initialization module, which is used to initialize information such as impedance parameters and expected contact force, and transmit the data information to the dynamic update rate calculation module;

[0151] The dynamic adaptive hybrid impedance controller includes a force information processing module, which is used to unpack the force data collected by the force collection module, convert the data format, and transmit the data information to the dynamic update rate calculation module;

[0152] The dynamic adaptive hybrid impedance controller includes a dynamic update rate calculation module, which is used to calculate the real-time update rate σ according to the six-dimensional force information and the impedance parameter, and transmit the real-time update rate to the adaptive compensation rate calculation module;

[0153] The dynamic adaptive hybrid impedance controller comprises an adaptive compensation rate calculation module, which is used to calculate the real-time adaptive compensation rate ρ according to the six-dimensional force information, the impedance parameters and the dynamic update rate σ, and transmit the real-time updated dynamic adaptive impedance parameters to the impedance function calculation module;

[0154] The dynamic adaptive hybrid impedance controller includes an impedance function calculation module, which is used to calculate the posture of the robot at the next moment according to the six-dimensional force information, the expected contact force information, the updated adaptive impedance parameters and the posture information of the robot at the previous moment, using the operation relationship of the impedance control algorithm shown in formula (5), and transmit the calculated posture information to the expected end posture calculation module;

[0155] The dynamic adaptive hybrid impedance controller includes an expected end posture calculation module, which is used to output the Cartesian coordinates of the next moment posture of the robot arm according to the calculation results of the impedance function calculation module, and transmit it to the robot inverse solution module of the robot arm posture control module to perform inverse kinematics calculation and convert it into the movement amount of each joint, thereby controlling the movement of the robot arm.

[0156] As an embodiment disclosed by the present invention, the present invention also provides a control method of a compliance tracking system for a robotic arm, the method being implemented by the system; and comprising the following steps:

[0157] S1. Start the system, start the robotic arm and the robotic arm posture control module in the robotic arm module, and the end effector on the robotic arm moves under the drive of the servo motor;

[0158] S2. The force acquisition module acquires the force and torque information of the end effector during movement at the current moment, and transmits the information to the dynamic adaptive hybrid impedance controller;

[0159] S3. The dynamic adaptive hybrid impedance controller calculates the Cartesian coordinates of the next moment posture of the robot arm according to the force and torque information, the robot arm posture information, the impedance parameters and the expected contact force information, and transmits it to the robot arm posture control module;

[0160] S4. The robotic arm posture control module converts the Cartesian coordinates into the joint coordinates of the robotic arm and sends them to the robotic arm, and the robotic arm moves according to the received joint coordinates.

[0161] Combination Figure 2 As shown, the algorithm implementation of the dynamic adaptive impedance control method of the impedance controller is composed of the following six steps. Step 2 to step 6 is a loop with a period of 4ms.

[0162] (1) System and parameter initialization module: The control algorithm first performs system initialization and parameter setting. System initialization includes turning on the enable and posture initialization on the robot arm, establishing a Modbus communication connection with the force sensor, clearing the force sensor data, and performing gravity compensation between the sensor and the end effector. Parameter setting includes setting impedance parameters, expected tracking force, and update rate upper limit.

[0163] (2) Force information processing module: The actual contact force is obtained in real time through the force sensor, and the sensor force information is converted to the coordinate system, and the force information is converted from the sensor's own coordinate system to the robot arm's base coordinate system. The force error Δf is calculated based on the actual contact force and the expected contact force, and the gradient of the force error Δf' is obtained by difference calculation.

[0164] (3) Dynamic update rate calculation module: According to the force error and force error gradient at this time, the dynamic parameter update rate σ is calculated by formula (7).

[0165] (4) Adaptive compensation rate calculation module: According to the error between the actual contact force and the expected force and the dynamic update rate σ at this time, the adaptive compensation rate ρ is calculated by formula (4).

[0166] (5) Impedance function calculation module: Impedance calculation is performed based on the above information, and the second-order differential equation is solved using double integrals. The position error that needs to be adjusted is gradually calculated based on the force error. In order to facilitate application in industrial robots, the control algorithm is discretized through sampling time. Figure 2 The formula in is a discretized form.

[0167] (6) Expected end position calculation module: Finally, the Cartesian position coordinates P of the robot arm at the next moment are calculated c , the position command is sent to the robot servo system. The whole cycle is completed, and it returns to step 2 and loops infinitely. The whole control cycle is 4ms.

[0168] Combination Figure 3 As shown, in some embodiments, Simulink can be used to simulate the designed robot arm compliance tracking system. Figure 3 The flowchart of the simulation implementation is shown in Figure 2. The control system block diagram is built in Simulink. First, the expected tracking force is set at the input signal, and the expected tracking force is subtracted from the actual tracking force fed back by the three-dimensional model of the robot to obtain the force tracking error. The force tracking error is input into the dynamic update rate calculation module, and the dynamic parameter update rate σ is calculated by formula (7), and then input into the adaptive compensation rate calculation module. The adaptive compensation rate calculation module calculates the adaptive compensation rate ρ according to the force tracking error and the update rate σ by formula (4), and then inputs into the impedance function calculation module. The impedance function calculation module performs impedance calculation, and calculates the position error at the next moment by the force tracking error and the robot's posture at the previous moment, and then inputs into the expected posture calculation module. The expected posture calculation module calculates the expected Cartesian posture and converts it into the six joint coordinates of the robot in the robot inverse solution module. The joint coordinates are input into the three-dimensional model of the robot to make the three-dimensional model move to the given coordinates. By continuously detecting the actual contact force at the end of the robot, the whole process is repeated, and the position of the end of the robot is continuously adjusted to achieve the control effect of contact force tracking the expected force.

[0169] The dynamic adaptive hybrid impedance control strategy used in the embodiment of the present invention is the same as that in the previous embodiment, and the implementation principles of the dynamic update rate calculation module, the adaptive compensation rate calculation module, the impedance function calculation module and other modules are the same as those in the previous embodiment.

[0170] Different from the deployment and implementation of the control strategy of the previous embodiment on an actual industrial robot, the embodiment of the present invention analyzes the transient response and steady-state error characteristics of the traditional impedance control and the improved dynamic adaptive hybrid impedance control strategy in a simulation environment, and points out the necessity of dynamic adaptation of the update rate. In view of the problem of dynamic contact force tracking in an uncertain environment, an improved dynamic adaptive hybrid impedance control method is proposed, and a robot arm compliance tracking system is built in a simulation environment using this method, realizing compliance tracking of planar forces, curved surface forces and irregular planar forces.

[0171] The above description shows and describes several preferred embodiments of the present invention, but as mentioned above, it should be understood that the present invention is not limited to the form disclosed herein, and should not be regarded as excluding other embodiments, but can be used in various other combinations, modifications and environments, and can be modified within the scope of the application concept described herein through the above teachings or the technology or knowledge of the relevant field. The changes and modifications made by those skilled in the art shall not depart from the spirit and scope of the present invention, and shall be within the scope of protection of the claims attached to the present invention.

Claims

1. A compliance tracking system for a robotic arm, characterized in that: The system comprises: a mechanical arm module, a mechanical arm posture control module, a force acquisition module and a dynamic adaptive hybrid impedance controller; The robot arm posture control module is connected to the robot arm module and is used to send control signals to the robot arm module in real time and receive motion information of the robot arm module in real time; The force acquisition module is mounted on the mechanical arm module and connected to the dynamic adaptive hybrid impedance controller, and is used to transmit the acquired force and torque of the mechanical arm module to the dynamic adaptive hybrid impedance controller; The dynamic adaptive hybrid impedance controller is connected to the robotic arm posture control module and the force acquisition module, and is used to receive the motion information of the robotic arm module and the force data of the force acquisition module in real time, and perform kinematic calculations based on the dynamic adaptive hybrid impedance control strategy, and output the calculated desired posture control information to the robotic arm posture control module. The robotic arm posture control module outputs the calculated coordinate information to the robotic arm module according to the desired posture control information.

2. The system according to claim 1, characterized in that It also includes a power supply module connected to the mechanical arm module, the mechanical arm posture control module, the force acquisition module and the dynamic adaptive hybrid impedance controller.

3. The system according to claim 1 or 2, characterized in that: The robot arm module comprises a robot arm, a servo motor and an end effector, wherein the end effector is arranged on the robot arm and connected to the servo motor.

4. The system according to claim 3, characterized in that The force acquisition module is implemented by a force sensor, and the force sensor is arranged at the end of the mechanical arm to collect the force and torque applied to the end effector.

5. The system according to claim 4, characterized in that The dynamic adaptive hybrid impedance controller includes a system and parameter initialization module, a mechanical arm information processing module, a force information processing module, a dynamic update rate calculation module, an impedance function calculation module, an adaptive compensation rate calculation module and a desired end position calculation module; The robot arm information processing module is used to process the robot arm posture information transmitted by the robot arm posture control module, and transmit the processed data information to the dynamic update rate calculation module; The system and parameter initialization module is used to initialize the impedance parameters and the expected contact force information, and transmit the initialized data information to the dynamic update rate calculation module; The force information processing module is used to unpack the force data collected by the force collection module, convert the data format, and transmit the converted data information to the dynamic update rate calculation module; The dynamic update rate calculation module is used to calculate the real-time update rate σ according to the processed data information and the converted force data information, and transmit the real-time update rate σ to the adaptive compensation rate calculation module; The adaptive compensation rate calculation module is used to calculate the real-time adaptive compensation rate ρ according to the converted six-dimensional force information, the impedance parameter and the dynamic update rate σ, and transmit the real-time updated dynamic adaptive impedance parameter to the impedance function calculation module; The impedance function calculation module is used to calculate the posture of the robotic arm at the next moment according to the converted six-dimensional force information, the expected contact force information, the updated adaptive impedance parameters and the posture information of the robotic arm at the previous moment, and transmit the calculated posture information to the expected end posture calculation module; The desired end position and posture calculation module is used to output the Cartesian coordinates of the next moment position and posture of the robot arm according to the position and posture information, and transmit it to the robot arm position and posture control module.

6. The system according to claim 5, characterized in that The robotic arm posture control module includes an inverse solution module, which converts the received Cartesian coordinates into joint coordinates of the robotic arm.

7. The system according to claim 3, characterized in that The robotic arm is a joint coordinate of a six-degree-of-freedom robotic arm.

8. The system according to claim 1, characterized in that The dynamic adaptive hybrid impedance controller is implemented on the PC side, and communication connection and data processing are performed in a C++ environment.

9. A control method for a compliance tracking system of a robotic arm, characterized in that: The method is implemented by the system according to any one of claims 1 to 8 and comprises the following steps: S1. Start the system, start the robotic arm and the robotic arm posture control module in the robotic arm module, and the end effector on the robotic arm moves under the drive of the servo motor; S2. The force acquisition module acquires the force and torque information of the end effector during movement at the current moment, and transmits the information to the dynamic adaptive hybrid impedance controller; S3. The dynamic adaptive hybrid impedance controller calculates the Cartesian coordinates of the next moment posture of the robot arm according to the force and torque information, the robot arm posture information, the impedance parameters and the expected contact force information, and transmits it to the robot arm posture control module; S4. The robotic arm posture control module converts the Cartesian coordinates into the joint coordinates of the robotic arm and sends them to the robotic arm, and the robotic arm moves according to the received joint coordinates.

10. The method according to claim 9, characterized in that The dynamic adaptive hybrid impedance controller uses a dynamic adaptive hybrid impedance control strategy to perform calculations, and the strategy includes: H1. Establish an adaptive hybrid impedance control algorithm formula and calculate the real-time position error e(t) based on the force tracking error ΔF: in, m d : The desired inertia parameters of the controller; b d : The desired damping parameter of the controller; ΔF: The error between the actual contact force and the expected force; e(t): The current position tracking error of the end of the robot arm, correspondingly, is the current speed tracking error, Then it is the current acceleration tracking error; ρ: Adaptive compensation law of force tracking error, which is dynamically adjusted through force tracking error and is calculated by the following formula: Where, T: sampling period; b: initial value of environmental stiffness; σ: update rate; f d (tT): the expected power at the previous moment; f e (tT): actual contact force at the last moment; ρ(tT): adaptive compensation rate at the previous moment; ρ(t): adaptive compensation rate at the current moment; H2. Use the following formula to calculate the position command at the next moment: P c (t)=P d (t)+e(t); in: e(t): current position tracking error of the end of the robotic arm; P d (t): expected position; P c (t): the command position output at the current moment; According to the position error e(t), the expected position P d (t) Real-time update to obtain the command position P c (t), which is tracked by the position-based robot servo control system, which adjusts the position of the robot end so that the contact force continuously tracks the desired force.

Images