一种关节空间连续点轨迹过渡方法及装置与流程

本发明涉及机器人运动控制领域，更具体涉及一种关节空间连续点轨迹过渡方法及装置。

背景技术：

机械臂运动空间分为关节空间和笛卡尔空间，当前工业机械臂在单独关节空间和笛卡尔空间运动已经十分容易实现。关节空间过渡的主要方法是将关节空间通过正向运动学转到笛卡尔空间，再将笛卡尔空间与笛卡尔空间指令做轨迹过渡，比如在交点处做降速为零的处理，然后切换空间再进行运动，这样在实际生产中会大大增加时间成本和能耗成本。例如，对于通用机器人的关节空间规划，也就是ptp，每个关节从起始点位到终止点位，往往是单独规划运动，各个关节能在同一时间启动，能到达指定点位。这样简单的关节运动对于运动过程控制要求不高，并且有快速定位要求的作业是适用的。但是对于连续关节空间规划运动，如果采用各关节独立运动，会出现各关节从运动起始点，到中间点，最后到目标点，速度从0加速，再到0，再加速，再到0，这种连续启停运动，每出现一个中间点和目标点，各关节就要启停一次。这样会造成运动过程不可控，各关节无法同步到中间点位置，也就是无法实现避开固定点障碍物功能，效率会大大降低，增加了时间成本和能耗成本，不适合实际作业的要求。

现有关节空间和笛卡尔空间的混合空间过渡的主要方法还包括映射到笛卡尔空间后做圆弧过渡算法，但是圆弧过渡算法曲率是常量，会有曲率突变，造成向心加速度的冲击，降低运动精度。

技术实现要素：

本发明所要解决的技术问题在于关节空间连续点轨迹过渡方法时间成本和能耗成本高的问题。

本发明通过以下技术手段实现解决上述技术问题的：一种关节空间连续点轨迹过渡方法，所述方法包括：

步骤一：获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度，其中，贝塞尔过渡的入射速度和出射速度相等；

步骤二：若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；

步骤三：获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；

步骤四：获取关节空间末速度；

步骤五：实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤一至步骤四，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；

步骤六：利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；

步骤七：获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；

步骤八：将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。

本发明采用贝塞尔过渡法，对关节空间末速度进行修正，使得最终过渡段可以保证加速度、速度连续，圆弧相切，不需要速度从0加速，再到0，再加速，再到0这种连续启停运动，减少时间成本以及能耗成本。贝塞尔过渡的入射速度和出射速度、过渡半径以及起始点终点等均可修正，因此曲率可以设置，防止过大的连接速度对机械结构产生冲击，减少能耗成本。

优选的，所述步骤二包括：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径。

优选的，所述步骤三还包括：

利用公式q′2p0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点p0，

利用公式q′2p5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点p5。

优选的，所述步骤四包括：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。

优选的，所述步骤五包括：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；

修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤一至步骤四，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。

优选的，所述步骤七包括：分别取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置q1与中间位置q2之间的夹角，θ2为中间位置q2与结束位置q3之间的夹角；

利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。

本发明还提供一种关节空间连续点轨迹过渡装置，所述装置包括：

第一获取模块，用于获取关节空间的开始位置、中间位置、结束位置以及各位置允许运行的速度中的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度，其中，贝塞尔过渡的入射速度和出射速度相等；

判断模块，用于若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；

第二获取模块，用于获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；

第三获取模块，用于获取关节空间末速度；

修正模块，用于实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；

第四获取模块，用于利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；

时间同步模块，用于获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；

规划模块，用于将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。

优选的，所述判断模块还用于：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径。

优选的，所述第二获取模块还用于：

利用公式q′2p0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点p0，

利用公式q′2p5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点p5。

优选的，所述第三获取模块还用于：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。

优选的，所述修正模块还用于：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；

修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。

优选的，所述时间同步模块还用于：分别取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置q1与中间位置q2之间的夹角，θ2为中间位置q2与结束位置q3之间的夹角；

利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。

本发明的优点在于：本发明采用贝塞尔过渡法，对关节空间末速度进行修正，使得最终过渡段可以保证加速度、速度连续，圆弧相切，前一段与后一段关节运动速度连续，不存在到达中间点或者目标点的时候为了与下一段关节运动速度保持一致将速度降为0，再在下一段开始时重新启动，因此不需要速度从0加速，再到0，再加速，再到0这种连续启停运动，减少时间成本以及能耗成本。贝塞尔过渡的入射速度和出射速度、过渡半径以及起始点终点等均可修正，因此曲率可以设置，防止过大的连接速度对机械结构产生冲击，减少能耗成本。

附图说明

图1为本发明实施例所公开的一种关节空间连续点轨迹过渡方法的流程示意图；

图2为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中五阶贝塞尔过渡段速度曲线示意图；

图3为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间ab点过度前半段示意图；

图4为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间ab点过度后半段示意图；

图5为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间bc点过度前半段示意图；

图6为本发明实施例所公开的一种关节空间连续点轨迹过渡方法中关节空间bc点过度后半段示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

实施例1

如图1和图2所示，一种关节空间连续点轨迹过渡方法，所述方法包括：

步骤s1：获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度，其中，贝塞尔过渡的入射速度和出射速度相等；

步骤s2：若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；具体过程为：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径，||为向量的模的符号，x1为第一贝塞尔直线长度，x2为第二贝塞尔直线长度，per为预设的百分比。

步骤s3：获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；具体过程为：

利用公式q′2p0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点p0，

利用公式q′2p5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点p5。

以下简要介绍贝塞尔过渡计算过程，相邻两条直线之间采用五次贝塞尔曲线作为过渡轨迹，五次贝塞尔曲线的6个控制器点p0，p1，p2，p3，p4，p5中，起始点p0和终点p5为以两条直线的交点为圆心，过渡精度为半径画圆，相交形成的交点，两条直线形成的交点作为另外两个控制点p2，p3，即p2与p3重合，p1是p0与p2的中点，p4是p3与p5的中点。将6个控制点坐标(px，py，pz)，代入以下公式：

b(t)＝p0(1-t)⁵+5p1t(1-t)⁴+10p2t²(1-t)³+10p3t³(1-t)²+5p4t⁴(1-t)+p5t⁵

t∈[0，1]

可得到贝塞尔曲线的表达式在各个分量的坐标。

bx(t)＝p0x(1-t)⁵+5p1xt(1-t)⁴+10p2xt²(1-t)³+10p3xt³(1-t)²+5p4xt⁴(1-t)+p5xt⁵

by(t)＝p0y(1-t)⁵+5p1yt(1-t)⁴+10p2yt²(1-t)³+10p3yt³(1-t)²+5p4yt⁴(1-t)+p5yt⁵

bz(t)＝p0z(1-t)⁵+5p1zt(1-t)⁴+10p2zt²(1-t)³+10p3zt³(1-t)²+5p4zt⁴(1-t)+p5zt⁵

其中p0x、p0y、p0z为p0点在x轴、y轴、z轴的投影点，同理可得p1、p2、p3、p4、p5点的投影点。

步骤s4：获取关节空间末速度；具体过程为：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。

步骤s5：实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤s1至步骤s4，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；具体过程为：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；

修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行步骤s1至步骤s4，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。

步骤s6：利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；

步骤s7：获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；具体过程为：分别取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置q1与中间位置q2之间的夹角，θ2为中间位置q2与结束位置q3之间的夹角；

利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。

步骤s8：将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。

如图3为本发明的关节空间ab点过度前半段示意图，图4为关节空间ab点过度后半段示意图，图5为关节空间bc点过度前半段示意图，图6关节空间bc点过度后半段示意图，图中可以看出，本发明的轨迹过渡方法使得轨迹连续，圆弧相切。

通过以上技术方案，本发明提供的一种关节空间连续点轨迹过渡方法，采用五次贝塞尔过渡法，对关节空间末速度进行修正，使得最终过渡段可以保证加速度、速度连续，圆弧相切，不需要速度从0加速，再到0，再加速，再到0这种连续启停运动，减少时间成本以及能耗成本。贝塞尔过渡的入射速度和出射速度、过渡半径以及起始点终点等均可修正，因此曲率可以设置，防止过大的连接速度对机械结构产生冲击，减少能耗成本。

实施例2

与实施例1相对应的，本发明实施例2还提供一种关节空间连续点轨迹过渡装置，所述装置包括：

第一获取模块，用于获取关节空间的开始位置、中间位置以及结束位置的最大速度，最大速度乘以预设的百分比作为贝塞尔过渡的入射速度和出射速度，其中，贝塞尔过渡的入射速度和出射速度相等；

判断模块，用于若预设的百分比为零，分别对前后两段关节空间进行单独的轨迹规划，不进行贝塞尔过渡；若预设的百分比不为零，则进行贝塞尔过渡；

第二获取模块，用于获取贝塞尔过渡的起始点以及贝塞尔过渡的终点；

第三获取模块，用于获取关节空间末速度；

修正模块，用于实际可达的关节空间末速度与关节空间末速度误差大于预设数值时，修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，对关节空间末速度进行修正，直至实际可达的关节空间末速度与关节空间末速度误差小于等于预设数值；

第四获取模块，用于利用关节空间的开始位置、中间位置以及终点位置通过贝塞尔过渡原理获取关于位置的贝塞尔过渡时间；

时间同步模块，用于获取贝塞尔过渡起始姿态和贝塞尔过渡结束姿态，并对贝塞尔过渡起始姿态和贝塞尔过渡结束姿态进行修正；使用修正后的姿态，对两姿态夹角进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步；

规划模块，用于将贝塞尔过渡的入射速度作为前一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的起始点作为前一条关节空间轨迹与贝塞尔过渡交点位置，将贝塞尔过渡的出射速度作为下一条关节空间轨迹与贝塞尔过渡交点处需要的速度，贝塞尔过渡的终点作为下一条关节空间轨迹与贝塞尔过渡交点位置，完成关节空间连续点轨迹过渡。

具体的，所述判断模块还用于：若预设的百分比不为零，将关节空间的开始位置q1、中间位置q2以及结束位置q3通过映射关系映射到笛卡尔空间得到笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3，完成关节空间到笛卡尔空间的切换，取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的位置信息组成开始位置点q1'、中间位置点q2'以及结束位置点q3'，由开始位置点q1'与中间位置点q2'构建第一向量q2'q1'，由中间位置点q2'以及结束位置点q3'构建第二向量q2'q3'，在前后两段关节空间上利用公式x1＝per×|q′2q′1|计算出第一贝塞尔直线长度，利用公式x2＝per×|q′2q′3|计算出第二贝塞尔直线长度，取第一贝塞尔直线长度与第二贝塞尔直线长度中值较小的为过渡半径。

具体的，所述第二获取模块还用于：

利用公式q′2p0＝q′2q′1×x1/|q′2q′1|获取贝塞尔过渡的起始点p0，

利用公式q′2p5＝per×q′2q′3×x2/|q′2q′3|获取贝塞尔过渡的终点p5。

具体的，所述第三获取模块还用于：根据预设的贝塞尔过渡的入射速度和出射速度，利用贝塞尔过渡原理获取贝塞尔过渡允许输入的最大速度大小，取贝塞尔过渡的入射速度、出射速度以及允许输入的最大速度中最小的速度作为笛卡尔空间的初速度，根据笛卡尔空间的初速度利用雅克比矩阵获取关节空间末速度。

具体的，所述修正模块还用于：将笛卡尔空间的初速度向量乘以系数保证速度方向不变，只修改速度大小，通过雅可比矩阵求对应的关节空间末速度，输入关节空间末速度返回实际可达的关节空间末速度，判断每个关节空间末速度误差，当每个实际可达的关节空间末速度与输入的关节空间末速度误差大于预设数值时，即为失败，继续进行笛卡尔空间的初速度的迭代修改，修改系数使得系数累加或者累减，当超出最大修改次数，仍然返回失败时，说明关节空间末速度无法到达与笛卡尔空间相切的速度大小，需要修改关节空间的开始位置、中间位置以及终点位置；

修改关节空间的开始位置、中间位置以及终点位置，修改后的关节空间的开始位置、中间位置以及终点位置作为关节空间的开始位置、中间位置以及结束位置，返回执行第一获取模块至第三获取模块，关节空间末速度误差小于等于预设数值时，即为成功，采用此时获取的关节空间末速度作为关节空间末速度，保证关节空间到笛卡尔空间速度连续，过渡轨迹曲线相切。

具体的，所述时间同步模块还用于：分别取笛卡尔空间的开始位置q1、中间位置q2以及结束位置q3的姿态信息组成三点的欧拉角，将三点的欧拉角分别转换为四元数得到开始四元数q″1、中间四元数q″2以及终点四元数q″3，根据预设的百分比，采用四元数差值法，通过公式qs＝q″1sin(perθ1)+q″2sin(θ1-perθ1)sinθ1获取贝塞尔过渡起始姿态qs，通过公式qe＝q″3sin(perθ2)+q″2sin(θ2-perθ2)sinθ2获取贝塞尔过渡结束姿态qe，其中，θ1为开始位置q1与中间位置q2之间的夹角，θ2为中间位置q2与结束位置q3之间的夹角；

利用公式q′s＝qs对贝塞尔过渡起始姿态进行修正，利用公式q′e＝qssin(perθ3)+q″2sin(θ3-perθ3)sinθ3对贝塞尔过渡结束姿态进行修正，其中，θ3为qs和q″2之间的夹角，q′s为修正后的起始点姿态，q′e为修正后的终点姿态，使用修正后的姿态，对两姿态夹角θ3进行s形速度规划，获取关于姿态的贝塞尔过渡时间，并与关于位置的贝塞尔过渡时间进行比较，最大时间作为基准时间，做位置姿态时间同步。

以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

起点商标作为专业知识产权交易平台，可以帮助大家解决很多问题，如果大家想要了解更多知产交易信息请点击 【在线咨询】或添加微信 【19522093243】与客服一对一沟通，为大家解决相关问题。

此文章来源于网络,如有侵权,请联系删除