轨迹跟踪控制方法及轨迹跟踪系统与流程

本发明涉及轨迹自动跟踪技术领域，尤其涉及轨迹跟踪控制方法以及实现该轨迹跟踪控制方法的轨迹跟踪系统。

背景技术：

焊接、涂胶、切割等作业是一类相似的常规操作，其共同特点都是沿着特定的轨迹进行操作，如果该特定的轨迹位置能利用传感器检测到，就可以用机器人等执行器对轨迹进行跟踪，从而实现自动化作业。

现有的轨迹跟踪系统中的先进者采用传感器来跟踪轨迹，例如弧焊机器人系统采用结构光视觉传感器来跟踪轨迹。轨迹跟踪系统中的执行器自身有一个通过示教信息或规划等手段预定的任务轨迹，带工具的执行器能根据该预定的任务轨迹生成驱动信息，使工具沿着预定的任务轨迹运行，而轨迹跟踪系统仅能简单地把传感器采集、提取的轨迹偏差信息直接反馈到执行器中，对预定的任务轨迹进行修正。由于传感器一般设置在执行器的前端，所以在轨迹跟踪过程中传感器检测位置与工具位置不一致，而现有的用于轨迹跟踪的传感器，仅能向外部提供其检测到的相对于其自身的轨迹偏差信息，该轨迹控制系统中采用的轨迹跟踪控制方法忽略了传感器检测位置与工具位置不一致的问题，粗略地将传感器检测到的位置偏差直接用于对工具位置的纠偏。这种方法是基于传感器检测到的轨迹偏差信息，无偏不纠，那么即便工具当前位置没有偏差，按照预定的任务轨迹前进后，下一时刻工具位置也会产生偏差，所以理论上无法实现全程无偏差的跟踪，只能把偏差减小到不大于每前进一步造成的新偏差，因而只有在轨迹的曲率半径较大或者轨迹的波动较小的情况下才能较好地实现轨迹跟踪目的，但是在轨迹的曲率半径较小或者轨迹的波动较大的情况下进行轨迹跟踪就会产生较大的轨迹跟踪误差，甚至无法完成轨迹跟踪目的。目前组建跟踪系统繁琐，而且对于轨迹，尤其是空间曲线轨迹，尚无通用的轨迹跟踪控制方法及实现轨迹跟踪控制方法的轨迹跟踪系统。

技术实现要素：

焊接、涂胶、切割等作业是一类相似的常规操作，其共同特点都是沿着特定的轨迹进行操作，轨迹是一条任意弯曲的、有“姿态”的线段，是工具在理想状态下工作时跟踪的目标，轨迹所在工件表面在检测点处“朝向哪个方向”的特性，这里将感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性称为轨迹的“姿态”，上述特定的轨迹都可以抽象成一条直线段或曲线段，这条直线段或曲线段就是轨迹跟踪系统所要跟踪的轨迹，这里将其称为轨迹特征线。

本发明所需解决的技术问题是：提供一种通用的、能脱离示教信息独立运行、能实现自动跟踪且跟踪效果好的轨迹跟踪控制方法及轨迹跟踪系统。

为解决上述问题，本发明所述的第一种轨迹跟踪方法是利用传感器事先检测轨迹的特点，采用坐标变换的方法，把传感器检测的轨迹原始位置信息标记到工件坐标系或世界坐标系中，形成一个由多个检测点构成的轨迹位置信息集合{q}，然后根据轨迹位置信息集合{q}确定下一时刻工具应该到达的正确位置及姿态，该方法克服了检测点超前带来的误差。

本发明所述的第一种轨迹跟踪控制方法，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系，具体包括以下步骤：

（1）检测轨迹：通过传感器采集工件上的轨迹上检测点的原始轨迹信息；

（2）提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息，并以传感器坐标系表达该位置信息；

（3）标记轨迹信息：将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；

（4）确定目标：根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置，具体如下：设工具在当前时刻的目标位置为p点，p点的工具坐标系为p—x-tool’y-tool’z-tool’，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，将参考点ref-a的位置作为工具在下一时刻的目标位置；

（5）驱动工具：执行器根据参考点ref-a的位置信息、使工具的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着y-tool’轴的正方向向前移动一个步距△=v*t，其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度；

（6）循环上述各步骤，直至完成整条轨迹的跟踪。

进一步地，前述的第一种轨迹跟踪控制方法，其中，步骤（4）中根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态，具体如下：设工具在当前时刻的目标位置为p点，p点的工具坐标系为p—x-tool’y-tool’z-tool’，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p-x-tooly-toolz-tool：过参考点ref-a做轨迹特征线的切线，以过轨迹特征线的切线和过与z-tool’轴平行的直线的平面为轨迹分割面；过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过ref-a点的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态作为工具在下一时刻的目标位置及姿态；此时步骤（5）中执行器根据参考点ref-a的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着y-tool轴的正方向向前移动一个步距△=v*t。

进一步地，前述的第一种轨迹跟踪控制方法，其中，步骤（2）中用一个与z-tool’轴方向相同的向量作为检测点的姿态信息，并以传感器坐标系表达该姿态信息。

此时步骤（3）中将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹姿态信息集合{n}，轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{q，n}。

此时步骤（4）中，根据轨迹位置姿态信息集合{q，n}，确定位于参考点ref-a处的工具坐标系p-x-tooly-toolz-tool：过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过ref-a点的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

此时步骤（5）中执行器根据参考点ref-a的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着y-tool轴的正方向向前移动一个步距△=v*t。

进一步地，前述的第一种轨迹跟踪控制方法，其中，步骤（4）中根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置，具体如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态；此时步骤（5）中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻目标位置。

进一步地，前述的第一种轨迹跟踪控制方法，其中，步骤（3）中将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的转化过程为：先将以传感器坐标系表达的位置信息转化为以工具坐标系表达的位置信息，然后将以工具坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息。

实现第一种轨迹跟踪控制方法的轨迹跟踪系统，包括传感器、工具和执行器，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系；还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块，信息应用处理器中还包括信息存储区；

传感器采集工件上轨迹的原始轨迹信息；

信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达；

正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻的目标位置与姿态信息；

信息应用处理器将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；

信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息以及轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态信息；

逆运动学模块调用信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息，并根据工具在下一时刻的目标位置及姿态信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

本发明所述的第二种轨迹跟踪方法是利用传感器是事先检测轨迹的特点，采用坐标变换的方法，把传感器检测的轨迹原始位置信息标记到工件坐标系或世界坐标系中，形成一个由多个检测点构成的轨迹位置信息集合{q}，根据轨迹位置信息集合{q}确定下一时刻工具应该到达的正确位置及姿态，然后进一步的求出工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息，根据该位置及姿态偏差信息改变工具当前时刻的目标位置、姿态使其达到下一时刻的正确位置姿态，那么工具每前进一步后，都落在正确的位置姿态上，就实现了理论上的无偏差的跟踪效果，而且该轨迹跟踪方法还克服了检测点超前带来的误差。

本发明所述的第二种轨迹跟踪方法，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系，具体包括以下步骤：

（1）检测轨迹：通过传感器采集工件上的轨迹上检测点的原始轨迹信息；

（2）提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息，并以传感器坐标系表达该位置信息；

（3）标记轨迹信息：将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；

（4）确定目标：根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态，并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x，或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合；其中根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态的具体确定方式如下：设工具在当前时刻的目标位置为p点，p点的工具坐标系为p—x-tool’y-tool’z-tool’，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p—x-tooly-toolz-tool：过参考点ref-a做轨迹特征线的切线，以过轨迹特征线的切线和过与z-tool’轴平行的直线的平面为轨迹分割面；过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过ref-a点的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态；

（5）驱动工具：执行器根据参考点ref-a的位置及姿态偏差信息、使工具的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着预定方向向前移动一个步距△=v*t，其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度；使工具的目标位置由当前时刻的目标位置运动至ref-a点的修正驱动信息为驱动信息d-tool，驱动信息d-tool为至少包含一个有效分量：△x的向量，且驱动信息d-tool为至多包含五个有效分量：△x，△y，△z，δx，δz的向量；其中在驱动信息d-tool不包含δx或δz时，所述预定方向为由于位移调整而自然地被确定的方向；在驱动信息d-tool包含δx或δz时，所述预定方向为y-tool轴的正方向；

（6）循环上述各步骤，直至完成整条轨迹的跟踪。

进一步地，前述的第二种轨迹跟踪控制方法，其中，步骤（4）中位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

δz是：轨迹特征线上位于参考点ref-a处的切线在过x-tool’轴及y-tool’轴的平面内的投影s-xy与y-tool’轴方向的夹角；

δx是：轨迹特征线上位于参考点ref-a处的切线在过y-tool’轴及z-tool’轴的平面内的投影s-yz与y-tool’轴方向的夹角；

△x是：p点到参考点ref-a的向量在x-tool’轴方向的投影长度；

△y是：p点到参考点ref-a的向量在y-tool’轴方向的投影长度；

△z是：p点到参考点ref-a的向量在z-tool’轴方向的投影长度。

进一步地，前述的第二种轨迹跟踪控制方法，其中，步骤（4）中根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定工具位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态；并根据控制参考点ref-a-next的位置及姿态信息求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。此时步骤（5）中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻的目标位置至下一时刻目标位置。

进一步地，前述的第二种轨迹跟踪控制方法，其中，步骤（4）中位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

δz是：轨迹特征线上位于控制参考点ref-a-next处的切线在过x-tool’轴及y-tool’轴确定的平面内的投影s-xy与y-tool’轴方向的夹角；

δx是：轨迹特征线上位于控制参考点ref-a-next处的切线在过y-tool’轴及z-tool’轴确定的平面内的投影s-yz与y-tool’轴方向的夹角；

△x是：p点到控制参考点ref-a-next的向量在x-tool’轴方向的投影长度；

△y是：p点到控制参考点ref-a-next的向量在y-tool’轴方向的投影长度；

△z是：p点到控制参考点ref-a-next的向量在z-tool’轴方向的投影长度。

进一步地，前述的第二种轨迹跟踪控制方法，其中，步骤（5）中的修正驱动信息为驱动信息d-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。

进一步地，前述的第二种轨迹跟踪控制方法，其中，步骤（3）中将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的转化过程为：先将以传感器坐标系表达的位置信息转化为以工具坐标系表达的位置信息，然后将以工具坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息。

实现第二种轨迹跟踪控制方法的轨迹跟踪系统，包括传感器、工具和执行器，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系；还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块，信息应用处理器中还包括信息存储区；

传感器采集工件上轨迹的原始轨迹信息；

信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达；

正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻的目标位置与姿态信息；

信息应用处理器将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；

信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置及姿态信息，并根据工具当前时刻的目标位置与姿态信息以及轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态信息，并求出工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息；

逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息，并根据工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时的关节角信息。

轨迹是一条任意弯曲的、有“姿态”的线段，是工具在理想状态下工作时跟踪的目标，其中“姿态”是指轨迹跟踪时不但工具上的特定点要落在轨迹特征线上，而且工具的轴线要与轨迹特征线呈一定的角度关系。上述第一种轨迹跟踪控制方法和第二种轨迹跟踪控制方法及各自对应的轨迹跟踪系统中涉及的传感器仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，合称为轨迹的位置信息，该传感器无法感知轨迹所在工件表面在检测点处“朝向哪个方向”的特性，这里将感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性称为轨迹的“姿态”。本发明所述的第三种轨迹跟踪方法及对应的轨迹跟踪系统中涉及的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，还能感知轨迹的姿态信息。本发明所述的第三种轨迹跟踪方法是利用传感器是事先检测轨迹的特点，采用坐标变换的方法，把传感器检测的轨迹原始位置及姿态信息标记到工件坐标系或世界坐标系中，形成一个由多个检测点构成的轨迹位置姿态信息集合{q，n}，然后根据轨迹位置姿态信息集合{q，n}确定下一时刻工具应该到达的正确目标位置及姿态，该方法克服了检测点超前带来的误差。

本发明所述的第三种轨迹跟踪方法，传感器能感知轨迹的位置信息和姿态，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系，具体包括以下步骤：

（1）检测轨迹：通过传感器采集工件上的轨迹上检测点的原始轨迹信息；

（2）提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息，并以传感器坐标系表达该位置信息和姿态信息；

（3）标记轨迹信息：将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合｛q,n｝；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；

（4）确定目标：根据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态，具体如下：

设工具在当前时刻的目标位置为p点，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p—x-tooly-toolz-tool：过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过参考点ref-a的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态；

（5）驱动工具：执行器根据参考点ref-a的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着y-tool轴的正方向向前移动一个步距△=v*t，其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度；

（6）循环上述各步骤，直至完成整条轨迹的跟踪。

进一步地，前述的第三种轨迹跟踪控制方法，其中，步骤（4）中根据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态。此时步骤（5）中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻的目标位置。

进一步地，前述的第三种轨迹跟踪控制方法，其中，步骤（3）中将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息的转化过程为：先将以传感器坐标系表达的位置及姿态信息转化为以工具坐标系表达的位置及姿态信息，然后将以工具坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息。

实现第三种轨迹跟踪控制方法的轨迹跟踪系统，包括传感器、工具和执行器，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系；还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块，信息应用处理器中还包括信息存储区；

传感器采集工件上轨迹的原始轨迹信息；

信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达；

正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻的目标位置及姿态信息；

信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合{q，n}；

信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置及姿态信息，并根据工具在当前时刻的目标位置及姿态信息、以及轨迹位置姿态信息集合{q，n}，确定工具在下一时刻的目标位置及姿态；

逆运动学模块调用信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息，并根据工具在下一时刻的目标位置及姿态，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

本发明所述的第四种轨迹跟踪方法及对应的轨迹跟踪系统中涉及的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，还能感知轨迹的姿态信息。本发明所述的第四种轨迹跟踪方法是利用传感器是事先检测轨迹的特点，采用坐标变换的方法，把传感器检测的轨迹原始位置及姿态信息标记到工件坐标系或世界坐标系中，形成一个由多个检测点构成的轨迹位置姿态信息集合{q}，根据轨迹位置姿态信息集合{q}确定下一时刻工具应该到达的正确位置及姿态，然后进一步的求出工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息，根据该位置及姿态偏差信息改变工具当前时刻目标位置、姿态使其达到下一时刻的正确位置姿态，那么工具每前进一步后，都落在正确的位置姿态上，就实现了理论上的无偏差的跟踪效果，而且该轨迹跟踪方法还克服了检测点超前带来的误差。

本发明所述的第四种轨迹跟踪方法，传感器能感知轨迹的位置信息和姿态，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系，具体包括以下步骤：

（1）检测轨迹：通过传感器采集工件上的轨迹上检测点的原始轨迹信息；

（2）提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息，并以传感器坐标系表达该位置信息和姿态信息；

（3）标记轨迹信息：将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合｛q,n｝；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；

（4）确定目标：根据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态，并求出工具在当前时刻目标位置姿态与工具在下一时刻的目标之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息；其中根据据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态的具体确定方式如下：设工具在当前时刻的目标位置为p点，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p—x-tooly-toolz-tool：过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过参考点ref-a的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态；

（5）驱动工具：执行器根据参考点ref-a位置及姿态偏差信息，使工具的目标位置由当前时刻目标位置运动至下一时刻的目标位置；使工具的目标位置由当前时刻目标位置运动至下一时刻的目标位置的修正驱动信息为驱动信息d-tool，驱动信息d-tool为至少包含两个有效分量：△x，δy的向量，且驱动信息d-tool为至多包含六个有效分量：△x，△y，△z，δx，δy，δz的向量；

（6）循环上述各步骤，直至完成整条轨迹的跟踪。

进一步地，前述的第四种轨迹跟踪控制方法，其中，步骤（4）中位置及姿态偏差信息的具体求解过程为：设位于当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

△x是：p点到参考点ref-a的向量在x-tool’轴方向的投影长度；

△y是：p点到参考点ref-a的向量在y-tool’轴方向的投影长度；

△z是：p点到参考点ref-a的向量在z-tool’轴方向的投影长度；

δx是：将位于参考点ref-a处的轨迹特征线直线化或过参考点ref-a作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过y-tool’轴和z-tool’轴的平面上的投影线段s-yz与y-tool’轴的夹角；

δy是：将位于参考点ref-a处的轨迹特征线直线化或过参考点ref-a作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和z-tool’轴的平面上的投影线段s-xz与z-tool’轴的夹角；

δz是：将位于参考点ref-a处的轨迹特征线直线化或过参考点ref-a作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和y-tool’轴的平面上的投影线段s-xy与y-tool’轴的夹角。

进一步地，前述的第四种轨迹跟踪控制方法，其中，步骤（4）中根据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态，并根据控制参考点ref-a-next的位置及姿态信息求出工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息；其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度，位于当前时刻位置的工具经间隔时间t后移动至下一时刻目标位置。此时，步骤（5）中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻的目标位置。

进一步地，前述的第四种轨迹跟踪控制方法，其中，步骤（4）中位置及姿态偏差信息的具体求解过程为：设工具当前时刻的目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

△x是：p点到控制参考点ref-a-next的向量在x-tool’轴方向的投影长度；

△y是：p点到控制参考点ref-a-next的向量在y-tool’轴方向的投影长度；

△z是：p点到控制参考点ref-a-next的向量在z-tool’轴方向的投影长度；

δx是：将位于控制参考点ref-a-next处的轨迹特征线直线化或过控制参考点ref-a-next作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过y-tool’轴和z-tool’轴的剖切面上的投影线段s-yz与y-tool’轴的夹角；

δy是：将位于控制参考点ref-a-next处的轨迹特征线直线化或过控制参考点ref-a-next作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和z-tool’轴的剖切面上的投影线段s-xz与z-tool’轴的夹角；

δz是：将位于控制参考点ref-a-next处的轨迹特征线直线化或过控制参考点ref-a-next作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和y-tool’轴的平面上的投影线段s-xy与y-tool’轴的夹角。

进一步地，前述的第四种轨迹跟踪控制方法，其中，步骤（5）中的修正驱动信息为驱动信息d-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。

进一步地，前述的第四种轨迹跟踪控制方法，其中，步骤（3）中将以传感器坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息的转化过程为：先将以传感器坐标系表达的位置姿态信息转化为以工具坐标系表达的位置姿态信息，然后将以工具坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息。

实现第四种轨迹跟踪控制方法的轨迹跟踪系统，包括传感器、工具和执行器，传感器能感知轨迹的位置信息和姿态，工具附着于执行器上，传感器附着于执行器的关节上或传感器直接附着于工具上，且传感器与工具之间的相对位置具有确定的几何关系；还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块，信息应用处理器中还包括信息存储区；

传感器采集工件轨迹上的原始轨迹信息；

信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达；

正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻的目标位置及姿态信息；

信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合{q，n}；

信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置及姿态信息，并根据工具当前时刻的目标位置与姿态信息，以及轨迹位置姿态信息集合{q，n}，确定工具在下一时刻的目标位置及姿态，并求出工具在当前时刻位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息；

逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置姿态偏差信息，并根据工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

实现第一种轨迹跟踪控制方法的轨迹跟踪系统、实现第二种轨迹跟踪控制方法的轨迹跟踪系统、实现第三种轨迹跟踪控制方法的轨迹跟踪系统和实现第四种轨迹跟踪控制方法的轨迹跟踪系统，均可以进一步划分为传感系统和执行系统两部分，具体划分如下：

第一种划分方式：传感器和信息检测处理器构成传感系统，在传感系统中设置有第一对外通信器和第一信息接口端；信息应用处理器、正运动学模块、逆运动学模块和执行器构成执行系统，在执行系统中设置有第二对外通信器和第二信息接口端，传感系统和执行系统通过第一信息接口端和第二信息接口端电连接。在实际使用过程中，传感器和信息检测处理器可以做成一个整体，也可以将信息检测处理器做成单独的一个后端信息处理器。工作时传感系统向执行系统下传以传感器坐标系表达的轨迹的位置信息。

第二种划分方式：传感器、信息检测处理器和信息应用处理器构成传感系统，在传感系统中设置有第一对外通信器和第一信息接口端；正运动学模块、逆运动学模块和执行器构成执行系统，在执行系统中设置有第二对外通信器和第二信息接口端，传感系统和执行系统通过第一信息接口端和第二信息接口端电连接。在实际使用过程中，传感器、信息检测处理器和信息应用处理器可以做成一个整体，也可以将信息检测处理器和信息应用处理器做成单独的一个后端信息处理器。工作时，传感系统接收执行系统上传的工具当前时刻的目标位置与姿态信息；传感系统向执行系统下传工具下一时刻的目标位置及姿态信息。

第三种划分方式：传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统，在传感系统中设置有第一对外通信器和第一信息接口端；逆运动学模块和执行器构成执行系统，在执行系统中设置有第二对外通信器和第二信息接口端，传感系统和执行系统通过第一信息接口端和第二信息接口端电连接。在实际使用过程中，传感器、信息检测处理器、信息应用处理器和正运动学模块可以做成一个整体，也可以将信息检测处理器、信息应用处理器和正运动学模块做成单独的一个后端信息处理器。工作时，传感系统接收执行系统上传的工具的目标位置在当前时刻目标位置时执行器的关节角信息；传感系统向执行系统下传工具下一时刻的目标位置及姿态信息。

第四种划分方式：传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统，在传感系统中设置有第一对外通信器和第一信息接口端；执行器构成执行系统，在执行系统中设置有第二对外通信器和第二信息接口端，传感系统和执行系统通过第一信息接口端和第二信息接口端电连接。在实际使用过程中，传感器、信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块可以做成一个整体，也可以将信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块做成单独的一个后端信息处理器。工作时，传感系统接收执行系统上传的工具的目标位置在当前时刻目标位置时执行器的关节角信息；传感系统向执行系统下传工具下一时刻到达目标位置及姿态时、执行器的关节角信息。

本发明的有益效果是：上述四种轨迹跟踪控制方法以及各自对应的轨迹跟踪系统，克服了检测点超前带来的误差，能脱离示教信息独立运行，在轨迹跟踪过程中能充分利用传感器检测的信息进行轨迹跟踪，不仅可以纠正工具的位置偏差，还可以纠正工具的姿态偏差，从而更好地实现轨迹跟踪目的。此外把轨迹跟踪系统进一步划分成传感系统和执行系统两部分，可以使跟踪系统的建立更加便利、灵活，而且便于由不同的专业厂家制造，提高性能、降低成本。

附图说明

图1是本发明所述的轨迹跟踪控制方法及轨迹跟踪系统中传感器与工具的结构示意图。

图2是本发明所述的轨迹跟踪系统中传感系统和执行系统的第一种划分方式信息关系框图。

图3是本发明所述的轨迹跟踪系统中传感系统和执行系统的第二种划分方式信息关系框图。

图4是本发明所述的轨迹跟踪系统中传感系统和执行系统的第三种划分方式信息关系框图。

图5是本发明所述的轨迹跟踪系统中传感系统和执行系统的第四种划分方式信息关系框图。

图6是本发明实施例三中确定参考点位置的示意图。

图7是本发明实施例三中另一种确定参考点位置的示意图。

图8是本发明实施例三中参考点的俯仰角度偏差δx的示意图。

图9是本发明实施例三中参考点的前进方向角度偏差δy的示意图。

图10是本发明实施例三中驱动信息、预定的任务轨迹信息、干预信息结合的综合驱动信息驱动执行系统的综合信息关系框图。

图11是本发明实施例四中控制参考点的偏差示意图。

图12是本发明实施例五中原始轨迹信息的表达示意图。

图13是本发明实施例五中轨迹信息的全面表达示意图。

图14是传感系统能力不足的第一种示意图。

图15是传感系统能力不足的第二种示意图。

图16是传感器与工具不在执行器的同一关节上的示意图。

图17是欠自由度执行器的第一种示意图。

图18是欠自由度执行器的第二种示意图。

图19是欠自由度执行器的第三种示意图。

具体实施方式

下面结合附图及优选实施例对本发明所述的技术方案作进一步详细的说明。

实施例一

本实施例所述的轨迹跟踪控制方法，工具2附着于执行器上，传感器3附着于执行器的关节上或传感器3直接附着于工具2上，且传感器3与工具2之间的相对位置具有确定的几何关系，具体包括以下步骤：

第一步：检测轨迹：通过传感器3采集工件1上的轨迹上检测点的原始轨迹信息，这里涉及的传感器3能将检测到的视觉信息转换成原始轨迹信息，目前这类传感器一般为视觉传感器，本发明以典型的结构光视觉传感器为例进行说明。

第二步：提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息，并以传感器坐标系表达该位置信息。

为方便描述，这里以机器人跟踪v形坡口焊缝、传感器3设置于焊枪前端为例进行说明，这里执行器为机器人，工具2为焊枪，轨迹为焊缝4，轨迹特征线为焊缝特征线，具体结构如图1所示。本发明所述的轨迹跟踪控制方法及对应轨迹跟踪系统的应用不限于焊接领域，只要是引导工具2沿着连续的几何轨迹运动的任务都可以适用，例如用于以特定几何边际为依据的材料切割、涂胶、打磨边角等任务。如图1所示，传感器3检测到的焊缝横截面信息集中在平面m-sense内，这里将平面m-sense定义为检测截面，将坐标系o-xyz定义为传感器坐标系。在焊缝4的截面上有一个点可以代表焊缝位置，例如角接焊缝截面上两个侧边的交点、v形坡口焊缝中v字最低处的交点等。我们将这个在检测截面上代表焊缝位置的点简称为检测点，在图1中标记为q点，在焊接时，传感器3在焊缝4上方扫过，得到一系列的检测点：q1、q2、q3……qi。把代表焊枪位置的点定义为焊枪特征点p，焊枪特征点p一般在焊枪的轴线上、焊嘴之外的位置，离开焊嘴的距离等于电极伸出长度加上电弧长度。机器人在传感器3的一系列检测点信息的引导下，带着焊枪运动，使焊枪特征点p沿着实际的焊缝4进行焊接，焊枪特征点p的位置姿态矩阵用tt表示。任何焊缝都可以抽象成一条直线段或曲线段，就是焊接中焊枪特征点p要跟踪的轨迹，这里称其为焊缝特征线。

如图1所示，对焊枪在当前位置时的坐标系的确定如下：将焊枪在焊接过程中前进的方向定义为y-tool’轴的正方向，另外两个坐标轴可根据需要实际加工过程定义，为方便描述，这里以惯用的方式定义另外两个坐标轴：x-tool’轴垂直y-tool’轴与焊枪轴线构成的平面，根据x-tool’轴、y-tool’轴就确定了z-tool’轴，z-tool’轴的正方向为离开焊枪、指向工件1的方向，最后再按照右手定则决定x-tool’轴的正方向。在y-tool’轴正方向上，传感器3在焊枪前方，焊枪的坐标系就是工具2位于当前时刻的坐标系p—x-tool’y-tool’z-tool’，传感器3的坐标系与工具坐标系之间的位置姿态变换关系矩阵t-sensor是已知的，因而对于具体位置姿态变换关系矩阵t-sensor，这里不再赘述。如果传感器3附着在执行器的其他关节上，只要知道这一变换关系t-sensor，仍然不影响后续计算。

第三步：标记轨迹信息：将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。

机器人工作于世界坐标系，在世界坐标系中，焊枪特征点p的位置及姿态可以用其位置姿态矩阵tt表示，是对焊枪2的全面描述。除了以矩阵形式表达以外，显然，位置姿态也可以用其他形式表述，例如以欧拉角，或者四元素数等形式表达。如果工件1不是附着在世界坐标系中，而是附着在其他坐标系中，那么轨迹也应该以所对应的工件坐标系表述，易通过机器人学知识实现，不再赘述。

将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的转化过程为：

第一、将以传感器坐标系表达的位置信息转化为以工具坐标系表达的位置信息，采用如下公式：q-t=（t-sensor）x（q-s），其中向量q-t表示检测点q点在工具坐标系的描述，q-s表示检测点q点在传感器坐标系下的描述，t-sensor为传感器坐标系与工具坐标系之间的位置姿态变换关系矩阵；

第二、将以工具坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息，采用如下公式：q-w=（tt）xq-t=（tt）x（t-sensor）x（q-s），其中向量q-w表示检测点q点在世界坐标系或工件坐标系的描述，tt表示焊枪特征点p点的位置姿态矩阵。

显然，上述四个式子中，点的坐标应该为齐次坐标，而非三维坐标，仅为了简洁未另行采用其它符号表达。随着焊枪2带着传感器3前进，就得到了用一系列的q[i]点位置完整地描述的焊缝4，将这一系列的q[i]点依次相连的线，就可以代表实际检测到的焊缝特征线，把这组对当前q点的描述信息添加到轨迹位置信息存储区中，成为一个集合，记为{q}，为后续步骤备用。

第四步：确定目标：根据轨迹位置信息集合{q}来确定工具在下一时刻的目标位置及姿态，具体如下：设工具2在当前时刻的目标位置为p点，p点的工具坐标系为p—x-tool’y-tool’z-tool’，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，将参考点ref-a的位置作为工具在下一时刻的目标位置。

第五步：驱动工具：执行器根据参考点ref-a的位置信息、使工具2的目标位置由当前时刻目标位置运动至ref-a点，并以ref-a为起点沿着y-tool’轴的正方向向前移动一个步距△=v*t，其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度。

第六步：循环上述各步骤，直至完成整条轨迹的跟踪。

其中，上述第四步根据轨迹位置信息集合{q}来确定工具2在下一时刻的目标位置及姿态可以具体展开，第一种具体展开如下：设工具2在当前时刻的目标位置为p点，p点的工具坐标系为p—x-tool’y-tool’z-tool’，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p-x-tooly-toolz-tool：过参考点ref-a做轨迹特征线的切线，以过轨迹特征线的切线和过与z-tool’轴平行的直线的平面为轨迹分割面；过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过ref-a点的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态作为工具在下一时刻的目标位置及姿态。此时第五步中执行器根据参考点ref-a的位置及姿态信息、使工具2的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着y-tool轴的正方向向前移动一个步距△=v*t。

上述第一种具体展开中由于检测环节没有姿态信息n，就不能有轨迹分割面，就不能调整姿态，在第一种具体展开中利用当前工具z-tool’轴的姿态信息补出缺失的n，从而就有轨迹分割面，就能调整姿态了。

理想状态下，焊枪轴线与焊缝4之间的位置姿态关系是由焊接工艺确定的，那么焊接该焊缝4时，焊枪的轴线就是确定的。考察经过检测点q的一个微小焊缝段，就可以把该微小焊缝段近似地用经过q点的一小段直线代替，那么定义：经过检测点q点的理想状态下的焊枪轴线、而且经过代表焊缝的直线的平面m-seam为焊缝分割面。以焊接为例时，第一种具体展开中提及的轨迹分割面就是焊缝分割面。

因而除了采用上述第一种展开方式中利用当前工具z-tool’轴的姿态信息补出缺失的n的方式来得到轨迹分割面外，还可以在第二步中采用一个与z-tool’轴方向相同的向量作为检测点的姿态信息，并以传感器坐标系表达该姿态信息。然后通过第三步将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹姿态信息集合{n}，轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{q，n}。

此时，第四步中根据轨迹位置姿态信息集合{q，n}，确定位于参考点ref-a处的工具坐标系p-x-tooly-toolz-tool：过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过ref-a点的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具2前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态。此时第五步执行器根据参考点ref-a的位置及姿态信息、使工具2的目标位置由当前时刻目标位置运动至ref-a点，并以ref-a为起点沿着y-tool轴的正方向向前移动一个步距△=v*t。

为方便论述，这里以传感器3生成新信息的节拍与执行器的控制节拍相同为例进行说明，实际使用中传感器3生成新信息的节拍很可能要比执行器的控制节拍慢。因为确定修正轨迹的信息并不需要最新的检测点信息，所以在没有新的监测信息生成时仅需直接根据轨迹位置信息集合{q}确定下一时刻的目标。

以上各步骤的分析中均以焊枪轴线垂直于轨迹为例，仅仅是为了简洁。如果实际焊接工艺要求以某一个非90°的角度工作，通过矩阵变换不难把工具调整到该角度，从而使其适用该轨迹跟踪控制方法。

以上描述中，没有涉及跟踪的最初寻找轨迹起点的时段。这时还没有积累任何轨迹信息，需要通过其他措施预先把工具2引导到实际轨迹的起点附近，然后发起沿着y-tool方向前进的搜索轨迹起点运动，要保证行进中传感器能够感知到轨迹的起点，在积累了至少两个检测点的轨迹信息后（获得一小段轨迹信息），才真正开始上述跟踪过程。

以上方法仅仅是以v形坡口的焊缝为例叙述，但是不限于此。对于角接焊缝、搭接焊缝、对接焊缝等，只要定义出检测截面上的焊缝特征点p均可应用。对于涂胶等任务，也完全可以参考上述轨迹跟踪控制方法。

上述描述是以工件1位置固定为前提进行阐述的，当工件1位置不固定时，可将传感器3的坐标系和工具2的坐标系都转换为以工件坐标系为参考基准的形式描述，此时就相当于工件位置固定的形式，因而工件位置固定和工件位置不固定都能采用本申请所述的轨迹跟踪控制方法。

实现轨迹跟踪控制方法的轨迹跟踪系统，包括传感器3、工具2和执行器，工具2附着于执行器上，执行器带动工具2运动，最终是由工具2的目标位置跟踪轨迹。传感器3附着于执行器的关节上或传感器3直接附着于工具2上，且传感器3与工具2之间的相对位置具有确定的几何关系，还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012，信息应用处理器1012中还包括信息存储区；

传感器3采集工件1上轨迹的原始轨迹信息；

信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达该位置信息；

正运动学模块2011获取工具2的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具2在当前时刻的目标位置与姿态信息；

信息应用处理器1012将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；

信息应用处理器1012调用正运动学模块中工具2当前时刻的目标位置与姿态信息，并根据工具2位于当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{q}，确定工具2在下一时刻的目标位置及姿态信息；

逆运动学模块2012调用信息应用处理器中工具2在下一时刻的目标位置及姿态信息，并根据工具2在下一时刻的目标位置及姿态信息，计算出工具2的目标位置位于下一时刻目标位置时执行器的关节角信息。

轨迹跟踪系统可进一步划分为传感系统5和执行系统6两部分，将传感系统5与执行系统6的功能明确分开，两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方便描述，这里将传感系统5向执行系统6发送信息定义为“下传”，将执行系统6向传感系统5发送信息定义为“上传”，轨迹跟踪系统中传感系统5和执行系统6的具体划分如下：

如图2所示，第一种划分方式以传感系统提供信息的工作模式划分：传感器3和信息检测处理器1011构成传感系统5，在传感系统5中设置有第一对外通信器和第一信息接口端；信息应用处理器1012、正运动学模块2011、逆运动学模块2012和执行器构成执行系统6，在执行系统6中设置有第二对外通信器和第二信息接口端，传感系统5和执行系6统通过第一信息接口端和第二信息接口端电连接。

工作时，传感系统5不需要上传信息，即执行系统5不需向传感系统5上传任何信息，仅需传感系统5向执行系统6下传以传感器坐标系表达的轨迹的位置信息即可。传感系统5主要承担轨迹位置信息采集、提取任务，执行系统6承担后续的信息处理工作。工作时，传感系统5通过第一对外通信器、第一信息接口端向执行系统6传送以传感器坐标系表达的轨迹位置信息，执行系统6通过第二信息接口端、第二对外通信器接收传感系统5传送的以传感器坐标系表达的轨迹位置信息并进行处理，处理后得到工具位于下一时刻的目标位置及姿态信息，执行器根据上述信息驱动工具2，使工具2的目标位置由当前目标位置运动至下一目标位置。

该种对轨迹跟踪系统的划分方式的优点是：传感系统5结构简单，可以进一步缩小传感系统的体积。

如图3所示，第二种划分方式以传感系统直接驱动执行系统的驱动模式划分：传感器3、信息检测处理器1011和信息应用处理器1012构成传感系统5，在传感系统5中设置有第一对外通信器和第一信息接口端；正运动学模块2011、逆运动学模块2012和执行器构成执行系统6，在执行系统6中设置有第二对外通信器和第二信息接口端，传感系统5和执行系统6通过第一信息接口端和第二信息接口端电连接。

工作时，执行系统6通过第二对外通信器、第二信息接口端向传感系统5传送工具工具当前时刻的位置与姿态信息，传感系统5通过第一信息接口端、第一对外通信器接收执行系统6传送的工具当前时刻的目标位置与姿态信息并进行处理，得到工具位于下一时刻的目标位置及姿态信息，然后传感系统5将工具位于下一时刻的目标位置及姿态信息反馈给执行系统6，执行器驱动工具2，使工具2的目标位置由当前时刻目标位置运动至下一时刻目标位置。

如图4所示，第三种划分方式以传感系统和执行系统约定模型的驱动模式划分：传感器3、信息检测处理器1011、信息应用处理器1012、正运动学模块2011构成传感系统5，在传感系统5中设置有第一对外通信器和第一信息接口端；逆运动学模块2012和执行器构成执行系统6，在执行系统6中设置有第二对外通信器和第二信息接口端，传感系统5和执行系统6通过第一信息接口端和第二信息接口端电连接。

工作时，执行系统6通过第二对外通信器、第二信息接口端向传感系统5传送工具2的目标位置在当前时刻目标位置时执行器的关节角信息，传感系统5通过第一对外通信器、第一信息接口端接收执行系统6上传的工具2的目标位置在当前时刻目标位置时执行器的关节角信息，由于上传的信息本身不足以直接求解工具2位于下一时刻的目标位置及姿态，因而需要事先约定执行器的运动学模型，对传感系统5、执行系统6设置相互协调配合的参数。传感系统5根据上传的执行器的关节角信息以及执行器的运动学模型求解工具2位于下一时刻的目标位置及姿态信息，并反馈给执行系统6，执行器驱动工具2，使工具2的目标位置由当前时刻目标位置运动至下一时刻目标位置。

如图5所示，第四种划分方式以传感系统和执行系统约定模型且传感系统以关节角控制执行系统的驱动模式划分：传感器3、信息检测处理器1011、信息应用处理器1012、正运动学模块2011、逆运动学模块2012构成传感系统5，在传感系统5中设置有第一对外通信器和第一信息接口端；执行器构成执行系统6，在执行系统6中设置有第二对外通信器和第二信息接口端，传感系统5和执行系统6通过第一信息接口端和第二信息接口端电连接。

工作时，执行系统6通过第二对外通信器、第二信息接口端向传感系统5传送工具2的目标位置在当前时刻目标位置时执行器的关节角信息，传感系统5通过第一对外通信器、第一信息接口端接收执行系统6上传的工具2的目标位置在当前时刻目标位置时执行器的关节角信息，由于上传的信息本身不足以直接求解工具2位于下一时刻目标位置时执行器的关节角信息，因而需要事先约定执行器的运动学模型，对传感系统5、执行系统6设置相互协调配合的参数。传感系统5根据上传执行器的关节角信息以及执行器的运动学模型求解工具位于下一时刻目标位置时执行器的关节角信息，并反馈给执行系统6，执行器驱动工具2，使工具2的目标位置由当前时刻目标位置运动至下一时刻目标位置。

第二种划分方式、第三种划分方式和第四种划分方式工作时都需要接收执行系统上传的同一时刻的运动学状态信息备用。该运动学状态信息的获取时机越接近传感器采集时刻越好，其优点是可以更精准地利用上传信息。

上述四种划分方式中涉及的第一信息接口端和第二信息接口端都可以做成信息接口凸端、信息接口凹端的形式，当第一信息接口端为凸端时，第二信息接口端则为凹端；当第一信息接口端为凹端时，第二信息接口端则为凸端。在实际使用过程中可将第一信息接口端和第二信息接口端电连接后的整体称为信息接口。

实施例二

本实施例与实施例一的不同之处在于：本实施例是在参考点ref-a的基础上，不以上述当前的控制参考点ref-a为跟踪目标，而改为以控制间隔时间t后的控制参考点ref-a-next为跟踪目标。本实施例中根据轨迹位置信息集合｛q｝来确定工具2在下一时刻的目标位置的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

第五步中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具2的目标位置由当前时刻目标位置运动至下一时刻目标位置。

轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例一相同，不再赘述。

不以上述当前的控制参考点ref-a为跟踪目标，而改为以控制间隔时间t后的控制参考点ref-a-next为跟踪目标，这样做的优点是使用预测的下一个控制调整时刻将会出现的偏差进行控制，跟踪性能更好。理论上，所有被控制引导达到的位置姿态都是理想的，不会产生偏差，亦即p[i+1]点与ref-a-next点重合，所说的“偏差”实际上是指当前时刻状态与下一时刻状态的差距。这时真正的误差仅在于轨迹信息检测中产生的误差，以及执行系统控制不到位产生的伺服控制误差。

实施例三

本实施例与实施例一的不同之处在于：本实施例在第四步确定目标中，在根据轨迹位置信息集合{q}来确定工具2在下一时刻的目标位置及姿态的基础上，还进一步求出工具2在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息，关于该轨迹跟踪控制方法其他步骤的具体展开均与实施例一相同，这里不再赘述。具体轨迹跟踪控制方法包括以下步骤：

第一步：检测轨迹：通过传感器3采集工件上的轨迹检测点的原始轨迹信息。

第二步：提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息，并以传感器坐标系表达该位置信息。

第三步：标记轨迹信息：将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。

第四步：确定目标：根据轨迹位置信息集合{q}来确定工具2在下一时刻的目标位置及姿态，并求出工具2在当前时刻目标位置与工具2在下一时刻的目标位置之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。

其中根据轨迹位置信息集合{q}来确定工具2在下一时刻的目标位置及姿态的具体确定方式如下：设工具在当前时刻的目标位置为p点，p点的工具坐标系为p—x-tool’y-tool’z-tool’，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定工具位于参考点ref-a处的工具坐标系p—x-tooly-toolz-tool：过参考点ref-a做轨迹特征线的切线，以过轨迹特征线的切线和过与z-tool’轴平行的直线的平面为轨迹分割面；过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过ref-a点的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

其中位置及姿态偏差信息的具体求解过程为：设工具当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

△x是：p点到参考点ref-a的向量在x-tool’轴方向的投影长度；

△y是：p点到参考点ref-a的向量在y-tool’轴方向的投影长度；

△z是：p点到参考点ref-a的向量在z-tool’轴方向的投影长度。

除了上述位置偏差之外，焊枪还可能存在姿态角度偏差，以工具坐标系为参考系，对实现机器人轨迹控制的几个参量进行描述。如图6所示，过x-tool’轴、y-tool’轴的平面图，其中线段s-xy为焊缝特征线在该平面上的投影。由于所考察的是焊枪特征点附近很小的尺寸范围，可以把焊缝特征线近似为直线段，所以也可以把s-xy看作直线段。那么需要焊枪绕着z-tool’轴调整的角度偏差为δz，它是s-xy与y-tool’轴的夹角。确定其他偏差量也需要在焊缝特征线上选择一个控制参考点，这样的点可以选择图6中的位置ref-a。由于实际工作中角度δz很小，所以控制参考点也可以选择图7中的位置ref-b，是过p、且过x-tool轴的平面上的参考点，这样做也不会给偏差计算带来较大误差。类似地，在计算其他偏差量时，由于焊缝特征线与每一个坐标轴的夹角都很小，所以控制参考点可以始终选择上述的ref-a点，也可以选择ref-b点，都不会造成大的误差。即：δz是：轨迹特征线上位于参考点ref-a或参考点ref-b处的切线在过x-tool’轴及y-tool’轴的平面内的投影s-xy与y-tool’轴方向的夹角。

如图8所示，过y-tool’轴、z-tool’坐标轴的剖切面图，其中线段s-yz为焊缝特征线在该剖切面上的投影。那么需要焊枪绕着x-tool’轴调整的角度偏差为δx，即s-yz与y-tool’轴的夹角。即δx是：轨迹特征线上位于参考点ref-a或参考点ref-b处的切线在过y-tool’轴及z-tool’轴的平面内的投影s-yz与y-tool’轴方向的夹角。

如图9所示，过x-tool’轴、z-tool’轴的剖切面图，其中线段s-xz为焊缝特征线在该剖切面上的投影，那么需要焊枪绕着y-tool’轴调整的角度偏差为δy，即s-xz与z-tool’轴的夹角。图9还同时表达了以ref-b为参考点时焊枪的位置调整量△x、△z。

可以把上述偏差量综合地表达为一个六维向量，记为：

d-tool=[△x，△y，△z，δx，δy，δz]，其中△y=0。

这也就是对工具轨迹偏差的完整描述，是在工具坐标系下的表述。由于它可以直接用于驱动执行器，也可称为驱动信息。

因为这里，△y=0，显然执行系统内部要补充出在y-tool坐标轴方向上的驱动信息才能向前推进跟踪，机器人工作时，就以焊接速度v沿着y-tool坐标轴方向运动。这时，就可以认为跟踪是执行外部的驱动命令d-tool与内部的推进驱动命令的综合结果。

所述工具跟踪轨迹的速度信息v，是当前时刻从执行系统接收的、或者是传感系统自行确定的、或者是执行系统设定的、或者是执行系统与传感系统之间约定的，均不影响本发明的核心思想。特殊情况下，执行系统忽略自身的速度调整信息△y，这时v=0，也能带来特殊需要的效果：如果再在y-tool’轴方向上，把外部对机器人运动速度的修正量改为△y=v*t，其中t为每次修正控制的调整间隔时间，则机器人仅靠外部的d-tool驱动就可以实现跟踪。这时机器人就是一个完全靠外部传感系统驱动的设备了。如果以其他手段生成另一个各分量含义与d-tool对应等同的六维向量d-tool-2，则也可以用于驱动执行系统，如图10所述，甚至可以用各种驱动信息的结合信息驱动执行系统。

第五步：驱动工具：执行器根据参考点ref-a的位置及姿态偏差信息、使工具2的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着预定方向向前移动一个步距△=v*t，其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度。使工具2的目标位置由当前时刻的目标位置运动至ref-a点的修正驱动信息为驱动信息d-tool，驱动信息d-tool为至少包含一个有效分量：△x的向量，且驱动信息d-tool为至多包含五个有效分量：△x，△y，△z，δx，δz的向量。其中在驱动信息d-tool不包含δx或δz时，所述预定方向为由于位移调整而自然地被确定的方向，这里是指不理会姿态的调整，仅由△x，△y，△z之间的至少一种产生位置调整，由此位置变化决定了工具的前进方向；在驱动信息d-tool包含δx或δz时，所述预定方向为y-tool轴的正方向。

实现完全自动控制的轨迹跟踪方案中，所谓“完全自动控制”是指可以不依靠预先编写的轨迹描述程序（例如示教程序），而能自动根据轨迹特征自动调整跟踪。这时图10中的“示教轨迹”信息不起作用。即：修正驱动信息可以仅仅是单独的驱动信息d-tool。

在不能确认跟踪过程中无干涉碰撞的环境中，也可以通过简单的示教信息，规避那些可预见的障碍，这时利用的是图10中的“示教轨迹”信息，由该信息决定工具的姿态、或者姿态中的一部分自由度，例如δy，或者δy与δz，可以确保不碰撞。即使是这种情况，示教信息也是非常简化的，而不是常规的决定行进轨迹的复杂示教信息。即修正驱动信息也可以是驱动信息d-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。

因而第五步中的修正驱动信息可以仅仅是驱动信息d-tool，也可以是驱动信息d-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。

如果执行系统接收的是外部传感系统以执行器的工具坐标系为参考系定义的轨迹修正信息，执行系统还可以接收人工操控而形成的、以执行器的工具坐标系为参考系定义的轨迹修正信息，这一信息与接收的外部传感系统提供的偏差控制信息结合，然后以结合后的信息进行轨迹修正；偏差信息结合的方法根据任务不同而确定。图中符号σ并非代表简单地将各个控制向量相加，而是代表信息融合，可以有选择、可以增强或削弱控制量的比例等变化。例如，人采用一个六维力/力矩传感器，生成一个相对于操作者的最多6维的信息：人手推动力/力矩传感器，就发出正的△y信息，拉动就发出负的△y信息，使△y的大小与推拉的力度成正比；把这一信息与d-tool相应的分量相加，或者加权相加，那么就可以实现人对跟踪速度的干预；同理，可以用侧向推拉力/力矩传感器，实现临时使工具偏离跟踪轨迹的调整；扭动力/力矩传感器产生的扭矩则可用于对工具姿态的调整。当然也可以“过滤”掉不希望产生的控制信息，从而抑制不希望变化的信息。为了避免不希望的某一维度上出现干扰，可以将其屏蔽掉，例如忽略不经意间产生的对工具的侧偏调整信息。这就是偏差信息结合的原理。以类似于游戏操纵杆的系统也可以用于引发这样的人为干预信息。

第六步：循环上述各步骤，直至完成整条轨迹的跟踪。

本实施例中实现轨迹跟踪控制方法的轨迹跟踪系统，包括传感器3、工具和执行器，工具2附着于执行器上，传感器3附着于执行器的关节上或传感器3直接附着于工具2上，且传感器3与工具之间的相对位置具有确定的几何关系，还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012，信息应用处理器1012中还包括信息存储区；

传感器3采集工件1上轨迹的原始轨迹信息；

信息检测处理器1011根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达该位置信息；

正运动学模块2011获取工具2的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具2在当前时刻的目标位置与姿态信息；

信息应用处理器1012将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{q}；

信息应用处理器1012调用正运动学模块中工具当前时刻的位置及姿态信息，并根据工具当前时刻的位置与姿态信息、以及轨迹位置信息集合{q}，确定工具在下一时刻的目标位置及姿态信息，并求出工具在当前时刻位置与工具在下一时刻的目标之间的位置及姿态偏差信息；

逆运动学模块2012调用信息应用处理器中工具2在当前时刻目标位置与工具2在下一时刻的目标位置之间的位置及姿态偏差信息，并根据工具2在当前时刻目标位置与工具2在下一时刻的目标位置之间的位置及姿态偏差信息，计算出工具2的目标位置位于下一时刻目标位置时的关节角信息。

轨迹跟踪系统可进一步划分为传感系统5和执行系统6两部分，将传感系统5与执行系统6的功能明确公开，两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方面描述，这里将传感系统5向执行系统6发送信息定义为“下传”，将执行系统6向传感系统5发送信息定义为“上传”，轨迹跟踪系统中传感系统5和执行系统6的具体划分与实施例一中四种划分方式相同，具体工作方式也相同，这里不再赘述。

实施例四

本实施例与实施例三的不同之处在于：本实施例是在参考点ref-a的基础上，不以上述当前的控制参考点ref-a为跟踪目标，而改为以控制间隔时间t后的控制参考点ref-a-next为跟踪目标。本实施例中根据轨迹位置信息集合｛q｝来确定工具在下一时刻的目标位置的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定工具位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态；并根据控制参考点ref-a-next的位置及姿态信息求出工具在当前时刻位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。

其中位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

δz是：轨迹特征线上位于控制参考点ref-a-next处的切线在过x-tool’轴及y-tool’轴确定的平面内的投影s-xy与y-tool’轴方向的夹角；

δx是：轨迹特征线上位于控制参考点ref-a-next处的切线在过y-tool’轴及z-tool’轴确定的平面内的投影s-yz与y-tool’轴方向的夹角；

△x是：p点到控制参考点ref-a-next的向量在x-tool’轴方向的投影长度；

△y是：p点到控制参考点ref-a-next的向量在y-tool’轴方向的投影长度；

△z是：p点到控制参考点ref-a-next的向量在z-tool’轴方向的投影长度。

此时第五步中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。

轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例三相同，不再赘述。

不以上述当前的控制参考点ref-a为跟踪目标，而改为以控制间隔时间t后的控制参考点ref-a-next为跟踪目标，则△x、△y、δz的意义如图11所示。这时，计算其他控制量的参考点也要做相应修改（仅须把ref-a-next当做ref-a点，进行上述计算），这里特意用曲线轨迹表明δz的值与以ref-a为跟踪目标时的不同。图11中，△是ref-a与ref-a-next之间在三维空间上的距离。这样做的优点是，使用预测的下一个控制调整时刻将会出现的偏差进行控制，跟踪性能更好：理论上，所有被控制引导达到的位置姿态都是理想的，不会产生偏差，亦即p[i+1]点与ref-a-next点重合，所说的“偏差”实际上是指当前时刻状态与下一时刻状态的差距。这时真正的误差仅在于轨迹信息检测中产生的误差，以及执行系统控制不到位产生的伺服控制误差。

上述实施例一、实施例二、实施例三和实施例四中的轨迹跟踪方法及各自对应的轨迹跟踪系统中涉及的传感器3只要具有感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差的能力即可，即便该传感器无法感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性，在这里称其为轨迹的“姿态”，也能很好地实现轨迹跟踪。

下述实施例五、实施例六、实施例七和实施例八中的轨迹跟踪方法及各自对应的轨迹跟踪系统中涉及的传感器则需要具有既能感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，也能感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性，在这里称其为轨迹的“姿态”的能力。

实施例五

本实施例与实施例一的不同之处在于：本实施例中的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，合称为轨迹的位置信息，还能感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性，即姿态信息。关于该轨迹跟踪控制方法各步骤的具体展开均与实施例一相同，这里不再赘述。

本实施例中所述的具体轨迹跟踪控制方法包括以下步骤：

第一步：检测轨迹：通过传感器3采集工件1上轨迹上检测点的原始轨迹信息。

第二步：提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息，并以传感器坐标系表达该位置信息和姿态信息。

在焊接时，传感器3在焊缝4上方扫过，得到一系列的检测点q，机器人在传感器的一系列检测点信息的引导下，带着焊枪运动，使焊枪特征点p沿着实际的焊缝进行焊接，焊枪特征点p的位置姿态矩阵用tt表示。

任何焊缝都可以抽象成一条直线段或曲线段，就是焊接中要跟踪的轨迹，这里称其为焊缝特征线。那么，所谓焊缝跟踪就是在焊枪运动过程中，使焊枪特征点p不脱离焊缝特征线，而且使焊枪轴线与焊缝特征线保持焊接工艺需要的特定角度关系。

理想状态下，焊枪轴线与焊缝之间的位置姿态关系是由焊接工艺确定的，那么焊接该焊缝时，焊枪的轴线就是确定的。考察经过检测点q的一个微小焊缝段，就可以把该微小焊缝段近似地用经过q点的一小段直线代替，那么定义：经过检测点q点的理想状态下的焊枪轴线、而且经过代表焊缝的直线的平面m-seam为焊缝分割面。传感器检测到的是在检测截面m-sense内的焊缝的断面轮廓，可以根据该轮廓计算出焊缝分割面m-seam与检测截面m-sense的交线，将该交线表示为矢量n-seam，它可以代表焊缝分割面的方向，称为焊缝姿态向量，或简称焊缝姿态。

实际上，除非检测截面m-sense与焊缝分割面m-seam垂直，否则过q点可以得到多个焊缝姿态向量n-seam。但是这并不影响n-seam的使命：就是为了通过n-seam与一小段轨迹共同确定过q点所决定的焊缝分割面m-seam，所以，只要m-sense与m-seam相交，得到的n-seam，都可以起到同样的作用。

图1是传感器坐标系的坐标轴平行于焊缝、而且无偏差的一种特殊状态，而图12则代表着一般的状态，图中所示为检测截面m-sense上测得的内容，它

是一系列的点或线段描述的焊缝轮廓，信息很丰富。我们要提取最有效、尽量少的信息，来代表焊缝的特性。我们选择q点的位置信息和向量n-seam共同代表焊缝的特性。可见，如果用向量q-s表示q点在传感器坐标系下的描述，则q-s=[xs,0,zs]。

根据几何关系，把点a到点e的向量按逆时针方向旋转90°，就可以用来表达向量n-seam，即为[-dz/l，0，dx/l]。用向量n-s表示n-seam在传感器坐标系下的描述，则n-s=[-dz/l，0，dx/l]。

可见，在需要向执行系统发送以传感器坐标系描述的焊缝信息q-s、n-s时，可以仅传输4个数据，因为它们的坐标表达中有2个0，可以默认地获得。进一步，还可以把n-s归一化为单位向量，那么，还可以通过归一化减少1个传输数据。当然，姿态信息n-s也可以用1个角度δy来表达。

第三步：标记轨迹信息：将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合｛q,n｝；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。

机器人工作于世界坐标系，在世界坐标中，焊枪特征点p的位置及姿态可以用其位置姿态矩阵tt表示，是对焊枪的全面描述。除了以矩阵形式表达以外，显然，位置姿态也可以用其他形式表述，例如以欧拉角，或者四元素数等形式表达。将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息的转化过程为：

第一、将以传感器坐标系表达的位置及姿态信息转化为以工具坐标系表达的位置及姿态信息，采用如下公式：

q-t=（t-sensor）x（q-s）

n-t=（t-sensor）x（n-s）

其中向量q-t表示下一时刻的目标位置在工具坐标系的描述，q-s表示下一时刻的目标位置在传感器坐标系下的描述，t-sensor为传感器坐标系与工具坐标系之间的位置姿态变换关系矩阵；向量n-s表示向量n-seam在传感器坐标系下的描述，向量n-t表示向量n-seam在工具坐标系下的描述；

第二、将以工具坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，采用如下公式：

q-w=（tt）xq-t=（tt）x（t-sensor）x（q-s）

n-w=（tt）xn-t=（tt）x（t-sensor）x（n-s）

其中向量q-w表示下一时刻的目标位置在世界坐标系或工件坐标系的描述，tt表示工具位于当前时刻位置p点的位置姿态矩阵，向量n-w表示向量n-seam在世界坐标系或工件坐标系下的描述。

显然，上述四个式子中，点的坐标应该为齐次坐标，而非三维坐标，仅为了简洁未另行采用其它符号表达。因此，如图13所示，随着焊枪带着传感器前进，就得到了用一系列的q[i]点位置及其附带的向量n-seam（简记为n[i]）完整地描述的焊缝，将这一系列的q点依次相连的线，就可以代表实际检测到的焊缝特征线；由各个n-seam构成的曲面就可以代表实际检测到的焊缝分割面。把这组对当前q点的描述信息添加到轨迹位置姿态信息存储区中，成为一个集合，记为{q,n}，为后续步骤备用。

如果工件不是附着在世界坐标系中，而是附着在其他坐标系中，那么轨迹也应该以所对应的工件坐标系表述，易通过机器人学知识实现，不再赘述。

如图13所示的是在一个扭曲的、非可展开曲面上进行焊接的情况，可以代表对任意空间曲线轨迹的处理方法。为了不掩盖焊缝特征线，图中将这一系列向量n-seam的方向倒置。焊枪在焊接过程中前进的方向定义为工具坐标系的y-tool轴的正方向。理论上另外两个坐标轴可以任意定义，为了叙述简明，这里以惯用的方式做如下假设：x-tool轴垂直于y-tool轴与焊枪轴线构成的平面，而z-tool的正方向，为离开焊枪、指向工件的方向，最后再按照右手定则决定x-tool的正方向。

第四步：确定目标：根据轨迹位置姿态信息集合{q，n}来确定工具在下一时刻的目标位置，具体如下：设工具在当前时刻的目标位置为p点，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p—x-tooly-toolz-tool：过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过参考点ref-a的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；传感器在ref-a点检测到的是在传感器的检测截面m-sense内的轨迹线的断面轮廓，根据该断面轮廓计算出轨迹分割面m-seam与检测截面m-sense的交线，将该交线表示为向量n-seam，向量n-seam为工具位于参考点ref-a的姿态，将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

第五步：驱动工具：执行器根据参考点ref-a的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至ref-a点，并以ref-a为起点沿着y-tool轴的正方向向前移动一个步距△=v*t，其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度。

第六步：循环上述各步骤，直至完成整条轨迹的跟踪。

本实施例中实现轨迹跟踪控制方法的轨迹跟踪系统，包括传感器3、工具2和执行器，工具附着于执行器上，传感器3附着于执行器的关节上或传感器3直接附着于工具上，且传感器3与工具2之间的相对位置具有确定的几何关系，还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012，信息应用处理器1012中还包括信息存储区；

传感器3采集工件上轨迹的原始轨迹信息；

信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达该位置及姿态信息；

正运动学模块2011获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具2在当前时刻的目标位置及姿态信息；

信息应用处理器1012将以传感器坐标系表达的轨迹位置及姿态信息转化为以轨迹所附着的坐标系表达的轨迹位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合{q，n}；

信息应用处理器调用正运动学模块2011中工具2当前时刻的目标位置及姿态信息，并根据工具2在当前时刻的目标位置及姿态信息、以及轨迹位置姿态信息集合{q，n}，确定工具2在下一时刻的目标位置及姿态；

逆运动学模块2012调用信息应用处理器1012中工具2在下一时刻的目标位置及姿态信息，并根据工具2在下一时刻的目标位置及姿态，计算出工具2的目标位置位于下一时刻目标位置时执行器的关节角信息。

轨迹跟踪系统可进一步划分为传感系统5和执行系统6两部分，将传感系统5与执行系统6的功能明确公开，两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方面描述，这里将传感系统5向执行系统6发送信息定义为“下传”，将执行系统6向传感系统5发送信息定义为“上传”，轨迹跟踪系统中传感系统5和执行系统6的具体划分与实施例一中四种划分方式相同，具体工作方式也相同，这里不再赘述。

实施例六

本实施例与实施例五的不同之处在于：本实施例是在参考点ref-a的基础上，不以上述当前的控制参考点ref-a为跟踪目标，而改为以控制间隔时间t后的控制参考点ref-a-next为跟踪目标。本实施例中根据轨迹位置姿态信息集合｛q，n｝来确定工具在下一时刻的目标位置的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；传感器在ref-a-next点检测到的是在传感器的检测截面m-sense内的轨迹线的断面轮廓，根据该断面轮廓计算出轨迹分割面m-seam与检测截面m-sense的交线，将该交线表示为向量n-seam，向量n-seam为工具位于控制参考点ref-a-next的姿态，将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态；其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度，位于当前时刻位置的工具经间隔时间t后移动至下一时刻目标位置。

第五步中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻的目标位置修正运动至下一时刻的目标位置。

轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例五相同，不再赘述。

实施例七

本实施例与实施例五的不同之处在于：本实施例在第四步确定目标中，在根据轨迹位置信息集合{q，n}来确定工具在下一时刻的目标位置及基础上，还要进一步求出工具在当前时刻位置与工具在下一时刻的目标位置之间的位置偏差信息，关于该轨迹跟踪控制方法其他步骤的具体展开均与实施例五相同，这里不再赘述。

关于该轨迹跟踪控制方法的适用性及其他步骤的具体展开均与实施例五相同，这里不再赘述。

本实施例中具体轨迹跟踪控制方法包括以下步骤：

第一步：检测轨迹：通过传感器3采集工件1上的轨迹上检测点的原始轨迹信息；

第二步：提取轨迹信息：从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息，并以传感器坐标系表达该位置信息和姿态信息；

第三步：标记轨迹信息：将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合｛q,n｝；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。

将以传感器坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息的转化过程为：

第一、将以传感器坐标系表达的位置姿态信息转化为以工具坐标系表达的位置姿态信息，采用如下公式：

q-t=（t-sensor）x（q-s）

n-t=（t-sensor）x（n-s）

其中向量q-t表示下一时刻的目标位置在工具坐标系的描述，q-s表示下一时刻的目标位置在传感器坐标系下的描述，t-sensor为传感器坐标系与工具坐标系之间的位置姿态变换关系矩阵；传感器在ref-a点检测到的是在传感器的检测截面m-sense内的轨迹特征线的断面轮廓，可以根据该断面轮廓计算出轨迹分割面m-seam与检测截面m-sense的交线，将该交线表示为向量n-seam；向量n-s表示向量n-seam在传感器坐标系下的描述，向量n-t表示向量n-seam在工具坐标系下的描述；

第二、将以工具坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息，采用如下公式：

q-w=（tt）xq-t=（tt）x（t-sensor）x（q-s）

n-w=（tt）xn-t=（tt）x（t-sensor）x（n-s）

其中向量q-w表示下一时刻的目标位置在世界坐标系或工件坐标系的描述，tt表示工具位于当前时刻位置p点的位置姿态矩阵，向量n-w表示向量n-seam在世界坐标系或工件坐标系下的描述。

第四步：确定目标：根据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态，并求出工具在当前时刻目标位置姿态与工具在下一时刻的目标之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置偏差及姿态信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息；其中根据据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置及姿态的具体确定方式如下：

设工具在当前时刻的目标位置为p点，过p点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点ref-a，以参考点ref-a为跟踪目标，确定位于参考点ref-a处的工具坐标系p—x-tooly-toolz-tool：过参考点ref-a做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool轴，z-tool轴的正方向指向工件表面；以过参考点ref-a的轨迹特征线的切线为y-tool轴，y-tool轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool轴及x-tool轴的正方向；传感器在ref-a点检测到的是在传感器的检测截面m-sense内的轨迹线的断面轮廓，根据该断面轮廓计算出轨迹分割面m-seam与检测截面m-sense的交线，将该交线表示为向量n-seam，向量n-seam为工具位于参考点ref-a的姿态，将参考点ref-a的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

其中位置及姿态偏差信息的具体求解过程为：设位于当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

△x是：p点到参考点ref-a的向量在x-tool’轴方向的投影长度；

△y是：p点到参考点ref-a的向量在y-tool’轴方向的投影长度；

△z是：p点到参考点ref-a的向量在z-tool’轴方向的投影长度；

δx是：将位于参考点ref-a处的轨迹特征线直线化或过参考点ref-a作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过y-tool’轴和z-tool’轴的平面上的投影线段s-yz与y-tool’轴的夹角；

δy是：将位于参考点ref-a处的轨迹特征线直线化或过参考点ref-a作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和z-tool’轴的平面上的投影线段s-xz与z-tool’轴的夹角；

δz是：将位于参考点ref-a处的轨迹特征线直线化或过参考点ref-a作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和y-tool’轴的平面上的投影线段s-xy与y-tool’轴的夹角。

第五步：驱动工具：执行器根据参考点ref-ade位置及姿态偏差信息，使工具的目标位置由当前时刻目标位置运动至下一时刻的目标位置。使工具的目标位置由当前时刻目标位置运动至下一时刻的目标位置的修正驱动信息为驱动信息d-tool，驱动信息d-tool为至少包含两个有效分量：△x，δy的向量，且驱动信息d-tool为至多包含六个有效分量：△x，△y，△z，δx，δy，δz的向量。

第六步：循环上述各步骤，直至完成整条轨迹的跟踪。

实现轨迹跟踪控制方法的轨迹跟踪系统，包括传感器3、工具和执行器，工具附着于执行器上，传感器3附着于执行器的关节上或传感器3直接附着于工具上，且传感器3与工具之间的相对位置具有确定的几何关系，还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012，信息应用处理器1012中还包括信息存储区；

传感器3采集工件1上轨迹的原始轨迹信息；

信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达该位置及姿态信息；

正运动学模块2011获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具2在当前时刻的目标位置及姿态信息；

信息应用处理器1012将以传感器坐标系表达的轨迹位置及姿态信息转化为以轨迹所附着的坐标系表达的轨迹位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置姿态信息集合{q，n}；

信息应用处理器1012调用正运动学模块2011中计算出的工具2当前时刻的目标位置及姿态信息，并根据工具2当前时刻的目标位置与姿态信息、以及轨迹位置姿态信息集合{q，n}，确定工具2在下一时刻的目标位置及姿态信息，并求出工具2在当前时刻目标位置与工具2在下一时刻的目标位置之间的位置及姿态偏差信息；

逆运动学模块2012能调用信息应用处理器1012中工具2在当前时刻目标位置与工具2在下一时刻的目标位置之间的位置姿态偏差信息，并根据工具2在当前时刻目标位置与工具2在下一时刻的目标位置之间的位置姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

轨迹跟踪系统可进一步划分为传感系统5和执行系统6两部分，将传感系统5与执行系统6的功能明确公开，两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方面描述，这里将传感系统5向执行系统6发送信息定义为“下传”，将执行系统6向传感系统5发送信息定义为“上传”，轨迹跟踪系统中传感系统5和执行系统6的具体划分与实施例一中四种划分方式相同，具体工作方式也相同，这里不再赘述。

实施例八

本实施例与实施例七的不同之处在于：本实施例是在参考点ref-a的基础上，不以上述当前的控制参考点ref-a为跟踪目标，而改为以控制间隔时间t后的控制参考点ref-a-next为跟踪目标。本实施例中根据轨迹位置姿态信息集合｛q,n｝来确定工具在下一时刻的目标位置的具体确定方式如下：以参考点ref-a为球心，以△=v*t为半径做球面，该球面与轨迹特征线有两个交点，取位于工具前进方向上的球面与轨迹特征线的交点为控制参考点ref-a-next，以控制参考点ref-a-next为跟踪目标，确定位于控制参考点ref-a-next处的工具坐标系p-x-tool”y-tool”z-tool”：过控制参考点ref-a-next做垂直于轨迹特征线的平面，该平面与轨迹分割面的交线为z-tool”轴，z-tool”轴的正方向指向工件表面；以过控制参考点ref-a-next的轨迹特征线的切线为y-tool”轴，y-tool”轴的正方向指向工具前进方向；根据坐标系的左手定则或右手定则确定x-tool”轴及x-tool”轴的正方向；传感器在ref-a-next点检测到的是在传感器的检测截面m-sense内的轨迹线的断面轮廓，根据该断面轮廓计算出轨迹分割面m-seam与检测截面m-sense的交线，将该交线表示为向量n-seam，向量n-seam为工具位于控制参考点ref-a-next的姿态，将控制参考点ref-a-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态，并根据控制参考点ref-a-next的位置及姿态信息求出工具在当前时刻位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息；其中t为工具每次位置修正控制的调整间隔时间，v为工具的前进速度，位于当前时刻位置的工具经间隔时间t后移动至下一时刻目标位置；

其中位置及姿态偏差信息的具体求解过程为：设位于当前时刻目标位置p点的工具坐标系为p—x-tool’y-tool’z-tool’，

△x是：p点到控制参考点ref-a-next的向量在x-tool’轴方向的投影长度；

△y是：p点到控制参考点ref-a-next的向量在y-tool’轴方向的投影长度；

△z是：p点到控制参考点ref-a-next的向量在z-tool’轴方向的投影长度；

δx是：将位于控制参考点ref-a-next处的轨迹特征线直线化或过控制参考点ref-a-next作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过y-tool’轴和z-tool’轴的剖切面上的投影线段s-yz与y-tool’轴的夹角；

δy是：将位于控制参考点ref-a-next处的轨迹特征线直线化或过控制参考点ref-a-next作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和z-tool’轴的剖切面上的投影线段s-xz与z-tool’轴的夹角；

δz是：将位于控制参考点ref-a-next处的轨迹特征线直线化或过控制参考点ref-a-next作轨迹特征线的切线，轨迹特征线直线化或轨迹特征线的切线在过x-tool’轴和y-tool’轴的平面上的投影线段s-xy与y-tool’轴的夹角。

第五步中执行器根据控制参考点ref-a-next的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻的目标位置。

轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例七相同，不再赘述。

以上所述仅是本发明的较佳实施例，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明要求保护的范围。

实施例一、实施例二、实施例三和实施例四中所述的轨迹跟踪系统对传感系统的要求比较低，即便传感系统能力不足导致不能提供焊缝位置的高度偏差以及工件倾斜程度以及不能完善地得到轨迹截面形状或不能频繁、快速地得到轨迹截面形状，也可以完成轨迹跟踪目的。

例如传感系统中的传感器采用2d相机，此时传感系统仅能提供焊缝位置的左右偏差信息，不能提供焊缝位置的高度偏差以及工件倾斜程度时，p-s=[xs，0,zs]，其中zs只能是假定无高度偏差时的合理位置，而不是检测的值，仅是固定值。假设传感坐标系与工具坐标系的z轴平行，那么n-s=[0，0，1]。

因为δx是由于高度zs变化引起的，所以δx=0；

因为δy是由于n-s变化引起的，所以δy=0；

因为δz是由于xs变化引起的，所以δz≠0。

所以，变换到工具坐标系、计算出的偏差：

d-tool=[xtool，0,0，0，0，δztool]

那么，只要轨迹是在垂直于z-tool的一个平面上，即轨迹高度已由设备自己保证，就仍然可以准确跟踪。类似的传感系统能力不足的情况如图14所示的用直角坐标型操作机跟踪没有弧高偏差zs的平面曲线，图15所示的跟踪回转圆柱面上可以忽略弧高变化的曲线。

不能频繁、快速地得到轨迹截面形状，只能得到一个、或十分有限的几个轨迹截面形状的情况的传感系统，例如传感系统中的传感器采用位移传感器，位移传感器是一种仅侧量一个点到传感器距离的设备，常用的激光位移传感器就是其典型的代表。用这样的传感器跟踪平面上的一条直焊缝，但是由于焊接夹具不准确，焊缝的位置、姿态不能足够精确地确定。采用本发明的方法，仍然可以进行准确跟踪。做法是，把传感器附着到焊枪上，与焊枪有已知几何关系。工作时焊枪带着它先在焊缝上扫过，那么就可以在焊缝的起点得到一个截面上的焊缝位置。那么就有了一个q1-s，和一个不变的n1-s，假设已知焊缝的前进方向，那么只要在起点处调整好tt，就能以不变的姿态进行焊接。如果焊缝的前进方向也不知道，那么可以在焊缝的终点或接近终点处再进行一次截面扫描，得到其信息q2-s、n2-s。其中n2-s=n1-s，因为平直焊缝轨迹姿态不变。用q2-s与q1-s共同确定y-tool轴的方向，进一步用y-tool轴的方向与n1-s向量决定起始点的tt。那么，就可以用如下两步完成跟踪：

第一步：把焊枪调整到tt，

第二步：每次循环都用相同的驱动信息d-tool=[x0，0,0，0，0，0]，达到终点。

像上述二个例子例那样，根据实际情况，可以通过分析补充出q-s、n-s、d-tool中的元素，然后就能应用本发明给出的跟踪方法了，这对简化执行设备是非常有意义的。这时有如下规律：

只要d-tool中△z=0，能跟踪的就是平面，或可展开曲面（如回转的圆锥面、圆柱面面）上的曲线轨迹，对于工件，尽量用变位机等设备转换成可展开曲面；如△z≠0，但是将执行器限制成δx=0时，仍可以跟踪有缓慢地起伏变化的轨迹；如n-s不是定值，但是执行器限制成δy=0时，仍可以跟踪有小幅度侧向偏摆变化的轨迹；如果不能检测某一参数，就用执行器的结构保证其变化不超出规定值。

实施例一至实施例八所述的轨迹跟踪系统对执行系统的要求也比较低，即便执行系统为欠自由度系统也能使用本发明所述的轨迹跟踪方法。

图16所示，执行器为一台6自由度机器人，而传感器安装在第6关节上、焊枪安装在第5关节上的情况。也可能焊枪确实装在关节6上，但是关节6的变化对于焊枪特征点p无影响，但是需要传感器坐标系的y轴与工具坐标系的y-tool轴错开一个角度θ才能检测到焊缝。亦即，跟踪过程中，传感器坐标系与工具坐标系之间的相对关系是变化的。这时仅需要将变换坐标系所用的系统加以区分，而不是笼统地采用tt即可。该例子也揭示了焊枪、以及一些涂胶枪的一个特点：工具的姿态不需要完全调整，那么就可以用缺失一个回转自由度的5个自由度的执行器跟踪任意空间轨迹，只需保证跟踪过程中轨迹始终落在传感器的检测范围之内即可。该实施例是一种很好的拓展传感系统检测范围的实例。

当执行系统少于6个自由度数时，是欠自由度的，理论上必定不能精确跟踪任意的曲线，但是仍然可以根据上述控制原理，类推出改进的方法用于特定条件，均属于本专利保的护范围。例如上述用5个自由度的执行器带动焊枪跟踪任意空间轨迹的情况。例如用仅有两个或三个平移自由度的执行器，就可以跟踪平面上的曲线。图17所示情况能够跟踪倾斜的不挠曲的倾斜平面上的轨迹；如果执行器没有升降调整的自由度，仍然能够跟踪水平放置的平面上的轨迹。例如如图18所示，二自由度的执行器，配置一个水平翻转的变位机，可以跟踪回转体上的曲线。例如如图19所示，三个平移自由度的执行器，配置一个竖直回转轴的变位机，跟踪台面上一个偏心的或不够规则的环形线等。所谓变位机是一种使工件与机械臂产生相对运动的装置。如果忽略小的焊枪姿态偏差，这些欠自由度的执行器还可以跟踪更复杂些的轨迹，不一一列举。

前面实施例1中有述控制系统自身对轨迹跟踪的贡献问题，在此也可以体现。例如，当d-tool中前进驱动分量△y=0的情况下，就需要执行系统中的，例如“示教轨迹”或者“规划轨迹”，补出至少△y≠0的一个分量。

当然，类似的偏差信息结合不限于这样两组偏差信息之间的结合，还可以有多个偏差信息来源，而且它们的表述参考坐标系也可能不同，即可以通过变换统一到同一个参考系中，也可以分别在不同的参考系下分步修正，最后得到综合的控制效果。

本发明提供的控制方法对于传感系统5、执行系统6没有特别限制，但是系统之间要能互相协调配合，要求根据各自独特的功能，做相应的信息调整，例如把不能感知或控制的参数定义为反应实际情况的标准值就可以，因此可以广泛应用本控制方法。本发明提供的轨迹跟踪系统，可以支持各种不同的传感系统5、执行系统6，只要按照信息交换的规则组建跟踪系统，就可以选用尽量简单的甚至不完善的装置，完成丰富的具体任务；也可以用完善的传感系统5，配合全自由度或冗余自由度的执行系统6，完成非常复杂的跟踪任务。此外把轨迹跟踪系统进一步划分成传感系统和执行系统两部分，可以使跟踪系统的建立更加便利、灵活，而且便于由不同的专业厂家制造，提高性能、降低成本。