一种基于全站仪的空间目标的位置和朝向标定方法
【技术领域】
[0001] 本发明涉及空间测量和坐标系统转换技术领域，具体涉及一种基于全站仪的空间 目标的位置和朝向标定方法。
【背景技术】
[0002] 目前，在多自由度机械臂自动控制系统工作过程中，需要将机械臂末端的工具台 移动到目标所在的空间位置。现目前，通常是利用反向运动学来进行分析求得机械臂的移 动轨迹，而这其中就涉及到空间目标位置的测量和标定方法。
[0003] 空间目标位置的测量和标定方法很多，如激光测距标定，超声波测距标定等。然 而，使用激光定位标定，除了激光测量仪测量的距离，还需要知道测距仪的旋转角度和俯仰 角度，在对结果精度要求严格的情况下，旋转角度和俯仰角度必须使用高精度的旋转编码 器进行测量标定，实际实施比较困难。而超声波对空气介质要求较高，对不同的介质需要采 用不同的方法对声速进行校正，并且需要使用复杂的空间定位算法进行位置解算和误差补 偿，实际的计算误差也往往较大。由此可见，激光和超声波的标定方法对工作环境都有很大 的要求，且难以得到较为理想的结果。
【发明内容】
[0004] 为了克服上述现有技术中存在的缺陷，本发明的目的是提供一种基于全站仪的空 间目标的位置和朝向标定方法，该方法简单有效，能快速准确的对空间目标位置及朝向进 行标定，为机械臂反向运动学分析提供位置和朝向信息。
[0005] 为了实现上述目的，本发明提供了提供一种基于全站仪的空间目标的位置和朝向 标定方法，包括以下步骤：
[0006]S1，构建机械臂所在的坐标系[xl，yl，zl]和全站仪所在的坐标系[x2，y2，z2]，其 中zl轴和z2轴保持平行；
[0007]S2、确定空间目标首尾两点P、Q在全站仪所在的坐标系中的坐标；
[0008]S3、构造全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵；
[0009]S4、将点P和点Q在全站仪所在的坐标系中的坐标按平移矩阵和旋转矩阵向机械 臂所在的坐标系进行转换，得到点P和点Q在机械臂所在的坐标系中的坐标；
[0010] S5,确定目标孔的位置点在机械臂坐标系统下的坐标为q(qx，qy，qz)，其朝向向量
至此，空间目标的位置和朝向标定完毕。
[0011] 全站仪，即全站型电子测距仪，是一种集光、机、电为一体的高技术测量仪器，是集 水平角、垂直角、距离（斜距、平距）、高差测量功能于一体的测绘仪器系统，广泛用于地上 大型建筑和地下隧道施工等精密工程测量或变形监测领域。利用全站仪测量得到的空间目 标的坐标准确性高，本发明先得到空间目标在全站仪即工作系统所在的坐标系下的空间坐 标，然后得到两个坐标系之间的平移矩阵和旋转矩阵，进而对给定目标位置的进行追踪。该 方法简单，直接，有效，能快速准确的对空间目标位置及朝向进行标定。而反向运动学分析 过程需要根据机械臂的具体坐标系统进行设计，因此将标定完成的目标位置和朝向作为机 械臂的反向运动学分析的输入数据使用，为机械臂反向运动学分析提供位置和朝向信息， 有助于反向运动学分析。
[0012] 进一步的，所述步骤S2包括以下步骤：
[0013]S2-1，由全站仪所在的坐标系原点02向机械臂所在坐标系的xl-yl平面作垂直投 影，得到投影点02'，在投影点02'处作平行于全站仪所在的坐标系y2轴的平行线y2'，确 定空间目标首尾两点P、Q，由点P和点Q向机械臂所在的坐标系的xl-yl平面上作垂直投 影，得到投影点P'和投影点Q'，由投影点P'和投影点Q'分别向平行线y2'作垂线，与平行 线y2'相交得到点K和点Η;
[0014]S2-2,测量全站仪所在的坐标系原点02到与投影点02'之间的距离hl，测量点Ρ 到投影点P'的距离h2,点Q到投影点Q'的距离h3,投影点P'到平行线y2'的距离d3,投 影点Q'到平行线y2'的距离d4,点K到投影点02'的距离d5,点Η到投影点02'的距离 d6；
[0015]S2-3,利用测量值表示在全站仪所在的坐标系中空间目标首尾两点P、Q的坐标， 其中，点P坐标为[-d3,d5,h2-hl]，点Q坐标为[-d4,d6,h3-hl]。
[0016] 此空间目标坐标确定的方法简单，直接，且准确性高，利用全站仪本身就能迅速的 测量出所需要的数值，进而得到空间目标首尾两点在全站仪所在坐标系中的坐标。
[0017] 进一步的，所述步骤S3包括以下步骤：
[0018]S3-1，测量全站仪所在的坐标系原点02在机械臂所在的坐标系的xl-yl平面上的 投影点02'到xl轴的距离dl和到yl轴的距离d2,测量全站仪所在的坐标系原点02与其 在机械臂所在坐标系的xl-yl平面的投影点02'之间的距离hl，测量得到两个坐标系的绕 z轴的旋转的相对角度Θ;
[0019]S3-2,将全站仪所在的坐标系沿着X轴平移d2个单位，变换到机械臂所 在的坐标系，全站仪所在的坐标系的点变换到机械臂所在的坐标系以后坐标变为：
[0020] 将上式写成矩阵形式：
[0021] 得到沿着X轴的平移变换矩罔
[0022] S3-3，同理，构造沿y轴的平移变换矩阵T(y，-dl)，和沿z轴的平移变换矩阵 T(z，_hl)，矩阵如下：
[0023]
[0024]S3-4,绕Z旋转矩阵的构造和推倒过程如下：全站仪所在的坐标系绕z轴旋 转Θ度，则全站仪所在的坐标系中的点旋转后在机械臂所在的坐标系中的坐标为：
[0025] 将上式写成矩阵形式为
[0026] 即得到沿着z轴的旋转矩阵
[0027] 该平移矩阵和旋转矩阵推算方法简单，准确，能快速得到全站仪所在的坐标系到 机械臂所在的坐标系的平移矩阵和旋转矩阵。
[0028] 进一步的，所述步骤S4中得到的点P在机械臂所在的坐标系中的坐标为：q=Q* R(z, 0)*T(x，-d2)*T(y，-dl)*T(z，_hl);点Q在机械臂所在的坐标系中的坐标为：p=P*R (z，0)*T(x，-d2)*T(y，-dl)*T(z，-hl)。由于符号矩阵运算结果非常复杂，所以此处不给出 最终的q和P点的坐标符号表示。在应用过程中，可以将具体的参数带入进行计算即可。
[0029] 本发明公开的方法采用全站仪方式进行距离和角度测量和标定，并且通过转换矩 阵将测量坐标和位置朝向转换到机械臂坐标系统中，核心内容包括：采用全站仪测量空间 目标的两个位置点，利用全站仪所在的坐标系和机械臂所在的坐标系之间的距离和转角构 建平移变换矩阵和旋转矩阵，将测量系统中的目标坐标转换到工作系统中，简单，直接、有 效地得到了空间目标首尾两点在全站仪所在坐标系中的坐标及朝向，为机械臂反向运动学 分析提供位置和朝向信息。
[0030] 本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变 得明显，或通过本发明的实践了解到。
【附图说明】
[0031] 本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变 得明显和容易理解，其中：
[0032] 图1是测量示意图。
【具体实施方式】
[0033] 下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终 相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附 图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
[0034] 在本发明的描述中，除非另有规定和限定，需要说明的是，术语"安装"、"相连"、 "连接"应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可 以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据 具体情况理解上述术语的具体含义。
[0035] 本发明提供了一种基于全站仪的空间目标的位置和朝向标定方法，包括以下步 骤：
[0036]S1，如图1所示，构建机械臂所在的坐标系[xl，yl，zl]和全站仪所在的坐标系 [x2,y2,z2]，其中zl轴和z2轴保持平行。
[0037] S2、确定空间目标首尾两点P、Q在全站仪所在的坐标系中的坐标。
[0038] 具体确定方法为：
[0039] 首先，由全站仪所在的坐标系原点02向机械臂所在坐标系的xl-yl平面作垂直投 影，得到投影点02'，在投影点02'处作平行于全站仪所在的坐标系y2轴的平行线y2'，确 定空间目标首尾两点P、