CN115157251A - Singularity protection method of robot and robot system - Google Patents
Singularity protection method of robot and robot system Download PDFInfo
- Publication number
- CN115157251A CN115157251A CN202210817967.3A CN202210817967A CN115157251A CN 115157251 A CN115157251 A CN 115157251A CN 202210817967 A CN202210817967 A CN 202210817967A CN 115157251 A CN115157251 A CN 115157251A
- Authority
- CN
- China
- Prior art keywords
- robot
- singular
- configuration
- moment
- motion
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 230000033001 locomotion Effects 0.000 claims abstract description 122
- 239000011159 matrix material Substances 0.000 claims abstract description 37
- 238000004364 calculation method Methods 0.000 abstract description 7
- 230000001133 acceleration Effects 0.000 description 21
- 238000004422 calculation algorithm Methods 0.000 description 6
- 230000036461 convulsion Effects 0.000 description 6
- 230000009977 dual effect Effects 0.000 description 5
- 239000013598 vector Substances 0.000 description 4
- 230000009286 beneficial effect Effects 0.000 description 3
- 239000000463 material Substances 0.000 description 2
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000026058 directional locomotion Effects 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J13/00—Controls for manipulators
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/1607—Calculation of inertia, jacobian matrixes and inverses
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Mathematical Physics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
Abstract
The invention provides a singularity protection method of a robot and a robot system, wherein the singularity protection method comprises the following steps of: constructing a kinematic model of the robot; planning the running track of the robot; controlling the joint motion of the robot according to the running track; obtaining a Jacobian matrix of the robot joint configuration at the current motion moment according to the kinematics model; judging the change trend of the robot joint configuration at the next movement moment according to the Jacobian matrix; and adjusting the running track according to the variation trend so as to control the robot to pass through the singular critical area. According to the invention, only whether the robot is close to or far away from the singular point is judged, and the distance from the singular point is not required to be judged, so that the calculation difficulty can be reduced, and the robot has better universality; and the robot can be ensured to accurately execute the expected path, so that the motion safety and the path precision of the robot are ensured.
Description
Technical Field
The invention relates to the technical field of robot control, in particular to a strange protection method of a robot and a robot system.
Background
A singular configuration of a robot refers to a robot that, when its joint angles are at certain angles, results in a loss of freedom for some directional movements, i.e. results in the robot losing the ability to translate or rotate in one or more directions in the workspace. Obviously, if the robot has a target speed in the direction of losing the degree of freedom, when inverse kinematics calculation is performed, the speed of some joints tends to approach infinity, and the safety of the robot is seriously damaged.
In order to solve the problem, a robot manufacturer generally adopts two measures, wherein one measure is that when approaching a singular point, a prompt is directly given, and the motion of a mechanical arm is stopped; secondly, when the motion track passes through a singular point, the motion track is adjusted to bypass the singular point by utilizing a numerical method such as damped least squares and the like to calculate. However, the above measures still have the following problems:
firstly, whether a mechanical arm is close to a singular point is judged and judged by calculating an operability index and a minimum singular value index through analyzing the characteristics of a Jacobian matrix, however, the degree (distance) that the joint configuration of the robot is close to a certain fixed singular configuration is measured by lacking a universal index at present, so that a threshold value of the index for judging whether the robot is close to the singular point needs to be obtained through repeated tests, the calculation is complex, and the universality is poor; secondly, the inverse solution characteristic near the singular point is improved by numerical values, and the original path precision of the robot is lost.
Disclosure of Invention
The invention provides a singularity protection method for a robot to solve the technical problems, which only needs to judge whether the robot is close to or far away from a singularity point without judging the distance from the singularity point, so that the calculation difficulty can be reduced, the universality is better, the robot can be ensured to execute an expected path, and the safety and the path precision of the robot motion are ensured.
The technical scheme adopted by the invention is as follows:
a singular protection method of a robot comprises the following steps: constructing a kinematic model of the robot; planning the running track of the robot; controlling the joint motion of the robot according to the running track; obtaining a Jacobian matrix of the robot joint configuration at the current motion moment according to the kinematic model; judging the change trend of the robot joint configuration at the next movement moment according to the Jacobian matrix; and adjusting the running track according to the change trend so as to control the robot to pass through a singular critical area.
According to one embodiment of the invention, the trend of the robot joint configuration change at the next moment of motion comprises a trend towards and away from a singular configuration.
According to an embodiment of the present invention, the adjusting the running track according to the variation trend to control the robot to pass through a singular critical area specifically includes the following steps: if the change trend of the robot joint configuration at the next motion moment is towards a singular configuration, the operation speed of the robot is replanned by adjusting the maximum speed constraint condition of the robot; if the change trend of the robot joint configuration at the next motion moment is far away from the singular configuration, judging whether the maximum speed constraint condition of the robot changes; and if so, adjusting the maximum speed constraint condition of the robot to an initial value to re-plan the running speed of the robot.
According to one embodiment of the present invention, the constraint of adjusting the maximum speed of the robot is: the speed profile maximum speed is updated with the safe speed.
According to an embodiment of the present invention, the secure speed is obtained by: acquiring the maximum speed and the current movement direction of each joint fed back by the robot in real time; and obtaining the safe speed of the robot in the current motion direction according to the maximum speed and the current motion direction of each joint fed back by the robot in real time and the Jacobian matrix.
According to an embodiment of the present invention, the method for determining a trend of the robot joint configuration at the next movement moment according to the jacobian matrix specifically includes the following steps: calculating the numerical value of the Jacobian matrix determinant; and judging the change trend of the robot joint configuration at the next motion moment according to the numerical value of the Jacobian matrix determinant.
According to an embodiment of the invention, the method for judging the change trend of the robot joint configuration at the next movement moment according to the numerical value of the Jacobian matrix determinant specifically comprises the following steps: adopting the formula w (q) = | JJ T Calculating the numerical value of the Jacobian matrix determinant in real time; judging the change of the numerical value at the next moment, wherein the change of the numerical value at the next moment comprises numerical value increase and numerical value decrease; and judging the change trend of the robot joint configuration at the next movement moment according to the change of the numerical value at the next moment, wherein if the numerical value of the numerical value at the next moment is reduced, the robot joint configuration is judged to tend to the singular configuration at the next movement moment, and if the numerical value of the numerical value at the next moment is increased, the robot joint configuration is judged to be far away from the singular configuration at the next movement moment.
According to one embodiment of the invention, the kinematic model comprises a positive kinematic model and an inverse kinematic model, wherein the positive kinematic model is used for obtaining the pose of the robot end according to the robot joint configuration; the inverse kinematics model is used for obtaining the robot joint configuration according to the pose of the robot tail end.
According to an embodiment of the present invention, the operation track includes an operation path and an operation speed, and the planning of the operation track of the robot specifically includes the following steps: acquiring operation setting data of the robot; and planning the running path and the running speed of the robot according to the running setting data.
When the control program is executed by the control equipment, the strange protection method of the robot is realized so as to control the robot body to pass through a strange critical area.
The invention has the following beneficial effects:
1) The robot can be controlled to pass through the singular adjacent domain, and the motion path of the robot is ensured to be consistent with the set data, so that the robot can be ensured to execute the expected path, and the motion safety and the path precision of the robot are ensured;
2) The invention only needs to judge whether the robot is close to or far away from the singular point, and does not need to judge the distance from the singular point, thereby reducing the calculation difficulty and ensuring that the robot has better universality.
Drawings
FIG. 1 is a flow chart of a singular protection method of a robot according to an embodiment of the present invention;
FIG. 2 is a schematic illustration of a dual S-shaped velocity profile of an embodiment of the present invention;
FIG. 3 is a flow chart of a singularity protection method for a robot according to one embodiment of the present invention;
FIG. 4 (a) is a robot joint configuration of one embodiment of the present invention;
FIG. 4 (b) is a robot joint configuration according to another embodiment of the invention;
FIG. 5 (a) is a robot joint configuration of one embodiment of the present invention;
FIG. 5 (b) is a robot joint configuration of another embodiment of the present invention;
FIG. 5 (c) is a robot joint configuration of yet another embodiment of the present invention;
FIG. 5 (d) is a robot joint configuration according to yet another embodiment of the present invention;
fig. 6 is a block diagram schematically illustrating a control device in the robot system according to the embodiment of the present invention.
Detailed Description
The technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are only a part of the embodiments of the present invention, and not all of the embodiments. All other embodiments, which can be obtained by a person skilled in the art without making any creative effort based on the embodiments in the present invention, belong to the protection scope of the present invention.
The present invention is based on the following recognition: the robot can move freely in the working space of the robot in theory, for example, the robot can move in the working space of the robot according to the MoveJ instruction, namely, each joint can move from the current position to the target position; however, if the robot is controlled to move in the working space according to a specific path, the target pose of the robot needs to be converted into a target joint angle through inverse solution, and in the conversion process, the closer the robot is to the singular configuration, the greater the movement speed of the robot in the working space is, so that the safety problem caused by the singular configuration of the robot can be solved by controlling the movement speed of the mechanical arm in the singular direction. In other words, the singular protection problem of the robot can be converted into a planning problem, specifically, the movement speed of the robot in each movement direction of a working space can be adjusted in real time in the movement process of the robot, and a track which can be stably operated by the mechanical arm can be planned in real time by combining the limit movement speed and the acceleration of each joint of the robot. The singular protection method of the robot of the present invention will be specifically described below with reference to fig. 1.
As shown in fig. 1, the singular protection method for a robot according to the embodiment of the present invention includes the following steps:
s1, constructing a kinematics model of the robot.
Specifically, a D-H coordinate system of the robot may be established first, and a kinematic model of the robot may be constructed based on the D-H coordinate system of the robot. The kinematics model can comprise a positive kinematics model and an inverse kinematics model, the positive kinematics model can be used for obtaining the pose of the robot tail end according to the robot joint configuration, and the inverse kinematics model can be used for obtaining the robot joint configuration according to the pose of the robot tail end.
More specifically, the positive kinematics model may be T = F (q), where q is the robot joint configuration, i.e. the joint angle of the robot, and T is the pose of the robot tip; the inverse kinematics model may be q = IK (T), where IK is the inverse solution algorithm for the robot. The motion range of the robot in the working space can be obtained through an inverse solution method, namely the motion range of the robot in the working space depends on the working space covered by the inverse solution of the robot.
And S2, planning the running track of the robot.
Specifically, operation setting data of the robot may be acquired, and then the operation trajectory, i.e., the operation path and the operation speed, of the robot may be planned according to the operation setting data. The operation setting data of the robot comprises a starting position, a starting speed, a starting acceleration, a target point position, a target point speed and a target point acceleration of the robot, as well as a maximum speed in the moving process, an acceleration in the moving process and a jerk constraint in the moving process of the robot.
More specifically, planning the operation path of the robot according to the operation setting data may include the steps of: the operation path of the robot is planned according to the robot workspace path, for example, if the starting position of the robot is a, the target point position is B, and the workspace path is a straight line, the operation path of the robot, i.e., the expression of each point on the robot workspace path, may be C (S) = P (a) + S (P (B) -P (a)), where S is the path arc length of the robot workspace.
More specifically, planning the operation speed of the robot according to the operation setting data may include the steps of: and calculating the acceleration, the speed and the speed profile of the robot from the initial position to the target point position according to the initial position, the initial speed, the initial acceleration, the target point position, the target point speed and the target point acceleration of the robot, the maximum speed in the moving process of the robot, the acceleration in the moving process and the jerk constraint in the moving process. In other words, the path arc length S of the robot workspace may be described as a function S (t) of the movement time t, and for example, a dual S-type velocity profile planning algorithm may be used to describe any point on the path arc length S of the robot workspace as a function of the movement time t according to the operation setting data, P = C (S (t)).
For example, the dual S-shaped velocity profile planning algorithm shown in fig. 2 can be used to describe the path arc length S of the robot working space as an S-shaped velocity planning profile of one dimension, with the motion start and target conditions: position from 2 to 15, velocity from-4 to 4, acceleration from 0 to 0; the motion constraint conditions are as follows: maximum speed is 10, maximum acceleration is 20, and maximum jerk is 60.
And S3, controlling the joint motion of the robot according to the running track.
Specifically, the moving path of the robot from the starting position to the target point position can be controlled according to the running path, and the acceleration, the speed and the position curve profile of the robot from the starting position to the target point position can be controlled according to the moving speed.
And S4, obtaining a Jacobian matrix of the robot joint configuration at the current motion moment according to the kinematics model.
Specifically, the robot joint configuration at the current movement time may be acquired, and then the jacobian matrix of the robot joint configuration at the current movement time may be calculated from the kinematic model.
More specifically, the robot joint configuration at the current movement time, i.e. the joint angle q, may be obtained, then the pose T of the robot end, e.g. the joint coordinate system, may be calculated through the positive kinematics model, i.e. T = F (q), and finally the jacobian matrix J (q) of the robot joint configuration at the current movement time may be calculated according to the pose T of the robot end, e.g. the joint coordinate system, with the specific expression:
wherein p is end Position of the robot end joint coordinate system, p i Is the position of the ith robot joint coordinate system origin, z i And (3) regarding a direction vector of the ith robot joint in a joint coordinate system, wherein i =1.. N is the number of the robot joints.
And S5, judging the change trend of the robot joint configuration at the next movement moment according to the Jacobian matrix.
Specifically, the numerical value of the Jacobian matrix determinant can be calculated, and then the variation trend of the robot joint configuration at the next movement moment can be judged according to the numerical value of the Jacobian matrix determinant.
More specifically, the jjj can be represented by the formula w (q) = | JJ T And | calculating the numerical value w of the determinant of the Jacobian matrix in real time, and then judging the numerical value change of the numerical value w at the next moment to judge the change trend of the robot joint configuration at the next motion moment. Wherein, if the value of the value w at the next momentIf the robot joint configuration is reduced, the robot joint configuration can be judged to tend to a singular configuration at the next motion moment; if the numerical value w at the next moment is increased, the robot joint configuration can be judged to be far away from the singular configuration at the next movement moment.
And S6, adjusting the running track according to the change trend so as to control the robot to pass through a singular critical area.
Specifically, as shown in fig. 3, the step S6 further includes the following steps:
s601, judging whether the robot joint configuration is far away from the singular configuration at the next motion moment, if not, executing a step S602, and if so, executing a step S603;
s602, if the change trend of the robot joint configuration at the next motion moment tends to a singular configuration, the operation speed of the robot is re-planned by adjusting the maximum speed constraint condition of the robot;
s603, if the change trend of the robot joint configuration at the next motion moment is far away from the singular configuration, judging whether the maximum speed constraint condition of the robot changes, if so, executing a step S604, otherwise, executing a step S605;
s604, adjusting the maximum speed constraint condition of the robot to an initial value to re-plan the running speed of the robot;
s605, controlling the robot to move according to the initial running track;
and S606, judging whether the robot reaches the target point position, if so, finishing the movement, otherwise, executing the step S3.
Wherein, it should be noted that the constraint condition of the maximum speed of the robot is the maximum working space speed allowed by the robot in the current motion direction
Wherein, sigma is the maximum work space velocity allowed by the robot in the current motion direction,
is the current direction of motion of the robot. Thereby, the maximum speed of the robot can be updatedThe speed profile is adjusted in real time according to the beam conditions, so that the movement speed of the robot is replanned, the movement speed of the robot is guaranteed to be consistent with the set data through real-time planning of the movement speed of the robot, the robot can be guaranteed to safely pass through a singular area, and the movement safety of the robot is guaranteed.
In one embodiment of the invention, the velocity profile maximum velocity may be updated with a safe velocity, where the safe velocity may be the maximum workspace velocity allowed for the robot in the current direction of motion
The maximum speed and the current motion direction of each joint can be fed back in real time according to the robot
And calculating a Jacobian matrix J (q).
Specifically, the Jacobian matrix J (q) and the current direction of motion may be first determined
Computing spatial velocity vectors for robotic joints
Further, a maximum velocity constraint of each robot joint may be obtained for calculating a maximum workspace velocity σ allowed for the robot in the current motion direction, wherein the maximum velocity constraint of each robot joint is
And is
Thereby obtaining the maximum working space velocity sigma allowed by the robot in the current motion direction;
further, it can be obtained that the maximum allowable working space velocity of the robot in the current motion direction is
The process of the robot for controlling the robot to pass through the singular area by the singular protection method of the robot according to the present invention will be specifically described with reference to fig. 4 (a) and 4 (b).
As shown in fig. 4 (b), the robot joint configuration is an adjacent singular configuration, wherein the singular configuration is that the robot 3 joint is straightened, i.e. the posture is at an angle of 0, and it can be seen from fig. 4 (b) that the joint angle of the robot 3 at this time is 0.11785, and the joint angle of the singular configuration is already approached; further, referring to fig. 4 (a) and 4 (b), when the robot moves from the configuration shown in fig. 4 (a) to the configuration shown in fig. 4 (b), only the position value, i.e., the position value enclosed by the square frame, among the position value and the attitude value of the robot changes, that is, when the robot moves from the configuration shown in fig. 4 (a) to the configuration shown in fig. 4 (b), the actual movement trajectory is a movement in the X-axis direction.
Therefore, when the robot moves from the configuration shown in fig. 4 (a) to the configuration shown in fig. 4 (b), the singular protection method of the robot can calculate the maximum working space velocity of the robot moving in the X-axis direction in real time, and can adjust the velocity profile in real time by updating the maximum velocity constraint condition of the robot, so that the moving path of the robot is not deviated when the robot passes through a singular area.
Further, referring to fig. 5 (a), 5 (b), 5 (c), and 5 (d), that is, when the robot moves from the configuration shown in fig. 5 (a) to the configurations shown in fig. 5 (b), 5 (c), and 5 (d), respectively, it can be seen that only the position value in the robot position value and the attitude value, that is, the position value enclosed by the box, changes (that is, the value in the X-axis direction changes) during the movement of the robot, and thus it can be known that the singularity protection method for a robot of the present invention can ensure the accuracy of the operation path of the robot.
The invention has the following beneficial effects:
1) The invention can control the robot to pass through the singular temporary domain and ensure that the motion path of the robot is consistent with the set data, thereby ensuring that the robot executes the expected path and ensuring the motion safety and path precision of the robot;
2) The invention only needs to judge whether the robot is close to or far away from the singular point, and does not need to judge the distance from the singular point, thereby reducing the calculation difficulty and ensuring that the robot has better universality.
Corresponding to the strange protection method of the robot in the embodiment, the embodiment of the invention also provides a robot system.
The robot system of the embodiment of the invention comprises a robot body, control equipment and a control program stored in the control equipment for operation, and when the control equipment executes the control program, the strange protection method of the robot of the embodiment can be realized so as to control the robot body to pass through a strange critical area.
Specifically, as shown in fig. 6, the control device in the robot system according to the embodiment of the present invention may include a first building module 10, a second building module 20, a first control module 30, a processing module 40, a determining module 50, and a second control module 60. Wherein, the first building module 10 is used for building a kinematic model of the robot; the second building module 20 is used for planning the running track of the robot; the first control module 30 is used for controlling the joint motion of the robot according to the running track; the processing module 40 is configured to obtain a Jacobian matrix of the robot joint configuration at the current motion moment according to the kinematics model; the judging module 50 is used for judging the change trend of the robot joint configuration at the next movement moment according to the Jacobian matrix; the second control module 60 is used for adjusting the running track according to the variation trend so as to control the robot to pass through the singular critical area.
In one embodiment of the invention, the first building module 10 is particularly useful for building a D-H coordinate system of a robot and for building a kinematic model of the robot based on the D-H coordinate system of the robot. The kinematics model can comprise a positive kinematics model and an inverse kinematics model, the positive kinematics model can be used for obtaining the pose of the robot tail end according to the robot joint configuration, and the inverse kinematics model can be used for obtaining the robot joint configuration according to the pose of the robot tail end.
More specifically, the positive kinematics model may be T = F (q), where q is the robot joint configuration, i.e. the joint angle of the robot, and T is the pose of the robot tip; the inverse kinematics model may be q = IK (T), where IK is the inverse solution algorithm for the robot. The motion range of the robot in the working space can be obtained through an inverse solution method, namely the motion range of the robot in the working space depends on the working space covered by the inverse solution of the robot.
In an embodiment of the present invention, the second building module 20 may specifically be configured to obtain operation setting data of the robot, and then plan a moving track, i.e. a moving path and a moving speed, of the robot according to the operation setting data. The operation setting data of the robot comprises a starting position, a starting speed, a starting acceleration, a target point position, a target point speed and a target point acceleration of the robot, as well as a maximum speed in the moving process, an acceleration in the moving process and a jerk constraint in the moving process of the robot.
More specifically, the process of the second building module 20 planning the operation path of the robot according to the operation setting data may further include: the operation path of the robot is planned according to the robot workspace path, for example, if the starting position of the robot is a, the target point position is B, and the workspace path is a straight line, the operation path of the robot, i.e., the expression of each point on the robot workspace path, may be C (S) = P (a) + S (P (B) -P (a)), where S is the path arc length of the robot workspace.
More specifically, the process of the second building module 20 planning the operation speed of the robot according to the operation setting data may further include: and calculating the acceleration, the speed and the speed profile of the robot from the initial position to the target point position according to the initial position, the initial speed, the initial acceleration, the target point position, the target point speed and the target point acceleration of the robot, the maximum speed in the moving process of the robot, the acceleration in the moving process and the jerk constraint in the moving process. In other words, the path arc length S of the robot workspace may be described as a function S (t) of the movement time t, and for example, a dual S-type velocity profile planning algorithm may be used to describe any point on the path arc length S of the robot workspace as a function of the movement time t according to the operation setting data, P = C (S (t)).
For example, the dual S-shaped velocity profile planning algorithm shown in fig. 2 can be used to describe the path arc length S of the robot working space as an S-shaped velocity planning profile of one dimension, with the motion start and target conditions: position from 2 to 15, velocity from-4 to 4, acceleration from 0 to 0; the motion constraint conditions are as follows: maximum speed is 10, maximum acceleration is 20, and maximum jerk is 60.
In an embodiment of the present invention, the first control module 30 may be specifically configured to control a moving path of the robot from the start position to the target point position according to the running path, and may control an acceleration, a velocity, and a position curve profile of the robot from the start position to the target point position according to the moving velocity.
In an embodiment of the present invention, the processing module 40 may be specifically configured to obtain the robot joint configuration at the current motion moment, and then may calculate a jacobian matrix of the robot joint configuration at the current motion moment according to the kinematic model.
More specifically, the robot joint configuration at the current movement time, i.e. the joint angle q, may be obtained, then the pose T of the robot end, e.g. the joint coordinate system, may be calculated through the positive kinematics model, i.e. T = F (q), and finally the jacobian matrix J (q) of the robot joint configuration at the current movement time may be calculated according to the pose T of the robot end, e.g. the joint coordinate system, with the specific expression:
wherein p is end Position of the robot end joint coordinate system, p i Is the position of the ith robot joint coordinate system origin, z i And (3) setting the direction vector of the ith robot joint in a joint coordinate system, wherein i =1.. N, and n is the number of the robot joints.
In an embodiment of the present invention, the determining module 50 may be specifically configured to calculate a value of a jacobian matrix determinant, and then determine a trend of the robot joint configuration at a next movement time according to the value of the jacobian matrix determinant.
More specifically, the jjj can be represented by the formula w (q) = | JJ T And I, calculating the numerical value w of the determinant of the Jacobian matrix in real time, and judging the numerical value change of the numerical value w at the next moment to judge the change trend of the robot joint configuration at the next movement moment. If the numerical value w at the next moment is reduced, the robot joint configuration can be judged to tend to a singular configuration at the next movement moment; if the numerical value w at the next moment is increased, the robot joint configuration can be judged to be far away from the singular configuration at the next movement moment.
In an embodiment of the present invention, as shown in fig. 3, the second control module 60 adjusts the movement track according to the variation trend to control the process of the robot passing through the singular critical area, and further includes the following steps:
s601, judging whether the robot joint configuration is far away from the singular configuration at the next motion moment, if not, executing a step S602, and if so, executing a step S603;
s602, if the change trend of the robot joint configuration at the next motion moment tends to a singular configuration, the operation speed of the robot is re-planned by adjusting the maximum speed constraint condition of the robot;
s603, if the change trend of the robot joint configuration at the next motion moment is far away from the singular configuration, judging whether the maximum speed constraint condition of the robot changes, if so, executing a step S604, and if not, executing a step S605;
s604, adjusting the maximum speed constraint condition of the robot to an initial value to re-plan the running speed of the robot;
s605, controlling the robot to move according to the initial running track;
and S606, judging whether the robot reaches the target point position, if so, finishing the movement, otherwise, executing the step S3.
It should be noted that the constraint condition of the maximum speed of the robot is the maximum working space speed allowed by the robot in the current motion direction
Wherein sigma is the maximum work space velocity allowed by the robot in the current motion direction,
is the current direction of motion of the robot. From this, the maximum speed constraint condition real-time adjustment speed profile of accessible update robot to realize the replanning of robot velocity of motion, through real-time planning robot velocity of motion, not only can guarantee that the velocity of motion of robot is unanimous with the setting data, can also guarantee that the robot passes through singular region safely, thereby guarantee the security of robot motion.
In one embodiment of the invention, the velocity profile maximum velocity may be updated with a safe velocity, where the safe velocity may be the maximum workspace velocity allowed by the robot in the current direction of motion
The maximum speed and the current motion direction of each joint can be fed back according to the real-time feedback of the robot
And calculating a Jacobian matrix J (q).
Specifically, the Jacobian matrix J (q) and the current direction of motion may be first determined
Computing spatial velocity vectors for robotic joints
Further, a maximum velocity constraint of each robot joint may be obtained for calculating a maximum workspace velocity σ allowed for the robot in the current direction of motion, wherein the maximum velocity constraint of each robot joint is
And is provided with
Thereby obtaining the maximum working space velocity sigma allowed by the robot in the current motion direction;
further, it can be obtained that the maximum allowable working space velocity of the robot in the current motion direction is
The process of the singularity protection device of the robot controlling the robot to pass through the singularity area will be specifically described in conjunction with fig. 4 (a) and 4 (b).
As shown in fig. 4 (b), the robot joint configuration is an adjacent singular configuration, wherein the singular configuration is that the robot 3 joint is straightened, i.e. the posture is at an angle of 0, and it can be seen from fig. 4 (b) that the joint angle of the robot 3 at this time is 0.11785, and the joint angle of the singular configuration is already approached; further, referring to fig. 4 (a) and 4 (b), when the robot moves from the configuration shown in fig. 4 (a) to the configuration shown in fig. 4 (b), only the position value, i.e., the position value enclosed in a square frame, among the position values and attitude values of the robot changes, that is, when the robot moves from the configuration shown in fig. 4 (a) to the configuration shown in fig. 4 (b), the actual movement trajectory moves in the X-axis direction.
Therefore, when the robot moves from the configuration shown in fig. 4 (a) to the configuration shown in fig. 4 (b), the singular protection method of the robot can calculate the maximum working space velocity of the robot moving in the X-axis direction in real time, and can adjust the velocity profile in real time by updating the maximum velocity constraint condition of the robot, so that the moving path of the robot is not deviated when the robot passes through a singular area.
Further, referring to fig. 5 (a), 5 (b), 5 (c), and 5 (d), that is, when the robot moves from the configuration shown in fig. 5 (a) to the configurations shown in fig. 5 (b), 5 (c), and 5 (d), respectively, it can be seen that only the position value in the robot position value and the attitude value, that is, the position value enclosed in the box changes (that is, the value in the X-axis direction changes) during the movement of the robot, and thus it can be seen that the singularity prevention apparatus for a robot of the present invention can ensure the accuracy of the operation path of the robot.
The invention has the following beneficial effects:
1) The invention can control the robot to pass through the singular temporary domain and ensure that the motion path of the robot is consistent with the set data, thereby ensuring that the robot executes the expected path and ensuring the motion safety and path precision of the robot;
2) The invention only needs to judge whether the robot is close to or far away from the singular point, and does not need to judge the distance from the singular point, thereby reducing the calculation difficulty and ensuring that the robot has better universality.
In the description of the present invention, the terms "first" and "second" are used for descriptive purposes only and are not to be construed as indicating or implying relative importance or implying any number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. The meaning of "plurality" is two or more unless explicitly defined otherwise.
In the present invention, unless otherwise expressly stated or limited, the terms "mounted," "connected," "secured," and the like are to be construed broadly and can, for example, be fixedly connected, detachably connected, or integrally formed; can be mechanically or electrically connected; either directly or indirectly through intervening media, either internally or in any other relationship. The specific meanings of the above terms in the present invention can be understood by those skilled in the art according to specific situations.
In the present invention, unless otherwise expressly stated or limited, the first feature "on" or "under" the second feature may be directly contacting the first and second features or indirectly contacting the first and second features through an intermediate. Also, a first feature "on," "over," and "above" a second feature may be directly or diagonally above the second feature, or may simply indicate that the first feature is at a higher level than the second feature. A first feature being "under," "below," and "beneath" a second feature may be directly under or obliquely under the first feature, or may simply mean that the first feature is at a lesser elevation than the second feature.
In the description of the specification, reference to the description of "one embodiment," "some embodiments," "an example," "a specific example," or "some examples" or the like means that a particular feature, structure, material, or characteristic described in connection with the embodiment or example is included in at least one embodiment or example of the invention. In this specification, the schematic representations of the terms used above are not necessarily intended to refer to the same embodiment or example. Furthermore, the particular features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples. Furthermore, various embodiments or examples and features of different embodiments or examples described in this specification can be combined and combined by one skilled in the art without contradiction.
Claims (10)
1. A singular protection method of a robot is characterized by comprising the following steps:
constructing a kinematic model of the robot;
planning the running track of the robot;
controlling the joint motion of the robot according to the running track;
obtaining a Jacobian matrix of the robot joint configuration at the current motion moment according to the kinematic model;
judging the change trend of the robot joint configuration at the next movement moment according to the Jacobian matrix;
and adjusting the running track according to the change trend so as to control the robot to pass through a singular critical area.
2. The singular protection method for a robot as claimed in claim 1, wherein the trend of the robot joint configuration change at the next moment of motion comprises a trend towards and away from a singular configuration.
3. The singularity protection method for a robot according to claim 2, wherein the adjusting the running trajectory according to the variation trend to control the robot to pass through a singularity critical area comprises the following steps:
if the change trend of the robot joint configuration at the next motion moment is towards a singular configuration, the operation speed of the robot is replanned by adjusting the maximum speed constraint condition of the robot;
if the change trend of the robot joint configuration at the next motion moment is far away from the singular configuration, judging whether the maximum speed constraint condition of the robot changes;
and if so, adjusting the maximum speed constraint condition of the robot to an initial value to re-plan the running speed of the robot.
4. The singular guarding method for robots of claim 3, wherein the constraint of adjusting the maximum speed of the robot is: the speed profile maximum speed is updated with the safe speed.
5. The singular guarding method of a robot of claim 4, wherein the safe speed is obtained by:
acquiring the maximum speed and the current movement direction of each joint fed back by the robot in real time;
and obtaining the safe speed of the robot in the current motion direction according to the maximum speed and the current motion direction of each joint fed back by the robot in real time and the Jacobian matrix.
6. The singular protection method for a robot as claimed in claim 5, wherein the step of determining the trend of the robot joint configuration at the next moment of motion according to the Jacobian matrix comprises the following steps:
calculating the value of the Jacobian matrix determinant;
and judging the change trend of the robot joint configuration at the next motion moment according to the numerical value of the Jacobian matrix determinant.
7. The singular guarding method for robot according to claim 6, characterized in that the method for determining the trend of the robot joint configuration change at the next moment of motion according to the numerical values of the Jacobian matrix determinant specifically comprises the following steps:
adopting the formula w (q) = | JJ T I, calculating the numerical value of the Jacobian matrix determinant in real time;
judging the change of the numerical value at the next moment, wherein the change of the numerical value at the next moment comprises numerical value increase and numerical value decrease;
and judging the change trend of the robot joint configuration at the next movement moment according to the change of the numerical value at the next moment, wherein if the numerical value of the numerical value at the next moment is reduced, the robot joint configuration is judged to tend to the singular configuration at the next movement moment, and if the numerical value of the numerical value at the next moment is increased, the robot joint configuration is judged to be far away from the singular configuration at the next movement moment.
8. Singular protection method of a robot according to any of the claims 1-7, characterized in that the kinematic models comprise a forward kinematic model and an inverse kinematic model, wherein,
the positive kinematic model is used for obtaining the pose of the robot tail end according to the robot joint configuration;
the inverse kinematics model is used for obtaining the robot joint configuration according to the pose of the robot tail end.
9. The singularity protection method for the robot according to claim 6, wherein the operation track comprises an operation path and an operation speed, and the planning of the operation track of the robot specifically comprises the following steps:
acquiring operation setting data of the robot;
and planning the running path and the running speed of the robot according to the running setting data.
10. A robot system comprising a robot body, a control device and a control program stored and run on the control device, wherein the control device implements the singularity protection method for the robot according to any one of claims 1 to 9 when executing the control program to control the robot body to pass through a singularity clinical domain.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210817967.3A CN115157251A (en) | 2022-07-12 | 2022-07-12 | Singularity protection method of robot and robot system |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210817967.3A CN115157251A (en) | 2022-07-12 | 2022-07-12 | Singularity protection method of robot and robot system |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN115157251A true CN115157251A (en) | 2022-10-11 |
Family
ID=83492798
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210817967.3A Pending CN115157251A (en) | 2022-07-12 | 2022-07-12 | Singularity protection method of robot and robot system |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115157251A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116038686A (en) * | 2022-10-14 | 2023-05-02 | 深圳市大族机器人有限公司 | Robot singular point avoidance method, apparatus, computer device, and storage medium |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4716350A (en) * | 1986-12-08 | 1987-12-29 | Ford Motor Company | Method to avoid singularity in a robot mechanism |
| US5590034A (en) * | 1994-02-21 | 1996-12-31 | Asea Brown Boveri Ab | Method for controlling the movement of an industrial robot at and near singularities |
| JPH10329066A (en) * | 1997-05-26 | 1998-12-15 | Mitsubishi Heavy Ind Ltd | Method for specific posture detection and specific posture avoidance for redundant manipulator |
| US20120290131A1 (en) * | 2011-05-09 | 2012-11-15 | King Fahd University Of Petroleum And Minerals | Parallel kinematic machine trajectory planning method |
| CN103909522A (en) * | 2014-03-19 | 2014-07-09 | 华南理工大学 | Method of six-DOF industrial robot passing singular region |
| CN105437235A (en) * | 2016-01-25 | 2016-03-30 | 珠海格力电器股份有限公司 | Singular point region deceleration protection method and system and industrial robot |
| CN110802600A (en) * | 2019-11-28 | 2020-02-18 | 合肥工业大学 | A Singularity Handling Method for 6-DOF Articulated Robot |
| CN113601512A (en) * | 2021-08-23 | 2021-11-05 | 太原理工大学 | A universal method and system for avoiding singular points of robotic arms |
-
2022
- 2022-07-12 CN CN202210817967.3A patent/CN115157251A/en active Pending
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US4716350A (en) * | 1986-12-08 | 1987-12-29 | Ford Motor Company | Method to avoid singularity in a robot mechanism |
| US5590034A (en) * | 1994-02-21 | 1996-12-31 | Asea Brown Boveri Ab | Method for controlling the movement of an industrial robot at and near singularities |
| JPH10329066A (en) * | 1997-05-26 | 1998-12-15 | Mitsubishi Heavy Ind Ltd | Method for specific posture detection and specific posture avoidance for redundant manipulator |
| US20120290131A1 (en) * | 2011-05-09 | 2012-11-15 | King Fahd University Of Petroleum And Minerals | Parallel kinematic machine trajectory planning method |
| CN103909522A (en) * | 2014-03-19 | 2014-07-09 | 华南理工大学 | Method of six-DOF industrial robot passing singular region |
| CN105437235A (en) * | 2016-01-25 | 2016-03-30 | 珠海格力电器股份有限公司 | Singular point region deceleration protection method and system and industrial robot |
| CN110802600A (en) * | 2019-11-28 | 2020-02-18 | 合肥工业大学 | A Singularity Handling Method for 6-DOF Articulated Robot |
| CN113601512A (en) * | 2021-08-23 | 2021-11-05 | 太原理工大学 | A universal method and system for avoiding singular points of robotic arms |
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116038686A (en) * | 2022-10-14 | 2023-05-02 | 深圳市大族机器人有限公司 | Robot singular point avoidance method, apparatus, computer device, and storage medium |
| CN116038686B (en) * | 2022-10-14 | 2023-12-08 | 深圳市大族机器人有限公司 | Robot singular point avoidance method, apparatus, computer device, and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Seraji et al. | Real-time collision avoidance for position-controlled manipulators | |
| CN112025772B (en) | Mechanical arm autonomous calibration method based on visual measurement | |
| US10065306B2 (en) | Traveling robot, motion planning method for traveling robot, and storage medium storing program for traveling robot | |
| CN103909522B (en) | A kind of Six-DOF industrial robot is by the method in unusual territory | |
| KR101479233B1 (en) | How to control robot and its cooperative work | |
| US8600554B2 (en) | System and method for robot trajectory generation with continuous accelerations | |
| CN113334393B (en) | Mechanical arm control method and system, robot and storage medium | |
| US9597798B2 (en) | Control method of robot apparatus and robot apparatus | |
| US11279021B2 (en) | Method and apparatus for controlling robots | |
| US8406921B2 (en) | Method and device for controlling a manipulator | |
| US9120223B2 (en) | Method of controlling seven-axis articulated robot, control program, and robot control device | |
| CN111515928B (en) | Mechanical arm motion control system | |
| JP2014208400A (en) | Robot controller and robot attitude interpolation method | |
| CN115157251A (en) | Singularity protection method of robot and robot system | |
| CN113768626B (en) | Surgical robot control method, computer device and surgical robot system | |
| JP2020171989A (en) | Robot teaching system | |
| CN115674208B (en) | Robot vibration suppression device, control method and robot | |
| CN114193449B (en) | Working arm track planning method of anchor bolt support robot | |
| CN112587237A (en) | Method for reducing operation error of orthopedic operation robot | |
| CN115366115B (en) | Mechanical arm control method and control system thereof and computer readable storage medium | |
| CN113608529B (en) | Wheel type mobile platform motion trail pre-aiming tracking control system and method | |
| CN116551688A (en) | Operation method of robot for detecting shield machine or replacing cutterhead and shield machine | |
| WO2020145403A1 (en) | Control device | |
| Lei et al. | Vision-based position/impedance control for robotic assembly task | |
| Renz et al. | Uncertainty estimation for predictive collision avoidance in human-robot collaboration |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |