基于STM 32的电动6-DOF加载系统设计*

廖连杰，桑 勇，郭联龙，蒋路明

（大连理工大学机械工程学院，辽宁大连 116024）

0 引言

并联机器人也叫做并联机构，其特点是在基座和末端执行器之间至少存在两个运动链。可以进行纵移、横移、升降、滚转、俯仰、偏航运动的六自由度并联机构被称为6-DOF并联平台，该类平台拥有结构强度高、精度高、响应速度快等优势，是运动模拟、精密对接、医疗手术等设备[1-3]的关键组成部分，广泛应用于工业与生活当中。

位移、力加载实验在工程中十分常见，比如研究材料压缩特性时，常用单自由度位移、力拉压实验机来进行压缩试验[4]；研究土的强度和本构特性时，常用动静三轴实验仪进行单自由度的位移、力加载测试[5]。单自由度加载设备应用已经很广泛，近年来一些学者对多自由度加载设备进行了研究，许国山等[6]研究了电液伺服三自由度加载系统的考虑几何非线性的控制方法，但是针对一些需要进行高精度多自由度的位移、力加载的场合，目前大部分设备难以实现。

6-DOF并联平台能完成复杂的多自由度组合运动[7-8]，另外机电伺服系统具有结构紧凑、响应快、节能环保、精度高等特点[9-10]，综合以上两点本文研制了基于Stewart平台构型的电动6-DOF加载系统，它能实现多自由度高精度位移、力加载功能。本文首先介绍加载系统的体系结构，详细阐述各系统硬件之间的关系，之后建立了加载系统的逆运动学数学模型。针对加载装置实现力控功能的难点问题，提出了基于位移环的闭环力控的解决方案。最后设计多自由度加载实验，测试了6-DOF加载系统。

1 电动6-DOF加载系统的体系结构

电动6-DOF加载装置、控制柜和上位机组成加载系统的体系结构，体系结构的核心是基于Stewart平台构型的电动6-DOF加载装置。如图1所示，加载装置由固定平台2、上下球铰3、伺服电机4、电缸5、称重传感器6、动平台7、高精度六维力传感器8以及其他连接件组成。加载装置在地面组装完成后，固定平台2与固定架1之间用高强度螺钉连接，使得整个加载装置呈现倒挂姿态。系统加载时伺服电机4旋转带动电缸5伸缩，同时上下球铰3同步旋转，从而改变动平台7的位姿（位置和姿态）。本文自主开发的加载系统控制程序运行在工控机13上，工控机通过高速网线12连接到控制柜，控制柜存放除加载装置和上位机以外的所有配件，包括伺服驱动器9、六维力放大器10以及STM32控制器11。控制柜侧面留有一个孔，加载系统的所有接线都接入该孔，这使得现场布置整洁有序。

图1 加载系统的硬件组成

从图1可以看出加载系统各硬件之间的关系。用户在上位机中输入加载指令，随后上位机发送数据给STM32控制器。控制器有两个功能：一是产生指定的PWM脉冲；二是通过TCP协议与上位机通信。除此之外控制器不进行任何其他操作，确保系统能够稳定运行。伺服驱动器解析控制器产生的PWM脉冲后，驱动伺服电机旋转，从而改变动平台位姿。

此外，高精度六维力传感器安装在动平台的中心，将动平台所受六维力转化为电信号，随后由六维力放大器采集、放大电信号，通过UDP协议将六维力信息传送至上位机。工控机有两个网线口，一个网口控制STM32控制器产生脉冲，另一个网口接收六维力信息，两网口同时工作时互不干扰。

2 加载系统的数学模型和控制体系

根据控制系统的设计步骤，在完成完电动6-DOF加载系统的搭建后需要建立系统输入与输出之间的关系，为此本文对加载系统的逆运动学数学模型进行了建立，并完成了位移加载和力控加载的控制体系设计。

2.1 加载系统的数学模型建立

逆运动学对加载系统的作业轨迹规划和轨迹跟踪控制具有重要意义，为研究动平台的位姿与伺服电缸伸缩量之间的数学关系，需要建立加载系统的逆运动学数学模型，该模型映射了动平台的位姿和各轴长度之间的关系[11]。在建模中作以下规定。

（1）坐标系规定：分别以动平台中心A0和固定平台中心B0为坐标系原点，建立右手规则的动坐标系{A}和静坐标系{B}，如图2所示。

图2 加载装置的俯视图

（2）轴编号规定：A1~A6是动平台上各球铰的旋转中心，B1~B6是固定平台上各球铰的旋转中心，A i B i表示加载系统的第i根轴。

（3）空间位姿规定：p=[φθψx y z]T为动平台的位姿，φ、θ、ψ被称之为横滚自由度、俯仰自由度和偏航自由度，x、y、z被称之为纵移自由度、平移自由度、升降自由度，动平台初始位姿为[0 0 0 0 0 0]T，初始位姿时动平台与固定平台之间的高度用常量h表示。

上述加载系统是基于Stewart平台构型建立，6根轴的逆运动学数学模型相同，为了图示简洁，本文只画出第6根轴的逆运动学数学模型，如图3所示。图中B w6为6个动平台球铰旋转中心在{B}系下的坐标，B l6为第6根轴在{B}系下的矢量表示，B q6为6个固定平台球铰旋转中心在{B}系下的坐标。由于各矢量都是在静系{B}下的表示，根据同一坐标系内矢量的叠加原理，可得出：

图3 加载系统第6根轴的逆运动学数学模型

式中的角标B代表相关矢量是在{B}下的表示。动平台的各球铰中心在{A}下的表示是不变的，通过坐标变换，{A}下的向量能够在{B}下表示出来。本文采用Z-Y-X欧拉角变换[12]表示动平台的姿态，其中涉及的旋转转换矩阵具体表达为：

式中：Rot(φ,θ,ψ)表示{A}绕着{A}的Z轴旋转ψ，再绕新Y轴旋转θ，最后绕新X轴旋转φ的变换过程。

动平台和固定平台之间的位置关系中心的位置关系表示为pt=[x y(z+h)]T。根据Z-Y-X欧拉角变换，将{A}下的A w6变换到{B}下，得到：

结合式（1）、（3）得到第6根轴的逆运动学数学模型：

同理，从单根轴推广到6根轴，可以写出整个加载装置的逆运动学数学模型。B L为6根轴的矢量表示，A W为动平台上6个球铰中心坐标，B X为平移矩阵，B Q为固定平台上6个球铰中心坐标，整个加载装置的逆运动学数学模型为：

假设各轴的初始长度为l0i(i=1,2,…,6)，最终各伺服电缸伸缩量Δl i表示为：

2.2 加载系统的控制体系设计

本文设计了加载系统的位移控制系统，能够实现位移加载控制，其原理如图4所示。

图4 位移控制系统的原理

如图5所示，输入的位移加载指令可以是6个自由度的任意组合，用户在上位机中输入位移加载指令后，加载系统控制程序便规划动平台的运行轨迹，然后将运行轨迹分成n小段。上位机将n小段数据逐次发送给STM32控制器，之后控制器产生PWM脉冲。为了观察位移控制系统的PWM脉冲分布是否均匀，本文使用示波器在伺服驱动器的PWM信号接收端进行测试。图5显示动平台进行匀速位移加载时轴6的伺服驱动器接收到的PWM脉冲信号，图中每两个脉冲对应一小段运行轨迹，脉冲分布十分均匀，从而验证了上述的位移控制系统。

图5 轴6伺服驱动器接收的PWM信号

本文提出基于位移环的闭环力控加载方案，能够实现动平台沿某个空间方位的动态力加载。力控加载的流程如图6所示。用户输入力加载指令Fi(t)时，加载系统的控制程序首先实时比较Fi(t)和Fu(t)，Fu(t)为高精度六维力传感器测出的六维力数值，然后控制程序将力的偏差量ΔF(t)通过模糊PID控制器转化为位移ΔS(t)。ΔS(t)作为位移控制系统的输入指令，使得动平台输出一定位移，这位移使得Fu(t)更加趋近于Fi(t)，从而降低了力的偏差。

图6 闭环力控的流程

3 多自由度加载实验

电动6-DOF加载系统能够实现多自由度的位移、力加载，加载方案组合众多，为了实验方便本文以YZ轴为代表进行了位移阶跃加载，以Z轴为代表进行了位移三角波加载，以Z轴为代表进行了力正弦加载和力正弦加载，轴的规定和实验装置如图7所示。

图7 多自由度加载的实验装置

为了得到动平台的运动位移，在X、Y、Z每一个方向都安装了一个高精度激光位移传感器；为了得到动平台中心受到的六维力，在动平台底部中心位置安装了高精度六维力传感器。

3.1 位移控加载

动平台在工作空间的中心时有最大的位移加载范围，故控制动平台下降100 mm到p=[0 0 0 0 0 100]T处，在此位置进行YZ轴的位移阶跃加载，加载的位移大小为30 2 mm，加载速度分别为1 mm/s、2 mm/s、5 mm/s。YZ轴的实验结果如图8所示。从图中的实验结果来看，加载系统完成了Y、Z轴的阶跃加载，Y和Z两个方向都加载了30 mm左右的位移，通过计算得到Y、Z轴的稳态加载误差分别小于0.25%和0.20%。

图8 YZ轴的位移阶跃加载

另外还进行了3组不同幅值不同频率的X轴的位移三角波加载，第1组幅值为5 mm，频率为0.004 Hz；第2组幅值为5 mm，频率为0.02 Hz；第3组幅值为30 mm，频率为0.004 Hz。X轴的实际位移波形如图9所示。从图中的实验结果来看，加载系统实现了不同频率、幅值的X轴三角波加载，3组实验的误差小于0.49%。

图9 X轴的位移三角波加载

由以上位移控加载的实验结果知：电动6-DOF加载系统能够实现多自由度的高精度位移加载。

3.2 力控加载

加载系统的闭环力控方案基于位移环，前文通过实验已经验证了多自由度的高精度位移加载功能，为此验证单自由度的高精度力加载功能，间接证明多自由度的高精度力加载功能。以Z轴为代表进行了力正弦加载，实验结果如图10所示。图中的曲线1为理论力，曲线2为实际加载力，不难发现每次加载出现一个波峰之后，曲线2都能很快趋近曲线1，而且稳定时实际加载力的偏差控制在±0.5 N以内。

图10Z轴的力位移加载

另外进行了3组不同幅值不同频率的Z轴的正弦加载，第1组幅值为80 N，频率为0.02 Hz；第2组幅值为50 N，频率为0.05 Hz；第3组幅值为10 N，频率为0.1 Hz，实验结果如图11所示。图示结果表明力正弦加载具有平滑的波形，3组实验的最大误差小于2.3%。

由以上力控加载的实验结果知：电动6-DOF加载系统同样能够实现多自由度的高精度力加载。

4 结束语

本文基于STM32研制了电动6-DOF加载系统，该加载系统实现了高精度的位移、力加载，其中力加载一直是Stewart平台构型加载系统的难点问题。本文详细介绍了6-DOF加载系统的体系结构，以及各硬件之间的关系，然后采用Z-Y-X欧拉角方法描述加载系统动平台的空间位姿，探讨了动平台的空间位姿与伺服电缸伸缩量之间的数学关系，建立了加载系统的逆运动学数学模型。针对加载系统实现力控功能的难点问题，提出了基于位移环的闭环力控的解决方案。最后设计了多自由度加载实验，实验结果表明YZ轴的位移阶跃加载误差小于0.25%，X轴的位移三角波加载误差小于0.49%，Z轴的力正弦加载偏差控制在±0.5 N以内，Z轴的力正弦加载误差小于2.3%，客观的实验结果验证了加载系统的多自由度高精度位移、力加载功能。