技术特征：
1.一种全姿态四轴转台框架角指令解算方法，其特征在于：所述方法具体包括以下步骤：步骤一：根据四轴转台的轴系在空间中的组合方式，建立坐标系，结合四轴转台的结构及末端执行器姿态变化的原理，建立基于四轴转台框架角与末端执行器的运动学模型，所述运动学模型包括角位置运动学模型及角速度运动学模型；步骤二：根据实际四轴转台运行过程中存在的物理转角约束，使用解析法分析末端执行器的三个欧拉角的工作空间；步骤三：以工作空间为依据来设计末端执行器的姿态期望指令，从而满足后续解算算法可以解出可行的框架角度指令，并根据末端执行器的期望指令的初始时刻，对仿真初始时刻的各框架角位置进行初始化，得到φ(0)＝[φ1(0),φ2(0),φ3(0),φ4(0)]
t
；其中，φ(0)表示初始时刻的各框架角位置，φ1(0),φ2(0),φ3(0),φ4(0)表示四个框架角位置初始值；步骤四：设计基于边界斥力优化加权最小范数理论的逆运动学解算算法，根据步骤二中设置的转角约束来选择合适权重以及边界斥力项函数，以此满足对框架角位置的限制；步骤五：通过设计防止突变的正运动学解算方法，以此实现从框架角位置得到末端执行器的位姿，结合正逆运动学解算算法来实现位置反馈参与角速度计算的算法，以此来对解算误差进行收敛，以提高主任务的精度。2.根据权利要求1所述的方法，其特征在于：所述四轴转台为立式四轴转台或卧式四轴转台。3.根据权利要求2所述的方法，其特征在于：所述立式四轴转台为冗余自由度机构，其结构是采用由外至内的串联结构，相邻的两个框轴均垂直正交，各框绕自身的轴转动，四个框的回转轴系相交于一点，四个框架由外至内分别被称为基框架、外框架、中框架和内框架，也分别被称作1框、2框、3框、4框；负载固定在内框架上，负载的姿态变化是由四个框架角的组合运动实现；所述建立坐标系包括：惯性坐标系ox
g
y
g
z
g
：假设其固联于转台基座，其中坐标系的原点o固定位于转台中心负载的中心，即转台四个回转轴的交点；oy轴垂直地面向上，ox轴沿着负载的纵轴线，其正方向指向负载的纵向前进方向，oz轴向按右手法则确定；四轴转台的每个框上建立相应的体坐标系，各框的体坐标系初始原点与惯性坐标系原点相同，坐标的各个轴向的初始指向与惯性坐标系的方向一致；其中ox
b
y
b
z
b
表示基框架坐标系，ox
o
y
o
z
o
表示外框架坐标系，ox
m
y
m
z
m
表示中框架坐标系，ox
i
y
i
z
i
表示内框架坐标系，ox
l
y
l
z
l
表示负载坐标系，所述负载坐标系始终与内框架坐标系重合。4.根据权利要求3所述的方法，其特征在于：关于四轴转台的框架角和末端执行器负载的运动学关系，根据机器人领域关节旋转运动学的概念，可以得到：
其中，r
gb
表示描述末端执行器负载以欧拉角2-3-1转序姿态角发生姿态变化的姿态矩阵，r
gl
表示描述立式四轴转台负载发生姿态变化的姿态矩阵；γ，θ分别表示姿态角中的滚转角、偏航角及俯仰角，φ1，φ2，φ3，φ4分别表示立式四轴转台由外至内四个框架的框架角；根据转台的工作原理，有r
gb
＝r
gl
，由此可以建立四轴转台框架和末端负载位姿之间的角位置关系；下面建立四轴转台框架和末端负载位姿的角速度关系：下面建立四轴转台框架和末端负载位姿的角速度关系：其中，j
gb
表示描述飞行器以2-3-1转序姿态角运动时的雅可比矩阵，j
gl
表示描述立式四轴转台负载运动时的雅可比矩阵，分别表示立式四轴转台由外至内四个框架的框架角速度；负载的旋转矢量为ω
l
＝[ω
lx
,ω
ly
,ω
lz
]
t
。5.根据权利要求4所述的方法，其特征在于：所述步骤二具体为：由实际四轴转台在实际使用中存在的避免视场遮挡和避免奇异的需求，需要对外框架角和中框架角的转角进行转动范围约束；不妨设对外框架角φ2和中框架角φ3的限制范围为：φ2∈[-α
l
，α
u
]，φ3∈[-β
l
，β
u
]；框架角转动范围约束对工作空间中俯仰角θ的影响：根据与四轴转台框架角位置的关系，θ＝＝arcsin(sinφ2cosφ3)，当此时外框架和中框架角的活动范围大，sinφ2∈[-1,1]，cosφ3∈[cosβ,1]，则此时，sinφ2cosφ3∈[-1,1]，故俯仰角工作空间为：θ
range
＝arcsin(sinφ2cosφ3)∈[-π,π]
ꢀꢀꢀꢀꢀꢀꢀꢀ
(5)在考虑视场遮挡的情况下，对于外框架角的限制是不对称的，对中框架角的限制也更加严格，即当且β
l
＝β
u
＝βsinφ2∈[-sinα,1]，cosφ3∈[cosβ,1]，此时sinφ2cosφ3∈[-sinα
l
,1]或sinφ2cosφ3∈[-cosβ,1]，故俯仰角工作空间如下所示：θ
range
＝arcsin(sinφ2cosφ3)∈[-arcsin(max(sinα
l
,cosβ)),π]
ꢀꢀꢀꢀꢀꢀꢀꢀ
(6)框架角转动范围约束对工作空间中偏航角的影响：在考虑视场遮挡的情况下，即外框架角和中框架角的约束范围满足，且β
l
＝β
u
＝β时，
得到偏航角与四轴转台框架角位置的关系：若sinφ1＝0，式(7)变为其中cosφ2∈[cosα
l
,1]，tanφ3∈[-tanβ,tanβ]，故偏航角的工作空间如下：此时偏航角的工作区间范围是以φ1为对称中心的一个对称区间，区间的大小由β和α
l
确定；若sinφ1≠0，对式(7)右端上下同时除以sinφ1，得到：当tanφ1→
∞，其中cosφ2∈[cosα
l
,1]，tanφ3∈[-tanβ,tanβ]，故可以得到，此时的工作区间范围为：经过对比式(8)和(10)，发现二者表示的工作空间大小是相等的，且都是关于φ1为对称中心的对称区间，在四轴转台的实际应用中，并不会对φ1的运行角度范围做约束，因此实际上，随着φ1的变化，偏航角的工作空间可以遍历整个圆周范围，说明其可以工作空间全范围可达；框架角转动范围约束对工作空间中滚转角γ的影响：在考虑视场遮挡的情况下，即外框架角和中框架角的约束范围满足，且β
l
＝β
u
＝β时，得到滚转角与四轴转台框架角位置的关系：滚转角的工作空间γ
range
为以φ4为对称中心，半径为arctan(sinβtanα
l
)的对称区间，且在四轴转台的实际应用中，并不会对φ4的运行角度范围做约束，因此实际上，随着φ4的变化，滚转角γ的工作空间可以遍历整个圆周范围，说明其可以工作空间全范围可达。6.根据权利要求5所述的方法，其特征在于：所述步骤三具体为：由框架角运动范围限制的情况下的末端执行器的工作空间可知，在满足避免视场遮挡情况下限制外框架角φ2和中框架角φ3时，末端执行器三个位姿中的俯仰角θ的工作空间会受到影响，因此在设计期望指令曲线的时候，重点要关注俯仰角的指令曲线；假设末端负载的工作空间为，那么在理论上，要满
足后续的逆运动学解算算法可以根据末端执行器姿态指令解出框架角的速度，指令的必要条件是：其中γ
e
,θ
e
分别是需要设计的末端执行器位姿中的滚转角、偏航角、俯仰角预期指令；姿态指令的形式采用符合飞行器在做各种角度机动时的正余弦形式，且设置初始时刻的速度为零；末端执行器姿态指令的幅值和截距取决于姿态角工作空间的大小和偏移量；设计末端执行器姿态幅值和截距为：其中，分别表示为了避免末端指令到达工作空间边界的幅值缩减项；设计末端执行器的姿态指令曲线之后，为了解算出的框架角的指令形式也满足初始速度为零，保证物理可实现性，在正式进行解算之前，需要进行框架角位置初始化，即满足初始时刻框架角位置的组合传递到末端的姿态和给定的末端姿态指令的初始点相等，即：其中，r
x
,r
y
,r
z
分别表示绕着x,y,z轴旋转的旋转矩阵；设置先验信息：不妨假设初始时刻设的四轴转台的基框架角度φ1(0)为此时φ1(0)作为一个已知量参与到运算中，式(13)两端同时右乘r
y-1
(φ1(0))，变为：此时通过四个量计算三个量，是可以找到唯一确定的三个量的，此时便可以通过设计算法来确定其余几个框架角的位置；令r
x
(φ4(0))r
y
(φ3(0))r
z
(φ2(0))＝t，则有其中，t
11-t
33
表示t中元素；则立式四轴转台内部俯仰外框架、偏航中框架及滚转内框架的初始角位置则立式四轴转台内部俯仰外框架、偏航中框架及滚转内框架的初始角位置求解算法如下：其中，
7.根据权利要求6所述的方法，其特征在于：所述设计基于边界斥力优化加权最小范数理论的逆运动学解算算法具体为：首先根据步骤一中建立的运动学模型，四轴转台框架角和末端负载姿态的运动学表示为如下等式：当雅克比矩阵j
gl
是方阵，满足等式的解是唯一的，对于冗余自由度机构，存在非零的齐次解，可以表示为如下通用形式：其中，是雅克比矩阵j
gl
的伪逆矩阵，是雅克比矩阵j
gl
的零空间算子，为任意向量；加权最小范数方法提出了加权关节速度概念，以及对应的加权雅克比，表示为：式中，j表示雅克比通用形式，为加权矩阵，其为对角阵且正定，为加权关节速度；采用如上变换，解算的等式可以重新定义为：若加权雅克比矩阵j
w
满秩，则原本的系统的加权最小范数解为：加权矩阵的设计方法满足关节限位，具体为：首先将第i关节的可行区间[φ
imin
,φ
imax
]划分为三个范围，负限位区间正常运行区间以及正限位区间权重w
i
可以定义为：加入阻尼因子λ，此时加权最小范数解转化为：其中，i
m
是维度为m的单位矩阵；
阻尼因子λ的设置方法为：σ
min
是矩阵jw-1/2
的最小奇异值，ε定义了近奇异的范围，定义了阻尼因子的最大值；在上式(21)的基础上加入边界斥力项：(h-w-1
)α(φ)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)式(22)中h矩阵是四阶对角矩阵：式(22)中是边界斥力调节函数，用于根据需要调节斥力的力度；第i个框架的斥力调节函数可以设置为：式中，k
i
为正常数，用来调节斥力强弱程度，k
i
越大则作用强度越大；斥力项的整体设计方法是：限位区域分为三块，正负限位和自由运动区间，当框架角运动于正限位区间时，α
i
(φ)为负值，反之当框架角运动于负限位区间时，α
i
(φ)为正值；在加入斥力项之后，可得假设第i个框架角处于负限位区间内，且非常靠近负限位q
imin
的位置，那么其对应权重的w
i
→
+∞，即w
i-1
→
0，且α(φ)＜0，由于解算式(23)中的元素jw-1
j
t
+λ2i
m
满足，jw-1
j
t
+λ2i
m
≥σ
λ
i
m
，其中，两边同时求逆，可以发现此时不等式依然成立，据此可以推出半实物仿真四轴转台的第i个框架角的速度满足不等式，即其中ρ为α(φ)模的上确界，δ为末端执行器负载角速度的上确界，则有μ＝||j
t
||
sup
≥||j
it
||
sup
，由于假设那么此时成立；可见当权重满足时，其中ε
min
＝|α
i
(φ)|σ
λ
/μ(δ+μρ)，此时算法解算得到的框架速度指令即当某个框架角对应的权重系数大到一定程度的时候，框架角会离开复限位区域，而不是继续向着负限位极限运动，相当于边界处存在一种力，驱使着框架角远离负限位极限，同理，当框架角进入正限位区间，并且在对应权重系数高于一定值后，边界斥力项也会驱使着框架角向着远离正限位极限的方向运行。8.根据权利要求7所述的方法，其特征在于：所述正运动学解算过程即为：根据转台系统四个框架的角位置，解出转台终端负载即等效模拟的飞行器三个姿态角的位置，求解的过程中存在四象限的反正切函数的解算，而四象限反正切函数的计算需要进行鉴别，且需
保证每两个时刻解算出的值不会有跳变，即正运动学解算得到的末端执行器姿态的连续性，以确保物理可实现性。9.一种电子设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-8所述方法的步骤。10.一种计算机可读存储介质，用于存储计算机指令，其特征在于，所述计算机指令被处理器执行时实现权利要求1-8所述方法的步骤。
技术总结
本发明提出了一种全姿态四轴转台框架角指令解算方法、设备和介质，所述方法通过数学建模理论分析在给定关节转角约束下的可行的工作空间，然后在工作空间中结合机械机构的特性来设计末端位姿的指令曲线，再改进加权最小范数法的基础上引入可以迫使框架角远离限位极限的边界斥力项来进行四轴转台的逆运动学解算，继承加权最小范数法计算速度快，对子任务严格执行的优点，克服其子任务限制不够灵活的缺点，在尽可能不损失主任务精度的情况下，保证关节角位置可以平顺地限制在给定范围内，且使关节角的速度加速度分配合理，降低对关节轴驱动能力的要求，最终使得四轴转台的性能可以满足实际需求。以满足实际需求。以满足实际需求。
技术研发人员：陈松林 邢凯 邢宝祥
受保护的技术使用者：哈尔滨工大航博科技有限公司
技术研发日：2022.01.26
技术公布日：2022/5/20