技术领域Technical field
本发明属于工业机器人运动控制技术领域，具体涉及一种工业机器人时间最优五次多项式速度规划方法。The invention belongs to the technical field of industrial robot motion control, and specifically relates to a time-optimal fifth-order polynomial speed planning method for industrial robots.
背景技术Background technique
在工业机器人领域，工业机器人末端按照一定轨迹运动时，为保证机器人运动过程中各项运动参数平滑连续，减小关节冲击和磨损，需要对轨迹运动参数加以控制。对工业机器人的轨迹规划可分为关节空间规划和笛卡尔空间规划，关节空间规划以机器人关节角度函数描述机器人的轨迹，可以避免运动中出现奇异点，且计算简单，但关节空间难以预见机器人末端的轨迹变化，不适用于末端要求高的场合。笛卡尔空间轨迹规划以机器人末端运动轨迹为规划目标，可以确定路径点之间的空间路径形状，末端轨迹精度高，规划的路径准确。目前常用速度规划方法有T型、S型、三次多项式和五次多项式等方法；其中，T型规划方法简单，但不能保证运行过程的平稳性；S型规划满足平稳性和时间最优要求，但对于任意始末运动状态的情况却很难处理，现有的技术方法十分复杂，且S型速度规划不能保证加加速度连续；关于多项式规划研究多在关节空间中进行，插补得到值可以直接作为机器人的关节角度，进行轨迹点间的过渡，仅能实现笛卡尔空间点到点控制，不能满足连续轨迹规划。现有关于五次多项式时间最优轨迹规划的研究都是基于关节空间进行的，且算法较为复杂，计算量大。In the field of industrial robots, when the end of the industrial robot moves according to a certain trajectory, in order to ensure smooth and continuous motion parameters during the robot's movement and reduce joint impact and wear, the trajectory motion parameters need to be controlled. Trajectory planning for industrial robots can be divided into joint space planning and Cartesian space planning. Joint space planning describes the trajectory of the robot as a function of the robot's joint angles, which can avoid singular points in the movement and is simple to calculate. However, it is difficult to predict the end of the robot in joint space. The trajectory changes and are not suitable for occasions with high terminal requirements. Cartesian spatial trajectory planning takes the robot's end motion trajectory as the planning goal, and can determine the shape of the spatial path between path points. The end trajectory has high accuracy and the planned path is accurate. Currently, commonly used speed planning methods include T-type, S-type, cubic polynomial and quintic polynomial methods; among them, T-type planning method is simple, but cannot guarantee the stability of the operation process; S-type planning meets the requirements of stability and time optimization. However, it is difficult to handle the situation of any beginning and ending motion state. The existing technical methods are very complicated, and S-shaped velocity planning cannot guarantee continuous acceleration. Research on polynomial planning is mostly conducted in joint space, and the value obtained by interpolation can be directly used as The robot's joint angles can only achieve point-to-point control in Cartesian space when transitioning between trajectory points, and cannot satisfy continuous trajectory planning. Existing research on quintic polynomial time optimal trajectory planning is all based on joint space, and the algorithm is relatively complex and requires a large amount of calculation.
中国专利申请(CN107390634A)公开了一种工业机器人轨迹五次多项式规划方法，该方法根据轨迹的起止位移、速度和加速度信息，利用S型轨迹规划的速度趋势，拟合出五次多项式轨迹规划的插补时间，进一步确定出五次多项式轨迹规划模型，能够解决五次多项式曲线形状不固定，易发生扭摆的情况。然而上述方法通过拟合公式计算得到运行时间，没有考虑运动的效率，其运行时间不是最优的，其次并没有对加加速度进行约束，仍存在很大缺陷。Chinese patent application (CN107390634A) discloses a quintic polynomial planning method for industrial robot trajectory. This method uses the speed trend of S-shaped trajectory planning to fit the quintic polynomial trajectory planning based on the starting and ending displacement, speed and acceleration information of the trajectory. The interpolation time further determines the fifth-order polynomial trajectory planning model, which can solve the problem that the shape of the fifth-order polynomial curve is not fixed and is prone to twisting. However, the above method calculates the running time through a fitting formula, without considering the efficiency of motion, and its running time is not optimal. Secondly, it does not constrain the acceleration, and there are still big flaws.
因此现有技术需要一种既能实现满足运动约束的笛卡尔空间连续轨迹规划，又能够实现任意始末运动条件的速度规简便方法，并且需要一种时间优化方法，来缩短轨迹运行时间，提高工业机器人运行效率。Therefore, the existing technology requires a simple method of speed regulation that can not only realize continuous trajectory planning in Cartesian space that satisfies motion constraints, but also can realize arbitrary starting and ending motion conditions. It also needs a time optimization method to shorten the trajectory running time and improve industrial efficiency. Robot operating efficiency.
发明内容Contents of the invention
本发明的目的在于提供一种工业机器人时间最优五次多项式速度规划方法，所述方法实现了笛卡尔空间连续轨迹规划，且运行时间最优，整个运动过程满足运动约束，适用于任意始末运动状态的速度规划，该方法过程简单，计算量小。The purpose of the present invention is to provide a time-optimal fifth-degree polynomial speed planning method for industrial robots. The method realizes continuous trajectory planning in Cartesian space and has optimal running time. The entire motion process satisfies motion constraints and is suitable for any beginning and end motion. State speed planning, this method has simple process and small calculation amount.
本发明的目的通过如下技术方案来实现：一种工业机器人时间最优五次多项式速度规划方法,包括：The purpose of the present invention is achieved through the following technical solutions: a time-optimal fifth-degree polynomial speed planning method for industrial robots, including:
步骤一：首先给定初末运动状态参数和运动约束，建立五次多项式速度规划时间最优化模型，包括目标函数、约束条件和决策变量；Step 1: First, given the initial and final motion state parameters and motion constraints, establish a quintic polynomial speed planning time optimization model, including objective function, constraints and decision variables;
其中，初末运动状态参数包括：初位置p0、末位置pf、初速度末速度/>初加速度/>末加速度/>Among them, the initial and final motion state parameters include: initial position p0 , final position pf, initial velocity Terminal speed/> Initial acceleration/> Final acceleration/>
运动约束为：速度约束Vmax、加速度约束Amax、加加速度约束Jmax；The motion constraints are: velocity constraint Vmax , acceleration constraint Amax , and jerk constraint Jmax ;
所述目标函数为最优运行时间，即满足运动约束情况下轨迹运行时间的最小值；决策变量为运行时间；The objective function is the optimal running time, that is, the minimum value of the trajectory running time when motion constraints are met; the decision variable is the running time;
五次多项式的定义为The definition of a fifth degree polynomial is
p(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5p(t)＝a0 +a1 t+a2 t2 +a3 t3 +a4 t4 +a5 t5
其中，p(t)为笛卡尔空间的位移，a0～a5为多项式系数，t作为自变量表示插补时间；Among them, p(t) is the displacement in Cartesian space, a0 ~ a5 are polynomial coefficients, and t as an independent variable represents the interpolation time;
步骤二：计算五次多项式的各项系数，并利用速度方程、加速度方程求导计算运动参数极限值；所述运动参数极限值包括最大速度vmax、最小速度vmin、最大加速度amax、最大加加速度jmax；Step 2: Calculate the coefficients of the quintic polynomial, and use the velocity equation and acceleration equation to derive the motion parameter limit values; the motion parameter limit values include the maximum speed vmax , the minimum speed vmin , the maximum acceleration amax , the maximum Jerk jmax ;
步骤三：判断步骤二获得的运动参数极限值是否满足约束条件，若满足约束条件执行步骤二，否则放大运行时间，返回步骤二更新五次多项式的各项系数和运动参数极限值，直到运行时间进入可行域；Step 3: Determine whether the motion parameter limit values obtained in Step 2 satisfy the constraint conditions. If the constraint conditions are met, proceed to Step 2. Otherwise, the running time is enlarged and return to Step 2 to update the coefficients of the quintic polynomial and the motion parameter limit values until the running time. Enter the feasible region;
步骤四：对步骤三获得的满足约束条件的运行时间使用二分法迭代，使其逼近可行域的边界，直到满足精度为止，得到最优运行时间，根据插补周期对运行时间进行调整，使其满足插补周期的整数倍；Step 4: Use the dichotomy method to iterate the running time that satisfies the constraints obtained in step 3, so that it approaches the boundary of the feasible region until the accuracy is met. The optimal running time is obtained, and the running time is adjusted according to the interpolation period to make it Satisfies the integer multiple of the interpolation period;
步骤五：根据最优运行时间和初末运动状态参数解出五次多项式的系数，确定五次多项式插值函数，按照已知的插补周期对五次多项式进行定时插补，得到五次多项式位移序列，再根据位移序列，对笛卡尔空间中的已有的轨迹路径进行插补，得到实际的轨迹点序列。Step 5: Solve the coefficients of the fifth-order polynomial based on the optimal running time and initial and final motion state parameters, determine the fifth-order polynomial interpolation function, and perform timed interpolation on the fifth-order polynomial according to the known interpolation period to obtain the fifth-order polynomial displacement. sequence, and then interpolate the existing trajectory path in Cartesian space according to the displacement sequence to obtain the actual trajectory point sequence.
进一步的，所述约束条件为运动过程中的运动参数约束，包括：速度始终小于给定最大速度值、速度始终大于零、加速度的绝对值始终小于给定最大加速度值、加加速度的绝对值始终小于给定最大加加速度值，决策变量为运行时间。Further, the constraints are motion parameter constraints during motion, including: the speed is always less than a given maximum speed value, the speed is always greater than zero, the absolute value of acceleration is always less than a given maximum acceleration value, the absolute value of jerk is always Less than the given maximum jerk value, the decision variable is the running time.
进一步的，所述放大运行时间是通过对运行时间乘以放大系数实现，放大系数为介于1-2之间的常数值。Further, the amplification of the running time is achieved by multiplying the running time by an amplification coefficient, and the amplification coefficient is a constant value between 1 and 2.
进一步的，对五次多项式求导得速度函数、加速度函数、加加速度函数：Furthermore, the velocity function, acceleration function, and jerk function are obtained by deriving the fifth degree polynomial:
V(t)＝a1+2a2t+3a3t2+4a4t3+5a5t4V(t)＝a1 +2a2 t+3a3 t2 +4a4 t3 +5a5 t4
A(t)＝2a2+6a3t+12a4t2+20a5t3A(t)＝2a2 +6a3 t+12a4 t2 +20a5 t3
J(t)＝6a3+24a4t+60a5t2J(t)＝6a3 +24a4 t+60a5 t2
则目标函数和约束条件为:Then the objective function and constraints are:
f(tf)＝min tff(tf )=min tf
其中，tf为运行时间，Vmax为速度约束，Amax为加速度约束，Jmax为加加速度约束。Among them, tf is the running time, Vmax is the speed constraint, Amax is the acceleration constraint, and Jmax is the acceleration constraint.
进一步的，其中最大和最小速度根据加速度函数进行计算，最终代入速度方程求得速度的极值，和初末条件比较大小后得到最大速度和最小速度；Further, the maximum and minimum velocities are calculated based on the acceleration function, and finally substituted into the velocity equation to obtain the extreme value of the velocity. After comparing with the initial and final conditions, the maximum velocity and minimum velocity are obtained;
最大加速度值通过对加速度方程求导后代入加速度方法，获得加速度多项式的极大值和极小值，取绝对值后比较大小，最终得到最大加速度；The maximum acceleration value is entered into the acceleration method by deriving the acceleration equation to obtain the maximum and minimum values of the acceleration polynomial. After taking the absolute value and comparing the values, the maximum acceleration is finally obtained;
最大加加速度的计算方法为：加加速度的极值为将极值和J(0),J(tf)比较大小后得到最大加加速度；The calculation method of the maximum jerk is: the extreme value of the jerk is After comparing the extreme value with J(0), J(tf ), the maximum jerk is obtained;
进一步的，所述步骤四包括如下步骤：Further, the step four includes the following steps:
首先定义二分区间，所述二分区间的下限为当前运行时间除以放大系数，所述二分区间的上限为当前运行时间；计算所述二分区间中间值，并更新当前运行时间值为二分区间中间值tf＝(Tmax+Tmin)/2；判断二分区间长度是否小于插补周期；如果是，则调整运行时间tf为插补周期的整数倍，调整后的当前运行时间tf即为最优运行时间；否则，将获得的当前运行时间tf代入步骤二，更新五次多项式的各项系数和运动参数极限值，直至二分区间长度小于插补周期为止。First, define a two-partition interval. The lower limit of the two-partition interval is the current running time divided by the amplification factor. The upper limit of the two-partition interval is the current running time. Calculate the intermediate value of the two-partition interval, and update the current running time value to the intermediate value of the two-partition interval. tf = (Tmax +Tmin )/2; determine whether the length of the two-partition interval is less than the interpolation period; if so, adjust the running time tf to an integer multiple of the interpolation period, and the adjusted current running time tf is The optimal running time; otherwise, substitute the obtained current running time tf into step 2, and update the coefficients of the quintic polynomial and the motion parameter limit values until the length of the two-part interval is less than the interpolation period.
与现有技术相比，本发明的技术方案所带来的有益效果是：Compared with the existing technology, the beneficial effects brought by the technical solution of the present invention are:
该方法首先建立了笛卡尔空间五次多项式时间最优化模型，通过计算最大速度、最大加速度和最大加加速度建立约束条件，使用二分法求解最优时间，解决了五次多项式规划的时间最优问题，保证运动任意时刻均满足速度、加速度、加加速度约束。本方法的关键在于求取最优运行时间，避免了五次多项规划的运动方向不稳定、速度和加速度峰值过大等问题；和S型速度规划相比，本方法能够简便实现任意始末运动状态下的速度规划，。和其他优化方法相比，该方法通过代数法求解，计算量小，且迭代速度快，有效降低了计算负担。This method first establishes a Cartesian space quintic polynomial time optimization model, establishes constraint conditions by calculating the maximum speed, maximum acceleration and maximum jerk, uses the bisection method to solve the optimal time, and solves the time optimization problem of quintic polynomial planning. , ensuring that the motion satisfies the speed, acceleration, and jerk constraints at any time. The key to this method is to find the optimal running time, which avoids problems such as unstable motion direction, excessive speed and acceleration peaks in the five-time polynomial planning; compared with S-type speed planning, this method can easily realize any start and end motion. Speed planning under state,. Compared with other optimization methods, this method is solved through algebra, has a small amount of calculation and fast iteration speed, effectively reducing the computational burden.
附图说明Description of the drawings
图1是本发明所述工业机器人时间最优五次多项式速度规划方法的流程图；Fig. 1 is a flow chart of the time-optimal fifth-order polynomial speed planning method of the industrial robot according to the present invention;
图2是步骤二至四的流程图；Figure 2 is a flow chart of steps two to four;
图3是步骤四中运行时间的迭代变化图；Figure 3 is an iterative change diagram of the running time in step 4;
图4a-4d是时间最优五次多项式速度规划算法仿真图；其中图4a体现采用普通五次多项式速度规划方法获得的轨迹和采用本发明所述的规划方法获得的轨迹的位移参数对比；图4b体现采用普通五次多项式速度规划方法获得的轨迹和采用本发明所述的规划方法获得的轨迹的加速度参数对比；图4c体现采用普通五次多项式速度规划方法获得的轨迹和采用本发明所述的规划方法获得的轨迹的速度参数对比；图4d体现采用普通五次多项式速度规划方法获得的轨迹和采用本发明所述的规划方法获得的轨迹的加加速度参数对比；Figures 4a-4d are simulation diagrams of the time-optimal fifth-degree polynomial speed planning algorithm; Figure 4a shows a comparison of the displacement parameters of the trajectory obtained by the ordinary fifth-degree polynomial speed planning method and the trajectory obtained by the planning method of the present invention; Figure 4b shows the comparison of the acceleration parameters of the trajectory obtained by using the ordinary fifth-degree polynomial speed planning method and the trajectory obtained by using the planning method of the present invention; Figure 4c shows the comparison of the acceleration parameters of the trajectory obtained by using the ordinary fifth-degree polynomial speed planning method and using the planning method of the present invention. Comparison of the velocity parameters of the trajectory obtained by the planning method; Figure 4d shows the comparison of the acceleration parameters of the trajectory obtained by the ordinary fifth-order polynomial velocity planning method and the trajectory obtained by the planning method of the present invention;
图5a-5c是运动参数曲线随运行时间变化趋势图；其中，图5a示出位移曲线变化趋势，4条数据分别是运行时间各为0.4s、0.6s、0.8s、1.0s时的位移曲线，随着运行时间增大位移曲线趋于平缓；图5b示出速度曲线变化趋势，4条数据分别是运行时间各为0.4s、0.6s、0.8s、1.0s时的速度曲线，随着运行时间增大速度曲线峰值逐渐降低；图5c示出加速度曲线变化趋势4条数据分别是运行时间各为0.4s、0.6s、0.8s、1.0s时的加速度曲线，随着运行时间增大加速度曲线上下峰值逐渐降低；Figures 5a-5c are graphs showing the change trend of motion parameter curves with running time; among them, Figure 5a shows the changing trend of the displacement curve, and the four pieces of data are the displacement curves when the running time is 0.4s, 0.6s, 0.8s, and 1.0s respectively. , as the running time increases, the displacement curve tends to be flat; Figure 5b shows the change trend of the speed curve. The four pieces of data are the speed curves when the running time is 0.4s, 0.6s, 0.8s, and 1.0s. As the running time increases, the displacement curve becomes gentler. As time increases, the peak value of the speed curve gradually decreases; Figure 5c shows the change trend of the acceleration curve. The four pieces of data are the acceleration curves when the running time is 0.4s, 0.6s, 0.8s, and 1.0s respectively. As the running time increases, the acceleration curve increases. The upper and lower peaks gradually decrease;
图6是实施例采用本发明所述方法获得的轨迹仿真图。Figure 6 is a trajectory simulation diagram obtained by using the method of the present invention according to the embodiment.
具体实施方式Detailed ways
为使本发明实施例的目的、技术方案、有益效果及显著进步更加清楚，下面结合本发明实例中所提供的附图，对本发明实施例中的技术方案进行清楚、完整的描述，显然，所有描述的这些实施例仅是本发明的部分实施例，而不是全部的实施例；基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。In order to make the purpose, technical solutions, beneficial effects and significant progress of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention are clearly and completely described below in conjunction with the drawings provided in the examples of the present invention. Obviously, all These described embodiments are only some of the embodiments of the present invention, not all of them; based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without making creative efforts, All belong to the protection scope of the present invention.
在本申请的描述中，除非另有明确的规定和限定，术语“第一”、“第二”、“第三”仅用于描述的目的，而不能理解为指示或暗示相对重要性；术语“多个”是指两个或两个以上；除非另有规定或说明，术语“连接”、“固定”等均应做广义理解，例如，“连接”可以是固定连接，也可以是可拆卸连接，或一体地连接，或电连接；“连接”可以是直接相连，也可以通过中间媒介间接相连。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。In the description of this application, unless otherwise expressly stated and limited, the terms "first", "second" and "third" are only used for descriptive purposes and cannot be understood as indicating or implying relative importance; terms "Multiple" refers to two or more; unless otherwise specified or stated, the terms "connection", "fixed", etc. should be understood in a broad sense. For example, "connection" can be a fixed connection or a detachable connection. Connection, or integral connection, or electrical connection; "connection" can be directly connected, or indirectly connected through an intermediary. For those of ordinary skill in the art, the specific meanings of the above terms in this application can be understood according to specific circumstances.
如图1所示,以某工业机器人为例，一种工业机器人时间最优五次多项式速度规划方法,具体包括如下步骤:As shown in Figure 1, taking an industrial robot as an example, a time-optimal fifth-degree polynomial speed planning method for industrial robots specifically includes the following steps:
步骤一：在笛卡尔空间中，存在从(0,500,500)到(380,500,500)的直线轨迹，给定初末运动状态参数和运动约束，建立笛卡尔坐标系下五次多项式速度规划时间最优化模型，包括目标函数、约束条件和决策变量；Step 1: In the Cartesian space, there is a straight-line trajectory from (0,500,500) to (380,500,500). Given the initial and final motion state parameters and motion constraints, establish a fifth-order polynomial speed planning time optimization model in the Cartesian coordinate system, including Objective function, constraints and decision variables;
其中，给定轨迹的初末运动状态参数为：初位置p0＝0mm、末位置pf＝380mm、初速度末速度/>初加速度/>末加速度/>Among them, the initial and final motion state parameters of the given trajectory are: initial position p0 =0mm, final position pf =380mm, initial velocity Terminal speed/> Initial acceleration/> Final acceleration/>
运动约束为：速度约束Vmax＝200mm/s、加速度约束Amax＝400mm/s2、加加速度约束Jmax＝1000mm/s3。The motion constraints are: velocity constraint Vmax =200mm/s, acceleration constraint Amax =400mm/s2 , and jerk constraint Jmax =1000mm/s3 .
所述给定初末运动状态参数应符合给定约束，并且不能出现以下自相矛盾的组合情况：The given initial and final motion state parameters should comply with the given constraints, and the following contradictory combinations cannot occur:
(1)初速度等于运动参数极限值的最大速度vmax，且初加速度/>大于零；(1)Initial velocity The maximum speed vmax equal to the limit value of the motion parameter, and the initial acceleration/> Greater than zero;
(2)初速度等于零，且初加速度/>小于零；(2)Initial velocity is equal to zero, and the initial acceleration/> less than zero;
(3)末速度等于最大速度vmax，且末加速度/>小于零；(3)Terminal speed Equal to the maximum speed vmax and no acceleration/> less than zero;
(4)末速度等于零，且末加速度/>大于零；(4)Terminal speed Equal to zero and no acceleration/> Greater than zero;
五次插值多项式的定义为The quintic interpolation polynomial is defined as
p(t)＝a0+a1t+a2t2+a3t3+a4t4+a5t5 (1)p(t)＝a0 +a1 t+a2 t2 +a3 t3 +a4 t4 +a5 t5 (1)
其中，p(t)为笛卡尔空间的位移，a0～a5为多项式系数，t作为自变量表示插补时间，对其求导得速度函数、加速度函数、加加速度函数：Among them, p(t) is the displacement in Cartesian space, a0 ~ a5 are polynomial coefficients, t is used as an independent variable to represent the interpolation time, and the velocity function, acceleration function, and jerk function are obtained by derivation:
V(t)＝a1+2a2t+3a3t2+4a4t3+5a5t4 (2)V(t)＝a1 +2a2 t+3a3 t2 +4a4 t3 +5a5 t4 (2)
A(t)＝2a2+6a3t+12a4t2+20a5t3 (3)A(t)＝2a2 +6a3 t+12a4 t2 +20a5 t3 (3)
J(t)＝6a3+24a4t+60a5t2 (4)J(t)＝6a3 +24a4 t+60a5 t2 (4)
给定运动约束为速度约束Vmax、加速度约束Amax和加加速度约束Jmax，运行时间tf；则目标函数和约束条件为:Given that the motion constraints are speed constraint Vmax , acceleration constraint Amax and jerk constraint Jmax , the running time is tf ; then the objective function and constraint conditions are:
f(tf)＝min tff(tf )=min tf
步骤二：计算五次多项式的各项系数a0-a5，并计算运动参数极限值；Step 2: Calculate the coefficients a0 -a5 of the quintic polynomial, and calculate the limit values of the motion parameters;
设置运行时间初始值tf＝0.1s，多项式系数通过如下方式得到：Set the initial value of the running time tf =0.1s, and the polynomial coefficients are obtained as follows:
运动参数极限值包括最大速度vmax、最小速度vmin、最大加速度amax、最大加加速度jmax。The motion parameter limit values include maximum speed vmax , minimum speed vmin , maximum acceleration amax , and maximum acceleration jmax .
最大速度vmax和最小速度vmin的具体计算方法如下：The specific calculation methods of the maximum speed vmax and the minimum speed vmin are as follows:
先求取加速度函数的零点，易得20a5t3+12a4t2+6a3t+2a2＝0，令得简化方程z3+3pz+2q＝0，其中，/>根的判别式为△＝p3+q2；其中z为代换变量，p、q为简化方程的系数。First find the zero point of the acceleration function, it is easy to get 20a5 t3 +12a4 t2 +6a3 t+2a2 =0, let The simplified equation z3 +3pz+2q=0 is obtained, where,/> The discriminant of the root is △=p3 +q2 ; where z is the substitution variable, and p and q are the coefficients of the simplified equation.
由于速度规划的输入参数对多项式系数约束，不会发生△≥0时出现复根的情况；当△<0时，方程有三个不同实根，若q≠0，则采用三角解法，设则解为若q＝0，此时简化立方方程为z3+3pz＝0，即z(z2+3p)＝0，由△<0知，p<0，解得z1＝0，/>Since the input parameters of the speed planning constrain the polynomial coefficients, complex roots will not occur when △≥0; when △<0, the equation has three different real roots. If q≠0, the trigonometric solution method is used, assuming Then the solution is If q=0, the simplified cubic equation is z3 +3pz=0, that is, z(z2 +3p)=0. From △<0, p<0, the solution is z1 =0,/>
将简化方程的解z1,z2,z3代入便得到原加速度方程的三个实根t1,t2,t3。舍去不在[0,tf]区间的根，最终代入速度方程求得速度的极值，和初末条件比较大小后得到最大速度和最小速度。Substitute the solutions z1 , z2 , z3 of the simplified equation into Then we get the three real roots t1 , t2 , t3 of the original acceleration equation. The roots that are not in the interval [0, tf ] are discarded, and finally substituted into the velocity equation to obtain the extreme value of the velocity. After comparing with the initial and final conditions, the maximum velocity and minimum velocity are obtained.
最大加速度的计算方法为：对加速度方程求导得A′(t)＝6a3+24a4t+60a5t2,其零点为排除t<0或t>tf的零点，代入加速度方程，得到加速度多项式的极大值和极小值，取绝对值后比较大小，最终得到最大加速度。The calculation method of the maximum acceleration is: Derive the acceleration equation to get A′(t)=6a3 +24a4 t+60a5 t2 , and its zero point is Exclude the zero points of t<0 or t>tf , substitute them into the acceleration equation, and get the maximum and minimum values of the acceleration polynomial. Take the absolute values and compare them, and finally get the maximum acceleration.
最大加加速度的计算方法为：加加速度的极值为将极值和J(0),J(tf)比较大小后得到最大加加速度。The calculation method of the maximum jerk is: the extreme value of the jerk is The maximum jerk is obtained by comparing the extreme value with J(0), J(tf ).
步骤三：如图2所示，判断步骤二获得的运动参数极限值是否满足以下约束条件：最小速度vmin≥0，最大速度vmax≤Vmax，最大加速度amax≤Amax，最大加加速度jmax≤Jmax；Step 3: As shown in Figure 2, determine whether the motion parameter limit values obtained in Step 2 satisfy the following constraints: minimum speed vmin ≥ 0, maximum speed vmax ≤ Vmax , maximum acceleration amax ≤ Amax , maximum accelerationjmax≤Jmax ;
若满足约束条件意味着运行时间在可行域，记录当前的运行时间tf，执行步骤四；If satisfying the constraints means that the running time is in the feasible region, record the current running time tf and perform step four;
否则，将运行时间乘放大系数α(设置放大系数α＝1.9)，以减小下次计算的运动参数最大值，返回步骤二更新五次多项式的各项系数和运动参数极限值，再次执行本步骤进行判断，直到运行时间进入可行域，运动参数随运行时间变化趋势如图5a-5c所示。Otherwise, multiply the running time by the amplification factor α (set the amplification factor α = 1.9) to reduce the maximum value of the motion parameter calculated next time, return to step 2 to update the coefficients of the quintic polynomial and the motion parameter limit value, and execute this procedure again. The judgment is made step by step until the running time enters the feasible region. The changing trend of the motion parameters with the running time is shown in Figure 5a-5c.
步骤四：对步骤三获得的满足约束条件的运行时间使用二分法迭代，使其逼近可行域的边界，直到满足精度为止，得到最优运行时间，根据插补周期对运行时间进行调整，使其满足插补周期的整数倍。Step 4: Use the dichotomy method to iterate the running time that satisfies the constraints obtained in step 3, so that it approaches the boundary of the feasible region until the accuracy is met. The optimal running time is obtained, and the running time is adjusted according to the interpolation period to make it Satisfies the integer multiple of the interpolation period.
具体操作为：首先定义二分区间[Tmin,Tmax]，其中Tmin＝tf/α，Tmax＝tf；然后判断是否满足二分法的精确度ε，(Tmax-Tmin)/2<ε，其中ε＝0.002，其值和插补周期大小相等，若满足精确度ε，则调整运行时间Tf为插补周期的整数倍，经过调整后的当前运行时间tf即为最优运行时间；若不满足精确度，则令tf＝(Tmax+Tmin)/2，并将该当前运行时间tf代入步骤二，返回执行步骤二更新五次多项式的各项系数和运动参数极限值，再次判断运动参数极限值是否满足约束条件，若满足约束条件，则调整区间上限为tf，若不满足，则调整区间下限为tf，区间调整后进行下一次迭代，直至满足精确度为止。最终得到最优时间tf＝3.18203125s，运行时间随迭代次数变化情况如图3所示。对得到的最优时间tf向上取整为插补周期的整数倍，设机器人插补周期为0.002s，则调整最优运行时间为tf＝3.184s。The specific operation is: first define the dichotomy interval [Tmin , Tmax ], where Tmin =tf /α, Tmax =tf ; then determine whether the accuracy of the dichotomy ε is met, (Tmax -Tmin )/ 2<ε, where ε=0.002, its value is equal to the interpolation period. If the accuracy ε is met, the running time Tf is adjusted to an integer multiple of the interpolation period. The adjusted current running time tf is the maximum Optimal running time; if the accuracy is not met, let tf = (Tmax +Tmin )/2, and substitute the current running time tf into step two, and return to step two to update the coefficient sum of the fifth degree polynomial. Motion parameter limit value, judge again whether the motion parameter limit value satisfies the constraint condition. If the constraint condition is met, the upper limit of the adjustment interval is tf . If not, the lower limit of the adjustment interval is tf . After the interval adjustment, the next iteration is performed until until the accuracy is met. Finally, the optimal time tf =3.18203125s is obtained. The change of running time with the number of iterations is shown in Figure 3. The obtained optimal time tf is rounded up to an integer multiple of the interpolation period. Assuming that the robot interpolation period is 0.002s, the optimal running time is adjusted to tf =3.184s.
步骤五：根据获得的最优运行时间和初末运动状态参数解出五次多项式的系数，确定五次多项式插值函数：Step 5: Solve the coefficients of the quintic polynomial based on the obtained optimal running time and initial and final motion state parameters, and determine the quintic polynomial interpolation function:
将步骤四获得的最优运行时间tf代入，式求解得到五次多项式系数a0＝0,a1＝10,a2＝0,a3＝95.406939,a4＝-44.226857,a5＝5.543618；则五次多项式p(t)＝10t+95.406939t3-44.226857t4+5.543618t5，对应的位移曲线、速度曲线、加速度曲线、和加加速度曲线如图4a-4d所示，其中虚线部分为给定运行时间为4s且不进行优化的对照。Substitute the optimal running time tf obtained in step 4 into the equation: The solution is to obtain the fifth-degree polynomial coefficients a0 =0, a1 =10, a2 =0, a3 =95.406939, a4 =-44.226857, a5 =5.543618; then the fifth-degree polynomial p(t) =10t+95.406939t3 -44.226857t4 +5.543618t5 , the corresponding displacement curve, velocity curve, acceleration curve, and jerk curve are shown in Figure 4a-4d, where the dotted line part is the control where the given running time is 4s and no optimization is performed.
对五次多项式定时插补，按照已知的插补周期对五次多项式p(t)进行定时插补得到五次多项式位移序列，t＝n*Δt，其中为周期个数，Δt为插补周期。For the fifth-order polynomial timed interpolation, the fifth-order polynomial p(t) is timed and interpolated according to the known interpolation period to obtain the fifth-order polynomial displacement sequence, t=n*Δt, where is the number of cycles, Δt is the interpolation period.
再根据位移序列，将插补得到的位移序列匹配对应到笛卡尔空间中的已有的实际轨迹路径,得到实际插补的轨迹点，P0＝(0,500,500),P1＝(0.0200,500,500),P2＝(0.0400,500,500),Then according to the displacement sequence, match the displacement sequence obtained by interpolation to the existing actual trajectory path in Cartesian space, and obtain the actual interpolated trajectory points, P0 = (0,500,500), P1 = (0.0200,500,500) ,P2 =(0.0400,500,500),
P3＝(0.0600,500,500),…,P1592＝(379.9984,500,500)；获得的轨迹序列图如图6所示。P3 =(0.0600,500,500),…,P1592 =(379.9984,500,500); the obtained trajectory sequence diagram is shown in Figure 6.
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当说明书作为一个整体，实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。In addition, it should be understood that although this specification is described in terms of implementations, not each implementation only contains an independent technical solution. This description is only for the sake of clarity. Those skilled in the art should take the description as a whole. The technical solutions in the embodiments can also be appropriately combined to form other implementations that can be understood by those skilled in the art.
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310757108.4ACN116880363A (en) | 2023-06-26 | 2023-06-26 | Industrial robot time optimal five-order polynomial speed planning method |
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310757108.4ACN116880363A (en) | 2023-06-26 | 2023-06-26 | Industrial robot time optimal five-order polynomial speed planning method |
| Publication Number | Publication Date |
|---|---|
| CN116880363Atrue CN116880363A (en) | 2023-10-13 |
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310757108.4APendingCN116880363A (en) | 2023-06-26 | 2023-06-26 | Industrial robot time optimal five-order polynomial speed planning method |
| Country | Link |
|---|---|
| CN (1) | CN116880363A (en) |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117826806A (en)* | 2023-12-25 | 2024-04-05 | 江苏金陵智造研究院有限公司 | Multi-constraint single-steering-wheel forklift smooth control method |
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117826806A (en)* | 2023-12-25 | 2024-04-05 | 江苏金陵智造研究院有限公司 | Multi-constraint single-steering-wheel forklift smooth control method |
| Publication | Publication Date | Title |
|---|---|---|
| CN111897216A (en) | A Multi-segment Velocity Planning and Interpolation Method | |
| CN109416529B (en) | Numerical control system, device with storage function and NURBS curve interpolation method | |
| CN112975992B (en) | Error-controllable robot track synchronous optimization method | |
| CN106393106B (en) | The robot nurbs curve of parameter adaptive densification moves interpolating method | |
| CN113204215B (en) | A real-time interpolation method of global NURBS trajectory in NC machining and its application | |
| CN106945042A (en) | A kind of discrete S types curve speed control method of manipulator | |
| CN107450471A (en) | A kind of method that cutter path parameter arc length is realized based on cubic PH curve interpolation | |
| CN107450472A (en) | A kind of method that cutter path parameter arc length is realized based on three bezier curve interpolation | |
| CN110865610A (en) | Tool path interpolation and speed planning method based on machine tool vibration suppression | |
| CN108568817A (en) | A kind of Delta robot trajectories connection control method based on Bezier | |
| CN113478489B (en) | A Robotic Arm Trajectory Planning Method | |
| CN114237161B (en) | A NURBS curve interpolation method for industrial robots based on digital filtering | |
| CN105955194A (en) | A Knee Point Smoothing Method for Discrete Machining Paths | |
| CN116880363A (en) | Industrial robot time optimal five-order polynomial speed planning method | |
| CN108489015B (en) | Air conditioning system temperature control method based on pole allocation and Pade approximation | |
| CN113253678B (en) | Motion trail planning method for precision motion platform | |
| CN109164759B (en) | Curve interpolation method, equipment and computer readable storage medium | |
| CN105843174B (en) | A kind of spline curve interpolation parameters computational methods | |
| CN108099908B (en) | Vehicle self-adaptive cruise optimization control calculation method | |
| CN109597962A (en) | A kind of parameter interpolation method based on prediction and iterative compensation | |
| CN109283892B (en) | Feed rate adaptive interpolation algorithm based on parametric curve geometric characteristics and bow-height error limitation | |
| WO2020093401A1 (en) | Method for controlling motion of device, device, and storage apparatus | |
| CN117001672A (en) | An energy-optimal trajectory planning method for robotic arms based on SMA algorithm | |
| CN115202293B (en) | Two-section type speed planning method for industrial robot | |
| CN114879678A (en) | Distance weight-based global path smoothing method for mobile robot |
| 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 |