CN114571452A - Industrial robot trajectory planning method, electronic device and readable storage medium - Google Patents

Industrial robot trajectory planning method, electronic device and readable storage medium Download PDF

Info

Publication number
CN114571452A
CN114571452A CN202210193976.XA CN202210193976A CN114571452A CN 114571452 A CN114571452 A CN 114571452A CN 202210193976 A CN202210193976 A CN 202210193976A CN 114571452 A CN114571452 A CN 114571452A
Authority
CN
China
Prior art keywords
joint
robot
representing
joints
point
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
Application number
CN202210193976.XA
Other languages
Chinese (zh)
Inventor
王曦鸣
皮杰
周虎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan Vocational College of Science and Technology
Original Assignee
Hunan Vocational College of Science and Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Hunan Vocational College of Science and Technology filed Critical Hunan Vocational College of Science and Technology
Priority to CN202210193976.XA priority Critical patent/CN114571452A/en
Publication of CN114571452A publication Critical patent/CN114571452A/en
Pending legal-status Critical Current

Links

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/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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

The invention discloses an industrial robot trajectory planning method, electronic equipment and a readable storage medium, which comprises the following steps: s1, constructing a robot terminal pose matrix; s2, calculating joint rotation angles of the front three joints of the robot in the joint space; s3, expressing the tail end postures of the robot at the path starting point and the path ending point by using quaternion, obtaining the tail end posture of the robot in the time sequence through quaternion interpolation, and obtaining a tail end posture rotation matrix of the robot and a posture angle corresponding to the quaternion interpolation point based on the tail end posture rotation matrix; s4, calculating joint rotation angles of the last three joints based on the attitude angle and the joint rotation angles of the first three joints; the robot posture change planned by the invention is continuous and smooth, the tail end posture of the robot at the middle point of any path can meet the requirement, and the calculated amount is small.

Description

Industrial robot trajectory planning method, electronic device and readable storage medium
Technical Field
The invention belongs to the technical field of robots, and particularly relates to an industrial robot trajectory planning method, electronic equipment and a readable storage medium.
Background
The motion planning of the industrial robot is a key link for completing an operation task, the completion quality of the operation task of the industrial robot is seriously influenced, and generally, the motion planning of the industrial robot is divided into two types, namely joint space planning and Cartesian planning methods according to a planning space. A common industrial robot carries, loads and unloads, and plans in the joint space of the industrial robot. The motion planning is carried out in the joint space, singular points do not need to be considered, the accurate poses of the starting point and the tail point of the path can be ensured, but the poses of the points in the middle of the path are difficult to ensure.
The robot paint spraying and welding tasks have clear requirements on the pose of each point of the path, when the path is planned, a feasible continuous path is planned in a Cartesian space, then the continuous path is dispersed into a series of key points, each joint corner corresponding to the pose of the key point is obtained through inverse kinematics, and finally a proper interpolation method is selected for joint interpolation in the joint space.
Disclosure of Invention
The embodiment of the invention aims to provide an industrial robot trajectory planning method, which can ensure the accurate poses of the starting point and the final point of a path, can ensure the poses of the middle points of the path, only needs to perform inverse kinematics solution calculation on the last 3 joints of all path points, and greatly reduces the calculation amount.
The embodiment of the invention also aims to provide the electronic equipment and the readable storage medium.
In order to solve the technical problem, the technical scheme adopted by the invention is that the industrial robot trajectory planning method comprises the following steps:
s1, constructing a terminal pose matrix of the robot;
s2, acquiring the position coordinates of the robot at the path starting point and the position coordinates of the path ending point, and calculating the joint rotation angles of the front three joints of the robot at the path starting point and the path ending point according to the position coordinates;
interpolating joint rotation angles of the front three joints of the robot by using a polynomial interpolation function to obtain joint rotation angle sequences of the front three joints in time;
s3, representing the terminal postures of the robot at the path starting point and the path ending point by using quaternion, and acquiring the terminal posture Q of the robot in the time sequence based on a quaternion interpolation functiontConstructing an attitude rotation matrix and an attitude quaternion QtThe terminal attitude rotation matrix of the robot on the time sequence and the attitude angle corresponding to the quaternion interpolation point are obtained based on the conversion equation;
and S4, deforming the terminal pose matrix of the robot based on the attitude angle, substituting the terminal pose matrix into joint corners of the previous three joints, and obtaining a joint corner sequence of the three joints of the robot in time.
Further, joint angles of the first three joints of the robot in S2 are calculated as follows:
Figure BDA0003526208780000021
Figure BDA0003526208780000022
Figure BDA0003526208780000023
wherein theta is1、θ2、θ3Respectively represents the joint angles p of the 1 st joint, the 2 nd joint and the 3 rd jointx、py、pzRespectively representing the position coordinates of the robot end in a reference coordinate system, k1、k2、k3、F1、F2Are all the intermediate variables of the series of the Chinese characters,
Figure BDA0003526208780000024
k3＝pz-d1，k2＝[(k1)2+(k3)2+(a3)2-(a4)2-(d4)2]/2a3，F1＝k1-a3c2，F2＝k3-a3s2，a3length of the 3 rd link, a4Length of the 4 th link, d1Denotes the 1 stOffset of the joint, d4Shows the offset of the 4 th joint, c1Cosine value of joint angle representing the 1 st joint, c2Cosine value, s, representing joint angle of 2 nd joint2The sine value of the joint angle of the 2 nd joint is represented.
Further, the polynomial interpolation function in S2 is as follows:
Figure BDA0003526208780000025
wherein i represents a variable of the number of joints, i is 1,2,3, t represents a time point when the robot moves from a starting point to an end point, t is 0,1, …, f, f represents a total time length when the robot moves from the starting point to the end point,
Figure BDA0003526208780000026
represents the joint angle of the ith joint at time t, ai0、ai1、ai2、ai3、ai4、ai5Each represents a polynomial coefficient that interpolates the joint angle of the ith joint.
Further, the interpolation function in S3 is as follows:
Qt＝Q0 sin(Ω-Ωl(t))/sinΩ+Qf sin(Ωl(t))/sinΩ,t∈[0,f]
Q0representing the end pose of the robot at the start of the path, QfRepresenting the end pose of the robot at the path termination point, QtRepresenting the terminal pose of the robot at time point t,/(t) representing a normalized time operator, Ω representing Q0And QfThe included angle of (c);
attitude rotation matrix and attitude quaternion QtThe conversion equation of (1) is as follows:
Figure BDA0003526208780000031
wherein q is0、q1、q2、q3Are respectively provided withRepresenting a quaternion QtFour components of (2), RtRepresents QtA corresponding attitude rotation matrix.
Further, joint angles of the last three joints in S4 are calculated as follows:
Figure BDA0003526208780000032
θ4＝arctan2(-r33,r13) Or theta4＝arctan2(r33,-r13)
θ6＝arctan2(-r22,r21) Or theta6＝arctan2(r22,-r21)
Wherein theta is4、θ5、θ6Respectively represents the joint rotation angles r of the 4 th joint, the 5 th joint and the 6 th joint23＝sin(αt-θ1-θ2-θ3)/2-sin(αt-θ1+θ2+θ3)/2，r33＝-s4s5，r13＝c4s5，r21＝c6s5，r22＝-s5s6，αtRepresenting the attitude angle, s, of the robot at time t4Sine value, s, representing the joint angle of the 4 th joint5Sine value representing the joint angle of the 5 th joint, c4Cosine value representing joint angle of 4 th joint, c6Cosine value, s, representing the joint angle of the 6 th joint6A sine value representing the joint angle of the 6 th joint.
An electronic device comprises a processor, a memory and a communication bus, wherein the processor and the memory are communicated with each other through the communication bus;
a memory for storing a computer program;
and the processor is used for realizing the steps of the method when executing the program stored in the memory.
A computer-readable storage medium, in which a computer program is stored which, when being executed by a processor, carries out the above-mentioned method steps.
The invention has the beneficial effects that: the embodiment of the invention can plan a feasible robot motion path between the starting point and the ending point, is easy to control the tail end posture of the robot, and is suitable for tasks with operation requirements on the tail end posture of the robot; according to the embodiment of the invention, the motion of the first three joints is planned in the joint space, the motion planning of the last three joints is obtained through the combination of the robot tail end attitude planning and the inverse kinematics solution, the problem of singular points of the robot is not required to be considered, the inverse kinematics solution calculation is only required to be carried out on the last three joints of the robot in all path points, the calculated amount is small, and the use is convenient.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
FIG. 1 is a flow chart of trajectory planning according to an embodiment of the present invention.
Fig. 2 is a schematic diagram of the structure and operation of the robot.
Fig. 3 is a schematic diagram of a link coordinate system of the robot.
Fig. 4 is a diagram of simulation results of an 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 derived by a person skilled in the art from the embodiments given herein without making any creative effort, shall fall within the protection scope of the present invention.
Examples
As shown in fig. 1, the method for planning the trajectory of an industrial robot specifically includes the following steps:
s1, taking the ABB IRB1200 robot as an example for carrying the cup containing the liquid, the robot trajectory planning method is verified, and the D-H parameters of the industrial robot are known to be shown in Table 1:
TABLE 1D-H PARAMETERS
Connecting rod i Angle of rotation theta of jointi Offset of joint di Length of connecting rod ai Connecting rod torsion angle alpha i
1 θ1 d1 0 0
2 θ 2 0 0 90°
3 θ3 0 a3 0
4 θ4 d4 a4 -90°
5 θ 5 0 0 90°
6 θ6 0 0 90°
FIG. 2 is a block diagram of an IRB1200 robot, in which a reference coordinate system o is established at the center of the bottom surface of the robot base0x0y0z0Establishing a connecting rod coordinate system o on the IRB1200 robot by adopting a D-H parameter pre-set coordinate system methodixiyizi(i ═ 1,2,3, …,6), the link coordinate system is as shown in fig. 3, from which the link coordinate system transformation matrix of the adjacent joints of the robot is obtained as shown in equation (1):
Ti i-1＝Rot(xi-1,αi-1)Trans(ai-1,0,0)Rot(zi,θi)Trans(0,0,di)(1)
Ti i-1representing the link coordinate system oi-1xi-1yi-1zi-1To the link coordinate system oixiyiziA transformation matrix ofi-1Length, alpha, of the i-1 th linki-1Denotes the torsion angle, theta, of the i-1 th linkiIndicates the joint angle of the ith joint, diDenotes the offset, Rot (x), of the ith jointi-1,αi-1) Denotes a winding xi-1Angle of rotation alphai-1Of (a), Trans (a)i-10,0) represents an edge xi-1Translation ai-1The motion matrix of, Rot (z)i,θi) Represents a winding ziAngle of rotation thetaiRotation matrix of, Trans (0,0, d)i) Is shown along ziDistance d of translationiThe motion matrix of (2).
The end pose matrix of the robot
Figure BDA0003526208780000051
Substituting the parameters in the table 1 into the terminal pose matrix of the robot
Figure BDA0003526208780000052
The following can be obtained:
Figure BDA0003526208780000053
wherein:
nx＝s6×(c4×s1-s4×(c1×s2×s3-c1×c2×c3))-c6×(c5×(s1×s4+c4×(c1×s2×s3-c1×c2×c3))+s5×(c1×c2×s3+c1×c3×s2))
ny＝c6×(c5×(c1×s4-c4×(s1×s2×s3-c2×c3×s1))-s5×(c2×s1×s3+c3×s1×s2))-s6×(c1×c4+s4×(s1×s2×s3-c2×c3×s1))
nz＝c6×(s5×(c2×c3-s2×s3)+c4×c5×(c2×s3+c3×s2))+s4×s6×(c2×s3+c3×s2)
oy＝-c6×(c1×c4+s4×(s1×s2×s3-c2×c3×s1))-s6×(c5×(c1×s4-c4×(s1×s2×s3-c2×c3×s1))-s5×(c2×s1×s3+c3×s1×s2))
oz＝c6×s4×(c2×s3+c3×s2)-s6×(s5×(c2×c3-s2×s3)+c4×c5×(c2×s3+c3×s2))
ax＝c5×(c1×c2×s3+c1×c3×s2)-s5×(s1×s4+c4×(c1×s2×s3-c1×c2×c3))
ay＝s5×(c1×s4-c4×(s1×s2×s3-c2×c3×s1))+c5×(c2×s1×s3+c3×s1×s2)
az＝c4×s5×(c2×s3+c3×s2)-c5×(c2×c3-s2×s3)
px＝c1×(a4×c23-d4×s23+a3×c2)
py＝s1×(a4×c23-d4×s23+a3×c2)
pz＝d1+d4×c23+a4×s23+a3×s2
ci＝cosθi，si＝sinθi，cij＝cos(θi+θj)，sij＝sin(θi+θj)，(i,j＝1,2,…,6)，ciangle of articulation theta for ith nodeiCosine value of cijIs the joint angle theta of the ith jointiJoint angle theta with jth jointjCosine value of sum, siIs the joint angle theta of the ith nodeiSine value of, sijThe joint angle theta of the ith jointiJoint angle theta with jth jointjThe sine value of the sum.
S2, performing motion planning on the 1 st to 3 rd joints of the robot;
to px、pzIs transformed as shown in the following equation:
Figure BDA0003526208780000061
d4×c23+a4×s23＝pz-a3×s2-d1
solving each joint angle of the industrial robot through matrix transformation and inverse transformation to obtain:
Figure BDA0003526208780000062
is composed of (p)x)2+(pz)2The following can be obtained:
k1 cosθ2+k3 sinθ2＝k2
Figure BDA0003526208780000063
wherein k is1、k2、k3Are all the intermediate variables of the series of the Chinese characters,
Figure BDA0003526208780000064
k3＝pz-d1，k2＝[(k1)2+(k3)2+(a3)2-(a4)2-(d4)2]/2a3，a3length of the 3 rd link, a4Length of the 4 th link, c1Cosine value, d, representing the joint angle of the 1 st joint1Denotes the offset of the 1 st joint, d4Indicating the offset of the 4 th joint.
Will theta2Substitution of pxAnd pzThe following can be obtained:
Figure BDA0003526208780000065
wherein F1、F2Are all intermediate variables, F1＝k1-a3c2，F2＝k3-a3s2，c2Cosine value, s, representing joint angle of 2 nd joint2The sine value of the joint angle of the 2 nd joint is represented.
When the robot grabs the cup containing liquid, assuming that the total time length from the beginning point of the robot to the end point of the robot to place the cup is f, the terminal pose matrix of the robot
Figure BDA0003526208780000071
Can know the position coordinates of the robot at the starting point
Figure BDA0003526208780000072
And the position coordinates of the end point
Figure BDA0003526208780000073
Will be provided with
Figure BDA0003526208780000074
Respectively substituting the formulas (2), (3) and (4) to solve the joint rotation angle of the 1 st to 3 rd joints of the robot at the initial point
Figure BDA0003526208780000075
And joint angle at termination point
Figure BDA0003526208780000076
Interpolating joint corners of 1 st to 3 rd joints of the robot by using a fifth-order polynomial interpolation function to obtain a joint corner sequence of the first 3 joints corresponding to time, wherein the fifth-order polynomial interpolation function is as follows:
Figure BDA0003526208780000077
where i denotes a number variable of joints, i is 1,2,3, t denotes a time point when the robot moves from a start point to an end point, t is 0,1, …, f,
Figure BDA0003526208780000078
represents the joint angle of the ith joint at time t, ai0、ai1、ai2、ai3、ai4、ai5The motion displacement, the angular velocity and the angular acceleration of the robot joint can be ensured to be continuous by using fifth-order polynomial interpolation.
S3, performing motion planning on the 4 th to 6 th joints of the robot;
in order to ensure that liquid in the cup clamped at the tail end of the robot cannot splash in the moving process of the robot, the robot needs to always keep the y axis of the coordinate system of the wrist joint at the tail end to be vertical when grabbing, conveying and placing the cup, as shown in fig. 2.
If the motions of the joints 4-6 are planned in the joint space, only the tail end postures of the starting and ending points of the path can meet requirements, and the poses of the middle points of the path cannot be guaranteed, according to Pieper criteria, the axes of 3 adjacent joints of the industrial robot intersect at one point or are mutually parallel, then the kinematics inverse solution can obtain a plurality of groups of certain closed solutions, for a six-degree-of-freedom robot with the last 3 joints as rotating joints and the axes intersecting at one point, the poses of a tail end actuator are determined by the last 3 joints, therefore, the motion planning of the last 3 nodes directly influences the pose control effect of the robot for conveying cups, in order to achieve the goal of controlling the pose of the tail end of the robot, the pose of the tail end of the robot is planned in a Cartesian space, and then the joint rotation angles of the joints 4-6 at each motion moment are obtained through the robot inverse solution.
In order to avoid the problems of jumping points, universal joint locks, singularity and the like generated by attitude planning by a rotation matrix and an Euler angle method, attitude planning is carried out on the tail end of the robot by using a unit quaternion method, and quaternions Q are respectively used0、QfRepresenting the end pose of the robot at the start and end points of the path, Q0＝[o0,o1,o2,o3]、Qf＝[f0,f1,f2,f3]，omRepresenting a quaternion Q0Four components of (a), (b), (c), (d) and (d)nRepresenting a quaternion QfM, n is 1,2,3, 4;
Q0and QfThe included angle between them is Ω, then cos Ω is Q0·QfThe interpolation function of the attitude quaternion is:
Qt＝Q0 sin(Ω-Ωl(t))/sinΩ+Qf sin(Ωl(t))/sinΩ,t∈[0,f] (5)
Qtrepresenting the terminal pose of the robot at the time point t, l (t) representing a normalized time operator;
at any time, the attitude rotation matrix RtAnd postureQuaternion Qt＝[q0,q1,q2,q3]The conversion equation between is:
Figure BDA0003526208780000081
q0、q1、q2、q3respectively representing quaternions QtThe four components of (2) can obtain a robot tail end attitude rotation matrix sequence on a time sequence through formulas (5) and (6), and further obtain an attitude angle alpha corresponding to the quaternion interpolation point one by onet；
Because the robot is in the process of grabbing, transporting and placing the cup, the y axis of the coordinate system of the wrist joint at the tail end of the robot is always kept vertically upward, and the rotation angle alpha is allowed around the y axistThe robot tip pose position matrix is thus expressed relative to the reference coordinate system of the robot base as:
Figure BDA0003526208780000082
px′、py′、pz' respectively indicate the specific position coordinate values of the robot tip in the reference coordinate system.
By
Figure BDA0003526208780000083
Can obtain the product
Figure BDA0003526208780000084
The equation is to the left:
Figure BDA0003526208780000085
to the right of the equation:
Figure BDA0003526208780000086
from equation (7) there is:
r23＝sin(αt-θ1-θ2-θ3)/2-sin(αt-θ1+θ2+θ3)/2
r13＝cos(αt-θ1-θ2-θ3)/2+cos(αt-θ1+θ2+θ3)/2
r33＝-sin(αt-θ1)
r22＝cos(θ2+θ3)
r21＝cos(αt-θ1-θ2-θ3)/2-cos(αt-θ1+θ2+θ3)/2
from r23＝-c5Can obtain
Figure BDA0003526208780000091
From r13＝c4s5，r33＝-s4s5To obtain theta4＝arctan2(-r33,r13) Or theta4＝arctan2(r33,-r13)；
From r21＝c6s5，r22＝-s5s6To obtain theta6＝arctan2(-r22,r21) Or theta6＝arctan2(r22,-r21)；
Theta corresponding to each time instant according to the planning of the first 3 joints1、θ2、θ3And alpha is known, the joint rotation angle theta of the joints 4-6 at each moment can be obtained by the formula4、θ5、θ6。
In summary, because there are multiple sets of solutions for joint rotation angles, the method adopted in selection is as follows: all solutions are firstly solved, effective solutions are determined according to the motion ranges of all joints, then the optimal solution which is closest to the joint corner at the last moment of the robot is selected from the effective solutions, the path obtained by the method is planned, the pose requirements of the starting point and the ending point of the path are met, the tail end pose of the robot at the middle point of the path also meets the requirements, and the tail end pose of the robot changes continuously and smoothly.
The quaternion method is used for planning the attitude of the end effector, the attitude angles of the end effector with the same number as the quaternion interpolation points are obtained, then the joint rotation angles of the last three joints are obtained by combining the angles of the first three joints, and the process shows that the angles of the last three joints are related to the attitude of the end effector and the angles of the first three joints, so that the path planning obtained based on the process is smoother.
As shown in fig. 4, it can be seen from fig. 4 that the coordinates of the grabbing point of the robot are (675.34, 19.45, 736), the coordinates of the placing point are (150, 700, 596.5), the rotation angle of the tail end of the robot at the grabbing point is 0, the rotation angle α at the placing point is 77.1373 °, as described above, the robot grabs the cup and then puts the cup down until the placing point, the y-axis of the tail end wrist joint coordinate system of the robot is always kept vertically upward, the motion of the robot from the starting point to the placing point is simulated in Matlab by using the motion planning method, the motion time t is 1s, and interpolation is performed at intervals of 0.02s, and the simulation result is obtained.
Of course, if the robot end joint is to be controlled in other postures, for example, the robot end coordinate system o is to be controlled6x6y6z6Is vertical in the z-axis of
Figure BDA0003526208780000092
The method comprises the following steps:
Figure BDA0003526208780000093
namely, if the tail end joint of the robot is controlled to be in other postures, only the posture and pose matrix of the tail end joint of the robot is modified according to the control requirement
Figure BDA0003526208780000101
And calculating joint rotation angles of the three joints behind the robot by using a similar method, and further obtaining a path planning track of the robot.
The present invention also encompasses an electronic device comprising a memory for storing various computer program instructions and a processor for executing the computer program instructions to perform all or a portion of the steps recited above; the electronic device may communicate with one or more external devices, may also communicate with one or more devices that enable user interaction with the electronic device, and/or with any device that enables the electronic device to communicate with one or more other computing devices, and may also communicate with one or more networks (e.g., local area networks, wide area networks, and/or public networks) through a network adapter.
The present invention also includes a computer-readable storage medium storing a computer program that can be executed by a processor, which can include, but is not limited to, magnetic storage devices, optical disks, digital versatile disks, smart cards, and flash memory devices, which can represent one or more devices and/or other machine-readable media for storing information, which term "machine-readable medium" includes, but is not limited to, wireless channels and various other media (and/or storage media) that can store, contain, and/or carry code and/or instructions and/or data.
All the embodiments in the present specification are described in a related manner, and the same and similar parts among the embodiments may be referred to each other, and each embodiment focuses on the differences from the other embodiments. In particular, for the system embodiment, since it is substantially similar to the method embodiment, the description is simple, and for the relevant points, reference may be made to the partial description of the method embodiment.
The above description is only for the preferred embodiment of the present invention, and is not intended to limit the scope of the present invention. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention shall fall within the protection scope of the present invention.

Claims (7)

1. The method for planning the track of the industrial robot is characterized by comprising the following steps:
s1, constructing a terminal pose matrix of the robot;
s2, acquiring the position coordinates of the robot at the path starting point and the position coordinates of the robot at the path ending point, and calculating the joint rotation angles of the front three joints of the robot at the path starting point and the path ending point according to the position coordinates;
interpolating joint rotation angles of the front three joints of the robot by using a polynomial interpolation function to obtain joint rotation angle sequences of the front three joints in time;
s3, representing the terminal postures of the robot at the path starting point and the path ending point by using quaternion, and acquiring the terminal posture Q of the robot in the time sequence based on a quaternion interpolation functiontConstructing an attitude rotation matrix and an attitude quaternion QtThe terminal attitude rotation matrix of the robot on the time sequence and the attitude angle corresponding to the quaternion interpolation point are obtained based on the conversion equation;
and S4, deforming the terminal pose matrix of the robot based on the attitude angle, substituting the terminal pose matrix into joint corners of the previous three joints, and obtaining a joint corner sequence of the three joints of the robot in time.
2. The method for planning a trajectory of an industrial robot according to claim 1, wherein joint angles of the first three joints of the robot in S2 are calculated as follows:
Figure FDA0003526208770000011
Figure FDA0003526208770000012
Figure FDA0003526208770000013
wherein theta is1、θ2、θ3Respectively represents the joint angles p of the 1 st joint, the 2 nd joint and the 3 rd jointx、py、pzRespectively representing the position coordinates of the robot end in a reference coordinate system, k1、k2、k3、F1、F2Are all the intermediate variables of the series of the Chinese characters,
Figure FDA0003526208770000014
k3＝pz-d1，k2＝[(k1)2+(k3)2+(a3)2-(a4)2-(d4)2]/2a3，F1＝k1-a3c2，F2＝k3-a3s2，a3length of the 3 rd link, a4Length of the 4 th link, d1Denotes the offset of the 1 st joint, d4Shows the offset of the 4 th joint, c1Cosine value of joint angle representing the 1 st joint, c2Cosine value, s, representing joint angle of 2 nd joint2The sine value of the joint angle of the 2 nd joint is represented.
3. The industrial robot trajectory planning method according to claim 1, wherein the polynomial interpolation function in S2 is as follows:
Figure FDA0003526208770000021
where i is a variable representing the number of joints, i is 1,2,3, t represents a time point when the robot moves from a start point to an end point, t is 0,1, …, f, f represents a total length of time the robot moves from the start point to the end point,
Figure FDA0003526208770000022
indicating the ith joint at time tAngle of articulation ai0、ai1、ai2、ai3、ai4、ai5Each represents a polynomial coefficient that interpolates the joint angle of the ith joint.
4. The method for planning a trajectory of an industrial robot according to claim 1, wherein the interpolation function in S3 is as follows:
Qt＝Q0sin(Ω-Ωl(t))/sinΩ+Qfsin(Ωl(t))/sinΩ,t∈[0,f]
Q0representing the end pose of the robot at the start of the path, QfRepresenting the end pose of the robot at the path termination point, QtRepresenting the terminal pose of the robot at time point t,/(t) representing a normalized time operator, Ω representing Q0And QfThe included angle of (A);
attitude rotation matrix and attitude quaternion QtThe conversion equation of (1) is as follows:
Figure FDA0003526208770000023
wherein q is0、q1、q2、q3Respectively represent quaternions QtFour components of (2), RtRepresents QtA corresponding attitude rotation matrix.
5. The industrial robot trajectory planning method according to claim 1, wherein joint angles of the last three joints in S4 are calculated as follows:
Figure FDA0003526208770000024
θ4＝arctan2(-r33,r13) Or theta4＝arctan2(r33,-r13)
θ6＝arctan2(-r22,r21) Or theta6＝arctan2(r22,-r21)
Wherein theta is4、θ5、θ6Respectively represents the joint rotation angles r of the 4 th joint, the 5 th joint and the 6 th joint23＝sin(αt-θ1-θ2-θ3)/2-sin(αt-θ1+θ2+θ3)/2，r33＝-s4s5，r13＝c4s5，r21＝c6s5，r22＝-s5s6，αtRepresents the attitude angle, s, of the robot at time t4Sine value, s, representing the joint angle of the 4 th joint5Sine value representing the joint angle of the 5 th joint, c4Cosine value representing joint angle of 4 th joint, c6Cosine value, s, representing the joint angle of the 6 th joint6Represents the sine value of the joint angle of the 6 th joint.
6. An electronic device is characterized by comprising a processor, a memory and a communication bus, wherein the processor and the memory are communicated with each other through the communication bus;
a memory for storing a computer program;
a processor for implementing the method steps of any one of claims 1 to 5 when executing a program stored in the memory.
7. A computer-readable storage medium, characterized in that a computer program is stored in the computer-readable storage medium, which computer program, when being executed by a processor, carries out the method steps of any one of the claims 1-5.
CN202210193976.XA 2022-03-01 2022-03-01 Industrial robot trajectory planning method, electronic device and readable storage medium Pending CN114571452A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210193976.XA CN114571452A (en) 2022-03-01 2022-03-01 Industrial robot trajectory planning method, electronic device and readable storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210193976.XA CN114571452A (en) 2022-03-01 2022-03-01 Industrial robot trajectory planning method, electronic device and readable storage medium

Publications (1)

Publication Number Publication Date
CN114571452A true CN114571452A (en) 2022-06-03

Family

ID=81771245

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210193976.XA Pending CN114571452A (en) 2022-03-01 2022-03-01 Industrial robot trajectory planning method, electronic device and readable storage medium

Country Status (1)

Country Link
CN (1) CN114571452A (en)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115488876A (en) * 2022-06-22 2022-12-20 湖北商贸学院 Robot sorting method and device based on machine vision
CN115922735A (en) * 2023-01-20 2023-04-07 深圳市大族机器人有限公司 Singular area trajectory planning method and device, computer equipment and storage medium
CN116000925A (en) * 2022-12-27 2023-04-25 北京纳通医用机器人科技有限公司 Mechanical arm tracking method, device, equipment and storage medium
CN117226858A (en) * 2023-11-16 2023-12-15 浩科机器人（苏州）有限公司 Coordinated control method and system for industrial multi-axis robot

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070013916A1 (en) * 2005-07-18 2007-01-18 Kim Jung H Methods and systems for ultra-precise measurement and control of object motion in six degrees of freedom by projection and measurement of interference fringes
JP2013059815A (en) * 2011-09-12 2013-04-04 Denso Wave Inc Positioning posture interpolation method and control device for robot
CN104991448A (en) * 2015-05-25 2015-10-21 哈尔滨工程大学 Solving method of kinematics of underwater mechanical arm based on configuration plane
CN106671079A (en) * 2015-11-06 2017-05-17 中国科学院沈阳计算技术研究所有限公司 Motion control method for welding robot in coordination with positioner
CN107116542A (en) * 2017-06-28 2017-09-01 华中科技大学 Control method and system that a kind of six joint industrial robot passes through posture singular point
CN112405519A (en) * 2019-08-23 2021-02-26 贵州大学 Motion trajectory planning method for loading and unloading robot

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20070013916A1 (en) * 2005-07-18 2007-01-18 Kim Jung H Methods and systems for ultra-precise measurement and control of object motion in six degrees of freedom by projection and measurement of interference fringes
JP2013059815A (en) * 2011-09-12 2013-04-04 Denso Wave Inc Positioning posture interpolation method and control device for robot
CN104991448A (en) * 2015-05-25 2015-10-21 哈尔滨工程大学 Solving method of kinematics of underwater mechanical arm based on configuration plane
CN106671079A (en) * 2015-11-06 2017-05-17 中国科学院沈阳计算技术研究所有限公司 Motion control method for welding robot in coordination with positioner
CN107116542A (en) * 2017-06-28 2017-09-01 华中科技大学 Control method and system that a kind of six joint industrial robot passes through posture singular point
CN112405519A (en) * 2019-08-23 2021-02-26 贵州大学 Motion trajectory planning method for loading and unloading robot

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
孔民秀;季晨;: "基于单位四元数机器人姿态插补算法", 新型工业化, vol. 2013, no. 08, 20 August 2013 (2013-08-20), pages 105 - 112 *
王效杰: "基于四元数样条曲线的姿态轨迹规划研究", 中国优秀硕士学位论文全文数据库, no. 2016, 15 March 2016 (2016-03-15), pages 13 - 15 *

Cited By (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115488876A (en) * 2022-06-22 2022-12-20 湖北商贸学院 Robot sorting method and device based on machine vision
CN116000925A (en) * 2022-12-27 2023-04-25 北京纳通医用机器人科技有限公司 Mechanical arm tracking method, device, equipment and storage medium
CN115922735A (en) * 2023-01-20 2023-04-07 深圳市大族机器人有限公司 Singular area trajectory planning method and device, computer equipment and storage medium
CN117226858A (en) * 2023-11-16 2023-12-15 浩科机器人（苏州）有限公司 Coordinated control method and system for industrial multi-axis robot
CN117226858B (en) * 2023-11-16 2024-03-05 浩科机器人（苏州）有限公司 Coordinated control method and system for industrial multi-axis robot

Similar Documents

Publication Publication Date Title
CN114571452A (en) Industrial robot trajectory planning method, electronic device and readable storage medium
CN107214701B (en) A kind of livewire work mechanical arm automatic obstacle avoiding paths planning method based on movement primitive library
Sundaralingam et al. Geometric in-hand regrasp planning: Alternating optimization of finger gaits and in-grasp manipulation
CN105382835B (en) A kind of robot path planning method for passing through wrist singular point
CN106737689B (en) Super redundant mechanical arm based on mode function mixes Converse solved method and system
CN109591014B (en) Double-arm coordinated carrying method of double-arm coordinated robot
CN113715016B (en) Robot grabbing method, system, device and medium based on 3D vision
CN109986564A (en) Industrial machinery arm paths planning method
CN114952868B (en) 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot
JPH09109072A (en) Control method for redundant manipulator
CN107378952B (en) A kind of solution that redundancy mechanical arm end effector posture is kept
CN114523477B (en) Method, system and storage medium for calibrating joint pose
CN111002315B (en) Trajectory planning method and device and robot
CN108189034A (en) A kind of robot continuous path implementation method
CN115179297A (en) Method and system for controlling joint limit of joint in combined obstacle avoidance mode through position and posture of surgical robot
Mbakop et al. Path planning and control of mobile soft manipulators with obstacle avoidance
Guilamo et al. Manipulability optimization for trajectory generation
CN113103240B (en) Method, device and system for realizing C2 continuous robot trajectory planning
CN109079779A (en) The optimal Synergistic method of more mobile mechanical arms based on end estimation and operational readiness adjustment
CN113012268A (en) Method, system, device and medium for dynamic motion of static pedestrian image
CN117340879A (en) Industrial machine ginseng number identification method and system based on graph optimization model
CN109531578B (en) Humanoid mechanical arm somatosensory control method and device
CN114536351B (en) Redundant double-arm robot teaching method and device, electronic equipment and system
CN116141341A (en) Method for realizing pointing action of five-degree-of-freedom mechanical arm meeting Cartesian space constraint
CN115771146A (en) Robot posture interpolation method and device and storage medium

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