基于MATLAB 算法的三自由度机电运动平台控制系统设计

摘 要

三自由度机电运动平台是Q10ZZD03-TY装甲救护车综合训练模拟器材的重要组成部分。其承载模拟座舱，可以模
拟不同路况、不同车速条件下的装甲救护车运动姿态，为受训对象提供近似真实的运动体感模拟。
　　三自由度机电运动平台包括运动平台台体和运动控制系统两部分。运动平台台体由上平台、下平台、电动缸、
导向支撑柱、防转摇臂组成，实现俯仰、侧倾以及上下运动。控制系统采用MATLAB软件语言编写相关控制算法，来
实现平台的各种运动控制。
关键词： 三自由度 MATLAB 控制算法

第1章绪论

1.1 课题来源及意义
1.1.1 课题来源
　　 本课题是来源于一个实际的具体项目，本课题需要设计出三自由度机电运动平台控制的正解和逆解的相关算
法，并且验证他们的正确性，为了让控制的机电运动平台更加的符合实际的操作情况，还需要完成相关的洗出算法
的编写，让机电运动平台的运动能更加真实的被使用者感受到，并且也能让机电运动平台的运动，在有限的空间里
被更好的展现，实现数据的滤波，平滑处理。
1.1.2 选题意义
　　机电运动平台是未来仿真平台的一种重要的形式，其发展也一定是重中之重，机电运动平台的使用范围很广，
可以完成很多方面的事情，而在其中以电机驱动的三自由度机电运动平台的控制的研究是这中的一种，该平台比起
以前的机电平台有更多的优势，而且该平台的研究也是是国内的一个较靠前的方向。所以对三自由度机电运动平台
的控制算法的研究是很有必要的，也是十分有意义的。
　　传统并联运动机构研究的理论和技术已经有了一定的发展,对于并联机构的问题的探讨一般都是从对象的机构，
物体的位置姿态，控制算法的算法误差以及相关的运动学理论知识等方面开始讨论,对于其中的研究一般都是从对象
物体的一些参数开始，参数包括位移的大小，加速度的值，物体的位置等多个方面,传统的控制算法一般都是PID算
法，经过一段时间的发展，现在大多已经用智能控制算法来完成设计。到目前为止,运动平台的应用已经是十分广泛
的了，运动平台的作用也是越来越重要，越来越多的领域，器件等的都开始运用运动平台来完成一些操作。运动平
台一般都采用的是并联机构,因为并联结构的好处相比于串联结构会更多。在现在运用的过程中，三自由度机电运动

平台使用的比较常见但是六自由度机电运动平台的使用也不少，因为六自由度平台相对于三自由度平台运用的范围
可以更广，所以是六自由度的机电运动平台的发展更是十分的受人关注。
　　我们所知的运动平台的控制多用并联的机构来控制，相对于串联的机构来说，并联机的构具有很多的优点，首
先与串联的机构相比，并联的机构的刚度更大，结构也相对来说更加的稳定并且并联的机构的承载能力更强操作过
程中的精度也更加的高，而且并联的机构在运动时候产生的运动惯性相对较小，最主要的是，串联的机构解出逆解
的过程会比较复杂，串联机构不适合求解相关的逆解，而并联的机构要解出逆解的话就会比较容易。一般来说逆解
的求解会更加的关键，在这一点上而并联的机构却容易实现，综合多种因素不难发现，并联机构的研究是十分有必
要，十分有意义的。
1.2 三自由度机电运动平台国内外研究概况
　　对于国内的三自由度机电运动平台的研究情况，我们知道的有青岛某学院研究过一个运用于潜艇训练的机电平
台，该平台可以模拟潜艇的运动从而达到训练的目的。在2010年，我国已经有研究所可以研究并使用无死角的六自
由度的机电运动平台。
　　运动平台有大型运动平台和小型运动平台之分，一般来说大型的运动平台都是采用液压的驱动的方式，但是该
平台的性能不是很好，容易损坏，受到一些外界的干扰，使用的效率也不是特别的高，而且危险性也相对较大。不
过随着科技的发展，液压驱动慢慢的就被另一种驱动方式所替代了，那就是电动缸驱动，这种驱动方式更加的安
全，而且也不容易收到干扰，精度相对于原来的驱动方式也有了更大的改善，更加的适用于未来的运动平台的使
用。在驱动方式上，平台的驱动越来越好，相对于的平台能够达到的自由度也更加的高，从一开始的二自由度，慢
慢的发展到了三自由度，到现在已经有了六自由度甚至七自由度的运动平台了。
　　在国外，早在很多年前，就有了训练用的平台的研究和运用，最先运用平台进行训练的行业是飞行的行业，国
外制造相关的平台来模拟仿真飞行，以此来训练飞行员。在一些平台上不仅仅可以模拟天空飞行的状态，还能够模
拟出不同的天气时候的飞行状态，让被训练的飞行员有更真实更全面的训练体验。国外也有研发由于汽车开车训练
的运动平台，用于培训模拟真实驾驶车辆的场景。
　　在国内外三自由度机电运动平台都有较长时间的发展，从液压驱动到电动缸，从大型的平台到小型的平台都有
涉猎，随着科技的发展，我认为运动平台将不仅仅局限于飞行员，驾驶员的模拟训练，运动平台将会更加好的发
展，然后运用于人们的生活之中。
1.3机电运动平台控制
1.3.1前言
　　本文所设计的控制算法是基于一个三自由度机电运动平台之上，该机电运动平台是一个实际项目中的Q10ZZD03-
TY装甲救护车综合训练模拟器材的重要组成部分，机电运动平台主要由上平台，下平台支撑柱等组成，需要能实现
俯仰，翻转以及上下平移即升降的作用。也需要能尽可能真实的模拟出平台运动的体感，让使用者能有更加真实的
操作体验。
1.3.2运动学分析
　　为了能够实现对机电平台的控制，需要在设计算法之前进行运动学分析，主要包括了位置姿态，速度等的正解
和逆解方面的问题。对于正解来说，我们已知的是机电平台的支撑柱的长度，我们需要利用D-H算法来表示各个点的
位置，然后利用勾股定理来联立方程。从而解出机电平台的位置姿态，解出正解。逆解则相对于容易，我们已知机
电平台的位置姿态，仅需要通过算子就可以获得支撑柱的位置，然后勾股定理就可以完成逆解。
1.4总体设计要求以及本文完成的主要工作
1.4.1总体设计要求
　　设计完成一个三自由度机电运动平台的控制算法，使其能够提供不同训练条件下模拟训练舱的空间姿态。完成
三自由度运动平台空间姿态的正，反解公式算法。使其能承载模拟座舱，模拟不同路况、不同车速条件下的装甲救
护车运动姿态，为受训对象提供近似真实的运动体感模拟，并具备异常检测和安全保护功能。
1.4.2本文完成的主要工作
　　（1）阐述三自由度机电运动平台控制的正解算法，并且比较两种算法的精确程度，完成正解算法的MATLAB代码
的编写，以及MATLAB仿真
　　（2）阐述三自由度机电运动平台控制的逆解算法，并完成逆解算法的MATLAB代码的编写，以及MATLAB仿真
（3）设计洗出算法的框图和原理图，并且利用Simulink软件完成洗出算法的仿真。

第2章 三自由度机电运动平台控制的正解算法

2.1 概述
　　三自由度机电运动平台的正解算法，是指由已知四个支撑柱的高度，通过四个高度来求得平台的Alf，Beta两个
角度的变化量。主要通过迭代法来获取方向余旋矩阵，解出四元数求得角度。三自由度机电运动平台控制算法的正
解仿真可以直观的看出Alf的角度的值和Beta角度的值以及z的量。通过与预计的量进行对比确定是否精确，以及四
个支撑柱的高度是否合理。
2.2 机电平台的主要正解解法
2.2.1 定义变量
　　对于该三自由度机电运动平台的正解算法的编写，第一步一定是定义需要的变量和已知的变量，由技术要求可
以知道本文研究的三自由度机电运动平台的运动方向和变换类型，对于机电运动平台已知的是动平台和定平台上点
的坐标，对于正解来说，我们知道的是几个支撑柱的高度的数据，即l0，l1，l2和l3，我们需要通过这些已知的数
据来求得我们需要的机电运动平台绕x轴旋转的角度Alf的值和机电运动平台绕y轴旋转的角度Beta的值，故可以先写
出需要的坐标点，并且定义出在其对应平台上的位置坐标。
1.定平台的四个点相对于定平台坐标系的数据:
Bx0 ，By0，Bz0 ；Bx1，By1，Bz1；Bx2，By2，Bz2；Bx3，By3，Bz3。
2.动平台的四个点相对于动平台坐标系的数据:
Ax0 ，Ay0，Az0 ；Ax1，Ay1，Az1；Ax2，Ay2，Az2；Ax3，Ay3，Az3。
3.支撑柱长度的数据:
l0，l1，l2，l3。
2.2.2 四元数
　　对于该三自由度机电运动平台的正解算法的求解中，能知道四元数是我们求解正解的一种有效的方法，通过四
元数我们可以方便的利用牛顿迭代来获得R阵，以此来求得机电运动平台绕x轴旋转的角度Alf角，以及机电运动平台
绕y轴旋转的角度Beta角。我们了解的表达空间刚体的姿态的方法通常用33方向余旋矩阵，可以知道方向余旋矩阵
是9个量。一般是用欧拉角表达式运算，但是用欧拉角表达运算非常复杂，它是先用三角函数运算，然后再用矩阵运
算。
　　单位四元数也可以表达空间刚体的姿态，并且单位四元数运算简单，它仅仅是代数运算。对于四元数来说，四
元数是由
四个基元线性组合构成的。
若把四元数的集合记为 ，则四元数
其中，
成为四元数 的实系数。
假设单位四元数是
位四元数的共轭，即
，因为单位四元数的模值为1，所以
，可知单位四元数的逆就是单
。
定义单位四元数在三维向量
阵：
上的作用
，可以证明
，这里
是33方向余旋矩
2.2.3算法推导
　　对于三自由度机电运动平台正解的求解，我们首先想到的是通过四元数来求解，通过代数方程组，运用牛顿迭
代，高斯牛顿迭代，或LM算法等的方式首先解出四元数，然后运用四元数我们可以推导出方向余旋矩阵，最后利用
方向余旋矩阵就可以很容易的解出我们需要的两个角度Alf和Beta的大小，解出正解。四元数
表达的是并联机构动平台的姿态，我们知道四元数是正解的未知变量，并且四元数满足
归一化条件，即:
。因此可以知道四元数其实也仅有3个自由度。
四元数与方向余旋矩阵的关系是：

（2.1）
　　假设动平台上的四个点位置坐标为：
其均是常数。动平台上的对应点的位
置坐标在静平台坐标系上表示为：
后点的位置坐标。
这几个量表示的是中间变量，也表示的是运动
三自由度机电平台的的原理示意图
我们可以利用
即
与
的关系来获得
的值，
(2.2)
展开R阵，并且化简公式我们可以获得如下结果：
　　因为机电运动平台的支撑住的长度是固定长度：
。而且定平台上的点位置坐标为：
是不会变化的。我们也已知定平台上的点的位置是

和运动后动平台的点的位置是
，由这些点的位
置，我们可以列出勾股定理方程，来获得部分方程：
（2.3）
将上式展开并且利用归一化条件:
（2.4）
　　可以获得4个方程，和4个未知变量
正式求解四元数，下面的问题就是代数方程组求解。
由此可得MATLAB代码如下：
迭代初始：
不大于4次的代数方程。获得方程组之后，采用迭代的方法来
x1=1;x2=0;x3=0;x4=0;
epsilon=2.4E-5;
　　 f=[((x1^2+x22-x3^2-x42)Ax1+2(x2x3-x1x4)Ay1+2(x2x4+x1x3)Az1-Bx1)^2+(2(x3x2+x1x4)
Ax1+(x1^2-x22+x3^2-x42)Ay1+2(x3x4-x1x2)Az1-By1)^2+(2(x4x2-x1x3)Ax1+2(x4x3+x1x2)Ay1+
(x1^2-x22-x3^2+x42)Az1+l0-Bz1)^2-l12; ((x1^2+x22-x3^2-x42)Ax2+2(x2x3-x1x4)Ay2+2
(x2x4+x1x3)Az2-Bx2)^{2+(2*(x3*x2+x1*x4)*Ax2+(x1}2-x2^2+x32-x4^{2)*Ay2+2*(x3*x4-x1*x2)*Az2-By2)}2+
(2(x4x2-x1x3)Ax2+2(x4x3+x1x2)Ay2+(x1^2-x22-x3^2+x42)Az2+l0-Bz2)^2-l22; ((x1^2+x22-x3^2-
x4^{2)*Ax3+2*(x2*x3-x1*x4)*Ay3+2*(x2*x4+x1*x3)*Az3-Bx3)}2+(2(x3x2+x1x4)Ax3+(x1^2-x22+x3^2-x42)
Ay3+2(x3x4-x1x2)Az3-By3)^{2+(2*(x4*x2-x1*x3)*Ax3+2*(x4*x3+x1*x2)*Ay3+(x1}2-x2^2-x32+x4^2)
Az3+l0-Bz3)^2-l32;
x1^2+x22+x3^2+x42-1]
　　 while norm(f)>epsilon列出限制条件，保证最后获取数值能够收敛，并且控制精度。
　　pf1px1=2(2Ay1x2 - 2Ax1x3 + 2Az1x1)(l0 - Bz1 + Az1(x1^2 - x2^2 - x3^2 + x4^2) - Ax1*
(2x1x3 - 2x2x4) + Ay1*(2x1x2 + 2x3x4)) - 2*(2Ax1x4 + 2Ay1x1 - 2Az1x2)(By1 - Ay1(x1^2

• x2^2 + x3^2 - x4^2) - Ax1*(2x1x4 + 2x2x3) + Az1*(2x1x2 - 2x3x4)) - 2*(2Ax1x1 - 2Ay1x4

• 2Az1x3)(Bx1 - Ax1(x1^2 + x2^2 - x3^2 - x4^2) + Ay1*(2x1x4 - 2x2x3) - Az1*(2x1x3 +
  2x2x4));
  　　pf1px2=2*(2Ay1x2 - 2Ax1x3 + 2Az1x1)(By1 - Ay1(x1^2 - x2^2 + x3^2 - x4^2) - Ax1*(2x1x4
• 2x2x3) + Az1*(2x1x2 - 2x3x4)) - 2*(2Ax1x2 + 2Ay1x3 + 2Az1x4)(Bx1 - Ax1(x1^2 + x2^2 -
  x3^2 - x4^2) + Ay1*(2x1x4 - 2x2x3) - Az1*(2x1x3 + 2x2x4)) + 2*(2Ax1x4 + 2Ay1x1 -
  2Az1x2)(l0 - Bz1 + Az1(x1^2 - x2^2 - x3^2 + x4^2) - Ax1*(2x1x3 - 2x2x4) + Ay1*(2x1x2 +
  2x3x4));
  　　pf1px3=- 2*(2Ay1x2 - 2Ax1x3 + 2Az1x1)(Bx1 - Ax1(x1^2 + x2^2 - x3^2 - x4^2) + Ay1*
  (2x1x4 - 2x2x3) - Az1*(2x1x3 + 2x2x4)) - 2*(2Ax1x2 + 2Ay1x3 + 2Az1x4)(By1 - Ay1(x1^2

• x2^2 + x3^2 - x4^2) - Ax1*(2x1x4 + 2x2x3) + Az1*(2x1x2 - 2x3x4)) - 2*(2Ax1x1 - 2Ay1x4

• 2Az1x3)(l0 - Bz1 + Az1(x1^2 - x2^2 - x3^2 + x4^2) - Ax1*(2x1x3 - 2x2x4) + Ay1*(2x1x2 +
  2x3x4));
  　　pf1px4=2*(2Ax1x4 + 2Ay1x1 - 2Az1x2)(Bx1 - Ax1(x1^2 + x2^2 - x3^2 - x4^2) + Ay1*(2x1x4

• 2x2x3) - Az1*(2x1x3 + 2x2x4)) - 2*(2Ax1x1 - 2Ay1x4 + 2Az1x3)(By1 - Ay1(x1^2 - x2^2 +
  x3^2 - x4^2) - Ax1*(2x1x4 + 2x2x3) + Az1*(2x1x2 - 2x3x4)) + 2*(2Ax1x2 + 2Ay1x3 +
  2Az1x4)(l0 - Bz1 + Az1(x1^2 - x2^2 - x3^2 + x4^2) - Ax1*(2x1x3 - 2x2x4) + Ay1*(2x1x2 +
  2x3x4));
  　　以此类推可得pf2px1，pf2px2，pf2px3，pf2px4；pf3px1，pf3px2，pf3px3，pf3px4.

pf4px1=2x1;
pf4px2=2x2;
pf4px3=2x3;
pf4px4=2x4;
我们可以建立并联机构的位姿方程组:
其中， 位姿输出
（2.5）
各驱动输入
　　运用联立的几个方程，我们就可以求解出唯一的解，因为机电运动平台的位姿是唯一确定的并且各驱动输入
的值固定不变。所以只要对（2.5）的方程组求解即可获得所需要的答案。
　　首先需要将方程（2.5） 对参数B 求导,可得到微分方程（2.6）初值问题，运用求导后得到的内容来组成 阵，
然后判断 阵是不是奇异阵从而来选择迭代的方式。已知方程初值是
大小。
，当驱动为
时，可以获得动平台的位姿X的
（2.6）
对方程组(2.6)，直接采用牛顿型下降程序进行迭代逼近，即：
（2.7）
对于预估和校正可以分两步进行：
（1）预估方面采用的方法是欧拉法
（2.8）
（2）对于点的校正，我们选择使用牛顿迭代
（2.9）
终止判矩
2.2.4数值迭代
　　对于三自由度机电运动平台的正解求解，其实重要的一步就是求解出多元多项式方程组
的解。其中
，n是未知数的个数,m是方程个数。有等式成立：
。
其中，
是 在 处的雅克比矩阵。
1.当n=m，并且雅克比矩阵可逆时,用牛顿迭代法
终止判矩
，会收敛在 附近得到一个数值解。
2.若雅可比矩阵不可逆，则可用高斯牛顿迭代来求解，即求出
来得到以下联立的方程组。

其中高斯牛顿迭代过程中
3.若高斯牛顿迭代过程中
异。这就是
必须非奇异。
奇异，且第一种方法也不适用时，可以通过引进 ，来克服
算法
的奇
　　选取恰当的参数 ，LM算法因为引入了一个参数值，所以在计算的时候就可以保证
是非奇
异阵，而且所求的
也不会有过大的偏差。
故可编写代码如下：
J=[ pf1px1 pf1px2 pf1px3 pf1px4 ;
pf2px1 pf2px2 pf2px3 pf2px4 ;
pf3px1 pf3px2 pf3px3 pf3px4 ;
pf4px1 pf4px2 pf4px3 pf4px4 ];
因为其中的J奇异并且高斯牛顿迭代方法也不适用，所以选用LM算法。
JTJ=J’J+0.0001[1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
norm(f)
dth=inv(JTJ)J’(-1.0f);
x1=x1+dth(1);
x2=x2+dth(2);
x3=x3+dth(3);
x4=x4+dth(4);
　　 f=[((x1^2+x22-x3^2-x42)Ax1+2(x2x3-x1x4)Ay1+2(x2x4+x1x3)Az1-Bx1)^2+(2(x3x2+x1x4)
Ax1+(x1^2-x22+x3^2-x42)Ay1+2(x3x4-x1x2)Az1-By1)^2+(2(x4x2-x1x3)Ax1+2(x4x3+x1x2)Ay1+
(x1^2-x22-x3^2+x42)Az1+l0-Bz1)^2-l12;
　　 ((x1^2+x22-x3^2-x42)Ax2+2(x2x3-x1x4)Ay2+2(x2x4+x1x3)Az2-Bx2)^2+(2(x3x2+x1x4)Ax2+
(x1^2-x22+x3^2-x42)Ay2+2(x3x4-x1x2)Az2-By2)^{2+(2*(x4*x2-x1*x3)*Ax2+2*(x4*x3+x1*x2)*Ay2+(x1}2-
x2^2-x32+x4^{2)*Az2+l0-Bz2)}2-l2^2;
　　 ((x1^2+x22-x3^2-x42)Ax3+2(x2x3-x1x4)Ay3+2(x2x4+x1x3)Az3-Bx3)^2+(2(x3x2+x1x4)Ax3+
(x1^2-x22+x3^2-x42)Ay3+2(x3x4-x1x2)Az3-By3)^{2+(2*(x4*x2-x1*x3)*Ax3+2*(x4*x3+x1*x2)*Ay3+(x1}2-
x2^2-x32+x4^{2)*Az3+l0-Bz3)}2-l3^2;
x1^2+x22+x3^2+x42-1];
norm(f);
由四元数转变成方向余旋矩阵可得T1：
T11=x1^2+x22-x3^2-x42;
T12=2(x2x3-x1x4);
T13=2(x2x4+x1x3);
T21=2*(x3x2+x1x4);
T22=x1^2-x22+x3^2-x42;
T23=2*(x3x4-x1x2);
T31=2*(x4x2-x1x3);
T32=2*(x4x3+x1x2);

T33=x1^2-x22-x3^2+x42;
T1=[T11 T12 T13;
T21 T22 T23;
T31 T32 T33]即R
以此获得所求的角度，正解得解
syms Alf Beta
Txx=[1,0,0;
0,cos(Alf/180pi),-sin(Alf/180pi);
0,sin(Alf/180pi),cos(Alf/180pi)];
Txy=[cos(Beta/180pi),0,sin(Beta/180pi);
0, 1, 0;
-sin(Beta/180pi),0,cos(Beta/180pi)];
T=Txy*Txx
Beta1=acos(T11);
　　Beta=rad2deg(beita1) 因为上一步求出的为角度的弧度，这一步可以把弧度转化成角度来表达。
Alf1=asin(T32/T11);
Alf=rad2deg(alf1)
2.2.5方法比较
　　按照上述的方法，输入预计Alf角为-10度，Beta角为10度时候的四个支撑住的高度，然后进行仿真，我们可得
到仿真结果如下图：
　　能够发现仿真结果与预计结果有些许的偏差，因为技术要求对于精度的要求，所以需要进一步减小该误差，为
了能够更好的减小偏差，尝试采用了另一种算法，因为机电运动平台的运动方式仅仅只有z轴的位移和绕x轴旋转，
绕y轴旋转这三种运动的方式，所以我们可以知道该机电运动平台的运动只和Alf，Beta两个角度的偏差的大小，以
及在z轴上的位置偏差的数值有关。故我们可以在初始迭代时可以直接化简矩阵f，并且在公式（2.2）使用R阵的时
候不直接用公式（2.1）的R代入四元数与方向余旋矩阵而是直接代入与欧拉角相关的矩阵，如下图，由此得到的J阵
虽然也是奇异矩阵，但是他却可以使用高斯牛顿迭代来求解所需要的两个角度，因为此时的
非奇异，从
而不需要采用LM算法来迭代求解。第一种使用LM算法的方法，引进一个变量
能够收敛，得到结果而放弃了对于精度的要求。
，具体的仿真结果因为会为了使结果

相比于第一种方法而言，这种化简后使用高斯牛顿迭代的方法，得到的精确度更加的高，也使得结果能够更加
的接近预计的角度。
相关代码只要令
　　f=[(By1-Ay1cos(Alf)+Az1cos(Beta)sin(Alf)-Ax1sin(Beta)sin(Alf))^2+(Ax1cos(Beta)-Bx1+Az1sin
(Beta))^{2+(l0-Bz1+Ay1*sin(Alf)+Az1*cos(Beta)*cos(Alf)-Ax1*cos(Alf))*sin(Beta))}2 - l1^2;
　　(By2-Ay2cos(Alf)+Az2cos(Beta)sin(Alf)-Ax2sin(Beta)sin(Alf))^2+(Ax2cos(Beta)-Bx2+Az2sin
(Beta))^2 + (l0 - Bz2 + Ay2sin(Alf) + Az2cos(Beta)cos(Alf) - Ax2cos（Alf)sin(Beta))^2 - l2^2;
　　 (By3-Ay3cos(Alf)+Az3cos(Beta)sin(Alf)-Ax3sin(Beta)sin(Alf))^2+ (Ax3cos(Beta) - Bx3 +
Az3sin(Beta))^2 + (l0 - Bz3 + Ay3sin(Alf) + Az3cos(Beta)cos(Alf) - Ax3cos(Alf)sin(Beta))^2 -
l3^2];
然后带入高斯牛顿迭代
dth=inv(JTJ)J’(-1.0f);
即可求得正解。
　　为了能够更进一步确认，第一种方法的精度是否能更好，所以又进行了几组仿真，来确认结果的严谨性，仿真
结果如下：
首先运用逆解的算法解出当Alf=20度，Beta=15度，时候的四个支撑柱的长度如下：
然后代入这四个支撑柱的高度，采用方法一的代码来进行仿真，得到结果如下：

不难发现方法一的结果的精确度很低。然后采用方法二来进行仿真，可以得到如下结果：
　　
可以看出方法二的结果精确度明显高于方法一。之后又按同样的方法选取了几组数据，获得的结果均是方法二的精
确度更好一点，所以由此可以比较得出两种正解方法的区别，并且也都验证了，两种方法均可以实现三自由度机电
运动平台控制算法的正解求解。
2.3 本章小结
　　本章主要是推导出三自由度机电运动平台控制的正解算法公式代码，并验证其准确性。本章通过了两种方法来
进行了正解公式的推导，主要区别在于迭代方面，方法一是因为J矩阵为奇异阵，为了解决这个问题，运用了LM算
法，使得能够获得正解，但是仿真发现，得到的精度却不是很理想，所以就寻求了新的方法，使得能更好的满足正
解的同时，希望精度可以更进一步，从而达到技术要求，固由此推导出来了方法二，方法二并没有如同方法一中一
般先去求四元数，然后从而迭代，而且利用了平台运动的原理，直接简化了f阵，使得J阵可以用高斯牛顿迭代，然
后进行仿真出的结果可以明显的看出比运用方法一后的仿真结果更加的精度，更好的满足并实现了正解的要求。

第3章 三自由度机电运动平台控制的逆解算法

3.1 概述
　　三自由度机电运动平台的逆解算法，是指由已知Alf，Beta的两个角度以及平台在z轴方向上的位置变化量，来
求得四个支撑柱的高度，并画出仿真图形。主要通过位置姿态变换矩阵来获取变化后的动平台的对应点的位置坐
标，然后通过勾股定理和定平台的对应点的坐标来求得四个支撑柱的高度，完成仿真。三自由度机电运动平台控制
算法的逆解仿真可以直观的看出四个支撑柱的高度以及平台的位置的变换，能够方便的判断出平台完成了什么样的
运动。
3.2 机电平台的主要逆解解法
3.2.1 定义变量
　　对于逆解算法的编写，第一步一定是定义需要的变量和已知的变量，由技术要求可以知道本文研究的三自由度
机电运动平台的运动方向和变换类型，对于机电运动平台已知的是动平台和定平台上点的坐标，对于逆解来说，我
们知道的是三个变化量的数据，即机电运动平台绕x轴旋转的角度大小Alf，机电运动平台绕y轴旋转的角度大小Beta
和机电运动平台在z轴上面的位置的量，需要通过这些已知的数据来求得我们需要的四个支撑柱的高度，故可以先写
出需要的坐标点，并且定义出在其对应平台上的位置坐标。
1.定平台的四个点相对于定平台坐标系的数据:
Bx0 ，By0，Bz0 ；Bx1，By1，Bz1；Bx2，By2，Bz2；Bx3，By3，Bz3。
2.动平台的四个点相对于动平台坐标系的数据:
Ax0 ，Ay0，Az0 ；Ax1，Ay1，Az1；Ax2，Ay2，Az2；Ax3，Ay3，Az3。
3.变化量的数据:

Alf，Beta，z。
3.2.2 算子
　　我们所需要用的算子一般包含平移算子，旋转算子和变换算子这三种，算子是一种表示坐标系间点的关系的数
学表达式，通常是用矩阵的形式来表示。本章我们的平台运动有向z轴方向平移，绕x轴和绕y轴的旋转，也有综合前
面三种运动方向的变换，固运算时，需要用到平移算子，旋转算子，同时也需要变换算子，并且，变换算子需要的
次数会更多，变换算子是一种既可以表达物体位移量也可以同时表示物体旋转量的一个算子。
3.2.2.1 平移算子
　　平移算子即表示一个空间上的点在经过x，y，z三个坐标轴上位置的平移变换后，为了能让这个点在原坐标系中
表示，而定义出来的矩阵，空间上的点乘以该矩阵即可获得对应方向平移之后的变换后的位置。一般的平移算子都
是有一个三阶单位阵和三个平移量组成的一个四阶方阵，该方阵可以表示出x轴，y轴和z轴的三个方向上的全部偏移
量。
通用的平移算子矩阵如下图：
　　但是本文的机电运动平台运动按技术要求来看，仅仅涉及到了机电运动平台在z轴上面的的位置的变换，平台不
能往x轴和y轴的方向进行移动，仅仅能够在z轴的方向上下平移，故可以使得在x轴上的偏移量qx和在y轴上的偏移量
qy直接为0，固可以简化通用的平移算子矩阵从而得到我们需要的平移算子矩阵，不难获得矩阵的定义如下：
3.2.2.2 旋转算子
　　平移算子即表示一个空间上的点在绕x，y，z三个坐标轴上旋转后的位置，为了能让这个点在原坐标系中表示，
而定义出来的矩阵，空间上的点乘以该矩阵即可获得对应方向旋转之后的变换后的位置。旋转算子因为绕的不同的
轴旋转而获得的旋转算子的矩阵都会有所不同：
比如若机电运动平台绕的是x轴旋转，并且旋转的角度的x度，那么我们就可以获得的旋转算子则是：
Rx= [1 0 0
0 cos(x) -sin(x)
0 sin(x) cos(x)]
如果机电运动平台绕y轴旋转，旋转的角度的y度，那么得到的旋转算子矩阵则会是这样：
Ry= [cos(y) 0 sin(y)
0 1 0
-sin(y) 0 cos(y)]
如果机电运动平台绕z轴旋转，旋转的角度的z度，那么得到的旋转算子矩阵则会是这样：
Rz= [cos(z) -sin(z) 0
sin(z) cos(z) 0

0 0 1]
　　如果机电平台线绕x轴旋转x度后绕y轴旋转y度，那么得到的旋转算子矩阵T，则用T=RxRy来表示。多个旋转的
旋转算子求法，以此类推可得。
　　但是本文的机电运动平台运动按技术要求来看，涉及的是机电运动平台绕x轴旋转和机电运动平台绕y轴旋转两
种情况，机电运动平台绕x轴旋转的角度定义变量为Alf，机电运动平台绕y轴旋转的角度定义变量为Beta，本文涉及
的机电运动平台的变换方式有：机电运动平台可以单独的绕x轴旋转，可以单独的绕y轴旋转，机电运动平台也可以
既绕x轴旋转，接着又绕y轴旋转。故可以定义机电运动平台绕x轴旋转的旋转算子矩阵为TXX，机电运动平台绕y轴旋
转的旋转算子矩阵为TXY，因为根据平台运动的需要可以把这两个矩阵相乘，得到可以满足机电运动平台运动变换要
求的旋转矩阵R，因为平台变换既有旋转也会有平移，固把三阶的旋转矩阵扩展到四阶，由此方便后续与平移变换一
起时候的计算，具体的绕x轴旋转和绕y轴旋转的旋转矩阵可以如下定义：
绕x轴旋转矩阵：
绕y轴旋转矩阵：
3.2.2.3 变换算子
　　变换算子是平移算子和旋转算子的综合，它既表示了一个空间上的点在绕x，y，z三个坐标轴上旋转，也包含了
空间上的点在经过x，y，z三个坐标轴上位置的平移。能够综合的描述点在平移并且旋转变化之后的位置。变换矩阵
是一个旋转算子矩阵和平移算子矩阵的组合，具体的组合方式如下，若定义旋转矩阵为Rx，可以知道Rx是一个三阶
的方阵，机电运动平台在x轴上面的偏移量为QX，机电运动平台在y轴上面的偏移量为QY，机电运动平台在z轴上面的
偏移量为QZ，假设平移算子矩阵是T=[QX;QY;QZ]是一个31的矩阵。那么我们可以获得由Rx这个旋转算子矩阵和T这
个平移算子矩阵一起构成的变换算子矩阵TT为：
TT= [ Rx T
0 0 0 1]
3.2.3算法推导
　　对于该机电运动平台的逆解算法的推导中，为了能够更加方便的求出运动后的动平台上面的点的位置坐标，所
以采取直接与算子矩阵相乘的方法来推导，为了能使得相乘能够顺利的进行，所以我们需要先将定义变量中的数据
矩阵化，用矩阵的方式来表示定义变量中动平台的点的坐标，定平台的点的坐标和变化量中的两个角度的值以及z轴
的位置，具体的定义可以如下表示：
1.动平台的四个点相对于动平台的坐标系的数据：
PA0=[0;0;0;1]
PA1=[700;0;0;1]
PA2=[-700;700/2;0;1]
PA3=[-700;-700/2;0;1]
使用矩阵的方式来定义数据：
Ax0=PA0(1,1);
Ay0=PA0(2,1);
Az0=PA0(3,1);

Ax1，Ay1，Az1；Ax2，Ay2，Az2；Ax3，Ay3，Az3。以此类推。
2.定平台的四个点相对于定平台的坐标系的数据：
PB0=[0;0;0;0]
PB1=[700;0;0;0]
PB2=[-700;700/2;0;0]
PB3=[-700;-700/2;0;0]
使用矩阵的方式来定义数据：
Bx0=PB0(1,1);
By0=PB0(2,1);
Bz0=PB0(3,1);
Bx1，By1，Bz1；Bx2，By2，Bz2；Bx3，By3，Bz3。以此类推。
3. 动平台的四个点相对于定平台的坐标系的数据：
坐标变换矩阵：
TAB=TwTxxTxy;
Pa01=TABPA0;
Pa11=TABPA1;
Pa21=TABPA2;
Pa31=TABPA3;
通过变换矩阵来获得变换后的动平台上的点的新的位置。
4.计算支撑柱的长度：
　　选用先获取各支撑柱的矢量长度，后直接采用勾股定理求解。以支撑柱l0为例，矢量长度可以Lx0=ax0-Bx0;
Ly0=ay0-By0;Lz0=az0-Bz0;来求解。
当输入平台z轴方向上变量为1200，对x轴偏角Alf值为-10，y轴偏角Beta值为10
时，可得到如下仿真结果：
支撑柱长度：

改变Alf，Beta的角度，使其为0，从而更直观的观察平台转动的变化：
支撑柱长度：
由此可以看出，平台能够实现倾斜运动。
　　若改变平台的则可以直接改变z轴的值，假定原图Alf为-10，Beta为10，z为1200，后改变z为900，其他的不

支撑柱的长度如下：
以此可以看出能实现平台的上下运动变化。至此可知逆解得解。
3.3 本章小结
　　本章主要是推导出三自由度机电运动平台控制的逆解算法公式代码，并验证其准确性。本章求解三自由度机电
运动平台逆解的方法主要是通过首先由已知的动平台坐标和算子矩阵，来先求出运动后的动平台上的点的坐标，并
且把他们在定平台的坐标系里面表示出来，求解的过程，为了计算能够更加的方便和准确，就都把点的坐标矩阵
化，然后进行矩阵相乘，在获得了用定平台坐标系表示的运动后的动平台的坐标之后，直接采用了数学中学过的勾
股定理的公式，来利用定平台的点和运动后的点的坐标值，直接解出了四个支撑柱的高度。通过MATLAB仿真的结
果，不难看出此方法可以实现三自由度机电运动平台控制的逆解，该逆解能够满足机电运动平台的z轴方向的运动，
机电运动平台的绕x轴方向的旋转和机电运动平台的绕y轴方向的旋转的变换。所以是能够满足三自由度机电运动平
台控制的。

第4章 三自由度机电运动平台控制的洗出算法

4.1 概述
　　前面两章内容写的是三自由度机电运动平台控制算法中的正解算法和逆解算法，但是在把算法放入实际的项目
中进行使用时，会发现预计的结果和真实的表现会有所偏差，因为各种的外界干扰，以及机电运动平台所能运动的
空间的限制，可能会让使用的人不能更真实的感受到机电运动平台的各种运动。算法中模拟的三自由度机电运动平
台的运动幅度要完成某些高难度的运动的时候，可能会超过实际技术要求中的所给定的范围，并且为了能够更加真
实的模拟出需要的场景，所以需要使用一下洗出算法来让算法在实际使用的时候更加的真实，洗出算法主要实现的
作用是让平台上面的受训练的人能够在有限的平台运动的空间内更加真实的感受到平台的运动。
4.2 洗出算法的模型的构建
4.2.1 洗出算法简述
　　本文研究的三自由度机电运动平台，由于动平台和定平台之间的四根支撑柱的伸缩量有限，并且机电运动平台
的运动幅度有限，所以只能通过一定的算法来改变对机电运动平台的控制输入，使得机电运动平台在一定的运动范
围内运动，可以在一次运动之后回到中心位置，为下一次的运动提供空间，并且使得有较高的运动逼真的感觉，这
种算法就是洗出算法。
　　洗出算法的种类有很多，有基于模拟预测控制的洗出算法，也有经典洗出算法等的多种，本文主要选用了经典
的洗出算法来完成洗出算法的仿真。我们可以通过图1对比找出最优的洗出算法，也可以通过其输入更直观的看出洗
出算法的作用，所以可以通过下图中的输出来观察，通过洗出算法之后输出量的变化。经典洗出算法能够使得机电
运动平台在完成一次运动后能够在感受者不发现的情况下慢慢移动到指定的位置，使得平台的运动空间可以大大的

增加，该算法最主要的部分是当中的线性低通滤波器和线性高通滤波器部分。但是组成该算法的坐标变换环节，限
制环节等的环节也是必不可少的。
4.2.2 洗出算法设计框图
输入量
输出变量
洗出算法人体前庭模型人体前庭模型输出变量

图2为人体的前庭模型，通过图2的模型我们可以清楚的看出，输入的加速度量和角速度量通过人体前庭模型之
后的变化，人们感觉到的角速度的量是由实际输入的角速度通过半规管通道模型之后输出来的角速度，而人们感觉
到的加速度的量则，不仅仅与输入的加速度的量通过耳石模型后产生的量有关，还与角速度的输入量有关，具体的
关系如图2所示。
实际输入加速度
感觉到的加速度
实际输入角速度
感觉到的角速度

4.3 洗出算法原理图
　　下图3为洗出算法的原理图。洗出算法的主要过程是，首先输入量进入系统，然后判断输入量的幅值大小是否超
过一定的界限，如果没有则正常输入，如果超过了给定的界限就按一定的缩放比例进行缩放，使得输入在给定的界
限之中，然后进行滤波的环节，坐标变换环节，最后输出对应的坐标。
加速度输出高通滤波器
加速度输入量缩放/正常输入幅值判定
平台角位移变换矩阵低通滤波器
+角速度输入量
+高通滤波器幅值判定缩放/正常输入

4.3.1 缩放
　　 在洗出算法中，缩放是很重要的一步，当一个输入信号进入系统时，如果输入信号的幅值过于的大从而超出了
平台能够移动到的范围，那么则需要对其进行缩放操作，因为使用场地等的限制，并且为了能够在有限的运动空间
内更好更多的模拟出一些加速，大幅度旋转等的场景，所以进行缩放的操作就十分有必要了。
　　我们首先将输入的信号波形放置于一个坐标系之中，我们把这个坐标系分成三个部分，其中包括正值需要缩放
部分、正常输入部分和负值需要缩放部分。为了能够让机电运动平台更好的完成需要的运动,我们需要在正、负值需
要缩放部分内对输入的过大的信号进行缩放变换。以正值部分的变换为例：假设输入的信号幅值为 , 是幅值判定
中高低的分界处，也是缩放区域和正常输入区域的分界处。 是机电运动平台运动空间幅值的最大值。 是幅值的
最大值。其中 ， ， 视机电运动平台的实际情况等的多种因素共同决定的。一般都是通过仿真实验来不断实
验，并且修改然后继续实验，反复多次然后能找到合适的数值，如机电运动平台的机械尺寸较大，那么分界处的值
和上限值都可以选的更大一点。
　　假设缩放前的输入如图4所示， 的值是1.5，而机电运动平台运动空间幅值的上限值 则是2，输入幅值的最
大值 是2.5。我们以正幅值缩放区域的幅值片段举例，那么进行缩放之后则可以达到如图5所示的修正后的效果。

图4.缩放前输入值
图5.缩放后输入值
其中缩放比例 我们可以按照如下的公式来求解，即：
(4.1)
缩放后的输入信号 的求解公式则如（4.2）所示：
(4.2)
　　其中 是缩放前的输入信号， 是缩放后的输入信号， 是幅值判定中高低的分界处，也是缩放区域和正常输入
区域的分界处的值。 是机电运动平台运动空间幅值的上限值。 是幅值的最大值。
4.3.2 高通滤波器
　　高通滤波器又被称为截止滤波器，能让高频信号通过并且阻止低频信号通过从而产生干扰的一种器件。高通滤
波器有无源高通滤波器和有源高通滤波器的区别，也有一阶高通滤波器和二阶高通滤波器的区别主要的差异是由于
构成高通滤波器的器件的不同而造成的。
　　在本文的洗出算法的过程中，滤波环节是核心环节，滤波环节包括：高通滤波和低通滤波两中，其中高通滤波
又包括高通加速度环节和高通角速度环节。在高通加速度环节需要代入的传递函数分成两种，一种是加速度信号在x

轴和y轴上传输所需要的传递函数（4.3）但本文所控制的三自由度机电运动平台因为在平移控制中只能进行z轴上的
位移，固不需要考虑这种传递函数，另一种则是加速度信号在z轴上传输所需要的传递函数（4.4），这是我们需要
的高通加速度滤波器的传递函数。同样在角速度输入信号的处理中，我们需要用到的则是高通角速度滤波器，其传
递函数如（4.5）所示。
(4.3)
是阻尼比，
是对应方向上的响应频率。
(4.4)
为z轴方向上的阻尼比，
是二阶环节的响应频率，
是一阶环节的响应频率。
(4.5)
是高通滤波器的阻尼比，
4.3.3 低通滤波器
是响应频率。
　　 低通滤波器和高通滤波器的作用类似，都是用来消除信号干扰，只是低通滤波器是允许低频信号通过，而高频
率信号则不能通过，常见的有源低通滤波器可以用于信号处理，但低通滤波器的主要作用还是消除干扰。
　　 本章洗出算法中的低通滤波器中的低通滤波通道又称为倾斜协调通道，起到的主要作用是低通滤波以及把部分
低通的加速度信号转化成角速度信号，用于获得坐标变换矩阵和机电运动平台的位姿。低通滤波器中所采用的传递
函数如（4.6）所示，而把低频加速度量转化成角速度所需要运用的公式则是（4.7）和（4.8）两个。
(4.6)
是角速度阻尼比，
是角速度响应频率。
(4.7)
(4.8)
是x轴方向输入的信号量， 是y轴方向输入的信号量， 是重力加速度。
4.4 洗出算法的仿真
4.4.1人体前庭模型仿真
　　 对于洗出算法的仿真，我们首先把人体前庭模型进行了仿真，采用的是Simulink软件，首先把器件按图6进行
连接，然后检测当仅有加速度信号输入且输入波形如图7所示时输出的加速度波形可以观察到如图8所示。若再加入
与加速度信号同样波形的角速度信号时输出的加速度信号和角速度信号分别如图9，图10所示。其中耳石模型中的
的值为0.4， 的值为13.2， 的值为0.66， 的值为5.33.而半规管模型中参数 的值是30， 的值是0.1， 的值
是5.3.

图9.同时输入角速度信号后输出 图10.同时输入角速度信号后输出的角速度
的加速度信号波形图 信号波形图
4.4.2洗出算法模型仿真
　　然后又建立了洗出算法的连接图如图11.所示。洗出算法中当加速度输入信号如图12所示，角速度输入信号则如
、
、
的值等于0.1倍的
所以等于0.4
.在公式（4.5）的传递
函数中
的值代入1，
的值是1
.低通滤波器则代入公式（4.6），其中 的值为1，
的值为2.5
.

图11.洗出算法连接图

若把输入的加速度信号波形变换成和前面人体前庭模型的加速度信号输入波形一致后，把洗出算法原理图和人
体前庭模型组合后再进行仿真，我们可以得到图15，通过和不连接洗出算法后输出的加速度信号波形图进行对比不
难发现，通过洗出算法后，加速度的波动更加的明显，在平台上的人能感知到的加速度变化也更加的明显。

图15.通过洗出算法后人体前庭模型的加速度信号输出波形
4.5 本章小结
　　本章主要是推导出三自由度机电运动平台控制的洗出算法，并仿真验证其准确性。本章首先是从洗出算法中的

经典洗出算法入手，通过经典洗出算法来完成三自由度机电运动平台的控制的洗出算法。洗出算法的主要作用是改
变机电运动平台的控制输入，使得机电运动平台在一次运动之后可以回到原来的位置，这样可以为下一次的运动提
供空间，并且使得体验的人有更加逼真的体验。
　　本章在洗出算法方面主要解决了洗出算法的设计框图，洗出算法的原理图以及原理图中的缩放，高通滤波器，
低通滤波器等的问题。对于洗出算法也有一些相关的简述。然后通过Simulink软件，进行了相关的仿真，首先仿真
的是人体前庭模型，通过仿真得到没有任何干扰之下的输入量，经过人体前庭模型后会输出的信号波形。然后再进
行洗出算法的模型的仿真，在输入一定的输入信号之后，进行仿真然后获得输出的波形。最后把洗出算法和人体前
庭模型进行结合，再输入相同的输入量，然后观察对比输出的结果波形。可以明显的发现仿真的结果有往好的一方
面的改善，洗出算法能够起到改善输出的效果。该洗出算法是有一定作用的。

致 谢

三自由度机电运动平台的控制算法开发工作和论文的撰写是在我的导师周万勇副教授的指导下认真完成的，这
是一个实际的项目，在整个项目中，周万勇老师付出了很多，在我遇到困难的时候，他会先听我诉说，然后和我一
起探讨，找到我的疑惑的点，然后和我一起寻找出解决的办法，有时候他不会直接告诉我答案，而是给了我一种指
引，让我自己去寻找那个答案，最后还会给出他的答案与我的进行比较。这种先让我自己思考最后一起总结的方
式，让我能更多的思考并且进行对比多种方法的不同，感觉真的很有收获。在撰写本文的过程中，我学到了很多关
于机器人方面的知识，从课题的开题到最后论文的完成，这一系列的过程让我深切的感受到认真完成一项工作的不
容易。在这中间遇到过许多的困难，但每次都在周老师的精心指导下得到成功解决，这无疑都隐藏着周老师辛勤汗
水的付出。从周老师对待学问严谨求实的态度和他渊博的专业知识储备以及他无私奉献的精神上我收获了很多，也
学到了很多。
所以，在此我要向我的导师周万勇副教授表达十分的感谢之意，感谢他对我的帮助和在这段时间他对我的教导。
　　时光匆匆，恰如朱自清所写的那样，时间总是在你不经意之间就过去了，恰如在你学习的时候，时间从你的身
边溜走，在你走路的时候，时间就从身边跑走，在玩耍的时候，时间就从指尖滑走了。一转眼大学四年就过去了，
也将与一起生活了四年的同学告别，思绪纷飞，犹想起一起经历过的悲欢往事，想起大学中教导过我的每一位老
师，不舍之情难以言表。真心的感谢每一位教导过我的老师，是你们让我明白了很多我不曾知晓的道理，知识。我
也要感谢在大学里的朋友，还记得在我生病的时候陪伴在我身边和我一起坐车的人，在我堕落的时候鼓励我重整旗
鼓的人，在我备战考研的时候，和我一起努力拼搏的人，谢谢你们的陪伴和帮助。
所以我要感谢每一位帮助过我，关心过我的老师和同学，谢谢你们的帮助和对我的关心。
我也要感谢参加评阅我的论文和我的毕业答辩的每一位老师和教授。