A smooth contact compliance control method based on depth map

Abstract

The application belongs to the technical field of robot automatic control, and particularly relates to a smooth contact compliance control method based on a depth map, step 1: collecting an environment depth map through a depth camera installed at the end of a robot, extracting the depth value of a central region pixel and calculating an average distance z p , as an actual distance of an object and the end of the robot; step 2: generating a virtual force f p according to the actual distance z p and a preset safety distance threshold r by using a quadratic function model; step 3: designing a virtual admittance contact impedance controller in series, the force in the admittance control part being a virtual force calculated by an output of a distance sensor; and step 4: selecting appropriate admittance and impedance parameters, so that the contact force is smoothly increased immediately after contact and finally transits to a contact impedance force defined by the impedance control part, and the application realizes smooth transition without impact when the robot contacts an environment and stably applies a desired force after contact.

Description

Technical Field

[0001] This invention belongs to the field of robot automatic control technology, specifically relating to a smooth contact compliance control method based on depth maps. Background Technology

[0002] In many robotic applications, robots need to handle the transition from free motion (non-contact) to constrained motion (contact), such as the contact between a robotic arm's end effector and an object, the landing of a bionic legged robot, the landing of a drone, and collisions between collaborative robots and people or the environment. This contact transition is often accompanied by drastic changes in physical constraints, causing a shift in the robot's dynamics model. If not handled properly, the robot may become unstable or even lose control during the transition, posing safety hazards. On the other hand, the transition from non-contact to contact often generates significant impact forces, i.e., instantaneous impact loads. Excessive impact forces can damage the robot itself or the environment, severely affecting the system's stability and safety.

[0003] To mitigate the impact of impact at the moment of contact, traditional methods mainly fall into two categories: First, introducing flexible elements, such as installing pads at the ends or using compliant materials to absorb impact energy. However, the introduction of soft materials can lead to decreased system accuracy and high-frequency oscillations, and their complex nonlinear characteristics make control difficult, failing to provide a quantitatively controllable solution for impact. Second, employing force control strategies, typically indirect force control methods such as impedance control (or admittance control). By adjusting the parameters of the impedance controller, impacts can be buffered under "soft" impedance (low stiffness, high damping) or the requirements for precise operation can be met under "hard" impedance (high stiffness). However, limited by the dynamic response speed of the force control system itself, when the frequency of the impact exceeds the bandwidth of the control system, the effect of impedance control in mitigating impact is limited. In other words, traditional impedance control struggles to generate sufficient reaction force in a timely manner to completely eliminate the impact in high-speed collisions.

[0004] In recent years, research has focused on incorporating distance sensors near the robot's end effector to anticipate impending contact. By converting the distance measured by the sensor into a virtual force input, the robot can decelerate before actual contact occurs, thus reducing impact. These methods typically use optical proximity sensors (such as infrared or laser rangefinders) to obtain the distance between the end effector and the object. The sensor output is then used to calculate a virtual impedance force, which serves as the input to an impedance controller. The introduction of this virtual force can alleviate the impact problem inherent in classical impedance control to some extent, achieving the goal of reducing contact impact force. However, in existing methods, the impedance control strategy operates simultaneously in both the pre- and post-collision phases, leading to coupling interference between the impact reduction task and the contact force control task. To balance both, compromises are often necessary on control parameters, or the control law needs to be switched at the moment of contact. The former degrades system performance in a certain aspect, while the latter, like general switching control, may introduce instability. For example, traditional force control methods may cause the robot to move too fast during the free motion phase, resulting in an impact force far exceeding the desired contact force at the moment of contact. This could not only damage the object or the robot but also cause unstable oscillations in the system.

[0005] In summary, existing technologies for smooth contact control of robots have the following shortcomings: (1) They lack effective means to eliminate the impact force at the moment of contact, making it difficult to guarantee safety during high-speed collisions; (2) Control strategies often require a trade-off between buffering impact and precise force control, or introduce additional complexity and potential instability through controller switching; (3) Sensor information is not fully utilized, failing to achieve optimized control throughout the contact process. To solve the above problems, it is necessary to provide a smooth contact compliance control method based on depth maps, which can achieve a smooth transition without impact when the robot contacts the environment and stably apply the desired force after contact. Summary of the Invention

[0006] The purpose of this invention is to provide a smooth contact compliance control method based on depth maps to solve the problems of excessive impact force and unstable control when robots transition from free movement to contact in the prior art. This method can mitigate the impact and smooth the transition during the contact process between the robot's end effector and the environment, and can accurately control the force after contact, thereby improving the safety and reliability of the robot when performing delicate tasks and human-robot interaction.

[0007] The specific technical solution adopted by this invention is as follows: A depth map-based smooth contact compliance control method employs a series virtual admittance-contact impedance control structure to achieve smooth transition of the collaborative robot's end effector along a predetermined direction. The method includes the following steps: Step 1: Depth Information Acquisition: Acquire environmental depth maps using a depth camera installed at the robot's end effector, extract depth values ​​from pixels in the central region, and calculate the average distance. , which is the actual distance between the object and the robot's end effector; Step 2: Virtual force generation: Based on the actual distance and preset safe distance threshold Virtual forces are generated using a quadratic function model. ; Step 3: Series Virtual Admittance-Contact Impedance Controller: The design of the series virtual admittance contact impedance controller uses virtual forces calculated from the distance sensor output in the admittance control section. Step 4: Smooth Transition Parameter Design: Select appropriate admittance and impedance parameters so that the contact force increases smoothly immediately after contact and eventually transitions to the contact impedance force defined by the impedance control section.

[0008] The technical effects achieved by this invention are as follows: Traditional force control methods often generate excessively rapid motion during the free motion phase, resulting in an impact force far exceeding the desired contact force upon contact. This impact force can not only damage the object or robot itself but also cause unstable oscillations in the system. This method, by introducing a virtual admittance component, enables its force control algorithm to avoid the excessively rapid motion generated by traditional force control methods before contact during the free motion phase. This method effectively reduces contact impact force, ensuring a smoother contact process and more precise force control.

[0009] This invention addresses the problems of excessive impact and unstable control when robots transition from free movement to contact in existing technologies. It achieves impact mitigation and smooth transition during the contact process between the robot's end effector and the environment, and enables precise control of the force after contact, thereby improving the safety and reliability of the robot when performing delicate tasks and human-robot interaction. Attached Figure Description

[0010] Figure 1 The schematic diagram of the core control structure provided in the embodiment of the present invention includes a depth information acquisition module, a virtual force generation module, a virtual admittance control module, and a contact impedance control module; Figure 2 This is a schematic diagram of a virtual force generation method provided in an embodiment of the present invention; Figure 3 A schematic diagram of the contact task experimental apparatus provided in an embodiment of the present invention; Figure 4 A schematic diagram illustrating the change between the expected and actual positions in the Z-direction during a collision experiment of a robotic arm, provided in an embodiment of the present invention. Figure 5 A schematic diagram of the estimated contact force variation in the Z direction during a collision experiment of a robotic arm provided in an embodiment of the present invention; Figure 6This is a schematic diagram illustrating the change between the desired and actual positions in the Z-direction during a force control task of a robotic arm, provided in an embodiment of the present invention. Figure 7 This is a schematic diagram illustrating the changes in the expected force and estimated actual external force in the Z-direction during a force control task of a robotic arm, as provided in an embodiment of the present invention.

[0011] Figure 8 This is a schematic diagram of parameters in the force control task of the robotic arm provided in an embodiment of the present invention. Detailed Implementation

[0012] To make the objectives and advantages of this invention clearer, the invention will be specifically described below with reference to embodiments. It should be understood that the following text is merely used to describe one or more specific embodiments of the invention and does not strictly limit the scope of protection specifically claimed by the invention.

[0013] like Figures 1-8 As shown in the figure, a smooth contact compliance control method based on depth maps is proposed. This method employs a series virtual admittance-contact impedance control structure to achieve smooth transition of the collaborative robot's end effector along a predetermined direction. The specific steps include: Step 1: Depth Information Acquisition: Acquire environmental depth maps using a depth camera installed at the robot's end effector, extract depth values ​​from pixels in the central region, and calculate the average distance. , which is the actual distance between the object and the robot's end effector; In step 1, depth information is acquired using a depth camera ranging method, where the distance between the object and the robot's end effector is measured. Greater than When the object is outside the virtual plane, the virtual resistance force is... ; Measuring distance Always satisfied ,when When that happens, contact occurs.

[0014] Step 2: Virtual force generation: Based on the actual distance and preset safe distance threshold Virtual forces are generated using a quadratic function model. ; In step 2, the virtual force should exhibit smooth changes without abrupt changes, and must satisfy the condition that... and hour, The virtual force is calculated according to formula (1): (1) Equation (1) shows the virtual force It's about distance. The quadratic function, in The maximum value is reached at this point. , and when or When, satisfy Furthermore, the virtual force defined in equation (1) has smoothness, ensuring that the object will not undergo abrupt changes after entering the safe plane, thereby avoiding instability caused by drastic changes in force.

[0015] Step 3: Series Virtual Admittance-Contact Impedance Controller: The design of the series virtual admittance contact impedance controller uses virtual forces calculated from the distance sensor output in the admittance control section. In step 3, the virtual admittance control and contact impedance control are in series. The output of the admittance control serves as the desired input for the impedance control. The admittance control part uses the following equation of motion to calculate the impedance... Impact on motion output: (2) in and These are the desired inertia, damping, and stiffness of the admittance control component; and It is the motion output of admittance. That is, the compliance coordinates; in traditional admittance control, the calculated compliance coordinates are tracked using a position / velocity controller; the control framework used employs impedance control for tracking, i.e. (3) (4) It is the physical external force after contact, which is measured by an observer or sensor; the impedance dynamic formula (3) is controlled by the following law. Substitution formula (4) to achieve: (5) The Laplace transform of equation (2) is as follows: (6) in ,and We obtain the following from equation (6): (7) Similarly, by solving the Laplace transform of equation (3), we obtain: (8) in Combining equations (2) and (3), we get: (9) According to formula (9). The effect on motion is determined solely by the parameters of the admittance control section, while The effect is determined solely by the parameters of the impedance control section.

[0016] Step 4: Smooth Transition Parameter Design: Select appropriate admittance and impedance parameters so that the contact force increases smoothly immediately after contact and eventually transitions to the contact impedance force defined by the impedance control section.

[0017] In step (4), appropriate admittance and impedance parameters are derived by analyzing the general solution of the dynamic equation of admittance after contact and the contact force transition term. set up When in contact, virtual force Equation (2) is expressed in the following form: (10) In the formula , ;set up The time after contact; if Then the general solution of equation (10) is as follows: (11) in ,and ; This indicates that formula (10) is the critical damping, possessing the fastest convergence rate without oscillation; if Equation (2) determines the robot's motion after contact; if Equation (2) determines the transition characteristics after contact; the time derivative of equation (11) is: (12) (13) From (3), (11), (12) and (13), we obtain: (14) Initial contact force By Substituting into equation (14), we get: (15) in , , The initial conditions here assume that the tracking error caused by the impedance control part can be ignored; Equation (8) shows that the assumption is satisfied; subsequent experimental results show that the effect of violating the assumption is negligible; the first term on the right side of Equation (15) is the inertial force determined by the impedance control part; the second term is affected by the initial position and velocity, and even if the impact is successfully reduced, the second term will generate an undesirable contact force. The first term is eliminated by modifying the impedance control part; the term is eliminated by deleting it from equation (5). Redefining : (16) At this point, (14) becomes: (17) in: (18) The initial value (15) becomes: (19) Due to the eliminated items It is the acceleration feedforward term of equation (5), so the system has a certain delay; the difference between equation (17) and traditional impedance control is that it adds a term caused by the virtual admittance part. Its initial and final values ​​are: (20) (twenty one) For the contact force to increase smoothly after contact and eventually transition to the contact resistance force defined by the impedance control section, the following condition must be met. or However, improper selection of control parameters may lead to... This condition is not met; the following analysis will follow. The extreme values ​​are determined to satisfy the conditions within the above range; since extreme value analysis involves evaluating inequalities, the initial state sign must be determined; the occurrence of contact includes various scenarios, such as the object gradually approaching the robot and eventually making contact, and the robot actively approaching the object and making contact; in each case, the parameters... and The notation between cases remains consistent, and the analytical results for all cases are equivalent; in the following analysis, it is assumed that... and All signs are positive; the derivative of equation (18) with respect to time yields: (twenty two) (twenty three) set up for The time; then or: (twenty four) From (23) and (24), we get: (25) because The positive and negative are only related to Relevant, namely: (26) According to (26), when hour, It has a maximum value, that is: (27) In the formula Because the second term on the right side of equation (24) is positive, therefore It is always positive; therefore, if condition (27) holds, then It has a maximum value after contact, which does not meet the condition for a smooth transition; On the other hand, when hour, It has a local minimum value, that is: (28) At this point, the second term on the right side of equation (24) is negative, so It can be positive or negative; if... If the contact value is zero, then there is no minimum value after contact; however, directly from... The derived parameter conditions clearly include the initial state; however, these initial states are unknown during parameter design; therefore, considering the contrapositive, a sufficient condition will be derived below; assume... Then we get the following equation: (29) because ,and (29) indicates: (30) because ,and Equation (30) can be written as: (31) Since the right side of equation (31) is always negative, under the premise that equation (28) holds, equation (31) proves that: if but Therefore, according to the contrapositive, we can conclude that, under the premise that equation (28) holds, if but Finally, combine the conditional expression (27). The parameter conditions are as follows when there is neither a maximum nor a minimum value after contact: (32) Notice, These are conservative conditions; if the initial state is obtained, it can be solved directly. To solve for the necessary conditions; equation (32) can be rearranged as: (33) Equation (33) shows the design conditions for controller parameters that satisfy smooth connection.

[0018] In this invention, to verify the effectiveness and practicality of the algorithm, a real-world experiment was conducted on a 7DOF Franka Emika Panda robot to verify that the robotic arm can perform smooth contact collision experiments, compliant force control tasks, and ensure safety during human-robot interaction. The algorithm used in the experiment was implemented on a laptop computer using the ROS environment and written in C++. Communication with the Franka Emika Panda robot was achieved using the libfranka and frankaros libraries. Before the experiment began, the Franka Emika Panda robotic arm was returned to its initial position with the following joint angles: .

[0019] The experiment simulates the situation where the end effector of a collaborative robot actively contacts an obstacle in the Z-direction, such as... Figure 3 As shown, the robot uses an impedance controller to track its desired position. Initially, the desired position is above the obstacle, and then its desired position in the Z direction is adjusted to be below the obstacle. Therefore, the robot will eventually collide with the obstacle.

[0020] At the start of the experiment, the desired position in the Z direction Set to 0.48m, adjust to 0.12m after approximately 5 seconds, desired speed. Set to 0. Since the desired position is below the obstacle, the robot collides and stops during its movement, causing the tracking error to fail to converge. At this point, the robot dynamically applies corresponding pressure to the obstacle based on impedance control.

[0021] Figure 4 This demonstrates the change in the distal end in the Z direction during this experiment. Figure 5 The Z-direction external force measured using a momentum observer is presented. Experimental results show that after adjusting the desired position, the robot end effector moves rapidly downwards using this method. However, traditional impedance control generates an impact force of nearly 28N at the moment of contact, which then gradually decreases and stabilizes at 12N. In contrast, this method can decelerate in advance during the motion, achieving smooth contact and mitigating 100% of the impact. The contact force gradually increases from 0 to 12N, avoiding the impact phenomenon in traditional impedance control. This result demonstrates that this method can effectively reduce the impact force and achieve a smooth transition from free motion to contact impedance control.

[0022] exist Figure 5 In the process, the estimated external force exhibited significant noise during the 5-7s time period. This is because the robot is currently in an accelerated motion state. While the momentum observer can provide a relatively accurate estimate when stationary, during non-uniform motion, the estimation results are affected by dynamic changes due to errors between the inertial force and Coriolis force terms and the actual values, as well as interference from velocity and acceleration noise. This results in noticeable noise.

[0023] The experiment simulates force-controlled operation. Based on a given desired force, the robot moves along the force-controlled degrees of freedom in a free motion space until it makes contact with an object. After contact, the robot slowly increases the contact force until it reaches the predetermined desired value.

[0024] Traditional force control methods often generate excessively rapid motion during the free motion phase, resulting in an impact force far exceeding the desired contact force upon contact. This impact force can not only damage the object or robot itself but also cause unstable oscillations in the system. This method, by introducing a virtual admittance component, enables its force control algorithm to avoid the excessively rapid motion generated by traditional force control methods before contact during the free motion phase. This method effectively reduces contact impact force, ensuring a smoother contact process and more precise force control.

[0025] The contact force generated by impedance control during contact is influenced by the environmental stiffness, the stiffness of the impedance control system, and the difference between the desired and actual positions. This experiment achieves force error convergence by adjusting the desired position of the impedance control component; the specific formula is as follows: (34) (35) in, To control the cycle, It is a constant. This represents the velocity caused by force tracking error.

[0026] As can be seen from equations (34) and (35), when the actual contact force is less than the desired force, the system adjusts the desired position toward the direction of the contacting object, thereby increasing the contact force; conversely, when the actual contact force is greater than the desired force, the system adjusts the desired position toward the opposite direction of the contacting object, thereby reducing the contact force. In this way, the system can dynamically adjust the desired position and achieve force error convergence without relying on environmental stiffness and position information.

[0027] During the experiment, the initial value of the desired position in the Z direction was set to 0.48m, and the desired contact force was set to 6N. Subsequently, the desired position was adjusted according to the desired force according to equations (34) and (35) until the robot made contact with the object and force control adjustment began. In this experiment, the control cycle was... The duration is 1ms. The impedance control parameters and virtual admittance parameters used in the experiment are as follows: Figure 8 As shown, constant The value is 0.001.

[0028] During the experiment, the actual location Output of the virtual admittance section (compliant coordinates) and desired location Changes such as Figure 6 As shown, there is a certain error between the actual position and the compliant coordinates, mainly due to friction and some unmodeled dynamic effects. Impedance control requires accurate compensation for coupling and nonlinear terms in robot dynamics; otherwise, control errors may occur.

[0029] The changes in contact force and expected force estimated by the momentum observer are as follows: Figure 7 As shown, the estimated external force gradually converges to the desired force of 6N after contact, and no impact force much greater than the desired force is generated throughout the process. After the force control error converges, the desired position stops changing, and the robot tends to a steady state.

[0030] The above description is merely a preferred embodiment of the present invention. It should be noted that those skilled in the art can make various improvements and modifications without departing from the principles of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. Structures, devices, and operating methods not specifically described or explained in this invention are implemented according to conventional methods in the art unless otherwise specified or limited.

Claims

1. A method for smooth contact compliance control based on depth maps, characterized in that: A series virtual admittance-contact impedance control structure is used to achieve compliant control and smooth transition of the end effector of a collaborative robot along a predetermined direction. The specific steps include: Step 1: Depth Information Acquisition: Acquire environmental depth maps using a depth camera installed at the robot's end effector, extract depth values ​​from pixels in the central region, and calculate the average distance. , which is the actual distance between the object and the robot's end effector; Step 2: Virtual force generation: Based on the actual distance and preset safe distance threshold Virtual forces are generated using a quadratic function model. ; In step 2, the virtual force should exhibit smooth changes without abrupt changes, and must satisfy the condition that... and hour, The virtual force is calculated according to formula (1): （1） Equation (1) shows the virtual force It's about distance. The quadratic function, in The maximum value is reached at this point. , and when or When, satisfy ; Step 3: Series Virtual Admittance-Contact Impedance Controller: Design of the series virtual admittance controller and contact impedance controller. The input in the admittance control section is a virtual force generated using a quadratic function model. ; Step 4: Smooth Transition Parameter Design: Select appropriate admittance and impedance parameters so that the contact force increases smoothly immediately after contact and eventually transitions to the contact impedance force defined by the impedance control section.

2. The method for smooth contact compliance control based on depth maps according to claim 1, characterized in that: In step 1, depth information is acquired using a depth camera ranging method, where the distance between the object and the robot's end effector is measured. Greater than When the object is outside the virtual plane, the virtual resistance force is... ; Measuring distance Always satisfied ,when When that happens, contact occurs.

3. The method for smooth contact compliance control based on depth maps according to claim 2, characterized in that: In step 3, the virtual admittance control and contact impedance control are in series. The output of the admittance control serves as the desired input for the impedance control. The admittance control part uses the following equation of motion to calculate the impedance... Impact on motion output: （2） in and These are the desired inertia, damping, and stiffness of the admittance control component; and It is the motion output of admittance. That is, compliant coordinates; The velocity of the compliant coordinates, The acceleration of the compliant coordinates; in conventional admittance control, the calculated compliant coordinates are tracked using a position / velocity controller; the control framework used employs impedance control for tracking, i.e. （3） （4） It is the physical external force after contact, which is measured by an observer or sensor; the impedance dynamic formula (3) is controlled by the following law. Substitution formula (4) to achieve: （5） The Laplace transform of equation (2) is as follows: （6） in ,and We obtain the following from equation (6): （7） Similarly, by solving the Laplace transform of equation (3), we obtain: （8） in Combining equations (2) and (3), we get: （9） According to formula (9). The effect on motion is determined solely by the parameters of the admittance control section, while The effect is determined solely by the parameters of the impedance control section.

4. The method for smooth contact compliance control based on depth maps according to claim 3, characterized in that: In step 4, appropriate admittance and impedance parameters are derived by analyzing the general solution of the dynamic equation of admittance after contact and the contact force transition term. set up When in contact, virtual force Equation (2) is expressed in the following form: （10） In the formula , ;set up The time after contact; if Then the general solution of equation (10) is as follows: （11） in ,and ; This indicates that formula (10) is the critical damping, possessing the fastest convergence rate without oscillation; if Equation (2) determines the robot's motion after contact; if Equation (2) determines the transition characteristics after contact; the time derivative of equation (11) is: （12） （13） From equations (3), (11), (12), and (13), we obtain: （14） Initial contact force By Substituting into equation (14), we get: （15） in , , The initial conditions here assume that the tracking error caused by the impedance control section can be ignored. Equation (8) shows that the assumption is satisfied; the first term on the right side of equation (15) is the inertial force determined by the impedance control part; the second term is affected by the initial position and velocity, and even if the impact is successfully reduced, the second term will generate an undesirable contact force; The first term is eliminated by modifying the impedance control part; the term is eliminated by deleting it from equation (5). Redefining : （16） At this point, equation (14) becomes: （17） in: （18） The initial value (15) becomes: （19） Due to the eliminated items It is the acceleration feedforward term of equation (5), so the system has a certain delay; the difference between equation (17) and traditional impedance control is that it adds a term caused by the virtual admittance part. Its initial and final values ​​are: （20） （21） For the contact force to increase smoothly after contact and eventually transition to the contact resistance force defined by the impedance control section, the following condition must be met. or ; parameters in each case and The notation between cases remains consistent, and the analytical results for all cases are equivalent; in the following analysis, it is assumed that... and All signs are positive; the derivative of equation (18) with respect to time yields: （22） （23） set up for The time; then or: （24） From equations (23) and (24), we get: （25） because The positive and negative are only related to Relevant, namely: （26） According to equation (26), when hour, It has a maximum value, that is: （27） In the formula Because the second term on the right side of equation (24) is positive, therefore It is always positive; therefore, if condition (27) holds, then It has a maximum value after contact, which does not meet the condition for a smooth transition; On the other hand, when hour, It has a local minimum value, that is: （28） At this point, the second term on the right side of equation (24) is negative, so It can be positive or negative; if... If the contact value is zero, then there is no minimum value after contact; however, directly from... The derived parameter conditions obviously include the initial state; however, these initial states are unknown during parameter design; therefore, considering the contrapositive, a sufficient condition will be derived below; assume... Then we get the following equation: （29） because ,and Equation (29) means: （30） because ,and Equation (30) can be written as: （31） Since the right side of equation (31) is always negative, under the premise that equation (28) holds, equation (31) proves that: if but ; Therefore, according to the contrapositive, we can conclude that, under the premise that equation (28) holds, if but Finally, combine the conditional expression (27). The parameter conditions are as follows when there is neither a maximum nor a minimum value after contact: （32） Notice, These are conservative conditions; if the initial state is obtained, it can be solved directly. To solve for the necessary conditions; equation (32) can be rearranged as: （33） Equation (33) shows the design conditions for controller parameters that satisfy smooth connection.

Images