Six-axis mechanical arm singularity avoidance control method and device based on virtual seven degrees of freedom

Abstract

The invention discloses a singular avoidance control method and device for a six-axis mechanical arm based on virtual seven degrees of freedom. The tail end of the mechanical arm is provided with a virtual rotating shaft which coincides with the original point of a tail end coordinate system and is perpendicular to a rotating shaft of the tail end coordinate system. The control method comprises the steps that position information and angle information of all joints of the mechanical arm and the actual pose and expected pose of the tail end in the current detection period are obtained; based on the position information and the angle information of all the joints, the weight value of the virtual rotating shaft is correspondingly adjusted by combining the state that the mechanical arm approaches the shoulder singular point and / or the wrist singular point, a quadratic programming problem used for avoiding the singular points is reconstructed, and a to-be-executed joint angle increment vector is solved; and on the basis of the to-be-executed joint angle increment vector, the angle value of each joint of the mechanical arm in the next detection period is calculated, and the mechanical arm is controlled to move on the basis of the angle value of each joint so as to avoid shoulder singular points and / or wrist singular points of the mechanical arm.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical arm control, in particular to a singular avoidance control method and device for a six-axis mechanical arm based on virtual seven degrees of freedom. BACKGROUND

[0002] The six-degree-of-freedom mechanical arm will have wrist singularity when joint 4 and joint 6 are coaxial. When this occurs, joint 4 and joint 6 will rotate at high speed. When the wrist position point is on the axis extension line of joint 1, the shoulder singularity phenomenon will occur, and at this time, joint 1 and joint 3 will have a large mutation. When the mechanical arm configuration is close to the singular point, the movement of the mechanical arm is no longer stable and safe.

[0003] There are two main categories of current singular avoidance schemes for six-degree-of-freedom mechanical arms, namely, the non-active avoidance method, such as calculating the minimum singular value of the Jacobian matrix of the mechanical arm or the value of the determinant of the Jacobian matrix to determine whether the mechanical arm is close to singularity, and directly reporting an error if it is close to singularity. Another category is the active avoidance method, which commonly includes the singular point speed reduction scheme achieved by forced speed reduction and the damping least squares method by adding damping disturbance to the kinematic differential mapping.

[0004] However, the non-active avoidance method affects the usability of the mechanical arm, and in some scenarios, it is easy to cause the mechanical arm to approach singularity. Moreover, the minimum singular value and the minimum value of the Jacobian determinant are different when approaching the singular configuration in different motion ways, and it is difficult to measure the degree of singularity. The forced speed reduction scheme in the active avoidance method is suitable for teaching situations of the mechanical arm, and the forced speed reduction function will directly affect the motion planning of the mechanical arm in actual working scenarios. The damping least squares method has high sensitivity to parameters, and the method will lose the accuracy of position and attitude to achieve the effect of avoiding singularity. The damping least squares method needs to constantly generate damping factors through the damping self-adaptive function in the implementation process, and the forced speed scheme also needs additional cycles. SUMMARY

[0005] The purpose of the embodiment of the present application is to provide a singular avoidance control method and device for a six-axis mechanical arm based on virtual seven degrees of freedom. By adding a virtual rotation axis at the end of the mechanical arm, the six-degree-of-freedom mechanical arm is expanded to a virtual seven-degree-of-freedom mechanical arm, and the characteristics of the seven-degree-of-freedom mechanical arm without wrist singularity and elbow singularity are used to indirectly achieve the singular avoidance of the six-degree-of-freedom mechanical arm with only the loss of attitude accuracy.

[0006] To solve the above technical problems, the first aspect of the embodiment of the present application provides a singular avoidance control method for a six-axis mechanical arm based on virtual seven degrees of freedom. The end of the mechanical arm is provided with a virtual rotation axis that coincides with the origin of the end coordinate system and is perpendicular to the rotation axis of the end coordinate system. The control method comprises the following steps:

[0007] obtain position information and angle information of each joint of the robot arm in the current detection period, and actual pose and expected pose of the end of the robot arm;

[0008] Based on the position information and angle information of each joint, the weight value of the virtual rotation axis is adjusted according to the state of the robot arm approaching the shoulder singular point and / or the wrist singular point, and a quadratic programming problem for avoiding singular points is reconstructed to obtain a joint angle increment vector to be executed;

[0009] Based on the joint angle increment vector to be executed, the angle value of each joint of the robot arm in the next detection period is calculated, and the robot arm is controlled based on the angle value of each joint to avoid the shoulder singular point and / or the wrist singular point of the robot arm.

[0010] Further, before the corresponding adjustment of the weight value of the virtual rotation axis, it further includes:

[0011] When the wrist position point of the robot arm is located on the extended line of the axis of joint 1, it is determined that the robot arm is in a state close to or at the shoulder singular point;

[0012] When the joint 4 and the joint 6 of the robot arm are coaxial, it is determined that the robot arm is in a state close to or at the wrist singular point.

[0013] Further, the calculation function of the weight value adjustment of the virtual rotation axis is:

[0014]

[0015] wherein w 7_awayfrom_singularity is the reference weight when far away from the singular configuration, w 7_near_singularity is the reference weight when close to the singular configuration, singularity wrist is the angle value of joint 5 of the original six-axis robot arm, the closer the value is to 0, the higher the coaxial rate of joint 4 and joint 6, and the closer to the wrist singular region of the robot arm, singularity shoulder is the perpendicular distance between the wrist center point of the original six-axis robot arm and the extended straight line of the axis direction of joint 1, the closer the value is to 0, the closer to the shoulder singular region of the robot arm, r H is the position vector of the wrist center point in the original six-axis robot arm coordinate system, z1 is the unit vector of the axis direction of joint 1, and θ5 is the angle value of joint 5.

[0016] Further, the calculation of the angle value increment vector of each joint based on the actual pose, the expected pose of the robot arm and the weight value includes:

[0017] Based on the virtual seven-degree-of-freedom robot arm kinematics mapping formula, a quadratic programming problem is constructed.

[0018] Solving the quadratic programming problem based on the Lagrange multiplier method, an angle increment vector of each joint is obtained.

[0019] Further, the quadratic programming problem is specifically:

[0020]

[0021] wherein θ v =[θ1,θ2,θ3,θ4,θ5,θ6,θ7] T , is an error increment of a virtual seven-degree-of-freedom corresponding mechanical arm end position, J v is a Jacobian matrix of the virtual seven-degree-of-freedom corresponding mechanical arm end relative to a base coordinate system, W is a weight diagonal matrix, and w1…w7 correspond to Δθ v 1…Δθ v 7.

[0022] Correspondingly, a second aspect of the embodiment of the application provides a singular point avoidance control device for a six-axis mechanical arm based on a virtual seven-degree-of-freedom, a virtual rotation axis coinciding with an origin of an end coordinate system and being perpendicular to a rotation axis of the end coordinate system is arranged at an end of the mechanical arm, and the control device comprises:

[0023] a data acquisition module configured to acquire position information and angle information of each joint of the mechanical arm and actual position and expected position of the end in a current detection period;

[0024] a weight adjustment module configured to adjust a weight value of the virtual rotation axis based on the position information and the angle information of each joint and a state of the mechanical arm approaching a singular point of a shoulder and / or a singular point of a wrist, and to reconstruct a quadratic programming problem for avoiding the singular point to obtain a joint angle increment vector to be executed;

[0025] a mechanical arm control module configured to calculate angle values of each joint of the mechanical arm in a next detection period based on the joint angle increment vector to be executed, and to control movement of the mechanical arm based on the angle values of each joint to avoid the singular point of the shoulder and / or the singular point of the wrist of the mechanical arm.

[0026] Further, the weight adjustment module comprises:

[0027] a shoulder judging unit configured to determine that the mechanical arm is in a state of approaching or being at the singular point of the shoulder when a wrist position point of the mechanical arm is located on an extended line of an axis of joint 1 of the mechanical arm;

[0028] a wrist judging unit configured to determine that the mechanical arm is in a state of approaching or being at the singular point of the wrist when joint 4 and joint 6 of the mechanical arm are coaxial.

[0029] Further, the weight value adjustment calculation function of the virtual rotation axis is:

[0030]

[0031] wherein, w 7_awayfrom_singularity is the reference weight when far from the singular configuration, w 7_near_singularity is the reference weight when close to the singular configuration, singularity wrist is the angle value of joint 5 of the original six-axis robot arm, the closer the value is to 0, the higher the coaxial rate of joint 4 and joint 6 is, and the closer to the wrist singular zone of the robot arm, singularity shoulder is the perpendicular distance between the wrist center point of the original six-axis robot arm and the straight line extended in the direction of the axis of joint 1, the closer the value is to 0, the closer to the shoulder singular zone of the robot arm; r H is the position vector of the wrist center point represented in the original six-axis robot arm coordinate system, z1 is the unit vector in the direction of the axis of joint 1, and θ5 is the angle value of joint 5.

[0032] Further, the weight adjustment module comprises:

[0033] a construction unit configured to construct a quadratic programming problem based on the virtual seven-degree-of-freedom robot kinematics mapping formula;

[0034] a calculation unit configured to solve the quadratic programming problem based on the Lagrange multiplier method to obtain the angle increment vector of each joint.

[0035] Further, the quadratic programming problem is specifically:

[0036]

[0037] wherein, θ v =[θ1,θ2,θ3,θ4,θ5,θ6,θ7] T , is the error increment of the virtual seven-degree-of-freedom corresponding robot end position and posture, J v is the Jacobian matrix of the virtual seven-degree-of-freedom corresponding robot end relative to the base coordinate system, W is a weight diagonal matrix, w1…w7 respectively correspond to Δθ v 1…Δθ v 7.

[0038] Correspondingly, a third aspect of the embodiment of the present application provides a robot arm, comprising any of the above singular avoidance control devices based on a virtual seven-degree-of-freedom six-axis robot arm.

[0039] Accordingly, a fourth aspect of the embodiment of the present application provides an electronic device, comprising: at least one processor; and a memory connected with the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to enable the at least one processor to perform the singular avoidance control method for the virtual seven-degree-of-freedom six-axis mechanical arm.

[0040] Accordingly, a fifth aspect of the embodiment of the present application provides a computer readable storage medium having stored thereon computer instructions, which, when executed by a processor, implement the singular avoidance control method for the virtual seven-degree-of-freedom six-axis mechanical arm.

[0041] The above technical solutions of the embodiment of the present application have the following beneficial technical effects:

[0042] 1. By introducing a virtual joint coinciding with the origin of the end coordinate system and perpendicular to the end rotation axis, the six-degree-of-freedom mechanical arm is expanded to a seven-degree-of-freedom system at the control level, effectively utilizing the core feature of the seven-degree-of-freedom mechanical arm without wrist and elbow singularities; with the assistance of this virtual redundancy, the present application can effectively avoid shoulder singularities and wrist singularities by moderately sacrificing and optimizing the adjustment of the end attitude, while strictly guaranteeing the end position tracking accuracy of the mechanical arm, overcoming the problem of simultaneous loss of position and attitude accuracy in the traditional damping least square method, and significantly improving the trajectory accuracy and practicality of the mechanical arm when operating near singular regions;

[0043] 2. By using an adaptive weight adjustment strategy based on double singularity source discrimination (shoulder singularity and wrist singularity), a continuous and smooth exponential function is used to dynamically integrate the two key indicators of shoulder singularity proximity and wrist singularity proximity, and the weight coefficient of the virtual joint in the optimization objective function is adjusted in real time accordingly, which can automatically and smoothly optimize the trade-off between "high attitude tracking accuracy" and "strong singularity avoidance ability" according to the actual configuration of the mechanical arm and the proximity of the singularity region, avoiding the mutation of control parameters and ensuring the stability of motion, while a single virtual axis is used to deal with multiple types of singularities, improving the universality and efficiency of the algorithm;

[0044] 3. An analytical solution strategy based on the Lagrange multiplier method is used to solve the quadratic programming problem constructed to avoid singularity, obtaining a closed-form analytical solution of the optimal increment of joint angle, which fundamentally avoids the slow convergence speed, strong initial value sensitivity, uncertain calculation time consumption and other shortcomings of numerical iteration algorithms, ensuring the real-time performance and reliability of the solving process, greatly improving the response speed and operating efficiency of the entire control system, and meeting the stringent requirements of high-speed and high-precision real-time control of the mechanical arm. BRIEF DESCRIPTION OF DRAWINGS

[0045] Figure 1 is a virtual seven degree of freedom based six-axis robot singularity avoidance control method flowchart provided by the embodiment of the present application;

[0046] Figure 2 is a virtual seven degree of freedom based six-axis robot principle schematic diagram provided by the embodiment of the present application;

[0047] Figure 3 is a mechanical arm shoulder singularity point schematic diagram provided by the embodiment of the present application;

[0048] Figure 4 is a mechanical arm wrist singularity point schematic diagram provided by the embodiment of the present application;

[0049] Figure 5 is a mechanical arm through the wrist singularity zone schematic diagram provided by the embodiment of the present application;

[0050] Figure 6a is a mechanical arm each joint through the wrist singularity zone movement position schematic diagram provided by the embodiment of the present application;

[0051] Figure 6b is a mechanical arm each joint through the wrist singularity zone movement speed schematic diagram provided by the embodiment of the present application;

[0052] Figure 7 is a mechanical arm through the shoulder singularity zone schematic diagram provided by the embodiment of the present application;

[0053] Figure 8a is a mechanical arm each joint through the shoulder singularity zone movement position schematic diagram provided by the embodiment of the present application;

[0054] Figure 8b is a mechanical arm each joint through the shoulder singularity zone movement speed schematic diagram provided by the embodiment of the present application;

[0055] Figure 9 is a virtual seven degree of freedom based six-axis robot singularity avoidance control device module block diagram provided by the embodiment of the present application;

[0056] Figure 10 is a weight adjustment module block diagram provided by the embodiment of the present application.

[0057] Reference signs:

[0058] 1, data acquisition module, 2, weight adjustment module, 21, shoulder judging unit, 22, wrist judging unit, 23, problem construction unit, 24, vector calculation unit, 3, mechanical arm control module. DETAILED DESCRIPTION

[0059] In order to make the objects, technical solutions and advantages of the present application clearer, further detailed description will be made to the present application with reference to the specific embodiments and the accompanying drawings. It should be understood that the description is only exemplary and is not intended to limit the scope of the present application. In addition, in the following description, the description of the known structures and technologies is omitted to avoid unnecessary confusion of the concept of the present application.

[0060] Please refer to Figure 1 and Figure 2 The first aspect of the embodiment of the present application provides a singular point avoidance control method for a six-axis mechanical arm based on virtual seven degrees of freedom. The end of the mechanical arm is provided with a virtual rotation axis which coincides with the origin of the end coordinate system and is perpendicular to the rotation axis of the end coordinate system. The control method comprises the following steps:

[0061] In step S100, the position information and angle information of each joint of the mechanical arm and the actual pose and expected pose of the end in the current detection period are obtained.

[0062] The angle displacement data of the joints are collected in real time by the absolute encoder or rotary transformer installed at each joint of the mechanical arm, and the joint position information is obtained by the joint torque sensor or the estimation method based on the joint model. After filtering, denoising and coordinate transformation processing of these raw sensor data, the accurate angle and position state of each joint in the base coordinate system of the mechanical arm are obtained.

[0063] Based on the above joint information, the actual pose of the end effector relative to the base coordinate system in the current detection period is recursively solved by forward kinematics calculation according to the MDH parameter model of the mechanical arm, which includes three-dimensional position coordinates and attitude direction represented by rotation matrix or Euler angle. At the same time, the expected pose instruction of the end effector in this detection period is received from the upper task planner or trajectory generation module, which is usually given in the form of homogeneous transformation matrix or pose vector. The actual pose and the expected pose together constitute the pose error input of the current control period, which provides accurate and real-time data basis for subsequent singular point discrimination and motion correction.

[0064] In step S200, based on the position information and angle information of each joint, the weight value of the virtual rotation axis is adjusted corresponding to the state of the mechanical arm approaching the singular point of the shoulder and / or the singular point of the wrist, and the quadratic programming problem for avoiding the singular point is reconstructed to obtain the joint angle increment vector to be executed.

[0065] Firstly, real-time singularity proximity assessment is performed. The singularity of shoulder is determined by calculating the norm of the cross product of two vectors. The closer the norm is to zero, the closer the wrist point is to the singularity line of shoulder (i.e. the axis of joint 1), and the higher the risk of shoulder singularity is. The singularity of wrist is determined by directly monitoring the angle value of joint 5, θ_5. When the value of θ_5 approaches zero, it means that joint 4 and joint 6 tend to be coaxial, which indicates that the wrist enters the singularity region.

[0066] Based on the real-time calculation results of the two determinants, the system calls a continuous adaptive weight function to dynamically set the weight coefficient of the virtual joint. The function adopts an exponential smoothing form, and the input is the square sum of the singularity determinants of shoulder and wrist. Two key reference weights are preset in the function: a larger weight when far from singularity and a smaller weight when close to singularity. When the manipulator configuration is far from singularity, the output value of the function tends to the larger weight far from singularity, which imposes a larger penalty on the movement of the virtual joint, thereby strictly restricting its movement and prioritizing the tracking accuracy of the end posture. When the manipulator approaches the singularity point, the output of the function smoothly transitions to the smaller weight close to singularity, significantly reducing the constraint penalty on the virtual joint, allowing it to move more to provide redundancy, thereby actively avoiding singularity, and at this time the system tolerates temporary and controllable posture errors.

[0067] Subsequently, a quadratic programming (QP) problem with equality constraints is constructed using the constructed virtual seven-degree-of-freedom manipulator model (whose Jacobian matrix is composed by adding a column of differential motion vectors about the virtual joint to the right side of the original six-axis Jacobian matrix).

[0068] Finally, an analytical Lagrange multiplier method is used to efficiently solve the convex quadratic programming problem. By constructing the Lagrange function and setting its partial derivative to zero, an analytical solution in closed form is obtained. This solving process avoids the convergence problems and calculation delays that may exist in numerical iterative algorithms, and directly outputs the optimal joint angle increment vector including the six real joints and one virtual joint in the current control period, providing input for the final motion control.

[0069] In step S300, based on the joint angle increment vector to be executed, the angle values of each joint of the manipulator in the next detection period are calculated, and the manipulator is controlled based on the angle values of each joint to avoid the shoulder singularity point and / or the wrist singularity point of the manipulator.

[0070] The joint angle increment vector obtained in the previous solving step is vector superimposed with the joint angle of the current period, and the numerical integration operation is performed to calculate the expected target angle of each joint of the mechanical arm (including six real joints) in the next control period. Before outputting the final control command, the set of target angles needs to be comprehensively checked: first, it is compared with the physical motion limit of each joint of the mechanical arm (such as maximum and minimum angle, speed limit) to ensure that the command is within the safety tolerance range of the mechanical structure; second, the feasibility is verified through the inverse kinematics principle to ensure that the set of joint angles can uniquely and accurately achieve the required end position, especially focusing on the continuity and reasonableness of the solution near the singular region. After verification, the motion control module does not directly output the discrete angle value, but uses it as a set point combined with the joint state of the previous period to process it through a smoothing filter algorithm (such as a first-order low-pass filter or a trajectory interpolation algorithm) to generate smooth, continuous and non- abrupt joint position or speed commands. This process effectively suppresses the command jumps or jitter that may be caused by discrete control periods and numerical calculations. Finally, these smoothed commands are sent in real time to the servo drivers of each joint to drive the motor to perform the corresponding motion. It ensures that even when the mechanical arm passes through the singular region of the shoulder or wrist, each joint still exhibits continuous and stable motion characteristics, effectively avoiding problems such as speed sudden change, joint lock or trajectory deviation, thereby ensuring the smoothness of the end effector motion trajectory and overall control accuracy in the Cartesian space.

[0071] The present application utilizes the characteristic that the seven-axis mechanical arm does not have the corresponding singular point of the six-axis mechanical arm. In the control process, by monitoring the position information, angle information of each joint of the mechanical arm and the actual pose, expected pose of the end, and judging whether it is close to the singular point, the potential singular risk can be identified in advance during the movement of the mechanical arm. When approaching the singular point, the weight value of the virtual rotation axis is adjusted to calculate the angle value increment vector of each joint, and then the movement of the mechanical arm is controlled, which effectively avoids the unstable phenomenon of joint movement caused by the singular point. For example, in a six-axis mechanical arm, when joint 4 and joint 6 are coaxial (wrist singular) or the wrist position point is on the extension line of the axis of joint 1 (shoulder singular), the joint may rotate at high speed or have a large mutation, but the control method of the present application can avoid this situation, ensuring the smoothness of the movement of the mechanical arm in the entire workspace, improving the reliability and safety of the movement of the mechanical arm, and being especially suitable for industrial production, precise operation and other scenes with high requirements for position accuracy and stability.

[0072] The present application realizes singularity avoidance with only the sacrifice of posture precision, and has important technical advantages. In actual application, the requirement for the end position precision of the robot arm is more strict, for example, in the operation of part assembly, welding, etc., accurate position control is the key to ensure the success of the task. In the process of avoiding singularity, the posture precision can be flexibly adjusted to a certain extent on the premise of ensuring the position precision by indirectly controlling the original six-axis robot arm through the motion of the virtual seven-axis robot arm. This is because the position vector of the virtual seven-axis robot arm end relative to the base coordinate system is the same as that of the original six-axis robot arm, and the posture can be adjusted by reasonably adjusting the weight of the virtual rotation axis and the like to realize singularity avoidance, thereby optimizing the motion performance of the robot arm as a whole and improving the success rate and quality of task execution.

[0073] The MDH parameters of the robot arm configuration in the present application are shown in the following table.

[0074]

[0075] The mechanical arm connecting rod configuration diagram after adding the virtual joint 7 is shown in Figure 2 By adding a rotation axis through the end joint coordinate origin, the six-axis robot arm is expanded to a virtual seven-axis robot arm. The virtual joint axis 7 is marked as axis7, the rotation angle is marked as θ7, the joint 5 and the joint 6 are marked as axis5 and axis6 respectively, and the axis7 is determined by the following formula.

[0076] axis7=axis5×axis6

[0077] At this time, the relationship between the posture of the virtual seven-axis robot arm end coordinate system relative to the base coordinate system and the posture of the original six-axis robot arm end coordinate system relative to the base coordinate system can be connected by the Rodrigues rotation, that is, the following formula

[0078]

[0079] Since the virtual joint axis 7 axis7 passes through the original six-axis robot arm end coordinate system origin, the position vector of the virtual seven-axis robot arm end relative to the base coordinate system is the same as that of the original six-axis robot arm, so in the process of avoiding singularity, the posture precision can be sacrificed to a certain extent on the premise of ensuring the position precision.

[0080] The pose homogeneous transformation matrix of the virtual degree of freedom robot arm end coordinate system relative to the base coordinate system can be expressed as the following formula:

[0081]

[0082] The kinematic mapping formula of the virtual seven-axis robot arm is as follows:

[0083]

[0084] wherein, θ v = [θ1, θ2, θ3, θ4, θ5, θ6, θ7] T , J v is the Jacobian matrix of the virtual 7-DOF robot arm end relative to the base coordinate system, is the error increment of the virtual 7-DOF robot arm end pose.

[0085] Further, referring to Figure 3 and Figure 4 , before the corresponding adjustment of the weight value of the virtual rotation axis in step S200, further comprising:

[0086] Step S201, when the wrist position point of the robot arm is located on the axis extension line of joint 1, it is determined that the robot arm is in the state of approaching or being in the shoulder singularity point.

[0087] The configuration of the robot arm is continuously monitored in real time to determine whether it is close to or in the shoulder singularity state. The core basis of the judgment is to calculate the position relationship of the wrist center point relative to the axis of joint 1. Specifically, in the base coordinate system of the robot arm, first, the unit vector of the rotation axis direction of joint 1 is obtained, and the vector of the current position of the wrist center point of the robot arm relative to the origin of the base is calculated. By calculating the cross product of the above two vectors, and calculating the module length of the cross product vector, a scalar discriminant is obtained. The discriminant represents the perpendicular distance from the wrist center point to the axis extension line of joint 1 in geometry. The system is pre-set a positive threshold close to zero, when the value of the discriminant is less than the threshold, it is determined that the wrist point is very close to or located on the axis of joint 1, indicating that the robot arm is currently in the state of approaching or has entered the shoulder singularity region, and the state flag is set to true, providing a key input signal for subsequent weight adaptive adjustment.

[0088] Step S202, when the joint 4 and joint 6 of the robot arm are coaxial, it is determined that the robot arm is in the state of approaching or being in the wrist singularity point.

[0089] Parallelly, the configuration that may lead to wrist singularity is independently monitored and judged. The occurrence of wrist singularity corresponds to the geometric state that the rotation axes of joint 4 and joint 6 tend to be coaxial. The judgment is based on the direct reading and monitoring of the real-time angle value of joint 5. The basic principle is that in a standard six-axis robot configuration, when the absolute value of the angle of joint 5 is zero, the axes of joint 4 and joint 6 will completely coincide. Therefore, the absolute value of the angle of joint 5 directly reflects the degree of approach to coaxiality of the two axes. Similarly, a positive angle threshold close to zero is preset, and the judgment is made by continuously comparing the absolute value of the angle of joint 5 with the size of the threshold. When it is monitored that the absolute value of the angle of joint 5 is less than the set threshold, it is determined that the axes of joint 4 and joint 6 have approached or reached the coaxial state, the robot is close to or has entered the wrist singularity region, and the corresponding state flag bit is triggered, and this information is used synchronously for subsequent control decision.

[0090] The continuous monitoring and threshold comparison judgment process described by steps S201 and S202 can accurately and independently identify in real time whether the robot is approaching or has entered the two typical singularity configuration states of shoulder singularity and wrist singularity. This double discrimination mechanism provides accurate and reliable logical input for the dynamic adaptive adjustment of the virtual joint weight, ensuring the timeliness and accuracy of the singularity avoidance strategy, and is the premise and foundation for the effective singularity avoidance of the entire control method.

[0091] When shoulder singularity occurs, the wrist position point of the robot is on the extension line of the axis of joint 1, at which time the singularity shoulder The degree of approach to 0 is used to measure whether it is close to shoulder singularity, as shown in the schematic diagram Figure 3 , and the discriminant is as shown in the following formula: singularity shoulder =‖r H ×z1‖; when wrist singularity occurs, i.e. joint 5 approaches 0, the axes of joint 4 and joint 6 become coaxial, at which time the degree of approach to 0 of joint 5 is directly used to measure whether it is close to wrist singularity, as shown in the schematic diagram Figure 4 , and the discriminant is as shown in the following formula: singularity wrist =θ5.

[0092] Further, the calculation function of the weight value adjustment of the virtual rotation axis in step 200 is:

[0093]

[0094] wherein w 7_awayfrom_singularity is the reference weight when far from the singularity configuration, w 7_near_singularity is the reference weight when close to the singularity configuration, singularity wristis the angle value of joint 5 of the original six-axis robot, the closer the value is to 0, the higher the coaxial rate of joint 4 and joint 6, and the closer to the wrist singularity of the robot shoulder is the perpendicular distance between the wrist center point of the original six-axis robot and the straight line extended along the direction of joint 1 axis, the closer the value is to 0, the closer to the shoulder singularity of the robot H is the position vector of the wrist center point expressed in the base coordinate system of the original six-axis robot, z1 is the unit vector in the direction of joint 1 axis, and θ5 is the angle value of joint 5. Optionally, the reference weight w 7_awayfrom_singularity of the reference weight w 7_near_singularity is 1.5.

[0095] The dynamic adjustment of the virtual rotation axis weight coefficient is realized by a continuous and smooth adaptive function, and the input variables are two singularity proximity discriminants from steps S201 and S202: the shoulder singularity discriminant singularity shoulder (that is, the perpendicular distance between the wrist center point and the joint 1 axis) and the wrist singularity discriminant singularity wrist (that is, the current angle value of joint 5). The core structure of the function is composed of two exponential decay terms, which are multiplied by the preset reference weight values w 7_awayfrom_singularity and w 7_near_singularity and then summed. Wherein, w 7_awayfrom_singularity is a larger positive weight value, representing that when the robot configuration is far away from all singularities, the system applies stronger constraints on the movement of the virtual joint, aiming to maximize the pose tracking accuracy of the end effector. On the contrary, w 7_near_singularity is a smaller positive weight value, representing that when the robot is very close to or in a singular configuration, the system relaxes the constraints on the virtual joint to allow it to move more freely to provide the required kinematic redundancy for singularity avoidance. The decay coefficient a in the exponential term is a positive adjustable parameter for controlling the rate of change of the weight with the square sum of the singularity discriminants, and its size directly affects the sensitivity and response speed of the system to the proximity of singularity. The output value of the whole function will change continuously with the robot configuration, when the robot is running normally and far away from singularity, the function output value tends to w 7_awayfrom_singularity ; when any singularity discriminant tends to zero is monitored, the output value of the function smoothly and continuously transitions to w 7_near_singularity . It ensures that the weight adjustment is not a sudden switching process, but a gradual and shock-free optimization process, thereby providing a weight parameter that is both adaptive to the working condition and numerically stable for subsequent solving.

[0096] This weighted adaptive adjustment mechanism transforms the abstract singularity measure into specific control parameters through a carefully designed mathematical function, achieving an automatic trade-off between singularity avoidance strategies and motion accuracy maintenance. Its smoothness avoids abrupt changes in control commands, ensuring the stability of the robotic arm's motion. Furthermore, its simultaneous response to dual singular sources ensures an efficient and unified singularity avoidance scheme, which is a key technical aspect for improving the motion performance of a six-axis robotic arm in complex trajectory tasks.

[0097] Furthermore, the reconstruction in step S200 is used to avoid the quadratic programming problem of singularities, and the solution is used to obtain the joint angle increment vector to be executed, including:

[0098] Step S210: Construct a quadratic programming problem based on the kinematic mapping of the virtual seven-degree-of-freedom robotic arm.

[0099] Under the constraint of satisfying the end-effector motion requirements, an optimal set of joint angle increments is sought. Specifically, the mathematical objective function is set to minimize the L2 norm squared of the weighted joint angle increments, where the weight matrix is ​​a diagonal matrix, and its diagonal elements are composed of the virtual joint weights adaptively calculated in step S203 and other fixed weights. This reflects the system's trade-off in the cost of joint space motion, that is, prioritizing virtual joint motion to avoid singularities while moderately constraining the motion range of real joints. The constraint condition of this optimization objective strictly follows the differential kinematics relationship of the virtual seven-degree-of-freedom system, that is, the product of the Jacobian matrix and the joint angle increment must be equal to the currently measured or calculated end-effector Cartesian pose error. In this way, the singularity avoidance problem of the robotic arm is transformed into a strict convex quadratic programming problem with linear equality constraints, ensuring the existence and uniqueness of the solution.

[0100] Step S220: Solve the quadratic programming problem based on the Lagrange multiplier method to obtain the angle increment vector of each joint.

[0101] Quadratic programming problems can also be called QP (Quadratic Programming) problems. There are numerical iteration methods for solving QP problems, as well as methods that use constrained function extrema such as the Lagrange multiplier method to find the analytical expression.

[0102] Objective function in quadratic programming problem The goal is to minimize the weighted joint angle increment vector Δθ under certain constraints. vnorm square of the error. The elements on the diagonal of the weight matrix W, including the adaptive adjustment according to the singular case, distinguish the importance of different joint angle increments. By minimizing this objective function, the joint angle changes can be reasonably adjusted while considering the singularity avoidance, so that the motion of the robot arm is more optimized. Without such optimization, near the singular point, the joint angle may have unreasonable large changes, resulting in unstable or inaccurate motion of the robot arm. By minimizing the objective function, the adjustment of the joint angle can be more smooth and reasonable, avoiding the robot arm from having a sharp motion, and improving the stability and safety of the motion.

[0103] Firstly, the Lagrange multiplier vector is introduced to transform the constrained optimization problem into an unconstrained Lagrange function. Subsequently, the partial derivatives of the function with respect to the joint angle increment vector and the Lagrange multiplier vector are taken, and they are set to zero, thereby obtaining a linear equation system, i.e., the Karush-Kuhn-Tucker condition. By analytically solving the linear system, a closed-form analytical solution of the joint angle increment vector can be directly obtained, which contains the optimal increments of the six real joints and a virtual joint. This analytical solving method completely avoids the problems of convergence speed, initial value sensitivity and real-time performance that the numerical iterative algorithm may face, and can provide a deterministic and computationally efficient optimal solution for each control period.

[0104] Further, the quadratic programming problem is specifically:

[0105]

[0106] where θ v 1, θ2, θ3, θ4, θ5, θ6, θ7 T , is the error increment of the virtual seven-degree-of-freedom corresponding robot arm end pose, J v is the Jacobian matrix of the virtual seven-degree-of-freedom corresponding robot arm end with respect to the base coordinate system, W is a weight diagonal matrix, w1…w7 correspond to Δθ v 1…Δθ v 7 respectively.

[0107] The function is half of the square of the two-norm of the weighted joint angle increment vector. The weight matrix in it is a seven-order diagonal matrix, and the elements on its diagonal correspond to the weight coefficients of the angle increment of the seven joints (six real joints and one virtual joint) respectively. These weight coefficients directly reflect the degree of punishment of the system to the movement of different joints; a higher weight means imposing stronger constraints on the movement of the joint, encouraging it to move as little as possible, and vice versa, allowing the joint to move a larger range to complete the main task. In particular, the weight coefficient corresponding to the virtual joint is dynamically generated by the aforementioned adaptive function, which is the core adjustment parameter to realize the singularity avoidance strategy. The constraint condition of this optimization problem is a linear equation system, which strictly stipulates that the linear mapping from the seven-dimensional joint angle increment space to the six-dimensional end Cartesian pose error space must be satisfied, that is, the end motion calculated by the Jacobian matrix of the virtual seven-degree-of-freedom manipulator must be exactly equal to the error between the actual pose and the expected pose detected in the current period. By solving this optimization problem, the most smooth or economic motion path in the joint space can be found while strictly guaranteeing the end pose tracking accuracy, realizing the optimization allocation of joint movement while avoiding singularity.

[0108] Constraint condition J v is the Jacobian matrix of the virtual seven-degree-of-freedom manipulator end relative to the base coordinate system, which describes the linear mapping relationship between the joint space velocity and the end effector in the Cartesian space velocity, and reflects the influence degree of the small movement of the joint on the pose change of the end effector. Δθ v is the joint angle increment vector, that is, the small change amount of each joint angle. Multiplying the Jacobian matrix and the joint angle increment vector represents the speed component of the corresponding pose change at the end effector (the change of velocity in the Cartesian space) calculated according to the current joint angle change. is the error increment of the virtual seven-degree-of-freedom manipulator end pose, which represents the expected end effector pose change amount (the difference compared with the target pose or the pose of the last detection period).

[0109] The constraint is based on the kinematic mapping relationship of the virtual seven-degree-of-freedom manipulator, which ensures that the calculated joint angle increment can accurately realize the expected end position error increment. That is, in the process of optimizing the joint angle, it is necessary to ensure that the kinematic relationship of the manipulator is established, so that the end of the manipulator can move according to the predetermined trajectory and attitude change. If this constraint is not met, the actual movement of the manipulator will deviate from the expectation and cannot accurately complete the task. For example, when performing grasping and other operations, the position and attitude of the end effector are not accurate, and effective operation cannot be achieved. By adding this constraint to the quadratic programming problem, the accuracy and effectiveness of the manipulator movement can be ensured while optimizing the joint movement, so that it can still accurately track the expected trajectory and complete the task requirements in the singularity avoidance case.

[0110] The elements in the weight matrix W are related to singularity judgment, especially w7 is determined by the degree of wrist singularity and shoulder singularity. The elements in the weight matrix are used to distinguish and adjust the importance of different joint angle increments. In the singularity avoidance process, the weight will adaptively change according to the singularity degree, for example, the weight corresponding to the virtual axis 7 will be adjusted according to the wrist singularity and shoulder singularity.

[0111] When solving the quadratic programming problem, the weights of different joints will be adjusted according to the singularity condition, so as to give priority to avoiding singular configurations in the optimization process. For example, when approaching the singularity point, the joint angle adjustment related to the virtual axis 7 is more reasonable by adjusting the weight to achieve singularity avoidance. The joint angle increment obtained by solving the quadratic programming problem is the result after considering singularity avoidance, kinematic constraints and optimization objectives. In this way, the manipulator can be more efficient in the movement process, avoiding unnecessary energy consumption and time waste. Compared with the method without the above optimization, the manipulator can complete the task more quickly and stably, improving the overall motion efficiency.

[0112] Specifically, the above quadratic programming problem is analytically solved using the Lagrange multiplier method. First, according to the adding mode of the virtual joint 7, the Jacobian matrix J of the virtual seven-degree-of-freedom manipulator is constructed as follows.

[0113]

[0114] Where, RODB(θ7, axis7) = sin(θ7)I + (1-cos(θ7))axis7, J xyz and J rp are the Jacobian matrices of the position and attitude parts of the original six-degree-of-freedom manipulator, respectively.

[0115] At this point, in the quadratic programming problem, in addition to the variables Δθ vThe other coefficient matrices are obtained, and the solving method using the Lagrange multiplier method is as follows.

[0116] First, introduce the Lagrange multiplier Then, the Lagrange equation is obtained as follows:

[0117]

[0118] The above formula is respectively for the variable Δθ v And the multiplier Take the derivative, and set the derivative to 0 respectively, as shown in the following formula:

[0119]

[0120] Combining the above two formulas, the solution is obtained as follows

[0121]

[0122] Finally, after obtaining Δθ v , the angle values of each joint of the robot arm in the next detection period are calculated by integration, and the robot arm is controlled based on the angle values of each joint to avoid the shoulder singularity and / or wrist singularity of the robot arm. The above-mentioned conversion of the previously calculated discrete incremental information into continuous joint angle information provides a basis for subsequent optimization and control.

[0123] In addition, after obtaining the angle values of each joint of the robot arm in the next detection period, the current robot arm configuration can also be further judged for singularity, and the shoulder singularity discriminant and the wrist singularity discriminant are calculated to evaluate whether it is close to a singularity point. According to the new singularity situation, the weight of the virtual axis 7 is updated again using the adaptive adjustment weight function, thereby affecting the subsequent kinematics calculation.

[0124] Secondly, the kinematics mapping formula of the virtual seven-degree-of-freedom robot arm is re-established, in which the Jacobian matrix may change due to the change of joint angle, and the error increment of the corresponding end pose of the virtual seven-degree-of-freedom robot arm and the calculated joint angle increment vector will also be adjusted according to the new situation. Then, based on the updated weight matrix (including the updated weight of the virtual axis 7) and the new kinematics mapping relationship, the quadratic programming problem is reconstructed.

[0125] Thirdly, the reconstructed quadratic programming problem is solved again using methods such as the Lagrange multiplier method to obtain a new joint angle increment vector. This process is an iterative optimization process, which continuously adjusts the joint angle increment according to the actual motion situation and singularity state of the robot arm, so that the robot arm can better avoid singular points during motion, while ensuring the accuracy and stability of motion.

[0126] Finally, the above singularity judgment, weight update, kinematic mapping reconstruction, quadratic programming problem solving and other steps are repeated until the robot arm completes the entire motion task or reaches the preset optimization stopping condition, such as the joint angle change within a certain threshold, the end effector error within an acceptable range, etc. Through iterative optimization, the robot arm can smoothly pass through the singular region without sudden changes in joint speed, effectively improving the performance of the robot arm in complex tasks.

[0127] Please refer to Figure 5 , based on the above control method, the control process of the robot arm avoiding wrist singularity is as follows: the initial joint angle is [30°, 60°, -30°, -10°, -30°, 10°], the original six-axis robot arm end moves 0.3m downward along the z-axis direction, from Figure 5 It can be seen that during the movement of the robot arm, joint 5 passes through the zero position, and joints 4 and 6 are coaxial. As shown in Figure 6a and Figure 6b , during the movement of the robot arm through the wrist singularity, the joint position trajectory is smooth and continuous, and the joint speed does not change suddenly.

[0128] Please refer to Figure 7 , based on the above control method, the control process of the robot arm avoiding shoulder singularity is as follows: the initial joint angle is [-5°, 90°, 0°, 0°, 90°, 0°], the original six-axis robot arm end moves 0.15m along the x-axis direction, from Figure 7 It can be seen that during the movement of the robot arm, the wrist position point passes through the extension line of the axis of joint 1. As shown in Figure 8a and Figure 8b , during the movement of the robot arm through the shoulder singularity, the joint position trajectory is smooth and continuous, and the joint speed does not change suddenly.

[0129] Correspondingly, please refer to Figure 9 , the second aspect of the embodiment of the application provides a singularity avoidance control device for a virtual seven-axis robot arm, the robot arm end is provided with a virtual rotation axis coinciding with the origin of the end coordinate system and perpendicular to the rotation axis of the end coordinate system, the control device comprises:

[0130] A data acquisition module 1 is used to acquire the position information, angle information of each joint of the robot arm and the actual pose, expected pose of the end in the current detection period;

[0131] A weight adjustment module 2 is used to adjust the weight value of the virtual rotation axis based on the position information and angle information of each joint, and adjust the weight value of the virtual rotation axis corresponding to the state of the robot arm approaching the shoulder singularity point and / or the wrist singularity point, and reconstruct the quadratic programming problem for avoiding the singularity point, and solve to obtain the joint angle increment vector to be executed;

[0132] a mechanical arm control module 3, configured to calculate the angle values of each joint of the mechanical arm in the next detection period based on the joint angle increment vector to be executed, and control the movement of the mechanical arm based on the angle values of each joint to avoid the shoulder singularity and / or the wrist singularity of the mechanical arm.

[0133] Further, referring to Figure 10 , the weight adjustment module 2 comprises:

[0134] a shoulder judging unit 21, configured to determine that the mechanical arm is in a state close to or at the shoulder singularity when the wrist position point of the mechanical arm is located on the extension line of the axis of the joint 1;

[0135] a wrist judging unit 22, configured to determine that the mechanical arm is in a state close to or at the wrist singularity when the joint 4 and the joint 6 of the mechanical arm are coaxial.

[0136] Further, the calculation function of the weight value adjustment of the virtual rotation axis is:

[0137]

[0138] wherein, w 7_awayfrom_singularity is the reference weight when far away from the singularity configuration, w 7_near_singularity is the reference weight when close to the singularity configuration, singularity wrist is the angle value of the joint 5 of the original six-axis mechanical arm, and the closer the value is to 0, the higher the coaxial rate of the joint 4 and the joint 6, and the closer to the wrist singularity of the mechanical arm, singularity shoulder is the perpendicular distance between the wrist center point of the original six-axis mechanical arm and the extension line of the axis direction of the joint 1, and the closer the value is to 0, the closer to the shoulder singularity of the mechanical arm; r H is the position vector of the wrist center point represented in the base coordinate system of the original six-axis mechanical arm, z1 is the unit vector of the axis direction of the joint 1, and θ5 is the angle value of the joint 5.

[0139] Further, referring to Figure 10 , the weight adjustment module comprises:

[0140] a problem constructing unit 23, configured to construct a quadratic programming problem based on the kinematic mapping of the virtual seven-degree-of-freedom mechanical arm;

[0141] a vector calculating unit 24, configured to solve the quadratic programming problem based on the Lagrange multiplier method to obtain the angle increment vector of each joint.

[0142] Further, the quadratic programming problem is specifically:

[0143]

[0144] wherein, θ v = [θ1, θ2, θ3, θ4, θ5, θ6, θ7] T , is the error increment of the virtual seven-degree-of-freedom corresponding mechanical arm end position and posture, J v is the Jacobian matrix of the virtual seven-degree-of-freedom corresponding mechanical arm end position relative to the base coordinate system, W is a weight diagonal matrix, and w1…w7 correspond to Δθ v 1…Δθ v 7 respectively.

[0145] Correspondingly, a third aspect of the embodiment of the present application provides a mechanical arm, comprising the above-mentioned singular avoidance control device for a six-axis mechanical arm based on virtual seven-degree-of-freedom.

[0146] Correspondingly, a fourth aspect of the embodiment of the present application provides an electronic device, comprising: at least one processor; and a memory connected with the at least one processor; wherein the memory stores instructions executable by the at least one processor, and the instructions are executed by the at least one processor to make the at least one processor execute the above-mentioned singular avoidance control method for a six-axis mechanical arm based on virtual seven-degree-of-freedom.

[0147] Correspondingly, a fifth aspect of the embodiment of the present application provides a computer readable storage medium, which stores computer instructions, and the instructions are executed by a processor to implement the above-mentioned singular avoidance control method for a six-axis mechanical arm based on virtual seven-degree-of-freedom.

[0148] The embodiment of the present application aims to protect a singular avoidance control method and device for a six-axis mechanical arm based on virtual seven-degree-of-freedom, and the above-mentioned technical solution has the following effects:

[0149] 1. By introducing a virtual joint coinciding with the origin of the end coordinate system and perpendicular to the end rotation axis, the six-degree-of-freedom mechanical arm is expanded to a seven-degree-of-freedom system in the control layer, effectively utilizing the core characteristics of the seven-degree-of-freedom mechanical arm without wrist and elbow singularities; with the assistance of this virtual redundancy, the present application can effectively avoid shoulder singularities and wrist singularities by moderately sacrificing and optimizing the adjustment of the end posture while strictly ensuring the accuracy of the position tracking of the mechanical arm end, overcoming the problem of simultaneous loss of position and posture accuracy in the traditional damping least square method, and significantly improving the trajectory accuracy and practicality of the mechanical arm when operating near the singular region;

[0150] 2. By the adaptive weight adjustment strategy based on the dual-singularity discrimination (shoulder singularity and wrist singularity), the shoulder singularity proximity and the wrist singularity proximity, two key indicators, are dynamically fused based on a continuous and smooth exponential function, and the weight coefficient of the virtual joint in the optimization objective function is adjusted in real time according to the actual configuration of the manipulator and the proximity of the singular region, so that the optimal trade-off between "high posture tracking accuracy" and "strong singularity avoidance ability" can be automatically and smoothly achieved, the mutation of the control parameters is avoided, the stability of the motion is ensured, and a single virtual axis is used to cope with multiple singularity types, thereby improving the universality and efficiency of the algorithm;

[0151] 3. An analytical solution strategy based on the Lagrange multiplier method is used to solve the quadratic programming problem constructed to avoid singularity, and a closed-form analytical solution of the optimal increment of the joint angle is obtained, which fundamentally avoids the slow convergence speed, strong initial value sensitivity, uncertain calculation time consumption and other shortcomings of the numerical iteration algorithm, ensures the real-time performance and reliability of the solving process, greatly improves the response speed and operation efficiency of the entire control system, and meets the harsh requirements of high-speed and high-precision real-time control of the manipulator.

[0152] Those skilled in the art will understand that the embodiments of the present application can be provided as a method, a system, or a computer program product. Therefore, the present application can take the form of an entirely hardware embodiment, an entirely software embodiment, or an embodiment combining software and hardware aspects. Moreover, the present application can take the form of a computer program product implemented on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROMs, optical storage, etc.) containing computer-usable program code.

[0153] The present application is described with reference to flowcharts and / or block diagrams of methods, devices (systems), and computer program products according to embodiments of the present application. It should be understood that each flow and / or block in the flowcharts and / or block diagrams, as well as combinations of flows and / or blocks in the flowcharts and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, a special-purpose computer, an embedded processor, or other programmable data processing apparatus to produce a machine, so that the instructions executed by the processor of the computer or other programmable data processing apparatus produce a device that implements the functions specified in the flowcharts and / or block diagrams. Figure 1 The functions specified in a flow or multiple flows and / or blocks Figure 1 The functions specified in a flow or multiple flows and / or blocks

[0154] These computer program instructions can also be stored in a computer-readable storage medium that can direct the computer or other programmable data processing apparatus to work in a specific manner, so that the instructions stored in the computer-readable storage medium produce a manufactured product including instruction devices that implement the functions specified in the flowcharts and / or block diagrams. Figure 1one or more processes and / or blocks Figure 1 the function specified in the one or more blocks.

[0155] These computer program instructions can also be loaded into computer or other programmable data processing devices, so that a series of operation steps are performed on the computer or other programmable data processing devices to generate computer-implemented processes, so that the instructions executed on the computer or other programmable data processing devices provide processes for implementing the flow Figure 1 one or more processes and / or blocks Figure 1 the function specified in the one or more blocks.

[0156] Finally, it should be noted that: the above examples are only used to illustrate the technical solutions of the present application, but not to limit it, although the above embodiments of the present application have been described in detail, those skilled in the art should understand: the specific embodiments of the present application can be modified or replaced by the same, without departing from the spirit and scope of the present application, any modification or equivalent replacement, which should be covered within the scope of protection of the claims of the present application.

Claims

1. A singularity avoidance control method for a virtual seven-degree-of-freedom based six-axis robot, characterized by, The mechanical arm end is provided with a virtual rotation axis coinciding with the end coordinate system origin and perpendicular to the end coordinate system rotation axis, and the control method comprises the following steps: Obtain the position information and angle information of each joint of the mechanical arm and the actual pose and expected pose of the end in the current detection period; Based on the position information and angle information of each joint, the weight value of the virtual rotation axis is adjusted corresponding to the state of the mechanical arm approaching the shoulder singular point and / or the wrist singular point, and a quadratic programming problem for avoiding singular points is reconstructed to obtain a joint angle increment vector to be executed; Based on the joint angle increment vector to be executed, the angle values of each joint of the mechanical arm in the next detection period are calculated, and the mechanical arm is controlled to move based on the angle values of each joint to avoid the shoulder singular point and / or the wrist singular point of the mechanical arm.

2. The virtual seven degree of freedom based six-axis robot singularity avoidance control method of claim 1, wherein, Before the corresponding adjustment of the weight value of the virtual rotation axis, it further comprises: When the wrist position point of the mechanical arm is located on the axis extension line of its joint 1, it is determined that the mechanical arm is in the state of approaching or being located at the shoulder singular point; When the joint 4 and the joint 6 of the mechanical arm are coaxial, it is determined that the mechanical arm is in the state of approaching or being located at the wrist singular point.

3. The virtual seven degree of freedom based six-axis robot singularity avoidance control method of claim 2, wherein, The calculation function of the weight value adjustment of the virtual rotation axis is: singularity shoulder =‖r H ×z1‖; singularity wrist = θ5; wherein w 7_awayfrom_singularity is the reference weight when far from the singular configuration, w 7_near_singularity is the reference weight when close to the singular configuration, singularity wrist is the angle value of the joint 5 of the original six-axis robot arm, the closer the value is to 0, the higher the coaxial rate of the joint 4 and the joint 6 is, and the closer to the wrist singular zone of the robot arm is, singularity shoulder is the perpendicular distance between the wrist center point of the original six-axis robot arm and the extension line of the joint 1 axis direction, the closer the value is to 0, the closer to the shoulder singular zone of the robot arm is; r H is the position vector of the wrist center point expressed in the original six-axis robot arm base coordinate system, z1 is the unit vector of the joint 1 axis direction, and θ5 is the angle value of the joint 5.

4. The virtual seven degree of freedom based six-axis robot singularity avoidance control method of claim 3, wherein, The reconstruction of the quadratic programming problem for avoiding singular points to obtain the joint angle increment vector to be executed comprises: Based on the virtual seven-degree-of-freedom mechanical arm kinematics mapping formula, a quadratic programming problem is constructed; Based on the Lagrange multiplier method, the quadratic programming problem is solved to obtain the angle increment vector of each joint.

5. The singular avoidance control method for the virtual seven-degree-of-freedom six-axis mechanical arm according to claim 4, characterized in that: The quadratic programming problem is specifically: wherein θ v = [θ1, θ2, θ3, θ4, θ5, θ6, θ7] T , is the error increment of the virtual seven-degree-of-freedom corresponding mechanical arm end position and posture, J v is the Jacobian matrix of the virtual seven-degree-of-freedom corresponding mechanical arm end relative to the base coordinate system, W is a weight diagonal matrix, w1…w7 respectively correspond to Δθ v 1…Δθ v 7.

6. A singularity avoidance control device for a virtual seven degree of freedom based six-axis robot, comprising: The mechanical arm end is provided with a virtual rotation axis coinciding with the end coordinate system origin and perpendicular to the end coordinate system rotation axis, and the control device comprises: A data acquisition module is configured to acquire the position information and angle information of each joint of the mechanical arm and the actual pose and expected pose of the end in the current detection period; A weight adjustment module is configured to adjust the weight value of the virtual rotation axis corresponding to the state of the mechanical arm approaching the shoulder singular point and / or the wrist singular point based on the position information and angle information of each joint, and reconstruct a quadratic programming problem for avoiding singular points to obtain a joint angle increment vector to be executed; A mechanical arm control module is configured to calculate the angle values of each joint of the mechanical arm in the next detection period based on the joint angle increment vector to be executed, and control the mechanical arm to move based on the angle values of each joint to avoid the shoulder singular point and / or the wrist singular point of the mechanical arm.

7. The virtual seven degree of freedom based six-axis robot singularity avoidance control device of claim 6, wherein, The weight adjustment module comprises: A shoulder judgment unit is configured to determine that the mechanical arm is in the state of approaching or being located at the shoulder singular point when the wrist position point of the mechanical arm is located on the axis extension line of its joint 1; A wrist judgment unit is configured to determine that the mechanical arm is in the state of approaching or being located at the wrist singular point when the joint 4 and the joint 6 of the mechanical arm are coaxial.

8. The virtual seven degree of freedom based six-axis robotic arm singularity avoidance control device of claim 7, wherein, The calculation function of the weight value adjustment of the virtual rotation axis is: singularity shoulder =‖r H ×z1‖; singularity wrist = θ5; wherein, wherein, w 7_awayfrom_singularity is the reference weight when far from the singular configuration, w 7_near_singularity is the reference weight when close to the singular configuration, singularity wrist is the angle value of the original six-axis robot arm joint 5, the closer the value is to 0, the higher the coaxial rate of joint 4 and joint 6 is, and the closer to the wrist singular zone of the robot arm, singularity shoulder is the perpendicular distance between the original six-axis robot arm wrist center point and the straight line extended in the direction of joint 1 axis, the closer the value is to 0, the closer to the shoulder singular zone of the robot arm; r H is the position vector of the wrist center point expressed in the original six-axis robot arm base coordinate system, z1 is the unit vector in the direction of joint 1 axis, and θ5 is the angle value of joint 5.

9. The virtual seven degree of freedom based six-axis robotic arm singularity avoidance control device of claim 8, wherein, The weight adjustment module comprises: a problem constructing unit configured to construct a quadratic programming problem based on a virtual seven-degree-of-freedom robot arm kinematic mapping; a vector calculating unit configured to solve the quadratic programming problem based on a Lagrange multiplier method to obtain an angle increment vector of each joint.

10. The singularity avoidance control device for a virtual seven-degree-of-freedom six-axis robot arm according to claim 9, characterized in that, the quadratic programming problem is specifically: wherein θ v = [θ1, θ2, θ3, θ4, θ5, θ6, θ7] T , is the error increment of the virtual seven-degree-of-freedom corresponding mechanical arm end position and posture, J v is the Jacobian matrix of the virtual seven-degree-of-freedom corresponding mechanical arm end position and posture relative to the base coordinate system, W is a weight diagonal matrix, w1…w7 respectively correspond to Δθ v 1…Δθ v 7.

Images