一种全姿态四轴转台框架角指令解算方法、设备和介质与流程

1.本发明属于飞行器半实物仿真技术领域，特别是涉及一种全姿态四轴转台框架角指令解算方法、设备和介质。


背景技术：

2.随着航空航天技术的快速发展，要求必须缩短各种飞行器的研制周期、降低研制成本，因此促进了飞行器仿真技术的产生和发展，在各种仿真方法中，地面半实物仿真拥有其不可比拟的优越性，能够有效模拟飞行器实际运行的真实情况，可以实现对飞行器制导系统、控制系统及软硬件的功能和性能进行充分的测试，极大地加快了研制进度，降低了研制成本，因此具有非常重要的应用价值。
3.在飞行器半实物仿真系统中，飞行仿真转台是最常用的硬件设备，它可以模拟飞行器在空中的滚转、偏航、俯仰三个姿态的变化，其中四轴仿真转台通过在三轴转台的基础上增加一个冗余自由度可以用来改善系统的运动学及动力学特性，如规避奇异位置、躲避障碍、框架角限位、增加灵活性、优化框架角运动学性能等，因此四轴转台可以实现飞行器的全姿态运动仿真。但不同于三轴转台各个框架角与模拟飞行器的滚转、偏航、俯仰姿态角之间的一一对应关系，四轴转台的四个框架角与三个姿态角存在非线性映射关系。对于系统给定的某种特定的飞行器姿态指令信号，四轴转台可以由四个框架的无数种角度组合来复现姿态，但其逆运动学解算存在多解问题。因此，如何实现对各个框架的角位置进行指令分配，以达到在避免奇异的同时，满足转台自身的物理约束。
4.目前，对于含有约束的冗余自由度机构的逆运动学求解目前主要有梯度投影法、扩展雅克比矩阵法、优化方法、加权最小范数法等算法。liegeois等人提出基于次任务约束的梯度投影法，通过建立雅可比矩阵零空间来尽量实现次任务的优化。l.sciavicco等提出扩展jacobian方法，其主要思想是将姿态复现视为主要任务，将避免奇异、回避障碍等视为次要任务，将次要任务以等式的形式描述，并且与速度的雅克比矩阵组成增广的矩阵，可以弥补雅克比矩阵秩的不足，但是这种方法可能通过次要任务人为引入额外的奇异，使机构的奇异位形更加复杂。chen等人提出采用二次规划方法求解带约束的运动冗余问题，将不同任务转化为不同的等式或不等式约束条件，进而转化为带约束的优化问题，并选取不同的优化方法进行求解。xu z等人使用最小速度范数方案，将控制问题被表述为多重约束下的二次规划情况，然后建立一个考虑系统模型的深度循环神经网络来在线解决二次规划问题。chan等人提出了通过求取加权最小范数解来避免冗余机构关节极限的方法，该方法不再构建雅可比矩阵的零空间，而是通过权重系数调节相应的关节速度进而实现其他的次任务约束，可以应对多项任务，当限制其运行范围时候，可以有效地停止关节运动并始终将关节保持在限制范围内，但是其限位过程中权重的切换不够灵活，容易使关节陷入死区，导致运动性能变差。
5.综上，目前常见的冗余机构的逆运动学解算方法较少能够满足飞行器四轴转台的使用场景，这些方法存在无法严格限制关节的转角范围，引入额外的奇异位形导致求解复
杂，实时性差等缺点，在实际四轴转台的运行中，会遇到转角限制，轴系电机功率力矩限制，末端负载位姿精度要求等诸多限制，因此，现有冗余机构逆运动解算方法无法满足四轴转台的实际需求。


技术实现要素：

6.本发明为了克服现有飞行器仿真四轴转台逆运动学解算中存在关节运动无法满足物理约束、关节运动速度加速度过大导致对电机驱动能力达不到要求、末端负载误差过大等缺陷，提出一种全姿态四轴转台框架角指令解算方法、设备和介质。
7.本发明是通过以下技术方案实现的，本发明提出一种全姿态四轴转台框架角指令解算方法，所述方法具体包括以下步骤：
8.步骤一：根据四轴转台的轴系在空间中的组合方式，建立坐标系，结合四轴转台的结构及末端执行器姿态变化的原理，建立基于四轴转台框架角与末端执行器的运动学模型，所述运动学模型包括角位置运动学模型及角速度运动学模型；
9.步骤二：根据实际四轴转台运行过程中存在的物理转角约束，使用解析法分析末端执行器的三个欧拉角的工作空间；
10.步骤三：以工作空间为依据来设计末端执行器的姿态期望指令，从而满足后续解算算法可以解出可行的框架角度指令，并根据末端执行器的期望指令的初始时刻，对仿真初始时刻的各框架角位置进行初始化，得到φ(0)＝[φ1(0),φ2(0),φ3(0),φ4(0)]
t
；其中，φ(0)表示初始时刻的各框架角位置，φ1(0),φ2(0),φ3(0),φ4(0)表示四个框架角位置初始值；
[0011]
步骤四：设计基于边界斥力优化加权最小范数理论的逆运动学解算算法，根据步骤二中设置的转角约束来选择合适权重以及边界斥力项函数，以此满足对框架角位置的限制；
[0012]
步骤五：通过设计防止突变的正运动学解算方法，以此实现从框架角位置得到末端执行器的位姿，结合正逆运动学解算算法来实现位置反馈参与角速度计算的算法，以此来对解算误差进行收敛，以提高主任务的精度。
[0013]
进一步地，所述四轴转台为立式四轴转台或卧式四轴转台。
[0014]
进一步地，所述立式四轴转台为冗余自由度机构，其结构是采用由外至内的串联结构，相邻的两个框轴均垂直正交，各框绕自身的轴转动，四个框的回转轴系相交于一点，四个框架由外至内分别被称为基框架、外框架、中框架和内框架，也分别被称作1框、2框、3框、4框；负载固定在内框架上，负载的姿态变化是由四个框架角的组合运动实现；
[0015]
所述建立坐标系包括：
[0016]
惯性坐标系oxgygzg：假设其固联于转台基座，其中坐标系的原点o固定位于转台中心负载的中心，即转台四个回转轴的交点；oy轴垂直地面向上，ox轴沿着负载的纵轴线，其正方向指向负载的纵向前进方向，oz轴向按右手法则确定；
[0017]
四轴转台的每个框上建立相应的体坐标系，各框的体坐标系初始原点与惯性坐标系原点相同，坐标的各个轴向的初始指向与惯性坐标系的方向一致；其中oxbybzb表示基框架坐标系，ox
oyozo
表示外框架坐标系，oxmymzm表示中框架坐标系，oxiyizi表示内框架坐标系，ox
lylzl
表示负载坐标系，所述负载坐标系始终与内框架坐标系重合。
[0018]
进一步地，关于四轴转台的框架角和末端执行器负载的运动学关系，根据机器人领域关节旋转运动学的概念，可以得到：
[0019][0020][0021]
其中，r
gb
表示描述末端执行器负载以欧拉角2-3-1转序姿态角发生姿态变化的姿态矩阵，r
gl
表示描述立式四轴转台负载发生姿态变化的姿态矩阵；γ，θ分别表示姿态角中的滚转角、偏航角及俯仰角，φ1，φ2，φ3，φ4分别表示立式四轴转台由外至内四个框架的框架角；根据转台的工作原理，有r
gb
＝r
gl
，由此可以建立四轴转台框架和末端负载位姿之间的角位置关系；
[0022]
下面建立四轴转台框架和末端负载位姿的角速度关系：
[0023][0024][0025]
其中，j
gb
表示描述飞行器以2-3-1转序姿态角运动时的雅可比矩阵，j
gl
表示描述立式四轴转台负载运动时的雅可比矩阵，分别表示立式四轴转台由外至内四个框架的框架角速度；负载的旋转矢量为
[0026]
进一步地，所述步骤二具体为：
[0027]
由实际四轴转台在实际使用中存在的避免视场遮挡和避免奇异的需求，需要对外框架角和中框架角的转角进行转动范围约束；
[0028]
不妨设对外框架角φ2和中框架角φ3的限制范围为：φ2∈[-α
l
，αu]，φ3∈[-β
l
，βu]；
[0029]
框架角转动范围约束对工作空间中俯仰角θ的影响：根据与四轴转台框架角位置的关系，θ＝＝arcsin(sinφ2cosφ3)，当此时外框架和中框架角的活动范围大，sinφ2∈[-1,1]，cosφ3∈[cosβ,1]，则此时，sinφ2cosφ3∈[-1,1]，故俯仰角工作空间为：
[0030]
θ
range
＝arcsin(sinφ2cosφ3)∈[-π,π]
ꢀꢀꢀ
(5)
[0031]
在考虑视场遮挡的情况下，对于外框架角的限制是不对称的，对中框架角的限制
也更加严格，即当且β
l
＝βu＝βsinφ2∈[-sinα,1]，cosφ3∈[cosβ,1]，此时sinφ2cosφ3∈[-sinα
l
,1]或sinφ2cosφ3∈[-cosβ,1]，故俯仰角工作空间如下所示：
[0032]
θ
range
＝arcsin(sinφ2cosφ3)∈[-arcsin(max(sinα
l
,cosβ)),π]
ꢀꢀꢀ
(6)
[0033]
框架角转动范围约束对工作空间中偏航角的影响：在考虑视场遮挡的情况下，即外框架角和中框架角的约束范围满足，且β
l
＝βu＝β时，得到偏航角与四轴转台框架角位置的关系：
[0034][0035]
若sinφ1＝0，式(7)变为其中cosφ2∈[cosα
l
,1]，tanφ3∈[-tanβ,tanβ]，故偏航角的工作空间如下：
[0036][0037]
此时偏航角的工作区间范围是以φ1为对称中心的一个对称区间，区间的大小由β和α
l
确定；
[0038]
若sinφ1≠0，对式(7)右端上下同时除以sinφ1，得到：
[0039][0040]
当tanφ1→
∞，其中cosφ2∈[cosα
l
,1]，tanφ3∈[-tanβ,tanβ]，故可以得到，此时的工作区间范围为：
[0041][0042]
经过对比式(8)和(10)，发现二者表示的工作空间大小是相等的，且都是关于φ1为对称中心的对称区间，在四轴转台的实际应用中，并不会对φ1的运行角度范围做约束，因此实际上，随着φ1的变化，偏航角的工作空间可以遍历整个圆周范围，说明其可以工作空间全范围可达；
[0043]
框架角转动范围约束对工作空间中滚转角γ的影响：在考虑视场遮挡的情况下，即外框架角和中框架角的约束范围满足，且β
l
＝βu＝β时，得到滚转角与四轴转台框架角位置的关系：
[0044][0045]
滚转角的工作空间γ
range
为以φ4为对称中心，半径为arctan(sinβtanα
l
)的对称
区间，且在四轴转台的实际应用中，并不会对φ4的运行角度范围做约束，因此实际上，随着φ4的变化，滚转角γ的工作空间可以遍历整个圆周范围，说明其可以工作空间全范围可达。
[0046]
进一步地，所述步骤三具体为：
[0047]
由框架角运动范围限制的情况下的末端执行器的工作空间可知，在满足避免视场遮挡情况下限制外框架角φ2和中框架角φ3时，末端执行器三个位姿中的俯仰角θ的工作空间会受到影响，因此在设计期望指令曲线的时候，重点要关注俯仰角的指令曲线；
[0048]
假设末端负载的工作空间为，那么在理论上，要满足后续的逆运动学解算算法可以根据末端执行器姿态指令解出框架角的速度，指令的必要条件是：其中γe,θe分别是需要设计的末端执行器位姿中的滚转角、偏航角、俯仰角预期指令；
[0049]
姿态指令的形式采用符合飞行器在做各种角度机动时的正余弦形式，且设置初始时刻的速度为零；
[0050]
末端执行器姿态指令的幅值和截距取决于姿态角工作空间的大小和偏移量；设计末端执行器姿态幅值和截距为：
[0051][0052]
其中，ε
γ
,ε
θ
分别表示为了避免末端指令到达工作空间边界的幅值缩减项；
[0053]
设计末端执行器的姿态指令曲线之后，为了解算出的框架角的指令形式也满足初始速度为零，保证物理可实现性，在正式进行解算之前，需要进行框架角位置初始化，即满足初始时刻框架角位置的组合传递到末端的姿态和给定的末端姿态指令的初始点相等，即：
[0054][0055]
其中，r
x
,ry,rz分别表示绕着x,y,z轴旋转的旋转矩阵；
[0056]
设置先验信息：不妨假设初始时刻设的四轴转台的基框架角度φ1(0)为此时φ1(0)作为一个已知量参与到运算中，式(13)两端同时右乘r
y-1
(φ1(0))，变为：
[0057][0058]
此时通过四个量计算三个量，是可以找到唯一确定的三个量的，此时便可以通过设计算法来确定其余几个框架角的位置；
[0059]
令r
x
(φ4(0))ry(φ3(0))rz(φ2(0))＝t，则有
[0060][0061]
其中，t
11-t
33
表示t中元素；
[0062]
则立式四轴转台内部俯仰外框架、偏航中框架及滚转内框架的初始角位置则立式四轴转台内部俯仰外框架、偏航中框架及滚转内框架的初始角位置求解算法如下：
[0063][0064]
其中，
[0065]
进一步地，所述设计基于边界斥力优化加权最小范数理论的逆运动学解算算法具体为：
[0066]
首先根据步骤一中建立的运动学模型，四轴转台框架角和末端负载姿态的运动学表示为如下等式：
[0067][0068]
当雅克比矩阵j
gl
是方阵，满足等式的解是唯一的，对于冗余自由度机构，存在非零的齐次解，可以表示为如下通用形式：
[0069][0070]
其中，是雅克比矩阵j
gl
的伪逆矩阵，是雅克比矩阵j
gl
的零空间算子，为任意向量；
[0071]
加权最小范数方法提出了加权关节速度概念，以及对应的加权雅克比，表示为：
[0072][0073]
式中，j表示雅克比通用形式，为加权矩阵，其为对角阵且正定，为加权关节速度；采用如上变换，解算的等式可以重新定义为：若加权雅克比矩阵jw满秩，则原本的系统的加权最小范数解为：
[0074]
加权矩阵的设计方法满足关节限位，具体为：
[0075]
首先将第i关节的可行区间[φ
imin
,φ
imax
]划分为三个范围，负限位区间
正常运行区间以及正限位区间权重wi可以定义为：
[0076][0077]
加入阻尼因子λ，此时加权最小范数解转化为：
[0078][0079]
其中，im是维度为m的单位矩阵；
[0080]
阻尼因子λ的设置方法为：σ
min
是矩阵jw-1/2
的最小奇异值，ε定义了近奇异的范围，定义了阻尼因子的最大值；
[0081]
在上式(21)的基础上加入边界斥力项：
[0082]
(h-w-1
)α(φ)
ꢀꢀꢀ
(22)
[0083]
式(22)中h矩阵是四阶对角矩阵：
[0084][0085]
式(22)中是边界斥力调节函数，用于根据需要调节斥力的力度；第i个框架的斥力调节函数可以设置为：
[0086][0087]
式中，ki为正常数，用来调节斥力强弱程度，ki越大则作用强度越大；
[0088]
斥力项的整体设计方法是：限位区域分为三块，正负限位和自由运动区间，当框架角运动于正限位区间时，αi(φ)为负值，反之当框架角运动于负限位区间时，αi(φ)为正值；
[0089]
在加入斥力项之后，可得
[0090][0091]
假设第i个框架角处于负限位区间内，且非常靠近负限位q
imin
的位置，那么其对应权重的wi→
+∞，即w
i-1
→
0，且α(φ)＜0，由于解算式(23)中的元素jw-1jt
+λ2im满足，jw-1jt
+λ2im≥σ
λim
，其中，两边同时求逆，可以发现此时不等式依然成立，据此可以推出半实物仿真四轴转台的第i个框架角的速度满足不等式，即其
中ρ为α(φ)模的上确界，δ为末端执行器负载角速度的上确界，则有由于假设那么此时成立；
[0092]
可见当权重满足时，其中ε
min
＝|αi(φ)|σ
λ
/μ(δ+μρ)，此时算法解算得到的框架速度指令即当某个框架角对应的权重系数大到一定程度的时候，框架角会离开复限位区域，而不是继续向着负限位极限运动，相当于边界处存在一种力，驱使着框架角远离负限位极限，同理，当框架角进入正限位区间，并且在对应权重系数高于一定值后，边界斥力项也会驱使着框架角向着远离正限位极限的方向运行。
[0093]
进一步地，所述正运动学解算过程即为：根据转台系统四个框架的角位置，解出转台终端负载即等效模拟的飞行器三个姿态角的位置，求解的过程中存在四象限的反正切函数的解算，而四象限反正切函数的计算需要进行鉴别，且需保证每两个时刻解算出的值不会有跳变，即正运动学解算得到的末端执行器姿态的连续性，以确保物理可实现性。
[0094]
本发明提出一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述全姿态四轴转台框架角指令解算方法的步骤。
[0095]
本发明还提出一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时实现所述全姿态四轴转台框架角指令解算方法的步骤。
[0096]
本发明的有益效果为：
[0097]
1.本发明根据实际四轴转台在工作中存在的必要的物理转角约束抽象化，并使用解析法分析在有约束的情况下的末端执行器的位姿的极限工作空间，此举有效地解决了以往设计末端位姿期望指令缺少理论指导的问题，对四轴转台在半实物仿真领域的应用有着重要的影响。
[0098]
2.本发明所给出的转台末端执行器指令位姿设计方法，结合实际被测试物件的运动特性及真实世界物理运动模型，在工作空间的限制下，进行合理的指令设计，并且根据所设计的指令曲线的初始时刻值，采用赋予先验信息的方式解决方程的不确定性，对四轴转台的框架角也进行相应的初始化，成功解决了飞行器姿态仿真初始时刻所对应四轴转台各框架角位置不为零的问题。
[0099]
3.本发明所给出的正运动学解算法，成功解决了在进行飞行器全姿态仿真时，传统正运动学解算法可能带来的解算结果与理论值差别较大且出现大幅度跳变的现象，改进后的算法将反三角函数的值域扩展到了全空间内。其中所给出的正解的结果作为反馈信号参与到旋转矢量计算的方法有效提升了转台末端负载的跟踪精度。
[0100]
4.本发明所给出的基于边界斥力优化加权最小范数理论的逆运动学解算算法，可以在不损失任务精度，且保证良好的实时性的前提下，根据实际物理限制的需要将中外框架角φ2和φ3限制在可行区域内，由于边界斥力函数的作用，框架角不会陷入死区中停止运动，而是将会运动在限位区间内的一个平衡位置附近，即对应框架角的逆权重为的框架角位置，此时框架不会停止运动，也不会继续向限位极限位置运动，而是产生朝着远离极限的方向运动的趋势，而且，当主任务驱动框架远离其限位时，边界斥力项能够帮助框架角加速离开限位区间，因此通过该算法解得的对应框架角的速度指令更加平滑，不会出现突增突减等超出电机驱动能力的情况，可以更好地满足四轴转台的实际
应用需求。
附图说明
[0101]
图1为本发明中基于2-3-1转序欧拉角的坐标系转换关系图。设空间中飞行器所在地面坐标系为oxgygzg，自身弹体坐标系为oxbybzb，以2-3-1转序的欧拉角表示飞行器由地面坐标系位置到弹体坐标系位置的姿态变化，可以分解为依次绕oyg、oz’、ox1轴的三次基元旋转；
[0102]
图2为本发明应用到四轴转台中的立式四轴转台的结构简图，其由外到内的四个框架依次模拟的是飞行器偏航、俯仰、偏航、滚转姿态，发生姿态变化时分别绕y轴方向、z轴方向、y轴方向、x轴方向发生旋转。其中，oxbybzb、ox
oyozo
、oxmymzm、oxiyizi分别表示基框架坐标系、外框架坐标系、中框架坐标系及内框架坐标系；
[0103]
图3为本发明满足转角约束的高实时性高精度四轴转台框架角指令解算方法的流程图；
[0104]
图4为实施例中末端执行器姿态指令示意图；
[0105]
图5为实施例中末端执行器旋转矢量示意图；
[0106]
图6为实施例中使用四轴转台解算方法得到的各框架角位置指令结果示意图；
[0107]
图7为实施例中使用四轴转台解算方法得到的各框架角速度结果示意图；
[0108]
图8为实施例中末端执行器预期姿态角与姿态角仿真解之间的解算偏差示意图。
具体实施方式
[0109]
下面将结合本发明实施例中的附图对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0110]
本发明想要解决其技术问题所采用的技术方案是：通过数学建模理论分析在给定关节转角约束下的可行的工作空间，然后在工作空间中结合机械机构的特性来设计末端位姿的指令曲线，再改进加权最小范数法的基础上引入可以迫使框架角远离限位极限的边界斥力项来进行四轴转台的逆运动学解算，继承加权最小范数法计算速度快，对子任务严格执行的优点，克服其子任务限制不够灵活的缺点，在尽可能不损失主任务精度的情况下，保证关节角位置可以平顺地限制在给定范围内，且使关节角的速度加速度分配合理，降低对关节轴驱动能力的要求，最终使得四轴转台的性能可以满足实际需求。
[0111]
结合图1-8，本发明提出一种全姿态四轴转台框架角指令解算方法，所述方法具体包括以下步骤：
[0112]
步骤一：根据四轴转台的轴系在空间中的组合方式，建立坐标系，结合四轴转台的结构及末端执行器姿态变化的原理，建立基于四轴转台框架角与末端执行器的运动学模型，所述运动学模型包括角位置运动学模型及角速度运动学模型；
[0113]
步骤二：根据实际四轴转台运行过程中存在的物理转角约束，使用解析法分析末端执行器的三个欧拉角的工作空间；
[0114]
步骤三：以工作空间为依据来设计末端执行器的姿态期望指令，从而满足后续解
算算法可以解出可行的框架角度指令，并根据末端执行器的期望指令的初始时刻，对仿真初始时刻的各框架角位置进行初始化，得到φ(0)＝[φ1(0),φ2(0),φ3(0),φ4(0)]
t
；其中，φ(0)表示初始时刻的各框架角位置，φ1(0),φ2(0),φ3(0),φ4(0)表示四个框架角位置初始值；
[0115]
步骤四：设计基于边界斥力优化加权最小范数理论的逆运动学解算算法，根据步骤二中设置的转角约束来选择合适权重以及边界斥力项函数，以此满足对框架角位置的限制；
[0116]
步骤五：通过设计防止突变的正运动学解算方法，以此实现从框架角位置得到末端执行器的位姿，结合正逆运动学解算算法来实现位置反馈参与角速度计算的算法，以此来对解算误差进行收敛，以提高主任务的精度。
[0117]
所述四轴转台为立式四轴转台或卧式四轴转台。
[0118]
所述立式四轴转台为冗余自由度机构，其结构是采用由外至内的串联结构，相邻的两个框轴均垂直正交，各框绕自身的轴转动，四个框的回转轴系相交于一点，四个框架由外至内分别被称为基框架、外框架、中框架和内框架，也分别被称作1框、2框、3框、4框；负载固定在内框架上，负载的姿态变化是由四个框架角的组合运动实现；
[0119]
所述建立坐标系包括：
[0120]
惯性坐标系oxgygzg：假设其固联于转台基座，其中坐标系的原点o固定位于转台中心负载的中心，即转台四个回转轴的交点；oy轴垂直地面向上，ox轴沿着负载的纵轴线，其正方向指向负载的纵向前进方向，oz轴向按右手法则确定；
[0121]
四轴转台的每个框上建立相应的体坐标系，各框的体坐标系初始原点与惯性坐标系原点相同，坐标的各个轴向的初始指向与惯性坐标系的方向一致；其中oxbybzb表示基框架坐标系，ox
oyozo
表示外框架坐标系，oxmymzm表示中框架坐标系，oxiyizi表示内框架坐标系，ox
lylzl
表示负载坐标系，所述负载坐标系始终与内框架坐标系重合。
[0122]
关于四轴转台的框架角和末端执行器负载的运动学关系，根据机器人领域关节旋转运动学的概念，可以得到：
[0123][0124][0125]
其中，r
gb
表示描述末端执行器负载以欧拉角2-3-1转序姿态角发生姿态变化的姿态矩阵，r
gl
表示描述立式四轴转台负载发生姿态变化的姿态矩阵；γ，θ分别表示姿态角中的滚转角、偏航角及俯仰角，φ1，φ2，φ3，φ4分别表示立式四轴转台由外至内四个框架的框架角；根据转台的工作原理，有r
gb
＝r
gl
，由此可以建立四轴转台框架和末端负载位姿之间的角位置关系；
[0126]
下面建立四轴转台框架和末端负载位姿的角速度关系：
[0127][0128][0129]
其中，j
gb
表示描述飞行器以2-3-1转序姿态角运动时的雅可比矩阵，j
gl
表示描述立式四轴转台负载运动时的雅可比矩阵，分别表示立式四轴转台由外至内四个框架的框架角速度；负载的旋转矢量为ω
l
＝[ω
lx
,ω
ly
,ω
lz
]
t
。
[0130]
所述步骤二具体为：
[0131]
由实际四轴转台在实际使用中存在的避免视场遮挡和避免奇异的需求，需要对外框架角和中框架角的转角进行转动范围约束；
[0132]
不妨设对外框架角φ2和中框架角φ3的限制范围为：φ2∈[-α
l
，αu]，φ3∈[-β
l
，βu]；
[0133]
框架角转动范围约束对工作空间中俯仰角θ的影响：根据与四轴转台框架角位置的关系，θ＝＝arcsin(sinφ2cosφ3)，当此时外框架和中框架角的活动范围大，sinφ2∈[-1,1]，cosφ3∈[cosβ,1]，则此时，sinφ2cosφ3∈[-1,1]，故俯仰角工作空间为：
[0134]
θ
range
＝arcsin(sinφ2cosφ3)∈[-π,π]
ꢀꢀꢀ
(5)
[0135]
这种情况下俯仰角是全范围可达的，这是比较理想的情况，但是根据建立的视场遮挡导致的约束范围限制表明，这两个框架角的范围限制不可能这么宽松。
[0136]
在考虑视场遮挡的情况下，对于外框架角的限制是不对称的，对中框架角的限制也更加严格，即当且β
l
＝βu＝βsinφ2∈[-sinα,1]，cosφ3∈[cosβ,1]，此时sinφ2cosφ3∈[-sinα
l
,1]或sinφ2cosφ3∈[-cosβ,1]，故俯仰角工作空间如下所示：
[0137]
θ
range
＝arcsin(sinφ2cosφ3)∈[-arcsin(max(sinα
l
,cosβ)),π]
ꢀꢀꢀ
(6)
[0138]
这种情况下，俯仰角的工作空间不是全范围可达的，且是不对称的，且正向的范围相较于反向的范围更大，这对指导末端执行器姿态期望指令曲线的设计有着很重要的意义。
[0139]
框架角转动范围约束对工作空间中偏航角的影响：在考虑视场遮挡的情况下，即外框架角和中框架角的约束范围满足，且β
l
＝βu＝β时，得到偏航角与四轴转台框架角位置的关系：
[0140]
[0141]
若sinφ1＝0，式(7)变为其中cosφ2∈[cosα
l
,1]，tanφ3∈[-tanβ,tanβ]，故偏航角的工作空间如下：
[0142][0143]
此时偏航角的工作区间范围是以φ1为对称中心的一个对称区间，区间的大小由β和α
l
确定；
[0144]
若sinφ1≠0，对式(7)右端上下同时除以sinφ1，得到：
[0145][0146]
当tanφ1→
∞，其中cosφ2∈[cosα
l
,1]，tanφ3∈[-tanβ,tanβ]，故可以得到，此时的工作区间范围为：
[0147][0148]
经过对比式(8)和(10)，发现二者表示的工作空间大小是相等的，且都是关于φ1为对称中心的对称区间，在四轴转台的实际应用中，并不会对φ1的运行角度范围做约束，因此实际上，随着φ1的变化，偏航角的工作空间可以遍历整个圆周范围，说明其可以工作空间全范围可达；
[0149]
框架角转动范围约束对工作空间中滚转角γ的影响：在考虑视场遮挡的情况下，即外框架角和中框架角的约束范围满足，且β
l
＝βu＝β时，得到滚转角与四轴转台框架角位置的关系：
[0150][0151]
滚转角的工作空间γ
range
为以φ4为对称中心，半径为arctan(sinβtanα
l
)的对称区间，且在四轴转台的实际应用中，并不会对φ4的运行角度范围做约束，因此实际上，随着φ4的变化，滚转角γ的工作空间可以遍历整个圆周范围，说明其可以工作空间全范围可达。
[0152]
综上所述，在根据避免视场遮挡的需求下进行的框架角运行范围限制下，会对末端执行器的俯仰角θ的工作空间产生影响，对欧拉角中的偏航角和滚转角γ产生影响，此结论对后续末端执行器姿态指令设计有着至关重要的作用。
[0153]
所述步骤三具体为：
[0154]
由框架角运动范围限制的情况下的末端执行器的工作空间可知，在满足避免视场遮挡情况下限制外框架角φ2和中框架角φ3时，末端执行器三个位姿中的俯仰角θ的工作空间会受到影响，因此在设计期望指令曲线的时候，重点要关注俯仰角的指令曲线；
[0155]
假设末端负载的工作空间为，那么在理论
上，要满足后续的逆运动学解算算法可以根据末端执行器姿态指令解出框架角的速度，指令的必要条件是：其中γe,θe分别是需要设计的末端执行器位姿中的滚转角、偏航角、俯仰角预期指令；
[0156]
关于末端执行器姿态指令的形式，因为转台的执行器负载一般是飞行器中用于指示姿态和导航定位用的惯导元件或者机体前端安装用于采集环境信息的摄像头等，因此，在姿态指令的形式决定采用比较符合飞行器在做各种角度机动时正余弦形式，且应该设置初始时刻的速度为零，以保证符合飞行器从静止起飞到逐渐加速的过程，符合实际物理情形。
[0157]
姿态指令的形式采用符合飞行器在做各种角度机动时的正余弦形式，且设置初始时刻的速度为零；
[0158]
关于末端执行器姿态指令的幅值和截距，这取决于姿态角工作空间的大小和偏移量，但是在工作空间的边界附近的机械结构的可操作度会比较低，这这类区域内四轴转台的速度传递效率较差，框架轴处的电机需要提供较大的扭矩和功率才能使得末端执行机构追踪上指令位姿，因此在设计期望指令曲线时，应该将这些因素纳入考虑范围，在边界处留有一定余量，设计末端执行器姿态幅值和截距为：
[0159][0160]
其中，ε
γ
,ε
θ
分别表示为了避免末端指令到达工作空间边界的幅值缩减项；
[0161]
设计末端执行器的姿态指令曲线之后，为了解算出的框架角的指令形式也满足初始速度为零，保证物理可实现性，在正式进行解算之前，需要进行框架角位置初始化，即满足初始时刻框架角位置的组合传递到末端的姿态和给定的末端姿态指令的初始点相等，即：
[0162][0163]
其中，r
x
,ry,rz分别表示绕着x,y,z轴旋转的旋转矩阵；
[0164]
但是，在没有任何先验信息可以利用的情况下，要通过三个已知量计算得到四个唯一确定的量是不可实现的，因此在初始化时，需要人为添加一些先验信息。
[0165]
设置先验信息：不妨假设初始时刻设的四轴转台的基框架角度φ1(0)为此时φ1(0)作为一个已知量参与到运算中，式(13)两端同时右乘r
y-1
(φ1(0))，变为：
[0166][0167]
此时通过四个量计算三个量，是可以找到唯一确定的三个量的，此时便可以通过设计算法来确定其余几个框架角的位置；
[0168]
令r
x
(φ4(0))ry(φ3(0))rz(φ2(0))＝t，则有
[0169][0170]
其中，t
11-t
33
表示t中元素；
[0171]
则立式四轴转台内部俯仰外框架、偏航中框架及滚转内框架的初始角位置则立式四轴转台内部俯仰外框架、偏航中框架及滚转内框架的初始角位置求解算法如下：
[0172][0173]
其中，
[0174]
所述设计基于边界斥力优化加权最小范数理论的逆运动学解算算法具体为：
[0175]
首先根据步骤一中建立的运动学模型，四轴转台框架角和末端负载姿态的运动学表示为如下等式：
[0176][0177]
当雅克比矩阵j
gl
是方阵，满足等式的解是唯一的，对于冗余自由度机构，存在非零的齐次解，可以表示为如下通用形式：
[0178][0179]
其中，是雅克比矩阵j
gl
的伪逆矩阵，是雅克比矩阵j
gl
的零空间算子，为任意向量；
[0180]
向量属于雅克比矩阵j的零空间，被称为“自运动”。投影算子被广泛地应用于处理多任务问题，在这些工作中，z是给定的关节速度，用于完成子任务的内容，采用精确的投影算子，子任务仅仅在主任务的零空间中执行，在此基础上可以定义惩罚函数产生梯度投影的方法。但是不同于梯度投影方法，本发明采用的解算算法的基础是加权最小范数法，采用权值来抑制关节速度的方式实现关节限位避障，这种方法可以令子任务严格执行且具有非常好的运算效率，满足四轴转台实时性的要求。
[0181]
加权最小范数方法提出了加权关节速度概念，以及对应的加权雅克比，表示为：
[0182]
[0183]
式中，j表示雅克比通用形式，为加权矩阵，其为对角阵且正定，为加权关节速度；采用如上变换，解算的等式可以重新定义为：若加权雅克比矩阵jw满秩，则原本的系统的加权最小范数解为：权重矩阵的设计方法有很多种，但是为了保证可以实现关节限制在一定的范围内，保证权重切换的足够平滑，并且要针对内外框架角运动性能的差异进行特殊设计，下面给出满足关节限位的权重设计方法。
[0184]
加权矩阵的设计方法满足关节限位，具体为：
[0185]
首先将第i关节的可行区间[φ
imin
,φ
imax
]划分为三个范围，负限位区间正常运行区间以及正限位区间权重wi可以定义为：
[0186][0187]
为了保证权重切换的平滑性，选择了任意阶导数都连续的正弦函数作为主要的作用函数。另外，因为四轴转台的结构特性，外框架角相比于内框架角的转动惯量要大很多，对应的运动能力也会比较差，考虑到这一点，本发明令框架角序号i参与到权重的设置中，给标号低的框架，即位于相对外部的框架，以更大的权重，来压低其运动速度。
[0188]
为了能够处理这类奇异情形，加入阻尼因子λ，此时加权最小范数解转化为：
[0189][0190]
其中，im是维度为m的单位矩阵；
[0191]
阻尼因子λ的设置方法为：σ
min
是矩阵jw-1/2
的最小奇异值，ε定义了近奇异的范围，定义了阻尼因子的最大值；
[0192]
但是这也引入了一个严重的问题，在转角限位的过程中，由于权重的变化缺少一个影响项，导致其会一直增长或者减小，于是会出现某个框架被压制到停止运行，那么此框架将会难以从限位中退出，这是因为采用连续权值的设定方法时，算法中缺少从框架角速度压制中退出的机制，导致某些框架直接停止运动，缺失的运动能力需要其余框架进行补偿，进而造成其余框架角的运动指标超出合理范围，最终使得四轴转台无法稳定运行。
[0193]
为了解决上述问题，在上式(21)的基础上加入边界斥力项：
[0194]
(h-w-1
)α(φ)
ꢀꢀꢀ
(22)
[0195]
式(22)中h矩阵是四阶对角矩阵：
[0196][0197]
式(22)中是边界斥力调节函数，用于根据需要调节斥力的力度，考虑到每个框架的特性不同，因此斥力调节函数的形式参数可以根据需要设定；第i个框架的斥力调节函数可以设置为：
[0198][0199]
式中，ki为正常数，用来调节斥力强弱程度，ki越大则作用强度越大；
[0200]
斥力项的整体设计方法是：限位区域分为三块，正负限位和自由运动区间，当框架角运动于正限位区间时，αi(φ)为负值，反之当框架角运动于负限位区间时，αi(φ)为正值；对角阵h-w-1
除了可以提供形式上的统一之外，还有一个作用就是可以令斥力任务只在正负限位区间内起到作用，并且在对角阵h-w-1
的作用下，斥力项的作用强度会根据接近限位角度的情况进行自动调节，从而对角度限位实现更明显的平滑效果。
[0201]
在加入斥力项之后，可得
[0202][0203]
式中右边的-j(h-w-1
)α(φ)是为了保证错位项不影响主任务的精度而加入的配平项，该解算算法可以在主任务速度有限的条件下，有效地实现框架角限位作用。
[0204]
假设第i个框架角处于负限位区间内，且非常靠近负限位q
imin
的位置，那么其对应权重的wi→
+∞，即w
i-1
→
0，且α(φ)＜0，由于解算式(23)中的元素jw-1jt
+λ2im满足，jw-1jt
+λ2im≥σ
λim
，其中，两边同时求逆，可以发现此时不等式依然成立，据此可以推出半实物仿真四轴转台的第i个框架角的速度满足不等式，即其中ρ为α(φ)模的上确界，δ为末端执行器负载角速度的上确界，则有由于假设那么此时成立；
[0205]
可见当权重满足时，其中ε
min
＝|αi(φ)|σ
λ
/μ(δ+μρ)，此时算法解算得到的框架速度指令即当某个框架角对应的权重系数大到一定程度的时候，框架角会离开复限位区域，而不是继续向着负限位极限运动，相当于边界处存在一种力，驱使着框架角远离负限位极限，同理，当框架角进入正限位区间，并且在对应权重系数高于一定值后，边界斥力项也会驱使着框架角向着远离正限位极限的方向运行。
[0206]
所述正运动学解算过程即为：根据转台系统四个框架的角位置，解出转台终端负载即等效模拟的飞行器三个姿态角的位置，求解的过程中存在四象限的反正切函数的解算，而四象限反正切函数的计算需要进行鉴别，且需保证每两个时刻解算出的值不会有跳变，即正运动学解算得到的末端执行器姿态的连续性，以确保物理可实现性。因此在进行一
系列有时间前后顺序的姿态的求取时，需要加入辨别是否发生跳变的筛选算法。
[0207]
以末端执行器姿态欧拉角中的滚转角γ为例，当|γc(k)-γc(k-1)|≥δ
γ
+σ
γ
，则令其中γc(k-1)和分别γc(k)为上一时刻和当前时刻正运动学解算得到的滚转角，表示从上一时刻的速度推断预测得到的位置增量，σ
γ
为人为设置的阈值。对于正解得到的末端执行器姿态欧拉角中的偏航角和俯仰角θc也经过这一步处理之后，正运动解算得到的姿态不再会发生跳变现象，保证了物理可实现性。
[0208]
接着就需要使正解的结果作为反馈信号参与到旋转矢量的计算中，以减小误差，具体的做法如下：
[0209][0210]
经过该方法处理之后，可以有效使四轴转台末端执行器位姿误差收敛，大幅度提高精度。
[0211]
本发明所述方法既能兼顾四轴转台各框架角解算精度、实时性，保证框架角严格限制在可运行区域内，又能优化框架角运动学性能等要求。
[0212]
本发明提出一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，所述处理器执行所述计算机程序时实现所述全姿态四轴转台框架角指令解算方法的步骤。
[0213]
本发明还提出一种计算机可读存储介质，用于存储计算机指令，所述计算机指令被处理器执行时实现所述全姿态四轴转台框架角指令解算方法的步骤。
[0214]
为验证发明的有效性，以某型立式四轴转台为研究对象，待模拟的飞行器姿态以2-3-1转序输入到转台系统中，其采样频率为2000hz，即采样时间间隔为δt＝0.0005s，飞行器飞行总时长约为30s。设定外中框架角φ2和φ3的约束范围为φ2∈[-80
°
,220
°
]、φ3∈[-85
°
,85
°
]，下面通过全姿态四轴转台框架角指令解算方法对框架角指令进行解算，验证本发明的有效性。具体步骤如下：
[0215]
(1)根据实际四轴转台运行过程中存在的物理转角约束，使用解析法分析末端执行器的三个欧拉角的工作空间，由中框架角φ2和φ3的约束范围为φ2∈[-80
°
,220
°
]、φ3∈[-85
°
,85
°
]，α
l
＝80
°
α
l
＝220
°
β
l
＝βu＝β＝85
°
经过解析法分析之后可以得到四轴转台末端执行器俯仰角工作空间如下：
[0216]
θ
range
＝arcsin(sinφ2cosφ3)∈[-arcsin(max(sinα
l
,cosβ)),π]＝[-80
°
，180
°
]。
[0217]
(2)以工作空间为依据来设计末端执行器的姿态期望指令，从而满足后续解算算法可以解出可行的框架角度指令，设定如下γe＝170
°
cos(0.05t)、θe＝75
°
cos(0.08t)+90
°
；
[0218]
根据末端执行器的期望指令的初始时刻，对仿真初始时刻的各框架角位置进行初始化，得到初始框架角位置的组合为：
[0219]
[φ1(0)＝0
°
φ2(0)＝15.231
°
φ3(0)＝9.659
°
φ4(0)＝12.617
°
]
[0220]
(3)将每一时刻预期的飞行器姿态角、前一时刻的飞行器姿态仿真解及前一时刻的四轴台各框架角位置输入到系统中，解算得到当前时刻对应的各框架角位置指令。
[0221]
(4)将当前时刻的各框架角位置指令投入到运动学正解计算中，解算得到当前时刻飞行器姿态的仿真解，并作为反馈信号参与到末端执行器旋转矢量的计算中。
[0222]
(5)重复进行上述步骤(3)(4)，逐渐得到全部框架角位置指令。
[0223]
根据本发明中的满足转角约束的高实时性高精度四轴转台框架角指令解算方法,最终得到的在工作空间指导下的末端执行器姿态指令如图4所示，位置反馈参与的末端执行器旋转矢量如图5所示，经过边界斥力加权最小范数法解算得到的四轴转台的框架角位置和速度指令如图6，7所示，末端执行器预期姿态角与姿态角仿真解之间的解算偏差如图8所示，展示了本发明提出的满足转角约束的高实时性高精度四轴转台框架角指令解算方法可以将框架角限制在根据实际使用情况设定的约束之内，其解算得到速度指令可以大幅度降低对轴系电机的驱动能力的需求，并且有着很好的解算精度和运算效率，可以满足高精度和高实时性的使用情况。
[0224]
以上对本发明所提出的一种全姿态四轴转台框架角指令解算方法、设备和介质进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。