一种姿态平滑过渡方法及系统

技术领域

本发明涉及机器人技术领域，具体来说是一种姿态平滑过渡方法及系统。

背景技术

机器人编程系统提供几种基本运动指令，如直线运动，圆弧运动等。机器人逐条执行运动指令，工具中心点沿指令定义的路径运动。若不进行设置，机器人在每条指令的 目标点处速度为零。对于某些应用，为提高效率，希望机器人在相邻指令间能够保持运 动，且允许机器人在一定范围内偏离路径。解决此问题的一个典型方法是：在相邻指令 定义的路径之间插入一段过渡路径。为保证运动平稳，避免加速度跳变对机械本体造成 冲击，过渡路径与前后路径段在衔接处应具有两阶及以上几何连续性。

路径包含位置和姿态两部分信息，相应地，过渡路径包含位置过渡和姿态过渡。不同运动指令的位置插值方法不同，但姿态插值通常统一采用球面线性插值。球面线性插 值产生的姿态序列可以用4维空间中单位球面上的大圆弧描述，姿态过渡即是在球面上 构建曲线，并在两段大圆弧间完成平滑过渡。作为对比，位置过渡是在欧式空间中构建 符合要求的过渡曲线。对于姿态过渡，在球面上而非欧式空间中构建符合要求的过渡曲 线，是一个较为困难的任务。

如申请号为201911300865.9公开的一种位姿同步的六轴工业机器人轨迹平顺方法， 该方法采用圆弧曲线对位置轨迹过渡，并采用四元数B样条对姿态轨迹过渡。过渡后的位置和姿态轨迹均具有高阶连续性，可同时约束位置和姿态的过渡误差，且过渡后的位 置轨迹和姿态轨迹具有参数同步性。该发明申请采用具有5个控制点的B样条构造姿态 过渡曲线，所构造的姿态过渡曲线与前后姿态插值曲线在衔接处具两阶几何连续性，但 过渡曲线的参数对时间导数无明确意义，难以根据角速度变化规律进行轨迹规划。

发明内容

本发明所要解决的技术问题在于提供一种姿态平滑过渡方法。

本发明通过以下技术手段实现解决上述技术问题的：

一种姿态平滑过渡方法，包括以下步骤：

S01.根据给定三个姿态q

S02.构造并求取过渡曲线的参数方程q(s)。

在步骤S01中，选取参考坐标系方法如下：记u

由此可以确定参考坐标系，在参考坐标系下，旋转轴u

在步骤S01中，在选取的参考坐标系下，按照球面线性插值方法，用单位四元数表示姿态，插值曲线q

其中

p

在步骤S01中，给定的α满足α＞0，α＜α

在步骤S02中，过渡曲线的构造和参数方程求取过程如下：首先将过渡曲线的参数方程q(s)表示成以下形式：

q(s)＝p(s)q

式中，s为过渡曲线的参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为 由q

p(s)＝(p

再确定p(s)的各个分量，其中最后一个分量p

式(3)中，η，θ

θ(s)，f(s)为关于s的函数，其表达式为

式(5)中，sd，am和П为三种与椭圆积分相关的特殊函数：sd(u，m)对应于Jacobi椭圆函数sd(u|m)，am(u，m)对应于Jacobi振幅函数am(u|m)，П(n；φ，m)对应于第三类不完全椭 圆积分П(n；φ|m)，m为椭圆积分的参数，n为椭圆积分的特征数，n与m有如下关系式：

c

再求解m，根据式θ(K(m)/c

式(8)中K(m)为第一类完全椭圆积分，П(n；m)为第三类完全椭圆积分；由式(7)和式(6)， 等式(8)两边只与m相关，使用数值方法在区间[0，m

最后确定过渡曲线的参数方程q(s)，由解出的m，根据式(6)和式(7)可以求出n和c

σ＝2K(m)/c

按式(3)～(8)所确定的p(s)各个分量，能够保证所构造的过渡曲线q(s)与前后姿态 插值曲线q

本发明还提供一种姿态平滑过渡系统，包括

过渡曲线起点姿态和终点姿态确定模块，根据给定三个姿态q

参数方程的构造和求取模块，构造并求取过渡曲线的参数方程q(s)。

进一步的，在插值曲线确定模块中，选取参考坐标系方法如下：记u

由此可以确定参考坐标系，在参考坐标系下，旋转轴u

进一步的，在插值曲线确定模块中，在选取的参考坐标系下，按照球面线性插值方法，用单位四元数表示姿态，插值曲线q

其中

p

进一步的，在过渡曲线起点姿态和终点姿态确定模块中，给定的α满足α＞0，α＜α

进一步的，在参数方程的构造和求取模块中，过渡曲线的构造和参数方程求取过程 如下：首先将过渡曲线的参数方程q(s)表示成以下形式：

q(s)＝p(s)q

式中，s为过渡曲线的参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为 由q

p(s)＝(p

再确定p(s)的各个分量，其中最后一个分量p

式(3)中，η，θ

θ(s)，f(s)为关于s的函数，其表达式为

式(5)中，sd，am和П为三种与椭圆积分相关的特殊函数：sd(u，m)对应于Jacobi椭圆函数 sd(u|m)，am(u，m)对应于Jacobi振幅函数am(u|m)，П(n；φ，m)对应于第三类不完全椭 圆积分П(n；φ|m)，m为椭圆积分的参数，n为椭圆积分的特征数，n与m有如下关系式：

c

再求解m，根据式θ(K(m)/c

式(8)中K(m)为第一类完全椭圆积分，П(n；m)为第三类完全椭圆积分；由式(7)和式(6)， 等式(8)两边只与m相关，使用数值方法在区间[0，m

最后确定过渡曲线的参数方程q(s)，由解出的m，根据式(6)和式(7)可以求出n和c

σ＝2K(m)/c

按式(3)～(8)所确定的p(s)各个分量，能够保证所构造的过渡曲线q(s)与前后姿态 插值曲线q

本发明的优点在于：

按本发明提供的方法构造的过渡曲线q(s)与前后姿态插值曲线q

附图说明

图1为本发明实施例2中求解m的示意图；

图2为本发明实施例2中姿态过渡曲线的示意图；

图3为本发明实施例3中角速度随时间的变化曲线；

图4为本发明实施例3中角加速度随时间的变化曲线；

具体实施方式

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

实施例1

本实施例提供姿态平滑过渡方法的详细说明。

一种姿态平滑过渡方法，已知三个姿态q

选取一个参考坐标系，参考系的坐标向量i，j，k按下式确定：

在参考坐标系下，旋转轴u

姿态用单位四元数表示，按照球面线性插值，由q

q

s为参数且

表示由姿态q

按照球面线性插值，由q

q

s为参数且

表示由姿态q

为实现姿态的平滑过渡，需要在q

根据给定的角度值α确定q

q

任意姿态都能够由q

q(s)＝p(s)q

式中，s为参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由姿态q

p(s)＝(p

若各个分量p

p(s)各个分量确定方法如下，最后一个分量p

式中，η，θ

θ(s)，f(s)为关于s的函数，其表达式为

式中，sd，am和П为三种与椭圆积分相关的特殊函数：sd(u，m)对应于Jacobi椭圆函数 sd(u|m)，am(u，m)对应于Jacobi振幅函数am(u|m)，П(n；φ，m)对应于第三类不完全椭 圆积分П(n；φ|m)，m为椭圆积分的参数，n为椭圆积分的特征数。

进一步，在式(5)中，n与m相关，n表示成关于m的表达式为

c

根据式θ(K(m)/c

式(8)中K(m)为第一类完全椭圆积分，П(n；m)为第三类完全椭圆积分；由式(7)和式(6)， 等式(8)两边只与m相关，使用数值方法，如二分法，在区间[0，m

m确定后，由式(6)和式(7)可以求出n和c

σ＝2K(m)/c

按上述方法确定的p(s)能够保证q(0)＝q

按以上方法构造的过渡曲线q(s)与前后姿态插值曲线q

对应的，本实施例还提供一种姿态平滑过渡的系统，包括

过渡曲线起点姿态和终点姿态确定模块，已知三个姿态q

选取一个参考坐标系，参考系的坐标向量i，j，k按下式确定：

在参考坐标系下，旋转轴u

姿态用单位四元数表示，按照球面线性插值，由q

q

s为参数且

表示由姿态q

按照球面线性插值，由q

q

s为参数且

表示由姿态q

为实现姿态的平滑过渡，需要在q

根据给定的角度值α确定q

q

参数方程的构造和求取模块，任意姿态都能够由q

q(s)＝p(s)q

式中，s为参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由姿态q

p(s)＝(p

若各个分量p

p(s)各个分量确定方法如下，最后一个分量p

式中，η，θ

θ(s)，f(s)为关于s的函数，其表达式为

式中，sd，am和П为三种与椭圆积分相关的特殊函数：sd(u，m)对应于Jacobi椭圆函数 sd(u|m)，am(u，m)对应于Jacobi振幅函数am(u|m)，П(n；φ，m)对应于第三类不完全椭 圆积分П(n；φ|m)，m为椭圆积分的参数，n为椭圆积分的特征数。

进一步，在式(5)中，n与m相关，n表示成关于m的表达式为

c

根据式θ(K(m)/c

式(8)中K(m)为第一类完全椭圆积分，П(n；m)为第三类完全椭圆积分；由式(7)和式(6)， 等式(8)两边只与m相关，使用数值方法，如二分法，在区间[0，m

m确定后，由式(6)和式(7)可以求出n和c

σ＝2K(m)/c

按上述方法确定的p(s)能够保证q(0)＝q

实施例2

本实施例提供当α

根据α和β，按式(4)求得

θ

由式(6)得到

由式(9)得到

使用二分法，在区间[0，m

m＝0.161838

图1为式(8)右侧在区间[0，m

根据式(6)，式(7)和式(10)计算得到

n＝0.265482，c

将m，n，c

q(s)＝p(s)，s∈[0，σ]

即q(s)与p(s)表达式相同。

按照球面线性插值，由q

同样按照球面线性插值，由q

q(s)，q

实施例3

本实施例提供根据角速度变化规律，对实施例2中的姿态过渡曲线进行轨迹规划的 说明。

对实施例2中得到的姿态过渡曲线进行轨迹规划，也即确定过渡曲线参数方程中的 参数s关于时间的变化规律s(t)。已知角速度变化规律为：在起点q

利用参数s对时间的导数等于角速度大小可以得到参数s关于时是的变化规律s(t)应 为

s(t)＝2t，t∈[0，σ/2]

q(s)和s(t)都确定后，能够得到姿态随时间的变化规律，图3为角速度ω的大小及各分量随时间的变化曲线，角速度大小|ω|恒为2。图4为角加速度

实施例4

对应的，本实施例提供一种姿态平滑过渡处理设备，包括至少一个处理器，以及与所述处理器通信连接的至少一个存储器，其中：存储器存储有可被处理器执行的程序指令，所述处理器调用所述程序指令能够执行上述任何一个实施例的方法。

实施例5

对应的，本实施例提供一种计算机可读存储介质，所述计算机可读存储介质存储计 算机指令，所述计算机指令使所述计算机执行上述任一的方法。

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