本发明属于机械臂运动控制领域，具体涉及一种基于三段等时长三次多项式的机械臂自由边界运动规划方法。
背景技术
机械臂在工业领域中有着广泛地运用，而根据操作目标的状态可分为静态目标操作和动态目标操作。对于静态目标操作，机械臂运动规划的末状态也为静止状态，因此运动规划中的末边界都为固定的零值。但针对于动态目标操作，为跟踪目标运动并达到相同的运动状态，以实现精确的动态操作，运动规划中的末边界就必须为非固定的自由值。因此，研究一种精确性高、稳定性强、模型参数计算快速的自由边界运动规划方法，可以有效地解决动态目标操作问题，提高机械臂动态操作的稳定性与精确性。
技术实现要素：
本发明为控制机械臂在满足运动学约束的前提下，从带有任意速度和加速度的任意当前位姿到达带有任意速度和加速度的任意目标位姿，提出一种基于三段等时长三次多项式的机械臂自由边界运动规划方法。该方法包括以下步骤：
步骤(1)：单维度带约束运动模型建立
对于单维度的三段等时长三次多项式模型：
其中ai，bi，ci和di是每一段三次多项式的系数，te为每一段三次多项式模型的时长。
1)为达到给定的边界值，根据给定的始末位置、速度和加速度，建立边界约束。对于当前位置为θc，当前速度为vc，当前加速度为ac，建立边界连续约束：
其中d1、c1、b1是第一段三次多项式的三个系数；
对于目标位置为θg，目标速度为vg，目标加速度为ag，建立边界连续约束：
其中d3、c3、b3、a3是第三段三次多项式的四个系数；
2)为确保运动模型的整体连续性，连接内部每段模型的始末边界，建立过程连续约束：
3)为保证机械臂运动的稳定，根据给定的速度运动学约束cv，加速度运动学约束ca，加加速度运动学约束cj，建立运动学约束。
速度模型由三段二次多项式构成，加速度模型由三段一次多项式构成，加加速度模型由三段常数构成。因此，对于加速度与加加速度，最大值只存在于边界点。而对于速度模型，最大值可能存在于可行区间内的任意点。因此，在建立速度约束时不仅考虑边界值约束，还需求解过程中的最大值并建立约束。
先建立速度、加速度和加加速度的过程边界运动学约束：
求取得满足边界运动学约束的单段时长te后，若该区间内不存在最值，则无需再考区间内的速度运动学约束。否则，需要求解该区间内的速度最值。但求解每一段速度最值时，涉及高次多项式求根问题，难以解出并选择正确的单段时长te。因此，采用迭代逼近的方法，求取满足整体速度运动学约束的单段时长te。求取每段速度模型的最值所在点tm和速度最值vm：
若最值点在单段时长te的区间内，即0＜tm＜te，且速度最值|vm|大于速度运动学约束cv，即|vm|＞cv，则迭代增加单段时长te，直至所有三段模型的速度最值|vm|都不大于速度运动学约束cv。具体迭代方法：
te＝kte,k＞1
其中，k为单段时长te的增加系数。
1)与2)求得三段等时长三次多项式的全部多项式系数，3)求得确保整体运动都满足运动学约束的每段时长，以此构建单维度上带约束的连续三段等时长三次多项式模型。
步骤(2)：多维度协同运动与模型建立
机械臂都具有多个自由度，因此需要对每一维度都需要进行运动规划。对于多维度的三段等时长三次多项式模型：
其中，n为维度数量。
在单维度模型的基础上，为实现机械臂的多维度协同运动，需要对每一维度都建立运动规划，再选取最大的运动时间作为整体运动时间，以确保任意维度上的运动都符合运动学约束。
以整体运动时间
本发明相对于与现有技术具有的有益效果：本发明以始末自由位置、速度和加速度边界及运动学约束为输入，实现机械臂的运动规划，使机械臂末端执行器最终带有指定的速度和加速度状态。本发明中能实现快速模型系数求解，可适用于实时性要求较高的运动规划中，能运用于任意自由度的机械臂，且对于静态与动态目标都能适用，具有较为广泛的可移植性。
具体实施方式
本发明基于三段三次多项式的机械臂自由边界运动规划方法，该方法的具体步骤是：
步骤(1)：单维度带约束运动模型建立
对于单维度的三段等时长三次多项式模型：
其中ai，bi，ci和di是每一段三次多项式的系数，te为每一段三次多项式模型的时长。
1)为达到给定的边界值，根据给定的始末位置、速度和加速度，建立边界约束。对于当前位置为θc，当前速度为vc，当前加速度为ac，建立边界连续约束：
其中d1、c1、b1是第一段三次多项式的三个系数；
对于目标位置为θg，目标速度为vg，目标加速度为ag，建立边界连续约束：
其中d3、c3、b3、a3是第三段三次多项式的四个系数；
2)为确保运动模型的整体连续性，连接内部每段模型的始末边界，建立过程连续约束：
3)为保证机械臂运动的稳定，根据给定的速度运动学约束cv，加速度运动学约束ca，加加速度运动学约束cj，建立运动学约束。
速度模型由三段二次多项式构成，加速度模型由三段一次多项式构成，加加速度模型由三段常数构成。因此，对于加速度与加加速度，最大值只存在于边界点。而对于速度模型，最大值可能存在于可行区间内的任意点。因此，在建立速度约束时不仅考虑边界值约束，还需求解过程中的最大值并建立约束。
先建立速度、加速度和加加速度的过程边界运动学约束：
求取得满足边界运动学约束的单段时长te后，若该区间内不存在最值，则无需再考区间内的速度运动学约束。否则，需要求解该区间内的速度最值。但求解每一段速度最值时，涉及高次多项式求根问题，难以解出并选择正确的单段时长te。因此，采用迭代逼近的方法，求取满足整体速度运动学约束的单段时长te。求取每段速度模型的最值所在点tm和速度最值vm：
若最值点在单段时长te的区间内，即0＜tm＜te，且速度最值|vm|大于速度运动学约束cv，即|vm|＞cv，则迭代增加单段时长te，直至所有三段模型的速度最值|vm|都不大于速度运动学约束cv。具体迭代方法：
te＝kte,k＞1
其中，k为单段时长te的增加系数。
1)与2)求得三段等时长三次多项式的全部多项式系数，3)求得确保整体运动都满足运动学约束的每段时长，以此构建单维度上带约束的连续三段等时长三次多项式模型。
步骤(2)：多维度协同运动与模型建立
机械臂都具有多个自由度，因此需要对每一维度都需要进行运动规划。对于多维度的三段等时长三次多项式模型：
其中，n为维度数量。
在单维度模型的基础上，为实现机械臂的多维度协同运动，需要对每一维度都建立运动规划，再选取最大的运动时间作为整体运动时间，以确保任意维度上的运动都符合运动学约束。
以整体运动时间