Mechanical arm singularity avoidance method, system, device and storage medium

Abstract

The application discloses a mechanical arm singular point avoidance method, system, device and storage medium, and technical scheme points thereof are as follows: acquiring a pose and a joint angle of a mechanical arm; judging whether to enter a singular region and a singular type according to the pose and the joint angle; performing first-level processing in the case of a wrist singular type, performing second-level processing in the case of a shoulder singular type, and performing third-level processing in the case of an elbow singular type. The application can improve obstacle avoidance efficiency, ensure motion stability and reduce calculation complexity.

Description

TECHNICAL FIELD

[0001] The application belongs to the technical field of robot control, and particularly relates to a mechanical arm singularity avoidance method, system, device and storage medium. BACKGROUND

[0002] The mechanical arm is a complex system with high precision, multiple inputs and outputs, high nonlinearity and strong coupling. For different tasks, the motion trajectory of the joint space of the mechanical arm needs to be planned to cascade the end pose. In the case of executing a complex motion trajectory or controlling the mechanical arm using a linear motion instruction, the mechanical arm is prone to singularity. When the mechanical arm reaches the singularity, the mechanical arm will be forced to stop, affecting the normal operation of the mechanical arm.

[0003] In the prior art, the value of the Jacobian matrix is calculated in real time according to the current joint angle of the mechanical arm, and whether the mechanical arm is close to the singularity is judged according to the value. However, real-time calculation of the value of the Jacobian matrix will cause a large calculation burden on the system, and when close to the singularity, the condition number of the Jacobian matrix increases sharply, so that the numerical value of the inverse matrix of the Jacobian matrix becomes unstable, resulting in low obstacle avoidance efficiency and poor motion stability. SUMMARY

[0004] The purpose of the present application is to provide a mechanical arm singularity avoidance method, system, device and storage medium, which can improve the obstacle avoidance efficiency, ensure the motion stability and reduce the calculation complexity.

[0005] The first aspect of the present application provides a mechanical arm singularity avoidance method, comprising:

[0006] obtaining the pose and joint angle of the mechanical arm;

[0007] judging whether to enter a singular region and a singular type according to the pose and joint angle;

[0008] in the case of the singular type being a wrist singular type, performing first-level processing, in the case of the singular type being a shoulder singular type, performing second-level processing, and in the case of the singular type being an elbow singular type, performing third-level processing.

[0009] In some embodiments, the joint angle includes the joint angle of the wrist joint and the joint angle of the elbow joint; and the judging whether to enter a singular region and a singular type according to the pose and joint angle includes:

[0010] in the case of the joint angle of the wrist joint reaching a wrist threshold range, determining that the mechanical arm enters a singular region, and the singular type being a wrist singular type;

[0011] An elbow angle is calculated according to the joint angle of the elbow joint, and in a case where the elbow angle reaches an elbow threshold range, it is determined that the robot arm enters a singular region, and the singular type is an elbow singular type;

[0012] A shoulder distance is calculated according to the pose and the axis vector of the shoulder joint, the shoulder distance being a radial distance between the wrist center point and the axis vector of the shoulder joint, and in a case where the shoulder distance reaches a shoulder threshold range, it is determined that the robot arm enters a singular region, and the singular type is a shoulder singular type.

[0013] In some embodiments, in a case where the singular type is a wrist singular type, first-level processing is performed, including:

[0014] In a case where the singular type is a wrist singular type, speed limiting processing is performed on the joints of the robot arm, and a warning is issued.

[0015] In some embodiments, in a case where the singular type is a shoulder singular type, second-level processing is performed, including:

[0016] In a case where the singular type is a shoulder singular type, a current pose of the robot arm is taken as a current starting point, and an avoidance path is planned according to the current starting point, a path endpoint, and a shoulder singular point;

[0017] The avoidance path is sampled to obtain a plurality of path points, and each path point is converted into a corresponding avoidance joint angle;

[0018] Time parameterization is performed based on maximum speed, maximum acceleration, and maximum jerk constraints of the robot arm to obtain a time point corresponding to each avoidance joint angle;

[0019] A joint path point sequence is obtained according to each avoidance joint angle and the time point corresponding thereto, and interpolation is performed on the joint path point sequence to obtain a joint space trajectory;

[0020] The robot arm joint motion is controlled according to the joint space trajectory.

[0021] In some embodiments, converting each path point into a corresponding avoidance joint angle includes:

[0022] Inverse kinematics is solved for each path point to obtain a plurality of candidate joint angles corresponding to each path point;

[0023] Singularity point detection is performed on each candidate joint angle to screen out non-singular joint angles;

[0024] Configuration distance is calculated between the non-singular joint angle and the joint angle of the previous path point, and the non-singular joint angle with the closest configuration distance is taken as the avoidance joint angle.

[0025] In some embodiments, the time parameterization based on the maximum speed, maximum acceleration, and maximum jerk constraints of the mechanical arm obtains a time point corresponding to each avoidance joint angle, including:

[0026] The maximum speed, maximum acceleration, and maximum jerk constraints are preset, and the avoidance joint angle is initialized;

[0027] According to the maximum speed, maximum acceleration, and maximum jerk constraints, the initialized avoidance joint angle is forward integrated to obtain a forward integral speed and a forward integral time of each avoidance joint angle, and the initialized avoidance joint angle is backward integrated to obtain a backward integral speed and a backward integral time of each avoidance joint angle;

[0028] According to the forward integral speed and the backward integral speed of each avoidance joint angle, the joint speed of each avoidance joint angle is determined;

[0029] According to the joint speed of each avoidance joint angle, a time point corresponding to each avoidance joint angle is calculated.

[0030] In some embodiments, when the singularity type is an elbow singularity type, a three-level processing is performed, including:

[0031] When the singularity type is a shoulder singularity type, the joint locking of the mechanical arm is controlled to obtain a current joint configuration, the current pose of the mechanical arm is taken as a target pose, and the target pose is converted into a target joint angle;

[0032] The target joint angle is solved by inverse kinematics to obtain a plurality of candidate joint configurations;

[0033] The singularity point of all candidate joint configurations is detected to screen out non-singular joint configurations;

[0034] The operability index of each non-singular joint configuration is calculated, and the non-singular joint configuration with the largest operability index is taken as a target joint configuration;

[0035] The joint locking of the mechanical arm is released, and the mechanical arm is controlled to move from the current joint configuration to the target joint configuration.

[0036] The second aspect of the present application provides a mechanical arm singularity avoidance system, including:

[0037] An acquisition module is configured to acquire a pose and a joint angle of a mechanical arm;

[0038] A judgment module is configured to judge whether a singularity region and a singularity type are entered according to the pose and the joint angle;

[0039] The processing module is used to perform first-level processing when the singularity type is a wrist singularity type, second-level processing when the singularity type is a shoulder singularity type, and third-level processing when the singularity type is an elbow singularity type.

[0040] A third aspect of the present invention provides a computer device, including a memory and a processor, wherein the memory stores a computer program, and the processor executes the computer program to implement the steps of the method described above.

[0041] A fourth aspect of the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the steps of the method described above.

[0042] The technical solution provided by this invention has the following advantages and effects: First, it detects whether the robotic arm has entered a singular region. After entering the singular region, it determines the type of singularity. Different processing methods are adopted for different types of singularities, which can improve obstacle avoidance efficiency, ensure motion stability, and reduce computational complexity. Attached Figure Description

[0043] Figure 1 This is a flowchart illustrating the singularity avoidance method for robotic arms provided by the present invention.

[0044] Figure 2 This is a three-dimensional structural schematic diagram of the robotic arm provided by the present invention;

[0045] Figure 3 This is a structural block diagram of the robotic arm singularity avoidance system provided by the present invention;

[0046] Figure 4 This is an internal structural diagram of a computer device provided in an embodiment of the present invention.

[0047] Reference numerals: 1. Base; 2. First joint; 3. Second joint; 4. First arm body; 5. Third joint; 6. Second arm body; 7. Fourth joint; 8. Third arm body; 9. Center point of wrist; 10. Fifth joint; 11. Sixth joint. Detailed Implementation

[0048] To facilitate understanding of the present invention, specific embodiments of the present invention will be described in more detail below with reference to the accompanying drawings.

[0049] Unless otherwise specified or defined, the terms "first," "second," etc., used in this document are for distinguishing names only and do not represent a specific number or order.

[0050] Unless otherwise stated or defined, the term “and / or” as used herein includes any and all combinations of one or more of the associated listed items.

[0051] It should be noted that "fixed to", "connected to" herein can be directly fixed or connected to an element, or indirectly fixed or connected to an element.

[0052] As shown in the embodiment, a mechanical arm singularity avoidance method is provided, including the following steps S1-S4: Figure 1

[0053] Step S1, obtaining the pose and joint angle of the mechanical arm.

[0054] In actual application, the pose of the mechanical arm refers to the position of the wrist center point. The pose of the mechanical arm in the base coordinate system is read through the robot controller, and the joint angles corresponding to each joint are read through the encoders of each joint of the mechanical arm. The mechanical arm singularity avoidance method of the present application is applicable to multi-axis mechanical arms such as four-axis, five-axis, six-axis, and seven-axis mechanical arms. In the embodiment, a six-axis mechanical arm is taken as an example, as shown in the figure, and then the joint angle Q = [q1, q2, q3, q4, q5, q6] is obtained, q1 represents the joint angle of the first joint 2, q2 represents the joint angle of the second joint 3, q3 represents the joint angle of the third joint 5, q4 represents the joint angle of the fourth joint 7, q5 represents the joint angle of the fifth joint 10, and q6 represents the joint angle of the sixth joint 11. Figure 2

[0055] Step S2, judging whether to enter a singular region and a singular type according to the pose and the joint angle.

[0056] Further, the joint angle includes the joint angle of the wrist joint and the joint angle of the elbow joint; and the judging whether to enter a singular region and a singular type according to the pose and the joint angle includes:

[0057] In the case where the joint angle of the wrist joint reaches a wrist threshold range, it is determined that the mechanical arm enters a singular region, and the singular type is a wrist singular type;

[0058] The elbow angle is calculated according to the joint angle of the elbow joint, and in the case where the elbow angle reaches an elbow threshold range, it is determined that the mechanical arm enters a singular region, and the singular type is an elbow singular type;

[0059] The shoulder distance is calculated according to the pose and the axis vector of the shoulder joint, the shoulder distance is the radial distance between the wrist center point and the axis vector of the shoulder joint, and in the case where the shoulder distance reaches a shoulder threshold range, it is determined that the mechanical arm enters a singular region, and the singular type is a shoulder singular type.

[0060] ​​In practical applications, the joint angle also includes the joint angle of the shoulder joint. In a six-axis robot arm, the shoulder-related joints include the first joint 2 and the second joint 3, the elbow-related joint includes the third joint 5, and the wrist-related joints include the fourth joint 7, the fifth joint 10, and the sixth joint 11. The wrist joint refers to the fifth joint 10, and the shoulder joint refers to the first joint 2. The wrist threshold range, the elbow threshold range, and the shoulder threshold range are preset. In the case where the joint angle of the wrist joint is within the wrist threshold range, it is determined that the robot arm enters a singular region, and the singular type of the singular region is a wrist singular type. By judging whether the robot arm is close to the wrist singular point through the joint angle of the wrist joint, relying on the encoder data of the wrist joint, without external measurement, the complexity and cost of the system are reduced, and when close to the singular point, the inverse kinematics calculation based on the Jacobian matrix becomes unstable. The joint angle of the wrist joint is used to determine whether the joint angle of the wrist joint is close to the wrist singular point. The wrist threshold range is determined according to the zero point calibration of the encoder of the wrist joint. Assuming that when the rotation axis of the fourth joint 7 of the robot arm and the rotation axis of the sixth joint 11 are collinear, the reading of the joint angle of the wrist joint is 0, the wrist threshold range can be set to [0°, 5°] and [355°, 360°], and in other embodiments, the wrist threshold range can be adjusted according to actual needs.

[0061] Specifically, in this application, the elbow included angle refers to the included angle of the first arm body 4 and the second arm body 6, and the elbow included angle is calculated according to the joint angle of the elbow joint. Therefore, the zero point calibration of the encoder of the elbow joint needs to be determined first. Assuming that when the robot arm is fully extended, the reading of the joint angle of the elbow joint is 0, and in this zero point calibration, the elbow included angle is calculated by subtracting 180° from the joint angle of the elbow joint. When the elbow included angle is 0° or 180°, i.e., the robot arm is fully retracted or fully extended, the robot arm is in an elbow singular point state. The elbow threshold range is set near 0° and 180°, such as [-5°, 5°], [-180°, -175°], and [175°, 180°]. After the elbow included angle enters the elbow threshold range, it is determined that the robot arm is close to the elbow singular point and enters a singular region, and the singular type is an elbow singular type. In other embodiments, the elbow threshold range can be adjusted according to actual conditions.

[0062] Specifically, the shoulder distance is calculated according to the pose and the axis vector of the shoulder joint, that is, the radial distance between the center point of the wrist and the axis vector of the shoulder joint is calculated, the axis vector of the first joint 2 is the same as the axial vector of the base 1, both are vertically upward, in the base coordinate system, the axis vector of the first joint 2 is the Z axis of the polar coordinate system, then the shoulder distance is the distance between the center point of the end effector in the XY plane and the Z axis, in the case of the shoulder distance being 0, the robot arm is in the shoulder singular point state. A shoulder threshold range is set near the shoulder distance of 0, such as [0, 0.1L], wherein L represents the arm length of the robot arm, after the shoulder distance of the robot arm enters the shoulder threshold range, it is determined that the robot arm is close to the shoulder singular point and enters the singular region, and the singular type is the shoulder singular type. In other embodiments, the shoulder threshold range can be adjusted according to actual conditions, such as adjusting 0.1L to 0.05L.

[0063] Step S3, in the case of the singular type being the wrist singular type, first-level processing is performed, in the case of the singular type being the shoulder singular type, second-level processing is performed, and in the case of the singular type being the elbow singular type, third-level processing is performed.

[0064] In actual applications, the danger caused by different types of singular points is different, the danger of the wrist singular point is less than that of the elbow singular point and the shoulder singular point, therefore, the robot arm entering the wrist singular region is subjected to first-level processing, such as speed limiting and warning processing, in the case of being unable to completely avoid, low-speed operation allows the robot arm to pass through the wrist singular region in a controllable and low-speed manner to complete the necessary task. The robot arm entering the shoulder singular region is subjected to second-level processing, such as trajectory re-planning processing. The robot arm entering the elbow singular region is subjected to third-level processing, such as pose reconstruction and trajectory re-planning processing.

[0065] In practical applications, different types of singular points have different mechanisms and effects on the motion of the robot arm. For example, when entering the elbow singular region, by reconstructing the joint configuration, the relevant joint angles can be adjusted without changing the pose of the end of the robot arm, so that the robot arm can avoid the singular configuration. Compared with directly performing trajectory re-planning, the robot arm can quickly escape from the singular state and restore normal motion ability. Then, trajectory re-planning can be performed to plan a smoother and more stable motion trajectory, avoid sudden changes in joint speed and excessive torque, and ensure the stability of the robot arm operation. When entering the shoulder singular region, the center of the wrist of the robot arm is located on the axis vector of the shoulder joint, so that the adjustment space of the reconstructed joint configuration is limited. Directly performing trajectory re-planning can quickly avoid the singular region. If the joint configuration is reconstructed first, it may waste time and affect the obstacle avoidance efficiency because the configuration cannot be effectively changed. Directly performing trajectory re-planning can avoid unstable situations such as loss of control or exceeding the physical limit of the robot arm motion caused by forcibly reconstructing the joint configuration. Different processing methods are adopted for different singular regions to avoid unnecessary calculations. For example, for the elbow singular region, the joint configuration is relatively simple and can be adjusted by adjusting part of the joint angles. When trajectory re-planning is performed, the calculation amount is also relatively small. For the shoulder singular region, if the joint configuration is reconstructed, a large amount of calculation is required to try different joint angle combinations, and effective results may not be obtained. If trajectory re-planning is directly performed, the configuration calculation is avoided, the calculation complexity is reduced, and the system operation efficiency is improved.

[0066] Further, in the case that the singular type is a wrist singular type, a first-level processing is performed, including:

[0067] In the case that the singular type is a wrist singular type, the joints of the robot arm are subjected to speed limiting processing, and a warning is issued.

[0068] In practical applications, the wrist-related joints of the robot arm are subjected to 30% speed limiting to avoid overloading, damage or loss of control of the motors of the wrist-related joints due to excessive speed instructions. In addition, due to the low risk of wrist singular points, in the case where complete avoidance is not possible, low-speed operation allows the robot arm to pass through the wrist singular region in a controllable and low-speed manner to complete the necessary tasks. In addition, after entering the wrist singular region, a warning such as an audible and visual alarm is issued to alert the staff and remind them to observe the state of the robot arm.

[0069] Further, in the case that the singular type is a shoulder singular type, a second-level processing is performed, including:

[0070] In a case where the singularity type is a shoulder singularity type, the robot arm is controlled to be joint-locked, a current pose of the robot arm is taken as a current starting point, and an avoidance path is planned according to the current starting point, a path end point, and the shoulder singularity point;

[0071] The avoidance path is sampled to obtain a plurality of path points, and each path point is converted into a corresponding avoidance joint angle;

[0072] Time parameterization is performed based on maximum speed, maximum acceleration, and maximum jerk constraints of the robot arm to obtain a time point corresponding to each avoidance joint angle;

[0073] A joint path point sequence is obtained according to each avoidance joint angle and the time point corresponding thereto, and interpolation is performed on the joint path point sequence to obtain a joint space trajectory;

[0074] The robot arm is controlled to move according to the joint space trajectory.

[0075] In actual application, in a case where the robot arm enters a shoulder singularity region, that is, in a case where it is determined that the robot arm enters a singularity region and the type is a shoulder singularity type, the joint of the robot arm is controlled to be locked, and the trajectory of the robot arm is re-planned. A time point at which the robot arm is detected to enter the shoulder singularity region is taken as a current time point, a pose at the current time point is taken as a current pose, a path end point is determined according to a task of the robot arm, a shoulder singularity point is taken as an obstacle point between the current starting point and the path end point, and a rapidly-exploring random tree (RRT) is used to obtain an avoidance path. The points of the avoidance path are usually sparse and are only used to describe a path direction, and therefore, high-density sampling is performed on the avoidance path to generate a plurality of path points, such as P0, P1, …, P n A sampling frequency is set to be far less than a preset accuracy in a chord error of a path segment between any two points in the avoidance path and an ideal curve, and the path points are increased by high-density sampling, so that the robot arm can smoothly, stably, and accurately execute the avoidance path. Inverse kinematics calculation is performed on each path point to obtain avoidance joint angles of each path point, such as Q0, Q1, …, Q n , so as to control the robot arm to move to the corresponding path point according to the avoidance joint angle.

[0076] Specifically, the conversion of each path point into a corresponding avoidance joint angle includes:

[0077] Inverse kinematics is solved on each path point to obtain a plurality of candidate joint angles corresponding to each path point;

[0078] Singularity points are detected on each candidate joint angle to screen out non-singular joint angles;

[0079] Calculate the configuration distance between the non-singular joint angle of each path point and the joint angle of the previous path point, and take the non-singular joint angle with the closest configuration distance as the avoidance joint angle.

[0080] In practical applications, the inverse kinematics can be solved by using an analytical solution method, candidate joint angles can be derived by using the geometric characteristics of the robot arm through algebraic and geometric methods, and the candidate joint angles can be solved by using the analytical solution method, which has a faster calculation speed, is beneficial to the fast response of the robot arm, and reduces the pause of the robot arm. Then, the wrist singular point and the elbow singular point of the robot arm are detected for the candidate joint angles to prevent the re-planned path from entering other singular point regions. The Euclidean distance between the non-singular joint angle of each path point and the joint angle of the previous path point is calculated to obtain the configuration distance, and the non-singular joint angle with the closest configuration distance is taken as the avoidance joint angle. The avoidance joint angle closest to the joint angle of the previous path point is screened out to ensure smooth joint movement.

[0081] Specifically, the time parameterization based on the maximum speed, maximum acceleration, and maximum jerk constraints of the robot arm obtains the time points corresponding to each avoidance joint angle, including:

[0082] The maximum speed, maximum acceleration, and maximum jerk constraints are preset, and the avoidance joint angles are initialized;

[0083] According to the maximum speed, maximum acceleration, and maximum jerk constraints, forward integration is performed on the initialized avoidance joint angles to obtain the forward integral speed of each avoidance joint angle, and backward integration is performed on the initialized avoidance joint angles to obtain the backward integral speed of each avoidance joint angle;

[0084] According to the forward integral speed and the backward integral speed of each avoidance joint angle, the joint speed of each avoidance joint angle is determined;

[0085] According to the joint speed of each avoidance joint angle, the time points corresponding to each avoidance joint angle are calculated.

[0086] In practical applications, maximum speed, maximum acceleration, maximum jerk constraints are set, i.e., maximum speed, maximum acceleration, maximum jerk of the joint are set. The initial speed of the avoidance joint angle at the current starting point is set to 0, the initial acceleration is set to 0, the final speed of the avoidance joint angle at the path end point is set to 0, the final acceleration is set to 0, the forward integral is from the avoidance joint angle at the current starting point to the avoidance joint angle at the path end point, each path segment is traversed, the speed from each avoidance joint angle is calculated, the maximum jerk is accelerated, the speed reached at the next avoidance joint angle is reached, and the forward integral speed of each avoidance joint angle is obtained. The reverse integral is from the avoidance joint angle at the path end point to the avoidance joint angle at the current starting point, each path segment is traversed in reverse, the speed from each avoidance joint angle is calculated, the maximum jerk is accelerated, the speed reached at the next avoidance joint angle is reached, and the reverse integral speed of each avoidance joint angle is obtained. During the forward integral and reverse integral, each avoidance joint angle is not greater than the maximum speed. The smaller of the forward integral speed and the reverse integral speed of the avoidance joint angle is taken as the joint speed of the avoidance joint angle. In other embodiments, a time constraint can also be set, the total time from the path starting point to the path end point is set, the total time is subtracted from the time spent by the robot arm from the path starting point to the current starting point, and the target time is obtained. The speed between the forward integral speed and the reverse integral speed of each avoidance joint angle is taken as the candidate speed, a plurality of speed solutions are obtained according to the candidate speeds of all avoidance joint angles, the time required from the current starting point to the path end point is calculated according to each speed solution, and the calculated time is obtained. The speed solution closest to the target time is taken as the joint speed of the avoidance joint angle.

[0087] Specifically, after obtaining each avoidance joint angle and its corresponding time point, a joint path point sequence is obtained, such as [(t0, Q0), (t1, Q1), …, (t n ,Q n )]. 10 11 n 1n A cubic polynomial is constructed for each joint to describe the motion of each joint, and the constructed cubic polynomial is:

[0088] q ij (t)=a ij +b ij (t-t j )+c ij (t-t j ) 2 +d ij (t-t​​j 3

[0089] wherein, t∈[t j ,t j+1 ], q ij represents the avoidance joint angle of the jth path segment of the ith joint, and then a constraint condition of a cubic polynomial is constructed, the constraint condition including a path point constraint, a continuity constraint and a boundary constraint. The path point constraint refers to the cubic polynomial corresponding to each path segment passing through the start point and the end point of the corresponding path segment, so that a ij is equal to q ij (t j ). The continuity constraint refers to the velocity and acceleration of two adjacent path segments being equal at the connection point, that is, the first derivative and the second derivative of the cubic polynomial are equal, so that the smoothness of the obtained curve is ensured. The boundary constraint refers to setting the velocity and acceleration of the current start point and the path point as zero, so that the stability of the robot arm during start and stop is ensured. After setting the constraint condition, the cubic polynomial is solved according to the constraint condition, and the solutions of a ij , b ij , c ij , d ij are obtained, so that the curve of each joint at each path segment is obtained, and then the joint space trajectory of each joint is obtained.

[0090] Specifically, the constraint condition further includes an energy constraint, which is used to control the motion consumption of the robot arm to prolong the service life of the device and improve the efficiency, and is suitable for the robot arm working for a long time. After calculating the solutions of the cubic polynomial according to the path point constraint, the continuity constraint and the boundary constraint, the first derivative and the second derivative corresponding to the solutions of the cubic polynomial are calculated, that is, the velocity and acceleration of each joint are obtained, the joint torque of each joint at each time point is calculated according to the avoidance joint angle, the velocity and the acceleration of each joint at each time point, the instantaneous mechanical power of each joint at each time point is calculated according to the joint torque and the velocity of each joint at each time point, the total instantaneous mechanical power is calculated according to the instantaneous mechanical power of each joint at each time point, the consumed energy is calculated according to the total instantaneous mechanical power, the path with low consumed energy is selected, and the efficient operation of the robot arm is ensured.

[0091] Further, in the case that the singularity type is the elbow singularity type, a three-level processing is performed, including:

[0092] In the case that the singularity type is the elbow singularity type, the robot arm is controlled to perform joint locking to obtain a current joint configuration, and the current pose of the robot arm is taken as a target pose.

[0093] ​perform inverse kinematics solving on the target pose to obtain a plurality of candidate joint configurations;

[0094] perform singularity point detection on all candidate joint configurations to screen out non-singular joint configurations;

[0095] calculate the operability indexes of each non-singular joint configuration, and take the non-singular joint configuration with the largest operability index as a target joint configuration;

[0096] release joint locking of the robot arm, and control the robot arm to move from a current joint configuration to the target joint configuration.

[0097] In actual application, when the robot arm enters an elbow singularity region, i.e., when it is determined that the robot arm enters a singularity region and the type is an elbow singularity type, joint configuration reconstruction and trajectory re-planning are performed on the robot arm. A time point at which the robot arm is detected to enter the elbow singularity region is taken as a current time point, a pose at the current time point is taken as a current pose, an analytical solution method is used to perform inverse kinematics solving on the current pose, candidate joint configurations are derived through algebraic and geometric methods based on geometric characteristics of the robot arm, the candidate joint configurations are solved through the analytical solution method, the calculation speed is relatively fast, which is beneficial to fast response of the robot arm and reduction of robot arm stop. Then, wrist singularity points and shoulder singularity points of the robot arm are detected on the candidate joint configurations to screen out non-singular joint configurations, so as to prevent the reconstructed joint configuration from entering other singularity regions. After the non-singular joint configurations are screened out, operability indexes of each non-singular joint configuration are calculated through a Jacobian matrix, the higher the operability index, the higher the flexibility and efficiency of motion under the non-singular joint configuration, and the non-singular joint configuration with the highest operability index is taken as a target joint configuration.

[0098] In the process of controlling the robot arm to move from the current joint configuration to the target joint configuration, the pose of the robot arm end is kept unchanged. When entering the elbow singularity region, the joint configuration of the robot arm is adjusted to make the joint away from the singular configuration, so as to actively avoid the elbow singularity point. After avoiding the elbow singularity point, trajectory re-planning is also needed, the elbow singularity point is taken as an obstacle point between a current starting point and a path terminal point, a rapid expansion random tree is used to obtain an avoidance path, the avoidance path is high-density sampled to generate a plurality of path points, the path points are increased through high-density sampling, so that the robot arm can smoothly, stably and accurately execute the avoidance path. Inverse kinematics calculation is performed on each path point to calculate the planned joint angle of each path point, so as to control the robot arm to move to the corresponding path point according to the planned joint angle and continue to execute the robot arm task.

[0099] The singular point avoidance method of the mechanical arm of the present application first detects whether the mechanical arm enters a singular region, judges the singular type after entering the singular region, and adopts different processing methods for different types of singular points. When entering the elbow singular region, the joint configuration is reconstructed to make the mechanical arm avoid the singular configuration. Compared with directly performing trajectory re-planning, the mechanical arm can be more quickly released from the singular state and recover normal motion ability, and then trajectory re-planning is performed to plan a smoother and more stable motion trajectory, avoid problems such as sudden change of joint speed and excessive torque, and ensure the stability of the mechanical arm operation. The joint configuration reconstruction is relatively simple and can be realized by adjusting part of the joint angle. When trajectory re-planning is performed again, the calculation amount is also relatively small. When entering the shoulder singular region, the adjustment space of the reconstructed joint configuration is limited, direct trajectory re-planning can quickly avoid the singular region, and if the reconstructed joint configuration is performed first, time may be wasted due to the inability to effectively change the configuration, affecting the obstacle avoidance efficiency. Direct trajectory re-planning can avoid unstable situations such as loss of control of the mechanical arm motion or exceeding the physical limit caused by forcibly performing the reconstructed joint configuration. In addition, joint configuration reconstruction for the shoulder singular region requires a large amount of calculation to try different joint angle combinations, and may not be able to obtain effective results. If direct trajectory re-planning is performed, the configuration calculation is avoided, the calculation complexity is reduced, and the system operation efficiency is improved.

[0100] As shown in Figure 3 The present application embodiment also provides a mechanical arm singular point avoidance system, comprising:

[0101] The acquisition module 100 is used to acquire the pose and joint angle of the mechanical arm.

[0102] The judgment module 200 is used to judge whether the singular region is entered and the singular type according to the pose and joint angle.

[0103] The processing module 300 is used to perform first-level processing in the case of the singular type being the wrist singular type, perform second-level processing in the case of the singular type being the shoulder singular type, and perform third-level processing in the case of the singular type being the elbow singular type.

[0104] The above-mentioned various modules of the mechanical arm singular point avoidance system can be all or part realized by software, hardware and combinations thereof. The above-mentioned various modules and units can be embedded in or independent of the processor in the computer device in hardware form, or can be stored in the memory in the computer device in software form, so as to be called and executed by the processor to perform the operations corresponding to the above-mentioned various modules.

[0105] As shown in Figure 4 The present application embodiment discloses a computer device, comprising a memory and a processor, and the memory stores a computer program.

[0106] The computer device can be a server, and an internal structure diagram thereof can be as shown in Figure 4 The computer device includes a processor, a memory, a network interface and a database connected through a system bus. The processor of the computer device is configured to provide computing and control capabilities. The memory of the computer device includes a non-volatile storage medium and an internal memory. The non-volatile storage medium stores an operating system, a computer program and a database. The internal memory provides an environment for running the operating system and the computer program in the non-volatile storage medium. The network interface of the computer device is configured to communicate with an external terminal through a network connection. The computer program is executed by the processor to implement the mechanical arm singularity avoidance method described in the above embodiments.

[0107] Those skilled in the art can understand that Figure 4 The structure shown in the above embodiments is only a block diagram of part of the structure related to the scheme of the present application, and does not constitute a limitation on the computer device to which the scheme of the present application is applied. Specifically, the computer device can include more or fewer components than those shown in the diagram, or combine certain components, or have a different arrangement of components.

[0108] The embodiments of the present application also disclose a computer readable storage medium storing a computer program, wherein the computer program causes a computer to execute the mechanical arm singularity avoidance method described in the above embodiments.

[0109] Those skilled in the art can understand that all or part of the processes in the above-mentioned embodiments can be completed by a computer program instructing related hardware. The computer program can be stored in a non-volatile computer readable storage medium, and when executed, can include the processes of the above-mentioned embodiments. Any reference to a memory, storage, database or other medium used in the embodiments provided by the present application can include non-volatile and / or volatile memory. The non-volatile memory can include read-only memory (ROM), programmable ROM (PROM), electrically programmable ROM (EPROM), electrically erasable programmable ROM (EEPROM) or flash memory. The volatile memory can include random access memory (RAM) or external cache memory. As an illustration but not a limitation, RAM is available in various forms, such as static RAM (SRAM), dynamic RAM (DRAM), synchronous DRAM (SDRAM), double data rate SDRAM (DDR SDRAM), enhanced SDRAM (ESDRAM), synchronous link (Synchlink) DRAM (SLDRAM), memory bus (Rambus) direct RAM (RDRAM), direct memory bus dynamic RAM (DRDRAM) and memory bus dynamic RAM (RDRAM), etc.

[0110] The technical features of the above embodiments can be combined in any manner. For the sake of brevity, not all possible combinations of the technical features described above are described, however, it is to be understood that any combination of the technical features is within the scope of the present specification, as long as the combination is not contradictory.

Claims

1. A method for avoiding singularities in a robotic arm, characterized in that, include: Obtain the pose and joint angles of the robotic arm; Determine whether the region or type of singularity has been entered based on the posture and joint angles. If the singularity type is a wrist singularity type, perform Level 1 processing; if the singularity type is a shoulder singularity type, perform Level 2 processing; if the singularity type is an elbow singularity type, perform Level 3 processing. The joint angles include: the joint angles of the wrist joint and the joint angles of the elbow joint; the determination of whether to enter a singular region and the singular type based on the posture and joint angles includes: If the joint angle of the wrist joint reaches the wrist threshold range, the robotic arm is determined to have entered a singular region, and the singular type is wrist singular type. The elbow angle is calculated based on the joint angle of the elbow joint. If the elbow angle reaches the elbow threshold range, the robotic arm is determined to have entered a singular region, and the singular type is elbow singular type. The shoulder distance is calculated based on the pose and the axis vector of the shoulder joint. The shoulder distance is the radial distance between the center point of the wrist and the axis vector of the shoulder joint. When the shoulder distance reaches the shoulder threshold range, it is determined that the robotic arm has entered the singular region, and the singular type is the shoulder singular type. In the case where the singularity type is a wrist singularity type, the first-level processing includes: In the case where the singularity is a wrist singularity, the joints of the robotic arm are subjected to speed limiting and a warning is issued. In the case where the singularity type is a shoulder singularity type, the secondary processing includes: When the singularity type is the shoulder singularity type, the current pose of the robotic arm is taken as the current starting point, and an avoidance path is planned based on the current starting point, the path ending point, and the shoulder singularity point. Multiple path points are obtained by sampling the avoidance path, and each path point is converted into a corresponding avoidance joint angle. Based on the constraints of the maximum speed, maximum acceleration, and maximum jerk of the robotic arm, time parameterization is performed to obtain the time points corresponding to the angles of each avoidance joint. Based on the joint angles of each avoidance joint and their corresponding time points, a joint path point sequence is obtained. The joint path point sequence is then interpolated to obtain the joint spatial trajectory. Control the movement of the robotic arm joints according to the joint space trajectory; In the case where the singularity type is the elbow singularity type, a three-level processing is performed, including: When the singularity type is the shoulder singularity type, the robotic arm is controlled to perform joint locking to obtain the current joint configuration, and the current pose of the robotic arm is used as the target pose. The target pose is solved by inverse kinematics to obtain multiple candidate joint configurations; Singularity detection is performed on all candidate joint configurations to filter out non-singular joint configurations. Calculate the operability index of each of the aforementioned non-singular joint configurations, and take the non-singular joint configuration with the largest operability index as the target joint configuration. Release the joint lock of the robotic arm and control the robotic arm to move from the current joint configuration to the target joint configuration.

2. The robotic arm singularity avoidance method as described in claim 1, characterized in that, The step of converting each of the path points into a corresponding avoidance joint angle includes: Inverse kinematics is performed on each of the path points to obtain multiple candidate joint angles corresponding to each path point. Singularity detection is performed on each of the candidate joint angles to filter out non-singular joint angles. Calculate the configuration distance between the non-singular joint angle and the joint angle of the previous path point, and take the non-singular joint angle with the closest configuration distance as the avoidance joint angle.

3. The robotic arm singularity avoidance method as described in claim 1, characterized in that, The time parameterization based on the maximum speed, maximum acceleration, and maximum jerk constraints of the robotic arm yields the time points corresponding to the angles of each avoidance joint, including: Pre-set the maximum speed, maximum acceleration, and maximum jerk constraints, and initialize the avoidance joint angles; Based on the constraints of maximum speed, maximum acceleration, and maximum jerk, the initial avoidance joint angles are integrated forward to obtain the forward integration velocity and forward integration time for each avoidance joint angle. The initial avoidance joint angles are then integrated backward to obtain the backward integration velocity and backward integration time for each avoidance joint angle. The joint velocities at each avoidance joint angle are determined based on the forward and backward integral velocities at each avoidance joint angle. The time points corresponding to each avoidance joint angle are calculated based on the joint velocities at each avoidance joint angle.

4. A robotic arm singularity avoidance system, characterized in that, include: The acquisition module is used to acquire the pose and joint angles of the robotic arm; The judgment module is used to determine whether the position and joint angle have entered a singular region and the type of singularity. The processing module is used to perform first-level processing when the singularity type is a wrist singularity type, second-level processing when the singularity type is a shoulder singularity type, and third-level processing when the singularity type is an elbow singularity type. The joint angles include: the joint angles of the wrist joint and the joint angles of the elbow joint; the determination of whether to enter a singular region and the singular type based on the posture and joint angles includes: If the joint angle of the wrist joint reaches the wrist threshold range, the robotic arm is determined to have entered a singular region, and the singular type is wrist singular type. The elbow angle is calculated based on the joint angle of the elbow joint. If the elbow angle reaches the elbow threshold range, the robotic arm is determined to have entered a singular region, and the singular type is elbow singular type. The shoulder distance is calculated based on the pose and the axis vector of the shoulder joint. The shoulder distance is the radial distance between the center point of the wrist and the axis vector of the shoulder joint. When the shoulder distance reaches the shoulder threshold range, it is determined that the robotic arm has entered the singular region, and the singular type is the shoulder singular type. In the case where the singularity type is a wrist singularity type, the first-level processing includes: In the case where the singularity is a wrist singularity, the joints of the robotic arm are subjected to speed limiting and a warning is issued. In the case where the singularity type is a shoulder singularity type, the secondary processing includes: When the singularity type is the shoulder singularity type, the current pose of the robotic arm is taken as the current starting point, and an avoidance path is planned based on the current starting point, the path ending point, and the shoulder singularity point. Multiple path points are obtained by sampling the avoidance path, and each path point is converted into a corresponding avoidance joint angle. Based on the constraints of the maximum speed, maximum acceleration, and maximum jerk of the robotic arm, time parameterization is performed to obtain the time points corresponding to the angles of each avoidance joint. Based on the joint angles of each avoidance joint and their corresponding time points, a joint path point sequence is obtained. The joint path point sequence is then interpolated to obtain the joint spatial trajectory. Control the movement of the robotic arm joints according to the joint space trajectory; In the case where the singularity type is the elbow singularity type, a three-level processing is performed, including: When the singularity type is the shoulder singularity type, the robotic arm is controlled to perform joint locking to obtain the current joint configuration, and the current pose of the robotic arm is used as the target pose. The target pose is solved by inverse kinematics to obtain multiple candidate joint configurations; Singularity detection is performed on all candidate joint configurations to filter out non-singular joint configurations. Calculate the operability index of each of the aforementioned non-singular joint configurations, and take the non-singular joint configuration with the largest operability index as the target joint configuration. Release the joint lock of the robotic arm and control the robotic arm to move from the current joint configuration to the target joint configuration.

5. A computer device, comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the method according to any one of claims 1-3.

6. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the steps of the method according to any one of claims 1-3.

Images