1 位置解算算法

对于并联机构的六自由度平台在运动过程中，要保证运动的实时性和正确性，就需要通过对伸缩杠的精确控制来实现，这就需要引入六自由度平台的实时位置正反解算法。所谓六自由度平台的位置反解，是指由运动平台的空间姿态求六个伸缩杠的伸缩量。而六自由度的位置正解，是指有六个伸缩杠的伸缩量来求运动平台的空间姿态。

本次设计仍然采用位置反解的算法作为六自由度平台的位置算法。经过推算后动平台的各点坐标相对于静平台的各点坐标的方向转换矩阵表示如下：

其中a，b，c对应为动平台在X轴，Y轴，Z轴三个方向上的位移；，，对应为动平台绕X轴，Y轴，Z轴三个轴旋转的角度。

2 连续运动规划

本设计将依次完成对YYPT平移、旋转以及X-Y圆周运动三种运行曲线。

1、平移运动：动平台不对X、Y、Z任一轴做旋转，以平台中心为控制点，在X、Y、Z任一轴向做往复轴向运动。

2、旋转运动：以平台中心位于X-Y平面的原点，并在Z轴行程中心，在X、Y、Z绕任一轴做角度旋转的往复运动。

3、X-Y平面圆周运动：YYPT平台在Z向行程为一半的X-Y平面上，以平台中心为原点沿所规划的圆形轨迹运动。其公式为，其中X、Y分别为圆形在X、Y上的距离，r为所画圆的半径，按照此公式计算上平台的运动轨迹。

3 控制策略

采用了位置环加驱动器内部的速度环、电流环相结合的三环反馈的控制方法。在控制算法中采用了前馈的算法，使控制效果更平稳、更精确。

4 控制软件

软件设计中采用了模块化、面向对象的设计方法，将程序分为以下模块：

1）六通道伺服控制模块：在1ms伺服周期内，采集每个通道的反馈信号，将PID计算的信号对应通道输出，并做必要的限位判断。

2）六通道PID运算模块：在1ms的伺服周期内，将反馈的信号和设置的信号进行PID运算，得出输出信号量。

3）运动曲线规划模块：设置平台按照规划好的曲线方程进行运动。

4）六通道PID参数设置模块：设置六支电动缸的PID参数。

5）数据采集及输出模块：采集编码器的反馈数据并根据PID计算出的结果输出相应的脉冲量。

5 软件流程框图

软件流程框图