CN116810802A - Offset mechanical arm discrete point track smooth planning method, system and storage medium - Google Patents

Offset mechanical arm discrete point track smooth planning method, system and storage medium Download PDF

Info

Publication number
CN116810802A
CN116810802A CN202311083054.4A CN202311083054A CN116810802A CN 116810802 A CN116810802 A CN 116810802A CN 202311083054 A CN202311083054 A CN 202311083054A CN 116810802 A CN116810802 A CN 116810802A
Authority
CN
China
Prior art keywords
planning
joint angle
joint
average
discrete 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.)
Granted
Application number
CN202311083054.4A
Other languages
Chinese (zh)
Other versions
CN116810802B (en
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.)
Jiangsu Yunmu Zhizao Technology Co ltd
Shanghai Aerospace Control Technology Institute
Original Assignee
Jiangsu Yunmu Zhizao Technology Co ltd
Shanghai Aerospace Control Technology Institute
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 Jiangsu Yunmu Zhizao Technology Co ltd, Shanghai Aerospace Control Technology Institute filed Critical Jiangsu Yunmu Zhizao Technology Co ltd
Priority to CN202311083054.4A priority Critical patent/CN116810802B/en
Publication of CN116810802A publication Critical patent/CN116810802A/en
Application granted granted Critical
Publication of CN116810802B publication Critical patent/CN116810802B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • 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/1628—Programme controls characterised by the control loop
    • 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
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

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

Abstract

The application relates to the technical field of mechanical arm space control, in particular to a method, a system and a storage medium for smoothly planning a discrete point track of an offset mechanical arm. The method comprises the following steps: s1, reading and determining a joint angle discrete point sequence; s2, judging the properties of the joint angle discrete point sequence, and S3, planning the primary speed according to a preset primary planning movement step length; s4, performing secondary interpolation to obtain joint angles, joint angular velocities and joint angular accelerations among discrete points, and realizing secondary position planning, speed planning and acceleration planning of paths among the discrete points; s5, calculating the corresponding tail end pose, tail end speed and tail end acceleration of the mechanical arm through positive kinematics; s6, comparing the detected value with a preset limit value, and judging whether the detected value exceeds the limit value; and S7, outputting joint angle planning information. The quadratic interpolation calculation is implemented in case the boundary condition of the input of the upper system has only the position of each segment path end point.

Description

Offset mechanical arm discrete point track smooth planning method, system and storage medium
Technical Field
The application relates to the technical field of mechanical arm space control, in particular to a method, a system and a storage medium for smoothly planning a discrete point track of an offset mechanical arm.
Background
When the mechanical arm moves from one position to another designated position to perform task operation, continuous and smooth joint angular positions, speeds and accelerations can be planned by only performing polynomial interpolation on the initial arm type and the target arm type. However, when the space where the mechanical arm is located is narrow and more obstacles exist, if the path planning is only performed for the initial arm type and the target arm type, the mechanical arm is easy to interfere with the environment in the moving process due to the overlong planned path, so that the mechanical arm is damaged or the environment is damaged, and the planned path is required to be decomposed into a plurality of small-section paths. The more the number of the segments of the planned path is divided, the more accurate the movement range of the mechanical arm is, and the interference with the environment can be avoided.
The current interpolation method for small segment paths needs to set boundary conditions of the segment paths by the upper layer system to ensure that the segments are connected, wherein the boundary conditions comprise a starting point, an ending point and the position and the speed of the end point of each segment path between the two points. Namely, assuming that the position and speed conditions of the end points of each segmented path are known, the existing interpolation method can be adopted to conduct secondary interpolation, and planning and solving can be conducted on each segment.
If the boundary condition of the input of the upper layer system only has the position of each segment path end point, namely only has a joint angle discrete sequence and does not have the angular velocity corresponding to the joint angle, the calculation amount of the upper layer system can be simplified, but the secondary interpolation calculation cannot be realized according to the existing calculation method. In addition, since the upper system has randomness when only the joint angle discrete sequence is input, it is difficult to ensure that the corresponding joint angle discrete sequence can be suitable for quadratic interpolation.
Disclosure of Invention
In order to overcome the defects of the prior art, the application provides a method, a system and a storage medium for smoothly planning the discrete point track of an offset mechanical arm, and the secondary interpolation calculation is realized under the condition that the input boundary condition of an upper system only has the positions of the end points of each sectional path; in addition, multiple return decisions are provided to accommodate the randomness of the discrete sequence of joint angles.
The technical scheme of the application is as follows: a smooth planning method for discrete point tracks of an offset mechanical arm comprises the following steps:
s1, reading and determining a joint angle discrete point sequence; the joint angle discrete point sequence is generated and transmitted by the upper system, and the joint angle discrete point sequence is a group of sequences with randomness, which are obtained by the upper system through inverse kinematics calculation according to the track required to be moved by the tail end of the mechanical arm. The lower layer system and the upper layer system corresponding to the offset mechanical arm discrete point track smooth planning method respectively belong to independent modules, so that the generation of the joint angle discrete point sequence by the lower layer system is unpredictable.
S2, judging the properties of the joint angle discrete point sequences, calculating the difference value of adjacent discrete joint angles, and comparing the difference value with a first preset difference value and a second preset difference value, wherein the second preset difference value is smaller than the first preset difference value; specifically, the first preset difference is 10 °, and the second preset difference is 0.2 °.
If the difference value is larger than a first preset difference value, judging that the joint angle discrete point sequence is unqualified, and returning to the step S1 to redetermine the joint angle discrete point sequence;
if the difference value is smaller than or equal to a second preset difference value, determining that secondary interpolation is not needed, and directly jumping to the step S7;
if the difference value is between the first preset difference value and the second preset difference value, judging that the secondary joint angle interpolation is needed, and continuing to step S3;
and step S3, performing primary speed planning according to a preset primary planning movement step length, and performing primary speed planning on a multi-section path corresponding to the joint angle discrete points through a preset speed calculation formula to obtain the speed at the discrete points, namely the joint angle discrete point sequence speed.
Step S4, performing joint angle secondary interpolation according to the joint angle discrete point sequence speed and a preset secondary planning motion step length to obtain joint angles, joint angle speeds and joint angle accelerations among discrete points, and realizing secondary position planning, speed planning and acceleration planning of paths among the discrete points; wherein the primary planning motion step size is an integer multiple of the secondary planning motion step size. Specifically, the initial preset primary planning motion step length is 2s, and the preset secondary planning motion step length is 20ms.
S5, calculating the corresponding tail end pose, tail end speed and tail end acceleration of the mechanical arm through positive kinematics according to the joint angle calculated in the step S4; step S5 is to calculate Cartesian space tracks, and the calculation is needed by combining DH parameters preset by the mechanical arm, and the specific DH parameter setting and calculating method belongs to the existing general method.
S6, comparing the joint angle, the joint angular velocity and the joint angular acceleration obtained in the step S4 and the velocity and acceleration information of the tail end of the mechanical arm obtained in the step S5 with preset limit values, and judging whether the mechanical arm exceeds the limit;
if any value exceeds the preset limit value, lengthening the primary planning movement step length for 1 time, and jumping to the step S3; preferably, the lengthening amount of the corresponding primary planning motion step is 1s.
If all the values do not exceed the preset limit value, step S7 is performed;
and S7, outputting joint angle planning information. The joint angle planning information corresponds to the joint angle discrete point sequence determined in step S2 that the secondary interpolation is not required or the joint angle parameter after the secondary interpolation is completed in step S4.
The comparison of the difference value of the adjacent discrete joint angles and the first preset difference value is to prevent the difference value of the joint angles of the adjacent discrete points from being too large, and if the difference value is too large, the tail end path of the mechanical arm is possibly inconsistent with the planned path; the comparison of the difference value of the adjacent discrete joint angles and the second preset difference value is to reduce calculation under the condition of ensuring that the motion trail of the tail end of the mechanical arm is not influenced, if the value is not set, each adjacent discrete joint angle carries out secondary interpolation, the interpolation frequency is increased, and the calculated amount is increased. Through the step S6, the planning result can be ensured not to exceed the movement limit of the mechanical arm, and the operation stability and reliability of the mechanical arm system can be ensured. The offset mechanical arm discrete point track smooth planning method can realize the planning and solving of each section under the condition that an upper system only gives the position of each sectional path end point, and in addition, the offset mechanical arm discrete point track smooth planning method carries out multiple return judgment according to the step 2 and the step 6, so that the offset mechanical arm discrete point track smooth planning method can adapt to the randomness of a joint angle discrete sequence. So as to realize smooth planning of the discrete point track of the mechanical arm.
Further, in the method for smoothly planning the track of the discrete point of the offset mechanical arm, in the step S3, the initial and final speeds of the velocity of the sequence of discrete points of the joint angle are zero. As a preferable mode of the present application, the mechanical impact caused by the mechanical arm when the mechanical arm is started and stopped can be prevented.
Further, in the method for smoothly planning the track of the discrete point of the offset mechanical arm, the process of calculating the velocity of the discrete point sequence of the joint angle in the step S3 is as follows: known joint angle discrete point sequence q= [q 1 ,q 2 ,...,q N ]And a first-level planning motion step length t_s1_set, wherein the initial and final speeds of all joints are zero, namely:
dq 1 = [0; 0; 0; 0; 0; 0; 0]，dq N = [0; 0; 0; 0; 0; 0; 0]；
the average velocity expression between discrete points of each joint is:
dq i-1_average =Δq i-1 /Δt= (q i -q i-1 )/ t_s1_set，i = 2,3,…,N；
the average speed between discrete points of each joint is:
dq _average = [dq 1_average , dq 2_average ,..., dq N-1_average ]；
and calculating the joint angle discrete point sequence speed dQ according to a speed calculation function related to the average speed between the discrete points so as to ensure that the mechanical arm moves continuously without abrupt change in a multi-section path after quadratic interpolation planning.
Further, in the method for smoothly planning the track of the discrete points of the offset mechanical arm, in the step S3, the process of calculating the velocity dQ of the sequence of the discrete points of the joint angle through the velocity function related to the average velocity between the discrete points is as follows:
dq 2 = f 1 (dq 1_average ）；
dq N-1 =f 2 (dq N-1_average )；
dq i-1_middle = g（dq i_average ） + h（dq i+1_average ），i = 2,3,…,M；
dq _middle = [dq 1_middle , dq 2_middle ,..., dq M-1_middle ]；
dQ= [dq 1 , dq 2 , dq _middle , dq N-1 ,dq N ]；
where M is the number of groups of average velocities between discrete points of each joint.
dq 2 = f 1 (dq 1_average ) And dq N-1 =f 2 (dq N-1_average ) A first velocity calculation function and a second velocity calculation function, d, respectively, corresponding to the average velocity between the discrete pointsq i-1_middle = g（dq i_average ） + h（dq i+1_average ) A function is calculated for a third speed related to the average speed between the discrete points.
Further, the bias mechanical arm discrete point track smooth planning method,
dq 2 = f 1 (dq 1_average ）=2·dq 1_average ；
dq N-1 =f 2 (dq N-1_average )=2·dq N-1_average ；
dq i-1_middle = = g（dq i_average ） + h（dq i+1_average ）= (dq i_average + dq i+1_average )/2。
in order to ensure continuous and non-abrupt change of the multi-section path motion of the mechanical arm after the quadratic interpolation planning, the speed calculation function corresponding to the functions is a preferable function verified by simulation.
Further, in the method for smoothly planning the track of the discrete point of the offset mechanical arm, the process of secondary interpolation of the joint angle discrete point sequence in the step S4 is as follows:
known joint angle discrete point sequence q= [q 1 ,q 2 ,...,q N ]The first-stage planning motion step length t_s1_set, the second-stage planning motion step length t_s2_set and the joint angle discrete point sequence speed dQ= [ d ] calculated in the step S3q 1 , dq 2 , dq _middle , dq N-1 ,dq N ]；
Acceleration dd at initial angle A and end angle B for each path between discrete points of joint angleq A And ddq B The restraint is carried out and the restraint is carried out,
calculating the coefficients of the fifth degree polynomial: a, a 0_k 、a 1_k 、a 2_k 、a 3_k 、a 4_k 、a 5_k ；
The joint angle of the kth joint at time tq i The method comprises the following steps:
q i_k = a 0_k + a 1_k ·t + a 2_k ·t 2 + a 3_k ·t 3 + a 4_k ·t 4 + a 5_k ·t 5 ；
joint angular velocity d of kth joint at time tq i The method comprises the following steps:
dq i_k = a 1_k + 2·a 2_k ·t+ 3·a 3_k ·t 2 + 4· a 4_k ·t 3 + 5·a 5_k ·t 4 ；
joint angular acceleration dd of kth joint at time tq i The method comprises the following steps:
ddq i_k = 2·a 2_k +6·a 3_k ·t+ 12· a 4_k ·t 2 + 20·a 5_k ·t 3 ；
wherein k represents a kth joint of the corresponding mechanical arm; t starts from 0, takes the second-level planning motion step length t_s2_set as a step length, and finishes traversing from the first-level planning motion step length t_s1_set; where n=1, …, N-1, all path segment interpolation ends when N > N-1.
Further, in the method for smoothly planning the track of the discrete point of the offset mechanical arm, if the step increase number exceeds the preset number limit value in the step S6, the step S1 is skipped to determine the sequence of the discrete point of the joint angle again. As a preferable scheme of the application, the problem that the number of circulation times is excessive or dead circulation is formed due to the problem of the joint angle discrete point sequence is prevented, so that planning is difficult to end.
The mechanical arm system comprises a mechanical arm and a mechanical arm driving module, wherein the mechanical arm driving module drives the mechanical arm to move according to joint angle planning information output by the offset mechanical arm discrete point track smooth planning method.
A computer readable storage medium storing a computer program which when executed by a processor performs the steps of the offset robotic arm discrete point trajectory smoothing planning method.
Compared with the prior art, the application has the advantages that:
(1) The offset mechanical arm discrete point track smooth planning method can realize the planning and solving of each section under the condition that an upper system only gives the position of each sectional path end point, and in addition, the offset mechanical arm discrete point track smooth planning method carries out multiple return judgment according to the step 2 and the step 6, so that the offset mechanical arm discrete point track smooth planning method can adapt to the randomness of a joint angle discrete sequence. So as to realize smooth planning of the discrete point track of the mechanical arm.
(2) The offset mechanical arm discrete point track smooth planning method can ensure continuous and non-abrupt multi-section path movement of the mechanical arm after quadratic interpolation planning.
Drawings
FIG. 1 is a flowchart of a method for smoothly planning a discrete point track of an offset manipulator according to an embodiment of the present application;
FIG. 2 is a table of DH parameters of a robotic arm according to an embodiment of the present application;
FIG. 3 is a graph of a sequence of discrete points of joint angles versus a planned joint trajectory provided by the present application;
FIG. 4 is a graph of a planned joint angle provided by an embodiment of the present application;
FIG. 5 is a graph of planned joint angular velocity provided by an embodiment of the present application;
FIG. 6 is a graph of planned joint angular acceleration provided by an embodiment of the present application;
FIG. 7 is a flow chart of a method of computing a Cartesian space trajectory in accordance with an embodiment of the present application;
FIG. 8 is a graph of planned end positions provided by an embodiment of the present application;
FIG. 9 is a graph of planned end linear velocity provided by an embodiment of the present application;
FIG. 10 is a graph of planned terminal linear acceleration provided by an embodiment of the present application;
FIG. 11 is a planned end pose graph provided by an embodiment of the present application;
FIG. 12 is a graph of planned tip angular velocity provided by an embodiment of the present application;
FIG. 13 is a graph of planned tip angular acceleration provided by an embodiment of the present application.
Detailed Description
The application is described in further detail below with reference to the drawings and the specific embodiments.
The application can be used in a narrow multi-obstacle space operation scene, and can solve the problem that the mechanical arm is damaged or the environment is damaged due to interference between the mechanical arm and the environment in the movement process due to overlong single planning path in the scene.
Through path division, the movement range of the mechanical arm is limited to avoid interference with the environment.
Example 1
A smooth planning method for a discrete point track of an offset mechanical arm, as shown in fig. 1, comprises the following steps:
and S1, reading and determining a joint angle discrete point sequence generated by a superior system.
The upper system generates a joint angle discrete point sequence corresponding to each joint angle according to the operation task of the mechanical arm: q= [q 1 ,q 2 ,...,q N ]. In this embodiment, if the mechanical arm is a 7-degree-of-freedom mechanical arm, Q corresponds to a 7×n matrix.
Setting the motion parameters of the mechanical arm, including: mechanical arm DH parameter, primary planning motion step length, secondary planning motion step length and limit value.
The parameters of the mechanical arm DH are determined according to the initial configuration of the mechanical arm, as shown in FIG. 2. The length of the connecting rod represents the length of a public perpendicular of the rotation axes of two adjacent joints, and the torsion angle of the shaft represents the included angle of the rotation axes of the two adjacent joints, which belong to the parameters of the connecting rod. The distance between the connecting rods and the included angle between the connecting rods belong to the parameters of the adjacent connecting rods, and the distance and the included angle between the two adjacent equivalent straight rods are respectively represented. As can be seen from the parameters of fig. 2, in this embodiment, the robot arm is a 7-degree-of-freedom elbow joint offset robot arm.
It should be noted that, the DH parameters of the mechanical arm in the zero position are shown in FIG. 2, and when each joint of the mechanical arm moves, the included angle of the connecting rod changes with the DH parameters, and the value of the DH parameters is zero position included angle ([ 0;90; 0)]) With joint angle ([ q ] i_1 ; q i_2 ; q i_3 ; q i_4 ; q i_5; q i_6 ; q i_7 ]) And (3) summing.
Setting a primary planning motion step length t_s1_set; setting a secondary planning motion step length t_s2_set; in this embodiment, t_s1_set=2s, t_s2_set=20 ms.
Wherein the limit value includes: joint movement limit and end movement limit; the articulation limit comprises: joint angle limit valueq lim Limit value d of angular velocityq lim Angular acceleration limit ddq lim The method comprises the steps of carrying out a first treatment on the surface of the The tip movement limit value includes: speed limit valuev lim Acceleration limita lim The speed limit value includes a linear speed limit value and an angular speed limit value, and the acceleration limit value includes a linear acceleration limit value and an angular acceleration limit value.
S2, judging the properties of the joint angle discrete point sequences, calculating the difference value of adjacent discrete joint angles, and comparing the difference value with a first preset difference value and a second preset difference value, wherein the second preset difference value is smaller than the first preset difference value; if the difference value is larger than a first preset difference value, judging that the joint angle discrete point sequence is unqualified, and returning to the step S1 to redetermine the joint angle discrete point sequence;
if the difference value is smaller than or equal to a second preset difference value, determining that secondary interpolation is not needed, and directly jumping to the step S7; it should be noted that, at this time, the step length corresponding to the joint angle discrete point sequence is a second-level planning motion step length.
If the difference is between the first preset difference and the second preset difference, determining that the secondary joint angle interpolation is needed, and continuing to step S3.
Specifically, in this embodiment, the step S2 includes the following two steps:
step S201, judging the eligibility of the joint angle discrete point sequence, specifically, calculating the difference value of adjacent discrete joint angles, if the difference value is larger than a first preset difference value, judging that the joint angle discrete point sequence is unqualified, returning to the step S1, and re-reading and determining the joint angle discrete point sequence; if the difference is less than or equal to the first preset difference, step S202 is continued. In this embodiment, the first preset difference is 10 °.
The expression of the adjacent joint angle difference value is:
Δq n-1 =q n -q n-1 , n=2,3,...,N；
Δq=[Δq 1 ,Δq 2 ,...,Δq N-1 ]；
and step S202, judging whether to perform secondary joint angle interpolation according to the qualified joint angle discrete point sequence judged in the step S2.
Specifically, if the difference value of the adjacent discrete joint angles is smaller than or equal to a second preset difference value, judging that secondary interpolation is not needed, and directly outputting a joint angle discrete point sequence; if the difference is greater than the second preset difference, the step S3 is continued. In this embodiment, the second preset difference is 0.2 °.
And step S3, according to the joint angle discrete point sequence which is judged to need secondary interpolation in the step S3, calculating the speed of the joint angle discrete point sequence to obtain the speed of the discrete points, namely, carrying out primary speed planning on the corresponding multi-section paths among the joint angle discrete points to obtain the initial speed and the final speed of a group of multi-section paths.
And (3) calculating the velocity of the joint angle discrete point sequence in the step S3:
according to the joint angle discrete point sequence Q= [q 1 ,q 2 ,...,q N ]And the first-level planning motion step length t_s1_set calculates the joint angle discrete point sequence speed.
The joint angle discrete point sequence contains a joint angle group number N, and N is expressed by the following function in Matlab:
N =size(Q, 2)；
setting the initial and final speeds of each joint to zero, namely:
dq 1 = [0; 0; 0; 0; 0; 0; 0]；
dq N = [0; 0; 0; 0; 0; 0; 0]；
the average velocity expression between discrete points of each joint is:
dq i-1_average =Δq i-1 /Δt= (q i -q i-1 )/ t_s1_set，i = 2,3,…,N；
the average speed between discrete points of each joint is:
dq _average = [dq 1_average , dq 2_average ,..., dq N-1_average ]；
and calculating the joint angle discrete point sequence speed dQ according to a speed calculation function related to the average speed between the discrete points so as to ensure that the mechanical arm moves continuously without abrupt change in a multi-section path after quadratic interpolation planning.
In this embodiment, the process of calculating the velocity dQ of the joint angle discrete point sequence by the velocity function related to the average velocity between the discrete points is:
the process of calculating the joint angle discrete point sequence speed dQ through a speed function related to the average speed among the discrete points is as follows:
dq 2 = f 1 (dq 1_average ）；
dq N-1 =f 2 (dq N-1_average )；
dq i-1_middle = g（dq i_average ） + h（dq i+1_average ），i = 2,3,…,M；
dq _middle = [dq 1_middle , dq 2_middle ,..., dq M-1_middle ]；
dQ= [dq 1 , dq 2 , dq _middle , dq N-1 ,dq N ]；
wherein dq 2 = f 1 (dq 1_average ) And dq N-1 =f 2 (dq N-1_average ) A first velocity calculation function and a second velocity calculation function, d, respectively, corresponding to the average velocity between the discrete pointsq i-1_middle = g（dq i_average ） + h（dq i+1_average ) A function is calculated for a third speed related to the average speed between the discrete points. M is the number of groups of average velocities between discrete points of each joint. M in Matlab is represented by the following function:
M=size(dq _average , 2)-2；
further:
dq 2 = f 1 (dq 1_average ）=2·dq 1_average ；
dq N-1 =f 2 (dq N-1_average )=2·dq N-1_average ；
dq i-1_middle = = g（dq i_average ） + h（dq i+1_average ）= (dq i_average + dq i+1_average )/2。
in order to ensure continuous and non-abrupt change of the multi-section path motion of the mechanical arm after the quadratic interpolation planning, the speed calculation function corresponding to the functions is a preferable function verified by simulation.
And S4, performing joint angle secondary interpolation according to the joint angle discrete point sequence speed calculated in the step S3 to obtain joint angles, joint angle speeds and joint angle accelerations among the discrete points, and performing secondary position planning, speed planning and acceleration planning on the multi-section path.
Step S4, namely a joint angle discrete point sequence secondary interpolation process:
according to the joint angle discrete point sequence Q= [q 1 ,q 2 ,...,q N ]The first-stage planning motion step length t_s1_set, the second-stage planning motion step length t_s2_set and the joint angle discrete point sequence speed dQ= [ d ] calculated in the step S3q 1 , dq 2 , dq _middle , dq N-1 ,dq N ]。
In this embodiment, the acceleration constraint at the initial angle a and the final angle B of each path between the discrete points of the joint angles is zero, that is:
ddq A =0，ddq B =0；
q A =q n ，q B =q n+1 ；
dq A = dq n ，dq B = dq n+1 ；
calculating the coefficients of the fifth degree polynomial:
a 0_k =q A_k ；
a 1_k =dq A_k ；
a 2_k =ddq A_k /2；
a 3_k =[ 20·(q B_k -q A_k )-(8·dq B_k + 12·dq A_k )·t_ AB +(ddq B_k -3·ddq A_k )·t_ AB 2 ] / (2·t_ AB 3 )；
a 4_k =[ -30·(q B_k -q A_k ) +(14·dq B_k + 16·dq A_k )·t_ AB -(2·ddq B_k -3·ddq A_k )·t_ AB 2 ]/ (2·t_ AB 4 )；
a 5_k =[ 12·(q B_k -q A_k )-(6·dq B_k + 6·dq A_k )·t_ AB +(ddq B_k -ddq A_k )·t_ AB 2 ]/ (2·t_ AB 5 )；
a 0 =[a 0_1 ; a 0_2 ; a 0_3 ; a 0_4 ; a 0_5 ; a 0_6 ; a 0_7 ]；
a 1 =[a 1_1 ; a 1_2 ; a 1_3 ; a 1_4 ; a 1_5 ; a 1_6 ; a 1_7 ]；
a 2 =[a 2_1 ; a 2_2 ; a 2_3 ; a 2_4 ; a 2_5 ; a 2_6 ; a 2_7 ]；
a 3 =[a 3_1 ; a 3_2 ; a 3_3 ; a 3_4 ; a 3_5 ; a 3_6 ; a 3_7 ]；
a 4 =[a 4_1 ; a 4_2 ; a 4_3 ; a 4_4 ; a 4_5 ; a 4_6 ; a 4_7 ]；
a 5 =[a 5_1 ; a 5_2 ; a 5_3 ; a 5_4 ; a 5_5 ; a 5_6 ; a 5_7 ]；
calculating the joint angle of the kth joint at the time tq i ：
q i_k = a 0_k + a 1_k ·t + a 2_k ·t 2 + a 3_k ·t 3 + a 4_k ·t 4 + a 5_k ·t 5 ；
Calculating the joint angular velocity d of the kth joint at the time tq i ：
dq i_k = a 1_k + 2·a 2_k ·t+ 3·a 3_k ·t 2 + 4· a 4_k ·t 3 + 5·a 5_k ·t 4 ；
Calculating the joint angular acceleration dd of the kth joint at the time tq i ：
ddq i_k = 2·a 2_k +6·a 3_k ·t+ 12· a 4_k ·t 2 + 20·a 5_k ·t 3 ；
Where k represents each joint k=1, 2, …,7.t starts from 0 and takes t_s2_set as a step size to the end of the t_s1_set traversal, i.e. the end of the loop traversal when i > (t_s1_set/t_s2_set) +1.
Where n=1, …, N-1. When N > N-1, all path segment interpolation ends.
The joint planning track shown in fig. 3, and the planned joint angle, joint angular velocity and joint angular acceleration graphs shown in fig. 4 to 6 are obtained through the above-described quadratic interpolation process. In fig. 3, the open dots represent the discrete joint angle points, and the broken lines represent the joint angle trajectories after the quadratic interpolation.
And S5, calculating corresponding terminal pose, terminal speed and terminal acceleration through positive kinematics according to the joint angle calculated in the step S4.
Step S5 is a Cartesian space trajectory calculation process, and a specific calculation flow is shown in fig. 7 of the specification.
The end trajectory plan shown in fig. 8 to 13 is calculated by the above-described procedure.
Fig. 8 to 10 are the arm end position, the end linear velocity, and the end linear acceleration, and fig. 11 to 13 are the arm end posture, the end angular velocity, and the end angular acceleration.
And S6, comparing the planned joint angle, the planned joint angular velocity and the planned joint angular acceleration obtained in the step S4 with the velocity and the acceleration information of the tail end of the mechanical arm obtained in the step S5 with the joint movement limit value and the tail end movement limit value of the step S1, and judging whether the planned joint angle, the planned joint angular velocity and the planned joint angular acceleration exceed the limit value. If the limit value of the joint movement or the limit value of the terminal movement is exceeded, the step length of the primary planning movement is lengthened by 1 time, and then the step S3 is skipped. In this example, the length of one time was 1s. The motion of the mechanical arm is slowed down by lengthening the primary planning motion step length, so that the condition that the motion parameters of the mechanical arm are out of limit to cause faults or damage is prevented.
When the lengthening times of the primary planning movement step exceeds the preset times, returning to the step S1 to redetermine the joint angle discrete point sequence; if the limit value is not exceeded, the planning is ended. In this embodiment, the preset number of times is 10, so as to prevent the problem of excessive circulation times or dead circulation caused by the problem of the joint angle discrete point sequence, and the planning is difficult to end.
And 7, outputting joint angle planning information, and forwarding the joint angle planning information to the offset mechanical arm to drive the mechanical arm to move so as to realize the movement visualization of the mechanical arm. Specifically, according to joint angle planning information, a bias mechanical arm simplified 3D model is built based on a Simulink platform, and movement process visualization is achieved.
Example 2
The mechanical arm system comprises a mechanical arm and a mechanical arm driving module, wherein the mechanical arm driving module drives the mechanical arm to move according to joint angle planning information output by the offset mechanical arm discrete point track smooth planning method in the embodiment 1.
Example 3
A computer readable storage medium storing a computer program which when executed by a processor performs the steps of the offset robotic arm discrete point trajectory smoothing planning method of embodiment 1.

Claims (9)

1. A smooth planning method for discrete point tracks of an offset mechanical arm comprises the following steps:
s1, reading and determining a joint angle discrete point sequence;
s2, judging the properties of the joint angle discrete point sequences, calculating the difference value of adjacent discrete joint angles, and comparing the difference value with a first preset difference value and a second preset difference value, wherein the second preset difference value is smaller than the first preset difference value;
if the difference value is larger than a first preset difference value, judging that the joint angle discrete point sequence is unqualified, and returning to the step S1 to redetermine the joint angle discrete point sequence;
if the difference value is smaller than or equal to a second preset difference value, determining that secondary interpolation is not needed, and directly jumping to the step S7;
if the difference value is between the first preset difference value and the second preset difference value, judging that the secondary joint angle interpolation is needed, and continuing to step S3;
step S3, performing primary speed planning according to a preset primary planning movement step length, and performing primary speed planning on a multi-section path corresponding to the joint angle discrete points through a preset speed calculation formula to obtain the speed at the discrete points, namely the joint angle discrete point sequence speed;
step S4, performing joint angle secondary interpolation according to the joint angle discrete point sequence speed and a preset secondary planning motion step length to obtain joint angles, joint angle speeds and joint angle accelerations among discrete points, and realizing secondary position planning, speed planning and acceleration planning of paths among the discrete points; wherein the primary planning motion step length is an integer multiple of the secondary planning motion step length;
s5, calculating the corresponding tail end pose, tail end speed and tail end acceleration of the mechanical arm through positive kinematics according to the joint angle calculated in the step S4;
s6, comparing the joint angle, the joint angular velocity and the joint angular acceleration obtained in the step S4 and the velocity and acceleration information of the tail end of the mechanical arm obtained in the step S5 with preset limit values, and judging whether the mechanical arm exceeds the limit;
if any value exceeds the preset limit value, lengthening the primary planning movement step length for 1 time, and jumping to the step S3; if all the values do not exceed the preset limit value, step S7 is performed;
and S7, outputting joint angle planning information.
2. The offset robotic arm discrete point trajectory smoothing programming method of claim 1, wherein: in the step S3, the initial and final speeds of the joint angle discrete point sequence speed are zero.
3. The offset robotic arm discrete of claim 2The point track smooth planning method is characterized in that: the process of calculating the joint angle discrete point sequence speed in the step S3 is as follows: known joint angle discrete point sequence q= [q 1 , q 2 ,..., q N ]And a first-level planning motion step length t_s1_set, wherein the initial and final speeds of all joints are zero, namely: d, dq 1 = [0; 0; 0; 0; 0; 0; 0]，dq N = [0; 0; 0; 0; 0; 0; 0]；
The average velocity expression between discrete points of each joint is:
dq i-1_average =Δq i-1 /Δt= (q i - q i-1 )/ t_s1_set，i = 2,3,…,N；
the average speed between discrete points of each joint is:
dq _average = [dq 1_average , dq 2_average ,..., dq N-1_average ]；
and calculating the joint angle discrete point sequence speed dQ according to a speed calculation function related to the average speed between the discrete points so as to ensure that the mechanical arm moves continuously without abrupt change in a multi-section path after quadratic interpolation planning.
4. The offset robotic arm discrete point trajectory smoothing programming method of claim 3, wherein: in the step S3, the process of calculating the velocity dQ of the joint angle discrete point sequence according to the velocity function related to the average velocity between the discrete points is as follows:
dq 2 = f 1 (dq 1_average ）；
dq N-1 =f 2 (dq N-1_average )；
dq i-1_middle =g（dq i_average ） + h（dq i+1_average ），i = 2,3,…,M；
dq _middle = [dq 1_middle , dq 2_middle ,..., dq M-1_middle ]；
dQ= [dq 1 , dq 2 , dq _middle , dq N-1 , dq N ]；
where M is the number of groups of average velocities between discrete points of each joint.
5. The method for smoothly planning the discrete point track of the offset manipulator according to claim 4, wherein the method comprises the following steps of:
dq 2 = f 1 (dq 1_average ）=2·dq 1_average ；
dq N-1 =f 2 (dq N-1_average )=2·dq N-1_average ；
dq i-1_middle = =g（dq i_average ） + h（dq i+1_average ）= (dq i_average + dq i+1_average )/2。
6. the offset robotic arm discrete point trajectory smoothing programming method of claim 1, wherein: the process of secondary interpolation of the joint angle discrete point sequence in the step S4 is as follows:
known joint angle discrete point sequence q= [q 1 , q 2 ,..., q N ]The first-stage planning motion step length t_s1_set, the second-stage planning motion step length t_s2_set and the joint angle discrete point sequence speed dQ= [ d ] calculated in the step S3q 1 , dq 2 , dq _middle , dq N-1 ,dq N ]；
Acceleration dd at initial angle A and end angle B for each path between discrete points of joint angleq A And ddq B The restraint is carried out and the restraint is carried out,
calculating the coefficients of the fifth degree polynomial: a, a 0_k 、a 1_k 、a 2_k 、a 3_k 、a 4_k 、a 5_k ；
The joint angle of the kth joint at time tq i The method comprises the following steps: q i_k = a 0_k + a 1_k ·t + a 2_k ·t 2 + a 3_k ·t 3 + a 4_k ·t 4 + a 5_k ·t 5 ；
Joint angular velocity d of kth joint at time tq i The method comprises the following steps: dq is (q) i_k = a 1_k + 2·a 2_k ·t+ 3·a 3_k ·t 2 + 4· a 4_k ·t 3 + 5·a 5_k ·t 4 ；
Joint angular acceleration dd of kth joint at time tq i The method comprises the following steps: ddq i_k = 2·a 2_k +6·a 3_k ·t+ 12· a 4_k ·t 2 + 20·a 5_k ·t 3 ；
Wherein k represents a kth joint of the corresponding mechanical arm; t starts from 0, takes the second-level planning motion step length t_s2_set as a step length, and finishes traversing from the first-level planning motion step length t_s1_set; where n=1, …, N-1, all path segment interpolation ends when N > N-1.
7. The offset robotic arm discrete point trajectory smoothing programming method of claim 1, wherein: and if the step increasing times exceeds the preset times limit value in the step S6, jumping to the step S1 to redetermine the joint angle discrete point sequence.
8. A robotic arm system, characterized by: the method comprises a mechanical arm and a mechanical arm driving module, wherein the mechanical arm driving module drives the mechanical arm to move according to joint angle planning information output by the offset mechanical arm discrete point track smooth planning method according to any one of claims 1 to 7.
9. A computer-readable storage medium storing a computer program, characterized by: the computer program, when executed by a processor, implements the steps of the offset robotic arm discrete point trajectory smoothing planning method of any one of claims 1 to 7.
CN202311083054.4A 2023-08-28 2023-08-28 Offset mechanical arm discrete point track smooth planning method, system and storage medium Active CN116810802B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311083054.4A CN116810802B (en) 2023-08-28 2023-08-28 Offset mechanical arm discrete point track smooth planning method, system and storage medium

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311083054.4A CN116810802B (en) 2023-08-28 2023-08-28 Offset mechanical arm discrete point track smooth planning method, system and storage medium

Publications (2)

Publication Number Publication Date
CN116810802A true CN116810802A (en) 2023-09-29
CN116810802B CN116810802B (en) 2024-01-26

Family

ID=88114801

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311083054.4A Active CN116810802B (en) 2023-08-28 2023-08-28 Offset mechanical arm discrete point track smooth planning method, system and storage medium

Country Status (1)

Country Link
CN (1) CN116810802B (en)

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107139173A (en) * 2017-06-15 2017-09-08 华南理工大学 A kind of industrial robot gate locus interpolation method
CN109343345A (en) * 2018-09-28 2019-02-15 江南大学 Mechanical arm polynomial interopolation method for planning track based on QPSO algorithm
WO2020127714A1 (en) * 2018-12-21 2020-06-25 Franka Emika Gmbh Regulating device for desired value compensation
CN112692826A (en) * 2020-12-08 2021-04-23 佛山科学技术学院 Industrial robot track optimization method based on improved genetic algorithm
CN115741718A (en) * 2022-12-07 2023-03-07 江西省智能产业技术创新研究院 Robot complete zero force control method and system

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107139173A (en) * 2017-06-15 2017-09-08 华南理工大学 A kind of industrial robot gate locus interpolation method
CN109343345A (en) * 2018-09-28 2019-02-15 江南大学 Mechanical arm polynomial interopolation method for planning track based on QPSO algorithm
WO2020127714A1 (en) * 2018-12-21 2020-06-25 Franka Emika Gmbh Regulating device for desired value compensation
CN112692826A (en) * 2020-12-08 2021-04-23 佛山科学技术学院 Industrial robot track optimization method based on improved genetic algorithm
CN115741718A (en) * 2022-12-07 2023-03-07 江西省智能产业技术创新研究院 Robot complete zero force control method and system

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
张一巍, 张永民, 黄元庆: "用高阶多项式插值解决机器人运动轨迹规划中的约束问题", 机器人技术与应用, no. 02, pages 19 - 21 *

Also Published As

Publication number Publication date
CN116810802B (en) 2024-01-26

Similar Documents

Publication Publication Date Title
CN106393106B (en) The robot nurbs curve of parameter adaptive densification moves interpolating method
CN101402199B (en) Hand-eye type robot movable target extracting method with low servo accuracy based on visual sensation
CN109343345B (en) Mechanical arm polynomial interpolation track planning method based on QPSO algorithm
Guan et al. On robotic trajectory planning using polynomial interpolations
CN114952868B (en) 7-degree-of-freedom SRS (sounding reference Signal) type mechanical arm control method and device and piano playing robot
CN112757306A (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN114670191B (en) Seven-degree-of-freedom elbow offset mechanical arm track planning method
CN110216673B (en) Non-dominated neighborhood immune genetic multi-target optimization method for joint track of electro-hydraulic robot
CN114603558B (en) Hybrid space transition track planning method and device
CN109683615B (en) Speed look-ahead method for continuous motion path of robot and robot controller
CN113296407A (en) Multi-machine cooperative track optimization method based on 5-time non-uniform rational B-spline
CN110561438A (en) Industrial robot manpower/position compliance control method based on kinetic parameter identification
CN107538493A (en) A kind of industrial robot method for planning track for avoiding cable interference constraint
Tangpattanakul et al. Minimum-time trajectory of robot manipulator using Harmony Search algorithm
CN111283683B (en) Servo tracking accelerated convergence method for robot visual feature planning track
Cao et al. Constrained time-efficient and smooth cubic spline trajectory generation for industrial robots
JP4667764B2 (en) Route setting method
CN116810802B (en) Offset mechanical arm discrete point track smooth planning method, system and storage medium
CN115476356A (en) Self-adaptive hybrid impedance control method of space manipulator without force sensor
CN114700938A (en) Redundant mechanical arm motion planning method based on jump gain integral neural network
Tangpattanakul et al. Optimal trajectory of robot manipulator using harmony search algorithms
CN107398903A (en) The method for controlling trajectory of industrial machinery arm actuating station
CN116117796B (en) Industrial robot gesture track transition and speed planning method and system
CN116512245A (en) Track optimization method and device for inhibiting residual vibration of flexible joint mechanical arm
CN116872197A (en) Adaptive neural network inversion control method and control system for single-rod mechanical arm

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
GR01 Patent grant
GR01 Patent grant