CN111694368A - 六自由度平台控制方法 - Google Patents

Abstract

六自由度平台控制方法，本发明涉及人机交互与平台控制，软件控制。本发明的目的是为了解决现有六个自由度平台控制复杂，操作不方便的问题的问题。过程为：一、根据双控制器的六自由度平台实际尺寸构建惯性坐标系；二、获取模型相对惯性坐标系的姿态信息；三、解算出每个支腿的长度值；四、判断各个支腿的长度值是否超出安全范围，当长度值超出安全范围，运动平台保持当前的姿态，不进行修改；否则就把解算出的各个支腿的长度值减去支腿长度初始值，得到六个支腿的伸长量数据；五、构建数据整理的脚本，把六个脉冲数据进行整合；六、构建UDP协议传输脚本，实现六自由度平台的控制任务。本发明用于人机交互与平台控制领域。

Description

六自由度平台控制方法

技术领域：

本发明涉及人机交互与平台控制，软件控制。

背景技术：

现有技术针对六自由度平台的运动模拟算法与结构进行了研究，设计运动控制的算法或是六自由度平台的结构。物体在空间具有六个自由度，即沿x、y、z三个直角坐标轴方向的移动自由度和绕这三个坐标轴的转动自由度。因此，要完全确定物体的位置，就必须清楚这六个自由度。

针对现在主流的游戏开发平台Unity进行运动控制的设计时，没有简易直接的平台控制方案，不能直观的实时的对平台进行操控，导致六个自由度平台控制复杂，操作不方便的问题。

发明内容：

本发明的目的是为了解决现有六个自由度平台控制复杂，操作不方便的问题，而提出六自由度平台控制方法。

六自由度平台控制方法具体过程为：

步骤一、根据双控制器的六自由度平台实际尺寸构建惯性坐标系，坐标系分为上下两个平面，上下平面各有六个支腿节点，两平面之间的距离为高度差；

所述双控制器的六自由度平台包括底座平台和运动平台；

步骤二、在游戏引擎里设置模型；利用脚本实时地获取模型相对惯性坐标系的姿态信息；

步骤三、构建反解运算的脚本，利用模型相对惯性坐标系的姿态信息，进行几何运算，解算出每个支腿的长度值；

步骤四、判断各个支腿的长度值是否超出安全范围，当长度值超出安全范围，运动平台保持当前的姿态，不进行修改；

否则就把解算出的各个支腿的长度值减去支腿长度初始值，得到六个支腿的伸长量数据；

所述支腿长度初始值是支腿的原长度，实际测量的；

步骤五、根据电动缸的类型，把支腿的伸长量数据转为对应电动缸的脉冲数据，再把数据取整转化为16进制字符串，存入设定好的字符串数组中；

构建数据整理的脚本，把六个脉冲数据进行整合；

步骤六、构建UDP协议传输脚本，设置电脑为服务端，设置两个控制器为客户端，利用UDP协议传输数据到两个控制器的IP地址中，实现六自由度平台的控制任务。

本发明的有益效果为：

本发明根据双控制器的六自由度平台实际尺寸构建惯性坐标系，所述双控制器的六自由度平台包括底座平台和运动平台；在游戏引擎里设置模型；利用脚本实时地获取模型相对惯性坐标系的姿态信息；构建反解运算的脚本，利用模型相对惯性坐标系的的姿态信息，进行几何运算，解算出每个支腿的长度值；判断各个支腿的长度值是否超出安全范围，当长度值超出安全范围，运动平台保持当前的姿态，不进行修改；否则就把解算出的各个支腿的长度值减去支腿长度初始值，得到六个支腿的伸长量数据；根据电动缸的类型，把支腿的伸长量数据转为对应电动缸的脉冲数据，再把数据取整转化为16进制字符串，存入设定好的字符串数组中；构建数据整理的脚本，把六个脉冲数据进行整合；构建UDP协议传输脚本，设置电脑为服务端，设置两个控制器为客户端，利用UDP协议传输数据到两个控制器的IP地址中；操作简单，效率高；利用本方法构建控制***，只需要在游戏中利用键盘控制车辆的姿态，就可以使得平台的姿态得到相应的变化，利用游戏内车辆姿态对平台进行运动控制，解决现有六个自由度平台控制复杂，操作不方便的问题的问题。

附图说明

图1为本发明流程图；

图2为Stewart平台示意图；

图3为对Stewart平台的俯视坐标系示意图；

图4为本发明支腿与节点矢量关系图。

具体实施方式

具体实施方式一：本实施方式六自由度平台控制方法具体过程为：

利用Unity游戏开发环境，对虚拟场景中的汽车姿态进行反解运算，进而得到每个电动缸的伸长量，转为控制信号，传给控制器。

建立汽车控制脚本，控制汽车运动。

利用六自由度平台构建坐标系，并建立反解运算的C#脚本。该脚本实时获取汽车姿态信息，得到6个电动缸的长度数据，存储在数组中。

建立数据转化和传输脚本。把反解脚本解算出的长度数据转化为16进制的数据，并且以16进制字符串的形式表示出来，最后通过UDP协议发送到控制器中。

步骤一、根据双控制器的六自由度平台(就是两个平台控制器，每个控制器控制三个支腿)实际尺寸构建惯性坐标系，如图2，坐标系分为上下两个平面(六自由度平台是上下两个平台)，上下平面各有六个支腿节点(六个支腿节点位于平台连接处的位置)，两平面之间的距离为高度差；

所述双控制器的六自由度平台包括底座平台和运动平台；

步骤二、在游戏引擎里设置模型，例如车辆模型；利用脚本实时地获取车辆模型相对惯性坐标系的姿态信息(高度，六个轴向的旋转角度)(利用Unity游戏引擎在C#脚本代码中写具有这个功能的函数)；

步骤三、构建反解运算的脚本，利用车辆模型相对惯性坐标系的姿态信息，进行几何运算，解算出每个支腿的长度值；

步骤四、判断各个支腿的长度值是否超出安全范围，由于长度值近似可以看成是连续增加或减少的，所以当长度值超出安全范围，运动平台保持当前的姿态，不进行修改；

否则就把解算出的各个支腿的长度值减去支腿长度初始值，得到六个支腿的伸长量数据；

所述支腿长度初始值是支腿的原长度，实际测量的；

电动缸是支腿里的装置，可以推动支腿伸长；

步骤五、根据电动缸的类型，把支腿的伸长量数据转为对应电动缸的脉冲数据，再把数据取整转化为16进制字符串，存入设定好的字符串数组中；

构建数据整理的脚本(写一个脚本，功能是整合数据)，把六个脉冲数据进行整合(这个脚本的功能就是整合数据，下面是具体如何整合的做法。)；

步骤六、构建UDP协议传输脚本，设置电脑为服务端，设置两个控制器为客户端，利用UDP协议传输数据到两个控制器的IP地址中，实现六自由度平台的控制任务。

具体实施方式二：本实施方式与具体实施方式一不同的是：所述步骤二中模型相对惯性坐标系的姿态信息为模型相对惯性坐标系的高度、模型相对惯性坐标系的六个轴向的旋转角度；

所述六个轴向为模型沿惯性坐标系x、y、z三个直角坐标轴方向的移动自由度和绕惯性坐标系x、y、z三个直角坐标轴的转动自由度。

其它步骤及参数与具体实施方式一相同。

具体实施方式三：本实施方式与具体实施方式一或二不同的是：所述步骤三中构建反解运算的脚本，利用车辆模型相对惯性坐标系的姿态信息，进行几何运算，解算出每个支腿的长度值；具体过程为：

游戏或软件内有车辆模型，他有个姿态，上平台为了模拟出这个姿态，就需要变化支腿的长度，电动缸就是驱动支腿长短的装置，在支腿内部。

根据实际情况，对实验环境的Stewart平台进行理论研究和测量，如图2，得到如图3的俯视坐标系。

内环为底座平台，底座平台的A₁-A₆为底座平台六个支腿节点的位置坐标，外环为运动平台，运动平台的B₁-B₆为运动平台六个支腿节点的位置坐标；

原点分别为底座平台和运动平台的质心，底座平台和运动平台两者具有一定的高度差，初始位置时运动平台与惯性坐标系平行；

底座平台的A₁、A₂、A₃、A₄、A₅、A₆支腿节点的位置坐标表示为：

式中，r_a为底座平台半径，β_a1i为底座平台第i个支腿节点与x轴的夹角，β_a2i为底座平台第i个支腿节点与x轴的夹角，A_1,3,5为底座平台的A₁、A₃、A₅支腿节点的坐标，A_2,4,6为底座平台的A₂、A₄、A₆支腿节点的坐标；T为转置；

运动平台的B₁、B₂、B₃、B₄、B₅、B₆支腿节点的位置坐标表示为：

式中，r_b为运动平台半径，β_b1i为运动平台第i个支腿节点与x轴的夹角，β_b2i为运动平台第i个支腿节点与x轴的夹角，B_1,3,5为运动平台的B₁、B₃、B₅支腿节点的坐标，B_2,4,6为运动平台的B₂、B₄、B₆支腿节点的坐标；T为转置；

根据运动平台的最终状态即位姿，求解出各个支腿中电动缸长度的过程就是运动学反解；反之，利用电动缸的伸长量及支腿长度，求解出平台的位姿信息为正解。平台的运动学反解是进行速度、加速度分析和角速度分析等问题的基础。

在反解的过程中，最方便的是利用欧拉角进行运动平台在惯性坐标系的姿态表示。

运动平台按照右手坐标系中zyx的顺序进行旋转变换：最初，运动平台平行于惯性坐标系，首先运动平台M绕自身的z轴旋转角度ψ，再绕自身的y轴旋转角度θ，最后再绕自身的x轴旋转角度

如下：

得:

整理得：

M^{3}＝R_xR_yR_zM＝RM

式中：

R_z为M绕z轴旋转矩阵；

R_y为M绕y轴旋转矩阵；

R_x为M绕x轴旋转矩阵；

最终，得到旋转变换矩阵如下

由R的形式知，R为正交矩阵，满足如下：

R^T＝R^-1

由此，根据Stewart底座平台和运动平台节点间的几何关系，如图4，利用矢量表达式，得到位置反解的表示形式如下：

l_i＝S+RB_i-A_i,i＝1,2,…,6

式中，向量l_i为各个支腿的长度，S为运动平台质心。

其它步骤及参数与具体实施方式一或二相同。

具体实施方式四：本实施方式与具体实施方式一至三之一不同的是，所述

式中，β_a为底座平台实际测量的角度值(可以是A₄和A₅之间的角度)；

式中，β_b为运动平台实际测量的角度值(可以是B₃和B₄之间的角度)。

其它步骤及参数与具体实施方式一至三之一相同。

具体实施方式五：本实施方式与具体实施方式一至四之一不同的是：所述A₁、A₃、A₅支腿节点分别相隔120度，A₂、A₄、A₆支腿节点也分别相隔120度，A₁支腿节点和A₆支腿节点夹角就是β_a，A₁、A₆关于x轴对称；

A₁、A₂、A₃、A₄、A₅、A₆支腿节点的位置坐标在底座平台上依次逆时针排列；

且A₁、A₆相邻，A₂、A₃相邻，A₄、A₅相邻；

B₁、B₃、B₅相隔120度，B₂、B₄、B₆也是相隔120度，然后B₃和B₄夹角就是β_b，B₃、B₄关于x轴对称；

B₁、B₂、B₃、B₄、B₅、B₆支腿节点的位置坐标在运动平台上依次逆时针排列；

且B₁、B₂相邻，B₃、B₄相邻，B₅、B₆相邻；

x轴的位置在B₃、B₄中间的角分线上，也在A₁、A₆的角分线上。

其它步骤及参数与具体实施方式一至四之一相同。

具体实施方式六：本实施方式与具体实施方式一至五之一不同的是：所述步骤五中根据电动缸的类型，把支腿的伸长量数据转为对应电动缸的脉冲数据，再把数据取整转化为16进制字符串，存入设定好的字符串数组中；

构建数据整理的脚本(写一个脚本，功能是整合数据)，把六个脉冲数据进行整合(这个脚本的功能就是整合数据，下面是具体如何整合的做法。)；

具体过程为：

由于我们使用的是双控制器的六自由度平台，所以需要将六个支腿的伸长量数据分为两个部分，第一个控制器的三个支腿的伸长量数据写在一条数据里，第二个控制器控制的三个支腿的伸长量数据写在另一条数据里，同时分别地传输给两个平台控制器，每个控制器控制三个电动缸的运动；

六个支腿由于是每个控制器控制三个，所以需要把第一个控制器的三个伸长量数据写在一条数据里，传给这个控制器。再把第二个控制器控制的三个支腿的伸长量数据写在另一条数据里，传给第二个控制器。

根据控制器的要求，每条传输的数据分三个部分，前后两部分都是固定的字节数组，中间部分按顺序为三个支腿的伸长量数据；

设立两个字节数组，填入字节数组的前后两部分；

再把16进制字符串转为字节类型的信息，按顺序填入字节数组的中间部分，构成了两条完整的字节数组，准备进行传输环节；

两条数据例如(数据本身是连在一起的没有空格换行)

一：

55aa 00 00 13 01 00 00 ff ff ff ff 00 00 00 01 00 00 00 00

00 00 c3 50 00 00 c3 50 00 00 c3 50

12 34 56 78 ab cd

二：

55 aa 00 00 13 01 00 00 ff ff ff ff 00 00 00 01 00 00 00 00

00 00 c3 50 00 00 c3 50 00 00 c3 50

12 34 56 78 ab cd

前后两部分就是第一行，第三行，对于所有数据都是一样的。中间的0000c350就是一个支腿的伸长量信息，

第一条数据表示三个支腿伸长分别是0000c350、0000c350、0000c350

第二条数据也是这个意思，这两条数据发给各自的控制器，控制各自三个支腿的长度。

其它步骤及参数与具体实施方式一至五之一相同。

具体实施方式七：本实施方式与具体实施方式一至六之一不同的是：所述步骤六中构建UDP协议传输脚本，设置电脑为服务端，设置两个控制器为客户端，利用UDP协议传输数据到两个控制器的IP地址中，实现六自由度平台的控制任务；具体过程为：

利用UDP协议将步骤五得到的两条完整的字节数组分别传输给两个控制器，让两个控制器同时对他们各自控制的三个支腿进行驱动，实现六自由度平台的控制任务。

其它步骤及参数与具体实施方式一至六之一相同。

采用以下实施例验证本发明的有益效果：

实施例一：

1)在娱乐场所内可以使用本产品进行娱乐体验，或者模拟训练的环境中进行虚拟仿真，用户可以感觉到真实的运动模拟情况。

2)利用六自由度平台的开发人员可以利用本方法进行简易直接的运动控制。

本发明还可有其它多种实施例，在不背离本发明精神及其实质的情况下，本领域技术人员当可根据本发明作出各种相应的改变和变形，但这些相应的改变和变形都应属于本发明所附的权利要求的保护范围。

Claims (7)

1.六自由度平台控制方法，其特征在于：所述方法具体过程为：

步骤一、根据双控制器的六自由度平台实际尺寸构建惯性坐标系，坐标系分为上下两个平面，上下平面各有六个支腿节点，两平面之间的距离为高度差；

所述双控制器的六自由度平台包括底座平台和运动平台；

步骤二、在游戏引擎里设置模型；利用脚本实时地获取模型相对惯性坐标系的姿态信息；

步骤三、构建反解运算的脚本，利用模型相对惯性坐标系的姿态信息，进行几何运算，解算出每个支腿的长度值；

步骤四、判断各个支腿的长度值是否超出安全范围，当长度值超出安全范围，运动平台保持当前的姿态，不进行修改；

否则就把解算出的各个支腿的长度值减去支腿长度初始值，得到六个支腿的伸长量数据；

所述支腿长度初始值是支腿的原长度，实际测量的；

步骤五、根据电动缸的类型，把支腿的伸长量数据转为对应电动缸的脉冲数据，再把数据取整转化为16进制字符串，存入设定好的字符串数组中；

构建数据整理的脚本，把六个脉冲数据进行整合；

步骤六、构建UDP协议传输脚本，设置电脑为服务端，设置两个控制器为客户端，利用UDP协议传输数据到两个控制器的IP地址中，实现六自由度平台的控制任务。

2.根据权利要求1所述六自由度平台控制方法，其特征在于：所述步骤二中模型相对惯性坐标系的姿态信息为模型相对惯性坐标系的高度、模型相对惯性坐标系的六个轴向的旋转角度；

所述六个轴向为模型沿惯性坐标系x、y、z三个直角坐标轴方向的移动自由度和绕惯性坐标系x、y、z三个直角坐标轴的转动自由度。

3.根据权利要求1或2所述六自由度平台控制方法，其特征在于：所述步骤三中构建反解运算的脚本，利用模型相对惯性坐标系的姿态信息，进行几何运算，解算出每个支腿的长度值；具体过程为：

内环为底座平台，底座平台的A₁-A₆为底座平台六个支腿节点的位置坐标，外环为运动平台，运动平台的B₁-B₆为运动平台六个支腿节点的位置坐标；

原点分别为底座平台和运动平台的质心，底座平台和运动平台两者具有高度差，初始位置时运动平台与惯性坐标系平行；

底座平台的A₁、A₂、A₃、A₄、A₅、A₆支腿节点的位置坐标表示为：

式中，r_a为底座平台半径，β_a1i为底座平台第i个支腿节点与x轴的夹角，β_a2i为底座平台第i个支腿节点与x轴的夹角，A_1,3,5为底座平台的A₁、A₃、A₅支腿节点的坐标，A_2,4,6为底座平台的A₂、A₄、A₆支腿节点的坐标；T为转置；

运动平台的B₁、B₂、B₃、B₄、B₅、B₆支腿节点的位置坐标表示为：

式中，r_b为运动平台半径，β_b1i为运动平台第i个支腿节点与x轴的夹角，β_b2i为运动平台第i个支腿节点与x轴的夹角，B_1,3,5为运动平台的B₁、B₃、B₅支腿节点的坐标，B_2,4,6为运动平台的B₂、B₄、B₆支腿节点的坐标；T为转置；

运动平台按照右手坐标系中zyx的顺序进行旋转变换：最初，运动平台平行于惯性坐标系，首先运动平台M绕自身的z轴旋转角度ψ，再绕自身的y轴旋转角度θ，最后再绕自身的x轴旋转角度

如下：

得:

整理得：

M^{3}＝R_xR_yR_zM＝RM

式中：

R_z为M绕z轴旋转矩阵；

R_y为M绕y轴旋转矩阵；

R_x为M绕x轴旋转矩阵；

最终，得到旋转变换矩阵如下

由R的形式知，R为正交矩阵，满足如下：

R^T＝R^-1

由此，根据底座平台和运动平台节点间的几何关系，利用矢量表达式，得到位置反解的表示形式如下：

l_i＝S+RB_i-A_i,i＝1,2,…,6

式中，向量l_i为各个支腿的长度，S为运动平台质心。

4.根据权利要求3所述六自由度平台控制方法，其特征在于：所述

式中，β_a为底座平台实际测量的角度值；

式中，β_b为运动平台实际测量的角度值。

5.根据权利要求4所述六自由度平台控制方法，其特征在于：所述A₁、A₃、A₅支腿节点分别相隔120度，A₂、A₄、A₆支腿节点也分别相隔120度，A₁支腿节点和A₆支腿节点夹角就是β_a，A₁、A₆关于x轴对称；

B₁、B₃、B₅相隔120度，B₂、B₄、B₆也是相隔120度，然后B₃和B₄夹角就是β_b，B₃、B₄关于x轴对称；

x轴的位置在B₃、B₄中间的角分线上，也在A₁、A₆的角分线上。

6.根据权利要求5所述六自由度平台控制方法，其特征在于：所述步骤五中根据电动缸的类型，把支腿的伸长量数据转为对应电动缸的脉冲数据，再把数据取整转化为16进制字符串，存入设定好的字符串数组中；

构建数据整理的脚本，把六个脉冲数据进行整合；具体过程为：

将六个支腿的伸长量数据分为两个部分，第一个控制器的三个支腿的伸长量数据写在一条数据里，第二个控制器控制的三个支腿的伸长量数据写在另一条数据里，同时分别地传输给两个平台控制器，每个控制器控制三个电动缸的运动；

每条传输的数据分三个部分，前后两部分都是固定的字节数组，中间部分按顺序为三个支腿的伸长量数据；

设立两个字节数组，填入字节数组的前后两部分；

再把16进制字符串转为字节类型的信息，按顺序填入字节数组的中间部分，构成了两条完整的字节数组，准备进行传输环节。

7.根据权利要求6所述六自由度平台控制方法，其特征在于：所述步骤六中构建UDP协议传输脚本，设置电脑为服务端，设置两个控制器为客户端，利用UDP协议传输数据到两个控制器的IP地址中，实现六自由度平台的控制任务；具体过程为：

利用UDP协议将步骤五得到的两条完整的字节数组分别传输给两个控制器，让两个控制器同时对他们各自控制的三个支腿进行驱动，实现六自由度平台的控制任务。

Images