精密并联机器人控制算法及控制系统研究概要

第４０卷第４期２００４年４月机械工程学报Ｖ０１．４０Ｎｏ．４ＣＨＩＮＥＳＥＪＯＵＲＮＡＬ０ＦＭＥＣＨＡＮＩＣＡＬＥＮＧＩＮＥＥＲＩＮＧＡｐｒ．２００４精密并联机器人控制算法及控制系统研究木张秀峰孙立宁（哈尔滨工业大学机器人研究所哈尔滨１５０００１）摘要：首次把数字ＰＩＤ算法应用到面向光纤作业的精密并联机器人控制中，介绍了这种高速、高精度小型并联机构控制系统的新控制算法及系统研究情况。

另外控制系统采用了ＤＳＰ新技术，解决了并联机构运动学逆解的实时在线计算问题，使系统运行更加稳定。

试验结果表明这种新算法在小型精密并联机构控制系统中，完全可以满足光纤对接等作业的高技术要求，同时也为同类高精度、大行程小型定位系统的控制与设计提供了一种新的实用方法。

关键词：并联机构运动学逆解ＰＩＤ控制算法中图分类号：ＴＰ２４０前言在高速、高精度、大行程小型并联机器人控制领域，所知文献介绍的实用控制算法还未见到。

在实际工程控制中ＰＩＤ控制算法不需要系统确切的数学模型，参数易调整，且具有很强的灵活性、适应性，其中数字ＰＩＤ控制算法在计算机上易修正，比模拟ＰＩＤ控制器性能更加完善。

首次将数字ＰＩＤ控制算法引进到高精度并联机构的控制中，并借助高速数字信号处理器ＤＳＰ解决了逆解的在线计算问题，试验结果表明可以满足高速、高精度等技术要求。

另外还介绍了系统的组成、性能、技术指标及一些关键参数的调整方法和经验公式，为小型精密定位系统的设计与控制提供了有价值的借鉴。

１ＰＩＤ控制算法１．１模拟ＰＩＤ控制器所谓ＰＩＤ控制器是指把偏差按比例、积分和微分进行的控制器，其中模拟ＰＩＤ控制器是用硬件来实现的。

设ｌ，为系统给定，Ｙ为系统输出，萨砷为系统偏差，ｕ为系统控制规律…¨，则“＝Ｋ，［Ｐ＋寺Ｊ：：酣ｒ＋％詈］＋“。

式中Ｋ，——比例系数正——积分常数毛——微分常数＝三——偏差微分ｄｆ在控制过程中系统有偏差产生，调节器产生控制作用使偏差不断减小，这种控制作用的强弱取决于凰的大小。

积分调节器主要用来消除静差，同时也会降低系统的响应速度，增加系统输出的超调。

由式（１）可以看出，积分常数死越大，积分的累积作用效果就越弱，反之积分作用则越强。

微分调节器是在ＰＩ调节器的基础上，根据偏差变化的情况预先给出适当的控制量，其作用主要是用来阻止偏差的变化，减小超调，克服振荡，使系统趋于稳定。

１．２数字ＰＩＤ控制器计算机控制只能根据采样时刻正的偏差值ｅ，计算控制量ｕ，，进行数字控制。

在式（１）中积分和微分项不能直接准确算出，只能用集和式代替积分，增量式代替微分做近似计算得式（２）。

当采样周期ｒ足够小时，数字控制与模拟控制结果基本接近“，＝Ｋ，［色＋署薹；色＋等Ｇ，一Ｐ，一。

）］＋“。

ｃ２，将式（２）中ｕｉ和ｕＨ相减并改写一下形式，最后得Ａｕｆ＝ＫｐＡｅｆ＋ＫｆＰｆ＋ＫＤ（Ａｅｆ）２（３）（１）式中ｕ，——第ｉ个采样时刻的输出值ｅ，——第ｉ个采样时刻的系统输出偏差＋国家８６３计划资助项目（２００１ＡＡ４２２１２０）。

２００３０２１４收到初稿２００３０９２５收到修改稿ｅ¨——第ｆ一１个采样时刻的系统输出偏差ｕ。

——原始控制初值一般式（２）称为绝对式ＰＩＤ算法，式（３）称为增量式ＰＩＤ算法，与前者相比，增量式算法计算量要小得多，这里所采用的算法为增量式ＰＩＤ控制算法。

万方数据１７８机械工程学报第４０卷第４期２系统控制的设计思想２．１控制系统的组成如图ｌ所示为６－ＰＳＳ精密并联机器人机构原理图。

它为立式直线输入方式，由电动机通过联轴器带动滚珠丝杠来实现【２＇３］机构末端６自由度的控制。

该控制系统主要是由一台ＰｅｎｔｉｕｍＩＶ工控机、６个ＨＣ—ＫＦＳ交流伺服电动机、６个ＭＲ．Ｊ２．ｓｕｐｅｒ驱动器和１块ＭＣＴ８０００ＦＸ８轴伺服卡组成，如图２所示。

该伺服系统具有以下几个特点：采用了高性能的ＣＰＵ（ＤＳＰ数据处理器），速度频响可达５５０Ｈｚ以上，解决了并联机构运动学逆解在线实时计算的问题，使系统控制更加稳定；采用了高分辨率编码器１３１０７２Ｐ／ｒ实现高性能化和提高低速稳定性，同时具有更高精度的控制能力；自动调谐性能；高适应性的防振控制；机械共振滤波器的设定；增益搜索功能等。

ＢｌｌＶＢ图１６－ＰＳＳ并联机器人机构原理图工控机ＭＣＴ８０００ＦＸ伺服运动控制器（速式控制方式）驱动器ＪＪ驱动器ＩＪ驱动器＂驱动器ｊｌ驱动器』驱动器到圜圜圜圉医ＭＲ—Ｊ２一ｓｕｐｅｒ驱动器可进行参数设定、试运行、状态显示等功能。

ＭＣＴ８０００ＦＸ伺服卡可通过ＩＳＡ／ＰＣＩ总线或ＵＳＢ接口直接与ＰＣ机相连接，提供两个双端口ＲＡＭ通信通道，一个通道用于发送和接收命令，另～个通道用于主机与控制器之间的数据传递，实现了在程序运行过程中，实时读取控制器运行状态和参数的功能。

编码器计数频率高达１７ＭＨｚ，每轴伺服刷新周期ｌＯＬＬｓ。

控制器上有２ＭＨｚ的闪存用于非易失性存储，可保存应用程序、系统参数、数组和底层软件。

２．２精密并联机器人控制算法研究若已知输出件的位置和姿态，求解机构输入件的位置，称之为机构运动学的反解…４，５１。

并联机构末端位姿的控制是通过运动学反解来实现的。

如图２所示，由几何关系得运动学反解公式ｒ１１ｚ；一Ｚ，＝±峙一（ｘ；一Ｘ∥～（ｙ；一ｔ）２ＦＰｊ２Ｚｊ式中只——机构输入Ｘ：，Ｙ：，ｚ：——动平台铰点坐标在定坐标系下的绝对坐标ｘ：，ｙ凡ｚ：——定平台铰点在定坐标系下的坐标，，——固定杆杆长，Ｊ＝１，２，…，６设输入为零时，定平台的位置为零位，由图２可以看出在动平台运动中，动平台应始终在零位之上，所以ｚ：一Ｚ，≥０，即．１１Ｐｊ＝ｚ；～ｔｌ；一（ｘ；一Ｘ∥～（ｙ；一＿）２』ｉ（４）在并联机构末端每完成一次位姿控制中，系统运行的条件、要求都基本相同，这样可以利用计算机的计算、储存功能，将上一次位姿实现后的信息应用到下一次目标位姿的控制中，从而使得系统更加满足目标位姿控制的要求，提高系统的动态响应速度和控制精度。

设第ｆ次迭代中，时间为［０，丁］，系统的控制输入为Ａｕ，，（ｔ），电动机码盘测得的实际位置为Ｐ，，（ｆ），将其保存在计算机内，在第ｉ次迭代完成后，由末端位姿给定值只，（ｆ）和码盘实际检测的位置值Ｐ，，（ｔ）计算得第ｆ次迭代的系统静差ｅｕ（ｔ）＝ｅ／，，（ｔ）一Ｐｕ（ｔ）由ｅｉ，ｊ（ｆ）和ＰⅢ（ｆ）得Ａｅｉ，ｊ（ｆ）＝ｅｉ，ｊ（ｆ）一ｅｉ－ｌ，ｊ（ｆ），进而得到△２巳．，（ｆ）＝Ａｅｉ，ｊ（ｆ）一Ａｅｉ１ｊ（ｆ），从而由式（３）得到第ｉ次迭代的控制输入Ａｕｉ，ｊ（ｆ）＝ＫｐＡｅｉ，ｊ（ｆ）＋Ｋｅｉ，ｊ（ｆ）＋Ｋ０△ｉ白，，（ｆ）０≤ｆ＜ｒ在本精密并联机构中，令移动副滚珠丝杠的螺距为Ｍ，电机转过一周时，光电码盘读数为只脉冲数），则Ｐｆ，，（Ｄ＝（码盘实际读取脉冲数厂Ｐ）×Ｍ，即为滚珠丝杠移动副当前位置。

另外在程序设计中还应该注意以下几个问题，这些问题直接影响着机构末端的定位精度。

（１）为了满足光纤对接作业控制精度的要求，万方数据２００４年４月张秀峰等：精密并联机器人控制算法及控制系统研究１７９设系统控制精度为ｅｓｔ，当Ｉｅ，，ｌ≤ｅｓｔ时认为本次给‟ｏ定位姿已达到，否则程序继续循环迭代，直到满足要求。

在系统初调时，如果给定控制精度过高，则满足精度要求会需要很长时间，此时可通过调整Ｋ，，Ｋ。

，ＫＤ参数来缩短调整时间。

（２）机构在完成给定位姿的过程中，应该在启动、结束阶段对速度进行规划，即有一个缓慢的上升和下降过程，以防止机构在启动、停止时有过大的冲击。

（３）在并联机构点位控制中，机构末端走过的轨迹是一条弧线，如果弧线过大往往会造成机构运动不平稳，因此应把预定轨迹分成若干段连续完成，这样机构在运行中才能平稳，同时也会减小机构末端的定位误差。

（４）在系统实际控制中，一般还要在主程序前加一个复位程序，使机构在运行前处于机械零点位置，然后再开始进行给定目标位姿的实时控制。

在复位程序中预先给出超过满行程的位置参数，该参数数值应使机构移动副向下运动。

在复位程序中还应编写６个对应的子控制程序ＰＩ功，用它们控制对应的各杆移动副同时向下移动。

当移动副碰到等高位置限位时，开始向上搜索电动机ｚ信号，如果６个ｚ信号都得到，机构６个移动副一起向上移动相同高度，即零点位置ｈ，此时系统静差为ｅｆ，，（ｆ）＝［只，，（ｆ）＋ｈ］一【Ｐｆ，，（ｆ）＋ｈ］机位移输出曲线，如图４所示。

吕暑＾挺遥图３精密并联机器人图４数字ＰＩＤ控制６轴位移输出曲线３鍪宝！１２算法在精密并联机构中的４结论实际应用……使用前应对系统控制精度进行精确调试，其中主要调整伺服周期丁及算法中琊，厨，ＫＤ等参数。

首先调整参数丁，在满足系统工作要求时丁越小，系统控制精度就越高。

随后调整局，令局＝０、ＫＤ＝０，在系统不发生振动的条件下逐渐增大Ｋｐ，直到得到满意的响应速度、稳定度。

最后调整局、ＫＤ，令ＫＤ＝０，在系统不发生振动的条件下逐渐增加局，直到满足系统控制精度要求。

如果在这个过程中系统发生振动，可适当增加ＫＤ抑制振动。

对于图３所示系统而言ｒ取Ｏ．４～１．０ｍｓ、Ｋｐ取４６～７２、ＫＩ取０．１～７．２、Ｋｏ取０～０．００２，经验公式为：Ｋ，≈１０Ｋ。

，Ｋ。

≈１００ＫＤ。

当末端位姿给定为沿Ｘ轴方向移动４／ｎｒｆｌ、Ｙ轴方向移动６ｎｌｍ、ｚ轴方向移动一７ＩｎｌＴｌ、绕ｘ轴转动５。

、绕Ｙ轴转动一６。

、绕ｚ轴转动ｌｏ、Ｔ＝０．４２ＩｌｌＳ、Ｋｐ＝６８、ＫＩ＝６．６、Ｋｎ＝０．００６４和工作速度为４ｍｍ／ｓ时得到的６轴电采用伺服速度控制方式，通过放大器内部速度环及外部位置反馈实现双闭环控制。

另外ＭＣＴ８０００伺服卡对编码器输入脉冲４倍频后计数，可４倍于原编码器精度的控制，同时该卡采用了高性能数据处理器ＤＳＰ，借助于这些前提条件实现了用常规ＰＩＤ算法对精密并联机构进行高精度的点位控制。

试验结果表明当各轴达到给定位姿时，系统控制精度０．０３３“ｍ，运动分辨率０．１ｐｍ。

系统没有发生振荡，响应快、稳定，因此这种算法能够满足高速、高精度、大行程等技术指标的控制要求，可以作为高精度小型并联机器人定位系统的控制算法。

该控制策略同样适用于其他类高速、高精度多轴定位系统的运动控制。

参考文献１陶永华．新型ＰＩＤ控制及其应用．北京：机械工业出版社，２００２万方数据１８０机械工程学报第４０卷第４期２姜兵，黄田．６－ＰＳＳ并联机器人操作机平动工作空间解析．机器人，２０００，２２（２）：１３６～１４２３金振林，王军，高峰．新型６－ＰＳＳ并联机器人工作空间分析．中国机械工程，２００２，１３（１３）：１０８８～１０９０４黄真，孔令富，方跃法．并联机器人机构学理论及控制．北京：机械工业出版社，１９９７５黄真．并联机器人及其机构学理论．燕山大学学报，１９９８，２２（１）：１３～１８Ｒ皿ＳＥＡＲ℃ＨｏＦＰＲＥＣＩＳＥＰＡＲＡＩ。