Method for controlling the motion of a robot arm, computing device and robot
A method for calculating an arm angle range of a robot arm includes: determining a pose at an tail end position of the robot arm; judging whether an angle of an elbow joint is within a limit thereof when an arm angle is 180 degrees; if yes, constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle; constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm; calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint; judging whether the robot arm has a secondary position joint; and intersecting with the calculated first arm angle feasible region to obtain an arm angle range if no secondary position joint is present at the robot arm.
1. A method for controlling the motion of a robot arm, comprising:
determining a pose at a tail end position of the robot arm;
judging whether an angle of an elbow joint is within a limit thereof when an arm angle is 180 degrees;
constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle if the angle of the elbow joint is within the limit;
constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm;
calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint, wherein the position joint is at least one of the other joints which are rotatable in the robot arm and affect the position of the elbow joint;
judging whether the robot arm has a secondary position joint, wherein the secondary position joint is at least one of the other joints which are rotatable in the robot arm but do not affect the position of the elbow joint; and
intersecting with the calculated first arm angle feasible region to obtain an arm angle range if no secondary position joint is present at the robot arm;
wherein the step of constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm comprises:
categorizing the other joints in the robot arm into front-end joints and rear-end joints according to a position of the elbow joint in the robot arm;
parameterizing the position of the elbow joint by using the DH parameters of the front-end joints; and
parameterizing the position of the elbow joint by using the DH parameters of the rear-end joint;
selecting the arm angle within the arm angle range; and
controlling the motion of the robot arm based on the selected arm angle, wherein the robot arm is rotatable only within the arm angle range.
2. The method according to claim 1 , further comprising:
intersecting with the calculated first arm angle feasible region to obtain a primary arm angle range if the robot arm has the secondary position joint;
iteratively calculating the arm angle of the elbow joint within the primary arm angle range; and
determining a final arm angle range when angular limit of the secondary position joint is satisfied.
3. The method according to claim 1 , wherein the step of parameterizing the position of the elbow joint by using the DH parameters of the front-end joints comprises:
acquiring a joint transformation matrix constructed for the robot arm based on the DH parameters;
determining a position matrix of each joint in the front-end joints according to the joint transformation matrix; and
calculating a position matrix in an origin coordinate system of the elbow joint according to the position matrix of each joint in the front-end joints and in combination with a positive kinematics formula 0 i T= i i-1 T* i−1 0 T to obtain tri-coordinates of the elbow joint position represented by the DH parameters of the front-end joints.
4. The method according to claim 1 , wherein the step of parameterizing the position of the elbow joint by using the DH parameters of the rear-end joints comprises:
acquiring a joint transformation matrix constructed for the robot arm based on the DH parameters;
determining a position matrix of each joint in the rear-end joints according to the joint transformation matrix; and
calculating a pose matrix in an origin coordinate system of the elbow joint according to the position matrix of each joint in the rear-end joints and in combination with a positive kinematics formula i 0 T= i−1 i T* 0 i-1 T to obtain tri-coordinates of the elbow joint position represented by the DH parameters of the rear-end joints.
5. The method according to claim 1 , wherein the step of constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle comprises:
constructing the first position matrix of the elbow joint based on a spatial circle C and in combination with the arm angle, wherein the spatial circuit C is determined by the pose, and the first position matrix is as follows:
[ Pc (1)+ R *cos(Phi)* u (1)+ R *sin(Phi)* v (1);
Pc (2)+ R *cos(Phi)* u (2)+ R *sin(Phi)* v (2);
Pc (3)+ R *cos(Phi)* u (3)+ R *sin(Phi)* v (3)]
wherein Pc(1), Pc(2) and Pc(3) represent tri-coordinates of the tail end position in the origin coordinate system, Phi represents an arm angle, R represents a shortest distance of the elbow joint to a line connecting a coordinate origin and the tail end position, u represents a direction vector of a line connecting the elbow joint to a droop vertical to a line connecting the coordinate origin and the tail end position, and v represents a product of a direction vector of a line connecting the droop to the coordinate origin and u.
6. The method according to claim 1 , wherein the step of calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint comprises:
combining the first position matrix and the second position matrix to obtain a solving equation of the arm angle of the elbow joint; and
substituting the limit of the position joint to the solving equation to obtain an arm angle feasible region corresponding to the elbow joint.
7. A computing device, comprising: a processor, a memory, a communication interface, and a communication bus; wherein the processor, the memory and the communication interface communicate with each other via the communication bus; and
the memory is configured to store at least one executable instruction, wherein the executable instruction, when being executed by the processor, causes the processor to perform the steps of:
determining a pose at a tail end position of the robot arm;
judging whether an angle of an elbow joint is within a limit thereof when an arm angle is 180 degrees;
constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle if the angle of the elbow joint is within the limit;
constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm;
calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint, wherein the position joint is at least one of the other joints which are rotatable in the robot arm and affect the position of the elbow joint;
judging whether the robot arm has a secondary position joint, wherein the secondary position joint is at least one of the other joints which are rotatable in the robot arm but do not affect the position of the elbow joint; and
intersecting with the calculated first arm angle feasible region to obtain an arm angle range if no secondary position joint is present at the robot arm;
wherein the step of constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm comprises:
categorizing the other joints in the robot arm into front-end joints and rear-end joints according to a position of the elbow joint in the robot arm;
parameterizing the position of the elbow joint by using the DH parameters of the front-end joints; and
parameterizing the position of the elbow joint by using the DH parameters of the rear-end joint;
selecting the arm angle within the arm angle range; and
controlling the motion of the robot arm based on the selected arm angle,
wherein the robot arm is rotatable only within the arm angle range.
8. The computing device according to claim 7 , wherein the executable instruction, when being executed by the processor, causes the processor further to perform the steps of:
intersecting with the calculated first arm angle feasible region to obtain a primary arm angle range if the robot arm has the secondary position joint;
iteratively calculating the arm angle of the elbow joint within the primary arm angle range; and
determining a final arm angle range when angular restraining of the secondary position joint is satisfied.
9. The computing device according to claim 7 , wherein the step of parameterizing the position of the elbow joint by using the DH parameters of the front-end joints comprises:
acquiring a joint transformation matrix constructed for the robot arm based on the DH parameters;
determining a position matrix of each joint in the front-end joints according to the joint transformation matrix; and
calculating a position matrix in an origin coordinate system of the elbow joint according to the position matrix of each joint in the front-end joints and in combination with a positive kinematics formula 0 i T= i i-1 T* i−1 0 T to obtain tri-coordinates of the elbow joint position represented by the DH parameters of the front-end joints.
10. The computing device according to claim 7 , wherein the step of parameterizing the position of the elbow joint by using the DH parameters of the rear-end joints comprises:
acquiring a joint transformation matrix constructed for the robot arm based on the DH parameters;
determining a position matrix of each joint in the rear-end joints according to the joint transformation matrix; and
calculating a pose matrix in an origin coordinate system of the elbow joint according to the position matrix of each joint in the rear-end joints and in combination with a positive kinematics formula i 0 T= i−1 i T* 0 i-1 T to obtain tri-coordinates of the elbow joint position represented by the DH parameters of the rear-end joints.
11. The computing device according to claim 7 , wherein the step of constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle comprises:
constructing the first position matrix of the elbow joint based on a spatial circle C and in combination with the arm angle, wherein the spatial circuit C is determined by the pose, and the first position matrix is as follows:
[ Pc (1)+ R *cos(Phi)* u (1)+ R *sin(Phi)* v (1);
Pc (2)+ R *cos(Phi)* u (2)+ R *sin(Phi)* v (2);
Pc (3)+ R *cos(Phi)* u (3)+ R *sin(Phi)* v (3)]
wherein Pc(1), Pc(2) and Pc(3) represent tri-coordinates of the tail end position in the origin coordinate system, Phi represents an arm angle, R represents a shortest distance of the elbow joint to a line connecting a coordinate origin and the tail end position, u represents a direction vector of a line connecting the elbow joint to a droop perpendicular to a line connecting the coordinate origin and the tail end position, and v represents a product of a direction vector of a line connecting the droop to the coordinate origin and u.
12. The computing device according to claim 7 , wherein the step of calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint comprises:
combining the first position matrix and the second position matrix to obtain a solving equation of the arm angle of the elbow joint; and
substituting the limit of the position joint to the solving equation to obtain an arm angle feasible region corresponding to the elbow joint.
13. A robot, comprising the computing device, the computing device comprising: a processor, a memory, a communication interface, and a communication bus; wherein the processor, the memory and the communication interface communicate with each other via the communication bus; and
the memory is configured to store at least one executable instruction, wherein the executable instruction, when being executed by the processor, causes the processor to perform the steps of:
determining a pose at a tail end position of the robot arm;
judging whether an angle of an elbow joint is within a limit thereof when an arm angle is 180 degrees;
constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle if the angle of the elbow joint is within the limit;
constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm;
calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint, wherein the position joint is at least one of the other joints which are rotatable in the robot arm and affect the position of the elbow joint;
judging whether the robot arm has a secondary position joint, wherein the secondary position joint is at least one of the other joints which are rotatable in the robot arm but do not affect the position of the elbow joint; and
intersecting with the calculated first arm angle feasible region to obtain an arm angle range if no secondary position joint is present at the robot arm;
wherein the step of constructing a second position matrix of the elbow joint by using DH parameters of other joints of the robot arm comprises:
categorizing the other joints in the robot arm into front-end joints and rear-end joints according to a position of the elbow joint in the robot arm;
parameterizing the position of the elbow joint by using the DH parameters of the front-end joints; and
parameterizing the position of the elbow joint by using the DH parameters of the rear-end joint;
selecting the arm angle within the arm angle range; and
controlling the motion of the robot arm based on the selected arm angle,
wherein the robot arm is rotatable only within the arm angle range.
14. The robot according to claim 13 , wherein the executable instruction, when being executed by the processor, causes the processor further to perform the steps of:
intersecting with the calculated first arm angle feasible region to obtain a primary arm angle range if the robot arm has the secondary position joint;
iteratively calculating the arm angle of the elbow joint within the primary arm angle range; and
determining a final arm angle range when angular restraining of the secondary position joint is satisfied.
15. The robot according to claim 13 , wherein the step of parameterizing the position of the elbow joint by using the DH parameters of the front-end joints comprises:
acquiring a joint transformation matrix constructed for the robot arm based on the DH parameters;
determining a position matrix of each joint in the front-end joints according to the joint transformation matrix; and
calculating a position matrix in an origin coordinate system of the elbow joint according to the position matrix of each joint in the front-end joints and in combination with a positive kinematics formula 0 i T= i i-1 T* i−1 0 T to obtain tri-coordinates of the elbow joint position represented by the DH parameters of the front-end joints.
16. The robot according to claim 13 , wherein the step of parameterizing the position of the elbow joint by using the DH parameters of the rear-end joints comprises:
acquiring a joint transformation matrix constructed for the robot arm based on the DH parameters;
determining a position matrix of each joint in the rear-end joints according to the joint transformation matrix; and
calculating a pose matrix in an origin coordinate system of the elbow joint according to the position matrix of each joint in the rear-end joints and in combination with a positive kinematics formula i 0 T= i−1 i T* 0 i-1 T to obtain tri-coordinates of the elbow joint position represented by the DH parameters of the rear-end joints.
17. The robot according to claim 13 , wherein the step of constructing a first position matrix characterizing the elbow joint by using the pose and the arm angle comprises:
constructing the first position matrix of the elbow joint based on a spatial circle C and in combination with the arm angle, wherein the spatial circuit C is determined by the pose, and the first position matrix is as follows:
[ Pc (1)+ R *cos(Phi)* u (1)+ R *sin(Phi)* v (1);
Pc (2)+ R *cos(Phi)* u (2)+ R *sin(Phi)* v (2);
Pc (3)+ R *cos(Phi)* u (3)+ R *sin(Phi)* v (3)]
wherein Pc(1), Pc(2) and Pc(3) represent tri-coordinates of the tail end position in the origin coordinate system, Phi represents an arm angle, R represents a shortest distance of the elbow joint to a line connecting a coordinate origin and the tail end position, u represents a direction vector of a line connecting the elbow joint to a droop perpendicular to a line connecting the coordinate origin and the tail end position, and v represents a product of a direction vector of a line connecting the droop to the coordinate origin and u.
18. The robot according to claim 13 , wherein the step of calculating, according to the first position matrix and the second position matrix, a first arm angle feasible region satisfying a limit of a position joint comprises:
combining the first position matrix and the second position matrix to obtain a solving equation of the arm angle of the elbow joint; and
substituting the limit of the position joint to the solving equation to obtain an arm angle feasible region corresponding to the elbow joint.