六自由度并联机构运动速度算法优化及速率提升方法与流程

本发明涉及飞行模拟领域,尤其是一种六自由度并联机构运动速度算法优化及速率提升方法。

背景技术

目前市场上现有六自由度运动平台(以下简称运动平台)接收数据频率基本采用20hz，速度均采用六轴矢量速度，在控制器接收到数据后进行分解，通过速度插补算法将矢量速度分解为每一只电缸的运行速度，每只电缸通过插补速度进行定位运动，在接收到新速度数据时再重复解算分解，来达到速度插补功能。

因数据刷新率较低，在飞行器频繁姿态及速度实时快速变化时无法完全模拟飞行器姿态及速度模拟响应，特别是在飞机出现高频振动的时候，因数据刷新频率低导致无法模拟振动的现象尤为明显。因此，对于上述问题有必要提出一种六自由度并联机构运动速度算法优化及速率提升方法。

零延迟：根据人眼“余晖效应”，视觉暂留约为0.04s，座椅控制器与上位机交换数据时间为0.005s/帧,座椅控制器运算周期为0.01s，通过运算将运动输出到座椅上，使得画面与运动单周期运行时间小于0.04s人眼视觉暂留时间，使得体验者无法区分两次运动之间的时间暂留。

绝对位置：基于零点为运动地址基准点，编码器采用绝对位置编码器，断电时由电池供电保持位置数据。

插补速度：通过平台运动总矢量速度，计算六只电缸单独运行速度，六只电缸同时到达目标位置。

技术实现要素：

针对上述现有技术中存在的不足，本发明的目的在于提供一种六自由度并联机构运动速度算法优化及速率提升方法，解决现有运动平台因数据刷新频率较低，模拟运行速度不够及时响应，体感响应速度低的问题。

一种六自由度并联机构运动速度算法优化及速率提升方法，运动控制器作为运动姿态算法解析及逻辑控制，通过使用总线通信的方式与六台伺服驱动器连接，伺服驱动器驱动电机带动电缸执行机构做出姿态运动模拟，包括x、y、z、α、β、γ六个方向的运动；其中，x指沿x轴直线运动，y指沿y轴直线运动，z指沿z轴直线运动，α指绕x轴旋转，β指绕y轴旋转，γ指绕z轴旋转；执行上位机命令的电机控制系统即伺服系统使用绝对位置方式控制，在通电后运动控制器与伺服系统通信读取当前姿态位置数据，并写入运动控制器的数据缓存寄存器，实现在断电重启后姿态位置读取，在绝对位置数据保持完整状态下，无需执行原点回归动作，所述绝对位置指基于零点为运动地址基准点；运动平台通过以太网与上位机连接交互数据，接收到数据后由运动控制器解析后存入数据缓存寄存器，再由运动控制器通过总线发送至伺服驱动器，同时运动控制器与伺服驱动器实时通信读取状态；通过上位机发送使能信号开启/关闭平台运动使能，或手动开启/关闭平台运动使能；在运动过程中按下停止运动按钮将停止姿态模拟，运动平台采用低速返回至零点位置，返回过程/返回至零点时按下运行按钮便可再次启动。

作为优选，运动平台还连接控制器与飞行模拟器两个紧急停止按钮，在控制程序中为最高等级，按下后运动平台立即减速停止在当前运动位置。

作为优选，还可以将运动平台工作模式切换为手动模式，通过手动操控进行x、y、z、α、β、γ六个方向的运动，运行速度可调整，在后期维护提供方便的操作。

作为优选，所述运动平台通过以太网与上位机连接交互数据，数据交互频率可在1到100hz内调节。

作为优选，所述运动控制器的逻辑控制包括：

a.运动控制器通过以太网连接接收上位机发送的模拟软件的姿

态角度、角速度，姿态角度姿态响应最小分辨率为0.1°，通

过平台电缸当前位置与新姿态解析后电缸运行位置差及平台运

行矢量速度分解出每只电缸的运行插补速度，并存入数据缓存

寄存器；

b.运行周期固定为5ms，实时刷新解析运动数据；

c.运动控制器根据上位机发送的模拟软件的姿态角度，结合运

动平台自身结构，解析当前角度对应的电机执行机构的位移地

址，再结合解析完的插补速度数据将电机运行数据通过总线发

送至伺服系统；

d.运动控制器根据上位机发送的姿态数据，结合当前绝对位置

偏移量实时运算电机运行速度，将运算完成的电机运行速度存

入数据缓存寄存器；

e.运动过程中可快速响应新的姿态位置及运动速度，当接收到

新的运动地址及速度，立即执行新的运动数据；

f.支持绝对地址限位，绝对地址是指基于零点位置开始计算的位置偏移量，运动平台只能够在限制绝对地址内运动，超出绝对位置限位，取最大/最小绝对位置运行。

作为优选，所述运动控制器运动姿态算法解析包括以下步骤：

s1.开始

s2.通过接收上位机发送的姿态数据，解析每只电缸的伸缩量，通过电缸的原始长度计算当前电缸的运动绝地地址；通过速度算法即平台姿态运动最大行程与电缸的最大运行速度计算得出平台最大运行矢量时间；

s3.通过位置反解算法解析s2接收到的上位机发送的姿态数据；

s4.通过位置反解算法计算目标地址与当前地址差值；

s5.取差值的绝对值；

s6.通过速度算法使用平台最高运行时间计算得出每只电缸的运动插补速度；

s7.当插补速度值大于平台极限速度时，根据大于最大速度限制再次计算插补速度；当插补速度值小于或等于平台极限速度时，进入s8；

s8.将运行速度存入数据缓存寄存器；

s9.通过运动控制器总线发送至伺服系统；

s10.结束。

作为优选，算法数据通信采用标准开放mc协议，使用以太网rj45接口连接。

作为优选，位置反解算法如下：

s1.动平台铰点，a：动平台i：坐标(x、y、z)j：上铰点(a1～a6)

s2.静平台铰点，b：动平台i：坐标(x、y、z)j：上铰点(b1～b6)

s3.变换矩阵，动平台姿态：x轴平移、y轴平移、z轴平移、α翻滚、β俯仰、γ偏航、r齐次变换矩阵

s4.动平台铰点在静平台中坐标矩阵

p：动平台齐次变换坐标值

p＝[pij]4×6＝ra

s5.位置反解

△li：伸缩量li：伸缩杆长l0：伸缩杆初始长度

i＝(1，2，3，4，5，6)

作为优选，速度算法如下：

s1.电缸目标位置差值

li：电缸位置差值

svi：目标位置

pvi：当前位置

abs:绝对值函数

i＝(1，2，3，4，5，6)

li＝abs(svi-pvi)

s2.电缸位置最大差值查找

max:最大值函数

l：电缸差值最大值

l＝max(li)

s3.运行时间

t:运动最大时间

s:电缸最大运行速度

s4.运行速度

i＝(1，2，3，4，5，6)

si:单缸运行速度

由于采用上述技术方案，本发明有益效果：

1.姿态数据包刷新速度快，可在1～100hz范围内任意调节，由于数据刷行频率高，可完全避免因数据刷新率不够而无法实时执行模拟器姿态及速度的不足。

2.因采用平台运行最大时间作为矢量速度，可模拟轻微振动。

3.速度算法是采用两帧数据的姿态位置差，平台运动最大矢量速度计算得出每只电缸的运行速度，可在上位机只发送姿态数据的情况下，便能实时的跟随模拟器的姿态运动，无需上位机发送运行速度，可减小数据包，减少带宽的占用，故而提高数据刷新频率。

4.以太网rj45接口，标准通信协议可连接各种飞行模拟器。

综上所述，本发明使用高速运动控制器作为运动姿态算法解析及逻辑控制，提高数据刷新率到100hz，通过控制伺服电机带动电缸执行机构，可以快速的进行速度、抖动、体感运动响应，能够完全模拟模拟器在低速运行时的振动颠簸。通过外部视景画面加上运动平台模拟，能够实现近零延迟的体感操作，使得模拟更加真实。

附图说明

图1是本发明的算法程序框图；

图2是本发明的运动平台结构俯视示意图；

图3是本发明的运动平台结构3d示意图。

具体实施方式

以下结合附图对本发明的实施例进行详细说明，但是本发明可以由权利要求限定和覆盖的多种不同方式实施。

如图1所示，实施例：

a)运动控制系统

i.运动控制器控制使用总线通信(光纤)的方式与六台伺服驱动器连接，通信速度达到150mbps/s伺服驱动器驱动电机带动电缸机构做出姿态运动模拟。

ii.伺服系统使用绝对位置方式控制(使用锂电池进行断电位置数据保持)，在上电后运动控制器与伺服系统通信读取当前姿态位置数据，并写入控制器控制寄存器，实现在断电重启后姿态位置读取，在绝对位置数据保持完整状态下，无需执行原点回归动作。

iii.运动平台通过以太网与上位机连接交互数据，数据交互频率可在1～100hz内调节，接收到数据后由运动控制器解析后存入数据缓冲区，再由运动控制器通过总线发送至伺服驱动器，同时运动控制器与伺服驱动器实时通信读取状态。

iv.通过上位机发送使能信号开启/关闭运动，或手动开启/关闭。

v.在运动过程中按下停止运动按钮将停止姿态模拟，运动平台采用低速返回至零点位置，返回过程/返回至零点时按下运行按钮便可再次启动。

vi.运动平台连接控制器与飞行模拟器两个紧急停止按钮，在控制程序中为最高等级，按下后运动平台立即减速停止在当前运动位置。

vii.将运动平台工作模式切换为手动，可进行x、y、z、α、β、γ六个方向的运动，运行速度可调整，为后期维护提供方便。

b)运动控制过程逻辑

i.通过以太网连接接收上位机发送的姿态角度、角速度，姿态角度姿态响应最小分辨率为0.1°，通过平台电缸当前位置与新姿态解析后电缸运行位置差，再通过平台运行矢量速度分解出每只电缸的运行插补速度并存入数据缓冲寄存器。

ii.算法运行周期固定为5ms，实时刷新解析运动数据。

iii.根据上位机发送模拟器姿态角度，结合运动平台自身结构(图二、图三)，解析当前角度对应的电机执行机构的位移地址，在结合解析完的插补速度数据将电机运行数据通过总线发送至伺服系统。

iv.根据上位机发送的姿态数据，结合当前绝对位置偏移量，实时运算电机运行速度，将运算完成的电机运行速度存入数据缓存寄存器。

v.运动过程中可快速响应新的姿态位置及运动速度，当接收到新的运动地址及速度，立即执行新的运动数据。

vi.支持软件绝对地址限位，运动座椅只能够在软件限制绝对地址内运动，超出软件绝对位置限位，取最大/最小绝对位置运行。

c)算法实现

i.运动位置反解算法，通过接收上位机发送的姿态数据，使用位置反解算法，解析每只电缸的伸缩量，通过电缸的原始长度计算当前电缸的运动绝地地址。

ii.速度算法

1)通过平台姿态运动最大行程与电缸的最大运行速度计算得出平台最大运行矢量时间。

2)计算得出平台电缸当前位置与姿态解析后的的电缸目标位置的差值。

3)使用平台最高运行时间计算得出每只电缸的运动插补速度。

d)通信协议

算法数据通信采用标准开放mc协议，使用以太网rj45接口连接。

e)支持连接

该算法不依靠飞行器自身运行速度，而是通过飞行器两次位置偏移差值计算每轴运行速度，只要飞行器飞行姿态变化速度≤运动平台的最大运行速度，运动平台便可自适应运动速度，可连接各种飞行模拟器。可连接各种飞行模拟器。

实施例详细参数