并联机器人仿真运动控制的多线程实现Multithreading Realization of Simulation Motion Control of the Parallel Robot（海军工程大学）彭利坤邢继峰肖志权曾晓华PENG, Likun XING, JifengXiao, Zhiquan Zeng, Xiaohua摘要：现代运动模拟器对响应快速性、跟踪准确性等仿真运动特性提出了更高要求，使得并联机器人机构的运动控制更为复杂。
以某型潜艇操纵模拟器为例，其控制软件采用模块化设计，利用NT环境下多线程技术,结合多媒体定时器、普通定时器,实现软件的洗出滤波、运动学反解、运动信息发送、安全保护等多任务的有机调度。
在外控线程中建立网络数据接收、数据处理、液压缸控制信息发送等三个子线程,将它们从外控线程中分离出来,大大提高了数据传输和处理及运动控制的实时性和可靠性。
关键词：并联机器人；多线程；多媒体定时器；运动控制中图分类号:TP311.1; TP391.9 文献标识码：A文章编号：Abastract: In order to meet the demand of the emulational kinetic characteristics of modern motion simulator such as fast response and precise tracking, the control system of the parallel robot mechanism becomes more complex. As an example of submarine manipulating simulator, the modularization design and the technologies such as the multimedia timer, common timer and multithreading under NT environment etc. are adopted in the control system programing, which realize the multitask scheduling of washout filter, inverse kinematics solution, sending control data and safeguarding. By separating three sub-threadings, the Ethernet data acquisition, the data processing and the hydraulic cylinder control information sending, from the external control threading, the real time performance and reliability of the data transmitting, processing and motion control can be improved.Key words:parallel robot; multithreading; multimedia timer; motion control1 引言飞机、舰船、赛车、列车等运动模拟器系统，是以Stewart平台为原型的并联机器人机构最重要的应用方向，它们一般构成分布式半实物仿真系统。
以某型潜艇操纵模拟器为例，整个模拟系统由教练控制台、模拟潜望镜、舱段操艇装置、六自由度（6DOF）并联机器人机构等四个分系统构成，几个分系统通过以太网传输交换数据。
其中液压6DOF并联机器人为模拟潜艇空间运动的关键机构，其控制软件必须完成潜艇姿态数据的接收、处理、控制执行器（一般为液压缸）动作、实时动态显示分析等繁杂的任务。
这种多任务的软件开发，基于过程的编程设计已显得力不从心，而面向对象的多线程编程因其具有接口能力强、并行处理、运用灵活等优点，成为设计本控制软件的首选。
2 控制模块该控制软件包括洗出滤波算法、运动学反解、内控、自检、外控、逻辑控制、安全保护、平台起停、实时动态显示、正解监控、网络通讯等多个控制模块。
2.1 洗出滤波算法虽然各种运动模拟器模拟运动的侧重点有所不同，但洗出滤波总是需要的。
在模拟运动过程中，液压缸的行程有限，故在一次动作完成后，必须换向回到中性位置，以使下一个运动模拟有足够的行程，通常将这种回到中性位置的附加运动称为运动的洗出。
通过运动学仿真、质心坐标转换等计算过程，而得到被仿真设备的速度、加速度，再通过高、低通滤波器滤波、积分等一系列算法转化为运动平台的线位移和角位移的过程称为运动的滤波。
经典的彭利坤：博士研究生基金项目：军队研制基金资助项目（JXB-2004-21）滤波算法如图1所示。
因Matlab 软件具有强大的数值计算功能，其自带的Simulink 软件也集成了大量可直接利用的工具箱，故洗出滤波算法利用Matlab 工具箱编制，使其在后台运行，通过动态链接库（DLL）调用。
2.2 运动学反解控制当已知并联机器人机构输出件（上平台）的位置和姿态，求其主动件（液压缸）的输入运动称为机构的运动学反解。
模拟器运动机构不断接收潜艇姿态的仿真数据，由该模块进行运动学反解计算后，控制液压缸实时跟踪其运动，提供给训练者尽可能逼真的体感运动。
2.3 运行方式因为不同的需要，将运行方式设置为三种模式：自检、内控、外控。
自检时并联机器人上平台作单自由度正弦运动，用于系统运行前的检查。
内控可以进行平台的手动调试，此时可通过“单自由度调节按钮”控制平台作单自由度或复合运动。
外控是平台的主要工作方式，此时平台通过网络通讯接收上位机的运动信号，跟随上位机输出信号运动。
2.4 逻辑保护逻辑控制和保护功能主要是通过数字I/O 采集卡和运动控制卡上的I/O 口，管理运动控制及其附属设备之间的所有I/O 操作，通过I/O 接口采集系统所有模拟转换和开关状态，在线监测系统的运行状态，并进行故障分析和处理，通过安全保护模块对平台实施相应的保护动作。
2.5 动态显示在实时动态显示线程中，可以反映系统运行的信息：液压缸缸长、液压缸初始定位、上平台名义位姿和实际位姿、油温油压、运行方式、声光报警等，使操作者在控制台上对并联机器人机构的运行情况一目了然。
2.6 正解监控在系统运动过程中，利用位移传感器通过运动控制板卡实时采集液压缸缸长，通过机构的运动学正解，求得上平台的实际位姿，以和上平台的名义位姿进行误差计算，达到监视与保护的目的。
3 多任务调度以上多个控制模块决定了系统控制软件的复杂性，故软件在VC++编程环境中开发，并综合运用多媒体定时器、普通定时器，以及多线程编程技术，实现仿真运动控制的多任务调度。
其总体流程图如图2所示。
基于WM_TIMER 消息的普通定时器是低精度的，它最多可以精确到54.915毫秒，并且WM_TIMER消息的优先级比较低，因此普通定时器定义的线程中进行作用相对次要的动态显示。
另外由于并联机构的位置正解耗时较长，而在控制中它只起监视作用，并不直接参予平台的控制，故该模块也置于普通定时器线程中。
为了随时采集系统的数据而“不丢失”，并尽快实施相应的控制与保护动作，需要精度和优先级都较高的抢占CPU 时间的方式，在此采用多媒体定时器，其定时分辨率最高为1ms，远远高于Windows 提供的默认普通Timer 的性能。
利用多媒体定时器开辟的独立线程作为运行方式（自检、内控、外控）切换控制、起动、停止并联机构运动平台的主程序。
当并联机构上平台起动完毕到达中性位置，且运行方式确定后，停止该线程的工作，进入相应的运行方式线程。
在运行方式线程（并行的其它两个线程停止）中，除完成上平台名义位姿的运动学反解计算，发送液压缸控制信息（至运动控制板卡）外，还必须实时监测系统的运行情况，如运行方式切换、或有停止运行指令、或发生严重故障时，立即停止该线程，同时起动主控线程（多媒体定时器）并产生相应动作。
在三个运行方式线程中，外控是仿真运动控制线程，其最为常用，且控制流程也最为复杂，除完成以上操作外，网络通讯数据的接收与处理的同步问题最为关键。
从以太网中接收到潜艇的仿真姿态（包括位移、速度、加速度），写入并存放于缓冲区中，而数据读出后的处理相对复杂，包括运动的洗出滤波计算、运动学反解、向运动控制板卡发送信息等等，耗时较长，所以在网络连续发送数据的情况下，采用一个工作线程专门负责数据接收工作，而用另一个线程负责对接收的数据进行处理，第三个线程发送控制信息。
这样读取数据不会出错，而且提高了效率。
其流程图见图3所示。
下面对多媒体定时器和外控线程的创建与工作作一简单说明。
首先多媒体定时器作如下定义#define WM_MAINPROCCTRL(WM_USER+1) //多媒体定时器的消息int TimerAccuracy = 1 //多媒体定时器精度UINT TimerID //多媒体定时器标志ON_MESSAGE(WM_MAINPROCCTRL,MainProcCtrl) //消息映射TimerID=timeSetEvent(100,1,(LPTIMECALLBACK)MMTimeProc,(DWORD)this,TIME_PERIODIC);//启动多媒体定时器多媒体定时器的回调函数为：void CALLBACK MMTimeProc(UINTTimerID,UINT uMsg,DWORDdwUser,DWORD dw1,DWORDdw2){CSixDofCtrlDlg* pDlg =(CSixDofCtrlDlg*) dwUser;::PostMessage(pDlg->GetSafeHwnd(),WM_MAINPROCCTRL,0,0);}外控线程也首先进行定义CWinThread *pExterConThrd，*pNetThrd，*pHandleThrd，*pSendThrd；//定义线程CCriticalSection DataSection; //数据缓冲区临界同步量后调用下列函数启动外控线程pExterConThrd=AfxBeginThread(ExterConProc,this,THREAD_PRIORITY_NORMAL)；在外控线程中启动网络接收线程、数据处理线程、发送线程后，将外控主线程悬挂 pExterConThrd->SuspendThread()；只有接收到数据后，处理线程才开始执行操作；只有计算出新的液压缸控制信息后，计算机才会发送。