CN112757306B - Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm - Google Patents

Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm Download PDF

Info

Publication number
CN112757306B
CN112757306B CN202110109515.5A CN202110109515A CN112757306B CN 112757306 B CN112757306 B CN 112757306B CN 202110109515 A CN202110109515 A CN 202110109515A CN 112757306 B CN112757306 B CN 112757306B
Authority
CN
China
Prior art keywords
solution
planning
inverse
algorithm
mechanical arm
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.)
Active
Application number
CN202110109515.5A
Other languages
Chinese (zh)
Other versions
CN112757306A (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.)
Beijing Jiaotong University
Original Assignee
Beijing Jiaotong University
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 Beijing Jiaotong University filed Critical Beijing Jiaotong University
Priority to CN202110109515.5A priority Critical patent/CN112757306B/en
Publication of CN112757306A publication Critical patent/CN112757306A/en
Application granted granted Critical
Publication of CN112757306B publication Critical patent/CN112757306B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1612—Programme controls characterised by the hand, wrist, grip control

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Health & Medical Sciences (AREA)
  • General Health & Medical Sciences (AREA)
  • Orthopedic Medicine & Surgery (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

The invention relates to the field of mechanical arms of foot type robots, in particular to an inverse solution multi-solution selection and time optimal trajectory planning algorithm for a mechanical arm. The algorithm can rapidly select a group of most appropriate solutions from multiple inverse solutions according to the pose of the current mechanical arm, effectively avoid singular position types, realize smooth switching among different inverse solutions, and ensure the continuity and no oscillation of mechanical arm control and action; and a trajectory planning method combining the seventh polynomial and the conditional proportional control is utilized to plan a trajectory with optimal time, so that smooth and continuous guidance of joint angles, angular velocities, angular accelerations and angular jerks is ensured, multi-target and multi-constraint conditions are met, the actuator specification is met, and the maximum constraint of the joint angular velocities or the maximum constraint of the joint angular accelerations is included. The algorithm greatly reduces the calculation amount of multi-solution selection and trajectory planning, can realize real-time and efficient time optimal trajectory planning, and can be popularized and applied to low-calculation force controllers.

Description

Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
Technical Field
The invention relates to the field of mechanical arms of foot type robots, in particular to an inverse solution multi-solution selection and time optimal trajectory planning algorithm for a mechanical arm.
Background
The mechanical arm is a complex system with high precision, multiple inputs and outputs, high nonlinearity and strong coupling. Because of its unique operational flexibility, it has been widely used in the fields of industrial assembly, safety and explosion protection. The mechanical arm is a complex system, and uncertainty such as parameter perturbation, external interference, unmodeled dynamic state and the like exists, so that uncertainty also exists in a modeling model of the mechanical arm. The mathematical model of the robot arm includes kinematics and dynamics, both of which include a positive solution and an inverse solution. Generally, multiple solutions exist in the inverse solution of the motion of the multi-degree-of-freedom mechanical arm, and for different tasks, the cartesian space or joint space trajectory of the mechanical arm needs to be planned and used as a target given signal of the mechanical arm for control. How to select the most appropriate solution group from the inverse solution multi-solutions and plan the optimal time trajectory in a concise and efficient manner is very much concerned in both theoretical research and practical application. According to the inverse solution multi-solution selection and time optimal trajectory planning algorithm for the mechanical arm, the most appropriate one group of solutions can be rapidly selected from the inverse solution multi-solutions according to the current pose of the mechanical arm, the singular position type is effectively avoided, smooth switching among different inverse solutions is realized, and the continuity and non-oscillation of mechanical arm control and action are ensured; and a trajectory planning method combining the seventh polynomial and the conditional proportional control is utilized to plan a trajectory with optimal time, so that smooth and continuous guidance of joint angles, angular velocities, angular accelerations and angular jerks is ensured, multi-target and multi-constraint conditions are met, the actuator specification is met, and the maximum constraint of the joint angular velocities or the maximum constraint of the joint angular accelerations is included. The algorithm greatly reduces the calculation amount of multi-solution selection and trajectory planning, can realize real-time and efficient time optimal trajectory planning, and can be popularized and applied to low-calculation force controllers.
Disclosure of Invention
The invention aims to provide an inverse solution multi-solution selection and time optimal trajectory planning algorithm for a mechanical arm, which is simple and efficient in algorithm architecture and has the capability of quickly selecting the most appropriate group of solutions from the inverse solution multi-solutions and planning the time optimal trajectory.
The technical scheme adopted by the invention for solving the technical problems is as follows:
the mechanical arm inverse solution multi-solution selection and time optimal trajectory planning algorithm is characterized in that: on the basis of a kinematic forward solution and a kinematic inverse solution, the overall algorithm framework of the method is composed of: the method comprises an inverse solution multi-solution selection algorithm and a time optimal trajectory planning algorithm. The general idea of solving the multi-solution selection algorithm is as follows: and performing forward-inverse solution or inverse-forward solution conversion according to the current pose of the mechanical arm, and taking the inverse solution with the minimum norm of the difference between the conversion result and the current pose of the mechanical arm as the most appropriate solution. The overall idea of the time optimal trajectory planning algorithm is as follows: and (3) carrying out track planning by using a seventh polynomial track planning method, calculating values corresponding to all constraint conditions under the track, verifying whether the values are equal to constraint condition thresholds or not, if not, taking the difference value between the values and the corresponding constraint condition thresholds to carry out proportional control and negative feedback to the original track planning time so as to reduce the difference value, and simultaneously, distinguishing and determining whether the negative feedback is carried out on different constraint conditions or not by using activation conditions.
Taking the point-to-point trajectory planning of a five-degree-of-freedom mechanical arm in a Cartesian space as an example, the algorithm comprises the following four steps:
the first step is as follows: a kinematic positive solution.
Firstly, establishing a mechanical arm base coordinate system {0}, each joint coordinate system {1,2.3.4} and a terminal coordinate system {5}, and establishing a mechanical arm D-H parameter table according to mechanical arm structure parameters;
TABLE 1.1D-H PARAMETER TABLE FOR MECHANICAL ARM
Figure GDA0003393390430000021
Secondly, obtaining a homogeneous transformation matrix among the coordinates according to the established coordinate system and the D-H parameter table of the mechanical arm; from the coordinate system oi-1-xi-1yi-1zi-1To the coordinate system oi-xiyiziIs transformed into
Figure GDA0003393390430000031
Thirdly, according to the homogeneous transformation matrix between the base coordinate system and the tail end coordinate system of the mechanical arm
Figure GDA0003393390430000032
The position of the robot arm tip relative to the base coordinate system and the pose of the robot arm tip relative to the base coordinate system are obtained, i.e., the kinematic positive solution p of the robot arm is Forward _ kinematics (θ), where p is [ x y z α β γ ═ y]For a known end pose of the arm, θ ═ θ1 θ2 θ3 θ4 θ5]The calculated joint angles of the robot.
The second step is that: inverse kinematics solution.
(i) can be obtained from the formula (2)
Figure GDA0003393390430000033
The first column and the fourth column of equation (2) are made equal, so that six equations can be obtained;
based on the six equations obtained above, the equation can be solved in turn to obtain theta1Two solutions of, each theta1Can determine theta5Two solutions of (each group of (theta)1,θ5) Can determine theta3Two solutions of (a) and then finding theta1A solution of (a) and theta4A solution of (a); summarizing, a total of eight solutions, i.e., Inverse kinematics solution θ of the mechanical arm — Inverse of Inverse kinematics (i, p), (i ∈ [1,8]), can be obtained])。
The third step: trajectory planning based on a seventh order polynomial.
In order to ensure that the acceleration is continuous and avoid the shock caused by sudden acceleration when the mechanical arm is started and stopped, a seventh-order polynomial can be adopted for optimization, and the optimization aim is to ensure that the acceleration is continuously derivable, namely the acceleration of a planning starting point and a terminal point of a constraint track is large. The specific mechanical arm tail end track planning formula is
Figure GDA0003393390430000041
Where p (t) is the known end pose of the robot arm p ═ x y z α β γ]As a function of the trajectory of the variation with respect to time t,
Figure GDA0003393390430000042
first, second and third derivatives of p (t), respectively.
Giving constraint conditions, namely the positions, the speeds, the accelerations and the jerks of the starting point and the ending point respectively as follows:
Figure GDA0003393390430000043
substituting formula (5) for formula (4) to obtain the coefficients of a seventh-order polynomial
Figure GDA0003393390430000044
Namely, the seven-degree polynomial track planning which aims at acceleration continuous guidance is completed.
The fourth step: and optimizing the track.
The inverse solution multi-solution selection algorithm: first, a set of solutions i ═ 1 is sequentially selected from eight sets of solutions of the inverse arm solution, (i ∈ [1,8], and]) (ii) a Then, reading the current joint angle theta of the mechanical arm ═ theta1 θ2 θ3 θ4 θ5]Carrying out forward solution according to the current pose of the mechanical arm to obtain the current end pose, and carrying out inverse solution according to the current end pose to obtain each joint angle theta under the i-th group of inverse solutionsi＝[θi1 θi2 θi3 θi4 θi5]Finally compare | θi-θ|≤εθIn which epsilonθFor self-defining an angle threshold value, the method is used for distinguishing the most appropriate solution from eight sets of inverse solutions, if thetai-θ|≤εθIf the solution is satisfied, the solution is the optimal solution; if not, another group of inverse solutions is taken for recalculation and comparison;
time optimal trajectory optimization algorithm based on condition proportional control
The algorithm can be divided into two cases:
case 1: the angular velocity of the planned trajectory obtained in the third step
Figure GDA0003393390430000051
And angular acceleration
Figure GDA0003393390430000052
Although all meet the actuator specification, i.e. the maximum angular velocity at which the joint can execute
Figure GDA0003393390430000053
And angular acceleration
Figure GDA0003393390430000054
But with a planning time T equal to Tf-t0Longer, not optimal, where t0,tfRespectively a passing start point and an end pointThe time of the spot. At this point, the planning time needs to be reduced to achieve time optimization while meeting actuator specifications.
Case 2: the angular velocity of the planned trajectory obtained in the third step
Figure GDA0003393390430000055
And angular acceleration
Figure GDA0003393390430000056
Albeit greater than the actuator specification, i.e. the maximum angular velocity at which the joint can execute
Figure GDA0003393390430000057
And angular acceleration
Figure GDA0003393390430000058
The simplest solution at this time is to increase the planning time to achieve time optimization while meeting the actuator specifications.
According to the two situations, the time controller based on the condition proportional control can be designed without iteration, is simple and easy to realize and combines the proportional-integral-derivative (PID) control principle:
Figure GDA0003393390430000059
wherein
Figure GDA00033933904300000510
The adjusting parameters are respectively the error adjusting parameters of the speed and the acceleration in the time optimal controller, and are similar to the proportional parameters in the PID control principle. Whether the proportional control in equation (7) is activated depends on whether its corresponding actuator specification is satisfied, i.e., the origin of the conditional proportional control. In particular, it is possible to use, for example,
Figure GDA0003393390430000061
and
Figure GDA0003393390430000062
can be composed of each joint or eachThe axes are defined individually. Under ideal conditions, the planning time T is T ═ Tf-t0When the optimal angular velocity and angular acceleration of the planned trajectory are equal to the maximum angular velocity and angular acceleration allowed by the actuator specifications, i.e. the angular velocity and angular acceleration of the planned trajectory are equal to the maximum angular velocity and angular acceleration allowed by the actuator specifications
Figure GDA0003393390430000063
In addition, according to the requirements of multiple constraints and multiple targets, the maximum jerk term and other kinematic and dynamic constraint terms are required to be added into the right formula (7). In order to improve the transient performance of the time-optimal controller, integral control and derivative control are also required to be added with reference to PID control.
The algorithm must first discuss case 1 and discuss case 2 to ensure that constraints such as actuator specifications are fully satisfied. Firstly, initializing planning parameters: planning starting point p0Planning an end point pfPlanning exercise time T ═ Tf-t0(ii) a Secondly, interpolating and planning the track based on a seventh-order polynomial: p is the sum of the total of the p,
Figure GDA0003393390430000064
the resulting inverse solution is again used to find each joint angle: the number of the theta's is,
Figure GDA0003393390430000065
finally, whether situation 1 is satisfied is determined:
if so, the planning time T-T is reduced by equation (7)f-t0Replanning the track; if not, case 2 is entered. In case 2, as in case 1, the trajectory is first interpolated based on a seventh order polynomial: p is the sum of the total of the p,
Figure GDA0003393390430000066
the resulting inverse solution is again used to find each joint angle: the number of the theta's is,
Figure GDA0003393390430000067
finally, whether situation 2 is satisfied is determined: if so, increasing the programming time T-T by equation (7)f-t0Replanning the track; if not, case 2 exits. Two-situation discussion knotAnd (4) bundling.
The output theta is output to the output end,
Figure GDA0003393390430000068
the algorithm ends as a given of the joint control.
In the algorithm, the order of polynomial interpolation track planning is determined by specific track planning requirements, and is not limited to seven times; in joint space, in continuous trajectory planning, the algorithm can obtain the same effect; the algorithm is also suitable for other multi-degree-of-freedom mechanical arms or series robots with multiple inverse solutions.
Compared with the prior art, the invention has the following beneficial effects: the inverse solution multi-solution selection and time optimal trajectory planning algorithm for the mechanical arm, provided by the invention, does not adopt an iteration principle, is simple and efficient in algorithm architecture, has the capability of quickly selecting the most appropriate one group of solutions from the inverse solution multi-solutions and planning the time optimal trajectory, can quickly select the most appropriate one group of solutions from the inverse solution multi-solutions according to the current pose of the mechanical arm, effectively avoids a singular position type, realizes smooth switching among different inverse solutions, and ensures the continuity and non-oscillation of control and action of the mechanical arm; and a trajectory planning method combining the seventh polynomial and the conditional proportional control is utilized to plan a trajectory with optimal time, so that smooth and continuous guidance of joint angles, angular velocities, angular accelerations and angular jerks is ensured, multi-target and multi-constraint conditions are met, and the actuator specifications, such as joint angular velocity maximum value constraint or joint angular acceleration maximum value constraint, are met. The algorithm greatly reduces the calculation amount of multi-solution selection and trajectory planning, can realize real-time and efficient time optimal trajectory planning, and can be popularized and applied to low-calculation force controllers.
Drawings
FIG. 1 is a schematic diagram of a five degree-of-freedom robotic arm;
FIG. 2 is a schematic diagram of structural parameters and a coordinate system of a five-degree-of-freedom mechanical arm;
FIG. 3 is a flow chart of the algorithm;
FIG. 4 illustrates the joint angles of the robot arm after the track optimization;
FIG. 5 shows the angular velocity of each joint of the mechanical arm after the track optimization;
FIG. 6 shows angular accelerations of joints of the mechanical arm after track optimization;
FIG. 7 illustrates the jerk of each joint angle of the mechanical arm after the trajectory optimization;
FIG. 8 track optimized end of arm position;
FIG. 9 trajectory optimized robotic arm tip pose;
FIG. 10 is a schematic diagram of a time optimization process for an initial planning time 10 s;
FIG. 11 is a schematic diagram of a time optimization process for the initial planning time 20 s;
Detailed Description
The invention is further explained with reference to the drawings.
The invention aims to provide an inverse solution multi-solution selection and time optimal trajectory planning algorithm for a mechanical arm, which is simple and efficient in algorithm architecture and has the capability of quickly selecting the most appropriate group of solutions from the inverse solution multi-solutions and planning the time optimal trajectory.
The technical scheme adopted by the invention for solving the technical problems is as follows:
the mechanical arm inverse solution multi-solution selection and time optimal trajectory planning algorithm is characterized in that: on the basis of a kinematic forward solution and a kinematic inverse solution, the overall algorithm framework of the method is composed of: the method comprises an inverse solution multi-solution selection algorithm and a time optimal trajectory planning algorithm. The general idea of solving the multi-solution selection algorithm is as follows: and performing forward-inverse solution or inverse-forward solution conversion according to the current pose of the mechanical arm, and taking the inverse solution with the minimum norm of the difference between the conversion result and the current pose of the mechanical arm as the most appropriate solution. The overall idea of the time optimal trajectory planning algorithm is as follows: and (3) carrying out track planning by using a seventh polynomial track planning method, calculating values corresponding to all constraint conditions under the track, verifying whether the values are equal to constraint condition thresholds or not, if not, taking the difference value between the values and the corresponding constraint condition thresholds to carry out proportional control and negative feedback to the original track planning time so as to reduce the difference value, and simultaneously, distinguishing and determining whether the negative feedback is carried out on different constraint conditions or not by using activation conditions.
As shown in fig. 1, taking a five-degree-of-freedom mechanical arm to perform point-to-point trajectory planning in cartesian space as an example, the algorithm includes the following four steps:
the first step is as follows: a kinematic positive solution.
Firstly, as shown in FIG. 2, establishing a mechanical arm basic coordinate system {0}, each joint coordinate system {1,2.3.4} and a terminal coordinate system {5}, and establishing a mechanical arm D-H parameter table according to mechanical arm structure parameters;
TABLE 1.1D-H PARAMETER TABLE FOR MECHANICAL ARM
Figure GDA0003393390430000081
Secondly, obtaining a homogeneous transformation matrix among the coordinates according to the established coordinate system and the D-H parameter table of the mechanical arm; from the coordinate system oi-1-xi-1yi-1zi-1To the coordinate system oi-xiyiziIs transformed into
Figure GDA0003393390430000082
Thirdly, according to the homogeneous transformation matrix between the base coordinate system and the tail end coordinate system of the mechanical arm
Figure GDA0003393390430000091
The position of the robot arm tip relative to the base coordinate system and the pose of the robot arm tip relative to the base coordinate system are obtained, i.e., the kinematic positive solution p of the robot arm is Forward _ kinematics (θ), where p is [ x y z α β γ ═ y]For a known end pose of the arm, θ ═ θ1 θ2 θ3 θ4 θ5]The calculated joint angles of the robot.
The second step is that: inverse kinematics solution.
(i) can be obtained from the formula (2)
Figure GDA0003393390430000092
The first column and the fourth column of equation (2) are made equal, so that six equations can be obtained;
based on the six equations obtained above, the equation can be solved in turn to obtain theta1Two solutions of, each theta1Can determine theta5Two solutions of (each group of (theta)1,θ5) Can determine theta3Two solutions of (a) and then finding theta1A solution of (a) and theta4A solution of (a);
from the above, a total of eight solutions, i.e., Inverse kinematics of the robot arm θ ═ Inverse _ kinematics (i, p), (i ∈ [1,8]), can be obtained.
The third step: trajectory planning based on a seventh order polynomial.
In order to ensure that the acceleration is continuous and avoid the shock caused by sudden acceleration when the mechanical arm is started and stopped, a seventh-order polynomial can be adopted for optimization, and the optimization aim is to ensure that the acceleration is continuously derivable, namely the acceleration of a planning starting point and a terminal point of a constraint track is large. The specific mechanical arm tail end track planning formula is
Figure GDA0003393390430000093
Where p (t) is the known end pose of the robot arm p ═ x y z α β γ]As a function of the trajectory of the variation with respect to time t,
Figure GDA0003393390430000094
first, second and third derivatives of p (t), respectively.
Giving constraint conditions, namely the positions, the speeds, the accelerations and the jerks of the starting point and the ending point respectively as follows:
Figure GDA0003393390430000101
substituting formula (5) for formula (4) to obtain the coefficients of a seventh-order polynomial
Figure GDA0003393390430000102
Namely, the seven-degree polynomial track planning which aims at acceleration continuous guidance is completed.
The fourth step: and optimizing the track.
The inverse solution multi-solution selection algorithm: first, a set of solutions i ═ 1 is sequentially selected from eight sets of solutions of the inverse arm solution, (i ∈ [1,8], and]) (ii) a Then, reading the current joint angle theta of the mechanical arm ═ theta1 θ2 θ3 θ4 θ5]Carrying out forward solution according to the current pose of the mechanical arm to obtain the current end pose, and carrying out inverse solution according to the current end pose to obtain each joint angle theta under the i-th group of inverse solutionsi＝[θi1 θi2 θi3 θi4 θi5]Finally compare | θi-θ|≤εθIn which epsilonθFor self-defining an angle threshold value, the method is used for distinguishing the most appropriate solution from eight sets of inverse solutions, if thetai-θ|≤εθIf the solution is satisfied, the solution is the optimal solution; if not, another group of inverse solutions is taken for recalculation and comparison;
time optimal trajectory optimization algorithm based on condition proportional control
The algorithm can be divided into two cases:
case 1: the angular velocity of the planned trajectory obtained in the third step
Figure GDA0003393390430000111
And angular acceleration
Figure GDA0003393390430000112
Although all meet the actuator specification, i.e. the maximum angular velocity at which the joint can execute
Figure GDA0003393390430000113
And angular acceleration
Figure GDA0003393390430000114
But with a planning time T equal to Tf-t0Is relatively long and notIs optimal, where t0,tfThe times of passing the starting point and the ending point, respectively. At this point, the planning time needs to be reduced to achieve time optimization while meeting actuator specifications.
Case 2: the angular velocity of the planned trajectory obtained in the third step
Figure GDA0003393390430000115
And angular acceleration
Figure GDA0003393390430000116
Albeit greater than the actuator specification, i.e. the maximum angular velocity at which the joint can execute
Figure GDA0003393390430000117
And angular acceleration
Figure GDA0003393390430000118
The simplest solution at this time is to increase the planning time to achieve time optimization while meeting the actuator specifications.
According to the two situations, the time controller based on the condition proportional control can be designed without iteration, is simple and easy to realize and combines the proportional-integral-derivative (PID) control principle:
Figure GDA0003393390430000119
wherein
Figure GDA00033933904300001110
The adjusting parameters are respectively the error adjusting parameters of the speed and the acceleration in the time optimal controller, and are similar to the proportional parameters in the PID control principle. Whether the proportional control in equation (7) is activated depends on whether its corresponding actuator specification is satisfied, i.e., the origin of the conditional proportional control. In particular, it is possible to use, for example,
Figure GDA00033933904300001111
and
Figure GDA00033933904300001112
may be defined individually by joints or axes. Under ideal conditions, the planning time T is T ═ Tf-t0When the optimal angular velocity and angular acceleration of the planned trajectory are equal to the maximum angular velocity and angular acceleration allowed by the actuator specifications, i.e. the angular velocity and angular acceleration of the planned trajectory are equal to the maximum angular velocity and angular acceleration allowed by the actuator specifications
Figure GDA00033933904300001113
In addition, according to the requirements of multiple constraints and multiple targets, the maximum jerk term and other kinematic and dynamic constraint terms are required to be added into the right formula (7). In order to improve the transient performance of the time-optimal controller, integral control and derivative control are also required to be added with reference to PID control.
The algorithm must first discuss case 1 and discuss case 2 to ensure that constraints such as actuator specifications are fully satisfied. Firstly, initializing planning parameters: planning starting point p0Planning an end point pfPlanning exercise time T ═ Tf-t0(ii) a Secondly, interpolating and planning the track based on a seventh-order polynomial: p is the sum of the total of the p,
Figure GDA0003393390430000121
the resulting inverse solution is again used to find each joint angle: the number of the theta's is,
Figure GDA0003393390430000122
finally, whether situation 1 is satisfied is determined:
if so, the planning time T-T is reduced by equation (7)f-t0Replanning the track; if not, case 2 is entered. In case 2, as in case 1, the trajectory is first interpolated based on a seventh order polynomial: p is the sum of the total of the p,
Figure GDA0003393390430000123
the resulting inverse solution is again used to find each joint angle: the number of the theta's is,
Figure GDA0003393390430000124
finally, whether situation 2 is satisfied is determined: if so, increasing the gauge by the formula (7)Time T is Tf-t0Replanning the track; if not, case 2 exits. The two case discussion ends.
The output theta is output to the output end,
Figure GDA0003393390430000125
the algorithm ends as a given of the joint control.
The algorithm flow chart is shown in fig. 3. Fig. 4-9 are the results of the trajectory planning optimization from point to point in cartesian space, which are: each joint angle, each joint angular velocity, each joint angular acceleration, each joint angular jerk, the tip position, and the tip attitude. Fig. 10-11 are time optimization processes with initial planning times of 10s and 20s, respectively.
In the algorithm, the order of polynomial interpolation track planning is determined by specific track planning requirements, and is not limited to seven times; in joint space, in continuous trajectory planning, the algorithm can obtain the same effect; the algorithm is also suitable for other multi-degree-of-freedom mechanical arms or series robots with multiple inverse solutions.
Compared with the prior art, the invention has the following beneficial effects: the inverse solution multi-solution selection and time optimal trajectory planning algorithm for the mechanical arm, provided by the invention, does not adopt an iteration principle, is simple and efficient in algorithm architecture, has the capability of quickly selecting the most appropriate one group of solutions from the inverse solution multi-solutions and planning the time optimal trajectory, can quickly select the most appropriate one group of solutions from the inverse solution multi-solutions according to the current pose of the mechanical arm, effectively avoids a singular position type, realizes smooth switching among different inverse solutions, and ensures the continuity and non-oscillation of control and action of the mechanical arm; and a trajectory planning method combining the seventh polynomial and the conditional proportional control is utilized to plan a trajectory with optimal time, so that smooth and continuous guidance of joint angles, angular velocities, angular accelerations and angular jerks is ensured, multi-target and multi-constraint conditions are met, and the actuator specifications, such as joint angular velocity maximum value constraint or joint angular acceleration maximum value constraint, are met. The algorithm greatly reduces the calculation amount of multi-solution selection and trajectory planning, can realize real-time and efficient time optimal trajectory planning, and can be popularized and applied to low-calculation force controllers.

Claims (3)

1. The mechanical arm inverse solution multi-solution selection and time optimal trajectory planning algorithm is characterized in that: on the basis of a kinematic forward solution and a kinematic inverse solution, the overall algorithm framework of the method is composed of: the method comprises an inverse solution multi-solution selection algorithm and a time optimal trajectory planning algorithm, wherein the inverse solution multi-solution selection algorithm and the time optimal trajectory planning algorithm are composed of two parts; the general idea of solving the multi-solution selection algorithm is as follows: performing forward-inverse solution or inverse-forward solution conversion according to the current pose of the mechanical arm, and taking the inverse solution with the minimum norm of the difference between the conversion result and the current pose of the mechanical arm as the most appropriate solution; the overall idea of the time optimal trajectory planning algorithm is as follows: carrying out trajectory planning by using a seventh polynomial trajectory planning method, calculating values corresponding to all constraint conditions under the trajectory, verifying whether the values are equal to constraint condition thresholds or not, if not, taking the difference value between the values and the corresponding constraint condition thresholds to carry out proportional control and negative feedback to the original trajectory planning time so as to reduce the difference value, and simultaneously distinguishing and determining whether different constraint conditions are subjected to negative feedback or not by using activation conditions;
taking the point-to-point trajectory planning of a five-degree-of-freedom mechanical arm in a Cartesian space as an example, the algorithm comprises the following four steps:
the first step is as follows: a kinematic positive solution;
firstly, establishing a mechanical arm base coordinate system {0}, each joint coordinate system {1,2.3.4} and a terminal coordinate system {5}, and establishing a mechanical arm D-H parameter table according to mechanical arm structure parameters;
TABLE 1.1D-H PARAMETER TABLE FOR MECHANICAL ARM
Figure FDA0003393390420000011
Secondly, obtaining a homogeneous transformation matrix among the coordinates according to the established coordinate system and the D-H parameter table of the mechanical arm; from the coordinate system oi-1-xi-1yi-1zi-1To the coordinate system oi-xiyiziIs transformed into
Figure FDA0003393390420000021
Thirdly, according to the homogeneous transformation matrix between the base coordinate system and the tail end coordinate system of the mechanical arm
Figure FDA0003393390420000022
The position of the robot arm tip relative to the base coordinate system and the pose of the robot arm tip relative to the base coordinate system are obtained, i.e., the kinematic positive solution p of the robot arm is Forward _ kinematics (θ), where p is [ x y z α β γ ═ y]For a known end pose of the arm, θ ═ θ1 θ2 θ3 θ4 θ5]Calculating the angle of each joint of the robot;
the second step is that: inverse kinematics solution;
(i) can be obtained from the formula (2)
Figure FDA0003393390420000023
The first column and the fourth column of equation (2) are made equal, so that six equations can be obtained;
based on the six equations obtained above, the equation can be solved in turn to obtain theta1Two solutions of, each theta1Can determine theta5Two solutions of (each group of (theta)1,θ5) Can determine theta3Two solutions of (a) and then finding theta1A solution of (a) and theta4A solution of (a);
in sum, a total of eight solutions, that is, Inverse kinematics solution θ of the mechanical arm is Inverse _ kinematics (i, p), (i ∈ [1,8 ]);
the third step: trajectory planning based on a seventh-order polynomial;
in order to ensure that the acceleration is continuous and avoid the shock caused by sudden change of the acceleration when the mechanical arm is started and stopped, a seventh-order polynomial is adopted for optimization, and the optimization target is to ensure that the acceleration is continuously derivable, namely the acceleration of a planning starting point and a terminal point of a constrained track is large; the track planning formula of the tail end pose p (t) of the mechanical arm is
Figure FDA0003393390420000031
Where p (t) is the known end pose of the robot arm p ═ x y z α β γ]As a function of the trajectory of the variation with respect to time t,
Figure FDA0003393390420000032
first, second and third derivatives of p (t), respectively;
giving constraint conditions, namely the positions, the speeds, the accelerations and the jerks of the starting point and the ending point respectively as follows:
Figure FDA0003393390420000033
substituting formula (5) for formula (4) to obtain the coefficients of a seventh-order polynomial
Figure FDA0003393390420000034
Namely, the seven-degree polynomial track planning which takes the acceleration continuous guidance as the target is completed;
the fourth step: optimizing a track;
the inverse solution multi-solution selection algorithm: first, a set of solutions i ═ 1 is sequentially selected from eight sets of solutions of the inverse arm solution, (i ∈ [1,8], and]) (ii) a Then, reading the current joint angle theta of the mechanical arm ═ theta1 θ2 θ3 θ4 θ5]Carrying out forward solution according to the current pose of the mechanical arm to obtain the current end pose, and carrying out inverse solution according to the current end pose to obtain each joint angle theta under the i-th group of inverse solutionsi＝[θi1 θi2 θi3 θi4 θi5]Finally compare | θi-θ|≤εθIn which epsilonθFor self-defining an angle threshold value, the method is used for distinguishing the most appropriate solution from eight sets of inverse solutions, if thetai-θ|≤εθIf the solution is satisfied, the solution is the optimal solution; if not, another group of inverse solutions is taken for recalculation and comparison;
time optimal trajectory optimization algorithm based on condition proportional control
The algorithm can be divided into two cases:
case 1: the angular velocity of the planned trajectory obtained in the third step
Figure FDA0003393390420000041
And angular acceleration
Figure FDA0003393390420000042
Although all meet the actuator specification, i.e. the maximum angular velocity at which the joint can execute
Figure FDA0003393390420000043
And angular acceleration
Figure FDA0003393390420000044
But with a planning time T equal to Tf-t0Longer, not optimal, where t0,tfRespectively the time of passing the starting point and the ending point; at this time, the planning time needs to be reduced to achieve the optimal time and meet the actuator specification;
case 2: the angular velocity of the planned trajectory obtained in the third step
Figure FDA0003393390420000045
And angular acceleration
Figure FDA0003393390420000046
Albeit greater than the actuator specification, i.e. the maximum angular velocity at which the joint can execute
Figure FDA0003393390420000047
And angular acceleration
Figure FDA0003393390420000048
At the moment, the simplest solution is to increase the planning time to achieve the optimal time and meet the actuator specification;
according to the two situations, the time controller based on the condition proportional control can be designed without iteration, is simple and easy to realize and combines the proportional-integral-derivative (PID) control principle:
Figure FDA0003393390420000049
wherein
Figure FDA00033933904200000410
The adjusting parameters are respectively the error adjusting parameters of the speed and the acceleration in the time optimal controller, and are similar to the proportional parameters in the PID control principle; whether the proportional control in equation (7) is activated depends on whether its corresponding actuator specification is satisfied, i.e., the origin of the conditional proportional control; in particular, it is possible to use, for example,
Figure FDA0003393390420000051
and
Figure FDA0003393390420000052
can be defined by each joint or each axis separately; under ideal conditions, the planning time T is T ═ Tf-t0When the optimal angular velocity and angular acceleration of the planned trajectory are equal to the maximum angular velocity and angular acceleration allowed by the actuator specifications, i.e. the angular velocity and angular acceleration of the planned trajectory are equal to the maximum angular velocity and angular acceleration allowed by the actuator specifications
Figure FDA0003393390420000053
In addition, according to the requirements of multiple constraints and multiple targets, the maximum jerk term and other kinematic and dynamic constraint terms are required to be added into the right formula (7); in order to improve the transient transition performance of the time optimal controller, integral control and differential control are required to be added by referring to PID control;
the algorithm must first be discussedForm 1 in discussion case 2 to ensure that constraints such as actuator specifications are fully satisfied; firstly, initializing planning parameters: planning starting point p0Planning an end point pfPlanning exercise time T ═ Tf-t0(ii) a Secondly, interpolating and planning the track based on a seventh-order polynomial:
Figure FDA0003393390420000054
the resulting inverse solution is again used to find each joint angle:
Figure FDA0003393390420000055
finally, whether situation 1 is satisfied is determined: if so, the planning time T-T is reduced by equation (7)f-t0Replanning the track; if not, go to case 2; in case 2, as in case 1, the trajectory is first interpolated based on a seventh order polynomial:
Figure FDA0003393390420000056
the resulting inverse solution is again used to find each joint angle:
Figure FDA0003393390420000057
finally, whether situation 2 is satisfied is determined: if so, increasing the programming time T-T by equation (7)f-t0Replanning the track; if not, then exit case 2; the discussion of the two cases ends;
output (c)
Figure FDA0003393390420000058
The algorithm ends as a given of the joint control.
2. The inverse solution multi-solution selection and time optimal trajectory planning algorithm of a mechanical arm of claim 1, characterized in that: in the algorithm, the order of polynomial interpolation track planning is determined by specific track planning requirements, and is not limited to seven times; in joint space, in continuous trajectory planning, the algorithm can obtain the same effect; the algorithm is also suitable for other multi-degree-of-freedom mechanical arms or series robots with multiple inverse solutions.
3. The inverse solution multi-solution selection and time optimal trajectory planning algorithm of a mechanical arm of claim 1, characterized in that: the algorithm does not adopt an iteration principle, is simple and efficient in algorithm architecture, has the capability of quickly selecting the most appropriate one group of solutions from the inverse solution multiple solutions and planning the time optimal track, can quickly select the most appropriate one group of solutions from the inverse solution multiple solutions according to the current pose of the mechanical arm, effectively avoids a singular position type, realizes smooth switching among different inverse solutions, and ensures the continuity and no oscillation of mechanical arm control and action; a trajectory planning method combining the seventh polynomial and the conditional proportional control is utilized to plan a trajectory with optimal time, so that smooth and continuous guidance of joint angles, angular velocities, angular accelerations and angular jerks is ensured, multi-target and multi-constraint conditions are met, and the actuator specifications are met, such as joint angular velocity maximum value constraint or joint angular acceleration maximum value constraint; the algorithm greatly reduces the calculation amount of multi-solution selection and trajectory planning, can realize real-time and efficient time optimal trajectory planning, and can be popularized and applied to low-calculation force controllers.
CN202110109515.5A 2021-01-25 2021-01-25 Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm Active CN112757306B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110109515.5A CN112757306B (en) 2021-01-25 2021-01-25 Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110109515.5A CN112757306B (en) 2021-01-25 2021-01-25 Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm

Publications (2)

Publication Number Publication Date
CN112757306A CN112757306A (en) 2021-05-07
CN112757306B true CN112757306B (en) 2022-03-01

Family

ID=75706051

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110109515.5A Active CN112757306B (en) 2021-01-25 2021-01-25 Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm

Country Status (1)

Country Link
CN (1) CN112757306B (en)

Families Citing this family (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113290555B (en) * 2021-05-08 2022-04-15 浙江大学 Optimization method for time optimal control trajectory of industrial robot
CN113119131B (en) * 2021-05-08 2022-08-16 北京壹点灵动科技有限公司 Robot control method and device, computer readable storage medium and processor
CN113305881B (en) * 2021-05-14 2022-07-12 北京配天技术有限公司 Singular area detection method in robot motion planning stage
CN113478489B (en) * 2021-07-29 2022-05-10 桂林电子科技大学 Mechanical arm track planning method
CN113771046B (en) * 2021-10-25 2023-06-30 中国北方车辆研究所 Method for planning swing track of minimized Jerk index
CN114367975A (en) * 2021-11-15 2022-04-19 上海应用技术大学 Verification system of series industrial robot control algorithm
CN114227687B (en) * 2021-12-28 2023-08-15 深圳市优必选科技股份有限公司 Robot control method and device, terminal equipment and storage medium
CN114670177B (en) * 2022-05-09 2024-03-01 浙江工业大学 Gesture planning method for two-to-one-movement parallel robot
CN114833836B (en) * 2022-07-06 2022-09-20 珞石（北京）科技有限公司 Efficient time optimal trajectory online generation method
CN116352725B (en) * 2023-05-23 2023-10-13 极限人工智能(北京)有限公司 Three-time three-section type mechanical arm track planning method, system, equipment and medium
CN117084790B (en) * 2023-10-19 2024-01-02 苏州恒瑞宏远医疗科技有限公司 Puncture azimuth control method and device, computer equipment and storage medium
CN117724400A (en) * 2024-02-05 2024-03-19 南京铖联激光科技有限公司 Geometric error analysis and compensation method for five-axis denture processing center

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2345512A1 (en) * 2010-01-14 2011-07-20 Syddansk Universitet Method of finding feasible joint trajectories for an n-dof robot with rotation invariant process (N>5)
CN104526695A (en) * 2014-12-01 2015-04-22 北京邮电大学 Space manipulator track planning method for minimizing base seat collision disturbance
CN105676636A (en) * 2016-01-11 2016-06-15 北京邮电大学 NSGA-II algorithm-based multi-objective optimization method for mechanical arm in redundant space
CN109910013A (en) * 2019-04-04 2019-06-21 江南大学 A kind of PTP method for planning track of the continuous bounded of SCARA robot acceleration
CN111070206A (en) * 2019-12-13 2020-04-28 同济大学 Station layout method for reducing robot motion energy consumption

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP2345512A1 (en) * 2010-01-14 2011-07-20 Syddansk Universitet Method of finding feasible joint trajectories for an n-dof robot with rotation invariant process (N>5)
CN104526695A (en) * 2014-12-01 2015-04-22 北京邮电大学 Space manipulator track planning method for minimizing base seat collision disturbance
CN105676636A (en) * 2016-01-11 2016-06-15 北京邮电大学 NSGA-II algorithm-based multi-objective optimization method for mechanical arm in redundant space
CN109910013A (en) * 2019-04-04 2019-06-21 江南大学 A kind of PTP method for planning track of the continuous bounded of SCARA robot acceleration
CN111070206A (en) * 2019-12-13 2020-04-28 同济大学 Station layout method for reducing robot motion energy consumption

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
Dobot型机器人运动学分析与仿真;蔡汉明等;《机电工程》;20161020(第10期);全文 *

Also Published As

Publication number Publication date
CN112757306A (en) 2021-05-07

Similar Documents

Publication Publication Date Title
CN112757306B (en) Inverse solution multi-solution selection and time optimal trajectory planning algorithm for mechanical arm
CN106647282B (en) Six-degree-of-freedom robot trajectory planning method considering tail end motion error
CN105500354B (en) Transitional track planning method applied by industrial robot
CN109159151B (en) Mechanical arm space trajectory tracking dynamic compensation method and system
CN110497411B (en) Industrial robot collaborative motion control method
CN110561440B (en) Multi-objective planning method for acceleration layer of redundant manipulator
CN113601512B (en) General avoidance method and system for singular points of mechanical arm
CN110842913B (en) Adaptive sliding mode iterative learning control method of single-joint mechanical arm
CN112222703B (en) Energy consumption optimal trajectory planning method for welding robot
CN109866222B (en) Mechanical arm motion planning method based on longicorn stigma optimization strategy
CN109623812B (en) Mechanical arm trajectory planning method considering spacecraft body attitude motion
CN111890349A (en) Four-degree-of-freedom mechanical arm motion planning method
CN110695994B (en) Finite time planning method for cooperative repetitive motion of double-arm manipulator
CN113650011B (en) Method and device for planning splicing path of mechanical arm
Furukawa Time-subminimal trajectory planning for discrete non-linear systems
CN108724195B (en) Coupling feedforward control method for robot
CN114055467A (en) Space pose online simulation system based on five-degree-of-freedom robot
Tangpattanakul et al. Optimal trajectory of robot manipulator using harmony search algorithms
CN116540721A (en) Space robot optimal track planning method based on improved genetic particle swarm algorithm
CN114063621B (en) Wheel type robot formation tracking and obstacle avoidance control method
Dian et al. A Novel Disturbance-Rejection Control Framework for Cable-Driven Continuum Robots With Improved State Parameterizations
Gao et al. A fixed-distance planning algorithm for 6-DOF manipulators
CN114840947A (en) Three-degree-of-freedom mechanical arm dynamic model with constraint
CN114800521A (en) Three-degree-of-freedom mechanical arm fixed path point motion control system with constraint
CN109794939B (en) Parallel beam planning method for welding robot motion

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