首页 / 专利库 / 畜牧业 / 家畜 / 反刍动物 / / 一种轨迹跟踪控制方法及轨迹跟踪系统

一种轨迹跟踪控制方法及轨迹跟踪系统

阅读:767发布:2023-12-08

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

1.一种轨迹跟踪控制方法,其特征在于:工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:
(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息,并以传感器坐标系表达该位置信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置、以及工具在下一时刻的目标位置;
根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,将参考点Ref-Q的位置作为传感器在下一时刻的检测点位置;
根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置的具体确定方式如下:
设工具在当前时刻的目标位置为P点,P点的工具坐标系为P—X-tool’ Y-tool ’Z-tool’,过P点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-A,以参考点Ref-A为工具支链的跟踪目标,将参考点Ref-A的位置作为工具在下一时刻的目标位置;
(5)分别驱动传感器支链和工具支链:
驱动传感器枝链:根据参考点Ref-Q的位置信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool’轴的正方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;
驱动工具支链:根据参考点Ref-A的位置信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool’轴的正方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
2.按照权利要求1所述的一种轨迹跟踪控制方法,其特征在于:步骤(4)中根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置与姿态、以及工具在下一时刻的目标位置与姿态;
根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’Yq-tool ’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q-Xq-toolYq-toolZq-tool:过参考点Ref-Q做轨迹特征线的切线,以过轨迹特征线的切线和过与Zq -tool’轴平行的直线的平面为轨迹分割面;过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
根据轨迹位置信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t。
3.按照权利要求1所述的一种轨迹跟踪控制方法,其特征在于:步骤(2)中用一个与Zq-tool’轴方向相同的向量作为检测点的姿态信息,并以传感器坐标系表达该姿态信息;
步骤(3)中将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹姿态信息集合{N},轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{Q,N};
步骤(4)中,根据轨迹位置姿态信息集合{Q,N},确定位于参考点Ref-Q处的传感器坐标系Q-Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
步骤(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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t。
4.按照权利要求2或3所述的一种轨迹跟踪控制方法,其特征在于:步骤(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的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻目标位置。
5.一种轨迹跟踪控制方法,其特征在于:工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:
(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息,并以传感器坐标系表达该位置信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,所述第一位置及姿态偏差信息为侧向偏差△xq,或侧向偏差△xq与前进偏差△yq、高度偏差△zq、俯仰度偏差δxq、前进方向角度偏差δzq中的至少一种相结合;
根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置及姿态,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,所述第二位置及姿态偏差信息为侧向偏差△x,或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合;
其中根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’Yq-tool’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做轨迹特征线的切线,以过轨迹特征线的切线和过与Zq -tool’轴平行的直线的平面为轨迹分割面;过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
其中根据轨迹位置信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着预定方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置的传感器驱动信息为驱动信息Dq-tool,驱动信息Dq-tool为至少包含一个有效分量:△xq的向量,且驱动信息Dq-tool为至多包含五个有效分量:△xq,△yq,△zq,δxq,δzq的向量;其中在驱动信息Dq-tool不包含δxq或δzq时,所述预定方向为由于位移调整而自然地被确定的方向;在驱动信息Dq-tool包含δxq或δzq时,所述预定方向为Yq-tool轴的正方向;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着预定方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置的工具驱动信息为驱动信息D-tool,驱动信息D-tool为至少包含一个有效分量:△x的向量,且驱动信息D-tool为至多包含五个有效分量:
△x,△y,△z,δx,δz的向量;其中在驱动信息D-tool不包含δx或δz时,所述预定方向为由于位移调整而自然地被确定的方向;在驱动信息D-tool包含δx或δz时,所述预定方向为Y-tool轴的正方向;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
6.按照权利要求5所述的一种轨迹跟踪控制方法,其特征在于:步骤(4)中第一位置及姿态偏差信息的具体求解过程为:设当前时刻检测点位置Q点的传感器坐标系为Q—Xq-tool’Yq-tool’Zq-tool’,
δzq 是:轨迹特征线上位于参考点Ref-Q处的切线在过Xq-tool’轴及Yq-tool’轴的平面内的投影Sq-xy与Yq-tool’轴方向的夹角;
δxq 是:轨迹特征线上位于参考点Ref-Q处的切线在过Yq-tool’轴及Z-tool’轴的平面内的投影Sq-yz与Yq-tool’轴方向的夹角;
△xq 是:Q点到参考点Ref-Q的向量在Xq-tool’轴方向的投影长度;
△yq 是:Q点到参考点Ref-Q的向量在Yq-tool’轴方向的投影长度;
△zq 是:Q点到参考点Ref-Q的向量在Zq-tool’轴方向的投影长度;
步骤(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’轴方向的投影长度。
7.按照权利要求5所述的一种轨迹跟踪控制方法,其特征在于:步骤(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的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。
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.按照权利要求5、6或7所述的一种轨迹跟踪控制方法,其特征在于:步骤(5)中的传感器驱动信息为驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息;
步骤(5)中的工具驱动信息为驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
10.一种轨迹跟踪控制方法,传感器能感知轨迹的位置信息和姿态,其特征在于:工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:
(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息,并以传感器坐标系表达该位置信息和姿态信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态、以及工具在下一时刻的目标位置及姿态;
根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过参考点Ref-Q的轨迹特征线的切线为Yq-tool轴, Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
根据轨迹位置姿态信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
11.按照权利要求10所述的一种轨迹跟踪控制方法,其特征在于:步骤(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的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻的目标位置。
12.按照权利要求10或11所述的一种轨迹跟踪控制方法,其特征在于:步骤(3)中将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息的转化过程为:先将以传感器坐标系表达的位置及姿态信息转化为以传感器枝链的末端坐标系表达的位置及姿态信息,然后将以传感器枝链的末端坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息。
13.一种轨迹跟踪控制方法,传感器能感知轨迹的位置信息和姿态,其特征在于:工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:
(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息,并以传感器坐标系表达该位置信息和姿态信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,所述的第一位置及姿态偏差信息为侧向偏差△xq、前进偏差△yq、高度偏差△zq、俯仰角度偏差δxq、侧偏角偏差δyq、前进方向角度偏差δzq这六个有效分量中至少含△xq,δyq两个有效分量的位置及姿态偏差信息,且所述的第一位置及姿态偏差信息为至多包含△xq,△yq,△zq,δxq,δyq,δzq六个有效分量的位置及姿态偏差信息;
根据轨迹位置姿态信息集合{Q,N}来确定工具在下一时刻的目标位置及姿态,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,所述的第二位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x,δy两个有效分量的位置及姿态偏差信息,且所述的第二位置及姿态偏差信息为至多包含△x,△y,△z,δx,δy,δz六个有效分量的位置及姿态偏差信息;
根据据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;
以过参考点Ref-Q的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
根据据轨迹位置姿态信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置;使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置的传感器驱动信息为驱动信息Dq-tool,驱动信息Dq-tool为至少包含两个有效分量:△xq,δyq的向量,且驱动信息Dq-tool为至多包含六个有效分量:
△xq,△yq,△zq,δxq,δyq,δzq的向量;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置;使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置的工具驱动信息为驱动信息D-tool,驱动信息D-tool为至少包含两个有效分量:
△x,δy的向量,且驱动信息D-tool为至多包含六个有效分量:△x,△y,△z,δx,δy,δz的向量;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
14.按照权利要求13所述的一种轨迹跟踪控制方法,其特征在于:步骤(4)中第一位置及姿态偏差信息的具体求解过程为:设当前时刻的检测点Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,
xq 是:Q点到参考点Ref-Q的向量在Xq-tool’轴方向的投影长度;
△yq 是:Q点到参考点Ref-Q的向量在Yq-tool’轴方向的投影长度;
△zq 是:Q点到参考点Ref-Q的向量在Zq-tool’轴方向的投影长度;
δxq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Yq-tool’轴和Zq-tool’轴的平面上的投影线段Sq-yz与Yq-tool’轴的夹角;
δyq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Xq-tool’轴和Zq-tool’轴的平面上的投影线段Sq-xz与Zq-tool’轴的夹角;
δzq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Xq-tool’轴和Yq-tool’轴的平面上的投影线段Sq-xy与Yq-tool’轴的夹角;
步骤(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’轴的夹角。
15.按照权利要求13所述的一种轨迹跟踪控制方法,其特征在于:步骤(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的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。
16.按照权利要求15所述的一种轨迹跟踪控制方法,其特征在于:第二位置及姿态偏差信息的具体求解过程为:设工具当前时刻的目标位置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’轴的夹角。
17.按照权利要求13、14或15所述的一种轨迹跟踪控制方法,其特征在于:步骤(5)中的传感器驱动信息为驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息;
步骤(5)中的工具驱动信息为驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
18.按照权利要求13、14或15所述的一种轨迹跟踪控制方法,其特征在于:步骤(3)中将以传感器坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息的转化过程为:先将以传感器坐标系表达的位置姿态信息转化为以传感器支链的末端坐标系表达的位置姿态信息,然后将以传感器支链的末端坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息。
19.实现权利要求1-4中任一轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,其特征在于:传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模和逆运动学模块,信息应用处理器中还包括信息存储区;
传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置与姿态信息,并根据传感器当前时刻的检测点位置与姿态信息以及轨迹位置信息集合{Q},确定传感器在下一时刻的检测点位置及姿态信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息,并根据工具当前时刻的目标位置与姿态信息以及轨迹位置信息集合{Q},确定工具在下一时刻的目标位置及姿态信息;
逆运动学模块调用信息应用处理器中计算出的传感器在下一时刻的检测点位置及姿态信息,并根据传感器在下一时刻的检测点位置及姿态信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息,并根据工具在下一时刻的目标位置及姿态信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
20.按照权利要求19所述的轨迹跟踪系统,其特征在于:传感器和信息检测处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;信息应用处理器、正运动学模块、逆运动学模块、工具支链和传感器支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统向执行系统下传以传感器坐标系表达的轨迹的位置信息。
21.按照权利要求19所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器和信息应用处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;正运动学模块、逆运动学模块、工具支链和传感器支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器当前时刻的检测点位置与姿态信息、以及工具当前时刻的目标位置与姿态信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
22.按照权利要求19所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;逆运动学模块和带控制器的执行器构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
23.按照权利要求19所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;带控制器的执行器构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;
传感系统向执行系统下传传感器到达下一时刻检测点位置及姿态时传感器支链的关节角信息、以及工具到达下一时刻目标位置及姿态时工具支链的关节角信息。
24.实现权利要求5-9中任一轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,其特征在于:传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块,信息应用处理器中还包括信息存储区;
传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置及姿态信息,并根据传感器当前时刻的检测点位置与姿态信息以及轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态信息,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息,并根据工具当前时刻的目标位置与姿态信息以及轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置及姿态信息,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息;
逆运动学模块调用信息应用处理器中计算出的传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,并根据第一位置及姿态偏差信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
25.按照权利要求24所述的轨迹跟踪系统,其特征在于:传感器和信息检测处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;信息应用处理器、正运动学模块、逆运动学模块、工具支链和传感器支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统向执行系统下传以传感器坐标系表达的轨迹的位置信息。
26.按照权利要求24所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器和信息应用处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器当前时刻的检测点位置与姿态信息、以及工具当前时刻的目标位置与姿态信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
27.按照权利要求24所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
28.按照权利要求24所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器到达下一时刻检测点位置及姿态时传感器支链的关节角信息、以及工具到达下一时刻目标位置及姿态时工具支链的关节角信息。
29.实现权利要求10-12中任一轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,传感器能感知轨迹的位置信息和姿态,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,其特征在于:传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块,信息应用处理器中还包括信息存储区;
传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置及姿态信息,并根据传感器当前时刻的检测点位置及姿态信息、以及轨迹位置姿态信息集合{Q,N},确定传感器在下一时刻的检测点位置及姿态信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置及姿态信息,并根据工具当前时刻的目标位置及姿态信息、以及轨迹位置姿态信息集合{Q,N},确定工具在下一时刻的目标位置及姿态信息;
逆运动学模块调用信息应用处理器中计算出的传感器在下一时刻的检测点位置及姿态信息,并根据传感器在下一时刻的检测点位置及姿态信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息,并根据工具在下一时刻的目标位置及姿态信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
30.按照权利要求29所述的轨迹跟踪系统,其特征在于:传感器和信息检测处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;信息应用处理器、正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统向执行系统下传以传感器坐标系表达的轨迹的位置及姿态信息。
31.按照权利要求29所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器和信息应用处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器当前时刻的检测点位置与姿态信息、以及工具当前时刻的目标位置与姿态信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
32.按照权利要求29所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
33.按照权利要求29所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器到达下一时刻检测点位置及姿态时传感器支链的关节角信息、以及工具到达下一时刻目标位置及姿态时工具支链的关节角信息。
34.实现权利要求13-18中任一轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,传感器能感知轨迹的位置信息和姿态,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,其特征在于:传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块,信息应用处理器中还包括信息存储区;
传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置与姿态信息,并根据传感器当前时刻的检测点位置与姿态信息、以及轨迹位置姿态信息集合{Q,N},确定传感器在下一时刻的检测点位置及姿态信息,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息,并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q},确定工具在下一时刻的目标位置及姿态信息,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息;
逆运动学模块调用信息应用处理器中计算出的传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,并根据第一位置姿态偏差信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
35.按照权利要求34所述的轨迹跟踪系统,其特征在于:传感器和信息检测处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;信息应用处理器、正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统向执行系统下传以传感器坐标系表达的轨迹的位置及姿态信息。
36.按照权利要求34所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器和信息应用处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器当前时刻的检测点位置与姿态信息、以及工具当前时刻的目标位置与姿态信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
37.按照权利要求34所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
38.按照权利要求34所述的轨迹跟踪系统,其特征在于:传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器到达下一时刻检测点位置及姿态时传感器支链的关节角信息、以及工具到达下一时刻目标位置及姿态时工具支链的关节角信息。

说明书全文

一种轨迹跟踪控制方法及轨迹跟踪系统

技术领域

[0001] 本发明涉及轨迹自动跟踪技术领域,尤其涉及一种用于传感器由传感器支链驱动、工具由工具支链驱动的轨迹跟踪控制方法以及实现该轨迹跟踪控制方法的轨迹跟踪系统。

背景技术

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

发明内容

[0004] 焊接、涂胶、切割等作业是一类相似的常规操作,其共同特点都是沿着特定的轨迹进行操作,轨迹是一条任意弯曲的、有“姿态”的线段,是工具在理想状态下工作时跟踪的目标,轨迹所在工件表面在检测点处“朝向哪个方向”的特性,这里将感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性称为轨迹的“姿态”,上述特定的轨迹都可以抽象成一条直线段或曲线段,这条直线段或曲线段就是轨迹跟踪系统所要跟踪的轨迹,这里将其称为轨迹特征线。
[0005] 本发明所需解决的技术问题是:提供一种通用的、能脱离示教信息独立运行、能实现自动跟踪且跟踪效果好的轨迹跟踪控制方法及轨迹跟踪系统,该轨迹跟踪控制方法及轨迹跟踪系统适用于传感器由传感器支链驱动、工具由工具支链驱动的场合。
[0006] 为解决上述问题,本发明所述的第一种轨迹跟踪方法是利用传感器事先检测轨迹的特点,采用坐标变换的方法,把传感器检测的轨迹原始位置信息标记到工件坐标系或世界坐标系中,形成一个由多个检测点构成的轨迹位置信息集合{Q},然后根据轨迹位置信息集合{Q}确定下一时刻传感器的检测点应该到达的正确位置及传感器的检测点位于该正确位置时传感器所呈现的姿态,以及根据轨迹位置信息集合{Q}确定下一时刻工具的目标位置应该到达的正确位置及工具的目标位置位于该正确位置时工具所呈现的姿态,该方法克服了检测点超前带来的误差。
[0007] 针对于工具来说,工具的姿态是指轨迹跟踪时不但工具上的目标位置要落在轨迹特征线上,而且工具的轴线要与轨迹特征线呈一定的度关系。同样针对于传感器来说,传感器的姿态是指轨迹跟踪时不但传感器上的检测点位置要落在轨迹特征线上,而且传感器的轴线要与轨迹特征线呈一定的角度关系。
[0008] 本发明所述的第一种轨迹跟踪控制方法,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息,并以传感器坐标系表达该位置信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置、以及工具在下一时刻的目标位置;
根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,将参考点Ref-Q的位置作为传感器在下一时刻的检测点位置;
根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置的具体确定方式如下:
设工具在当前时刻的目标位置为P点,P点的工具坐标系为P—X-tool’ Y-tool ’Z-tool’,过P点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-A,以参考点Ref-A为工具支链的跟踪目标,将参考点Ref-A的位置作为工具在下一时刻的目标位置;
(5)分别驱动传感器支链和工具支链:
驱动传感器枝链:根据参考点Ref-Q的位置信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool’轴的正方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;
驱动工具支链:根据参考点Ref-A的位置信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool’轴的正方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
[0009] 进一步地,前述的第一种轨迹跟踪控制方法,其中,步骤(4)中根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置与姿态、以及工具在下一时刻的目标位置与姿态;根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q-Xq-toolYq-toolZq-tool:过参考点Ref-Q做轨迹特征线的切线,以过轨迹特征线的切线和过与Zq -tool’轴平行的直线的平面为轨迹分割面;过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
根据轨迹位置信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t。
[0010] 进一步地,前述的第一种轨迹跟踪控制方法,其中,步骤(2)中用一个与Zq-tool’轴方向相同的向量作为检测点的姿态信息,并以传感器坐标系表达该姿态信息;步骤(3)中将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹姿态信息集合{N},轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{Q,N}。
[0011] 步骤(4)中,根据轨迹位置姿态信息集合{Q,N},确定位于参考点Ref-Q处的传感器坐标系Q-Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;步骤(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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t。
[0012] 进一步地,前述的第一种轨迹跟踪控制方法,其中,步骤(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的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻目标位置。
[0013] 进一步地,前述的第一种轨迹跟踪控制方法,其中,步骤(3)中将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的转化过程为:先将以传感器坐标系表达的位置信息转化为以传感器枝链的末端坐标系表达的位置信息,然后将以传感器枝链的末端表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息。
[0014] 实现第一种轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模和逆运动学模块,信息应用处理器中还包括信息存储区;传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置与姿态信息,并根据传感器当前时刻的检测点位置与姿态信息以及轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息,并根据工具当前时刻的目标位置与姿态信息以及轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置及姿态信息;
逆运动学模块调用信息应用处理器中计算出的传感器在下一时刻的检测点位置及姿态信息,并根据传感器在下一时刻的检测点位置及姿态信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
[0015] 本发明所述的第二种轨迹跟踪方法是利用传感器是事先检测轨迹的特点,采用坐标变换的方法,把传感器检测的轨迹原始位置信息标记到工件坐标系或世界坐标系中,形成一个由多个检测点构成的轨迹位置信息集合{Q},根据轨迹位置信息集合{Q}确定下一时刻传感器的检测点应该到达的正确位置及传感器的检测点位于该正确位置时传感器所呈现的姿态,然后进一步的求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,根据该第一位置及姿态偏差信息改变传感器当前位置及姿态,使传感器的检测点达到下一时刻的正确位置,那么传感器每前进一步后,都落在正确的位置及姿态上,就实现了理论上的无偏差的跟踪效果。同样根据轨迹位置信息集合{Q}确定下一时刻工具的目标位置应该到达的正确位置及工具的目标位置位于该正确位置时工具所呈现的姿态,然后进一步的求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,根据该第二位置及姿态偏差信息改变工具当前位置及姿态,使工具的目标位置达到下一时刻的正确位置,那么工具每前进一步后,都落在正确的位置及姿态上,就实现了理论上的无偏差的跟踪效果。而且该轨迹跟踪方法还克服了检测点超前带来的误差。
[0016] 本发明所述的第二种轨迹跟踪方法,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息,并以传感器坐标系表达该位置信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,所述第一位置及姿态偏差信息为侧向偏差△xq,或侧向偏差△xq与前进偏差△yq、高度偏差△zq、俯仰角度偏差δxq、前进方向角度偏差δzq中的至少一种相结合;
根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置及姿态,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,所述第二位置及姿态偏差信息为侧向偏差△x,或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合;
其中根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’Yq-tool’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做轨迹特征线的切线,以过轨迹特征线的切线和过与Zq -tool’轴平行的直线的平面为轨迹分割面;过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
其中,根据轨迹位置信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着预定方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置的传感器驱动信息为驱动信息Dq-tool,驱动信息Dq-tool为至少包含一个有效分量:△xq的向量,且驱动信息Dq-tool为至多包含五个有效分量:△xq,△yq,△zq,δxq,δzq的向量;其中在驱动信息Dq-tool不包含δxq或δzq时,所述预定方向为由于位移调整而自然地被确定的方向;在驱动信息Dq-tool包含δxq或δzq时,所述预定方向为Yq-tool轴的正方向;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着预定方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置的工具驱动信息为驱动信息D-tool,驱动信息D-tool为至少包含一个有效分量:△x的向量,且驱动信息D-tool为至多包含五个有效分量:
△x,△y,△z,δx,δz的向量;其中在驱动信息D-tool不包含δx或δz时,所述预定方向为由于位移调整而自然地被确定的方向;在驱动信息D-tool包含δx或δz时,所述预定方向为Y-tool轴的正方向;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
[0017] 进一步地,前述的第二种轨迹跟踪控制方法,其中,步骤(4)中第一位置及姿态偏差信息的具体求解过程为:设当前时刻检测点位置Q点的传感器坐标系为Q—Xq-tool’Yq-tool’Zq-tool’,δzq 是:轨迹特征线上位于参考点Ref-Q处的切线在过Xq-tool’轴及Yq-tool’轴的平面内的投影Sq-xy与Yq-tool’轴方向的夹角;
δxq 是:轨迹特征线上位于参考点Ref-Q处的切线在过Yq-tool’轴及Z-tool’轴的平面内的投影Sq-yz与Yq-tool’轴方向的夹角;
△xq 是:Q点到参考点Ref-Q的向量在Xq-tool’轴方向的投影长度;
△yq 是:Q点到参考点Ref-Q的向量在Yq-tool’轴方向的投影长度;
△zq 是:Q点到参考点Ref-Q的向量在Zq-tool’轴方向的投影长度。
[0018] 步骤(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’轴方向的投影长度。
[0019] 进一步地,前述的第二种轨迹跟踪控制方法,其中,步骤(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的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。
[0020] 进一步地,前述的第二种轨迹跟踪控制方法,其中,第二位置及姿态偏差信息的具体求解过程为:设当前时刻的目标位置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’轴方向的投影长度。
[0021] 进一步地,前述的第二种轨迹跟踪控制方法,其中,步骤(5)中的传感器驱动信息为驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息;步骤(5)中的工具驱动信息为驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
[0022] 进一步地,前述的第二种轨迹跟踪控制方法,其中,步骤(3)中将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的转化过程为:先将以传感器坐标系表达的位置信息转化为以传感器枝链的末端坐标系表达的位置信息,然后将以传感器枝链的末端坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息。
[0023] 实现第二种轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块,信息应用处理器中还包括信息存储区;传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置及姿态信息,并根据传感器当前时刻的检测点位置与姿态信息以及轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态信息,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息,并根据工具当前时刻的目标位置与姿态信息以及轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置及姿态信息,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息;
逆运动学模块调用信息应用处理器中计算出的传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,并根据第一位置及姿态偏差信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
[0024] 实现第一种轨迹跟踪控制方法的轨迹跟踪系统和实现第二种轨迹跟踪控制方法的轨迹跟踪系统,均可以进一步划分为传感系统和执行系统两部分,具体划分如下:第一种划分方式:传感器和信息检测处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;信息应用处理器、正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统向执行系统下传以传感器坐标系表达的轨迹的位置信息。
[0025] 第二种划分方式:传感器、信息检测处理器和信息应用处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器当前时刻的检测点位置与姿态信息、以及工具当前时刻的目标位置与姿态信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
[0026] 第三种划分方式:传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
[0027] 第四种划分方式:传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器到达下一时刻检测点位置及姿态时传感器支链的关节角信息、以及工具到达下一时刻目标位置及姿态时工具支链的关节角信息。
[0028] 上述第一种轨迹跟踪控制方法和第二种轨迹跟踪控制方法及各自对应的轨迹跟踪系统中涉及的传感器仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差,合称为轨迹的位置信息,该传感器无法感知轨迹所在工件表面在检测点处“朝向哪个方向”的特性,这里将感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性称为轨迹的“姿态”。本发明所述的第三种轨迹跟踪方法及对应的轨迹跟踪系统中涉及的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差,还能感知轨迹的姿态信息。本发明所述的第三种轨迹跟踪方法是利用传感器是事先检测轨迹的特点,采用坐标变换的方法,把传感器检测的轨迹原始位置及姿态信息标记到工件坐标系或世界坐标系中,形成一个由多个检测点构成的轨迹位置姿态信息集合{Q,N},然后根据轨迹位置姿态信息集合{Q,N}确定下一时刻传感器的检测点应该到达的正确位置及传感器的检测点位于该正确位置时传感器所呈现的姿态,以及根据轨迹位置姿态信息集合{Q,N}确定下一时刻工具的目标位置应该到达的正确位置及工具的目标位置位于该正确位置时工具所呈现的姿态,该方法克服了检测点超前带来的误差。
[0029] 本发明所述的第三种轨迹跟踪方法,传感器能感知轨迹的位置信息和姿态,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息,并以传感器坐标系表达该位置信息和姿态信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态、以及工具在下一时刻的目标位置及姿态;
根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过参考点Ref-Q的轨迹特征线的切线为Yq-tool轴, Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
根据轨迹位置姿态信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
[0030] 进一步地,前述的第三种轨迹跟踪控制方法,其中,步骤(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的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻的目标位置。
[0031] 进一步地,前述的第三种轨迹跟踪控制方法,其中,步骤(3)中将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息的转化过程为:先将以传感器坐标系表达的位置及姿态信息转化为以传感器枝链的末端坐标系表达的位置及姿态信息,然后将以传感器枝链的末端坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息。
[0032] 实现第三种轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,传感器能感知轨迹的位置信息和姿态,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块,信息应用处理器中还包括信息存储区;传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置及姿态信息,并根据传感器当前时刻的检测点位置及姿态信息、以及轨迹位置姿态信息集合{Q,N},确定传感器在下一时刻的检测点位置及姿态信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置及姿态信息,并根据工具当前时刻的目标位置及姿态信息、以及轨迹位置姿态信息集合{Q,N},确定工具在下一时刻的目标位置及姿态信息;
逆运动学模块调用信息应用处理器中计算出的传感器在下一时刻的检测点位置及姿态信息,并根据传感器在下一时刻的检测点位置及姿态信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在下一时刻的目标位置及姿态信息,并根据工具在下一时刻的目标位置及姿态信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
[0033] 本发明所述的第四种轨迹跟踪方法及对应的轨迹跟踪系统中涉及的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差,还能感知轨迹的姿态信息。本发明所述的第四种轨迹跟踪方法是利用传感器是事先检测轨迹的特点,采用坐标变换的方法,把传感器检测的轨迹原始位置及姿态信息标记到工件坐标系或世界坐标系中,形成一个由多个检测点构成的轨迹位置姿态信息集合{Q,N},根据轨迹位置姿态信息集合{Q,N}确定下一时刻传感器的检测点应该到达的正确位置及传感器的检测点位于该正确位置时传感器所呈现的姿态,然后进一步的求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,根据该第一位置及姿态偏差信息改变传感器当前位置及姿态,使传感器的检测点达到下一时刻的正确位置,那么传感器每前进一步后,都落在正确的位置及姿态上,就实现了理论上的无偏差的跟踪效果。同样根据轨迹位置姿态信息集合{Q,N}确定下一时刻工具的目标位置应该到达的正确位置及工具的目标位置位于该正确位置时工具所呈现的姿态,然后进一步的求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,根据该第二位置及姿态偏差信息改变工具当前位置及姿态,使工具的目标位置达到下一时刻的正确位置,那么工具每前进一步后,都落在正确的位置及姿态上,就实现了理论上的无偏差的跟踪效果。而且该轨迹跟踪方法还克服了检测点超前带来的误差。
[0034] 本发明所述的第四种轨迹跟踪方法,传感器能感知轨迹的位置信息和姿态,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系,具体包括以下步骤:(1)检测轨迹:通过传感器采集工件上的轨迹上检测点的原始轨迹信息;
(2)提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息,并以传感器坐标系表达该位置信息和姿态信息;
(3)标记轨迹信息:将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种;
(4)确定目标:根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,所述的第一位置及姿态偏差信息为侧向偏差△xq、前进偏差△yq、高度偏差△zq、俯仰角度偏差δxq、侧偏角偏差δyq、前进方向角度偏差δzq这六个有效分量中至少含△xq,δyq两个有效分量的位置及姿态偏差信息,且所述的第一位置及姿态偏差信息为至多包含△xq,△yq,△zq,δxq,δyq,δzq六个有效分量的位置及姿态偏差信息;
根据轨迹位置姿态信息集合{Q,N}来确定工具在下一时刻的目标位置及姿态,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,所述的第二位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x,δy两个有效分量的位置及姿态偏差信息,且所述的第二位置及姿态偏差信息为至多包含△x,△y,△z,δx,δy,δz六个有效分量的位置及姿态偏差信息;
根据据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的的检测点位置及姿态的具体确定方式如下:
设传感器在当前时刻的检测点位置为Q点,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;
以过参考点Ref-Q的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
根据据轨迹位置姿态信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置;使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置的传感器驱动信息为驱动信息Dq-tool,驱动信息Dq-tool为至少包含两个有效分量:△xq,δyq的向量,且驱动信息Dq-tool为至多包含六个有效分量:
△xq,△yq,△zq,δxq,δyq,δzq的向量;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置;使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置的工具驱动信息为驱动信息D-tool,驱动信息D-tool为至少包含两个有效分量:
△x,δy的向量,且驱动信息D-tool为至多包含六个有效分量:△x,△y,△z,δx,δy,δz的向量;
(6)循环上述各步骤,直至完成整条轨迹的跟踪。
[0035] 进一步地,前述的第四种轨迹跟踪控制方法,其中,步骤(4)中第一位置及姿态偏差信息的具体求解过程为:设当前时刻的检测点Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,△ xq 是:Q点到参考点Ref-Q的向量在Xq-tool’轴方向的投影长度;
△yq 是:Q点到参考点Ref-Q的向量在Yq-tool’轴方向的投影长度;
△zq 是:Q点到参考点Ref-Q的向量在Zq-tool’轴方向的投影长度;
δxq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Yq-tool’轴和Zq-tool’轴的平面上的投影线段Sq-yz与Yq-tool’轴的夹角;
δyq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Xq-tool’轴和Zq-tool’轴的平面上的投影线段Sq-xz与Zq-tool’轴的夹角;
δzq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Xq-tool’轴和Yq-tool’轴的平面上的投影线段Sq-xy与Yq-tool’轴的夹角。
[0036] 步骤(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’轴的夹角。
[0037] 进一步地,前述的第四种轨迹跟踪控制方法,步骤(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的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。
[0038] 进一步地,前述的第四种轨迹跟踪控制方法,其中,第二位置及姿态偏差信息的具体求解过程为:设当前时刻的目标位置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’轴的夹角。
[0039] 进一步地,前述的第四种轨迹跟踪控制方法,其中,步骤(5)中的传感器驱动信息为驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息;步骤(5)中的工具驱动信息为驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
[0040] 进一步地,前述的第四种轨迹跟踪控制方法,其中,步骤(3)中将以传感器坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息的转化过程为:先将以传感器坐标系表达的位置姿态信息转化为以传感器支链的末端坐标系表达的位置姿态信息,然后将以传感器支链的末端坐标系表达的位置姿态信息转化为以轨迹所附着的坐标系表达的位置姿态信息。
[0041] 实现第四种轨迹跟踪控制方法的轨迹跟踪系统,包括传感器、工具、传感器支链和工具支链,传感器能感知轨迹的位置信息和姿态,工具附着于工具支链上且工具由工具支链驱动,传感器附着于传感器支链上且传感器由传感器支链驱动,传感器位于工具前进方向的前侧,且传感器与工具之间的相对位置具有确定的几何关系;还包括信息检测处理器、信息应用处理器、正运动学模块和逆运动学模块,信息应用处理器中还包括信息存储区;传感器采集工件上轨迹的原始轨迹信息;
信息检测处理器根据传感器采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达;
正运动学模块获取工具的目标位置在当前时刻目标位置时工具支链的关节角信息,并根据该工具支链的关节角信息,计算出工具在当前时刻的目标位置与姿态信息;
正运动学模块获取传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息,并根据该传感器支链的关节角信息,计算出传感器在当前时刻的检测点位置与姿态信息;
信息应用处理器将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};
信息应用处理器调用正运动学模块中计算出的传感器当前时刻的检测点位置与姿态信息,并根据传感器当前时刻的检测点位置与姿态信息、以及轨迹位置姿态信息集合{Q,N},确定传感器在下一时刻的检测点位置及姿态信息,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息;
信息应用处理器调用正运动学模块中计算出的工具当前时刻的目标位置与姿态信息,并根据工具当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q},确定工具在下一时刻的目标位置及姿态信息,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息;
逆运动学模块调用信息应用处理器中计算出的传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,并根据第一位置姿态偏差信息,计算出传感器的检测点位置位于下一时刻检测点位置时传感器支链的关节角信息;
逆运动学模块调用信息应用处理器中计算出的工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具的目标位置位于下一时刻目标位置时工具支链的关节角信息。
[0042] 实现第三种轨迹跟踪控制方法的轨迹跟踪系统和实现第四种轨迹跟踪控制方法的轨迹跟踪系统,均可以进一步划分为传感系统和执行系统两部分,具体划分如下:第一种划分方式:传感器和信息检测处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;信息应用处理器、正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统向执行系统下传以传感器坐标系表达的轨迹的位置及姿态信息。
[0043] 第二种划分方式:传感器、信息检测处理器和信息应用处理器构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;正运动学模块、逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器当前时刻的检测点位置与姿态信息、以及工具当前时刻的目标位置与姿态信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
[0044] 第三种划分方式:传感器、信息检测处理器、信息应用处理器、正运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;逆运动学模块、传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器下一时刻的检测点位置与姿态信息、以及工具下一时刻的目标位置与姿态信息。
[0045] 第四种划分方式:传感器、信息检测处理器、信息应用处理器、正运动学模块、逆运动学模块构成传感系统,在传感系统中设置有第一对外通信器和第一信息接口端;传感器支链和工具支链构成执行系统,在执行系统中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接;传感系统接收执行系统上传的传感器的检测点位置在当前时刻检测点位置时传感器支链的关节角信息、以及工具的目标位置在当前时刻目标位置时工具支链的关节角信息;传感系统向执行系统下传传感器到达下一时刻检测点位置及姿态时传感器支链的关节角信息、以及工具到达下一时刻目标位置及姿态时工具支链的关节角信息。
[0046] 本发明的有益效果是:上述四种轨迹跟踪控制方法以及各自对应的轨迹跟踪系统,克服了检测点超前带来的误差,能脱离示教信息独立运行,在轨迹跟踪过程中能充分利用传感器检测的信息进行轨迹跟踪,不仅可以纠正工具及传感器的位置偏差,还可以纠正工具及传感器的姿态偏差,从而更好地实现轨迹跟踪目的。此外把轨迹跟踪系统进一步划分成传感系统和执行系统两部分,可以使跟踪系统的建立更加便利、灵活,而且便于由不同的专业厂家制造,提高性能、降低成本。附图说明
[0047] 图1是本发明所述的一种轨迹跟踪控制方法及轨迹跟踪系统中传感器与工具的结构示意图。
[0048] 图2是图1中去除传感器支链、工具支链后传感器与工具的工作原理示意图。
[0049] 图3是本发明所述的轨迹跟踪系统的信息关系框图
[0050] 图4是本发明所述的轨迹跟踪系统中传感系统和执行系统的第一种划分方式信息关系框图。
[0051] 图5是本发明所述的轨迹跟踪系统中传感系统和执行系统的第二种划分方式信息关系框图。
[0052] 图6是本发明所述的轨迹跟踪系统中传感系统和执行系统的第三种划分方式信息关系框图。
[0053] 图7是本发明所述的轨迹跟踪系统中传感系统和执行系统的第四种划分方式信息关系框图。
[0054] 图8是本发明实施例三中确定参考点Ref-A位置的示意图。
[0055] 图9是本发明实施例三中另一种确定种参考点Ref-B位置的示意图。
[0056] 图10是本发明实施例三中确定俯仰角度偏差δx的示意图。
[0057] 图11是本发明实施例三中确定前进方向角度偏差δy的示意图。
[0058] 图12是本发明实施例三中驱动信息、预定的任务轨迹信息、干预信息结合的综合驱动信息驱动执行系统的综合信息关系框图。
[0059] 图13是本发明实施例四中控制参考点Ref-A-next位置的示意图。
[0060] 图14是本发明实施例五中原始轨迹信息的表达示意图。
[0061] 图15是本发明实施例五中轨迹信息的全面表达示意图。
[0062] 图16是传感系统能不足的第一种结构示意图。
[0063] 图17是传感系统能力不足的第二种结构示意图。
[0064] 图18是欠自由度执行器的第一种结构示意图。
[0065] 图19是欠自由度执行器的第二种结构示意图。
[0066] 图20是欠自由度执行器的第三种结构示意图。
[0067] 图21是两个附着于世界坐标系的传感器的结构示意图。
[0068] 图22是图21的坐标系关系模型示意图。

具体实施方式

[0069] 下面结合附图及优选实施例对本发明所述的技术方案作进一步详细的说明。
[0070] 实施例一该轨迹跟踪控制方法用于传感器3和工具2分别由不同支链驱动的场合。参见图1所示,这里支链有两个:工具支链4和传感器支链5,工具2附着于工具支链4上且工具2由工具支链
4驱动,传感器3附着于传感器支链5上且传感器3由传感器支链5驱动,传感器3与工具2之间的相对位置具有确定的几何关系。传感器支链5和工具支链4可以是同一个执行器上的两个分支支链,也可以是由两个单独的执行器构成的支链。传感器支链5驱动传感器3前进的前进速度与工具支链4驱动工具2前进的前进速度可以相同,即传感器3与工具2能以同一前进速度前进,当然,传感器支链5驱动传感器3前进的前进速度与工具支链4驱动工具2前进的前进速度也可以不相同,只要保证在跟踪轨迹的全过程中,传感器3始终不被工具2追尾即可,即传感器支链5和工具支链2在分别驱动传感器3、工具2的过程中,传感器3始终位于工具2前进方向的前侧即可,一般将传感器支链5驱动传感器3前进的前进速度设置为不小于工具支链4驱动工具2前进的前进速度。
[0071] 本实施例所述的一种轨迹跟踪控制方法,具体包括以下步骤:第一步:检测轨迹:通过传感器3采集工件1上的轨迹上检测点的原始轨迹信息,这里涉及的传感器3能将检测到的视觉信息转换成原始轨迹信息,目前这类传感器一般为视觉传感器,本发明以典型的结构光视觉传感器为例进行说明。
[0072] 第二步:提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息,并以传感器坐标系表达该位置信息。
[0073] 为方便描述,这里以传感器支链5和工具支链4分别跟踪V形坡口焊缝、工具2为焊枪为例进行说明,如图1所示,工具支链4可以看成是驱动焊枪前进的机器人A,传感器支链5可以看成是驱动传感器3前进的机器人B,轨迹为焊缝8,轨迹特征线为焊缝特征线。本实施例虽然以焊接为例进行说明,但是本发明所述的轨迹跟踪控制方法及对应轨迹跟踪系统的应用不限于焊接领域,只要是引导工具沿着连续的几何轨迹运动的场合都可以适用,例如用于以特定几何边际为依据的材料切割、涂胶、打磨边角等任务。如图2所示,传感器检测到的焊缝横截面信息集中在平面M-sense内,这里将平面M-sense定义为检测截面,将坐标系O-XYZ定义为传感器坐标系。在焊缝8的截面上有一个点可以代表焊缝位置,例如角接焊缝截面上两个侧边的交点、V形坡口焊缝中V字最低处的交点等。我们将这个在检测截面上代表焊缝位置的点简称为检测点,在图2中标记为Q点,在焊接时,传感器3在焊缝8上方扫过,得到一系列的检测点:Q1、Q2、Q3……Qi。把代表焊枪目标位置的点定义为焊枪特征点P,焊枪特征点P一般在焊枪的轴线上、焊嘴之外的位置,离开焊嘴的距离等于电极伸出长度加上电弧长度。机器人A在传感器3的一系列检测点信息的引导下,带着焊枪运动,使焊枪特征点P沿着实际的焊缝8进行焊接,焊枪特征点P的位置姿态矩阵用Tt表示。任何焊缝都可以抽象成一条直线段或曲线段,就是焊接中焊枪特征点P要跟踪的轨迹,这里称其为焊缝特征线。
[0074] 如图2所示,对焊枪在当前目标位置P点的坐标系的确定如下:将焊枪在焊接过程中前进的方向定义为Y-tool’轴的正方向,另外两个坐标轴可根据需要实际加工过程定义,为方便描述,这里以惯用的方式定义另外两个坐标轴:X-tool’轴垂直Y-tool’轴与焊枪轴线构成的平面,根据X-tool’轴、Y-tool’轴就确定了Z-tool’轴,Z-tool’轴的正方向为离开焊枪、指向工件1的方向,最后再按照右手定则决定X-tool’轴的正方向。在Y-tool’轴正方向上,焊枪在当前目标位置P点的坐标系就是工具位于当前时刻目标位置P点的工具坐标系P—X-tool ’Y-tool ’Z-tool’。同样对传感器在当前检测点Q点的传感器坐标系Q—Xq-tool’ Yq-tool ’Zq-tool’的确认也是如此:将传感器在检测过程中前进的方向定义为Yq-tool’轴的正方向,另外两个坐标轴可根据需要实际加工过程定义,为方便描述,这里以惯用的方式定义另外两个坐标轴:Xq-tool’轴垂直Yq-tool’轴与传感器轴线构成的平面,根据Xq-tool’轴、Yq-tool’轴就确定了Zq-tool’轴,Zq-tool’轴的正方向为指向工件1的方向,最后再按照右手定则决定Xq-tool’轴的正方向。
[0075] 第三步:标记轨迹信息:将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。
[0076] 机器人A工作于世界坐标系,在世界坐标系中,焊枪特征点P的位置及姿态可以用其位置姿态矩阵Tt表示,是对焊枪的全面描述。除了以矩阵形式表达以外,显然,位置姿态也可以用其他形式表述,例如以欧拉角,或者四元素数等形式表达。如果工件不是附着在世界坐标系中,而是附着在其他坐标系中,那么轨迹也应该以所对应的工件坐标系表述,易通过机器人学知识实现,不再赘述。
[0077] 同样,机器人B工作于世界坐标系,在世界坐标系中,检测点Q的位置及姿态可以用其位置姿态矩阵Tc表示,是对传感器的全面描述。除了以矩阵形式表达意外,显然,位置姿态也可以用其他形式表述,例如以欧拉角,或者四元素数等形式表达。如果工件不是附着在世界坐标系中,而是附着在其他坐标系中,那么轨迹也应该以所对应的工件坐标系表述,易通过机器人学知识实现,不再赘述。
[0078] 这里以焊枪特征点P的位置及姿态以及检测点Q的位置及姿态为例,参见图2和图3所示,说明将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的具体转化过程为:第一、将以传感器坐标系表达的位置信息转化为以传感器支链末端坐标系表达的位置信息,采用如下公式:Q-t =(T-sensor)X(Q-s),其中向量Q-t表示检测点Q点在O-XYZ坐标系的描述,Q-s表示检测点Q点在传感器坐标系下的描述,T-sensor为传感器坐标系与O-XYZ坐标系之间的位置姿态变换关系矩阵;
第二、将以传感器支链的末端坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,采用如下公式:Q-w =(Tc)X Q-t =(Tc)X(T-sensor)X(Q-s),其中向量Q-w表示检测点Q点在世界坐标系或工件坐标系的描述,Tc表示传感器枝链参考点Q点的位置姿态矩阵。
[0079] 显然,上述四个式子中,点的坐标应该为齐次坐标,而非三维坐标,仅为了简洁未另行采用其它符号表达。随着传感器3的前进,就得到了用一系列的Q[i]点位置完整地描述的焊缝8,将这一系列的Q[i]点依次相连的线,就可以代表实际检测到的焊缝特征线,把这组对当前Q点的描述信息添加到轨迹位置信息存储区中,成为一个集合,记为{Q},为后续步骤备用。
[0080] 第四步:确定目标:根据轨迹位置信息集合{Q}来确定传感器3在下一时刻的检测点位置、以及工具2在下一时刻的目标位置。
[0081] 其中,根据轨迹位置信息集合{Q}来确定传感器3在下一时刻的检测点位置的具体确定方式如下:设传感器3在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链5的跟踪目标,将参考点Ref-Q的位置作为传感器3在下一时刻的检测点位置;
其中,根据轨迹位置信息集合{Q}来确定工具2在下一时刻的目标位置的具体确定确定方式如下:
设工具2在当前时刻的目标位置为P点,P点的工具坐标系为P—X-tool’ Y-tool ’Z-tool’,过P点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-A,以参考点Ref-A为工具支链4的跟踪目标,将参考点Ref-A的位置作为工具2在下一时刻的目标位置;
第五步:分别驱动传感器支链5和工具支链4。
[0082] 驱动传感器枝链5:根据参考点Ref-Q的位置信息、使传感器3的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool’轴的正方向向前移动一个步距△q=Vq*tq,其中tq为传感器3每次位置修正控制的调整间隔时间,Vq为传感器3的前进速度。
[0083] 驱动工具支链4:根据参考点Ref-A的位置信息、使工具2的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool’轴的正方向向前移动一个步距△=V*t,其中t为工具2每次位置修正控制的调整间隔时间,V为工具2的前进速度。
[0084] 第六步:循环上述各步骤,直至完成整条轨迹的跟踪。
[0085] 该轨迹跟踪控制方法中通过传感器采集的原始轨迹信息提取出检测点的位置信息,并未涉及提取姿态信息,因而这里传感器可以采用仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差的传感器,即只能提取轨迹的位置信息的传感器,该传感器无法感知轨迹所在工件表面在检测点处“朝向哪个方向”的特性,即无法提取轨迹的姿态信息。由于于检测环节没有姿态信息N,就不能有轨迹分割面,就不能调整姿态,这里可以利用当前工具Z -tool’轴的姿态信息补出缺失的N,从而就有轨迹分割面,就能调整工具的姿态了,对于调整传感器的姿态同样如此。这样就能根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态、以及工具在下一时刻的目标位置及姿态。
[0086] 根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q-Xq-toolYq-toolZq-tool:过参考点Ref-Q做轨迹特征线的切线,以过轨迹特征线的切线和过与Zq -tool’轴平行的直线的平面为轨迹分割面;过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态。
[0087] 根据轨迹位置信息集合{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的位置及姿态作为工具在下一时刻的目标位置及姿态。
[0088] 此时,第五步中分别驱动传感器支链和工具支链:驱动传感器枝链:根据参考点Ref-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t。
[0089] 除了利用当前工具Z -tool’轴的姿态信息补出缺失的N,从而调整工具姿态之外,可以采用其他方式。理想状态下,焊枪轴线与焊缝8之间的位置姿态关系是由焊接工艺确定的,那么焊接该焊缝8时,焊枪的轴线就是确定的。考察经过下一目标位置的一个微小焊缝段,就可以把该微小焊缝段近似地用经过下一目标位置的一小段直线代替,那么定义:经过下一目标位置的理想状态下的焊枪轴线、而且经过代表焊缝的直线的平面M-seam为焊缝分割面,通过上述方式补出缺失的N,从而就有轨迹分割面,就能调节工具的姿态了,对于调整传感器的姿态同样如此。这样就能根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态、以及工具在下一时刻的目标位置及姿态。
[0090] 根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置与姿态、以及根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置与姿态的第二种具体展开如下:第二步中用一个与Zq-tool’轴方向相同的向量作为检测点的姿态信息,并以传感器坐标系表达该姿态信息。这样做的前提条件是检测点Q与当前工具点P较接近,不会带来不能允许的姿态误差。
[0091] 第三步中将以传感器坐标系表达的姿态信息转化为以轨迹所附着的坐标系表达的姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹姿态信息集合{N},轨迹姿态信息集合与轨迹位置信息集合共同构成轨迹位置姿态信息集合{Q,N}。
[0092] 第四步中,根据轨迹位置姿态信息集合{Q,N},确定位于参考点Ref-Q处的传感器坐标系Q-Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态。
[0093] 第四步中,根据轨迹位置姿态信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。
[0094] 第五步中,分别驱动传感器支链和工具支链:驱动传感器枝链:根据参考点Ref-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t。
[0095] 以上各步骤的分析中均以焊枪轴线垂直于轨迹为例,仅仅是为了简洁。如果实际焊接工艺要求以某一个非90°的角度工作,通过矩阵变换不难把工具调整到该角度,从而使其适用该轨迹跟踪控制方法。
[0096] 以上描述中,没有涉及跟踪的最初寻找轨迹起点的时段。这时还没有积累任何轨迹信息,需要通过其他措施预先把传感器引导到实际轨迹的起点附近,然后发起沿着Yq-tool方向前进的搜索轨迹起点运动,要保证行进中传感器能够感知到轨迹的起点,在积累了至少两个检测点的轨迹信息后(获得一小段轨迹信息),才真正开始上述跟踪过程。
[0097] 以上方法仅仅是以V形坡口的焊缝为例叙述,但是不限于此。对于角接焊缝、搭接焊缝、对接焊缝等,只要定义出检测截面上的焊缝特征点P均可应用。对于涂胶等任务,也完全可以参考上述轨迹跟踪控制方法。
[0098] 上述描述是以工件位置固定为前提进行阐述的,当工件位置不固定时,可将传感器的坐标系和工具的坐标系都转换为以工件坐标系为参考基准的形式描述,此时就相当于工件位置固定的形式,因而工件位置固定和工件位置不固定都能采用本申请所述的轨迹跟踪控制方法。
[0099] 实现轨迹跟踪控制方法的轨迹跟踪系统,包括传感器3、工具2、传感器支链5和工具支链4,工具2附着于工具支链4上且工具2由工具支链4驱动,传感器3附着于传感器支链5上且传感器3由传感器支链5驱动,传感器3位于工具2前进方向的前侧,且传感器3与工具2之间的相对位置具有确定的几何关系;还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012,信息应用处理器1012中还包括信息存储区。参见图3所示,正运动学模块2011由正运动学模块A和正运动学模块B构成,逆运动学模块2012由逆运动学模块A和逆运动学模块B构成。
[0100] 传感器3采集工件上轨迹的原始轨迹信息;信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达;
正运动学A模块获工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息,并根据该工具支链4的关节角信息,计算出工具2在当前时刻的目标位置与姿态信息;
正运动学模块B获取传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息,并根据该传感器支链5的关节角信息,计算出传感器3在当前时刻的检测点位置与姿态信息;
信息应用处理器1012将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};
信息应用处理器1012调用正运动学模块B中计算出的传感器3当前时刻的检测点位置与姿态信息,并根据传感器3当前时刻的检测点位置与姿态信息以及轨迹位置信息集合{Q}来确定传感器3在下一时刻的检测点位置及姿态信息;
信息应用处理器1012调用正运动学模块A中计算出的工具2当前时刻的目标位置与姿态信息,并根据工具2当前时刻的目标位置与姿态信息以及轨迹位置信息集合{Q}来确定工具2在下一时刻的目标位置及姿态信息;
逆运动学模块B调用信息应用处理器B中计算出的传感器3在下一时刻的检测点位置及姿态信息,并根据传感器3在下一时刻的检测点位置及姿态信息,计算出传感器3的检测点位置位于下一时刻检测点位置时传感器支链5的关节角信息;
逆运动学模块A调用信息应用处理器A中计算出的工具2在下一时刻的目标位置及姿态信息,并根据工具2在下一时刻的目标位置及姿态信息,计算出工具2的目标位置位于下一时刻目标位置时工具支链4的关节角信息。
[0101] 轨迹跟踪系统可进一步划分为传感系统6和执行系统7两部分,将传感系统6与执行系统7的功能明确分开,两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方便描述,这里将传感系统6向执行系统7发送信息定义为“下传”,将执行系统7向传感系统6发送信息定义为“上传”,轨迹跟踪系统中传感系统7和执行系统7的具体划分如下:如图4所示,第一种划分方式以传感系统6提供信息的工作模式划分:传感器3和信息检测处理器1011构成传感系统6,在传感系统6中设置有第一对外通信器和第一信息接口端;
信息应用处理器1012、正运动学模块2011、逆运动学模块2012、传感器支链5和工具支链4构成执行系统7,在执行系统7中设置有第二对外通信器和第二信息接口端,传感系统6和执行系统7通过第一信息接口端和第二信息接口端电连接。
[0102] 工作时,传感系统6不需要上传信息,即执行系统7不需向传感系统6上传任何信息,仅需传感系统6向执行系统7下传以传感器坐标系表达的轨迹的位置信息即可。传感系统6主要承担轨迹位置信息采集、提取任务,执行系统7承担后续的信息处理工作。工作时,传感系统6通过第一对外通信器、第一信息接口端向执行系统7传送以传感器坐标系表达的轨迹位置信息,执行系统7通过第二信息接口端、第二对外通信器接收传感系统6传送的以传感器坐标系表达的轨迹位置信息并进行处理,处理后得到工具2位于下一时刻的目标位置及姿态信息及传感器3位于下一时刻的检测点位置及姿态信息,工具支链4、传感器支链5根据上述处理后得到的信息分别驱动工具2和传感器3,使工具2的目标位置由当前目标位置运动至下一时刻目标位置、传感器3的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置。
[0103] 该种对轨迹跟踪系统的划分方式的优点是:传感系统6结构简单,可以进一步缩小传感系统的体积。
[0104] 如图5所示,第二种划分方式以传感系统6直接驱动执行系统7的驱动模式划分:传感器3、信息检测处理器1011和信息应用处理器1012构成传感系统6,在传感系统6中设置有第一对外通信器和第一信息接口端;正运动学模块2011、逆运动学模块2012、传感器支链5和工具支链4构成执行系统7,在执行系统7中设置有第二对外通信器和第二信息接口端,传感系统6和执行系统7通过第一信息接口端和第二信息接口端电连接。
[0105] 工作时,传感系统6接收执行系统7上传的传感器3当前时刻的检测点位置与姿态信息、以及工具2当前时刻的目标位置与姿态信息;传感系统6向执行系统7下传传感器3下一时刻的检测点位置与姿态信息、以及工具2下一时刻的目标位置与姿态信息。
[0106] 如图6所示,第三种划分方式以传感系统6和执行系统7约定模型的驱动模式划分:传感器3、信息检测处理器1011、信息应用处理器1012、正运动学模块2011构成传感系统6,在传感系统6中设置有第一对外通信器和第一信息接口端;逆运动学模块2012、传感器支链
5和工具支链4构成执行系统7,在执行系统7中设置有第二对外通信器和第二信息接口端,传感系统6和执行系统7通过第一信息接口端和第二信息接口端电连接。
[0107] 工作时,传感系统6接收执行系统7上传的传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息、以及工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息;传感系统6向执行系统7下传传感器3下一时刻的检测点位置与姿态信息、以及工具2下一时刻的目标位置与姿态信息。由于上传的信息本身不足以直接求解工具2位于下一时刻的目标位置及姿态和传感器3位于下一时刻的检测点位置及姿态,因而需要事先约定传感器支链5和工具支链4的运动学模型,对传感系统6、执行系统7设置相互协调配合的参数。传感系统6根据上传的传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息、工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息、以及传感器支链5和工具支链4各自的运动学模型,求解出工具2位于下一时刻的目标位置及姿态信息、以及传感器3位于下一时刻的检测点位置及姿态,并反馈给执行系统7。
[0108] 如图7所示,第四种划分方式以传感系统6和执行系统7约定模型且传感系统6以关节角控制执行系统7的驱动模式划分:传感器3、信息检测处理器1011、信息应用处理器1012、正运动学模块2011、逆运动学模块2012构成传感系统6,在传感系统6中设置有第一对外通信器和第一信息接口端;传感器支链5和工具支链4构成执行系统7,在执行系统7中设置有第二对外通信器和第二信息接口端,传感系统和执行系统通过第一信息接口端和第二信息接口端电连接。
[0109] 工作时,传感系统6接收执行系统7上传的传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息、以及工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息;传感系统6向执行系统7下传传感器3到达下一时刻检测点位置及姿态时传感器支链5的关节角信息、以及工具2到达下一时刻目标位置及姿态时工具支链4的关节角信息。由于上传的信息本身不足以直接求解工具2位于下一时刻时传感器3到达下一时刻检测点位置及姿态时传感器支链5的关节角信息、以及工具2到达下一时刻目标位置及姿态时工具支链4的关节角信息,因而需要事先约工具支链4和传感器支链5的运动学模型,对传感系统6、执行系统7设置相互协调配合的参数。传感系统6根据上传的传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息、以及工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息、以及传感器支链5和工具支链4的运动学模型求解出工具2位于下一时刻的目标位置及姿态信息、以及传感器3位于下一时刻的检测点位置及姿态,进而求解出位于下一时刻时传感器3到达下一时刻检测点位置及姿态时传感器支链5的关节角信息、以及工具2到达下一时刻目标位置及姿态时工具支链4的关节角信息,并反馈给执行系统7。
[0110] 上述各种划分方式所对应的框图(图4、图5、图6、图7)中,为了简洁,均未区分各枝链的正逆运动学,甚至没有表示出传感器的信息,因为对于相对独立的传感器枝链,其功能、地位及处理方法,与“工具”是相同的。
[0111] 第二种划分方式、第三种划分方式和第四种划分方式工作时都需要接收执行系统7上传的同一时刻的运动学状态信息备用。该运动学状态信息的获取时机越接近传感器3采集时刻越好,其优点是可以更精准地利用上传信息。
[0112] 上述四种划分方式中涉及的第一信息接口端和第二信息接口端都可以做成信息接口凸端、信息接口凹端的形式,当第一信息接口端为凸端时,第二信息接口端则为凹端;当第一信息接口端为凹端时,第二信息接口端则为凸端。在实际使用过程中可将第一信息接口端和第二信息接口端电连接后的整体称为信息接口。
[0113] 实施例二本实施例与实施例一的不同之处在于:本实施例是在参考点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的位置及姿态信息作为工具2在下一时刻的目标位置及姿态;
步骤(5)中根据控制参考点Ref-A-next的位置及姿态信息、使工具2的目标位置由当前时刻的目标位置运动至下一时刻目标位置。
[0114] 轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例一相同,不再赘述。
[0115] 不以上述当前的控制参考点Ref-A为跟踪目标,而改为以控制间隔时间t后的控制参考点Ref-A-next为跟踪目标,这样做的优点是使用预测的下一个控制调整时刻将会出现的偏差进行控制,跟踪性能更好。理论上,所有被控制引导达到的位置姿态都是理想的,不会产生偏差,亦即P[i+1]点与Ref-A-next点重合,所说的“偏差”实际上是指当前时刻状态与下一时刻状态的差距。这时真正的误差仅在于轨迹信息检测中产生的误差,以及执行系统控制不到位产生的伺服控制误差。
[0116] 实施例三本实施例与实施例一的不同之处在于:本实施例在第四步确定目标中,在根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态、工具在下一时刻的目标位置及姿态的基础上,还进一步求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息、工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,关于该轨迹跟踪控制方法其他步骤、各步骤的具体展开均与实施例一相同,这里不再赘述。
[0117] 本实施例所述的一种轨迹跟踪控制方法包括以下步骤:第一步:检测轨迹:通过传感器3采集工件1上的轨迹检测点的原始轨迹信息。
[0118] 第二步:提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息,并以传感器坐标系表达该位置信息。
[0119] 第三步:标记轨迹信息:将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。
[0120] 将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的具体转化过程与实施例一相同,参见实施例一,这里不再赘述。
[0121] 第四步:确定目标:根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,所述第一位置及姿态偏差信息为侧向偏差△xq,或侧向偏差△xq与前进偏差△yq、高度偏差△zq、俯仰角度偏差δxq、前进方向角度偏差δzq中的至少一种相结合。根据轨迹位置信息集合{Q}来确定工具在下一时刻的目标位置及姿态,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,所述第二位置及姿态偏差信息为侧向偏差△x,或侧向偏差△x与前进偏差△y、高度偏差△z、俯仰角度偏差δx、前进方向角度偏差δz中的至少一种相结合。
[0122] 其中,根据轨迹位置信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。
[0123] 第二位置及姿态偏差信息的具体求解过程为:设当前时刻目标位置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’轴方向的投影长度。
[0124] 除了上述位置偏差之外,焊枪还可能存在姿态角度偏差,以工具坐标系为参考系,对实现机器人A轨迹控制的几个参量进行描述。如图8所示,过X-tool’轴、Y-tool’轴的平面图,其中线段S-xy为焊缝特征线在该平面上的投影。由于所考察的是焊枪特征点附近很小的尺寸范围,可以把焊缝特征线近似为直线段,所以也可以把S-xy看作直线段。那么需要焊枪绕着Z-tool’轴调整的角度偏差为δz,它是S-xy与Y-tool’轴的夹角。确定其他偏差量也需要在焊缝特征线上选择一个控制参考点,这样的点可以选择图8中的位置Ref-A。由于实际工作中角度δz很小,所以控制参考点也可以选择图9中的位置Ref-B,是过P、且过X-tool轴的平面上的参考点,这样做也不会给偏差计算带来较大误差。类似地,在计算其他偏差量时,由于焊缝特征线与每一个坐标轴的夹角都很小,所以控制参考点可以始终选择上述的Ref-A点,也可以选择Ref-B点,都不会造成大的误差。即:δz 是:轨迹特征线上位于参考点Ref-A或参考点Ref-B处的切线在过X-tool’轴及Y-tool’轴的平面内的投影S-xy与Y-tool’轴方向的夹角。
[0125] 如图10所示,过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’轴方向的夹角。。
[0126] 如图11所示,过X-tool’轴、Z-tool’轴的剖切面图,其中线段S-xz为焊缝特征线在该剖切面上的投影,那么需要焊枪绕着Y-tool’轴调整的角度偏差为δy,即S-xz与Z-tool’轴的夹角。图11还同时表达了以Ref-B为参考点时焊枪的位置调整量△x、△z。
[0127] 可以把上述偏差量综合地表达为一个六维向量,记为:D-tool = [△x,△y,△z,δx,δy,δz],其中△y=0。
[0128] 这也就是对工具轨迹偏差的完整描述,是在工具坐标系下的表述。由于它可以直接用于驱动机器人A,也可称为驱动信息。
[0129] 因为这里,△y=0,显然执行系统内部要补充出在Y-tool坐标轴方向上的驱动信息才能向前推进跟踪,机器人A工作时,就以焊接速度V沿着Y-tool坐标轴方向运动。这时,就可以认为跟踪是执行外部的驱动命令D-tool与内部的推进驱动命令的综合结果。
[0130] 所述工具跟踪轨迹的速度信息V,是当前时刻从执行系统接收的、或者是传感系统自行确定的、或者是执行系统设定的、或者是执行系统与传感系统之间约定的,均不影响本发明的核心思想。特殊情况下,执行系统忽略自身的速度调整信息△y,这时V=0,也能带来特殊需要的效果:如果再在Y-tool’轴方向上,把外部对机器人A运动速度的修正量改为△y=V*t,其中t为每次修正控制的调整间隔时间,则机器人仅靠外部的D-tool驱动就可以实现跟踪。这时机器人A就是一个完全靠外部传感系统驱动的设备了。如果以其他手段生成另一个各分量含义与D-tool对应等同的六维向量D-tool-2,则也可以用于驱动执行系统,如图10所述,甚至可以用各种驱动信息的结合信息驱动执行系统。
[0131] 其中,根据轨迹位置信息集合{Q}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:设传感器在当前时刻的检测点位置为Q点,Q点的传感器坐标系为Q—Xq-tool’Yq-tool’Zq-tool’,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做轨迹特征线的切线,以过轨迹特征线的切线和过与Zq -tool’轴平行的直线的平面为轨迹分割面;过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过Ref-Q点的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
第一位置及姿态偏差信息的具体求解过程和第二位置及姿态偏差信息的具体求解过程原理相同,这里就简单描述,具体展开可参见上述对第二位置及姿态偏差信息求解的具体展开描述。第一位置及姿态偏差信息的具体求解过程为:设当前时刻检测点位置Q点的传感器坐标系为Q—Xq-tool’Yq-tool’Zq-tool’,
δzq 是:轨迹特征线上位于参考点Ref-Q处的切线在过Xq-tool’轴及Yq-tool’轴的平面内的投影Sq-xy与Yq-tool’轴方向的夹角;
δxq 是:轨迹特征线上位于参考点Ref-Q处的切线在过Yq-tool’轴及Z-tool’轴的平面内的投影Sq-yz与Yq-tool’轴方向的夹角;
△xq 是:Q点到参考点Ref-Q的向量在Xq-tool’轴方向的投影长度;
△yq 是:Q点到参考点Ref-Q的向量在Yq-tool’轴方向的投影长度;
△zq 是:Q点到参考点Ref-Q的向量在Zq-tool’轴方向的投影长度;
第五步:分别驱动传感器支链和工具支链:
驱动传感器枝链:根据参考点Ref-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着预定方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度;使传感器的检测点位置由当前时刻的检测点位置运动至下一时刻检测点位置的传感器驱动信息为驱动信息Dq-tool,驱动信息Dq-tool为至少包含一个有效分量:△xq的向量,且驱动信息Dq-tool为至多包含五个有效分量:△xq,△yq,△zq,δxq,δzq的向量;其中在驱动信息Dq-tool不包含δxq或δzq时,所述预定方向为由于位移调整而自然地被确定的方向;在驱动信息Dq-tool包含δxq或δzq时,所述预定方向为Yq-tool轴的正方向。
[0132] 驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着预定方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度;使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置的工具驱动信息为驱动信息D-tool,驱动信息D-tool为至少包含一个有效分量:△x的向量,且驱动信息D-tool为至多包含五个有效分量:△x,△y,△z,δx,δz的向量;其中在驱动信息D-tool不包含δx或δz时,所述预定方向为由于位移调整而自然地被确定的方向;在驱动信息D-tool包含δx或δz时,所述预定方向为Y-tool轴的正方向。
[0133] 实现完全自动控制的轨迹跟踪方案中,所谓“完全自动控制”是指可以不依靠预先编写的轨迹描述程序(例如示教程序),而能自动根据轨迹特征自动调整跟踪。这时图12中的“示教轨迹”信息不起作用。即:传感器驱动信息可以仅仅是单独的驱动信息Dq-tool,工具驱动信息可以仅仅是单独的驱动信息D-tool。
[0134] 在不能确工具认跟踪过程中无干涉碰撞的环境中,也可以通过简单的示教信息,规避那些可预见的障碍,这时利用的是图12中的“示教轨迹”信息,由该信息决定工具的姿态、或者姿态中的一部分自由度,例如δy,或者δy与δz,可以确保不碰撞。即使是这种情况,示教信息也是非常简化的,而不是常规的决定行进轨迹的复杂示教信息。即工具驱动信息也可以是驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。同理,传感器驱动信息也可以是驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
[0135] 因而第五步中的工具驱动信息可以仅仅是驱动信息D-tool,也可以是驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。传感器驱动信息可以仅仅是驱动信息Dq-tool,也可以是驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
[0136] 如果执行系统接收的是外部传感系统以机器人A的工具坐标系为参考系定义的轨迹修正信息,执行系统还可以接收人工操控而形成的、以机器人A的工具坐标系为参考系定义的轨迹修正信息,这一信息与接收的外部传感系统提供的偏差控制信息结合,然后以结合后的信息进行轨迹修正;偏差信息结合的方法根据任务不同而确定。图中符号Σ并非代表简单地将各个控制向量相加,而是代表信息融合,可以有选择、可以增强或削弱控制量的比例等变化。例如,人采用一个六维力/力矩传感器,生成一个相对于操作者的最多6维的信息:人手推动力/力矩传感器,就发出正的△y信息,拉动就发出负的△y信息,使△y的大小与推拉的力度成正比;把这一信息与D-tool相应的分量相加,或者加权相加,那么就可以实现人对跟踪速度的干预;同理,可以用侧向推拉力/力矩传感器,实现临时使工具偏离跟踪轨迹的调整;扭动力/力矩传感器产生的扭矩则可用于对工具姿态的调整。当然也可以“过滤”掉不希望产生的控制信息,从而抑制不希望变化的信息。为了避免不希望的某一维度上出现干扰,可以将其屏蔽掉,例如忽略不经意间产生的对工具的侧偏调整信息。这就是偏差信息结合的原理。以类似于游戏操纵杆的系统也可以用于引发这样的人为干预信息。同理对机器人B也是如此,这里不再赘述。
[0137] 与对图4、图5、图6、图7的处理类似,为了简洁,图12未给出各枝链的细分、未示出传感器。
[0138] 第六步:循环上述各步骤,直至完成整条轨迹的跟踪。
[0139] 本实施例中实现轨迹跟踪控制方法的轨迹跟踪系统,包括传感器3、工具2、传感器支链5和工具支链4,工具2附着于工具支链4上且工具2由工具支链4驱动,传感器3附着于传感器支链5上且传感器3由传感器支链5驱动,传感器3位于工具2前进方向的前侧,且传感器3与工具2之间的相对位置具有确定的几何关系;还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012,信息应用处理器1012中还包括信息存储区;如图3所示,正运动学模块2011由正运动学模块A和正运动学模块B构成,逆运动学模块2012由逆运动学模块A和逆运动学模块B构成。
[0140] 传感器3采集工件1上轨迹的原始轨迹信息;信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置信息、并以传感器坐标系表达;
正运动学A模块获工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息,并根据该工具支链4的关节角信息,计算出工具2在当前时刻的目标位置与姿态信息;
正运动学模块B获取传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息,并根据该传感器支链5的关节角信息,计算出传感器3在当前时刻的检测点位置与姿态信息;
信息应用处理器均能将以传感器坐标系表达的轨迹的位置信息转化为以轨迹所附着的坐标系表达的位置信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置信息集合{Q};
信息应用处理器调用正运动学模块B中计算出的传感器3当前时刻的检测点位置及姿态信息,并根据传感器3当前时刻的检测点位置与姿态信息以及轨迹位置信息集合{Q}来确定传感器3在下一时刻的检测点位置及姿态信息,并求出传感器3在当前时刻检测点位置与传感器3在下一时刻检测点位置之间的第一位置及姿态偏差信息;
信息应用处理器调用正运动学模块A中计算出的工具2当前时刻的目标位置与姿态信息,并根据工具2当前时刻的目标位置与姿态信息以及轨迹位置信息集合{Q}来确定工具2在下一时刻的目标位置及姿态信息,并求出工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的第二位置及姿态偏差信息;
逆运动学模块B调用信息应用处理器B中计算出的传感器3在当前时刻检测点位置与传感器3在下一时刻检测点位置之间的第一位置及姿态偏差信息,并根据第一位置及姿态偏差信息,计算出传感器3的检测点位置位于下一时刻检测点位置时传感器支链5的关节角信息;
逆运动学模块A调用信息应用处理器A中计算出的工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具2的目标位置位于下一时刻目标位置时工具支链4的关节角信息。
[0141] 轨迹跟踪系统可进一步划分为传感系统6和执行系统7两部分,将传感系统6与执行系统7的功能明确公开,两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方面描述,这里将传感系统6向执行系统7发送信息定义为“下传”,将执行系统7向传感系统6发送信息定义为“上传”,轨迹跟踪系统中传感系统6和执行系统7的具体划分与实施例一中四种划分方式相同,具体工作方式也相同,参见实施例一,这里不再赘述。
[0142] 实施例四本实施例与实施例三的不同之处在于:本实施例是在参考点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中的至少一种相结合。
[0143] 其中,第二位置及姿态偏差信息的具体求解过程为:设当前时刻的目标位置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’轴方向的投影长度。
[0144] 步骤(5)中根据控制参考点Ref-A-next的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。
[0145] 轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例三相同,不再赘述。
[0146] 不以上述当前的控制参考点Ref-A为跟踪目标,而改为以控制间隔时间t后的控制参考点Ref-A-next为跟踪目标,则△x、△y、δz的意义如图13所示。这时,计算其他控制量的参考点也要做相应修改(仅须把Ref-A-next当做Ref-A点,进行上述计算),这里特意用曲线轨迹表明δz的值与以Ref-A为跟踪目标时的不同。图13中,△是Ref-A与Ref-A-next之间在三维空间上的距离。这样做的优点是,使用预测的下一个控制调整时刻将会出现的偏差进行控制,跟踪性能更好:理论上,所有被控制引导达到的位置姿态都是理想的,不会产生偏差,亦即P[i+1]点与Ref-A-next点重合,所说的“偏差”实际上是指当前时刻状态与下一时刻状态的差距。这时真正的误差仅在于轨迹信息检测中产生的误差,以及执行系统控制不到位产生的伺服控制误差。
[0147] 上述实施例一、实施例二、实施例三和实施例四中的轨迹跟踪方法及各自对应的轨迹跟踪系统中涉及的传感器3只要具有感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差的能力即可,即便该传感器无法感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性,在这里称其为轨迹的“姿态”,也能很好地实现轨迹跟踪。
[0148] 下述实施例五、实施例六、实施例七和实施例八中的轨迹跟踪方法及各自对应的轨迹跟踪系统中涉及的传感器则需要具有既能感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差,也能感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性,在这里称其为轨迹的“姿态”的能力。
[0149] 实施例五本实施例与实施例一的不同之处在于:本实施例中的传感器不仅能够感知目标轨迹在离开工具的方向上的偏差、在轨迹的两侧方向上的偏差,合称为轨迹的位置信息,还能感知轨迹所在工件是表面在检测点处“朝向哪个方向”的特性,即姿态信息。关于该轨迹跟踪控制方法各步骤的具体展开均与实施例一相同,这里不再赘述。
[0150] 本实施例中所述的一种轨迹跟踪控制方法包括以下步骤:第一步:检测轨迹:通过传感器3采集工件1上轨迹上检测点的原始轨迹信息。
[0151] 第二步:提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息,并以传感器坐标系表达该位置信息和姿态信息。
[0152] 在焊接时,传感器3在焊缝4上方扫过,得到一系列的检测点Q,机器人在传感器的一系列检测点信息的引导下,带着焊枪运动,使焊枪特征点P沿着实际的焊缝进行焊接,焊枪特征点P的位置姿态矩阵用Tt表示。
[0153] 任何焊缝都可以抽象成一条直线段或曲线段,就是焊接中要跟踪的轨迹,这里称其为焊缝特征线。那么,所谓焊缝跟踪就是在焊枪运动过程中,使焊枪特征点P不脱离焊缝特征线,而且使焊枪轴线与焊缝特征线保持焊接工艺需要的特定角度关系。
[0154] 理想状态下,焊枪轴线与焊缝之间的位置姿态关系是由焊接工艺确定的,那么焊接该焊缝时,焊枪的轴线就是确定的。考察经过下一目标位置的一个微小焊缝段,为方便描述,假设下一目标位置为Q点,就可以把该微小焊缝段近似地用经过Q点的一小段直线代替,那么定义:经过Q点的理想状态下的焊枪轴线、而且经过代表焊缝的直线的平面M-seam为焊缝分割面。传感器的检测点在该Q点检测到的是在检测截面M-sense内的焊缝的断面轮廓,可以根据该轮廓计算出焊缝分割面M-seam与检测截面M-sense的交线,将该交线表示为矢量N-seam,它可以代表焊缝分割面的方向,称为焊缝姿态向量,或简称焊缝姿态。
[0155] 实际上,除非检测截面M-sense与焊缝分割面M-seam垂直,否则过Q点可以得到多个焊缝姿态向量N-seam。但是这并不影响N-seam的使命:就是为了通过N-seam与一小段轨迹共同确定过下一目标位置所决定的焊缝分割面M-seam,所以,只要M-sense与M-seam相交,得到的N-seam,都可以起到同样的作用。同理,对于传感器检测点的分析也同上所述。
[0156] 图2是传感器坐标系的坐标轴平行于焊缝、而且无偏差的一种特殊状态,而图14则代表着一般的状态,图中所示为检测截面M-sense上测得的内容,是一系列的点或线段描述的焊缝轮廓,信息很丰富。我们要提取最有效、尽量少的信息,来代表焊缝的特性。我们选择Q点的位置信息和向量N-seam共同代表焊缝的特性。可见,如果用向量Q-s表示Q点在传感器坐标系下的描述,则Q-s = [Xs,0, Zs]。
[0157] 根据几何关系,把点a到点e的向量按顺时针方向旋转90°,就可以用来表达向量N-seam,即为 [ -dz/L,0,dx/L]。用向量N-s表示N-seam在传感器坐标系下的描述,则N-s = [ -dz/L,0,dx/L]。
[0158] 可见,在需要向执行系统发送以传感器坐标系描述的焊缝信息Q-s、N-s时,可以仅传输4个数据,因为它们的坐标表达中有2个0,可以默认地获得。进一步,还可以把N-s归一化为单位向量,那么,还可以通过归一化减少1个传输数据。当然,姿态信息N-s也可以用1个角度δy来表达。
[0159] 第三步:标记轨迹信息:将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。
[0160] 机器人工作于世界坐标系,在世界坐标中,其焊枪特征点的位置及姿态可以用其位置姿态矩阵表示,是对焊枪、传感器的全面描述。除了以矩阵形式表达以外,显然,位置姿态也可以用其他形式表述,例如以欧拉角,或者四元素数等形式表达。
[0161] 参见图2和图3所示,将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息的转化过程为:第一、将以传感器坐标系表达的位置及姿态信息转化为以传感器枝链的末端表达的位置及姿态信息,采用如下公式:
Q-t =(T-sensor)X(Q-s)
N-t =(T-sensor)X(N-s)
其中向量Q-t表示下一时刻的目标位置在O-XYZ坐标系的描述,Q-s表示下一时刻的目标位置在传感器坐标系下的描述,T-sensor为传感器坐标系与O-XYZ坐标系之间的位置姿态变换关系矩阵;向量N-s表示向量N-seam在传感器坐标系下的描述,向量N-t表示向量N-seam在工具坐标系下的描述;
第二、将以传感器枝链的末端表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,采用如下公式:
Q-w =(Tc)X Q-t =(Tc)X(T-sensor)X(Q-s)
N-w =(Tc)X N-t =(Tc)X(T-sensor)X(N-s)
其中向量Q-w表示目标位置在世界坐标系或工件坐标系的描述,Tt表示O点位于当前时刻的位置姿态矩阵,向量N-w表示向量N-seam在世界坐标系或工件坐标系下的描述。
[0162] 显然,上述四个式子中,点的坐标应该为齐次坐标,而非三维坐标,仅为了简洁未另行采用其它符号表达。因此,如图13所示,随着焊枪带着传感器前进,就得到了用一系列的Q[i]点位置及其附带的向量N-seam(简记为N[i])完整地描述的焊缝,将这一系列的Q点依次相连的线,就可以代表实际检测到的焊缝特征线;由各个N-seam构成的曲面就可以代表实际检测到的焊缝分割面。把这组对当前Q点的描述信息添加到轨迹位置姿态信息存储区中,成为一个集合,记为{Q,N},为后续步骤备用。
[0163] 如果工件不是附着在世界坐标系中,而是附着在其他坐标系中,那么轨迹也应该以所对应的工件坐标系表述,易通过机器人学知识实现,不再赘述。
[0164]如图15所示的是在一个扭曲的、非可展开曲面上进行焊接的情况,可以代表对任意空间曲线轨迹的处理方法。为了不掩盖焊缝特征线,图中将这一系列向量N-seam的方向倒置。
焊枪在焊接过程中前进的方向定义为工具坐标系的Y-tool轴的正方向。理论上另外两个坐标轴可以任意定义,为了叙述简明,这里以惯用的方式做如下假设:X-tool轴垂直于 Y-tool轴与焊枪轴线构成的平面,而Z-tool的正方向,为离开焊枪、指向工件的方向,最后再按照右手定则决定X-tool的正方向。
[0165] 第四步:确定目标:根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态、以及工具在下一时刻的目标位置及姿态。
[0166] 其中,根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态的具体确定方式如下:设传感器在当前时刻的检测点位置为Q点,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链的跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;以过参考点Ref-Q的轨迹特征线的切线为Yq-tool轴, Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态。
[0167] 其中,根据轨迹位置姿态信息集合{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-Q的位置及姿态信息、使传感器的检测点位置由当前时刻的检测点位置运动至Ref-Q点,并以Ref-Q为起点沿着Yq-tool轴的正方向向前移动一个步距△q=Vq*tq,其中tq为传感器每次位置修正控制的调整间隔时间,Vq为传感器的前进速度。
[0168] 驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至Ref-A点,并以Ref-A为起点沿着Y-tool轴的正方向向前移动一个步距△=V*t,其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度。
[0169] 第六步:循环上述各步骤,直至完成整条轨迹的跟踪。
[0170] 本实施例中实现轨迹跟踪控制方法的轨迹跟踪系统,包括传感器3、工具2、传感器支链5和工具支链4,工具2附着于工具支链4上且工具2由工具支链4驱动,传感器3附着于传感器支链5上且传感器3由传感器支链5驱动,传感器3位于工具2前进方向的前侧,且传感器3与工具2之间的相对位置具有确定的几何关系;还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012,信息应用处理器1012中还包括信息存储区;如图3所示,正运动学模块2011由正运动学模块A和正运动学模块B构成,逆运动学模块2012由逆运动学模块A和逆运动学模块B构成。
[0171] 传感器3采集工件上轨迹的原始轨迹信息;信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达;
正运动学A模块获工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息,并根据该工具支链4的关节角信息,计算出工具2在当前时刻的目标位置与姿态信息;
正运动学模块B获取传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息,并根据该传感器支链5的关节角信息,计算出传感器3在当前时刻的检测点位置与姿态信息;
信息应用处理器均能将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};
信息应用处理器调用正运动学模块B中计算出的传感器3当前时刻的检测点位置及姿态信息,并根据传感器3当前时刻的检测点位置及姿态信息、以及轨迹位置姿态信息集合{Q,N},确定传感器3在下一时刻的检测点位置及姿态信息;
信息应用处理器调用正运动学模块A中计算出的工具2当前时刻的目标位置及姿态信息,并根据工具2当前时刻的目标位置及姿态信息、以及轨迹位置姿态信息集合{Q,N},确定工具2在下一时刻的目标位置及姿态信息;
逆运动学模块B调用信息应用处理器B中计算出的传感器3在下一时刻的检测点位置及姿态信息,并根据传感器3在下一时刻的检测点位置及姿态信息,计算出传感器3的检测点位置位于下一时刻检测点位置时传感器支链5的关节角信息;
逆运动学模块A调用信息应用处理器A中计算出的工具2在下一时刻的目标位置及姿态信息,并根据工具2在下一时刻的目标位置及姿态信息,计算出工具2的目标位置位于下一时刻目标位置时工具支链4的关节角信息。
[0172] 轨迹跟踪系统可进一步划分为传感系统6和执行系统7两部分,将传感系统6与执行系统7的功能明确公开,两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方面描述,这里将传感系统6向执行系统7发送信息定义为“下传”,将执行系统7向传感系统6发送信息定义为“上传”,轨迹跟踪系统中传感系统6和执行系统7的具体划分与实施例一中四种划分方式相同,具体工作方式也相同,参见实施例一,这里不再赘述。
[0173] 实施例六本实施例与实施例五的不同之处在于:本实施例是在参考点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的位置及姿态信息作为工具在下一时刻的目标位置及姿态;
步骤(5)中执行器根据控制参考点Ref-A-next的位置及姿态信息、使工具的目标位置由当前时刻的目标位置运动至下一时刻的目标位置。
[0174] 轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例五相同,不再赘述。
[0175] 实施例七本实施例与实施例五的不同之处在于:本实施例在第四步确定目标中,在根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置的基础上,还进一步求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息;在根据轨迹位置姿态信息集合{Q,N}来确定工具在下一时刻的目标位置的基础上,还进一步求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息。关于该轨迹跟踪控制方法其他步骤的具体展开均与实施例五相同,这里不再赘述。
[0176] 关于该轨迹跟踪控制方法的适用性及其他步骤的具体展开均与实施例五相同,这里不再赘述。
[0177] 本实施例中所述的一种轨迹跟踪控制方法包括以下步骤:第一步:检测轨迹:通过传感器3采集工件上的轨迹上检测点的原始轨迹信息;
第二步:提取轨迹信息:从原始轨迹信息中提取出检测点的位置信息和在该检测点处轨迹分割面的姿态信息,并以传感器坐标系表达该位置信息和姿态信息;
第三步:标记轨迹信息:将以传感器坐标系表达的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};其中工件位置不固定时轨迹所附着的坐标系为工件坐标系,工件位置固定时轨迹所附着的坐标系为工件坐标系或世界坐标系中的一种。
[0178] 将以传感器坐标系表达的位置信息转化为以轨迹所附着的坐标系表达的位置信息的具体转化过程与实施例五相同,参见实施例五,这里不再赘述。
[0179] 第四步:确定目标:根据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的检测点位置及姿态,并求出传感器在当前时刻检测点位置与传感器在下一时刻检测点位置之间的第一位置及姿态偏差信息,所述的第一位置及姿态偏差信息为侧向偏差△xq、前进偏差△yq、高度偏差△zq、俯仰角度偏差δxq、侧偏角偏差δyq、前进方向角度偏差δzq这六个有效分量中至少含△xq,δyq两个有效分量的位置及姿态偏差信息,且所述的第一位置及姿态偏差信息为至多包含△xq,△yq,△zq,δxq,δyq,δzq六个有效分量的位置及姿态偏差信息;根据轨迹位置姿态信息集合{Q,N}来确定工具在下一时刻的目标位置及姿态,并求出工具在当前时刻目标位置与工具在下一时刻目标位置之间的第二位置及姿态偏差信息,所述的第二位置及姿态偏差信息为侧向偏差△x、前进偏差△y、高度偏差△z、俯仰角度偏差δx、侧偏角偏差δy、前进方向角度偏差δz这六个有效分量中至少含△x,δy两个有效分量的位置及姿态偏差信息,且所述的第二位置及姿态偏差信息为至多包含△x,△y,△z,δx,δy,δz六个有效分量的位置及姿态偏差信息。
[0180] 其中,根据据轨迹位置姿态信息集合{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的位置及姿态信息作为工具在下一时刻的目标位置及姿态。
[0181] 第二位置及姿态偏差信息的具体求解过程为:设位于当前时刻目标位置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’轴的夹角。
[0182] 其中,根据据轨迹位置姿态信息集合{Q,N}来确定传感器在下一时刻的的检测点位置及姿态的具体确定方式如下:设传感器在当前时刻的检测点位置为Q点,过Q点做与轨迹特征线垂直的平面,该平面与轨迹特征线的交点为参考点Ref-Q,以参考点Ref-Q为传感器支链跟踪目标,确定位于参考点Ref-Q处的传感器坐标系Q—Xq-toolYq-toolZq-tool:过参考点Ref-Q做垂直于轨迹特征线的平面,该平面与轨迹分割面的交线为Zq-tool轴,Zq-tool轴的正方向指向工件表面;
以过参考点Ref-Q的轨迹特征线的切线为Yq-tool轴,Yq-tool轴的正方向指向传感器前进方向;根据坐标系的左手定则或右手定则确定Xq-tool轴及Xq-tool轴的正方向;将参考点Ref-Q的位置及姿态作为传感器在下一时刻的检测点位置及姿态;
第一位置及姿态偏差信息的具体求解过程为:设位于当前时刻的检测点Q点的传感器坐标系为Q—Xq-tool’ Yq-tool ’Zq-tool’,
△ xq 是:Q点到参考点Ref-Q的向量在Xq-tool’轴方向的投影长度;
△yq 是:Q点到参考点Ref-Q的向量在Yq-tool’轴方向的投影长度;
△zq 是:Q点到参考点Ref-Q的向量在Zq-tool’轴方向的投影长度;
δxq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Yq-tool’轴和Zq-tool’轴的平面上的投影线段Sq-yz与Yq-tool’轴的夹角;
δyq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Xq-tool’轴和Zq-tool’轴的平面上的投影线段Sq-xz与Zq-tool’轴的夹角;
δzq是:将位于参考点Ref-Q处的轨迹特征线直线化或过参考点Ref-Q作轨迹特征线的切线,轨迹特征线直线化或轨迹特征线的切线在过Xq-tool’轴和Yq-tool’轴的平面上的投影线段Sq-xy与Yq-tool’轴的夹角;
第五步:驱动工具:分别驱动传感器支链和工具支链:
驱动传感器枝链:根据参考点Ref-Q的位置及姿态信息、使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置;使传感器的检测点位置由当前时刻检测点位置运动至下一时刻检测点位置的传感器驱动信息为驱动信息Dq-tool,驱动信息Dq-tool为至少包含两个有效分量:△xq,δyq的向量,且驱动信息Dq-tool为至多包含六个有效分量:
△xq,△yq,△zq,δxq,δyq,δzq的向量;
驱动工具支链:根据参考点Ref-A的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置;使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置的工具驱动信息为驱动信息D-tool,驱动信息D-tool为至少包含两个有效分量:
△x,δy的向量,且驱动信息D-tool为至多包含六个有效分量:△x,△y,△z,δx,δy,δz的向量;
传感器驱动信息为可以是单独的驱动信息Dq-tool,也可以是驱动信息Dq-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息;工具驱动信息可以是单独的驱动信息D-tool,也可以是驱动信息D-tool与预定的任务轨迹信息、干预信息中的至少一种相结合的综合驱动信息。
[0183] 第六步:循环上述各步骤,直至完成整条轨迹的跟踪。
[0184] 实现轨迹跟踪控制方法的轨迹跟踪系统,包括传感器3、工具2、传感器支链5和工具支链4,工具2附着于工具支链4上且工具2由工具支链4驱动,传感器3附着于传感器支链5上且传感器3由传感器支链5驱动,传感器3位于工具2前进方向的前侧,且传感器3与工具2之间的相对位置具有确定的几何关系;还包括信息检测处理器1011、信息应用处理器1012、正运动学模块2011和逆运动学模块2012,信息应用处理器1012中还包括信息存储区;如图3所示,正运动学模块2011由正运动学模块A和正运动学模块B构成,逆运动学模块2012由逆运动学模块A和逆运动学模块B构成,信息应用处理器1012由信息应用处理器A和信息应用处理器B构成。
[0185] 传感器3采集工件上轨迹的原始轨迹信息;信息检测处理器1011根据传感器3采集的原始轨迹信息提取出轨迹的位置及姿态信息、并以传感器坐标系表达;
正运动学A模块获工具2的目标位置在当前时刻目标位置时工具支链4的关节角信息,并根据该工具支链4的关节角信息,计算出工具2在当前时刻的目标位置与姿态信息;
正运动学模块B获取传感器3的检测点位置在当前时刻检测点位置时传感器支链5的关节角信息,并根据该传感器支链5的关节角信息,计算出传感器3在当前时刻的检测点位置与姿态信息;
信息应用处理器A和信息应用处理器B将以传感器坐标系表达的轨迹的位置及姿态信息转化为以轨迹所附着的坐标系表达的位置及姿态信息,然后标记于轨迹所附着的坐标系中,并存储于信息存储区中形成轨迹位置姿态信息集合{Q,N};
信息应用处理器B调用正运动学模块B中计算出的传感器3当前时刻的检测点位置与姿态信息,并根据传感器3当前时刻的检测点位置与姿态信息、以及轨迹位置姿态信息集合{Q,N},确定传感器3在下一时刻的检测点位置及姿态信息,并求出传感器3在当前时刻检测点位置与传感器3在下一时刻检测点位置之间的第一位置及姿态偏差信息;
信息应用处理器A调用正运动学模块A中计算出的工具2当前时刻的目标位置与姿态信息,并根据工具2当前时刻的目标位置与姿态信息、以及轨迹位置信息集合{Q,N},确定工具
2在下一时刻的目标位置及姿态信息,并求出工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的第二位置及姿态偏差信息;
逆运动学模块B调用信息应用处理器B中计算出的传感器3在当前时刻检测点位置与传感器3在下一时刻检测点位置之间的第一位置及姿态偏差信息,并根据第一位置姿态偏差信息,计算出传感器3的检测点位置位于下一时刻检测点位置时传感器支链5的关节角信息;
逆运动学模块A调用信息应用处理器A中计算出的工具2在当前时刻目标位置与工具2在下一时刻目标位置之间的第二位置及姿态偏差信息,并根据第二位置及姿态偏差信息,计算出工具2的目标位置位于下一时刻目标位置时工具支链4的关节角信息。
[0186] 轨迹跟踪系统可进一步划分为传感系统6和执行系统7两部分,将传感系统6与执行系统7的功能明确公开,两者仅需频繁交换信息即可很好地实现轨迹跟踪。为方面描述,这里将传感系统6向执行系统7发送信息定义为“下传”,将执行系统7向传感系统6发送信息定义为“上传”,轨迹跟踪系统中传感系统6和执行系统7的具体划分与实施例一中四种划分方式相同,具体工作方式也相同,这里不再赘述。
[0187] 实施例八本实施例与实施例七的不同之处在于:本实施例是在参考点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-next的位置及姿态信息求出工具在当前时刻目标位置与工具在下一时刻的目标位置之间的第二位置及姿态偏差信息;其中t为工具每次位置修正控制的调整间隔时间,V为工具的前进速度,位于当前时刻位置的工具经间隔时间t后移动至下一时刻目标位置。
[0188] 第二位置及姿态偏差信息的具体求解过程为:设位于当前时刻的目标位置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’轴的夹角。
[0189] 此时第五步中执行器根据控制参考点Ref-A-next的位置及姿态信息、使工具的目标位置由当前时刻目标位置运动至下一时刻目标位置。
[0190] 轨迹跟踪控制方法的其他步骤、各步骤的具体展开及实现轨迹跟踪控制方法的轨迹跟踪系统均与实施例七相同,不再赘述。
[0191] 以上所述仅是本发明的较佳实施例,并非是对本发明作任何其他形式的限制,而依据本发明的技术实质所作的任何修改或等同变化,仍属于本发明要求保护的范围。
[0192] 实施例一、实施例二、实施例三和实施例四中所述的轨迹跟踪系统对传感系统的要求比较低,即便传感系统能力不足导致不能提供焊缝位置的高度偏差以及工件倾斜程度以及不能完善地得到轨迹截面形状或不能频繁、快速地得到轨迹截面形状,也可以完成轨迹跟踪目的。
[0193] 例如传感系统中的传感器采用2D相机,此时传感系统仅能提供焊缝位置的左右偏差信息,不能提供焊缝位置的高度偏差以及工件倾斜程度时,P-s = [Xs,0, Zs],其中Zs只能是假定无高度偏差时的合理位置,而不是检测的值,仅是固定值。假设传感坐标系与工具坐标系的Z轴平行,那么N-s = [ 0,0,1]。
[0194] 因为δx是由于高度Zs变化引起的,所以δx=0;因为δy是由于N-s变化引起的,所以δy=0;
因为δz是由于Xs变化引起的,所以δz≠0。
[0195] 所以,变换到工具坐标系、计算出的偏差: D-tool = [Xtool,0, 0,0,0,δztool ]
那么,只要轨迹是在垂直于Z-tool的一个平面上,即轨迹高度已由设备自己保证,就仍然可以准确跟踪。类似的传感系统能力不足的情况如图16所示的用直角坐标型操作机跟踪没有弧高偏差Zs的平面曲线,图17所示的跟踪回转圆柱面上可以忽略弧高变化的曲线。
[0196] 不能频繁、快速地得到轨迹截面形状,只能得到一个、或十分有限的几个轨迹截面形状的情况的传感系统,例如传感系统中的传感器采用位移传感器,位移传感器是一种仅侧量一个点到传感器距离的设备,常用的激光位移传感器就是其典型的代表。用这样的传感器跟踪平面上的一条直焊缝,但是由于焊接夹具不准确,焊缝的位置、姿态不能足够精确地确定。采用本发明的方法,仍然可以进行准确跟踪。工作时焊枪带着它先在焊缝上扫过,那么就可以在焊缝的起点得到一个截面上的焊缝位置。那么就有了一个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 ],达到终点。
[0197] 像上述二个例子那样,根据实际情况,可以通过分析补充出Q-s、N-s、D-tool中的元素,然后就能应用本发明给出的跟踪方法了,这对简化执行设备是非常有意义的。这时有如下规律:只要D-tool中△z=0,能跟踪的就是平面,或可展开曲面(如回转的圆锥面、圆柱面面)上的曲线轨迹,对于工件,尽量用变位机等设备转换成可展开曲面;如△z≠0,但是将机器人A、机器人B限制成δx=0, δxq=0时,仍可以跟踪有缓慢地起伏变化的轨迹;如N-s不是定值,但是机器人A、机器人B限制成δy=0、δyq=0时,仍可以跟踪有小幅度侧向偏摆变化的轨迹;如果不能检测某一参数,就用机器人A、机器人B的结构保证其变化不超出规定值。
[0198] 实施例一至实施例八所述的轨迹跟踪系统对执行系统的要求也比较低,即便执行系统为欠自由度系统也能使用本发明所述的轨迹跟踪方法。
[0199] 当执行系统少于6个自由度数时,是欠自由度的,理论上必定不能精确跟踪任意的曲线,但是仍然可以根据上述控制原理,类推出改进的方法用于特定条件,均属于本专利保的护范围。例如上述用5个自由度的执行器带动焊枪跟踪任意空间轨迹的情况。例如用仅有两个或三个平移自由度的执行系统,就可以跟踪平面上的曲线。图18所示情况能够跟踪倾斜的不挠曲的倾斜平面上的轨迹;如果执行器没有升降调整的自由度,仍然能够跟踪平放置的平面上的轨迹。例如如图19所示,二自由度的执行器,配置一个水平翻转的变位机,可以跟踪回转体上的曲线。例如如图20所示,三个平移自由度的执行器,配置一个竖直回转轴的变位机,跟踪台面上一个偏心的或不够规则的环形线等。所谓变位机是一种使工件与机械臂产生相对运动的装置。如果忽略小的焊枪姿态偏差,这些欠自由度的执行器还可以跟踪更复杂些的轨迹,不一一列举。
[0200] 实施例一至实施例八所述的轨迹跟踪方法对于传感器附着于世界坐标系的情况的也适用。如图21所示,用一个变位机使筒形工件旋转,焊接筒壁上的螺旋线焊缝。这里,执行系统由一个二自由度的直角坐标机械臂与一个旋转的变位机共同构成。假如工件有圆度误差,而且工件在转台上安装时定心也不够准确,那么就不能准确定位其焊缝。这里采用了两个附着于世界坐标系的位移传感器3-1、3-2检测螺旋线轨迹上的两个点Q1、Q2。这时,由于轨迹是随着变位机运动的,如果要完全按照本发明的基本跟踪方法操作,需要把工件假设为固定的,那么传感器就是附着于一个三自由度执行器的第1个关节上了,工具附着于第3个关节上,而第1个关节是绕着固定的工件坐标系旋转的,其坐标系关系如图22所示,其中空心箭头矢量Tw、Tt分别为相对于世界坐标系表示的传感信息及工具信息,将其转化为相对于工件坐标系表示的实心箭头矢量Twc、Twt即可。还可以灵活运用本发明采用的原理:因为圆柱面是可展开的曲面,可以理解为在展开的平板上跟踪一条倾斜的直线。
[0201] 前面实施例1中有述控制系统自身对轨迹跟踪的贡献问题,在此也可以体现。例如,当D-tool中前进驱动分量△y=0的情况下,就需要执行系统中的,例如“示教轨迹”或者“规划轨迹”,补出至少△y≠0的一个分量。
[0202] 当然,类似的偏差信息结合不限于这样两组偏差信息之间的结合,还可以有多个偏差信息来源,而且它们的表述参考坐标系也可能不同,即可以通过变换统一到同一个参考系中,也可以分别在不同的参考系下分步修正,最后得到综合的控制效果。
[0203] 本发明提供的轨迹跟踪控制方法及各自对应的轨迹跟踪系统克服了检测点超前带来的误差,能脱离示教信息独立运行,在轨迹跟踪过程中能充分利用传感器检测的信息进行轨迹跟踪,不仅可以纠正工具及传感器的位置偏差,还可以纠正工具及传感器的姿态偏差,从而更好地实现轨迹跟踪目的。本发明所述的轨迹跟踪控制方法对于传感系统6、执行系统7没有特别限制,但是传感系统6和执行系统7之间要能互相协调配合,要求根据各自独特的功能,做相应的信息调整,例如把不能感知或控制的参数定义为反应实际情况的标准值就可以,因此可以广泛应用本轨迹跟踪控制方法。本发明提供的轨迹跟踪系统,可以支持各种不同的传感系统6、执行系统7,只要按照信息交换的规则组建跟踪系统,就可以选用尽量简单的甚至不完善的装置,完成丰富的具体任务;也可以用完善的传感系统6,配合全自由度或冗余自由度的执行系统7,完成非常复杂的跟踪任务。此外把轨迹跟踪系统进一步划分成传感系统和执行系统两部分,可以使跟踪系统的建立更加便利、灵活,而且便于由不同的专业厂家制造,提高性能、降低成本。
高效检索全球专利

专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台,是提供专利分析,专利查询,专利检索等数据服务功能的知识产权数据服务商。

我们的产品包含105个国家的1.26亿组数据,免费查、免费专利分析。

申请试用

分析报告

专利汇分析报告产品可以对行业情报数据进行梳理分析,涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度,系统通过AI智能系统对图表进行解读,只需1分钟,一键生成行业专利分析报告。

申请试用

QQ群二维码
意见反馈