[发明专利]一种轨迹跟踪控制方法及轨迹跟踪系统

摘要

本发明公开了四种轨迹跟踪控制方法及各自对应的轨迹跟踪系统，四种轨迹跟踪控制方法的基本思想都是一致的，都是先把传感器得到的以变动的传感器坐标系描述的轨迹信息固定化，即在检测后转化为适合整体记录轨迹信息的坐标系描述的形式进行标记，逐步呈现真实的轨迹，为后续求解目标提供基础，最后根据目标找到当前时刻工具应该呈现的理想状态或找到完成本次修正后在下一次修正时刻到来时要达到的状态，然后根据上述信息驱动执行器，修正工具的轨迹。上述四种轨迹跟踪控制方法以及各自对应的轨迹跟踪系统，克服了检测点超前带来的误差，能脱离示教信息独立运行，既能纠正工具的位置偏差，又能纠正工具的姿态偏差，轨迹跟踪效果好。

主权项

1.一种轨迹跟踪控制方法，其特征在于：工具附着于工具支链上且工具由工具支链驱动，传感器附着于传感器支链上且传感器由传感器支链驱动，传感器位于工具前进方向的前侧，且传感器与工具之间的相对位置具有确定的几何关系，具体包括以下步骤：/n（1）检测轨迹：通过传感器采集工件上的轨迹上检测点的原始轨迹信息；/n（2）提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息，并以传感器坐标系表达该位置信息；/n（3）标记轨迹信息：将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{Q}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；/n（4）确定目标：根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置、以及工具在下一时刻的目标位置；/n根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置的具体确定方式如下：/n设传感器在当前时刻的检测点位置为Q点，Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’，过Q点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点Ref-Q，以参考点Ref-Q为传感器支链的跟踪目标，将参考点Ref-Q的位置作为传感器在下一时刻的检测点位置；/n根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置的具体确定方式如下：/n设工具在当前时刻的目标位置为P点，P点的工具坐标系为P—X-tool’ Y-tool ’Z-tool’，过P点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点Ref-A，以参考点Ref-A为工具支链的跟踪目标，将参考点Ref-A的位置作为工具在下一时刻的目标位置；/n（5）分别驱动传感器支链和工具支链：/n驱动传感器枝链：根据参考点Ref-Q的位置信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点，并以Ref-Q为起点沿着Yq-tool’轴的正方向向前移动一个步距△q=Vq*tq，其中tq为传感器每次位置修正控制的调整间隔时间，Vq为传感器的前进速度；/n驱动工具支链：根据参考点Ref-A的位置信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点，并以Ref-A为起点沿着Y-tool’轴的正方向向前移动一个步距△=V*t，其中t为工具每次位置修正控制的调整间隔时间，V为工具的前进速度；/n（6）循环上述各步骤，直至完成整条轨迹的跟踪。/n