TW202021752A - Mechanical arm singular point control method and system - Google Patents

Abstract

A mechanical arm singular point control method and system. The control method comprises: acquiring mechanical arm model information, path starting point information, and path end point information, and performing reverse motion to solve the solution, and inserting a plurality of interpolation points on the straight line connecting the start point of the path and the end point of the path in the process; controlling the end of the robot arm sequentially moves through the respective interpolation points along the straight path toward the end point of the path, and pre-calculating the inverse solution result of the next interpolation point; determining whether the robot arm enters the singular area according to the inverse solution result of the next interpolation point, and when the mechanical arm enters the singular area, the motion of the arm is switched from the end along the linear path to the motion controlled by the axis motion planning algorithm; when it is determined that the robot arm does not enter the singular region, the motion of the end of the arm is controlled to follow the linear path according to the result of the inverse kinematic solution.

Description

Singular point control method and system of mechanical arm

The invention relates to the technical field of mechanical arm control, and particularly refers to a method and system for controlling singularities of a mechanical arm.

Generally speaking, there are singularity problems in the operation of mechanical arms. Take a four-axis SCARA robot arm as an example. The first, second, and fourth axis have rotational characteristics, and the third axis has linear movement characteristics. The singularity of the robotic arm appears when the second joint angle is 0, that is, the two links are collinear; take the 6-axis 6R robotic arm as an example, which has two more joints than the four-axis robotic arm. There are three singularities: the first and sixth joints are collinear, the fourth and sixth joints are collinear, and the second, third, and fifth joints are collinear.

However, in the process of implementing the present invention, the inventor found that there are at least the following technical problems in the prior art: When the robotic arm is at a singular point: (1) the degree of freedom of the robotic arm will be reduced and certain movements cannot be achieved; (2) some The angular velocity of joints tends to infinity, leading to loss of control; (3) Inverse calculation is impossible.

The purpose of the present invention is to overcome the defects of the prior art and provide A singular point control method and system for a mechanical arm can solve the problem of singular points in the operation of the mechanical arm.

To achieve the above objective, the present invention provides a robotic arm singularity control method for controlling the robotic arm to pass through a singular area, the singular area being an area including the singular point, and the control method includes the following steps: obtaining information about the robotic arm model , Path start information and path end information, and perform inverse kinematics solution; in inverse kinematics solution, insert multiple interpolation points on the straight line connecting the path start point and path end point; control according to the result of inverse kinematics solution The end of the robot arm moves toward the end of the path through each interpolation point in sequence along a linear path, and the inverse solution result of the next interpolation point is calculated in advance; the machine is judged according to the inverse solution result of the next interpolation point Whether the arm enters the singular zone, when it is determined that the manipulator enters the singular zone, when the manipulator completes the current inverse solution result, switch the motion of the manipulator from the end of the linear path to the axis motion plan Algorithm-controlled movement; when it is determined that the robotic arm does not enter the singular zone, continue to control the end of the robotic arm to move along a linear path according to the result of the inverse kinematics solution.

The beneficial effect of the present invention is that in the process of moving the end of the robotic arm along a straight path, the inverse solution result of the next interpolation point is calculated in advance and it is judged whether the robotic arm enters the singular zone. When the robotic arm enters the singular zone, the The end moves along a straight path and is switched to the movement controlled by the axis motion planning algorithm. Since the axis motion planning algorithm does not need to be solved by inverse kinematics, it avoids the singularity problem of the robotic arm and enables the robotic arm to enter the singularity zone without Report an error.

The further improvement of the singular point control method of the mechanical arm of the present invention is Therefore, the step of judging whether the manipulator enters the singular zone according to the inverse solution result of the next interpolation point includes: judging whether the inverse solution result of the next interpolation point has no solution, and when there is no solution, judging the mechanical arm The arm enters the singular zone; or it is judged whether the motion speed of each joint axis of the manipulator in the inverse solution result of the next interpolation point is greater than the preset joint axis speed threshold value, when it is greater than the preset joint axis speed threshold value, then Determine whether the robot arm enters the singular zone; or determine whether the rotation angle corresponding to the specific joint axis in the inverse solution result of the next interpolation point is within the preset rotation angle threshold value, and is within the preset rotation angle threshold value Then it is determined to enter the singular zone.

The further improvement of the singular point control method of the manipulator of the present invention is that the use of axis motion planning algorithm to control the movement of the manipulator includes: obtaining the current angle of each joint axis of the manipulator as the starting angle; according to the next interpolation point The position information calculates the corresponding joint axis angle when the end of the robot arm is at the position of the next interpolation point as the end angle; controls each joint axis of the robot arm to rotate from the starting angle to the End angle.

A further improvement of the singular point control method of the mechanical arm of the present invention is that controlling each joint axis of the mechanical arm to rotate from the starting angle to the ending angle at a set time includes: calculating the corresponding set time according to the axis motion calculation formula The motion curve of each joint axis, the axis motion calculation formula is:

Among them, qs is the starting angle of each joint axis, qe is the ending angle of each joint axis, vs is the starting speed of each joint axis, t1 is the starting time of the axis motion planning algorithm, t1=0, t2 is The termination time of the axis motion planning algorithm, t2 is equal to the set time; the motion of each joint axis is controlled according to the current angle and current speed of each joint axis corresponding to the current time in the calculated motion curve of each joint axis.

The further improvement of the singular point control method of the robot arm of the present invention is that the step of calculating the corresponding joint axis angles when the end of the robot arm is at the position of the next interpolation point according to the position information of the next interpolation point includes: Solve to obtain the end angle of each joint axis corresponding to the position of the next interpolation point.

The further improvement of the singular point control method of the manipulator of the present invention is to perform inverse kinematics solution to the path end information; when judging that the manipulator enters the singular zone according to the inverse kinematics solution result of the path end information, it is determined Whether the distance between the current position of the end of the robotic arm and the end point information of the path is greater than the set distance threshold, if it is greater than the set distance threshold, the axis motion planning algorithm is used to control the robotic arm to move to the next interpolation point; if less than Set the distance threshold value, then use the axis motion planning algorithm to control the robot arm to move directly to the end of the path.

The further improvement of the singular point control method of the robot arm of the present invention is that the step of using the axis motion planning algorithm to control the robot arm to directly move to the end of the path further includes: obtaining the current position information of the end of the robot arm in real time; Whether the current position information of the end of the arm is consistent with the path end information; when the determination is consistent, the robot arm is controlled to stop moving.

A further improvement of the singular point control method of the robot arm of the present invention is that determining whether the current position information of the end of the robot arm is consistent with the path end information, and further includes: calculating the current position information of the end of the robot arm from the The distance of the path end information, and determine whether the calculated distance is within the preset convergence threshold; when the calculated distance is within the preset convergence threshold, a consistent result is obtained .

The further improvement of the singular point control method of the robot arm of the present invention is that before the axis motion planning algorithm is used to control the motion of the robot arm, the method further includes: acquiring the current motion speed of the end of the robot arm, and determining the speed of the end of the robot arm Whether the current movement speed is greater than the preset end speed threshold; when it is determined to be greater than the preset end speed threshold, control the end of the robotic arm to smoothly decelerate until the current movement speed of the end of the robotic arm is less than or equal to the preset End speed threshold.

The present invention also provides a robotic arm singularity control system for controlling the robotic arm to pass through a singular area, the singular area being an area including the singularity, and the control system includes: an information acquisition module for acquiring a model of the robotic arm Information, path starting point information, and path ending information; the first calculation module connected to the information acquisition module is used to perform inverse kinematics solving based on the robot arm model information, path starting point information, and path ending information, and In the inverse kinematics solution, multiple interpolation points are inserted on the straight line connecting the path start point and the path end point, and the inverse solution result of the next interpolation point is obtained in advance; connected to the first calculation module for processing The module is used to control the end of the manipulator to move toward the end of the path through the interpolation points in sequence along the linear path according to the results of the inverse kinematics solution, and also to control the end of the path according to the next interpolation point The result of the inverse solution determines whether the robotic arm enters the singular area. When determining that the robotic arm enters the singular area, the processing module controls a second calculation module to calculate the result of the next interpolation point using the axis motion planning algorithm When the robot arm completes the current inverse solution result, the processing module switches the motion of the robot arm from the end of the robot arm along a linear path to the motion controlled by the axis motion planning algorithm; after determining that the robot arm has not entered the When the singular zone is described, the processing module continues to control the end of the robotic arm to move along a linear path according to the result of the inverse kinematics solution.

11‧‧‧Information Acquisition Module

12‧‧‧The first calculation module

13‧‧‧Second calculation module

14‧‧‧Processing Module

S1~S6‧‧‧Step

In order to make the above and other objectives, features, advantages and embodiments of the present invention more comprehensible, the description of the accompanying drawings is as follows: FIG. 1 is a flowchart of a method for controlling the singularity of a robotic arm according to an embodiment of the present invention; and Figure 2 is a schematic structural diagram of a singular point control system for a robotic arm according to an embodiment of the present invention.

The following disclosure provides many different embodiments or illustrations for implementing different features of the present invention. The elements and configurations in the specific examples are used in the following discussion to simplify the disclosure. Any examples discussed are only for illustrative purposes, and will not limit the scope and significance of the present invention or its examples in any way. In addition, the present disclosure may repeatedly quote numbers and/or letters in different examples. These repetitions are for simplification and explanation, and the following discussion is not specified by itself. The relationship between different embodiments and/or configurations in the

In the present invention, the "singular point" refers to the position where the robotic arm loses one or several degrees of freedom in space; the "singular area" refers to the area including the singular point, specifically refers to the specific point that causes the robotic arm to enter the singular point A settable range of joints near the singular point.

Referring to Figure 1, the present invention provides a singular point control method of a robotic arm for controlling the robotic arm to pass through the singular area. The control method includes the following steps: Step S1: Obtaining the model information of the robotic arm, path start information, and path end information, and According to the obtained manipulator model information, path start point information and path end point information, the inverse kinematics is solved.

Step S2: In the inverse kinematics solution, insert multiple interpolation points on the straight line connecting the path start point and the path end point.

Step S3: Drive each joint axis of the manipulator according to the result of the inverse kinematics solution, thereby controlling the end of the manipulator to move along the linear path through each interpolation point to the end of the path, and calculate the next interpolation point in advance The result of the inverse solution.

Specifically, when the end of the robotic arm is at the starting point of the path, the inverse solution result of the first interpolation point and the inverse solution result of the second interpolation point are calculated, and the inverse solution result of the first interpolation point is used to control the robotic arm The end of is moved along a linear path. The inverse solution result of the second interpolation point is used to determine in advance whether the robot arm enters the singular zone. When the robot arm moves to the first interpolation point, the inverse solution of the third interpolation point is pre-calculated As a result, the inverse solution result of the next interpolation point is calculated in advance. At this time, there is no need to repeat the calculation of the inverse solution result of the second interpolation point, because it has been pre-calculated at the beginning of the path. Similarly, the robot arm moves to the first When two interpolation points, pre First calculate the inverse solution result of the fourth interpolation point, that is, the robot arm pre-calculates the inverse solution result of the next interpolation point at the current interpolation point.

Step S4: According to the inverse solution result of the next interpolation point, judge whether the manipulator enters the singularity zone.

When it is determined that the robot arm has not entered the singularity zone, step S5 is executed; when it is determined that the robot arm has entered the singularity zone, step S6 is executed.

Step S5: Continue to control the end of the manipulator to move along a straight path according to the result of the inverse kinematics solution.

Step S6: When the manipulator arm completes the current inverse solution result, switch the movement of the manipulator arm from the end movement along the linear path to the movement controlled by the axis motion planning algorithm.

Next, another example is used to describe the process of controlling the robot arm to pass through the singular area by the control method of the present invention. In this example, 5 interpolation points are inserted on the straight line connecting the path start point and the path end point, which are the first interpolation point to the fifth interpolation point, and the fourth interpolation point is assumed to be a singular point. The specific control process is: the end of the robot arm is at the beginning of the path at the beginning, and the inverse solution result of the first interpolation point is calculated at this time and the inverse solution result of the second interpolation point is calculated in advance, based on the inverse solution of the first interpolation point As a result, the end of the robot arm is controlled to move along a straight path from the starting point of the path to the first interpolation point. According to the pre-calculated inverse solution result of the second interpolation point, it is judged whether the robot arm enters the singular zone. When the end of the arm reaches the first interpolation point, the inverse solution result of the third interpolation point is pre-calculated, and the inverse solution result of the third interpolation point is pre-calculated to determine whether the robotic arm enters the singular zone, and the second interpolation The result of the inverse solution of the point controls the end of the robotic arm to move from the first interpolation point to the second interpolation point along a straight path. At this time, the third The judgment result of the inverse solution result of the interpolation point is still No; when the end of the robot arm reaches the second interpolation point, the inverse solution result of the fourth interpolation point is pre-calculated, and the inverse solution result of the fourth interpolation point is calculated according to the pre-calculated inverse of the fourth interpolation point. The solution result judges and judges whether the robot arm enters the singular zone, and controls the end of the robot arm to move from the second interpolation point to the third interpolation point along a linear path according to the inverse solution result of the third interpolation point. The judgment result at this time If yes, it means that there is no inverse solution result at the fourth interpolation point. At this time, switch the axis motion mode at the third interpolation point, and obtain the rotation angle of each joint calculated by the inverse solution of the reduced degree of freedom as the axis The end of the motion, the end of the robot arm is controlled to move to the end of the path, so that when the robot encounters a singular point during the motion, it can pass the singular point smoothly.

When the path end point is located in the singularity zone, the control method of the present invention further includes the following steps: performing inverse kinematics solution to the path end point information; when it is judged that the robot arm enters the singularity zone according to the inverse kinematics solution result of the path end point information, Determine whether the distance between the current position of the end of the robotic arm and the path end information is greater than the set distance threshold; if it is greater than the set distance threshold, use the axis motion planning algorithm to control the robotic arm to move to the next interpolation point; if it is less than the set distance For the threshold value, the axis motion planning algorithm is used to control the robot arm to move directly to the end of the path.

In the following, an example is used to describe the process of the control method of the present invention to control the robot arm to reach the path located in the singular area. In this example, 5 interpolation points are inserted on the straight line connecting the path start point and the path end point, which are the first interpolation point to the fifth interpolation point. It is assumed that the third, fourth, and fourth interpolation points are The five interpolation points and the end points are located in the singular area, and the distance from the third interpolation point to the end of the path is greater than the set distance threshold and greater than the fourth interpolation point to the end of the path distance. The specific control process is as follows: the end of the robot arm is at the beginning of the path at the beginning. At this time, it is judged whether the end of the path is in the singular area. If the result of the judgment is that it is in the singular area, the inverse solution of the reduced degree of freedom is used to obtain the corresponding end point The angle of each joint axis; then calculate the inverse solution result of the first interpolation point and pre-calculate the inverse solution result of the second interpolation point. According to the inverse solution result of the first interpolation point, control the end of the robot arm from the starting point of the path along a straight path Move to the first interpolation point, and judge whether the robot arm enters the singular zone according to the pre-calculated inverse solution result of the second interpolation point. If the judgment result is no at this time, then when the end of the robot arm reaches the first interpolation point , Pre-calculate the inverse solution result of the third interpolation point, and then judge whether the manipulator enters the singular zone according to the pre-calculated inverse solution result of the third interpolation point, if the judgment result of the inverse solution result of the third interpolation point is yes , Which means that there is no inverse solution result at the third interpolation point. At this time, switch the axis motion mode at the second interpolation point, and obtain the path end point. The rotation angle of each joint calculated by the inverse solution of the reduced degree of freedom is used as the axis motion end point To control the end of the robotic arm to move to the end of the path.

The specific implementation of each step is described below.

Step S1 specifically, the path start point information and path end point information include the space coordinates of the path start point and the path end point, and the spatial posture of the robot arm when the end is at the path start point and the path end point. Those skilled in the art can understand that the inverse kinematics solution is a commonly used method in the calculation of the motion of the robot arm, so it will not be repeated here.

Step S2 Specifically, in the process of solving inverse kinematics, it is necessary to obtain the linear equation of the end of the robot moving along the linear path based on the space coordinates of the starting point of the path and the space coordinates of the end point of the path. The variable is time. The set Cartesian space velocity can be calculated from The distance that the end of the robotic arm moves in each equal time interval from the beginning of the path, so as to obtain the position information of the multiple interpolation points that the end of the robotic arm needs to pass when moving from the beginning of the path to the end of the path along a straight path.

And preferably, the spacing between the interpolation points is 0.01mm, so that when the axis motion planning algorithm is used to control the end motion of the robot arm, although the path of the end of the robot arm is an arc, the length of the arc is extremely short. , Thus very close to a straight line.

(

為機械臂末端笛卡爾空間線速度，

為機械臂各關節軸的旋轉角速度，J為Jacobian矩陣)將機械臂末端的笛卡爾空間速度換算成機械臂各關節軸的旋轉速度，並驅動機械臂的各個關節軸運動。 Step S3 specifically, obtain the Jacobian matrix through inverse kinematics, and then use the formula

(

Is the Cartesian space linear velocity at the end of the robotic arm,

Is the rotational angular velocity of each joint axis of the manipulator, J is the Jacobian matrix) Convert the Cartesian space velocity at the end of the manipulator into the rotational speed of each joint axis of the manipulator, and drive each joint axis of the manipulator to move.

Step S4 specifically includes the following three embodiments: In one embodiment, it is determined whether the inverse solution result of the next interpolation point has no solution, and when there is no solution, it is determined that the robotic arm enters the singularity zone.

In another embodiment, it is determined whether the motion speed of each joint axis of the robot arm in the inverse solution result of the next interpolation point is greater than the preset joint axis speed threshold value, and when it is greater than the preset joint axis speed threshold value, the robot arm is determined Enter the strange zone.

In another embodiment, it is determined whether the rotation angle corresponding to the specific joint axis in the inverse solution result of the next interpolation point is within the preset rotation angle threshold, and when it is within the preset rotation angle threshold, it is determined to enter the singularity zone.

Step S6 is specifically, in the use of axis motion planning algorithm control Before the robotic arm moves, first obtain the current movement speed of the end of the robotic arm, and determine whether the current movement speed of the end of the robotic arm is greater than the preset end speed threshold; when it is determined to be greater than the preset end speed threshold, control the The end speed is smoothly reduced until the current movement speed of the end of the robotic arm is less than or equal to the preset end speed threshold. Through this step, the manipulator can smoothly switch from the motion of the end along a straight path to the motion controlled by the axis motion planning algorithm.

Then, obtain the current angle of each joint axis of the robotic arm as the starting angle; obtain the angle of each joint axis at the end of the path as the end angle.

Control each joint axis of the robotic arm to rotate from the start angle to the end angle at the set time, and specifically calculate the motion curve of each joint axis corresponding to the set time according to the axis motion calculation formula. The axis motion calculation formula is: "Formula 1" . Among them, qs is the starting angle of each joint axis, qe is the ending angle of each joint axis, vs is the starting speed of each joint axis, t1 is the starting time of the axis motion planning algorithm, t1=0, t2 is The end time of the axis motion planning algorithm, t2 is equal to the set time, and the set time is determined by the acceleration/deceleration time and the length of the distance; the current angle and current speed of each joint axis corresponding to the current time in the calculated motion curve of each joint axis Control the movement of each joint axis.

When the end point of the path is in the singular area, the step of using the axis motion planning algorithm to control the robot arm to move directly to the end of the path also includes: Using the axis motion planning algorithm to control the robot arm to directly move to the end of the path, obtain the current position information of the end of the robot in real time; determine whether the current position information of the end of the robot is consistent with the path end point information, and further, calculate the machine The distance between the current position information of the end of the arm and the end point information of the path, and determine whether the calculated distance is within the preset convergence threshold; when the calculated distance is within the preset convergence threshold, then A consistent result is obtained, and the robot arm is controlled to stop moving at this time.

The singularity control system of the robot arm of the present invention will be described below with reference to the accompanying drawings.

As shown in Figure 2, the present invention also provides a robotic arm singularity control system for controlling the robotic arm to pass through the singular area. The control system includes: an information acquisition module 11 for acquiring model information of the robotic arm, path starting point information, and Path end information. Among them, the path start point information and path end point information include the space coordinates of the path start point and the path end point, and the space posture of the robot arm when the end is at the path start point and the path end point.

The first calculation module 12 connected to the information acquisition module 11 is used to perform inverse kinematics solving based on the acquired manipulator model information, path starting point information, and path end point information, and to perform inverse kinematics solving at the beginning of the path Insert multiple interpolation points on the straight line connected with the end point of the path, and calculate the inverse solution result of the next interpolation point in advance.

The processing module 14 connected to the first calculation module 12 is used to control the end of the manipulator to move toward the end of the path along the linear path through the interpolation points in sequence according to the result of the inverse kinematics solution. The result of the inverse solution of the interpolation point determines whether the robot arm enters the singularity zone, including the following Three implementation modes: In one embodiment, the processing module 14 determines whether the inverse solution result of the next interpolation point has no solution, and when there is no solution, it determines that the robotic arm enters the singularity zone.

In another embodiment, the processing module 14 determines whether the motion speed of each joint axis of the robot arm in the inverse solution result of the next interpolation point is greater than the preset joint axis speed threshold value, when it is greater than the preset joint axis speed threshold value It is determined that the robotic arm has entered the singularity zone.

In another embodiment, the processing module 14 determines whether the rotation angle corresponding to the specific joint axis in the inverse solution result of the next interpolation point is within the preset rotation angle threshold value, and determines if it is within the preset rotation angle threshold value Enter the strange zone.

When determining that the robotic arm enters the singularity zone, the processing module 14 controls a second calculation module 13 connected to the processing module 14 to calculate the result of the next interpolation point using the axis motion planning algorithm, and wait for the robotic arm to complete the current inverse When solving the result, switch the motion of the manipulator from the end along the linear path to the motion controlled by the axis motion planning algorithm; when it is determined that the manipulator does not enter the singular zone, the processing module 14 continues to control according to the result of the inverse kinematics solution The end of the robotic arm moves along a straight path.

The control system also includes a terminal speed acquisition module, which is used to acquire the current movement speed of the end of the robot arm before the axis motion planning algorithm is used to control the motion of the robot arm; the control system also includes a terminal speed acquisition module connected to And the end speed judging module of the processing module 14 for judging whether the current movement speed of the end of the manipulator is greater than the preset end speed threshold; when the judgment is greater than the preset end speed threshold, the processing module 14 controls The end of the robotic arm smoothly decelerates until the current movement speed of the end of the robotic arm is less than or equal to the preset end speed threshold, so that the robotic arm can smoothly switch from the movement of the end along a straight path to the movement controlled by the axis motion planning algorithm .

The control system also includes an angle acquisition module for acquiring the current angle of each joint axis of the manipulator as a starting angle; the angle acquisition module is connected to the second calculation module 13, and the second calculation module 13 further It is used to calculate the corresponding joint axis angles when the end of the robot arm is at the position of the next interpolation point according to the position information of the next interpolation point as the end angle, and specifically, the second calculation module 13 obtains the next An interpolation point position corresponds to the end angle of each joint axis; then the processing module 14 controls each joint axis of the robotic arm to rotate from the start angle to the end angle at a set time.

Further, the first calculation module 12 is also used to solve the inverse kinematics of the path end information; the control system also includes a distance judgment module connected to the processing module 14 for performing the reverse motion based on the path end information The result of the scientific solution judges that when the robotic arm enters the singular zone, it is judged whether the distance between the current position of the robotic arm and the end point information of the path is greater than the set distance threshold; if it is greater than the set distance threshold, the processing module 14 uses axis motion planning The algorithm controls the robot arm to move to the next interpolation point; if it is less than the set distance threshold, the processing module 14 uses the axis motion planning algorithm to control the robot arm to directly move to the end of the path.

Specifically, the control system further includes a position acquisition module, which is used to obtain the current position information of the end of the robot in real time during the process of using the axis motion planning algorithm to control the robot arm to directly move to the end of the path; The control system also includes a position determination module connected to the position acquisition module and the processing module 14 for determining whether the current position information of the end of the robot arm is consistent with the path end information. When the determination is consistent, the processing module 14 controls The robotic arm stops moving.

The beneficial effects of the singular point control method and system of the mechanical arm of the present invention are: in the process of the end of the mechanical arm moving along the linear path, the inverse solution result of the next interpolation point is calculated in advance and it is judged whether the mechanical arm enters the singular area. When the manipulator enters the singularity zone, the end moves along the linear path and switches to the motion controlled by the axis motion planning algorithm. Since the axis motion planning algorithm does not need to be solved by inverse kinematics, it avoids the singularity problem of the robot arm and makes The robotic arm can enter the singularity zone without error.

In addition, the above examples include sequential exemplary steps, but these steps need not be executed in the order shown. Performing these steps in a different order is within the scope of the present disclosure. Within the spirit and scope of the embodiments of the present disclosure, the steps may be added, replaced, changed, and/or omitted as appropriate.

Although this case has been disclosed as above by way of implementation, it is not intended to limit the case. Anyone who is familiar with this technique can make various changes and modifications without departing from the spirit and scope of this case. Therefore, the scope of protection of this case should be reviewed. The attached patent application scope shall prevail.

S1~S6‧‧‧Step

Claims (10)

A singular point control method for a mechanical arm is used to control a mechanical arm to pass through a singular area, the singular area being an area including a singular point, and the control method includes the following steps: obtaining information on the mechanical arm model, path starting point information, and path ending point information, And perform the inverse kinematics solution; in the inverse kinematics solution, insert multiple interpolation points on the straight line connecting the path starting point and the path end point; according to the inverse kinematics solution result, control the end of the robot arm along the straight path Move to the end of the path through each interpolation point in sequence, and calculate the inverse solution result of the next interpolation point in advance; judge whether the robot arm enters the singular area according to the inverse solution result of the next interpolation point, When judging that the manipulator enters the singular zone, when the manipulator completes the current inverse solution result, switch the movement of the manipulator from the end of the linear path to the movement controlled by the axis motion planning algorithm; and When the robotic arm does not enter the singular zone, continue to control the end of the robotic arm to move along a linear path according to the result of the inverse kinematics solution. The singular point control method of a manipulator according to claim 1, wherein the step of judging whether the manipulator enters the singular area according to the inverse solution result of the next interpolation point comprises: judging the inverse of the next interpolation point Whether the solution result has no solution, if there is no solution, it is determined that the robotic arm enters the singular area; or Determine whether the motion speed of each joint axis of the robot arm in the inverse solution result of the next interpolation point is greater than the preset joint axis speed threshold value, and when it is greater than the preset joint axis speed threshold value, determine that the robot arm enters the singular zone Or determine whether the rotation angle corresponding to the specific joint axis in the inverse solution result of the next interpolation point is within the preset rotation angle threshold, and when it is within the preset rotation angle threshold, it is determined to enter the singularity zone . The singular point control method of a robotic arm according to claim 1, wherein the step of using an axis motion planning algorithm to control the movement of the robotic arm includes: obtaining the current angle of each joint axis of the robotic arm as the starting angle; The position information of the interpolation point calculates the corresponding joint axis angle when the end of the robot arm is at the position of the next interpolation point as the end angle; controls each joint axis of the robot arm to set the time from the starting angle Rotate to the end angle. The singular point control method of a robotic arm according to claim 3, wherein the step of controlling each joint axis of the robotic arm to rotate from the starting angle to the ending angle at a set time includes: calculating according to an axis motion calculation formula The motion curve of each joint axis corresponding to the set time is obtained, and the axis motion calculation formula is:

Among them, qs is the starting angle of each joint axis, qe is the ending angle of each joint axis, vs is the starting speed of each joint axis, t1 is the starting time of the axis motion planning algorithm, t1=0, t2 is The end time of the axis motion planning algorithm, t2 is equal to the set time; the motion of each joint axis is controlled according to the current angle and current speed of each joint axis corresponding to the current time in the calculated motion curve of each joint axis. The singular point control method of a robotic arm according to claim 3, wherein the step of calculating the corresponding joint axis angles when the end of the robotic arm is at the position of the next interpolation point according to the position information of the next interpolation point includes: Obtain the end angle of each joint axis corresponding to the position of the next interpolation point through the inverse solution. The singular point control method of a robotic arm as described in claim 3 further includes: performing inverse kinematics on the path end point information; and judging that the robotic arm enters the singularity zone based on the inverse kinematics solution result of the path end point information When determining whether the current position of the end of the robotic arm is greater than the set distance threshold value from the path end information, if it is greater than the set distance threshold value, use the axis motion planning algorithm to control The robotic arm moves to the next interpolation point; if it is less than the set distance threshold, the axis motion planning algorithm is used to control the robotic arm to move directly to the end of the path. The singular point control method of a robotic arm according to claim 6, wherein the step of using an axis motion planning algorithm to control the robotic arm to directly move to the end of the path further includes: obtaining the current position information of the end of the robotic arm in real time; Whether the current position information of the end of the robot arm is consistent with the path end point information; when the determination is consistent, the robot arm is controlled to stop moving. The method for controlling the singularity of a robot arm according to claim 7, wherein determining whether the current position information of the end of the robot arm is consistent with the path end information, and further comprises: calculating the current position information distance of the end of the robot arm The distance of the path endpoint information, and determine whether the calculated distance is within the preset convergence threshold; when the calculated distance is within the preset convergence threshold, the judgment is consistent the result of. The singular point control method of the robotic arm as described in claim 1, before adopting the axis motion planning algorithm to control the movement of the robotic arm, it further includes: obtaining the current movement speed of the end of the robotic arm, and determining the end of the robotic arm Is the current movement speed of the motor greater than the preset end speed threshold Value; when determining that it is greater than the preset end speed threshold, control the end of the robotic arm to smoothly decelerate until the current movement speed of the end of the robotic arm is less than or equal to the preset end speed threshold. A robotic arm singular point control system for controlling the robotic arm to pass through a singular area, the singular area being an area including singular points, and the control system comprising: an information acquisition module for acquiring information on the robotic arm model and path starting point And path end information; the first calculation module connected to the information acquisition module is used to solve inverse kinematics based on the manipulator model information, path start point information, and path end point information, and perform inverse kinematics In the solution, a number of interpolation points are inserted on the straight line connecting the path start point and the path end point, and the inverse solution result of the next interpolation point is obtained in advance; the processing module connected to the first calculation module uses According to the result of the inverse kinematics solution, the end of the manipulator is controlled to move toward the end of the path through each interpolation point in sequence along a linear path, and it is also used to determine whether the manipulator is based on the inverse solution result of the next interpolation point Enter the singular area, when determining that the robotic arm enters the singular area, the processing module controls a second calculation module to calculate the result of the next interpolation point using the axis motion planning algorithm, and wait for the robotic arm to complete When the current inverse solution results, the processing module switches the motion of the robot arm from the end of the motion along the linear path to the motion controlled by the axis motion planning algorithm; when it is determined that the robot arm does not enter the singular zone, The processing module continues Continue to control the end of the robotic arm to move along a straight path according to the result of the inverse kinematics solution.

Images