基于并联稳定平台的火炮跟踪仿真研究

杜黎童，荆学东，王旭飞，李 诚

（1.上海应用技术大学机械工程学院，上海 200000；2.上海应用技术大学电气与电子工程学院，上海 200000）

0 引言

并联机构由于具有很高的刚度、准确的定位能力，且负载能力大，已经被广泛地应用在飞机装配［1］和并联机床［2］等各个领域。目前，国内外对六自由度并联平台的研究都比较多，而很多的研究对象都是基于Stewart D 在1965 年提出的用作飞行模拟器的六自由度的平台［3］。

并联平台的运动性能对整个机构的应用有很大的影响。并联机器人的逆运动学通常是比较容易求解的，而正运动学一般是很复杂的［4-9］。本文通过牛顿- 欧拉法建立机构的动力学模型，并且通过Adams 仿真验证理论模型的正确性，通过建立逆动力学控制模型，引入Adams/Control 模块，在Matlab的Simulink 模块中搭建控制程序，进行轨迹跟踪的联合控制仿真。

1 理论方法

1.1 坐标系的建立

基于Stewart 建立的并联机构的三维模型如图1 所示，包括动平台（上平台）、静平台（下平台）、6个结构相同的支链。

图1 并联平台坐标系Fig.1 Coordinate system of parallel platform

图2 绕移动坐标系{B}的x 轴旋转Fig.2 Rotation of axis x around the moving coordinate system{B}

其中，静平台与支链通过虎克铰连接，动平台与支链通过球副等效结构（虎克铰加一个转动副）连接。动平台的位置和姿态是由6 条支链的伸缩来改变的。在静平台中心建立坐标系{A}，在动平台中心建立坐标系{B}。下铰链点Ai在静平台的坐标用向量ai表示，上铰链点Bi在动平台的坐标用向量bi表示。

1.2 姿态描述

图3 绕移动坐标系{B'}的y'轴旋转Fig.3 Rotation ofaxis y' around the moving coordinate system{B'}

图4 绕移动坐标系{B''}的z''轴旋转Fig.4 Rotation of z''axis around the moving coordinate system{B''}

本文采用的是旋转矩阵的方法描述上平台的姿态。在旋转开始前，移动坐标系{B}的初始方位与固定坐标系{A}重合，先将{B}绕x 旋转α 角度，此时的移动坐标系记作{B'}，再将{B'}绕y'旋转β 角度，此时的移动坐标系记作{B''}，最后将{B''}绕z''旋转角度，得到最终的移动坐标系{B}。

最终可以得到，坐标系{B}相对于坐标系{A}的姿态为

其中，cα 是cosα 的简写，sα 是sinα 的简写，以此类推。

1.3 逆向运动学分析

并联机构的运动学分析涉及机构运动几何关系的研究，不考虑引起运动的力和力矩。正向运动学分析的是已知各支链的杆长，求解运动平台的位置和姿态；逆向运动学分析的问题是假设运动平台的位置和姿态是已知的，求解各个支链的长度。

每条链的封闭环方程为：

1.4 动力学分析

本文求解并联平台动力学方程采用的是牛顿-欧拉法，它是机器人学中用于求解并联机械臂动力学方程的最为流行的方法。牛顿-欧拉法考虑了所有支链以及动平台，并且应用于每一个独立的个体。首先列出支链的Newton-Euler 方程，将支链的速度，加速度带入方程，得到各支链对动平台的作用力；再分析动平台的动力学问题，本文假设动平台受到的外力Fd和外力矩nd为0，最后将支链的动力学方程与动平台的动力学方程结合，可以消去支链对动平台的作用力fbi这个内部力，得到整个并联平台的封闭式动力学方程。动平台的受力分析图和支链的受力分析图如图5 和图6 所示。

图5 动平台的受力分析图Fig.5 Force analysis diagram of moving platform

图6 支链的受力分析图Fig.6 Force analysis diagram of branch chain

由文献［10］可知，利用Newton-Euler 法得到的Stewart 并联机构的封闭式动力学方程如下所示：

其中，

1.5 逆动力学模型控制

逆动力学控制（inverse dynamics control，IDC）可以明显削弱并联机构的非线性和耦合特性，从而提高线性控制器的性能［11］。图7 为并联平台IDC 的控制器框架，在闭环系统在增加了修正力Ffl，它是由动力学模型中的科氏力和离心力矩阵以及重力向量得到的。在前馈系统中增加了期望轨迹的加速

图7 逆动力学模型控制框图Fig.7 Control block diagram of inverse dynamics model

并联平台的闭环动态方程可以写成如下形式：

因为动态矩阵是由并联平台的反馈值计算得到的，因此，动态矩阵等于实际的M、C、G，即。此时，并联平台的闭环动态方程可以简化成如下形式：

从此方程可以看出，误差的动态特性为二阶系统。因为质量矩阵在所有系统状态下都是正定的，因此，动态方程可以写为：

通过选择合适的PD 控制器增益，控制系统可以具有更快的暂态响应和更小的稳态误差。

2 利用ADAMS 建立模型及仿真分析

2.1 ADAMS 虚拟样机模型的建立

本文分析的六自由度并联机构是通过SolidWorks 建模，装配后，另存为*.X_T 格式后导入的，导入完成后，利用连接工具库添加需要的约束，使各个构件联接成一个机械系统。

设置材料属性，将上平台和每条链的上下部分都设置为不锈钢材料，则上平台的质量为m=0.815 kg，支链的上半部分质量为mi2=0.14 kg，支链的下半部分质量为mi1=0.82 kg。

2.2 运动学仿真

在模型的上平台质心处，施加点运动（general point motion），可以实现空间六自由度的运动，如图8 所示。参考点（reference point）选择地面或静平台上的点，设置运动类型（type）为位移（disp），设置运动方程。定义模型的上平台沿Z 方向做幅值为10 mm、频率为1 Hz 的正弦运动，即位移函数为：

将仿真的结束时间（end time）设置为一个运动周期1 s，仿真步数（Steps）设置为100 步。建立点到点的测量，选择上铰链中心点到下铰链中心点。因为是沿z 轴方向的正弦运用，所以如图9 所示，6 条链的长度随时间的变化是一样的。

图9 运动学仿真的6 条链的长度曲线Fig.9 The length curve of 6 chains simulated by dynamics

为了验证仿真模型的正确性，将仿真模型的结构参数以及运动函数带入到运动学逆解的理论模型中，得到链长变化曲线如图10 所示。

图10 Matlab 计算的链长变化曲线Fig.10 chain length variation curve calculated by matlab

将Matlab 编程得到的链长变化的理论结果与仿真结果进行做差，得到误差曲线如图11 所示，可以看到链长误差在10-3mm 的量级，说明理论结果与仿真结果在每一时刻的链长是一样的，因此，说明虚拟样机模型是正确的。

图11 链长变化的误差曲线Fig.11 Error curve of chain length variation

2.3 动力学仿真

将上节中仿真得到的6 条支链的长度变化曲线添加依次添加为样条函数。

在后处理模块（post processor）中，选择将曲线创建样条曲线按钮，这样可以得到6 个样条曲线函数：SPLINE_1～SPLINE_6。然后将在动平台上添加的点驱动设置为失效（deactivate），再在每条支链的移动副上添加移动驱动MOTION_1～MOTION_6。设置每个驱动的驱动函数依次为 AKISPL（time，0，SPLINE_1，0），…，AKISPL（time，0，SPLINE_6，0），如图12 所示。

图12 设置驱动函数Fig.12 Driving parameter setting

在每个驱动上添加力测量，设置仿真时间1 s，步长为100，运行仿真，可以得到平台产生相应运动所需要的驱动力，如图13 所示，并与在matlab 中计算的驱动力作对比，如图14 所示，得到对比的误差曲线，如图15 所示。

图13 动力学仿真各支链的驱动力Fig.13 Driving force of each branch chain simulated by dynamics

图14 Matlab 计算的各支链的驱动力Fig.14 Driving force of each branch chain calculated by matlab

图15 各个支链的驱动力误差曲线Fig.15 Driving force error curve of each branch chain

从驱动力误差曲线可以看到，仿真结果与理论计算的结果基本相同，驱动力误差在10-2N 量级，因此，可以证明根据本文并联机构建立的动力学模型是正确的。

2.4 逆动力学控制联合仿真

本文通过ADAMS 和Simulink 搭建机电联合控制仿真系统，ADAMS 进行系统方程的解算，并且提供机械系统的输入、输出数据接口，Simulink 实现对控制方程的解算，并且负责接收ADAMS 的求解结果。在求解过程中，每隔一段时间两者进行一次数据交换［12］。控制系统的框图如图16 所示。

图16 ADAMS/Simulink 联合控制框图Fig.16 Joint control block diagram of adams/simulink

打开Adams/Control 模块，在输入框中添加的是施加在6 条支链上的驱动力，输出框中添加的是上平台的线位移和线速度，以及角位移和角速度，如下页图17 所示。

图17 ADAMS/Control 输入、输出接口Fig.17 Input and output interface of adams/simulink

打开Matlab，在命令窗口输入命令adams_sys，该命令是ADAMS 和Matlab 的接口命令。保留adams_sub 模块，根据逆动力学模型创建需要的函数模块，在simulink 库中添加PID 模块以及生成矩阵和可视化模块，最终搭建的整个系统的控制流程图，如图18 所示。

图18 Simulink 逆动力学控制流程图Fig.18 Inverse dynamics control flowchart of simulink

本文所进行的算例是，上平台1 s 内沿z 轴的平移30 mm，同时绕z 轴旋转30°。

其直线位移随时间变化的曲线方程为

其角位移随时间变化的曲线方程为

最终的仿真结果如下页图19 和20 所示，图19为沿z 轴位移的期望轨迹和实际轨迹图，图20 为绕z 轴旋转的期望轨迹与实际轨迹图。从两张图中可以看到，无论是平移还是旋转，并联平台在逆动力学模型控制下，都能够很好地跟踪期望轨迹。

图19 沿z 方向位移的轨迹跟踪图Fig.19 Trajectory tracking diagram of shifting along z direction

图20 绕z 轴旋转的轨迹跟踪图Fig.20 Trajectory tracking diagram of rotating around axis z

图21 是沿z 轴方向移动的轨迹跟踪误差，图22 是绕z 轴转动的轨迹跟踪误差。

图21 平移的跟踪误差图Fig.21 Tracking error diagram of shift

图22 旋转的跟踪误差图Fig.22 Tracking error diagram of rotation

由跟踪误差图可以看到，无论平移方向的误差，还是旋转方向的误差都在10-2量级，说明利用逆动力学控制模型可以达到很好的路径跟踪效果，对其应用在火炮跟踪，稳定调姿方面提供了坚实的理论基础。

3 结论

为验证六自由度并联稳定平台在路径跟踪方面的快速调姿性能及其准确性，本文采用ADAMS多体动力学和运动分析软件，对建立的并联机构模型进行分析，并且结合Simulink，验证所建立的动力学模型。然后采用逆动力学控制模型，引入Adams/Control 模块，并结合Simulink 进行路径跟踪的联合仿真。最终结果表明，该控制模型可以实现很好的路径跟踪控制。

该仿真方法可以实现参数化建模，无需繁杂的公式推导及计算，可以实现运动参数测量以及运动可视化，以及对后续的结构优化、搭建六自由度稳定平台的物理样机和火炮系统的跟踪控制提供了理论基础。