一种持续载荷模拟器多轴耦合运动奇异性控制方法与流程

本发明属于载荷模拟器技术领域，具体涉及一种持续载荷模拟器多轴耦合运动奇异性控制方法。

背景技术：

随着航空技术的发展和战机机动性能的提升，加速度意识丧失(g-loc)和空间定向障碍(sd)问题日益突出，已成为危害世界各国飞行安全的主要因素。针对此类问题，目前世界各主要航空大国都采用载人离心机进行训练。随着载人离心机的快速发展以及飞行训练科目对装备的需求，现在的载人离心机不仅仅是提供持续过载的模拟，还在向着全姿态模拟的方向快速发展，是一种能够提供持续载荷的飞行模拟器。三轴载人离心机控制姿态只有两个轴，不能实现全姿态的飞行模拟，更多轴的持续载荷模拟器应运而生，在四、五、六轴持续载荷模拟器上，可以通过俯仰、滚转和偏航三个转动框来实现对座舱姿态的控制。

当姿态控制的转轴增加到三个时，增加了很多控制计算上的问题，万向锁的问题就是其中一个，万向锁在更广泛的数学意义上属于奇异性概念。万向锁和奇异性的关系是在万向锁是针对多框嵌套这种万向架的结构下的奇异问题。

万向锁的数学直观解释是控制矩阵不满秩，呈现奇异性，在求逆运动的时候出现无解的情况。例如在外框为偏航，中框为滚转，内框为俯仰的结构下，如果中框转到90°时，外框和内框都将只改变座舱的俯仰状态，无法通过轴运动改变座舱的偏航姿态，这时，关于座舱的姿态控制就丢失了一个自由度。现有的持续载荷模拟器研究也没有关于多轴耦合的奇异性问题研究。

技术实现要素：

本发明目的在于提供一种持续载荷模拟器多轴耦合运动奇异性控制方法，用于解决上述现有技术中存在的技术问题之一，如：现有技术中，现有的持续载荷模拟器研究也没有关于多轴耦合的奇异性问题研究。

为实现上述目的，本发明的技术方案是：

一种持续载荷模拟器多轴耦合运动奇异性控制方法，包括以下步骤：

s1：建立模拟器的轴运动数学关系；

s2：在姿态控制的雅克比中将主臂与其他转轴进行解耦合；

s3：对当前结果进行奇异程度的量化；

s4：对奇异程度量化进行快速求解；

s5：解算出奇异程度量化结果后，设计奇异控制模块。

进一步的，步骤s1具体如下：

依据持续载荷模拟器的运动轴连接情况，构建运动学雅克比矩阵，用数学的方法直观表示轴转速和座舱转速之间的运动学关系。

进一步的，步骤s2具体如下：

在姿态控制的雅克比中需要将主臂与其他转轴进行解耦合，处理方法为如下：

其中v是末端的速度，ω是末端的角速度，jli第i个关节与平移相关的雅克比转换矩阵，jai第i个关节与角运动相关的雅克比转换矩阵，是第i个运动关节的速度；

所以与主轴解耦之后的其他轴运动控制的雅克比矩阵可以表示为：

其中jli第i个关节与平移相关的雅克比转换矩阵，jai第i个关节与角运动相关的雅克比转换矩阵。

进一步的，步骤s3具体如下：

对当前结果进行奇异程度的量化，选用矩阵的条件数作为奇异程度量化标准；条件数的定义：

cond(a)v＝||a^-1||v||a||v，其中(v＝1,2或∞)；

其中谱条件数为：

条件数越大，代表当前各个轴的转动位置越接近奇异点。

进一步的，步骤s4具体如下：

奇异控制采用近似的求解方法，采用条件数的倒数和中间框的角度的齐次关系拟合来快速求解条件数；

其中ki表示对应阶次的系数，β为将当前中框转角θ映射到范围内的坐标值；选择一次拟合关系：

其中

进一步的，步骤s5具体如下：

快速估算出当前位置的奇异程度量化结果后，就需要对当前解算的速度输出进行奇异控制；

其中，奇异控制模块在整个运动控制结构中的位置为：

首先角速度输入至逆运动解算模块进行逆运动解算，然后输入至奇异性控制模块进行奇异性控制；然后奇异性控制信号一方面输入至正运动解算模块进行正运动解算，然后输入至输出比较模块；奇异性控制信号另一方面输入至速度积分器模块解算角位置，然后将角位置一方面输入至逆运动计算模块进行逆运动解算，将角位置另一方面输入至奇异量化模块进行奇异量化，然后在输入至奇异性控制模块；

奇异性控制模块的主要功能是以奇异量化结果为依据对逆运动解算结果进行比例限幅输出；建立比例系数和条件数的关系如下：

其中k为限幅比例系数，nc为条件数，a、b为参数；

以轴运动的限速和限幅作为更高一层的安全限制，奇异控制模块的结构为：

转动位置经过条件数计算模块的计算后得出实时条件数，并将实时条件数输入至奇异性控制模块；逆运动解算模块的逆运动解算结果也输入至奇异性控制模块；周运动限速和限幅也输入至奇异性控制模块；最后奇异性控制模块输出控制输出信号；

轴运动的控制指令有lm和ls两个限制条件；

lm是轴运动的机械特性限制，各个轴的设计运动参数；ls为基于条件数的奇异性解算限制；

轴运动输出控制指令可以表示为：

c内外＝min{lm,ls·ci}；

中框可根据需要进行设定；

c中＝min{lm,c}。

与现有技术相比，本发明所具有的有益效果为：

本方案的一个创新点在于，在充分利用轴运动行程的情况下，进行了奇异点的过渡。通过控制指令比例缩小的方式来避免奇异点的附近因为轴机械范围限制而出现突变的情况，从而提高飞行模拟的平稳性和逼真度。

本方案的一个创新点在于，利用当前结构下的条件数拟合的方式实现在实时解算的场景下对结构奇异程度的快速估计，提升控制算法对奇异问题的处理速度，提高模拟效果。

本方案的一个创新点在于，在保证逼真度的同时实现控制系统的奇异区域过渡，最大限度地利用轴运动行程。

本方案的一个创新点在于，通过雅克比矩阵右乘向量的拆分，实现主臂与姿态框的解耦。该方案可以应用于其他机器人中部分关节因他用或故障等原因出现无法控制情况下的处理方法；通过雅克比矩阵条件数实现对持续载荷模拟器结构下运动奇异位形的衡量，使得持续载荷模拟器结构在不同位置的奇异性能够被量化处理；条件数能够量化机械结构的奇异性，但是实时运算量比较大，通过条件数倒数的齐次拟合实现对条件数的近似估计，从而降低奇异控制模块的运算量；更大范围地，对于任意的串联或者并联机械结构，一旦结构确定，也就是各个轴之间的运动关系确定，那么描述这个结构的雅克比矩阵就确定了，可以通过对该雅克比矩阵进行条件数分析建立条件数与轴位置之间的关系，从而提高条件数的实时计算效率，降低系统的运算量；奇异过渡模型的基本结构，利用轴的固有机械特性和当前轴位置对持续载荷模拟器奇异进行过渡控制，在奇异性逐渐增加或减小的情况下，避免了大部分指令输出的突然限幅，充分利用了轴运动范围。

附图说明

图1是本发明具体实施方式的奇异控制模块在整个运动控制结构中的位置示意图。

图2是本发明具体实施方式的奇异性控制模块结构示意图。

图3是本发明具体实施方式的步骤流程示意图。

具体实施方式

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

实施例：

本发明主要解决四、五、六轴持续载荷模拟器的奇异性问题，奇异问题的最直接表现是欧拉角的万向锁问题。万向锁问题在欧拉角上可以通过四元数、双欧法等数学变换来解决奇异问题，但是在具体的机械结构下，很难实现结构变换来回避奇异，所以需要通过控制算法来进行奇异点的回避。

奇异性问题在持续载荷模拟器多轴耦合控制中主要表现在：

1、当结构处于奇异点时，座舱可控的实际操作自由度减少，无法通过控制逆解算来实现某些需要模拟的姿态；

2、接近奇异点状态时，为了模拟某方向上一个小的转动，需要部分关节角速度变的很大，容易引起控制失控，超出轴运动的转速限制。

针对奇异问题的处理，在机器人领域大多采用规避的方法，但是在目前的持续载荷模拟器结构中，如果中框达到90°时，将出现自由度丢失的问题。而为了提高感知模拟的逼真度，转轴的运动范围设计一般是大于90°的，很多时候甚至能达到360°的运动范围，所以回避奇异点并不是最好的解决思路，为了能更有效的利用行程，本专利采用奇异过渡的解决办法。

持续载荷模拟器各框轴的运动学关系可以用雅克比矩阵来表示，但是雅克比矩阵在持续载荷模拟器上的应用和一般的机器人的场景的不同之处体现在几个方面：

1、结构上，持续载荷模拟器的转动框是内外嵌套的结构，不同于机械手的直连结构，所以持续载荷模拟器需要重点研究座舱的角运动关系；

2、控制目的不同，机器人是控制到具体的空间点实现某个任务，持续载荷模拟器是通过控制转动实现感知的近似；

3、持续载荷模拟器主臂位置不确定性。持续载荷模拟器主臂是产生过载量的，在姿态控制中，主臂转速不能作为被控制量，可以作为一个变化的扰动。即在持续载荷模拟器运动正解算的时候，主臂转动参数是作为系统输入，在逆解算的时候，主臂转动参数仍然是作为系统的输入参与运算。这个问题也不能通过将基坐标换在主臂末端来解决，因为主臂的转动也会不停的对座舱姿态产生影响。

因此,提出一种持续载荷模拟器多轴耦合运动奇异性控制方法；

具体步骤为：

1、建立模拟器的轴运动数学关系。

依据持续载荷模拟器的运动轴连接情况，构建运动学雅克比矩阵，用数学的方法直观表示轴转速和座舱转速之间的运动学关系。如：

其中v是末端的速度，ω是末端的角速度，jli第i个关节与平移相关的雅克比转换矩阵，jai第i个关节与角运动相关的雅克比转换矩阵，是第i个运动关节的速度。

2、主臂与其他运动轴解耦

因为主臂是用来产生过载的，所以在姿态解算中，是不可控制的量，但是又会对姿态控制造成影响。即在正解和逆解的过程中，主臂始终充当输入量。在姿态控制的雅克比中需要将主臂与其他转轴进行解耦合，处理方法为如下：：

其中v是末端的速度，ω是末端的角速度，jl1主臂与平移相关的雅克比转换矩阵，ja1主臂与角运动相关的雅克比转换矩阵，jli第i个关节与平移相关的雅克比转换矩阵，jai第i个关节与角运动相关的雅克比转换矩阵，是各个运动关节的速度；

所以与主轴解耦之后的其他轴运动控制的雅克比矩阵可以表示为：

其中jli第i个关节与平移相关的雅克比转换矩阵，jai第i个关节与角运动相关的雅克比转换矩阵；

在姿态感知模拟中，我们关注的就是角运动关系，所以重点选取雅克比矩阵的后三行进行分析和研究。例如，在滚转-偏航-俯仰结构下的角速度雅克比矩阵为(第一列为雅克比矩阵解耦后的结果，可以忽略)：

其中s表示sin，c表示cos，q1、q2、q3、q4分别表示主臂、外框、中框和内框的转角。当前结构的很多运动学问题都可以通过对雅克比矩阵的分析来实现。

3、量化奇异程度

在奇异控制处理上，如果仅仅采用当求解的转速超过轴运动能力就限幅控制，那么会使得模拟运动出现突变，极大影响运动模拟效果，所以需要对当前结果进行奇异程度的量化，这里选用矩阵的条件数作为奇异程度量化标准。条件数的定义：

cond(a)v＝||a^-1||v||a||v，其中(v＝1,2或∞)

其中常用的谱条件数为：

其中σmax是矩阵a的极大奇异值，σmin是矩阵a的极小奇异值；

条件数越大，代表当前各个轴的转动位置越接近奇异点。也就需要对运动解算格外注意。

4、奇异程度量化的快速求解

求矩阵条件数需要对矩阵进行奇异值分解，有着较大的运算量，在飞行模拟器中，需要尽可能实时模拟运动感知，所以奇异控制采用近似的求解方法，通过对条件数的分析可以发现，当前结构下，条件数只与中框的转角有关，在转角为±90°时，条件数趋于无穷大。所以，可以采用条件数的倒数和中间框的角度的齐次关系拟合来快速求解条件数。

其中ki表示对应阶次的系数，β为将当前中框转角θ映射到范围内的坐标值。在条件数解算实时性和条件数精度中综合考虑，可以选择一次拟合关系：

其中

此方法可以快速求解出当前轴运动位置的近似条件数，也可以认为是持续载荷模拟器姿态控制的奇异程度量化参数，在要求高实时性的持续载荷模拟器轴运动解算的环境下，保证基本奇异量化需求的前提下降低运算量和运算时间。

5、奇异控制模块设计

快速估算出当前位置的奇异程度量化结果后，就需要对当前解算的速度输出进行奇异控制。奇异控制模块在整个运动控制结构中的位置如图1所示，

其中逆运动解算模块主要完成模拟器的逆运动学解算；

奇异性控制模块主要完成对解算后的控制进行奇异性控制；

奇异量化模块主要完成对当前结构状态的奇异性进行量化；

速度积分器模块主要通过对框的转速进行积分得到框的角位移；

正运动解算模块主要通过对当前框的状态进行解算，对末端运动进行验证；

输出比较模块主要是对预期运动参数和模拟器输出运动参数进行比较。

可以看出奇异控制模块的主要功能是以奇异量化结果为依据对逆运动解算结果进行比例限幅输出。建立比例系数和条件数的关系如下：

其中k为限幅比例系数，nc为条件数，a、b为参数。

为了进一步提高系统的安全性，还需要以轴运动的限速和限幅作为更高一层的安全限制，奇异控制模块的结构如图2所示，

条件数计算模块是依据角运动参数实时对结构的奇异性进行量化；

奇异控制模块只要是依据条件数、逆解算输出以及机械限制等输入进行奇异解算输出。

轴运动的控制指令主要有lm和ls两个限制条件。

lm是轴运动的机械特性限制，各个轴的设计运动参数；ls为基于条件数的奇异性解算限制。

轴运动输出控制指令可以表示为：

c内外＝min{lm,ls·ci}；

c内外是内外框的奇异限制系数，ci是第i个关节的奇异解算限制系数；

中框因不会出现突变的解，所以不需要进行奇异限制，只需要进行机械特性限制。如果需要快速过渡奇异点，也可以适当扩大中框的运动特性，帮助系统快速脱离奇异位形。该方法能达到缩短系统在奇异区域的时间，但是会降低感知模拟的逼真度，可以根据需要进行设定。

c中＝min{lm,c}；

经过基于条件数的比例控制方法对运动解算指令进行比例缩放，能在实现奇异点的平稳过渡，也在奇异性逐渐增加的情况下，避免了大部分指令输出的突然限幅，充分利用了轴运动范围。

以上是本发明的较佳实施例，凡依本发明技术方案所作的改变，所产生的功能作用未超出本发明技术方案的范围时，均属于本发明的保护范围。