一种基于正弦曲线的机器人圆弧轨迹规划方法与流程

本发明涉及轨迹规划技术领域，更具体的，涉及一种基于正弦曲线的机器人圆弧轨迹规划方法。

背景技术：

机器人在运动过程中，需要机器人的各个关节协同运动才能使机器人的末端执行器达成需要的状态，包括空间位置和姿态。但是，机器人的关节个数越多，分解到各个关节的运动计算就越复杂；再加上自定义空间轨迹参数的变化比较复杂，导致每次规划机器人运动的圆弧轨迹都需要重新进行大量的计算。因此，目前在规划机器人的运动轨迹时大多都不考虑机器人的关节角度变化，规划效果不够好。

现有技术中，如2020年05月19日公开的中国专利，一种圆弧轨迹插补的方法，公开号为cn111176482a，通过对设备结构及位置信息采集，建立x\y轴坐标系，进行插补点的设定，实现圆弧触摸检测的轨迹设定，但没有考虑机器人的关节角度变化，不适用于规划机器人的运动轨迹。

技术实现要素：

本发明为克服目前在规划机器人的运动轨迹时大多都不考虑机器人的关节角度变化的技术缺陷，提供一种基于正弦曲线的机器人圆弧轨迹规划方法。

为解决上述技术问题，本发明的技术方案如下：

一种基于正弦曲线的机器人圆弧轨迹规划方法，包括以下步骤：

s1：选定不在同一直线的三个位置点并获取所述位置点在机器人坐标系下的位姿；

s2：根据所述位置点的位姿求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；

s3：以插补圆的圆心为原点建立圆弧坐标系，并获取圆弧坐标系和机器人坐标系之间的转换关系；

s4：设定圆弧轨迹的当前插补步数k和插补总步数m，并令k=0；其中，k=0，1，…，m；

s5：基于正弦曲线计算第k步插补时机器人末端执行器在圆弧坐标系中的位姿分速度；

s6：根据转换关系，将机器人末端执行器在圆弧坐标系中的位姿分速度转换到机器人坐标系下表示，并由位姿分速度计算出机器人关节的角速度；

s7：根据机器人关节的角速度计算出机器人关节的角度，从而得到第k步插补时机器人关节的角度变化；

s8：判断当前插补步数k是否等于插补总步数m；

若不等于，则将k+1的值赋予k，并返回步骤s5；

若等于，则结束迭代，完成圆弧轨迹的规划。

上述方案中，首先由三个不共线的位置点确定插补圆，然后基于正弦曲线求解机器人末端执行器的位姿分速度，再由机器人末端执行器的位姿分速度计算出机器人关节的角速度，进而求解出机器人关节的角度，最后通过迭代循环至当前插补步数k达到插补总步数m，完成圆弧轨迹的规划，实现了根据机器人的关节角度变化来规划机器人的圆弧运动轨迹，提高了圆弧轨迹的规划效果。

优选的，在步骤s1中，三个所述位置点的位姿分别为，，；

其中，位置点在机器人坐标系下的坐标为，位置点在机器人坐标系下的姿态为；位置点在机器人坐标系下的坐标为，位置点在机器人坐标系下的姿态为；位置点在机器人坐标系下的坐标为，位置点在机器人坐标系下的姿态为。

优选的，步骤s2具体为：

根据所述位置点在机器人坐标系下的坐标求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；

设插补圆在机器人坐标系下的圆心坐标为，半径为，根据圆心坐标和半径的关系得到方程组：

。

优选的，在步骤s3中，圆弧坐标系的x轴、y轴、z轴分别为、、，其中，为插补圆圆心指向位置点点的向量，为过插补圆圆心的法向量，为和根据右手定则的方向向量。

优选的，在步骤s5中，位姿分速度包括移动分速度和姿态角速度。

优选的，基于正弦曲线计算机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度的步骤如下：

s5.1.1：计算机器人末端执行器在时的移动量、绕插补圆圆心转动的角度和第k步的插补移动量：

其中，插补时间，为每步插补所需的时间，为插补圆的半径，为到时机器人末端执行器绕回转轴转动的角度，为过插补圆圆心的法向量；

s5.1.2：计算时刻机器人末端执行器分别在圆弧坐标系的三个坐标轴上的位置分量：

；

其中，分别对应时机器人末端执行器在圆弧坐标系的x轴、y轴、z轴上的位置分量；

s5.1.3：计算到时机器人末端执行器绕回转轴旋转的角速度：

；

s5.1.4：通过将进行微分得到第k步插补时机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度：

；

其中，为第k步插补时机器人末端执行器在圆弧坐标系的x轴上的移动分速度，为第k步插补时机器人末端执行器在圆弧坐标系的y轴上的移动分速度，为第k步插补时机器人末端执行器在圆弧坐标系的z轴上的移动分速度。

优选的，基于正弦曲线计算机器人末端执行器的姿态角速度的步骤如下：

s5.2.1：设机器人末端执行器从起始位置运动到目标位置的总姿态偏转角度为，将其姿态偏转的角度按正弦曲线插入，则时刻的姿态偏转角度为：

；

其中，插补时间，为每步插补所需的时间，为运动总时间；

在圆弧坐标系中，时刻由机器人末端执行器绕回转轴旋转引起的姿态旋转变化矩阵为：

；

时刻机器人末端执行器的姿态偏转变化矩阵为：

；

则时刻的姿态变化矩阵为：

；

s5.2.2：计算从时刻到时刻的第k步姿态变化矩阵：

；

其中，为时刻的姿态变化矩阵；

s5.2.3：将机器人末端执行器从时刻到时刻的姿态变化认为是绕某一旋转轴旋转得到，通过轴-角的关系变换得到旋转轴和角度变量：

；

其中，为里第一行第一列的元素，为里第一行第二列的元素，为里第一行第三列的元素，为里第二行第一列的元素，为里第二行第二列的元素，为里第二行第三列的元素，为里第三行第一列的元素，为里第三行第二列的元素，为里第三行第三列的元素，为旋转轴在圆弧坐标系的x轴上的分量，为旋转轴在圆弧坐标系的y轴上的分量，为旋转轴在圆弧坐标系的z轴上的分量；

s5.2.4：由旋转轴求取的等效旋转矩阵：

；

将等效旋转矩阵进行微分变换得到：

；

其中，，，，为的微分形式，为的微分值；

s5.2.5：由微分定义得到时刻的微分：

；

；

其中，为时间微分，为单位矩阵；

s5.2.6：计算机器人末端执行器从时刻到时刻的姿态变化的角速度：

；

将代入，得到机器人末端执行器的姿态角速度为：

。

优选的，在步骤s6中，时刻机器人末端执行器在圆弧坐标系下的位姿分速度为：

；

根据圆弧坐标系和机器人坐标系之间的转换关系，得到时刻机器人末端执行器在机器人坐标系下的位姿分速度：

；

其中，为机器人末端执行器在圆弧坐标系的x轴上的姿态角速度分量，为机器人末端执行器在圆弧坐标系的y轴上的姿态角速度分量，为机器人末端执行器在圆弧坐标系的z轴上的姿态角速度分量。

优选的，在步骤s6中，由位姿分速度计算出机器人关节的角速度：

；

其中，为机器人末端执行器和关节的速度雅可比矩阵，，为机器人第一关节的角速度，为机器人第二关节的角速度，为机器人第三关节的角速度，为机器人第四关节的角速度，为机器人第五关节的角速度，为机器人第六关节的角速度。

优选的，在步骤s7中，通过对机器人关节的角速度进行积分计算得到机器人关节的角度：

；

其中，为第k步插补时机器人关节转动的角度，插补时间，为每步插补所需的时间。

与现有技术相比，本发明技术方案的有益效果是：

本发明提供了一种基于正弦曲线的机器人圆弧轨迹规划方法，首先由三个不共线的位置点确定插补圆，然后基于正弦曲线求解机器人末端执行器的位姿分速度，再由机器人末端执行器的位姿分速度计算出机器人关节的角速度，进而求解出机器人关节的角度，最后通过迭代循环至当前插补步数k达到插补总步数m，完成圆弧轨迹的规划，实现了根据机器人的关节角度变化来规划机器人的圆弧运动轨迹，提高了圆弧轨迹的规划效果。

附图说明

图1为本发明的技术方案实施步骤流程图；

图2为本发明中圆弧坐标系的示意图；

图3为本发明中机器人末端执行器在圆弧轨迹中的姿态示意图；

图4为本发明中机器人关节在圆弧轨迹中的角度-时间关系示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

如图1所示，一种基于正弦曲线的机器人圆弧轨迹规划方法，包括以下步骤：

s1：选定不在同一直线的三个位置点并获取所述位置点在机器人坐标系下的位姿；

s2：根据所述位置点的位姿求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；

s3：以插补圆的圆心为原点建立圆弧坐标系，并获取圆弧坐标系和机器人坐标系之间的转换关系；

s4：设定圆弧轨迹的当前插补步数k和插补总步数m，并令k=0；其中，k=0，1，…，m；

s5：基于正弦曲线计算第k步插补时机器人末端执行器在圆弧坐标系中的位姿分速度；

s6：根据转换关系，将机器人末端执行器在圆弧坐标系中的位姿分速度转换到机器人坐标系下表示，并由位姿分速度计算出机器人关节的角速度；

s7：根据机器人关节的角速度计算出机器人关节的角度，从而得到第k步插补时机器人关节的角度变化；

s8：判断当前插补步数k是否等于插补总步数m；

若不等于，则将k+1的值赋予k，并返回步骤s5；

若等于，则结束迭代，完成圆弧轨迹的规划。

在具体实施过程中，首先由三个不共线的位置点确定插补圆，然后基于正弦曲线求解机器人末端执行器的位姿分速度，再由机器人末端执行器的位姿分速度计算出机器人关节的角速度，进而求解出机器人关节的角度，最后通过迭代循环至当前插补步数k达到插补总步数m，完成圆弧轨迹的规划，实现了根据机器人的关节角度变化来规划机器人的圆弧运动轨迹，使机器人能够完成一个完整的圆弧轨迹运动，提高了圆弧轨迹的规划效果。

更具体的，在步骤s1中，三个所述位置点的位姿分别为，，；

其中，位置点在机器人坐标系下的坐标为，位置点在机器人坐标系下的姿态为；位置点在机器人坐标系下的坐标为，位置点在机器人坐标系下的姿态为；位置点在机器人坐标系下的坐标为，位置点在机器人坐标系下的姿态为。

在具体实施过程中，机器人坐标系由机器人建立，能够从机器人中直接获得。

在具体实施过程中，位姿包括位置和姿态，位置为位置点在机器人坐标系下的坐标。

更具体的，步骤s2具体为：

根据所述位置点在机器人坐标系下的坐标求解得到经过三个所述位置点的插补圆的所在平面、圆心坐标和半径；

设插补圆在机器人坐标系下的圆心坐标为，半径为，根据圆心坐标和半径的关系得到方程组：

。

更具体的，如图2所示，在步骤s3中，圆弧坐标系的x轴、y轴、z轴分别为、、，其中，为插补圆圆心指向位置点点的向量，为过插补圆圆心的法向量，为和根据右手定则的方向向量。

更具体的，在步骤s5中，位姿分速度包括移动分速度和姿态角速度。

更具体的，基于正弦曲线计算机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度的步骤如下：

s5.1.1：计算机器人末端执行器在时的移动量、绕插补圆圆心转动的角度和第k步的插补移动量：

其中，插补时间，为每步插补所需的时间，为插补圆的半径，为到时机器人末端执行器绕回转轴转动的角度，为过插补圆圆心的法向量；

s5.1.2：计算时刻机器人末端执行器分别在圆弧坐标系的三个坐标轴上的位置分量：

；

其中，分别对应时机器人末端执行器在圆弧坐标系的x轴、y轴、z轴上的位置分量；

s5.1.3：计算到时机器人末端执行器绕回转轴旋转的角速度：

；

s5.1.4：通过将进行微分得到第k步插补时机器人末端执行器分别在圆弧坐标系的三个坐标轴上的移动分速度：

；

其中，为第k步插补时机器人末端执行器在圆弧坐标系的x轴上的移动分速度，为第k步插补时机器人末端执行器在圆弧坐标系的y轴上的移动分速度，为第k步插补时机器人末端执行器在圆弧坐标系的z轴上的移动分速度。

更具体的，基于正弦曲线计算机器人末端执行器的姿态角速度的步骤如下：

s5.2.1：设机器人末端执行器从起始位置运动到目标位置的总姿态偏转角度为，将其姿态偏转的角度按正弦曲线插入，则时刻的姿态偏转角度为：

；

其中，插补时间，为每步插补所需的时间，为运动总时间；

在圆弧坐标系中，时刻由机器人末端执行器绕回转轴旋转引起的姿态旋转变化矩阵为：

；

时刻机器人末端执行器的姿态偏转变化矩阵为：

；

则时刻的姿态变化矩阵为：

；

s5.2.2：计算从时刻到时刻的第k步姿态变化矩阵：

；

其中，为时刻的姿态变化矩阵；

s5.2.3：将机器人末端执行器从时刻到时刻的姿态变化认为是绕某一旋转轴旋转得到，通过轴-角的关系变换得到旋转轴和角度变量：

；

其中，为里第一行第一列的元素，为里第一行第二列的元素，为里第一行第三列的元素，为里第二行第一列的元素，为里第二行第二列的元素，为里第二行第三列的元素，为里第三行第一列的元素，为里第三行第二列的元素，为里第三行第三列的元素，为旋转轴在圆弧坐标系的x轴上的分量，为旋转轴在圆弧坐标系的y轴上的分量，为旋转轴在圆弧坐标系的z轴上的分量；

s5.2.4：由旋转轴求取的等效旋转矩阵：

；

将等效旋转矩阵进行微分变换得到：

；

其中，，，，为的微分形式，为的微分值；

s5.2.5：由微分定义得到时刻的微分：

；

；

其中，为时间微分，为单位矩阵；

s5.2.6：计算机器人末端执行器从时刻到时刻的姿态变化的角速度：

；

将代入，得到机器人末端执行器的姿态角速度为：

。

在具体实施过程中，机器人末端执行器在圆弧运动过程中，除了会围绕回转轴旋转引起姿态旋转之外，还会产生姿态偏转，如图3所示，分别为机器人末端执行器无偏转时的旋转矢量，分别为机器人末端执行器有偏转时的旋转矢量，两个状态之间的旋转矢量偏转角度为。

更具体的，在步骤s6中，在步骤s6中，时刻机器人末端执行器在圆弧坐标系下的位姿分速度为：

；

根据圆弧坐标系和机器人坐标系之间的转换关系，得到时刻机器人末端执行器在机器人坐标系下的位姿分速度：

；

其中，为机器人末端执行器在圆弧坐标系的x轴上的姿态角速度分量，为机器人末端执行器在圆弧坐标系的y轴上的姿态角速度分量，为机器人末端执行器在圆弧坐标系的z轴上的姿态角速度分量。

更具体的，在步骤s6中，由位姿分速度计算出机器人关节的角速度：

；

其中，为机器人末端执行器和关节的速度雅可比矩阵，，为机器人第一关节的角速度，为机器人第二关节的角速度，为机器人第三关节的角速度，为机器人第四关节的角速度，为机器人第五关节的角速度，为机器人第六关节的角速度。如图4所示，关节1、关节2、关节3、关节4、关节5、关节6分别对应第一关节、第二关节、第三关节、第四关节、第五关节、第六关节，图中的曲线分别为机器人各个关节的角度随当前插补步数k（即插补时间）的变化情况，通过将机器人的各个关节的角度对时间求导，得到相应关节的角速度，即。

在具体实施过程中，机器人末端执行器和关节的速度雅可比矩阵根据实际情况设置。

更具体的，在步骤s7中，通过对机器人关节的角速度进行积分计算得到机器人关节的角度：

；

其中，为第k步插补时机器人关节转动的角度，插补时间，为每步插补所需的时间。

在具体实施过程中，由于机器人在进行圆弧轨迹运动的时候，需要机器人的各个关节协同运动才能使机器人末端执行器达成需要的状态，因此通过对从时刻到时刻的角速度积分，得到第k步插补时机器人关节需要转动的角度，从而实现机器人圆弧轨迹规划，使机器人进行圆弧轨迹运动。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。