A Robot Singularity Control Method and System Based on Admittance Algorithm

Abstract

The application discloses a robot singularity control method and system based on an admittance algorithm. The robot force control is realized through the admittance algorithm, whether the robot enters a singularity area is judged through a robot joint angle, when the robot is in the singularity, the method of locking the joint to reduce the dimension of the degree of freedom is adopted, and the corresponding input and output values are processed, so that the robot smoothly passes through the singularity point in the Cartesian space, and the robot has a larger and more continuous working space while passing through the singularity point.

Description

Technical Field

[0001] This invention relates to the field of robot control, and in particular to a robot singularity control method and system based on admittance algorithm. Background Technology

[0002] In robot force control, if the robot moves within Cartesian space, it will inevitably encounter singularities. Singularities cause a decrease in the rank of the robot's Jacobian matrix, leading to uncontrollable or infinitely large forces or movements in certain directions at the robot's end effector. This can cause even small force inputs to result in large-scale uncontrollable vibrations at the end effector or joints, or cause the control system to diverge, resulting in collisions with the environment or damage to joint actuators. To avoid the effects of singularities, conventional Cartesian space robot force control restricts the robot's joint range of motion to prevent the robot from entering singularities or singular regions. To avoid motion instability when the robot enters or exits singularities, the range of motion of the robot's joints is usually limited, allowing the robot to move within a relatively stable workspace and avoiding entry into singularities or singular regions. However, limiting the range of motion of the joints has a significant impact on the robot's workspace, restricting the robot's reachable area and potentially affecting the continuity of the robot's end effector trajectory, causing a loss of robot performance and reduced efficiency, and affecting the stability and operability of the entire system.

[0003] In summary, a robot singularity control method and system based on admittance algorithm is needed to address the shortcomings of existing technologies. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a robot singularity control method and system based on admittance algorithm, aiming to solve the aforementioned problems.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a robot singularity control method based on admittance algorithm, comprising the following steps:

[0006] Step S1: Force signal acquisition. Apply a force to the robot end effector and acquire the end effector force signal in real time through a force sensor.

[0007] Step S2: Execute the admittance algorithm to calculate the desired acceleration at the end effector. Input the collected force signal into the admittance controller to calculate the desired acceleration of the robot's end effector in Cartesian space.

[0008] Step S3: Real-time determination of whether a singular region has been entered. Based on the current joint angles and movement trends of the robot, real-time determination of whether a singular region has been entered and determination of the singularity type.

[0009] Step S4: Dimensional reduction control. Based on the singularity type, stable control with reduced degrees of freedom is achieved through joint locking strategy and force input correction strategy.

[0010] Step S5: Continuous monitoring and degree of freedom recovery. Continuously detect whether the robot is in a singular region. If it leaves the singular region, release the restrictions on all locked joints and restore the original Jacobian matrix and complete degree of freedom control.

[0011] Step S6: Control commands are issued, and normal admittance control continues. The final calculated joint position and velocity commands are sent to the robot joint actuator for smooth motion control.

[0012] Optionally, the singularity types in step S3 include shoulder singularity, elbow singularity, and wrist singularity, and the criteria for determining shoulder singularity are:

[0013] The center point of the fifth joint of the six-dimensional robotic arm is close to the cylindrical region of the first joint rotation axis in the base coordinate system. The minimum distance from the center point of the fifth joint to the first joint rotation axis is calculated. If the minimum distance is less than the distance threshold, a shoulder singularity is triggered.

[0014] Optionally, the criteria for determining the elbow singularity are:

[0015] Calculate the angle of the third joint of the six-dimensional robotic arm. If the angle of the third joint is less than the angle threshold, an elbow singularity is triggered.

[0016] Optionally, the criteria for determining wrist anomalies are:

[0017] Calculate the angle of the fifth joint of the six-dimensional robotic arm. If the angle of the fifth joint is less than the angle threshold, or if the fourth axis and the sixth axis are coaxial, then a wrist singularity is triggered.

[0018] Optionally, in step S4, stability control is performed based on the singularity type, through the following methods:

[0019] Lock the corresponding joint, adjust the Jacobian matrix, calculate the motion of the remaining joints using the adjusted Jacobian matrix, and output control commands.

[0020] Optionally, the unusual treatment of the shoulder can be performed in the following ways:

[0021] Step A1: Set the output position change and output speed of the first joint of the six-dimensional robotic arm to zero and lock the joint;

[0022] Step A2: Identify the unit vector of the most sensitive direction at the end effector, set the force component to zero to correct the end effector force, re-introduce it into the admittance controller, and obtain the new end effector acceleration;

[0023] Step A3: Delete the column corresponding to the first joint in the original Jacobian matrix, or set its corresponding row elements to zero to obtain the pseudo-inverse Jacobian matrix;

[0024] Step A4: Calculate the acceleration, velocity, and displacement of the remaining joints using the pseudo-inverse Jacobian matrix, send motion commands to the remaining joints, and continue executing the task.

[0025] Optionally, the elbow anomaly is addressed in the following manner:

[0026] Set the output position change and output speed of the second joint of the six-dimensional robotic arm to zero, lock the joint, set the row element corresponding to the second joint in the Jacobian matrix to zero, calculate the speed and displacement of the remaining joints, and output control commands.

[0027] Optionally, the unusual wrist treatment is performed in the following ways:

[0028] Set the output position change and output speed of the fourth joint of the six-dimensional robotic arm to zero, lock the joint, separate and extract the torque component in the Z-axis direction of the end-effector input torque, calculate the motion increment of the sixth joint, set the corresponding row element of the fourth joint in the Jacobian matrix to zero, calculate the motion commands of other joints except the fourth and sixth joints, and output the motion increment of the sixth joint together with the other joint commands.

[0029] Optionally, the condition for escaping the singular region in step S5 is:

[0030] The condition for detaching from the shoulder singularity: the center point of the fifth joint of the six-dimensional robotic arm leaves the shoulder singularity area, the first joint is unlocked, and all degrees of freedom and normal control are restored;

[0031] The unusual disengagement condition of the elbow: When the angle of the third joint of the six-dimensional robotic arm is far from zero degrees, the second joint lock is released, and all degrees of freedom and normal control are restored.

[0032] The unusual release condition of the wrist: When the angle of the fifth joint of the six-dimensional robotic arm is far from zero degrees, the fourth joint lock is released, and all degrees of freedom and normal control are restored.

[0033] A robot singularity control system based on admittance algorithm, employing the robot singularity control method based on admittance algorithm, includes a force signal acquisition module, an admittance algorithm calculation module, a singularity region judgment module, a dimension reduction control module, a degree of freedom recovery module, and a control command issuance module;

[0034] Force signal acquisition module, used to acquire external forces at the robot end effector in real time;

[0035] The admittance algorithm module is used to convert the acquired force signal into the desired terminal acceleration in Cartesian space;

[0036] The singular region detection module is used to monitor the robot's current state in real time and determine whether it has entered a singular region;

[0037] The dimensionality reduction control module is used to execute a degree-of-freedom dimensionality reduction control strategy when a singular state is detected.

[0038] The degree-of-freedom recovery module is used to continue monitoring the robot's status, determine whether it has escaped the singular region, and restore full degree-of-freedom control;

[0039] The control command sending module is used to send the final calculated joint position and velocity commands to the actuators of each joint of the robot.

[0040] The beneficial effects of this invention are:

[0041] 1. In this invention, the force control of the robot is realized through the admittance algorithm, and the robot is judged to enter the singular region by the robot joint angle. When the robot is in the singular, the degree of freedom is reduced by locking the joints and the corresponding input and output values ​​are processed so that the robot can smoothly pass through the singular point in Cartesian space. While being able to pass through the singular point, the robot has a larger and more continuous robot workspace.

[0042] 2. In this invention, robot force control is achieved through admittance algorithm. To address the pain point that admittance force control cannot pass through singularities, a method of reducing degrees of freedom by locking joints is proposed. By restricting joints within a small range and with conditions, the admittance force control method can easily and smoothly pass through singularities in the workspace. This avoids the problems of not being able to pass through singularities and the large-scale restriction of the robot's workspace in conventional methods, enabling the admittance force control robot to achieve stable control in most of its workspace. Attached Figure Description

[0043] Figure 1 This is a schematic diagram of a force control system according to the present invention.

[0044] Figure 2 This is a schematic diagram of a system control structure according to the present invention.

[0045] Figure 3 This is a schematic diagram of a singular algorithm for judgment according to the present invention.

[0046] Figure 4 This is an example diagram of a singular case of a six-dimensional robotic arm according to the present invention. Detailed Implementation

[0047] To more clearly illustrate the technical solutions in the embodiments of the invention or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0048] like Figures 1 to 4 As shown, a robot singularity point control method and system based on admittance algorithm includes the following:

[0049] Robotic systems such as Figure 1 As shown, the system consists of an operator, a six-dimensional force sensor, and a robot. The six-dimensional force sensor is fixedly connected to the robot's end effector. This sensor acquires the force information generated when the operator contacts the robot's end effector. This force information is used as input to the control system. An admittance algorithm converts the end effector force into end effector acceleration. Through robot kinematics calculations, the end effector acceleration is finally calculated into the displacement and velocity of each joint of the robot, and then sent to the robot's joint motors to drive the robot's motion. Ultimately, the operator controls the robot to perform corresponding end effector trajectory movements by outputting force to the robot.

[0050] The system control structure diagram of the robot force control system based on admittance algorithm is as follows: Figure 2 As shown in the diagram, the force exerted by the operator on the robot is collected by a six-dimensional force sensor at the end effector of the master robot. This collected force information is input into the control system. In the system's control loop, the force information is input into the admittance algorithm of the master robot to obtain the desired end-effector acceleration in Cartesian space. Then, through a Cartesian space joint space solver, the desired joint acceleration in joint space is obtained. Integration yields the desired joint velocity and position. These are then used to calculate the desired end-effector velocity and position using the Jacobian matrix, which are input as feedback values ​​into the admittance algorithm. The desired joint position and velocity are output to the robot, enabling its motion.

[0051] The formula for calculating admittance is:

[0052]

[0053]

[0054]

[0055] Where F is the force input, and M, D, and S are the mass coefficient, damping coefficient, and stiffness coefficient, respectively. , x represents the acceleration, velocity, and displacement at the end point, t represents the unit time, and D represents the displacement. ratio Let f be the damping ratio, f and τ be the force and torque, and the subscripts x, y and z be the corresponding directions.

[0056] During actual operation of the robot force control system, the robotic arm will move arbitrarily within the workspace according to the actual task requirements. The robotic arm may enter or exit singular regions during movement or work, resulting in unstable motion, affecting operator use, or even causing dangerous events such as collisions. To avoid such situations, the control system will determine in real time whether the robot has entered or exited a singular region based on the robot's current joint angles and movement trends. Taking a 6R robotic arm with six rotational degrees of freedom as an example, based on the different joints that trigger singularities, they can be roughly divided into three situations: shoulder singularity, elbow singularity, and wrist singularity. The singularity trigger judgment process is as follows: Figure 3 As shown.

[0057] In singular configurations, the Jacobian matrix J loses its full rank, leading to irreversible velocity or force mappings in certain directions, causing joint velocities to approach infinity and force amplification. Locking joints to reduce degrees of freedom can preserve the original Jacobian matrix. After removing the columns corresponding to the locked joints, we obtain the Jacobian matrix after dimensionality reduction. Where n is the number of degrees of freedom of the robotic arm and m is the number of locked joints. The Jacobian matrix after dimensionality reduction is no longer full-rank missing, the solution of the inverse matrix or pseudo-inverse matrix is ​​stable, avoiding data divergence, and the mapping relationship between the remaining degrees of freedom and the end force is more stable, maintaining good controllability.

[0058] Joint angle value Divided into Two groups:

[0059]

[0060] in, To lock the joint, The joint is not locked. .

[0061] When the joint is locked:

[0062]

[0063] The end effector speed of the robotic arm is:

[0064]

[0065] in, To lock the Jacobian matrix of the joints, For the Jacobian matrix of the non-locking joint, This refers to the joint velocity.

[0066] The singularity is caused by the locked joint, i.e. The column caused the issue; after deleting the corresponding column... Usually, rank is restored, then used. Inverse kinematics yields the joint velocities or accelerations in singular cases:

[0067]

[0068] in, It is a pseudo-inverse matrix

[0069] Let the axis of the shoulder joint (axis 1) in the base coordinate system be the axis of the cylinder, and the radius threshold be... Let the position of the center point of joint 5 in the base coordinate system be... Minimum distance to the axis of the cylinder for:

[0070]

[0071] in The direction vector of axis 1, Let be any point on the axis of the cylinder.

[0072] When the center point of the 5 joints of the 6R robotic arm enters the shoulder singularity region represented by the dashed cylinder:

[0073]

[0074] Triggering shoulder singularity, such as Figure 4 As shown in 'a', to prevent the operator-applied end-effector force from causing instability in the rotation direction and speed of joint 1, the end-effector force input in the corresponding direction is set to zero to avoid the influence of the force in the most sensitive direction of the end-effector in singular cases.

[0075]

[0076] in For force input, It is the identity matrix. It is the unit vector in the direction most sensitive to the end.

[0077] To obtain the end-effector acceleration under shoulder singularity, the force input value after removing unstable directional forces is substituted into the admittance algorithm. And set the position change output and velocity output of joint 1 to zero, putting joint 1 into a temporary locked state:

[0078]

[0079] The robot's degrees of freedom are reduced in dimensionality, and the Jacobian matrix is ​​adjusted so that the elements in the rows corresponding to the locked joints are set to zero. Finally, the position changes of the remaining joints after the locking joints are locked are calculated and used to continue controlling the robot's movement. When the center of joint 5 leaves the shoulder singularity region, the locking state of joint 1 is released, and the robot returns to its normal movement state.

[0080] The formula for calculating the joint acceleration of the shoulder is:

[0081]

[0082]

[0083]

[0084] in These are joint acceleration and end-effector acceleration, respectively. It is a pseudo-inverse matrix. α is the element of the pseudo-inverse matrix, I is the damping factor, and I is the identity matrix.

[0085] The final output value is:

[0086]

[0087] Discrete implementation requires:

[0088]

[0089] in For joint velocity, This refers to joint displacement.

[0090] When the three joints of the 6R robotic arm reach near 0° during movement, an elbow singularity is triggered, such as... Figure 4 As shown in b, to prevent the operator-applied end force from causing instability in the rotational direction and speed of joint 3, the positional change of joint 2 is set to zero, putting joint 2 in a temporarily locked state:

[0091]

[0092] The robot's degrees of freedom are reduced in dimensionality, at which point the motion direction of the 3 joints will be unique and controllable. Then, the Jacobian matrix is ​​adjusted so that the elements of the row corresponding to the locked joint are set to zero. Finally, the position changes of the remaining joints after the locked joints are calculated and output, which are used to continue controlling the robot's motion. When the 3 joints retract or pass through the position near 0°, the locking state of the 2 joints is released, and the robot returns to its normal motion state.

[0093] The output value obtained by elbow singularity is:

[0094]

[0095]

[0096] When the 5th joint of the 6R robotic arm reaches near 0° during movement, a wrist singularity is triggered, such as... Figure 4 As shown in c, joints 4 and 6 are coaxial at this point and are simultaneously driven by the torque of the end force in the z-axis direction. This will cause a sudden change in motion speed and uncertainty in motion direction. To avoid the instability caused by this singularity, the position change of joint 4 is set to zero, putting joint 4 in a temporarily locked state.

[0097]

[0098] The robot's degrees of freedom are reduced in dimensionality. At this point, the torque in the z-axis direction of the end effector will only be reflected in 6 joints. The original value of the z-axis torque in the end effector force input is extracted and used to calculate the joint position change of 6 joints separately. Then, the original value is set to zero.

[0099]

[0100] The Jacobian matrix is ​​adjusted so that the elements in the row corresponding to the locked joint are set to zero. Finally, the position changes of the remaining joints after the locking joint is obtained and used to continue controlling the robot's movement. When joint 5 is no longer near 0°, the locking state of joint 4 is released, and the robot resumes normal movement.

[0101] The output value obtained by the wrist singularity is:

[0102]

[0103]

[0104] The change in joint position of joint 6 is as follows:

[0105]

[0106] Where Fτz is the torque input in the z-direction at the end, and Mw, Dw, and Sw are the mass coefficient, damping coefficient, and stiffness coefficient under wrist singularity, respectively.

[0107] Based on the aforementioned singular triggering conditions, output restrictions are imposed on the corresponding joints. A method of reducing degrees of freedom is used to stably traverse the singular point, and then the degrees of freedom and end effector force input are restored, allowing the robot to return to normal motion. Even when the aforementioned singular conditions are triggered simultaneously, the robot can still continue to move stably by locking the corresponding multiple joints.

[0108] The above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions or improvements made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. A robot singularity control method based on an admittance algorithm, characterized by, The method comprises the following steps: Step S1: force signal collection, exerting force on the end of the robot, and collecting the end force signal in real time through the force sensor; Step S2: performing a mobility algorithm to calculate the end desired acceleration, inputting the collected force signal into the mobility controller, and calculating the desired acceleration of the robot end in the Cartesian space; Step S3: judging whether to enter a singular region in real time, judging whether to enter a singular region and judging the singular type according to the current robot joint angle and motion trend in real time; Step S4: dimension reduction control, performing stable control of freedom reduction through joint locking strategy and force input correction strategy according to the singular type; Step S5: continuous monitoring and freedom recovery, continuously detecting whether the robot is in a singular region, and if the robot is out of the singular region, releasing the restrictions of all locked joints, recovering the original Jacobian matrix and complete freedom control; Step S6: control instruction issuing, continuing normal mobility control, and sending the finally calculated joint position and speed instruction to the robot joint driver for smooth motion control.

2. The robot singularity control method based on the admittance algorithm according to claim 1, characterized in that, The singular type in the step S3 comprises shoulder singular, elbow singular and wrist singular, and the judgment condition of the shoulder singular is that: The fifth joint center point of the six-axis robot is close to the cylindrical region of the first joint rotation axis in the base coordinate system, the minimum distance from the fifth joint center point to the first joint rotation axis is calculated, and if the minimum distance is less than the distance threshold, the shoulder singular is triggered. 3.The robot singularity control method based on the admittance algorithm of claim 2, wherein, The judgment condition of the elbow singular is that: The third joint angle of the six-axis robot is calculated, and if the third joint angle is less than the angle threshold, the elbow singular is triggered.

4. The robot singularity control method based on the admittance algorithm according to claim 2, characterized in that, The judgment condition of the wrist singular is that: The fifth joint angle of the six-axis robot is calculated, and if the fifth joint angle is less than the angle threshold or the fourth axis is coaxial with the sixth axis, the wrist singular is triggered.

5. The robot singularity control method based on the admittance algorithm according to claim 2, characterized in that, The stable control according to the singular type in the step S4 is performed through the following manner: Locking the corresponding joint, adjusting the Jacobian matrix, calculating the motion amount of the remaining joints through the adjusted Jacobian matrix, and outputting the control instruction.

6. The robot singularity control method based on the admittance algorithm according to claim 5, characterized in that, The processing of the shoulder singular is performed through the following manner: Step A1: setting the output position change amount and output speed of the first joint of the six-axis robot to zero, and locking the joint; Step A2: identifying the end most sensitive direction unit vector, correcting the end execution force by setting the force component to zero, reimporting the mobility controller, and obtaining a new end acceleration; Step A3: deleting the column corresponding to the first joint in the original Jacobian matrix or setting the corresponding row elements to zero, and obtaining a pseudo-inverse Jacobian matrix; Step A4: calculating the acceleration, speed and displacement of the remaining joints by using the pseudo-inverse Jacobian matrix, sending the motion instruction to the remaining joints, and continuing to execute the task.

7. The robot singularity control method based on the admittance algorithm according to claim 5, characterized in that, The processing of the elbow singular is performed through the following manner: Setting the output position change amount and output speed of the second joint of the six-axis robot to zero, locking the joint, setting the row elements corresponding to the second joint in the Jacobian matrix to zero, calculating the speed and displacement of the remaining joints, and outputting the control instruction. 8.The robot singularity control method based on the admittance algorithm of claim 5, wherein, The processing of the wrist singular is performed through the following manner: The output position change amount and output speed of the fourth joint of the six-dimensional robot arm are set to zero, joint locking is performed, the Z-axis direction torque component in the end input torque is separated and extracted, the motion increment of the sixth joint is calculated, the fourth joint corresponding row elements in the Jacobian matrix are set to zero, the motion instructions of the other joints except the fourth joint are calculated, and the motion increment of the sixth joint is output together with the instructions of the other joints.

9. The robot singularity control method based on an admittance algorithm according to claim 2, characterized in that, The condition for leaving the singular region in the step S5 is: The leaving condition of the shoulder singular region: the fifth joint center point of the six-dimensional robot arm leaves the shoulder singular region, the first joint locking is released, and all degrees of freedom and normal control are restored; The leaving condition of the elbow singular region: when the third joint angle of the six-dimensional robot arm is far away from zero degrees, the second joint locking is released, and all degrees of freedom and normal control are restored; The leaving condition of the wrist singular region: when the fifth joint angle of the six-dimensional robot arm is far away from zero degrees, the fourth joint locking is released, and all degrees of freedom and normal control are restored.

10. A robot singularity control system based on the admittance algorithm, employing the robot singularity control method based on the admittance algorithm according to any one of claims 1 to 9, characterized in that, The control system comprises a force signal acquisition module, an admittance algorithm calculation module, a singular region judgment module, a dimension reduction control module, a degree of freedom recovery module and a control instruction issuing module. The force signal acquisition module is used for real-time acquisition of the external force acting on the robot end; The admittance algorithm module is used for converting the collected force signal into the end desired acceleration in the Cartesian space; The singular region judgment module is used for real-time monitoring of the current state of the robot and judging whether the robot enters the singular region; The dimension reduction control module is used for detecting the singular state and executing the degree of freedom dimension reduction control strategy; The degree of freedom recovery module is used for continuing to monitor the state of the robot, judging whether the robot leaves the singular region, and recovering the complete degree of freedom control; The control instruction issuing module is used for sending the finally calculated joint position and speed instructions to the drivers of the joints of the robot.

Images