一种全姿态四轴转台框架角指令解算方法技术

本发明专利技术提出一种全姿态四轴转台框架角指令解算方法，所述方法包括建立基于双欧拉法的四轴转台框架角与全空间飞行器姿态角的运动学模型、对仿真初始时刻的各框架角位置进行初始化、设计基于约束最优化理论的转台运动学反解算法、根据运动学反解算法，计算得到当前时刻四轴转台各框架角位置指令、设计四轴转台的运动学正解算法最终得到框架角位置的全部指令。本发明专利技术所述方法可以保证四轴转台的指令解算在满足实时性及有效规避运动学奇异位置等的前提下，不会出现在全姿态内的大幅度解算偏差跳变，消除了由于正解多解性带来的大幅度误差现象。

【技术实现步骤摘要】
一种全姿态四轴转台框架角指令解算方法
本专利技术属于飞行器半实物仿真
，特别是涉及一种全姿态四轴转台框架角指令解算方法。
技术介绍
在飞行器半实物仿真和测试中，飞行仿真转台是常用的硬件设备之一。在各种仿真转台中，三轴仿真转台最为常用，能够精确地模拟飞行器在空间中俯仰、滚转、偏航三个姿态变化。但由于自身结构特点，三轴仿真转台存在运动学奇异位置，无法适应全姿态飞行仿真这一新的需求。为在保证模拟空间内飞行器姿态的前提下规避奇异位置，工程上提出了四轴转台方案，通过增加冗余自由度的方式，实现额外的次级任务。但不同于三轴转台各个框架角与模拟飞行器的姿态角一一对应，四轴转台的四个框架角与三个姿态角之间存在耦合关系，即给定一组姿态角可能有无数组框架角与之对应。对于四轴转台各框架角指令的解算，目前主要有两类方案，分别是基于自适应调整算法的解算方案以及基于含约束最优化问题的解算方案。其中的第一类，基于自适应调整算法的解算方案通过主动设计基框架的运动律，求解其余三个框架的角位置指令，进而实现对飞行器姿态的模拟。该方案能够有效规避四轴转台自身的运动学奇异位置，但难以设计出理想的基框架运动律。此外，在借助此方案实现飞行器全姿态仿真时，可能出现求解得到的各框架角位置指令不连续以及解算结果出现大幅度误差跳变的情况。第二类方案将指令解算问题视为多解性问题，并转化为最优化问题来求解。这类方案根据用户的预期性能指标设计最优化问题的优化变量、约束条件、目标函数及优化算法等，但难以同时兼顾解算实时性、精确度、对奇异位置规避等所有性能指标。此外，在采用此类方案实现对飞行器全姿态的模拟时，同样可能出现指令不连续及大幅度误差跳变的现象。
技术实现思路
本专利技术目的是为了克服已有方案无法在全姿态范围内仿真的局限，提出了一种全姿态四轴转台框架角指令解算方法。本专利技术是通过以下技术方案实现的，本专利技术提出一种全姿态四轴转台框架角指令解算方法，具体包括以下步骤：步骤一：根据四轴转台的结构及模拟飞行器姿态变化的原理，建立基于双欧拉法的四轴转台框架角与全空间飞行器姿态角的运动学模型，所述运动学模型包括角位置运动学模型及角速度运动学模型；步骤二：输入仿真初始时刻的飞行器姿态γ(0)为仿真初始时刻的滚转角，为仿真初始时刻的偏航角，θ(0)为仿真初始时刻的俯仰角，测量四轴转台各框架角的初始位置Φ0＝[φ10,φ20,φ30,φ40]T，根据建立的四轴转台框架角与飞行器姿态角的角位置运动学关系，对仿真初始时刻的各框架角位置进行初始化，得到Φ(0)＝[φ1(0),φ2(0),φ3(0),φ4(0)]T；步骤三：设计基于约束最优化理论的转台运动学反解算法，选取最优化问题中的优化变量即各框架角位置Φ＝[φ1,φ2,φ3,φ4]T、约束条件m表示约束条件的个数，目标函数f(Φ)以及优化算法；步骤四：输入当前时刻即第n时刻待模拟的飞行器姿态前一时刻即第n-1时刻的飞行器姿态仿真解及前一时刻四轴转台的各框架角位置Φ(n-1)＝[φ1(n-1),φ2(n-1),φ3(n-1),φ4(n-1)]T，根据运动学反解算法，计算得到当前时刻四轴转台各框架角位置指令Φ(n)＝[φ1(n),φ2(n),φ3(n),φ4(n)]T；步骤五：设计四轴转台的运动学正解算法，根据当前时刻各框架角位置指令Φ(n)＝[φ1(n),φ2(n),φ3(n),φ4(n)]T，解算得到飞行器当前时刻的姿态仿真解并将输入到下一时刻的运动学反解计算中；步骤六：往复迭代进行上述步骤三至步骤五，解算得到框架角位置的全部指令。进一步地，所述四轴转台为立式四轴转台。进一步地，所述步骤一具体为：以2-3-1转序姿态角描述飞行器姿态的正欧拉法表示角位置运动学模型为：RGb＝RGl(1)以2-1-3转序姿态角描述飞行器姿态的反欧拉法表示角位置运动学模型为：RrGb＝RGl(2)其中，RGb表示描述飞行器以2-3-1转序姿态角发生姿态变化的姿态矩阵，RrGb表示描述飞行器以2-1-3转序姿态角发生姿态变化的姿态矩阵，RGl表示描述立式四轴转台负载发生姿态变化的姿态矩阵；γ，θ分别表示正欧拉法中的滚转角、偏航角及俯仰角；γr，θr分别表示反欧拉法中的滚转角、偏航角及俯仰角；φ1，φ2，φ3，φ4分别表示立式四轴转台由外至内四个框架的框架角；G表示地面坐标系，b表示飞行器坐标系，l表示负载坐标系；以2-3-1转序姿态角描述飞行器姿态的正欧拉法表示角速度运动学模型为：以2-1-3转序姿态角描述飞行器姿态的反欧拉法表示角速度运动学模型为：其中，JGb表示描述飞行器以2-3-1转序姿态角运动时的雅可比矩阵，分别表示正欧拉法中的滚转角速度、偏航角速度及俯仰角速度；JrGb表示描述飞行器以2-1-3转序姿态角运动时的雅可比矩阵，分别表示反欧拉法中的滚转角速度、偏航角速度及俯仰角速度；JGl表示描述立式四轴转台负载运动时的雅可比矩阵，分别表示立式四轴转台由外至内四个框架的框架角速度；飞行器的旋转矢量记作ωb＝[ωbx,ωby,ωbz]T，其中ωbx、ωby、ωbz分别为旋转矢量在飞行器坐标系X轴、Y轴、Z轴方向的投影；负载的旋转矢量为ωl＝[ωlx,ωly,ωlz]T，其中ωlx、ωly、ωlz分别为旋转矢量在负载坐标系X轴、Y轴、Z轴方向的投影；对于公式(6)及公式(7)中的各个角速度，在工程中都采用角位置差分来近似表示，设采样时间间隔为Δt，当前时刻飞行器的姿态角分别为γn，θn，γrn，θrn，其前一时刻各姿态角则为γn-1，θn-1，γrn-1，θrn-1，则有：设公式(1)(2)中姿态矩阵RGb＝RrGb＝R，矩阵中第(i,j)项记作Rij，则描述同一姿态变化时由正欧拉角求反欧拉角的转换公式如下：由反欧拉角求解正欧拉角时，正欧拉角转换公式如下：公式(12)及(13)中，k＝0或1；定义取舍函数Δ(x1,x2)如公式(14)所示：Δ(x1,x2)＝min(|x1-x2|,2π-|x1-x2|)(14)在实际应用中进行正反欧拉角切换时，设当前时刻各欧拉角为γn，θn，γrn，θrn，其前一时刻各欧拉角则为γn-1，θn-1，γrn-1，θrn-1，引入判别函数dk根据k的取值，分别计算d0和d1，取令dk值小的一组姿态角作为当前时刻正反欧拉角的转换结果。进一步地，在步骤二中，根据输入的仿真初始时刻的飞行器姿态及测量得到的四轴转台各框架角的初始位置Φ0＝[φ10,φ20,φ30,φ40]T，保持在仿真初始时刻四轴转台基框架角位置φ1(0)＝φ10不变，其他三个框架角位置φ2(0)，φ3(0)，φ4(0)满足其中RX(γ(0))表示绕X轴旋转γ(0)角度的旋转矩阵，RZ(θ(0))表示绕Z轴旋转θ(0)角度的旋转矩阵，表示绕Y轴旋本文档来自技高网...

【技术保护点】
1.一种全姿态四轴转台框架角指令解算方法，其特征在于：具体包括以下步骤：/n步骤一：根据四轴转台的结构及模拟飞行器姿态变化的原理，建立基于双欧拉法的四轴转台框架角与全空间飞行器姿态角的运动学模型，所述运动学模型包括角位置运动学模型及角速度运动学模型；/n步骤二：输入仿真初始时刻的飞行器姿态

【技术特征摘要】
1.一种全姿态四轴转台框架角指令解算方法，其特征在于：具体包括以下步骤：
步骤一：根据四轴转台的结构及模拟飞行器姿态变化的原理，建立基于双欧拉法的四轴转台框架角与全空间飞行器姿态角的运动学模型，所述运动学模型包括角位置运动学模型及角速度运动学模型；
步骤二：输入仿真初始时刻的飞行器姿态γ(0)为仿真初始时刻的滚转角，为仿真初始时刻的偏航角，θ(0)为仿真初始时刻的俯仰角，测量四轴转台各框架角的初始位置Φ0＝[φ10,φ20,φ30,φ40]T，根据建立的四轴转台框架角与飞行器姿态角的角位置运动学关系，对仿真初始时刻的各框架角位置进行初始化，得到Φ(0)＝[φ1(0),φ2(0),φ3(0),φ4(0)]T；
步骤三：设计基于约束最优化理论的转台运动学反解算法，选取最优化问题中的优化变量即各框架角位置Φ＝[φ1,φ2,φ3,φ4]T、约束条件h(Φ)＝[h1(Φ),…,hm(Φ)]T、m表示约束条件的个数，目标函数f(Φ)以及优化算法；
步骤四：输入当前时刻即第n时刻待模拟的飞行器姿态前一时刻即第n-1时刻的飞行器姿态仿真解及前一时刻四轴转台的各框架角位置Φ(n-1)＝[φ1(n-1),φ2(n-1),φ3(n-1),φ4(n-1)]T，根据运动学反解算法，计算得到当前时刻四轴转台各框架角位置指令Φ(n)＝[φ1(n),φ2(n),φ3(n),φ4(n)]T；
步骤五：设计四轴转台的运动学正解算法，根据当前时刻各框架角位置指令Φ(n)＝[φ1(n),φ2(n),φ3(n),φ4(n)]T，解算得到飞行器当前时刻的姿态仿真解并将输入到下一时刻的运动学反解计算中；
步骤六：往复迭代进行上述步骤三至步骤五，解算得到框架角位置的全部指令。


2.根据权利要求1所述的方法，其特征在于：所述四轴转台为立式四轴转台。


3.根据权利要求2所述的方法，其特征在于：所述步骤一具体为：
以2-3-1转序姿态角描述飞行器姿态的正欧拉法表示角位置运动学模型为：
RGb＝RGl(1)
以2-1-3转序姿态角描述飞行器姿态的反欧拉法表示角位置运动学模型为：
RrGb＝RGl(2)









其中，RGb表示描述飞行器以2-3-1转序姿态角发生姿态变化的姿态矩阵，RrGb表示描述飞行器以2-1-3转序姿态角发生姿态变化的姿态矩阵，RGl表示描述立式四轴转台负载发生姿态变化的姿态矩阵；γ，θ分别表示正欧拉法中的滚转角、偏航角及俯仰角；γr，θr分别表示反欧拉法中的滚转角、偏航角及俯仰角；φ1，φ2，φ3，φ4分别表示立式四轴转台由外至内四个框架的框架角；G表示地面坐标系，b表示飞行器坐标系，l表示负载坐标系；
以2-3-1转序姿态角描述飞行器姿态的正欧拉法表示角速度运动学模型为：



以2-1-3转序姿态角描述飞行器姿态的反欧拉法表示角速度运动学模型为：












其中，JGb表示描述飞行器以2-3-1转序姿态角运动时的雅可比矩阵，分别表示正欧拉法中的滚转角速度、偏航角速度及俯仰角速度；JrGb表示描述飞行器以2-1-3转序姿态角运动时的雅可比矩阵，分别表示反欧拉法中的滚转角速度、偏航角速度及俯仰角速度；JGl表示描述立式四轴转台负载运动时的雅可比矩阵，分别表示立式四轴转台由外至内四个框架的框架角速度；飞行器的旋转矢量记作ωb＝[ωbx,ωby,ωbz]T，其中ωbx、ωby、ωbz分别为旋转矢量在飞行器坐标系X轴、Y轴、Z轴方向的投影；负载的旋转矢量为ωl＝[ωlx,ωly,ωlz]T，其中ωlx、ωly、ωlz分别为旋转矢量在负载坐标系X轴、Y轴、Z轴方向的投影；
对于公式(6)及公式(7)中的各个角速度，在工程中都采用角位置差分来近似表示，设采样时间间隔为Δt，当前时刻飞行器的姿态角分别为γn，θn，γrn，θrn，其前一时刻各姿态角则为γn-1，θn-1，γrn-1，θrn-1，则有：



设公式(1)(2)中姿态矩阵RGb＝RrGb＝R，矩阵中第(i,j)项记作Rij，则描述同一姿态变化时由正欧拉角求反欧拉角的转换公式如下：



由反欧拉角求解正欧拉角时，正欧拉角转换公式如下：



公式(12)及(13)中，k＝0或1；
定义取舍函数Δ(x1,x2)如公式(14)所示：
Δ(x1,x2)＝mi...

【专利技术属性】
技术研发人员：陈松林，李宏禄，邢宝祥，杨宝庆，
申请(专利权)人：哈尔滨工大航博科技有限公司，
类型：发明
国别省市：黑龙江;23