CN112356032B - 一种姿态平滑过渡方法及*** - Google Patents

Abstract

本发明提出一种姿态平滑过渡方法，构造的姿态过渡曲线与前后姿态插值曲线在衔接处具有两阶几何连续性，能够实现角速度处处平滑和角加速度处处连续；另外，过渡曲线参数方程中的参数对时间导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

Description

一种姿态平滑过渡方法及***

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

S01.根据给定三个姿态q₀，q₁，q₂，选取一个参考坐标系，并在参考坐标系下按照球面线性插值方法，确定由q₀到q₁的姿态插值曲线q₀₁(s)，以及由q₁到q₂的姿态插值曲线q₁₂(s)；根据给定的角度α，在姿态插值曲线q₀₁(s)上选取过渡曲线的起点姿态q_i，在姿态插值曲线q₁₂(s)上选取过渡曲线终点姿态q_f；

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

在步骤S01中，选取参考坐标系方法如下：记u₀，α₀为由q₀到q₁的旋转轴和旋转角，u₁，α₁为由q₁到q₂的旋转轴和旋转角，u₀和u₁的夹角记为β，则参考坐标系的三个坐标向量i，j，k由下式确定

由此可以确定参考坐标系，在参考坐标系下，旋转轴u₀、u₁表示为

在步骤S01中，在选取的参考坐标系下，按照球面线性插值方法，用单位四元数表示姿态，插值曲线q₀₁(s)和q₁₂(s)可以表示为

其中

p₀₁，p₁₂分别表示由q₁到q₀₁(s)以及由q₁到q₁₂(s)的旋转变换，旋转变换同样用单位四元数表示。

在步骤S01中，给定的α满足α＞0，α＜α₀且α＜α₁，起点姿态q_i和终点姿态q_f按下式选取

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

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

式中，s为过渡曲线的参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由q₁到过渡曲线上任一姿态q(s)的旋转变换，同样用单位四元数表示，写成分量形式为：

p(s)＝(p₀(s)，(p₁(s)，p₂(s)，p₃(s)))

再确定p(s)的各个分量，其中最后一个分量p₃(s)恒为0，其余三个分量具有如下形式表达式

式(3)中，η，θ_m为与α，β相关的常量，计算公式为

θ(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，n有关的常数，其表达式为

再求解m，根据式θ(K(m)/c_ψ)＝θ_m求解m，即求解方程

式(8)中K(m)为第一类完全椭圆积分，Π(n；m)为第三类完全椭圆积分；由式(7)和式(6)，等式(8)两边只与m相关，使用数值方法在区间[0，m_max]上求解式(8)确定的关于m的方程，其中

最后确定过渡曲线的参数方程q(s)，由解出的m，根据式(6)和式(7)可以求出n和c_ψ，再根据式(5)和式(3)可以确定p(s)各个分量，最后根据式(2)，能够确定过渡曲线的参数方程，参数s的最大取值σ由下式决定：

σ＝2K(m)/c_ψ (10)

按式(3)～(8)所确定的p(s)各个分量，能够保证所构造的过渡曲线q(s)与前后姿态插值曲线q₀₁(s)、q₁₂(s)分别在q_i和q_f衔接，且在衔接处具有两阶几何连续性；此外，式(3)～(8)所确定的p(s)各个分量，同样能够保证过渡曲线q(s)的参数s对时间的导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

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

过渡曲线起点姿态和终点姿态确定模块，根据给定三个姿态q₀，q₁，q₂，，选取一个参考坐标系，并在参考坐标系下按照球面线性插值方法，确定由q₀到q₁的姿态插值曲线q₀₁(s)，以及由q₁到q₂的姿态插值曲线q₁₂(s)；根据给定的角度α，在姿态插值曲线q₀₁(s)上选取过渡曲线的起点姿态q_i，在姿态插值曲线q₁₂(s)上选取过渡曲线终点姿态q_f；

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

进一步的，在插值曲线确定模块中，选取参考坐标系方法如下：记u₀，α₀为由q₀到q₁的旋转轴和旋转角，u₁，α₁为由q₁到q₂的旋转轴和旋转角，u₀和u₁的夹角记为β，则参考坐标系的三个坐标向量i，j，k由下式确定

由此可以确定参考坐标系，在参考坐标系下，旋转轴u₀、u₁表示为

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

其中

p₀₁，p₁₂分别表示由q₁到q₀₁(s)以及由q₁到q₁₂(s)的旋转变换，旋转变换同样用单位四元数表示。

进一步的，在过渡曲线起点姿态和终点姿态确定模块中，给定的α满足α＞0，α＜α₀且α＜α₁，起点姿态q_i和终点姿态q_f按下式选取

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

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

式中，s为过渡曲线的参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由q₁到过渡曲线上任一姿态q(s)的旋转变换，同样用单位四元数表示，写成分量形式为：

p(s)＝(p₀(s)，(p₁(s)，p₂(s)，p₃(s)))

再确定p(s)的各个分量，其中最后一个分量p₃(s)恒为0，其余三个分量具有如下形式表达式

式(3)中，η，θ_m为与α，β相关的常量，计算公式为

θ(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，n有关的常数，其表达式为

再求解m，根据式θ(K(m)/c_ψ)＝θ_m求解m，即求解方程

式(8)中K(m)为第一类完全椭圆积分，Π(n；m)为第三类完全椭圆积分；由式(7)和式(6)，等式(8)两边只与m相关，使用数值方法在区间[0，m_max]上求解式(8)确定的关于m的方程，其中

最后确定过渡曲线的参数方程q(s)，由解出的m，根据式(6)和式(7)可以求出n和c_ψ，再根据式(5)和式(3)可以确定p(s)各个分量，最后根据式(2)，能够确定过渡曲线的参数方程，参数s的最大取值σ由下式决定：

σ＝2K(m)/c_ψ (10)

按式(3)～(8)所确定的p(s)各个分量，能够保证所构造的过渡曲线q(s)与前后姿态插值曲线q₀₁(s)、q₁₂(s)分别在q_i和q_f衔接，且在衔接处具有两阶几何连续性；此外，式(3)～(8)所确定的p(s)各个分量，同样能够保证过渡曲线q(s)的参数s对时间的导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

本发明的优点在于：

按本发明提供的方法构造的过渡曲线q(s)与前后姿态插值曲线q₀₁(s)和q₁₂(s)在衔接处具有两阶几何连续性，能够实现角速度处处平滑和角加速度处处连续；且过渡曲线q(s)的参数s对时间的导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

附图说明

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

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

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

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

具体实施方式

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

实施例1

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

一种姿态平滑过渡方法，已知三个姿态q₀，q₁，q₂，其中由姿态q₀到q₁的旋转轴为u₀，旋转角为α₀＞0；由q₁到q₂的旋转轴为u₁，旋转角为α₁＞0；u₀和u₁的夹角为β，β≠0。

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

在参考坐标系下，旋转轴u₀、u₁表示为

姿态用单位四元数表示，按照球面线性插值，由q₀到q₁的插值曲线可以用以下参数方程表示：

q₀₁(s)＝p₀₁(s)q₁，s∈[0，α₀]

s为参数且

表示由姿态q₁到姿态q₀₁(s)的旋转变换，同样用单位四元数表示。p₀₁(s)和q₁之间的运算遵从四元数乘法。

按照球面线性插值，由q₁到q₂的插值曲线可以用以下参数方程表示：

q₁₂(s)＝p₁₂(s)q₁，s∈[0，α₁]

s为参数且

表示由姿态q₁到姿态q₁₂(s)的旋转变换，同样用单位四元数表示。p₁₂(s)和q₁之间的运算遵从四元数乘法。

为实现姿态的平滑过渡，需要在q₀₁(s)和q₁₂(s)上分别选取一个起点姿态q_i和终点姿态q_f，并在两个姿态之间构造一条过渡曲线。

根据给定的角度值α确定q_i和q_f，α满足α＞0，α＜α₀且α＜α₁。将姿态q₁绕旋转轴u₀旋转(α₀-α)角度到达的姿态作为q_i，姿态q₁绕旋转轴u₁旋转α角度到达的姿态作为q_f，有

q_i位于插值曲线q₀₁(s)上，q_f位于插值曲线q₁₂(s)上。

任意姿态都能够由q₁经过一定的旋转变换得到，参考插值曲线q₀₁(s)和q₁₂(s)的形式，过渡曲线的参数方程可以表示成以下形式：

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

式中，s为参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由姿态q₁到姿态q(s)的旋转变换，用单位四元数表示，p(s)写成分量形式为

p(s)＝(p₀(s)，(p₁(s)，p₂(s)，p₃(s)))

若各个分量p₀(s)，p₁(s)，p₂(s)，p₃(s)得以确定，p(s)和q(s)也能确定。

p(s)各个分量确定方法如下，最后一个分量p₃(s)恒为0，其余三个分量具有以下形式：

式中，η，θ_m为与α，β相关的常量，其计算公式为

θ(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_ψ与m，n相关，其表达式为

根据式θ(K(m)/c_ψ)＝θ_m确定m，即求解方程

式(8)中K(m)为第一类完全椭圆积分，Π(n；m)为第三类完全椭圆积分；由式(7)和式(6)，等式(8)两边只与m相关，使用数值方法，如二分法，在区间[0，m_max]上求解式(8)确定的关于m的方程，其中

m确定后，由式(6)和式(7)可以求出n和c_ψ，再根据式(5)和式(3)可以确定p(s)各个分量，最后根据式(2)，能够确定过渡曲线的参数方程，参数s的最大取值σ由下式决定：

σ＝2K(m)/c_ψ (10)

按上述方法确定的p(s)能够保证q(0)＝q_i，q(σ)＝q_f，即过渡曲线在起点处与前一姿态插值曲线p₀₁(s)衔接，在终点处与后一姿态插值曲线p₁₂(s)衔接，且过渡曲线与前后插值曲线在衔接处具有两阶几何连续性；此外，过渡曲线参数方程中的参数对时间的导数等于角速度的大小，便于根据角速度变化规律进行轨迹规划。

按以上方法构造的过渡曲线q(s)与前后姿态插值曲线q₀₁(s)和q₁₂(s)在衔接处具有两阶几何连续性，能够实现角速度处处平滑和角加速度处处连续；且过渡曲线q(s)的参数s对时间的导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

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

过渡曲线起点姿态和终点姿态确定模块，已知三个姿态q₀，q₁，q₂，其中由姿态q₀到q₁的旋转轴为u₀，旋转角为α₀＞0；由q₁到q₂的旋转轴为u₁，旋转角为α₁＞0；u₀和u₁的夹角为β，β≠0。

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

在参考坐标系下，旋转轴u₀、u₁表示为

姿态用单位四元数表示，按照球面线性插值，由q₀到q₁的插值曲线可以用以下参数方程表示：

q₀₁(s)＝p₀₁(s)q₁，s∈[0，α₀]

s为参数且

表示由姿态q₁到姿态q₀₁(s)的旋转变换，同样用单位四元数表示。p₀₁(s)和q₁之间的运算遵从四元数乘法。

按照球面线性插值，由q₁到q₂的插值曲线可以用以下参数方程表示：

q₁₂(s)＝p₁₂(s)q₁，s∈[0，α₁]

s为参数且

表示由姿态q₁到姿态q₁₂(s)的旋转变换，同样用单位四元数表示。p₁₂(s)和q₁之间的运算遵从四元数乘法。

为实现姿态的平滑过渡，需要在q₀₁(s)和q₁₂(s)上分别选取一个起点姿态q_i和终点姿态q_f，并在两个姿态之间构造一条过渡曲线。

根据给定的角度值α确定q_i和q_f，α满足α＞0，α＜α₀且α＜α₁。将姿态q₁绕旋转轴u₀旋转(α₀-α)角度到达的姿态作为q_i，姿态q₁绕旋转轴u₁旋转α角度到达的姿态作为q_f，有

q_i位于插值曲线q₀₁(s)上，q_f位于插值曲线q₁₂(s)上。

参数方程的构造和求取模块，任意姿态都能够由q₁经过一定的旋转变换得到，参考插值曲线q₀₁(s)和q₁₂(s)的形式，过渡曲线的参数方程可以表示成以下形式：

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

式中，s为参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由姿态q₁到姿态q(s)的旋转变换，用单位四元数表示，p(s)写成分量形式为

p(s)＝(p₀(s)，(p₁(s)，p₂(s)，p₃(s)))

若各个分量p₁(s)，p₂(s)，p₃(s)，p₄(s)得以确定，p(s)和q(s)也能确定。

p(s)各个分量确定方法如下，最后一个分量p₃(s)恒为0，其余三个分量具有以下形式：

式中，η，θ_m为与α，β相关的常量，其计算公式为

θ(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_ψ与m，n相关，其表达式为

根据式θ(K(m)/c_ψ)＝θ_m确定m，即求解方程

式(8)中K(m)为第一类完全椭圆积分，Π(n；m)为第三类完全椭圆积分；由式(7)和式(6)，等式(8)两边只与m相关，使用数值方法，如二分法，在区间[0，mmax]上求解式(8)确定的关于m的方程，其中

m确定后，由式(6)和式(7)可以求出n和c_ψ，再根据式(5)和式(3)可以确定p(s)各个分量，最后根据式(2)，能够确定过渡曲线的参数方程，参数s的最大取值σ由下式决定：

σ＝2K(m)/c_ψ (10)

按上述方法确定的p(s)能够保证q(0)＝q_i，q(σ)＝q_f，即过渡曲线在起点处与前一姿态插值曲线p₀₁(s)衔接，在终点处与后一姿态插值曲线p₁₂(s)衔接，且过渡曲线与前后插值曲线在衔接处具有两阶几何连续性；此外，过渡曲线参数方程中的参数对时间的导数等于角速度的大小，便于根据角速度变化规律进行轨迹规划。

实施例2

本实施例提供当α₀＝120°，α₁＝150°，α＝90°，β＝90°，q₁＝(1，(0，0，0))时，确定姿态过渡曲线的详细说明。

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

θ_m＝arcsin(sin45°cos45°)＝30°

由式(6)得到

由式(9)得到

使用二分法，在区间[0，m_max]上搜索式(8)中m的根，得到

m＝0.161838

图1为式(8)右侧在区间[0，m_max]上的变化图，图形中纵坐标为θ_m的点所对应的横坐标值等于m。

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

n＝0.265482，c_ψ＝1.268094，σ＝2.587944

将m，n，c_ψ，η，θ_m的值依次代入式(5)和式(3)，可以得到p₀(s)，p₁(s)和p₂(s)的详细表达式；再由式(1)可得到q(s)的表达式，本例中，q₁＝(1，(0，0，0))，根据四元数乘法规则可以知道

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

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

按照球面线性插值，由q₀到q₁的插值曲线q₀₁(s)为

同样按照球面线性插值，由q₁到q₂的插值曲线q₁₂(s)为

q(s)，q₀₁(s)，q₁₂(s)所表示的三条姿态曲线位于三维单位球面上，而它们最后一个分量都恒为零，在忽略第四个分量后，q(s)，q₀₁(s)，q₁₂(s)可以由三维空间中单位球面上的曲线表示，图2示意了三条姿态曲线，q(s)完成了q₀₁(s)到q₁₂(s)的平滑过渡，且在衔接点q_i和q_f处，相接的两条曲线具有两阶几何连续性。

实施例3

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

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

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

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

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

大小及各分量随时间的变化曲线，起点和终点角加速度大小都为0。

实施例4

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

实施例5

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

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

Claims (8)

1.一种姿态平滑过渡方法，其特征在于：包括以下步骤：

S01.根据给定三个姿态q₀，q₁，q₂，选取一个参考坐标系，并在参考坐标系下按照球面线性插值方法，确定由q₀到q₁的姿态插值曲线q₀₁(s)，以及由q₁到q₂的姿态插值曲线q₁₂(s)；根据给定的角度α，在姿态插值曲线q₀₁(s)上选取过渡曲线的起点姿态q_i，在姿态插值曲线q₁₂(s)上选取过渡曲线终点姿态q_f；

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

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

q(s)＝p(s)q₁,s∈[0,σ] (2)

式中，s为过渡曲线的参数，σ为参数s的最大取值，q()为过渡曲线上的任一姿态，()为由q₁到过渡曲线上任一姿态q(s)的旋转变换，同样用单位四元数表示，写成分量形式为：

p(s)＝(p₀(s),(p₁(s),p₂(s),p₃(s)))

再确定p(s)的各个分量，其中最后一个分量p₃(s)恒为0，其余三个分量具有如下形式表达式

式(3)中，η,θ_m为与α,β相关的常量，计算公式为

θ(s),(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为椭圆积分的特征数，与m有如下关系式：

c_ψ为与m,n有关的常数，其表达式为

再求解m，根据式θ(K(m)/c_ψ)＝θ_m求解m，即求解方程

式(8)中K(m)为第一类完全椭圆积分，Π(n；m)为第三类完全椭圆积分；由式(7)和式(6)，等式(8)两边只与m相关，使用数值方法在区间[0,m_max]上求解式(8)确定的关于m的方程，其中

最后确定过渡曲线的参数方程q(s)，由解出的m，根据式(6)和式(7)可以求出n和c_ψ，再根据式(5)和式(3)可以确定p(s)各个分量，最后根据式(2)，能够确定过渡曲线的参数方程，参数s的最大取值σ由下式决定：

σ＝2K(m)/c_ψ (10)

按式(3)～(8)所确定的p(s)各个分量，能够保证所构造的过渡曲线q(S)与前后姿态插值曲线q₀₁(s)、q₁₂(s)分别在q_i和q_f衔接,且在衔接处具有两阶几何连续性；此外，式(3)～(8)所确定的p(s)各个分量，同样能够保证过渡曲线q(s)的参数s对时间的导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

2.根据权利要求1所述的一种姿态平滑过渡方法，其特征在于，在步骤S01中，选取参考坐标系方法如下：记u₀，α₀为由q₀到q₁的旋转轴和旋转角，u₁，α₁为由q₁到q₂的旋转轴和旋转角，u₀和u₁的夹角记为β，则参考坐标系的三个坐标向量i,j,k由下式确定

由此可以确定参考坐标系，在参考坐标系下，旋转轴u₀、u₁表示为

3.根据权利要求1所述的一种姿态平滑过渡方法，其特征在于，在步骤S01中，在选取的参考坐标系下，按照球面线性插值方法，用单位四元数表示姿态，插值曲线q₀₁(s)和q₁₂(s)可以表示为

其中

p₀₁，p₁₂分别表示由q₁到q₀₁(s)以及由q₁到q₁₂(s)的旋转变换，旋转变换同样用单位四元数表示。

4.根据权利要求1所述的一种姿态平滑过渡方法，其特征在于，在步骤S01中，给定的α满足α＞0,α＜α₀且α＜α₁，起点姿态q_i和终点姿态q_f按下式选取

5.一种姿态平滑过渡***，其特征在于：包括

过渡曲线起点姿态和终点姿态确定模块，根据给定三个姿态q₀，q₁，q₂，选取一个参考坐标系，并在参考坐标系下按照球面线性插值方法，确定由q₀到q₁的姿态插值曲线q₀₁(s)，以及由q₁到q₂的姿态插值曲线q₁₂(s)；根据给定的角度α，在姿态插值曲线q₀₁(s)上选取过渡曲线的起点姿态q_i，在姿态插值曲线q₁₂(s)上选取过渡曲线终点姿态q_f；

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

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

q(s)＝p(s)q₁,s∈[0,σ] (2)

式中，s为过渡曲线的参数，σ为参数s的最大取值，q(s)为过渡曲线上的任一姿态，p(s)为由q₁到过渡曲线上任一姿态q(s)的旋转变换，同样用单位四元数表示，写成分量形式为：

p(s)＝(p₀(s),(p₁(s),p₂(s),p₃(s)))

再确定p(s)的各个分量，其中最后一个分量p₃(s)恒为0，其余三个分量具有如下形式表达式

式(3)中，η,θ_m为与α,β相关的常量，计算公式为

θ(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,n有关的常数，其表达式为

再求解m，根据式θ(K(m)/c_ψ)＝θ_m求解m，即求解方程

式(8)中K(m)为第一类完全椭圆积分，Π(n；m)为第三类完全椭圆积分；由式(7)和式(6)，等式(8)两边只与m相关，使用数值方法在区间[0,m_max]上求解式(8)确定的关于m的方程，其中

最后确定过渡曲线的参数方程q(s)，由解出的m，根据式(6)和式(7)可以求出n和c_ψ，再根据式(5)和式(3)可以确定p(s)各个分量，最后根据式(2)，能够确定过渡曲线的参数方程，参数s的最大取值σ由下式决定：

σ＝2K(m)/c_ψ (10)

按式(3)～(8)所确定的p(s)各个分量，能够保证所构造的过渡曲线q(s)与前后姿态插值曲线q₀₁(s)、q₁₂(s)分别在q_i和q_f衔接,且在衔接处具有两阶几何连续性；此外，式(3)～(8)所确定的p(s)各个分量，同样能够保证过渡曲线q(s)的参数s对时间的导数等于角速度大小，便于根据角速度变化规律进行轨迹规划。

6.根据权利要求5所述的一种姿态平滑过渡***，其特征在于，在过渡曲线起点姿态和终点姿态确定模块中，选取参考坐标系方法如下：记u₀，α₀为由q₀到q₁的旋转轴和旋转角，u₁，α₁为由q₁到q₂的旋转轴和旋转角，u₀和u₁的夹角记为β，则参考坐标系的三个坐标向量i,j,k由下式确定

由此可以确定参考坐标系，在参考坐标系下，旋转轴u₀、u₁表示为

7.根据权利要求6所述的一种姿态平滑过渡***，其特征在于，在过渡曲线起点姿态和终点姿态确定模块中，在选取的参考坐标系下，按照球面线性插值方法，用单位四元数表示姿态，插值曲线q₀₁(s)和q₁₂(s)可以表示为

其中

p₀₁，p₁₂分别表示由q₁到q₀₁(s)以及由q₁到q₁₂(s)的旋转变换，旋转变换同样用单位四元数表示。

8.根据权利要求5所述的一种姿态平滑过渡***，其特征在于，在过渡曲线起点姿态和终点姿态确定模块中，给定的α满足α＞0,α＜α₀且α＜α₁，起点姿态q_i和终点姿态q_f按下式选取

Images