专利名称：两平行轴位置速度同步控制的算法的制作方法
技术领域：
本发明涉及一种同步控制的算法，尤其涉及对两平行轴的位置和速度 进行同步控制的算法。
背景技术：
在机器人运动控制中，需要对两个平行轴的位置和速度同步控制，目 前最常见的方法有两种， 一种是两平行轴通过刚性同步轴连接，另一种是 两轴分别用两个伺服电机控制。前者的优点是，结构简单，控制成本低， 运行稳定可靠，在工业上的直角坐标机器人获得了应用，其缺点是，轴线 方向很长的同步轴加工困难，加工成本极高，故一般不可能做得很长，限 制了两平行轴的跨距，同步轴越长径向挠度越大，高速旋转时受到离心力 的作用导致机械振动和噪音，故不适合用于大跨距高转速的两平行轴的同 步控制。后者采用两个伺服电机控制，增设一台伺服电机，增加了控制成 本，但是提高了运行的平稳性，在高速重载大跨距领域具有重要的实际意 义。
用两台电机做同步运动，目前比较常见的做法是把两轴中其中的一 个设为主轴，对应的伺服电机设为主电机，该伺服电机的编码器作为位置 环反馈。另外一个伺服电机设为从电机，该伺服电机的编码器反馈值不参 与运动控制。让从电机跟随主电机运动，实现两轴位置环同步，但是不能 实现速度同步，而且这种跟踪同步算法只能在少数高档运动控制卡上实 现
发明内容
本发明的目的是提供一种对两平行轴的位置和速度进行同步控制的 算法，旨在有效解决大跨距两平行轴高速运行位置和速度同步控制的问 题，该算法不设置主动被动轴，两个位置速度力矩三闭环系统按插补规则 各自动力运行，能充分达到位置同步和速度同步，可在普通运动卡上成功 获得实现。
本发明的目的通过以下技术方案来实现
两平行轴位置速度同步控制的算法，其特征在于首先采用量具标定 两平行轴的初始同步位置，并把对应的两个初始点作为两个轴的零点o， 将两平行轴中的一根看作X轴，另外一根看作Y轴，在XOY平面从零点 起插补斜率为1的直线，直线在两轴上的投影相等且等于同步运动的目标 位置，实现两轴的位置同步，按数控插补直接搜索法，在同一个插补周期 内两轴的位移相等，实现两轴速度同步，具体包括以下步骤——
(l)标定坐标原点对于X轴，利用量具量取X轴离基准的距离， 设为L1;对于Y轴，同样利用量具量取Y轴离基准的距离，设为L2; 将这两个位置分别作为两轴的同步起点， 一般情况下两轴的同步起点取 为与某一参考基准平行；
(2a)位置同步的实现
目标直线方程的判别方程为F = 乂l _ X《，设插补点的坐标为
(《A)判别值为S，那么巧,,=^^;—Z,《，向+x方向插补一步
^+W=C.-K，向+Y方向插补一步^，州二^,乂+^;,向+X、 +
Y方向插补一步= + Ze - K ，
比较《+U， ^J+1， ^+w+i的绝对值，取最小的作为下一个插补点， 在一个插补周期开始，原点(O,O)，向+X、 +Y方向插补一步 j = 0 + - = 0 ，那么点作为下一个插补点，
依次类推(2 *械2 *敲)，(3 *械3 * fife//) ， (4 *械4 *敲)， (5*&&5*&//)作为下一个插补点，最后到达，冬点(6*&//,6*&/0，从
5原点到终点的轨迹向两个坐标轴X、 Y投影，投影大小都为6WW ，从
而实现位置同步；
(2b)速度同步的实现 在第一个插补周期T:起始点(0,0),终点(&夂A/f) ， X轴速度 ^=de/〃r， Y轴速度^-"^/r，故X轴与Y轴速度相同，
在第二个插补周期T:起点(^&c/e/0,终点(2*^/^,2X轴速 度^=&〃/7\ Y轴速度^二"e/〃r，故X轴与Y轴速度相同，
依次类推，在每个插补周期内，X轴与Y轴速度相等，从而实现速 度同步。
进一步地，上述的两平行轴位置速度同步控制的算法，两平行轴的 跨距不受任何机械结构的限制，解决同步位置和速度的大跨距难题。
更进一步地，上述的两平行轴位置速度同步控制的算法，所述量具 为游标卡尺，其精度i0.01mm，初始误差小。实际运行过程中，位置精 度不受影响。同步控制过程中，精度几乎完全取决于初始误差，便于操 作和施工。
本发明技术方案的突出的实质性特点和显著的进步主要体现在 本发明两平行轴位置速度同步控制的算法，利用量具确定两平行轴位 置同步的起始点，并把该位置作为两轴的零位，两轴按平面插补的方式运 动的最终轨迹是在第一象限斜率为1的直线，采用直接搜索法进行插补运 算，最终轨迹在两轴上的投影距离相等，达到两轴位置同步，在同一个插 补周期内两轴运动位移相等，达到两轴速度同步。直接搜索法插补斜率为 1的直线，在一个插补周期内没有误差。与现有技术相比，不设置主动被 动轴，能充分达到位置同步和速度同步，可在普通运动卡上成功获得实现， 产生良好的实际意义，社会经济效益显著。
下面结合附图对本发明技术方案作进一步说明 图h位置同步控制示意图； 图2:速度同步控制示意具体实施例方式
本发明首先利用量具标定的两平行轴的的初始同步位置，并把对应的
两个初始点作为两个轴的零点；接着采用直接搜索法，把一个轴看作X轴， 另外一个轴看作Y轴，在XOY平面插补斜率为1的直线，该直线在两轴 上的投影相等，实现两轴的位置同步；按直接搜索法，在同一个插补周期 内，两轴的位移相等，实现两轴速度同步；直接搜索法插补斜率为1的直 线，在一个插补周期内无误差。
使用量具确定同步位置的零点，两平行轴同步运动前都有一个起始位 置，起始位置一般在机械基准上或者离基准有一定距离，如果起始位置离 基准有一定距离，那么可以采用游标卡尺等量具标定好两轴的起始位置， 然后把标定的位置作为同步运动的零点。
两平行轴平行设置，其中一个看作X轴，另外一个看作Y轴，标点好 的零点作为坐标原点，那么同步运动的最终轨迹就是在XOY平面内以原 点为起点的一条斜率为1的直线，利用插补的方法走出一条斜率为1的直 线是轻而易举的。
插补平面直线有很多种方法，比如逐点比较法，直接搜索法，数字积 分法和数据采样法等。本发明技术方案采用直接搜索法作为同步运动的核 心算法，直接搜索法计算在X、 Y和斜率为1的三个方向各走一步，哪一 步的终点离理想轨迹最近，哪一步就作为下一个插补点。同步运动插补斜 率为1的直线，直接搜索法比其他算法有不可比拟的优越性。
图l、图2所示，具体通过以下详细步骤
1、标定坐标原点
坐标原点的标定是本算法实现的第一步，也是关键一歩，如图1为例，每个轴同步运动的起始点进行这样标定对于轴X，利用游标卡尺(精度
士0.01mm)量取X轴滑块距离基准的距离，设为L1，并固定X轴滑块； 对Y轴，同样用游标卡(精度i0.01mm)量取Y轴滑块距离基准的距离， 设为L2，并固定Y轴滑块，然后安装中间横梁，加上负载，把这两个位 置分别作为两轴的同步起点。 2a、位置同步的实现
目标直线方程的判别方程为F = - x《，设插补点的坐标为 (Z,，")判别值为^，那么^=^《—I/e，向+X方向插补一歩 ^+W二《V-《，向+￥方向插补一步巧,,+1=巧,,^，向+X、 + Y方向插补一步^+ww +Ze -K ，
比较^+1,,，《J+1， ^+W+i的绝对值，取最小的作为下一个插补点，
在一个插补周期开始，原点(o,o)，向+xr+Y方向插补一步
Fu =0 + ^//-A& = 0 ，那么点(^/"d^)作为下一个插补点，
依次类推(2 *础,2 *敲)，(3 *械3 *敲)，(4 *械4 *赫)，
(5 *5 *作为下一个插补点，最后到达终点(6* de/f，6* cfe/0 ，从原
点到终点的轨迹向两个坐标轴X、 Y投影，投影大小都为6*&"，从而实
现位置同步。
2b、速度同步的实现
在第一个插补周期T:起始点(O,O),终点， X轴速度 K = cfe〃/r， Y轴速度^二"e/f/r，故X轴与Y轴速度相同，
在第二个插补周期T:起点(^^,^/0，终点(2*^&2*^/0， X轴速 度^=&"/7\ Y轴速度^^de"/r，故X轴与Y轴速度相同，
依次类推，在每个插补周期内，X轴与Y轴速度相等，从而实现速度 同步。
可以表明，本发明算法利用量具确定两平行轴位置同步的起始点，并 把该位置作为两轴的零位，两轴按平面插补的方式运动的最终轨迹是在第一象限斜率为1的直线，采用直接搜索法进行插补运算，最终轨迹在两轴 上的投影距离相等，达到两轴位置同步，在一个插补周期内两轴运动位移 相等，达到两轴速度同步。直接搜索法插补斜率为1的直线，在一个插补 周期内没有误差，误差仅取决于初始标定误差，误差容易控制，实施方便。 本发明技术方案实现大跨距两平行轴采用两伺服电机单独驱动在速度和 位置上同步运行，不设置主动被动轴，充分达到位置同步和速度同步，可 在普通运动卡上成功获得实现，具有极好的实际意义，应用前景美好。
以上仅是本发明的具体应用范例，对本发明的保护范围不构成任何限 制。凡采用等同变换或者等效替换而形成的技术方案，均落在本发明权利 保护范围之内。
权利要求
1. 两平行轴位置速度同步控制的算法，其特征在于采用量具标定两平行轴的初始同步位置，并把对应的两个初始点作为两个轴的零点O，采用直接搜索法，将两平行轴中的一根看作X轴，另外一根看作Y轴，在XOY平面插补斜率为1的直线，直线在两轴上的投影相等，实现两轴的位置同步，按直接搜索法，在同一个插补周期内两轴的位移相等，实现两轴速度同步，具体包括以下步骤——(1)标定坐标原点对于X轴，利用量具量取X轴离基准的距离，设为L1；对于Y轴，同样利用量具量取Y轴离基准的距离，设为L2；将这两个位置分别作为两轴的同步起点；(2a)位置同步的实现目标直线方程的判别方程为F＝yXe-xYe，设插补点的坐标为(Xi，Yi)判别值为Fi，j，那么Fi，j＝YjXe-XjYe，向+X方向插补一步Fi+1，j＝Fi，j-Ye，向+Y方向插补一步Fi，j+1＝Fi，j+Xe，向+X、+Y方向插补一步Fi+1，j+1＝Fi，j+Xe-Ye，比较Fi+1，j，Fi，j+1，Fi+1，j+1的绝对值，取最小的作为下一个插补点，在一个插补周期开始，原点(0，0)，向+X、+Y方向插补一步F1，1＝0+delt-delt＝0，那么点(delt，delt)作为下一个插补点，依次类推(2*delt，2*delt)，(3*delt，3*delt)，(4*delt，4*delt)，(5*delt，5*delt)作为下一个插补点，最后到达终点(6*delt，6*delt)，从原点到终点的轨迹向两个坐标轴X、Y投影，投影大小都为6*delt，从而实现位置同步；(2b)速度同步的实现在第一个插补周期T起始点(0，0)，终点(delt，delt)，X轴速度Vx＝delt/T，Y轴速度Vy＝delt/T，故X轴与Y轴速度相同，在第二个插补周期T起点(delt，delt)，终点(2*delt，2*delt)，X轴速度Vx＝delt/T，Y轴速度Vy＝delt/T，故X轴与Y轴速度相同，依次类推，在每个插补周期内，X轴与Y轴速度相等，从而实现速度同步。
2.根据权利要求1所述的两平行轴位置速度同步控制的算法，其特征 在于所述量具为游标卡尺，其精度士0.01mm，同步控制精度为士0.01mm。
全文摘要
本发明提供一种两平行轴位置速度同步控制的算法，利用量具标定两平行轴的初始同步位置，并把对应的两个初始点作为两个轴的零点，采用直接搜索法，将两平行轴中的一根看作X轴，另外一根看作Y轴，在XOY平面插补斜率为1的直线，直线在两轴上的投影相等，实现两轴的位置同步，按直接搜索法，在同一个插补周期内两轴的位移相等，实现两轴速度同步。本发明技术方案实现大跨距两平行轴采用两伺服电机单独驱动在速度和位置上同步运行，不设置主动被动轴，充分达到位置同步和速度同步，可在普通运动卡上成功获得实现。
文档编号G05B19/41GK101470435SQ20071019227
公开日2009年7月1日 申请日期2007年12月24日 优先权日2007年12月24日
发明者倪胜一, 刘相权, 张银花, 伟 王, 超 贠, 赵雪峰, 马永波 申请人:苏州艾隆科技有限公司