一种基于RTK定位的机械臂引导方法与流程

本发明属于自动化领域，涉及一种基于rtk定位的机械臂引导方法。

背景技术：

传统工业机器人对于产品生产线工作效果很好，但只能工作在结构化及封闭的环境中，从而限制了其应用领域，且应变能力弱。现有机械臂定位主要靠视觉传感器，机器视觉技术提高了机器人对环境的感知和应变能力，但当机械臂作业于户外太阳光干扰环境时，基于光传感的摄像头受到干扰无法识别特征也就不能实现定位。实时动态(rtk)载波相位差分技术广泛用于无人机、无人驾驶、环境测绘等领域。rtk是一种通过基准站和移动站的同步观测，利用载波相位观测值实现快速高精度定位功能的差分测量技术。rtk系统由1个基准站、若干个移动站及无线通讯系统组成。作业时在地面上静置1台基准站，对gps卫星进行连续观测，并将观测数据和测站信息通过无线传输设备实时地发送给移动站，移动站接收机在接收gps卫星信号和采集卫星数据的同时，通过无线接收设备接收来自基准站的数据，并在系统内对采集和接收的2组数据进行载波相位差分处理，实时解算出移动站在大地下的三维坐标及其精度。现有rtk用于组合测量时直接用经纬度计算，没有涉及将经纬度和高程转换到大地坐标(x,y,z)，且没有使用到坐标系变换。由于机械臂工作时需要准确的作业点的位姿(x,y,z,qx,qy,qz)，因此，本发明旨在户外太阳光干扰条件下，通过使用1～3个rtk移动站，来解决工业机器人坐标系与大地坐标系之间的位置和方向的确定问题。

技术实现要素：

有鉴于此，本发明的目的在于提供一种基于rtk定位的机械臂引导方法。结合与机械臂坐标系固定的两个rtk测量点之间的关系可得知机械臂在大地坐标系下的位姿，工作测量点rtk与陀螺仪相对于机械臂的位置与方向可通过坐标变换关系求解，机械臂控制器得到目标工作点的位置和方向即可引导机械臂运动作业。

为达到上述目的，本发明提供如下技术方案：

一种基于rtk定位的机械臂引导方法，在基于rtk定位的机械臂系统中，基于rtk定位的机械臂系统包括机械臂承载平台、工业机械臂、rtk基准站和rtk移动站；

rtk移动站在接收卫星观测值的同时接收到rtk基站的信息，进行rtk定位解算，得到rtk高精度定位的经纬度和高程信息，将经纬度和高程信息转换到大地坐标系下得到rtk移动站在大地坐标系下的x,y,z值；纬度φr，经度λr，高程h转换到大地坐标系下的三维信息x,y,z公式如下：

e²＝f(2-f)

x＝(v+h)cosφrcosλr

y＝(v+h)cosφrsinλr

z＝v(1-e²)sinφr

式中a为地球参考椭球长度，f为接地参考椭球的展平；在wgs-84坐标系下a＝6378137.0(m)，f＝1.0/298.257223563；

通过rtk移动站在rtk移动站第一监测点(4)和rtk移动站第二监测点(5)在大地坐标系下的差值δx,δy,δz计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz；

确定机械臂坐标系与大地坐标系之间的关系后，最后测量rtk移动站在rtk移动站第三监测点(6)处的大地坐标系下坐标位置；由于rtk移动站在rtk移动站第一监测点(4)和rtk移动站第三监测点(6)都在大地坐标系下并且相对位置已经测得，rtk移动站在rtk移动站第一监测点(4)的测量点又与机械臂坐标原点相对位置已知，根据转换关系求解rtk移动站在rtk移动站第三监测点(6)处时在机械臂坐标系下的位置；结合rtk移动站在rtk移动站第三监测点(6)处的陀螺仪方向信息，最终获取到rtk移动站在rtk移动站第三监测点(6)时在机械臂坐标系下的位置与方向，机械臂获取目标工作点的位姿(x,y,z,qx,qy,qz,qw)就能控制机械臂末端工具精确运动到该位姿进行工作。

可选的，根据所述rtk移动站的数量实现引导定位有三种情况：

情况一：当有三个移动站时，只需将非固定移动站放置在工作点测量一次；

情况二：当有两个移动站时，需要将非固定移动站依次放置在辅助点和工作点测量两次；

情况三：当只有一个移动站时，需要将其依次放置在初始点、辅助点和工作点测量三次。

可选的，所述情况一具体为：

工作时，其中两个rtk移动站固定在机械臂平台机械臂承载平台(1)上的rtk移动站第一监测点(4)和rtk移动站第二监测点(5)，这两个位置的差值δx,δy,δz用于计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz；引导机械臂时只需将rtk移动站放置在目标工作点rtk移动站第三监测点(6)定位一次，利用坐标变换关系确定机械臂末端工具工作位姿。

可选的，所述情况二具体为：

工作时，一个rtk移动站固定在rtk移动站第二监测点(5)，另一个rtk移动站放置在rtk移动站第一监测点(4)定位第一次，这两个位置的差值δx,δy,δz用于计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz；确定目标点时将没有固定的rtk移动站放置在目标工作点rtk移动站第三监测点(6)定位第二次，利用坐标变换关系确定机械臂末端工具工作位姿。

可选的，所述情况三具体为：

工作时，第一步将移动站放置在rtk移动站第一监测点(4)定位第一次，第二步将rtk移动站放置到rtk移动站第二监测点(5)定位第二次，这两个位置的差值δx,δy,δz用于计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz；第三步将rtk移动站放置到目标工作点rtk移动站第三监测点(6)定位第三次，利用坐标变换关系确定机械臂末端工具工作位姿。

本发明的有益效果在于：本发明克服现有的基于视觉定位的机械臂引导方法在户外太阳光干扰环境下无法工作，而提供一种新颖的基于rtk高精度定位的机械臂引导方法，定位精度为1cm左右。在户外工作具有更高的可靠性，对目标点的定位操作比较简单，容易实现，避免了传统机器视觉技术的复杂算法的研究。该发明的定位引导效率高、能耗低、数据传输量小，适用于在户外有太阳光等干扰环境下机械臂控制引导，具有广阔的应用前景和市场需求。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书来实现和获得。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作优选的详细描述，其中：

图1为系统结构示意图；

图2为实施例一的整体结构示意图；

图3为实施例二的整体结构示意图；

图4为实施例三的整体结构示意图。

附图标记：1-机械臂承载平台，2-机械臂，3-rtk基准站，4-rtk移动站第一监测点，5-rtk移动站第二监测点，6-rtk移动站第三监测点。

具体实施方式

以下通过特定的具体实例说明本发明的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需要说明的是，以下实施例中所提供的图示仅以示意方式说明本发明的基本构想，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。

其中，附图仅用于示例性说明，表示的仅是示意图，而非实物图，不能理解为对本发明的限制；为了更好地说明本发明的实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

本发明实施例的附图中相同或相似的标号对应相同或相似的部件；在本发明的描述中，需要理解的是，若有术语“上”、“下”、“左”、“右”、“前”、“后”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此附图中描述位置关系的用语仅用于示例性说明，不能理解为对本发明的限制，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明公开了一种基于rtk定位的机械臂引导方法。该系统由机械臂承载平台、工业机械臂、rtk基准站和rtk移动站等组成，系统结构示意图如图1所示。1为机械臂承载平台，2为机械臂，3为rtk基准站，4为rtk移动站第一监测点，5为rtk移动站第二监测点，为rtk移动站第三监测点。

为实现由rtk定位引导机械臂工作，本发明工作原理如下：rtk移动站在接收卫星观测值的同时也接收到rtk基站的信息，进行rtk定位解算，得到rtk高精度定位的经纬度和高程信息，将经纬度和高程信息转换到大地坐标系下得到rtk移动站在大地坐标系下的x,y,z值。纬度φr，经度λr，高程h转换到大地坐标系下的三维信息x,y,z公式如下：

e²＝f(2-f)

x＝(v+h)cosφrcosλr

y＝(v+h)cosφrsinλr

z＝v(1-e²)sinφr

式中a为地球参考椭球长度，f为接地参考椭球的展平。在wgs-84坐标系下a＝6378137.0(m)，f＝1.0/298.257223563。

如图2～图4所示，由于rtk移动站在rtk移动站第一监测点4和rtk移动站第二监测点5相对于机械臂的坐标系是固定不变的，因此可以通过rtk移动站在rtk移动站第一监测点4和rtk移动站第二监测点5在大地坐标系下的差值δx,δy,δz计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz。确定了机械臂坐标系与大地坐标系之间的关系后，最后测量rtk移动站在rtk移动站第三监测点6处的大地坐标系下坐标位置。由于rtk移动站在rtk移动站第一监测点4和rtk移动站第三监测点6都在大地坐标系下并且相对位置已经测得，rtk移动站在rtk移动站第一监测点4的测量点又与机械臂坐标原点相对位置已知，这时根据转换关系即可求解rtk移动站在rtk移动站第三监测点6处时在机械臂坐标系下的位置。结合rtk移动站在rtk移动站第三监测点6处的陀螺仪方向信息，最终获取到rtk移动站在rtk移动站第三监测点6时在机械臂坐标系下的位置与方向，机械臂获取目标工作点的位姿(x,y,z,qx,qy,qz,qw)就能控制机械臂末端工具精确运动到该位姿进行工作。

根据rtk移动站的数量实现引导定位有三种情况。当有三个移动站时，只需将非固定移动站放置在工作点测量一次；当有两个移动站时，需要将非固定移动站依次放置在辅助点和工作点测量两次；当只有一个移动站时，需要将其依次放置在初始点、辅助点和工作点测量三次。

实施例一

本实例使用三个rtk移动站。工作时，其中两个rtk移动站固定在机械臂平台机械臂承载平台1上的rtk移动站第一监测点4和rtk移动站第二监测点5，这两个位置的差值δx,δy,δz用于计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz。引导机械臂时只需将rtk移动站放置在目标工作点rtk移动站第三监测点6定位一次，利用坐标变换关系即可确定机械臂末端工具工作位姿。

实施例二

本实例使用两个rtk移动站。工作时，一个rtk移动站固定在rtk移动站第二监测点5，另一个rtk移动站放置在rtk移动站第一监测点4定位第一次，这两个位置的差值δx,δy,δz用于计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz。确定目标点时将没有固定的rtk移动站放置在目标工作点rtk移动站第三监测点6定位第二次，利用坐标变换关系即可确定机械臂末端工具工作位姿。

实施例三

本实例使用一个rtk移动站。工作时，第一步将移动站放置在rtk移动站第一监测点4定位第一次，第二步将rtk移动站放置到rtk移动站第二监测点5定位第二次，这两个位置的差值δx,δy,δz用于计算机械臂坐标系相对于大地坐标系偏转方向的欧拉角rx,ry,rz。第三步将rtk移动站放置到目标工作点rtk移动站第三监测点6定位第三次，利用坐标变换关系即可确定机械臂末端工具工作位姿。

本发明结合三个rtk监测点的数据即可求得目标监测点在机械臂坐标系下的x,y,z坐标位置，再结合目标点陀螺仪的方向即可求解目标点在机械臂坐标系下的位姿(x,y,z,qx,qy,qz,qw)。

本发明所需的rtk移动站数量可在1与3之间适当选择，当移动站有3个时只需一次测量，当移动站少于3个时需要多次测量。

根据与机械臂坐标系固定的两个rtk监测点求解机械臂坐标系与大地坐标的关系，进一步求解作业监测点在机械臂坐标系下的位置。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。