CN110455272A - 轨迹跟踪系统中的传感系统 - Google Patents

Abstract

本发明公开了一种轨迹跟踪系统中的传感系统，包括：传感器和信息检测处理器、带信息存储区的信息应用处理器和对外通信器，传感系统能通过对外通信器与执行系统进行信号互传。工作时先把传感器得到的以变动的传感器坐标系描述的轨迹信息固定化，即在检测后转化为适合整体记录轨迹信息的坐标系描述的形式进行标记，逐步呈现真实的轨迹，为后续求解目标提供基础，最后根据目标找到当前时刻工具应该呈现的理想状态或找到完成本次修正后在下一次修正时刻到来时要达到的状态，从而实现将传感器检测到的轨迹的原始轨迹信息变换成一种直接引导执行系统进行轨迹跟踪的通用信息的目的。

Description

轨迹跟踪系统中的传感系统

技术领域

本发明涉及轨迹跟踪领域，尤其涉及轨迹跟踪系统中的传感系统。

背景技术

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

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

发明内容

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

本发明所需解决的技术问题是：提供一种轨迹跟踪系统中的传感系统，该传感系统能将传感器检测到的轨迹的原始轨迹信息变换成一种直接引导常规、通用的执行系统进行轨迹跟踪的通用信息。

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

下述涉及的配合使用的执行系统包括执行器，其功能是带动工具与传感器运动，或者带动工具、传感器及工件运动，工具与传感器附着于执行器，执行器带着工具及传感器运动，实现轨迹跟踪，而工具与传感器之间的相对几何关系是可以确定的。

本发明所述的第一种轨迹跟踪系统中的传感系统，包括：传感器、信息检测处理器、带信息存储区的信息应用处理器和对外通信器，传感系统能通过对外通信器与执行系统进行信号互传；为方便描述，这里将传感系统向执行系统发送信息定义为“下传”，将执行系统向传感系统发送信息定义为“上传”；

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

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

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

信息应用处理器获取执行器当前时刻的位置与姿态信息，而工具与传感器之间的相对关系是固定的，或者是随时都可以计算确定的，从而获取工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q}，确定工具在下一时刻的目标位置；确定工具在下一时刻的目标位置的具体确定方式如下：

设工具在当前时刻的目标位置为P点，P点的工具坐标系为P—X-tool’ Y-tool’Z-tool’，过P点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点Ref-A，以参考点Ref-A为跟踪目标，将参考点Ref-A的位置作为工具在下一时刻的目标位置。

进一步地，前述的第一种轨迹跟踪系统中的传感系统，信息应用处理器根据传感器、信息检测处理器得到的信息还能进一步确定工具在下一时刻的目标位置及姿态，确定工具在下一时刻的目标位置及姿态的具体确定方式有两种，第一种确定方式为：

设工具在当前时刻的目标位置为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的位置及姿态作为工具在下一时刻的目标位置及姿态。

第二种确定方式为：用一个与Z-tool’轴方向相同的向量作为检测点的姿态信息，并以传感器坐标系表达该姿态信息；将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹姿态信息集合{N}，轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态；

进一步地，前述的第一种轨迹跟踪系统中的传感系统，还可在参考点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的位置作为工具在下一时刻的目标位置时，求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息；所述的位置及姿态偏差信息为侧向偏差△x或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。此时所述的位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置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’轴方向的投影长度。

进一步地，前述的第一种轨迹跟踪系统中的传感系统，其中信息应用处理器中将控制参考点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或控制参考点Ref-A-next的位置作为工具在下一时刻的目标位置时，还可在该传感系统中设置正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

进一步地， 在传感系统中设置正运动学模块的基础上，还可设置逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息，并根据工具在下一时刻的目标位置及姿态信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

进一步地，前述的第一种轨迹跟踪系统中的传感系统，将参考点Ref-A或控制参考点Ref-A-next的位置作为工具在下一时刻的目标位置，并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息时，还可在该传感系统中设置正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

进一步地， 在传感系统中设置正运动学模块的基础上，还可设置逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在当前目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

轨迹是一条任意弯曲的、有“姿态”的线段，是工具在理想状态下工作时跟踪的目标，其中“姿态”是指轨迹跟踪时不但工具上的特定点要落在轨迹特征线上，而且工具的轴线要与轨迹特征线呈一定的角度关系。本发明所述的第二种轨迹跟踪系统中的传感系统中涉及的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，还能感知轨迹的姿态信息。

本发明所述的第二种轨迹跟踪系统中的传感系统，包括：传感器、信息检测处理器、带信息存储区的信息应用处理器和对外通信器，传感器能感知轨迹的位置信息和姿态，传感系统能通过对外通信器与执行系统进行信号互传；

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

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

信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{Q，N}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；

信息应用处理器获取执行器当前时刻的位置与姿态信息，而工具与传感器之间的相对关系是可知的，从而获取工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

进一步地，前述的第二种轨迹跟踪系统中的传感系统，在参考点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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

进一步地，前述的第二种轨迹跟踪系统中的传感系统，其中，信息应用处理器将参考点Ref-A的位置及姿态信息作为工具在下一时刻的目标位置及姿态时，求出工具在当前时刻目标位置姿态与工具在下一时刻的目标之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息。

此时，所述的位置及姿态偏差信息的具体求解过程为：设位于当前时刻目标位置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-A-next的位置及姿态信息作为工具在下一时刻的目标位置及姿态时，求出工具在当前时刻目标位置姿态与工具在下一时刻的目标之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息。

此时，所述的位置及姿态偏差信息的具体求解过程为：设工具当前时刻的目标位置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或控制参考点Ref-A-next的位置作为工具在下一时刻的目标位置时，还可在该传感系统中设置正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

进一步地， 在传感系统中设置正运动学模块的基础上，还可设置逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息，并根据工具在下一时刻的目标位置及姿态信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

进一步地，前述的第二种轨迹跟踪系统中的传感系统，将参考点Ref-A或控制参考点Ref-A-next的位置作为工具在下一时刻的目标位置，并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息时，还可在该传感系统中设置正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

进一步地， 在传感系统中设置正运动学模块的基础上，还可设置逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在当前目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

本发明的有益效果是：上述二种轨迹跟踪系统中的传感系统均能将传感器检测到的轨迹的原始轨迹信息变换成一种可直接引导执行系统进行轨迹跟踪的通用信息，从而使复杂的轨迹跟踪中的控制问题从构建执行系统的工作中剥离出来，因此得以采用常规的执行系统、简化执行系统的结构。

另外二种传感系统均克服了检测点超前带来的误差，能使外部执行系统脱离示教信息独立运行，在轨迹跟踪过程中能充分利用传感器检测的信息进行轨迹跟踪，不仅可以纠正工具的位置偏差，还可以纠正工具的姿态偏差，从而更好地实现轨迹跟踪目的。

附图说明

图1是本发明所述的轨迹跟踪系统中的传感系统中传感器与工具的关系示意图。

图2是传感系统与执行系统配合使用的第一种信息关系框图。

图3是传感系统与执行系统配合使用的第二种信息关系框图。

图4是传感系统与执行系统配合使用的第三种信息关系框图。

图5是确定参考点Ref-A位置及姿态的原理图。

图6是确定另一种参考点Ref-B位置及姿态的原理图。

图7是确定参考点Ref-A或另一种参考点Ref-B部分位置及姿态的原理图。

图8是确定参考点Ref-A或另一种参考点Ref-B部分位置及姿态的原理图。

图9是确定控制参考点Ref-A-next位置及姿态的原理图。

图10是本发明所述的原始轨迹信息的表达示意图。

图11是本发明所述的轨迹信息的全面表达示意图。

图12是本发明所述的轨迹跟踪系统中的传感系统的拓展。

具体实施方式

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

实施例一

本实施例所述的轨迹跟踪系统中的传感系统5需要与外部执行系统6配合使用，工具2安装于执行系统6中的执行器上，传感系统5中的传感器3附着于执行器的关节上、或者传感器3直接附着于工具2上，且传感系统5与外部执行系统6配合使用的过程中需要保证传感器3与工具2之间的相对位置随时具有确定的几何关系。传感系统5和执行系统6配合使用需要事先约定执行器的运动学模型，对传感系统5、执行系统6设置相互协调配合的参数。

本实施例以传感器3直接附着于工具2上为例进行说明，如图1和图2所示，本实施例所述的轨迹跟踪系统中的传感系统5，包括：传感器3和信息检测处理器1011、带信息存储区的信息应用处理器1012和对外通信器，传感系统5能通过对外通信器与执行系统6进行信号互传。为方便描述，这里将传感系统5向执行系统6发送信息定义为“下传”，将执行系统6向传感系统5发送信息定义为“上传”。

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

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

为方便描述，这里结合执行系统6、以机器人跟踪V形坡口焊缝、传感器3设置于焊枪前端为例进行说明，这里执行器为机器人，工具2为焊枪，轨迹为焊缝4，轨迹特征线为焊缝特征线，具体参见图1所示。本发明所述的轨迹跟踪系统中的传感系统的应用不限于焊接领域，只要是引导工具2沿着连续的几何轨迹运动的任务都可以适用，例如用于以特定几何边际为依据的材料切割、涂胶、打磨边角等任务。如图1所示，传感器3检测到的焊缝横截面信息集中在平面M-sense内，这里将平面M-sense定义为检测截面，将坐标系O-XYZ定义为传感器坐标系。在焊缝4的截面上有一个点可以代表焊缝位置，例如角接焊缝截面上两个侧边的交点、V形坡口焊缝中V字最低处的交点等。我们将这个在检测截面上代表焊缝位置的点简称为检测点，在图1中标记为Q点，在焊接时，传感器3在焊缝4上方扫过，得到一系列的检测点：Q₁、Q₂、Q₃……Q_i。把代表焊枪位置的点定义为焊枪特征点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，仍然不影响后续计算。

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

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

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

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

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

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

信息应用处理器1012获取执行系统6上传的工具2当前时刻的目标位置与姿态信息，并根据工具2当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q}，确定工具2在下一时刻的目标位置。

这里需要执行系统6通过对外通信器向传感系统5上传工具2当前时刻的目标位置与姿态信息Tt[i]，根据该信息以及轨迹位置信息集合{Q}，信息应用处理器1012就可以计算工具在下一时刻的目标位置的信息；信息应用处理器1012计算出的工具在下一时刻的目标位置的信息就是一种能直接引导执行系统6进行轨迹跟踪的通用信息。

确定工具2在下一时刻的目标位置的具体确定方式如下：

设工具2在当前时刻的目标位置为P点，P点的工具坐标系为P—X-tool’ Y-tool’Z-tool’，过P点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点Ref-A，以参考点Ref-A为跟踪目标，将参考点Ref-A的位置作为工具2在下一时刻的目标位置。

信息应用处理器1012根据传感器、信息检测处理器1012得到的信息还能进一步确定工具在下一时刻的目标位置及姿态，确定工具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轴的正方向指向工具2前进方向；根据坐标系的左手定则或右手定则确定X-tool轴及X-tool轴的正方向；将参考点Ref-A的位置及姿态作为工具在下一时刻的目标位置及姿态。

由于检测环节没有姿态信息N，就不能有轨迹分割面，就不能调整姿态，在第一种确定方式中利用当前工具Z -tool’轴的姿态信息补出缺失的N，从而就有轨迹分割面，就能调整姿态了。

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

因而除了利用当前工具Z -tool’轴的姿态信息补出缺失的N的方式来得到轨迹分割面外，还可以采用一个与Z-tool’轴方向相同的向量作为检测点的姿态信息。下述第二种确定方式就是采用一个与Z-tool’轴方向相同的向量作为检测点的姿态信息。

第二种确定方式为：用一个与Z-tool’轴方向相同的向量作为检测点的姿态信息，并以传感器坐标系表达该姿态信息；将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹姿态信息集合{N}，轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

上述信息应用处理器1012得到的工具在下一时刻的目标位置的信息就是一种能直接引导执行系统进行轨迹跟踪的通用信息。当然，在实际设计过程中，还可以在将参考点Ref-A的位置及姿态信息作为工具在下一时刻的目标位置及姿态的基础上，或者通过其他途径，进一步的求出工具在当前时刻目标位置与工具在下一时刻的目标位置之间的位置及姿态偏差信息，此时信息应用处理器1012得到的位置与姿态偏差信息，即图2中的D[i+1]，也是一种能直接引导执行系统进行轨迹跟踪的通用信息。根据该位置及姿态偏差信息改变工具当前时刻的目标位置、姿态使其达到下一时刻的正确位置姿态，那么工具每前进一步后，都落在正确的位置姿态上，就实现了理论上的无偏差的跟踪效果，而且该轨迹跟踪方法还克服了检测点超前带来的误差。

所述的位置及姿态偏差信息为侧向偏差△x或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。此时所述的位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置P点的工具坐标系为P—X-tool’ Y-tool ’Z-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’轴方向的投影长度。

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

如图7所示，过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’轴方向的夹角。

如图8所示，过X-tool’轴、Z-tool’轴的剖切面图，其中线段S-xz为焊缝特征线在该剖切面上的投影，那么需要焊枪绕着Y-tool’轴调整的角度偏差为δy，即S-xz与Z-tool’轴的夹角。图8还同时表达了以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驱动就可以实现跟踪。这时机器人就是一个完全靠外部传感系统驱动的设备了。

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

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

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

以上仅仅是以V形坡口的焊缝为例叙述，但是不限于此。对于角接焊缝、搭接焊缝、对接焊缝等，只要定义出检测截面上的焊缝特征点P均可应用。

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

实施例二

本实施例与实施例一的不同之处在于：在参考点Ref-A的基础上，不以上述当前的控制参考点Ref-A为跟踪目标，而改为以控制间隔时间t后的控制参考点Ref-A-next为跟踪目标，此时根据工具2当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{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的位置及姿态信息作为工具2在下一时刻的目标位置及姿态。

上述信息应用处理器1012得到的工具2在下一时刻的目标位置的信息就是一种能直接引导执行系统进行轨迹跟踪的通用信息。当然，在实际设计过程中，还可以在将控制参考点Ref-A-next的位置及姿态信息作为工具2在下一时刻的目标位置及姿态的基础上，求出工具2在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，或者通过其他途径求出位置及姿态偏差信息；此时信息应用处理器1012得到的位置与姿态偏差信息也是一种能直接引导执行系统进行轨迹跟踪的通用信息。

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

实施例三

本实施例与实施例一或实施例二的不同之处在于：参见图3所示，在传感系统5中设置有正运动学模块2011，正运动学模块2011获取工具的目标位置在当前时刻目标位置时执行器的关节角信息θ[i]，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息Tt[i]。这里需要执行系统6通过对外通信器向传感系统5传送工具2的目标位置在当前时刻目标位置时执行器的关节角信息。

参见图4所示，在传感系统5中设置正运动学模块2011的基础上，还可设置逆运动学模块2012，以便进一步简化执行系统。

在图2、图3、乃至图4中，如若信息应用处理器1012得到工具2在下一时刻的目标位置后，没有继续往下求出工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的位置及姿态偏差信息时，逆运动学模块2012获取信息应用处理器中计算出的工具2在下一时刻的目标位置及姿态信息，并根据工具2在下一时刻的目标位置及姿态信息，计算出工具2的目标位置位于下一时刻目标位置时执行器的关节角信息。此时逆运动学模块2012得到的工具2的目标位置位于下一时刻目标位置时执行器的关节角信息是一种能直接引导执行系统进行轨迹跟踪的通用信息。

如若信息应用处理器1012得到工具2在下一时刻的目标位置后，继续往下求出工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的位置及姿态偏差信息，或者采用其它方法直接求得位置及姿态偏差信息时，逆运动学模块2012获取信息应用处理器1012中计算出的工具2在当前目标位置与工具2在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具2的目标位置位于下一时刻目标位置时执行器的关节角信息。此时逆运动学模块2012得到的工具2的目标位置位于下一时刻目标位置时执行器的关节角信息是一种能直接引导执行系统进行轨迹跟踪的通用信息。

实施例四

本实施例与实施例一的不同之处在于：本实施例中的传感器3不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差，合称为轨迹的位置信息，还能感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性，即姿态信息。

本发明所述的轨迹跟踪系统中的传感系统，包括：传感器3和信息检测处理器1011、带信息存储区的信息应用处理器1012和对外通信器，传感器3能感知轨迹的位置信息和姿态，传感系统5能通过对外通信器与执行系统6进行信号互传；

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

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

在焊接时，传感器3在焊缝4上方扫过，得到一系列的检测点Q，机器人在传感器3的一系列检测点信息的引导下，带着焊枪运动，使焊枪特征点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是传感器坐标系的坐标轴平行于焊缝、而且无偏差的一种特殊状态，而图10则代表着一般的状态，图中所示为检测截面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来表达。

信息应用处理器1012将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{Q，N}；其中工件1位置不固定时轨迹所附着的坐标系为工件坐标系，工件1位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。

机器人工作于世界坐标系，在世界坐标中，焊枪特征点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）X Q-t =（Tt）X（T-sensor）X（Q-s）

N-w =（Tt）X N-t =（Tt）X（T-sensor）X（N-s）

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

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

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

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

信息应用处理器1012获取工具2当前时刻的目标位置与姿态信息，并根据工具2当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q，N}，确定工具2在下一时刻的目标位置及姿态。

这里需要执行系统6通过对外通信器向传感系统上传工具2当前时刻的目标位置与姿态信息，信息应用处理器1012得到的工具在下一时刻的目标位置的信息就是一种能直接引导执行系统6进行轨迹跟踪的通用信息。

确定工具2在下一时刻的目标位置及姿态的具体确定方式如下：设工具2在当前时刻的目标位置为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的位置及姿态信息作为工具2在下一时刻的目标位置及姿态。

上述信息应用处理器1012得到的工具2在下一时刻的目标位置的信息就是一种能直接引导执行系统进行轨迹跟踪的通用信息。当然，在实际设计过程中，还可以在将参考点Ref-A的位置及姿态信息作为工具2在下一时刻的目标位置及姿态时，求出工具2在当前时刻目标位置姿态与工具2在下一时刻的目标之间的位置及姿态偏差信息，此时信息应用处理器1012得到的位置与姿态偏差信息也是一种能直接引导执行系统进行轨迹跟踪的通用信息。

所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息。

此时，所述的位置及姿态偏差信息的具体求解过程为：设位于当前时刻目标位置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-A的基础上，不以上述当前的控制参考点Ref-A为跟踪目标，而改为以控制间隔时间t后的控制参考点Ref-A-next为跟踪目标，此时根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q，N}确定工具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的位置及姿态信息作为工具2在下一时刻的目标位置及姿态。

上述信息应用处理器1012得到的工具2在下一时刻的目标位置的信息就是一种能直接引导执行系统进行轨迹跟踪的通用信息。当然，在实际设计过程中，还可以在将控制参考点Ref-A-next的位置及姿态信息作为工具2在下一时刻的目标位置及姿态时，求出工具2在当前时刻目标位置姿态与工具2在下一时刻的目标之间的位置及姿态偏差信息，此时信息应用处理器1012得到的位置与姿态偏差信息也是一种能直接引导执行系统进行轨迹跟踪的通用信息。

所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息。

此时，所述的位置及姿态偏差信息的具体求解过程为：设工具当前时刻的目标位置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’轴的夹角。

实施例六

本实施例与实施例四或实施例五的不同之处在于：参见图3所示，在传感系统5中设置有正运动学模块2011，正运动学模块2011获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。这里需要执行系统通过对外通信器向传感系统传送工具的目标位置在当前时刻目标位置时执行器的关节角信息。

参见图4所示，在传感系统5中设置正运动学模块2011的基础上，还可设置逆运动学模块2012。为了简洁，图4中未象图3那样将逆运动学模块2012详细表达，而是舍弃了可被选择使用的“位姿修正”环节，仅用一个“逆运动学”方框笼统地表达，实际上完全可以是一样的。图4所示系统中还可以加入类似于传统机器人的示教-再现、轨迹规划等功能模块，以便实现更加丰富的功能。

实际上，图2、图3、图4所示传感系统，还可以更加灵活地配置功能模块。例如，图2中还表示了直接向外部执行系统6提供以传感器坐标系表达的轨迹特征信息的“提供信息模式”，可供具备相应信息处理能力的执行系统使用。同样，图3、图4所示传感系统也完全可以这样做。假如图4所示传感系统配备图2、图3所示传感系统涉及的各种信息传输能力以及相应的软件模块，那么就可以配合各种外部执行系统使用了。图12就是一种拓展的传感系统。

如若信息应用处理器1012得到工具2在下一时刻的目标位置后，没有继续往下求出工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的位置及姿态偏差信息时，逆运动学模块2012获取信息应用处理器中计算出的工具2在下一时刻的目标位置及姿态信息，并根据工具2在下一时刻的目标位置及姿态信息，计算出工具2的目标位置位于下一时刻目标位置时执行器的关节角信息。此时逆运动学模块2012得到的工具2的目标位置位于下一时刻目标位置时执行器的关节角信息是一种能直接引导执行系统进行轨迹跟踪的通用信息。

如若信息应用处理器1012得到工具2在下一时刻的目标位置后，继续往下求出工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的位置及姿态偏差信息时，逆运动学模块2012获取信息应用处理器1012中计算出的工具2在当前目标位置与工具2在下一时刻目标位置之间的位置及姿态偏差信息，或者采用其它方法直接求得位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具2的目标位置位于下一时刻目标位置时执行器的关节角信息。此时逆运动学模块2012得到的工具2的目标位置位于下一时刻目标位置时执行器的关节角信息是一种能直接引导执行系统进行轨迹跟踪的通用信息。

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

上述二种轨迹跟踪系统中的传感系统5均能将传感器3检测到的轨迹的原始轨迹信息变换成一种可直接引导执行系统6进行轨迹跟踪的通用信息，从而使复杂的轨迹跟踪中的控制问题从构建执行系统6的工作中剥离出来，因此得以采用常规的执行系统、简化执行系统6的结构。

另外二种传感系统5均克服了检测点超前带来的误差，能使外部执行系统6脱离示教信息独立运行，在轨迹跟踪过程中能充分利用传感器3检测的信息进行轨迹跟踪，不仅可以纠正工具2的位置偏差，还可以纠正工具2的姿态偏差，从而更好地实现轨迹跟踪目的。

Claims (26)

1.轨迹跟踪系统中的传感系统，包括：传感器和信息检测处理器，其特征在于：还包括带信息存储区的信息应用处理器和对外通信器，传感系统能通过对外通信器与配合使用执行系统进行信号互传；

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

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

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

信息应用处理器获取工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q}，确定工具在下一时刻的目标位置；确定工具在下一时刻的目标位置的具体确定方式如下：

设工具在当前时刻的目标位置为P点，P点的工具坐标系为P—X-tool’ Y-tool’Z-tool’，过P点做与轨迹特征线垂直的平面，该平面与轨迹特征线的交点为参考点Ref-A，以参考点Ref-A为跟踪目标，将参考点Ref-A的位置作为工具在下一时刻的目标位置。

2.按照权利要求1所述的轨迹跟踪系统中的传感系统，其特征在于：信息应用处理器获取执行系统上传的工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{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的位置及姿态作为工具在下一时刻的目标位置及姿态。

3.按照权利要求1所述的轨迹跟踪系统中的传感系统，其特征在于：信息应用处理器获取执行系统上传的工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q}，确定工具在下一时刻的目标位置及姿态；确定工具在下一时刻的目标位置及姿态的具体确定方式如下：

用一个与Z-tool’轴方向相同的向量作为检测点的姿态信息，并以传感器坐标系表达该姿态信息；将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹姿态信息集合{N}，轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

4.按照权利要求2或3所述的轨迹跟踪系统中的传感系统，其特征在于：根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{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.按照权利要求2或3所述的轨迹跟踪系统中的传感系统，其特征在于：信息应用处理器求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息；所述的位置及姿态偏差信息为侧向偏差△x，或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。

6.按照权利要求5所述的轨迹跟踪系统中的传感系统，其特征在于：所述的位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置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’轴方向的投影长度。

7.按照权利要求4所述的轨迹跟踪系统中的传感系统，其特征在于：信息应用处理器求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息；所述的位置及姿态偏差信息为侧向偏差△x，或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。

8.按照权利要求7所述的轨迹跟踪系统中的传感系统，其特征在于：所述的位置及姿态偏差信息的具体求解过程为：设当前时刻目标位置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’轴方向的投影长度。

9.按照权利要求1所述的轨迹跟踪系统中的传感系统，其特征在于：还包括正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

10.按照权利要求9所述的轨迹跟踪系统中的传感系统，其特征在于：还包括逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息，并根据工具在下一时刻的目标位置及姿态信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

11.按照权利要求5所述的轨迹跟踪系统中的传感系统，其特征在于：还包括正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

12.按照权利要求11所述的轨迹跟踪系统中的传感系统，其特征在于：还包括逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在当前目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

13.按照权利要求7所述的轨迹跟踪系统中的传感系统，其特征在于：还包括正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

14.按照权利要求13所述的轨迹跟踪系统中的传感系统，其特征在于：还包括逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在当前目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

15.轨迹跟踪系统中的传感系统，包括：传感器和信息检测处理器，传感器能感知轨迹的位置信息和姿态信息，其特征在于：还包括带信息存储区的信息应用处理器和对外通信器，传感系统能通过对外通信器与配合使用的执行系统进行信号互传；

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

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

信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息，然后标记于轨迹所附着的坐标系中，并存储于信息存储区中形成轨迹位置信息集合{Q，N}；其中工件位置不固定时轨迹所附着的坐标系为工件坐标系，工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种；

信息应用处理器获取工具当前时刻的目标位置与姿态信息，并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

16.按照权利要求15所述的轨迹跟踪系统中的传感系统，其特征在于：根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。

17.按照权利要求15所述的轨迹跟踪系统中的传感系统，其特征在于：信息应用处理器求出工具在当前时刻目标位置姿态与工具在下一时刻的目标之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息。

18.按照权利要求17所述的轨迹跟踪系统中的传感系统，其特征在于：所述的位置及姿态偏差信息的具体求解过程为：设位于当前时刻目标位置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’轴的夹角。

19.按照权利要求16所述的轨迹跟踪系统中的传感系统，其特征在于：信息应用处理器求出工具在当前时刻目标位置姿态与工具在下一时刻的目标之间的位置及姿态偏差信息，所述的位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x，δy两个有效分量的位置偏差信息，且所述的位置及姿态偏差信息为至多包含△x，△y，△z，δx，δy，δz六个有效分量的位置偏差信息。

20.按照权利要求19所述的轨迹跟踪系统中的传感系统，其特征在于：所述的位置及姿态偏差信息的具体求解过程为：设工具当前时刻的目标位置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’轴的夹角。

21.按照权利要求15或16所述的轨迹跟踪系统中的传感系统，其特征在于：还包括正运动学模块，正运动学模块获取工具的目标位置位于当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

22.按照权利要求21所述的轨迹跟踪系统中的传感系统，其特征在于：还包括逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息，并根据工具在下一时刻的目标位置及姿态信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

23.按照权利要求17所述的轨迹跟踪系统中的传感系统，其特征在于：还包括正运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

24.按照权利要求23所述的轨迹跟踪系统中的传感系统，其特征在于：还包括逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在当前目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。

25.按照权利要求19所述的轨迹跟踪系统中的传感系统，其特征在于：还包括正运动学模块和逆运动学模块，正运动学模块获取工具的目标位置在当前时刻目标位置时执行器的关节角信息，并根据该执行器的关节角信息计算出工具在当前时刻目标位置与姿态信息。

26.按照权利要求25所述的轨迹跟踪系统中的传感系统，其特征在于：还包括逆运动学模块，逆运动学模块获取信息应用处理器中计算出的工具在当前目标位置与工具在下一时刻目标位置之间的位置及姿态偏差信息，并根据该位置及姿态偏差信息，计算出工具的目标位置位于下一时刻目标位置时执行器的关节角信息。