[发明专利]一种姿态平滑过渡方法及系统

权利要求书

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₁表示为