一种获取机器人TCP坐标的方法和装置与流程
本发明实施例涉及人工智能领域，尤其涉及一种获取机器人tcp坐标的方法和装置。
背景技术：
随着科技的发展，机器人的应用领域越来越广泛。对于高精度加工领域和手术领域这两个应用领域中的机器人来说，其需要完成的工作任务对精度有很高的要求。
一般来说，高精度加工机器人和手术机器人在执行工作任务时，均需要在其自身机械臂上加装工具才能完成相关工作。由于机械臂末端坐标系与机械臂坐标系之间的坐标转换关系可以通过dh参数计算得出，其中的误差很小，那么，如何准确得到工具自身坐标系原点(toolcenterpoint，tcp)在机械臂末端坐标系中的坐标是影响其工作精度的关键因素。目前，tcp在机械臂末端坐标系中的坐标可以通过机械臂末端坐标系结合工具自身的理论尺寸计算或测量得到，其称作tcp的理论坐标。也就是说，当工具装配到机械臂末端后，只能利用tcp的理论坐标来执行工作任务。
但是在根据设计图纸对工具进行加工的过程中一般会产生尺寸误差，将工具装配到机械臂末端的过程中可能会产生装配误差，工具受环境的影响也可能产生形变误差，测量过程中也可能产生测量误差等，这些误差均会使tcp在机械臂末端坐标系中的实际坐标与tcp的理论坐标产生偏差，若以tcp的理论坐标对tcp进行标定，那么必然造成所标定的tcp的位置和实际的tcp位置之间产生误差，从而影响机器人的工作精度。
技术实现要素：
本申请实施例解决的技术问题之一在于提供一种获取机器人tcp坐标的方法和装置，通过设置计算次数阈值和/或误差阈值，将机器人的机械臂末端工具上所标记的点在机械臂末端坐标系中的坐标产生的误差控制在较小的范围内，从而缩小了机器人在执行工作任务过程中产生误差的范围，进而提高了机器人的工作精度。
一方面，本申请实施例提供了一种获取机器人tcp坐标的方法，包括：
s100：标记机械臂末端的工具上能够被传感器识别的一个点为tcp，并将tcp的理论坐标设置为所标记的点在机械臂末端坐标系中的当前坐标，设置计算次数阈值并初始化计算次数为0，和/或设置误差阈值并初始化f(tcpx,y,z)past的值为0；
s200：根据所标记的点在传感器识别范围内的移动，得到用于建立机械臂坐标系和传感器坐标系之间坐标转换关系的m个位置以及用于采集误差的n个位置，其中，m个位置中至少有三个不共线的位置，m为整数，且m≧3，n为整数，且n≧1；
s300：根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，所标记的点在传感器坐标系中的坐标，以及利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系，得到所标记的点在所述n个位置上坐标误差的平方和f(tcpx,y,z)now。
s400：当|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值和/或计算次数大于等于计算次数阈值时，执行s600；否则，将计算次数加1和/或将f(tcpx,y,z)now赋值给f(tcpx,y,z)past后执行s500。
s500：根据f(tcpx,y,z)now和最小二乘法得到所标记的点在机械臂末端坐标系下的坐标，并将得到的坐标作为所标记的点在机械臂末端坐标系中的当前坐标，执行s300；
s600：将所标记的点在机械臂末端坐标系中的当前坐标作为tcp的实际坐标。
可选地，利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系步骤包括：
将所述m个位置上对应的机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，与所标记的点在机械臂末端坐标系中的当前坐标相结合，得到所标记的点在机械臂坐标系中的m个坐标；
根据所标记的点在机械臂坐标系中的m个坐标，以及所标记的点在传感器坐标系中的坐标，得到机械臂坐标系和传感器坐标系之间的坐标转换关系。
可选地，根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，所标记的点在传感器坐标系中的坐标，以及利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系，得到所标记的点在所述n个位置上坐标误差的平方和f(tcpx,y,z)now的步骤包括：
根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri，
根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在机械臂坐标系中的坐标ri转换到传感器坐标系下得到坐标r′i，
比较坐标r′i与所标记的点在传感器坐标系下的坐标ci，得到所标记的点在n个位置上的坐标误差的平方和，或者，
根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri；
根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在传感器坐标系中的坐标ci转换到机械臂坐标系下得到坐标c′i，
比较坐标c′i与所标记的点在机械臂坐标系下的坐标ri，得到所标记的点在n个位置上的坐标误差的平方和，其中，1≦i≦n。
可选地，根据f(tcpx,y,z)now和最小二乘法得到所标记的点在机械臂末端坐标系下的坐标的步骤为：
根据f(tcpx,y,z)now和非线性最小二乘法，得到所标记的点在机械臂末端坐标系下的坐标。
可选地，得到所标记的点在n个位置上的坐标误差的平方和的步骤为：
计算所述n个位置中每个位置上，坐标r′i在各个坐标轴上的坐标与坐标ci在各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的差值的平方和相加，得到所标记的点在n个位置上的坐标误差的平方和，或者，
计算所述n个位置中每个位置上，坐标c′i在各个坐标轴上的坐标与坐标ri在各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的差值的平方和相加，得到所标记的点在n个位置上的坐标误差的平方和。
可选地，所述根据f(tcpx,y,z)now和最小二乘法得到所标记的点在机械臂末端坐标系下的坐标的计算公式具体为：
另一方面，本申请实施例还提供了一种获取机器人tcp坐标的装置，包括：
标记tcp和设置阈值模块，用于标记机械臂末端的工具上能够被传感器识别的一个点为tcp，并将tcp的理论坐标设置为所标记的点在机械臂末端坐标系中的当前坐标，设置计算次数阈值并初始化计算次数为0，和/或设置误差阈值并初始化f(tcpx,y,z)past的值为0；
位置获取模块，用于根据所标记的点在传感器识别范围内的移动，得到用于建立机械臂坐标系和传感器坐标系之间坐标转换关系的m个位置以及用于采集误差的n个位置，其中，m个位置中至少有三个不共线的位置，m为整数，且m≧3，n为整数，且n≧1；
坐标误差的平方和获取模块，用于根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，所标记的点在传感器坐标系中的坐标，以及利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系，得到所标记的点在所述n个位置上坐标误差的平方和f(tcpx,y,z)now；
阈值比较模块，用于比较|f(tcpx,y,z)now-f(tcpx,y,z)past|和误差阈值的大小和比较计算次数和计算次数阈值的大小，并将比较结果发送给数据处理模块和/或实际坐标设置模块；
数据处理模块，用于当计算次数小于计算次数阈值和/或|f(tcpx,y,z)now-f(tcpx,y,z)past|大于误差阈值时，将计算次数加1和/或将从阈值比较模块中获取的f(tcpx,y,z)now赋值给f(tcpx,y,z)past；
坐标计算模块，用于根据数据处理模块中的f(tcpx,y,z)now和最小二乘法计算得到所标记的点在机械臂末端坐标系下的坐标，并将得到的坐标发送到坐标误差的平方和获取模块；
实际坐标设置模块，用于当计算次数大于等于计算次数阈值和/或|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值时，将所标记的点在机械臂末端坐标系中的当前坐标作为tcp的实际坐标。
可选地，所述坐标误差的平方和获取模块还包括：坐标转换关系建立模块，用于将所述m个位置上对应的机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，与所标记的点在机械臂末端坐标系中的当前坐标相结合，得到所标记的点在机械臂坐标系中的m个坐标，并根据所标记的点在机械臂坐标系中的m个坐标，以及所标记的点在传感器坐标系中的坐标，得到机械臂坐标系和传感器坐标系之间的坐标转换关系。
可选地，所述坐标误差的平方和获取模块还包括：坐标确定模块，用于根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri；
坐标转换模块，用于根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在机械臂坐标系中的坐标ri转换到传感器坐标系下得到坐标ri′，或者用于根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在传感器坐标系中的坐标ci转换到机械臂坐标系下得到坐标ci′；
坐标比较模块，用于比较坐标ri′与所标记的点在传感器坐标系下的坐标ci，得到所标记的点在n个位置上的坐标误差的平方和，或者用于比较坐标ci′与所标记的点在机械臂坐标系下的坐标ri，得到所标记的点在n个位置上的坐标误差的平方和，其中，1≦i≦n。
可选地，所述坐标计算模块具体用于根据f(tcpx,y,z)now和非线性最小二乘法，得到所标记的点在机械臂末端坐标系下的坐标。
可选地，所述坐标比较模块具体用于计算所述n个位置中每个位置上，坐标r′i在各个坐标轴上的坐标与坐标ci在各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的平方和相加，得到标记点在n个位置上的坐标误差的平方和，或者，
计算所述n个位置中每个位置上，坐标c′i在各个坐标轴上的坐标与坐标ri在各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的平方和相加，得到所标记的点在n个位置上的坐标误差的平方和。
可选地，所述坐标计算模块中得到所标记的点在机械臂末端坐标系下的坐标的计算公式为：
由以上技术方案可见，本申请实施例提供的一种获取机器人tcp坐标的方法和装置，通过将机械臂末端的工具上所标记的点在机械臂坐标系下的坐标和在传感器坐标系下的坐标转换到同一个坐标系下，计算将上述两个坐标转换到同一个坐标系下的坐标误差的平方和，并将前后两次的平方和的差值的绝对值和设置的误差阈值比较，当该平方和的差值的绝对值小于等于设置的误差阈值时，将所标记的点在机械臂末端坐标系中的当前坐标作为tcp的实际坐标，或者当根据坐标误差的平方和利用最小二乘法计算所标记的点在机械臂末端坐标系中的当前坐标的计算次数超过设定的计算次数阈值时，将所标记的点在机械臂末端坐标系中的当前坐标设置为tcp的实际坐标，或者前后两次的坐标误差平方和的差值的绝对值小于等于设置的误差阈值且计算次数超过设定的计算次数阈值时，将所标记的点在机械臂末端坐标系中的当前坐标设置为tcp的实际坐标，前述三种方式不管哪种方式，都能够很大程度上减少工具加工过程中的尺寸误差，工具装配到机械臂末端的装配误差，工具受环境影响的形变误差等误差所造成的对tcp坐标值标定的影响，将标定过程中产生的误差控制在很小的范围内，很大程度上满足了机器人完成工作任务时对精度的要求。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明实施例中记载的一些实施例，对于本领域普通技术人员来讲，还可以根据这些附图获得其他的附图。
图1为本申请实施例提供的一种机器人获取tcp坐标的方法流程示意图；
图2为本申请实施例提供的一种机器人获取tcp坐标的装置结构示意图；
图3为本申请实施例提供的机械臂和传感器的示意图。
具体实施方式
当然，实施本发明实施例的任一技术方案不一定需要同时达到以上的所有优点。
为了使本领域的人员更好地理解本发明实施例中的技术方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本发明实施例一部分实施例，而不是全部的实施例。基于本发明实施例中的实施例，本领域普通技术人员所获得的所有其他实施例，都应当属于本发明实施例保护的范围。
针对已有技术中，利用机械臂末端坐标系结合工具自身的理论尺寸计算或者测量得到的工具自身坐标系原点(toolcenterpoint，tcp)在机械臂末端坐标系中的坐标与tcp的实际坐标之间可能存在较大误差的问题，本申请实施例提供了一种获取机器人tcp坐标的方法，如图1所示，包括步骤s100-s600，具体地：
步骤s100：标记机械臂末端的工具上能够被传感器识别的一个点为tcp，并将tcp的理论坐标设置为所标记的点在机械臂末端坐标系中的当前坐标，设置计算次数阈值并初始化计算次数为0，和/或设置误差阈值并初始化f(tcpx,y,z)past的值为0。
在实际操作过程中，在机器人末端的工具上找到一个可被传感器识别到的点，或者从机械臂末端的工具上很多可被传感器识别到的点中选择一个点，并将找到或者选择的这个点标记为tcp。需要说明的是，当传感器为摄像头时，可以将所标记的点设置为光学标记点；当传感器为电磁传感器时，可设置所标记的点为电磁标记点；当传感器为超声探头时，可设置所标记的点为超声标记点；当传感器为红外传感器时，可设置所标记的点为红外标记点,亦或是当传感器为其他类型的传感器时，设置与其他类型的传感器相应的标记点。且这里tcp在机械臂末端坐标系中的实际坐标在步骤s100中并没有确定，本申请实施例根据该所标记的点来确定tcp的实际坐标。而tcp的理论坐标可根据工具自身的理论尺寸和机械臂末端坐标系，利用测量工具或者根据粗标定算法获得。
需要说明的是，本申请实施例中可以对计算次数进行初始化并设置计算次数阈值，以及对f(tcpx,y,z)past进行初始化并设置误差阈值，也可以仅对计算次数进行初始化并设置计算次数阈值，或者仅对f(tcpx,y,z)past进行初始化并设置误差阈值，其中f(tcpx,y,z)past代表前一次的计算误差。这里的计算次数阈值和误差阈值是作为设置tcp实际坐标时所用到的坐标计算循环的结束条件，也就是说，可以仅将误差阈值作为循环的结束条件，也可以仅将计算次数阈值作为循环的结束条件，还可以将计算次数阈值和误差阈值同时作为循环的结束条件。
在实际操作过程中，本申请实施例设置计算次数阈值并初始化计算次数为0，和/或设置误差阈值并初始化f(tcpx,y,z)past的值为0。
步骤s200：根据所标记的点在传感器识别范围内的移动，得到用于建立机械臂坐标系和传感器坐标系之间坐标转换关系的m个位置以及用于采集误差的n个位置，其中，m个位置中至少有三个不共线的位置，m为整数，且m≧3，n为整数，且n≧1。
具体地，根据所标记的点在传感器识别范围内的移动，可以得到所标记的点处于传感器识别范围内的多个位置，这里可以得到用于建立机械臂坐标系和传感器坐标系之间坐标转换关系的m个位置以及用于采集误差的n个位置，其中，m个位置中至少有三个不共线的位置，m为整数，且m≧3，同时，根据所标记的点在传感器识别范围内的移动，得到n个不同于m个位置的位置，用于采集所标记的点在机械臂坐标系中的坐标和在传感器坐标系中的坐标之间的误差，即采集将所标记的点在机械臂坐标系中的坐标和在传感器坐标系中的坐标转换到同一个坐标系下所得到的坐标误差，其中，n为整数，且n≧1。
步骤s300：根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，所标记的点在传感器坐标系中的坐标，以及利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系，得到所标记的点在所述n个位置上坐标误差的平方和f(tcpx,y,z)now。
具体地，实现步骤s300的方式为：根据所标记的点在机械臂末端坐标系中的当前坐标，与所标记的点位于所述m个位置时的m个机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，以及所标记的点位于所述m个位置时在传感器坐标系中的m个坐标，建立机械臂坐标系和传感器坐标系之间的坐标转换关系；
并根据所述机械臂坐标系和传感器坐标系之间的坐标转换关系，得到所标记的点在所述n个位置上坐标误差的平方和f(tcpx,y,z)now。
其中，利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系的步骤包括：
将所述m个位置上对应的机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，与所标记的点在机械臂末端坐标系中的当前坐标相结合，得到所标记的点在机械臂坐标系中的m个坐标；
根据所标记的点在机械臂坐标系中的m个坐标，以及所标记的点在传感器坐标系中的坐标，得到机械臂坐标系和传感器坐标系之间的坐标转换关系。
即将所标记的点位于所述m个位置上对应的m个机械臂末端坐标系与机械臂坐标系之间的坐标转换关系(即所标记的点处于某一个位置时，机械臂均有一个位姿与其对应，而可通过dh参数，得到这个位姿对应的机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，若有m个位置，则存在对应的m个机械臂末端坐标系与机械臂坐标系之间的坐标转换关系)，与所标记的点在机械臂末端坐标系中的当前坐标相结合，得到所标记的点在机械臂坐标系中的m个坐标；
根据所标记的点在机械臂坐标系中的m个坐标，以及所标记的点位于所述m个位置上对应的在传感器坐标系中的m个坐标，得到机械臂坐标系和传感器坐标系之间的坐标转换关系。
具体地，可以首先根据所标记的点(即s100中的tcp)在传感器识别范围内的移动，得到用于建立机械臂坐标系和传感器坐标系之间坐标转换关系的m个位置，m为大于等于3的整数，且m个位置中存在至少三个非共线的位置；由于机械臂末端坐标系与机械臂坐标系之间的坐标转换关系可以通过dh参数计算得出，因此，按照图3所示的机械臂和传感器的示意图，将机械臂末端坐标系与机械臂坐标系之间的坐标转换关系与所标记的点在机械臂末端坐标系中的当前坐标结合，就可以得到m个位置上所标记的点在机械臂坐标系中的m个坐标，而且由于传感器可以识别到所标记的点，因此所标记的点在m个位置中的每个位置上都有一个对应的传感器坐标系下的坐标，进而根据m个位置中每个位置上所标记的点在机械臂坐标系中的坐标和在传感器坐标系中的坐标，运用现有技术中的利用同一个点在两个坐标系中的坐标得到两个坐标系之间转换关系的方法，就可以得到机械臂坐标系和传感器坐标系之间的坐标转换关系。
优选地，m的值为3或者m的值为4。当m的值为3时，所标记的点在实际空间中的3个位置为非共线位置；当m的值为4时，所标记的点在实际空间中的4个位置可以为非共面位置。
当m的值为3时，利用所述m个位置建立机械臂坐标系和传感器坐标系之间的坐标转换关系的过程可以为：
假设通过移动机械臂得到所标记的点在实际空间中三个非共线的位置时，机械臂处于位姿1，位姿2，位姿3这三个位姿。
当机械臂移动到位姿1下时，通过dh参数能够得到当前机械臂位于位姿1时，机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，及机械臂末端坐标系原点在机械臂坐标系下的坐标p1，通过工具的加工图纸可以得到tcp在机械臂末端坐标系中的理论坐标，将其记作当前坐标t1，因此这里以p1+t1来表示当前tcp在机械臂坐标系下的坐标(需要说明的是，当前tcp在机械臂坐标系下的坐标，并不是将p1的坐标与t1的坐标进行相加，此处仅仅是利用“p1+t1”这个表现形式来对机械臂位于位姿1时当前tcp在机械臂坐标系下的坐标进行解释说明，下面类似描述中同样如此)，即可以得到机械臂位于位姿1时所标记的点在机械臂坐标系下的坐标p1+t1。通过传感器识别所标记的点，还能够得到机械臂位于位姿1时所标记的点在传感器坐标系下的坐标s1。
当机械臂移动到位姿2下时，通过dh参数能够得到当前机械臂位于位姿2时，机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，及机械臂末端坐标系原点在机械臂坐标系下的坐标p2，由于已经得到的tcp在机械臂末端坐标系中的坐标t1，因此这里以p2+t1来表示当前tcp在机械臂坐标系下的坐标，即得到机械臂位于位姿2时所标记的点在机械臂坐标系下的坐标p2+t1。通过传感器识别所标记的点，还能够得到机械臂位于位姿2时所标记的点在传感器坐标系下的坐标s2。
当机械臂移动到位姿3下时，通过dh参数能够得到当前机械臂位于位姿3时，机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，及机械臂末端坐标系原点在机械臂坐标系下的坐标p3，加之已经得到的tcp在机械臂末端坐标系中的坐标t1，因此这里以p3+t1来表示当前tcp在机械臂坐标系下的坐标p3+t1，即得到机械臂位于位姿3时所标记的点在机械臂坐标系下的坐标p3+t1。通过传感器识别所标记的点，还能够得到机械臂位于位姿3时所标记的点在传感器坐标系下的坐标s3。
本申请实施例中可利用p1+t1与s1，p2+t1与s2，p3+t1与s3，即这三个非共线的点分别在机械臂坐标系下的坐标与在传感器坐标系下的坐标建立机械臂坐标系和传感器坐标系之间的坐标转换关系。
当m的值为4时，利用m个位置建立机械臂坐标系和传感器坐标系之间的坐标转换关系的过程可以为：
假设通过移动机械臂得到所标记的点在实际空间中四个非共面的位置时，机械臂处于位姿1，位姿2，位姿3，位姿4这四个位姿。
通过和上述m的值为3时同样的方式得到四个非共面的点分别在机械臂坐标系下的坐标和在传感器坐标系下的坐标，以四个非共面的点中的一个点作为原点，并以所述原点与其它三个点的连线为坐标轴，建立仿射坐标系；根据四个非共面的点中每个点在传感器坐标系中的坐标和所述仿射坐标系，建立传感器坐标系与所述仿射坐标系之间的第一空间关系，并根据四个非共面的点中每个点在机械臂坐标系中的坐标和所述仿射坐标系，建立机械臂坐标系与所述仿射坐标系之间的第二空间关系；根据所述第一空间关系和所述第二空间关系，确定所述机械臂坐标系和所述传感器坐标系的转换关系。
可选地，还可以使得所标记的点在传感器识别范围内移动到p个位置，其中，p为大于等于4的整数，然后从p个位置中选择用于建立传感器坐标系和机械臂坐标系之间坐标转换关系的m个位置，将剩下的n个与m个位置不同位置用于采集误差，即p＝m+n。
需要说明的是，使所标记的点在传感器范围内移动到p个位置，m个位置或者n个位置时，可以为机械臂设定移动程序或者移动路径，让机械臂自动在传感器识别范围内移动，当然也可以采用其他现有方法，本申请实施例中对此不作具体限定。
另外，得到所标记的点在n个位置上坐标误差的平方和f(tcpx,y,z)now的步骤可以为：
根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri；然后根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在机械臂坐标系中的坐标ri转换到传感器坐标系下得到坐标r′i；比较坐标r′i与所标记的点在传感器坐标系下的坐标ci(坐标ci是通过传感器识别所标记的点得到的)，得到所标记的点在n个位置上的坐标误差的平方和，其中，1≦i≦n。
由于所标记的点在机械臂末端坐标系中的当前坐标是已知的，所标记的点在n个位置当中的任何一个位置上的机械臂末端坐标系和机械臂坐标系之间的坐标转换关系也是已知的(可以通过dh参数得到)，这样就可以得到所标记的点在机械臂坐标系中的坐标ri；利用m个位置能够得到机械臂坐标系和传感器坐标系之间的坐标转换关系，因此可以将所标记的点在机械臂坐标系中的坐标ri转换到传感器坐标系下得到坐标ri′；加之所标记的点在n个位置当中的任何一个位置上时其都可以被传感器识别，也就能够得到所标记的点在传感器坐标系下的坐标ci；接下来只要比较坐标ri′与所标记的点在传感器坐标系下的坐标ci，就可以得到所标记的点在n个位置中每个位置上的坐标误差，进而得到所标记的点在n个位置上的坐标误差的平方和。
得到所标记的点在n个位置上坐标误差的平方和f(tcpx,y,z)now的步骤还可以为：
根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri；根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在传感器坐标系中的坐标ci转换到机械臂坐标系下得到坐标c′i；比较坐标c′i与所标记的点在机械臂坐标系下的坐标ri，得到所标记的点在n个位置上的坐标误差的平方和，其中，1≦i≦n。
可选地，比较坐标r′i与坐标ci，得到所标记的点在n个位置上的坐标误差的平方和的步骤为：
计算所述n个位置中每个位置上，坐标r′i在各个坐标轴上的坐标与坐标ci在对应的各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的差值的平方和相加，得到所标记的点在n个位置上的坐标误差的平方和，或者，
计算所述n个位置中每个位置上，坐标c′i在各个坐标轴上的坐标与坐标ri在对应的各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的差值的平方和相加，得到所标记的点在n个位置上的坐标误差的平方和。
具体地，若根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri(xi，yi，zi)，根据机械臂坐标系和传感器坐标系之间的转换关系，将坐标ri(xi，yi，zi)转换到传感器坐标系下得到坐标r′i(x′i,y′i,z′i),将坐标r′i(x′i,y′i,z′i)与所标记的点在传感器坐标系下的坐标ci(xci，yci，zci)进行比较，就能得到所标记的点在当前位置上的坐标误差的平方和errori＝δxi2+δyi2+δzi2＝(x′i-xci)2+(y′i-yci)2+(z′i-zci)2。
或者根据机械臂坐标系和传感器坐标系之间的转换关系，将所标记的点在传感器坐标系下的坐标ci(xci，yci，zci)转换到机械臂坐标系下得到坐标c′i(x′ci,y′ci,z′ci),然后将坐标c′i(x′ci,y′ci,z′ci)与所标记的点在机械臂坐标系下的坐标ri(xi，yi，zi)进行比较得到所标记的点在当前位置上的坐标误差的平方和errori＝δxi2+δyi2+δzi2＝(xi-x′ci)2+(yi-y′ci)2+(zi-z′ci)2。
若n的值为2，即根据所标记的点在传感器识别范围内的移动，得到2个用于采集误差的位置，以将所标记的点在机械臂坐标系下的坐标转换到传感器坐标系下的坐标为例进行说明，将所标记的点在机械臂坐标系下的坐标记为r1(x1，y1，z1)和r2(x2，y2，z2)，在传感器坐标系下的坐标记为c1(xc1，yc1，zc1)和c2(xc2，yc2，zc2)，根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将r1和r2转换到传感器坐标系下得到r′1(x′1，y′1，z′1)和r′2(x′2，y′2，z′2)，将坐标r′1与c1，r′2与c2进行比较，得到当n为2时的坐标误差的平方和
综上所述，所标记的点在用于采集误差的位置上的坐标误差的平方和
步骤s400：当|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值和/或计算次数大于等于计算次数阈值时，执行步骤s600；否则，将计算次数加1和/或将f(tcpx,y,z)now赋值给f(tcpx,y,z)past后执行步骤s500。
步骤s500：根据f(tcpx,y,z)now和最小二乘法得到所标记的点在机械臂末端坐标系下的坐标，并将得到的坐标作为所标记的点在机械臂末端坐标系中的当前坐标，执行步骤s300。
在本申请实施例中，将步骤s400和步骤s500结合在一起进行说明。当第一次对|f(tcpx,y,z)now-f(tcpx,y,z)past|和误差阈值进行比较和/或第一次对计算次数和计算次数阈值进行比较时，tcp的坐标就是所标记的点在机械臂末端坐标系中的理论坐标值，且f(tcpx,y,z)past的值就是初始化的值，即0，且计算次数也是初始化的值，同样也为0。因此，当第一次对|f(tcpx,y,z)now-f(tcpx,y,z)past|进行比较时，|f(tcpx,y,z)now-f(tcpx,y,z)past|其实就是|f(tcpx,y,z)now|。而经过步骤s500的计算再返回到步骤s300时，所标记的点在机械臂末端坐标系中的当前坐标已经是根据f(tcpx,y,z)now并结合最小二乘法得到的坐标了，因此，将经过步骤s500的计算后得到的所标记的点在机械臂末端坐标系中的坐标代入到步骤s300后(即更新了所标记的点在机械臂末端坐标系中的当前坐标)，就会得到新的所标记的点在机械臂坐标系中的新坐标，那么，根据所标记的点在机械臂坐标系中新坐标和在传感器坐标系中的坐标，就会得到机械臂坐标系和传感器坐标系之间的新的坐标转换关系，进而根据新的坐标转换关系，得到所标记的点在所述n个位置上新的坐标误差的平方和，然后将新的坐标误差的平方和赋值给f(tcpx,y,z)now，把之前得到的f(tcpx,y,z)now赋值给f(tcpx,y,z)past，然后利用新的f(tcpx,y,z)now的值结合最小二乘法计算所标记的点在机械臂末端坐标系中的另一个新的坐标……如此循环，直至|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值和/或计算次数大于等于计算次数阈值。
在本申请实施例中，触发上述循环的结束条件可以有三种判断方式：
第一种，计算次数大于等于计算次数阈值。这里说的计算次数指的是根据f(tcpx,y,z)now结合最小二乘法计算所标记的点在机械臂末端坐标系中的坐标的次数，当这个次数大于等于步骤s100中设置的计算次数阈值时，满足触发条件，结束循环；
第二种，|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值，即前后两次所标记的点在用于采集误差的位置上的坐标误差的平方和的差值的绝对值小于等于步骤s100中设置的误差阈值，举例来说，如果步骤s100中设置的误差阈值为10-5，那么当|f(tcpx,y,z)now-f(tcpx,y,z)past|<10-5时，结束循环，执行步骤s600；
第三种，同时满足计算次数大于等于计算次数阈值和|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值。
当采用上述三种判断方式的任何一种且满足判断条件时，执行步骤s600：将所标记的点在机械臂末端坐标系中的当前坐标作为tcp的实际坐标。
当不满足第一种判断方式时，将计算次数加1，即在计算次数当前数值的基础上加1，并且在后续的循环判断时将该计算次数的数值代入判断；
当不满足第二种判断方式时，则将f(tcpx,y,z)now赋值给f(tcpx,y,z)past；
当不满足第三种判断方式时，将计算次数加1和将f(tcpx,y,z)now赋值给f(tcpx,y,z)past。
可选地，根据f(tcpx,y,z)now结合最小二乘法得到所标记的点在机械臂末端坐标系下的坐标的优选步骤为：
根据f(tcpx,y,z)now和非线性最小二乘法，得到所标记的点在机械臂末端坐标系下的坐标。
本申请实施例中，根据f(tcpx,y,z)now和非线性最小二乘法，得到所标记的点在机械臂末端坐标系下的坐标的计算公式可以为：
此公式中，tcp为所标记的点在机械臂末端坐标系中的待求解的坐标，公式中以矩阵形式体现；
其中，
其中，
其中，
若以所标记的点在传感器的识别范围内移动到的4个位置来建立机械臂坐标系及传感器坐标系之间转换关系，则将具体数值带入后就可以得到
基于相同的发明构思，如图2所示，本申请实施例还提供了一种获取机器人tcp坐标的装置，包括：
标记tcp和设置阈值模块201，用于标记机械臂末端的工具上能够被传感器识别的一个点为tcp，并将tcp的理论坐标设置为所标记的点在机械臂末端坐标系中的当前坐标，设置计算次数阈值并初始化计算次数为0，和/或设置误差阈值并初始化f(tcpx,y,z)past的值为0；
位置获取模块202，用于根据所标记的点在传感器识别范围内的移动，得到用于建立机械臂坐标系和传感器坐标系之间坐标转换关系的m个位置以及用于采集误差的n个位置，其中，m个位置中至少有三个不共线的位置，m为整数，且m≧3，n为整数，且n≧1；
坐标误差的平方和获取模块203，用于根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，所标记的点在传感器坐标系中的坐标，以及利用所述m个位置建立的机械臂坐标系和传感器坐标系之间的坐标转换关系，得到所标记的点在所述n个位置上坐标误差的平方和f(tcpx,y,z)now；
阈值比较模块204，与坐标误差的平方和获取模块203相连，用于比较|f(tcpx,y,z)now-f(tcpx,y,z)past|和误差阈值的大小和/或比较计算次数和计算次数阈值的大小，并将比较结果发送给数据处理模块205和实际坐标设置模块207；
数据处理模块205，与阈值比较模块204相连，用于当计算次数小于计算次数阈值和/或|f(tcpx,y,z)now-f(tcpx,y,z)past|大于误差阈值时，将计算次数加1和/或将从阈值比较模块中获取的f(tcpx,y,z)now赋值给f(tcpx,y,z)past；
坐标计算模块206，与所述数据处理模块205相连，用于根据数据处理模块205中的f(tcpx,y,z)now和最小二乘法计算得到所标记的点在机械臂末端坐标系下的坐标，并将得到的坐标发送到坐标误差的平方和获取模块203，使平方和获取模块203中所标记的点在机械臂末端坐标系中的当前坐标得到替换；
实际坐标设置模块207，与阈值比较模块204相连，用于当计算次数大于等于计算次数阈值和/或|f(tcpx,y,z)now-f(tcpx,y,z)past|小于等于误差阈值时，将所标记的点在机械臂末端坐标系中的当前坐标作为tcp的实际坐标。
本申请实施例中，标记tcp和设置阈值模块201，位置获取模块202，坐标误差的平方和获取模块203，阈值比较模块204，数据处理模块205，坐标计算模块206，实际坐标设置模块207可以执行上述方法实施例中的优选实施例。
进一步地，本申请实施例提供的一种获取机器人tcp坐标的装置，所述坐标误差的平方和获取模块203还包括坐标转换关系建立模块，用于将所述m个位置上对应的机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，与所标记的点在机械臂末端坐标系中的当前坐标相结合，得到所标记的点在机械臂坐标系中的m个坐标，并根据所标记的点在机械臂坐标系中的m个坐标，以及所标记的点在传感器坐标系中的坐标，得到机械臂坐标系和传感器坐标系之间的坐标转换关系。
由于所标记的点在处于实际空间中某一个位置时，机械臂均有一个位姿与所标记的点的位置相对应，通过dh参数能够得到机械臂处于某一个位姿下的机械臂末端坐标系与机械臂坐标系之间的坐标转换关系，本申请实施例中，所标记的点在传感器识别范围内移动到的用于建立坐标系转换关系的位置有m个，所以就存在m个机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，其中，m个位置中至少有三个不共线的位置，m为整数，且m≧3，n为整数，且n≧1。
将m个机械臂末端坐标系和机械臂坐标系之间的坐标转换关系与所标记的点在机械臂末端坐标系中的当前坐标结合，就能够得到所标记的点在机械臂坐标系中的m个坐标，从而结合所标记的点在传感器坐标系中的m个坐标，就可以建立机械臂坐标系和传感器坐标系之间的坐标转换关系。
进一步地，坐标误差的平方和获取模块203还要通过坐标确定模块，坐标转换模块和坐标比较模块三个模块共同获取坐标误差的平方和。其中：
坐标确定模块，用于根据所标记的点在机械臂末端坐标系中的当前坐标与机械臂末端坐标系和机械臂坐标系之间的坐标转换关系，得到所标记的点在机械臂坐标系中的坐标ri；
坐标转换模块，用于根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在机械臂坐标系中的坐标ri转换到传感器坐标系下得到坐标ri′，或者用于根据机械臂坐标系和传感器坐标系之间的坐标转换关系，将所标记的点在传感器坐标系中的坐标ci转换到机械臂坐标系下得到坐标c′i；
坐标比较模块，用于比较坐标r′i与所标记的点在传感器坐标系下的坐标ci，得到所标记的点在n个位置上的坐标误差的平方和，或者用于比较坐标c′i与所标记的点在机械臂坐标系下的坐标ri，得到所标记的点在n个位置上的坐标误差的平方和，其中，1≦i≦n。
进一步地，坐标计算模块具体用于根据f(tcpx,y,z)now和非线性最小二乘法，得到所标记的点在机械臂末端坐标系下的坐标。
需要说明的是，坐标计算模块206也可以用其他方法得到所标记的点在机械臂末端坐标系下的坐标，比如线性最小二乘法等，本申请实施例对此不作具体限定，只要能准确得到所标记的点在机械臂末端坐标系的坐标即可。
进一步地，坐标比较模块具体用于：
计算所述n个位置中每个位置上，坐标r′i在各个坐标轴上的坐标与坐标ci在各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的差值的平方和相加，得到标记点在n个位置上的坐标误差的平方和，或者，
计算所述n个位置中每个位置上，坐标c′i在各个坐标轴上的坐标与坐标ri在各个坐标轴上的坐标的差值的平方和；
将所述n个位置中每个位置上得到的差值的平方和相加，得到所标记的点在n个位置上的坐标误差的平方和。
进一步地，所述坐标计算模块206中得到所标记的点在机械臂末端坐标系下的坐标的计算公式为：
此公式中，tcp为所标记的点在机械臂末端坐标系中的待求解的坐标，公式中以矩阵形式体现；
其中，
其中，
需要说明的是，本申请实施例提供的一种获取机器人tcp坐标的装置可以设置在机器人中，也可以设置在机器人以外的其他设备中，也可以是一个独立的不依赖于其他设备的装置。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独处理，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。
上述以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例方法的部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-onlymemory，简称rom)、随机存取存储器(randomaccessmemory，简称ram)、磁碟或者光盘等各种可以存储程序代码的介质。
最后应说明的是：以上实施例仅用以说明本申请实施例的技术方案，而非对其限制；尽管参照前述实施例对本申请进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本申请各实施例技术方案的精神和范围。
- 还没有人留言评论。精彩留言会获得点赞！