CN105424024A - 一种基于全站仪的空间目标的位置和朝向标定方法 - Google Patents

Abstract

本发明提出了一种基于全站仪的空间目标的位置和朝向标定方法，采用全站仪方式进行距离和角度测量和标定，并且通过转换矩阵将测量坐标和位置朝向转换到机械臂坐标系统中，核心内容包括：采用全站仪测量空间目标的两个位置点，利用全站仪所在的坐标系和机械臂所在的坐标系之间的距离和转角构建平移变换矩阵和旋转矩阵，将测量系统中的目标坐标转换到工作系统中，简单，直接、有效地得到了空间目标首尾两点在全站仪所在坐标系中的坐标及朝向，为机械臂反向运动学分析提供位置和朝向信息。

Description

一种基于全站仪的空间目标的位置和朝向标定方法

技术领域

本发明涉及空间测量和坐标系统转换技术领域，具体涉及一种基于全站仪的空间目标的位置和朝向标定方法。

背景技术

目前，在多自由度机械臂自动控制系统工作过程中，需要将机械臂末端的工具台移动到目标所在的空间位置。现目前，通常是利用反向运动学来进行分析求得机械臂的移动轨迹，而这其中就涉及到空间目标位置的测量和标定方法。

空间目标位置的测量和标定方法很多，如激光测距标定，超声波测距标定等。然而，使用激光定位标定，除了激光测量仪测量的距离，还需要知道测距仪的旋转角度和俯仰角度，在对结果精度要求严格的情况下，旋转角度和俯仰角度必须使用高精度的旋转编码器进行测量标定，实际实施比较困难。而超声波对空气介质要求较高，对不同的介质需要采用不同的方法对声速进行校正，并且需要使用复杂的空间定位算法进行位置解算和误差补偿，实际的计算误差也往往较大。由此可见，激光和超声波的标定方法对工作环境都有很大的要求，且难以得到较为理想的结果。

发明内容

为了克服上述现有技术中存在的缺陷，本发明的目的是提供一种基于全站仪的空间目标的位置和朝向标定方法，该方法简单有效，能快速准确的对空间目标位置及朝向进行标定，为机械臂反向运动学分析提供位置和朝向信息。

为了实现上述目的，本发明提供了提供一种基于全站仪的空间目标的位置和朝向标定方法，包括以下步骤：

S1，构建机械臂所在的坐标系[x1，y1，z1]和全站仪所在的坐标系[x2，y2，z2]，其中z1轴和z2轴保持平行；

S2、确定空间目标首尾两点P、Q在全站仪所在的坐标系中的坐标；

S3、构造全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵；

S4、将点P和点Q在全站仪所在的坐标系中的坐标按平移矩阵和旋转矩阵向机械臂所在的坐标系进行转换，得到点P和点Q在机械臂所在的坐标系中的坐标；

S5，确定目标孔的位置点在机械臂坐标系统下的坐标为q(qx,qy,qz)，其朝向向量为至此，空间目标的位置和朝向标定完毕。

全站仪，即全站型电子测距仪，是一种集光、机、电为一体的高技术测量仪器，是集水平角、垂直角、距离(斜距、平距)、高差测量功能于一体的测绘仪器系统，广泛用于地上大型建筑和地下隧道施工等精密工程测量或变形监测领域。利用全站仪测量得到的空间目标的坐标准确性高，本发明先得到空间目标在全站仪即工作系统所在的坐标系下的空间坐标，然后得到两个坐标系之间的平移矩阵和旋转矩阵，进而对给定目标位置的进行追踪。该方法简单，直接，有效，能快速准确的对空间目标位置及朝向进行标定。而反向运动学分析过程需要根据机械臂的具体坐标系统进行设计，因此将标定完成的目标位置和朝向作为机械臂的反向运动学分析的输入数据使用，为机械臂反向运动学分析提供位置和朝向信息，有助于反向运动学分析。

进一步的，所述步骤S2包括以下步骤：

S2-1，由全站仪所在的坐标系原点O2向机械臂所在坐标系的x1-y1平面作垂直投影，得到投影点O2’，在投影点O2’处作平行于全站仪所在的坐标系y2轴的平行线y2’，确定空间目标首尾两点P、Q，由点P和点Q向机械臂所在的坐标系的x1-y1平面上作垂直投影，得到投影点P’和投影点Q’，由投影点P’和投影点Q’分别向平行线y2’作垂线，与平行线y2’相交得到点K和点H；

S2-2，测量全站仪所在的坐标系原点O2到与投影点O2’之间的距离h1，测量点P到投影点P’的距离h2，点Q到投影点Q’的距离h3，投影点P’到平行线y2’的距离d3，投影点Q’到平行线y2’的距离d4，点K到投影点O2’的距离d5，点H到投影点O2’的距离d6；

S2-3，利用测量值表示在全站仪所在的坐标系中空间目标首尾两点P、Q的坐标，其中，点P坐标为[-d3,d5,h2-h1]，点Q坐标为[-d4,d6,h3-h1]。

此空间目标坐标确定的方法简单，直接，且准确性高，利用全站仪本身就能迅速的测量出所需要的数值，进而得到空间目标首尾两点在全站仪所在坐标系中的坐标。

进一步的，所述步骤S3包括以下步骤：

S3-1，测量全站仪所在的坐标系原点O2在机械臂所在的坐标系的x1-y1平面上的投影点O2’到x1轴的距离d1和到y1轴的距离d2，测量全站仪所在的坐标系原点O2与其在机械臂所在坐标系的x1-y1平面的投影点O2’之间的距离h1，测量得到两个坐标系的绕z轴的旋转的相对角度θ；

S3-2，将全站仪所在的坐标系沿着x轴平移d2个单位，变换到机械臂所在的坐标系，全站仪所在的坐标系的点变换到机械臂所在的坐标系以后坐标变为： $\begin{array}{c}x1=x+0+0-d2\\ y1=0+y+0+0\\ z1=0+0+z+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right],$

得到沿着x轴的平移变换矩阵 $T(x,-d2)=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

S3-3，同理，构造沿y轴的平移变换矩阵T(y,-d1)，和沿z轴的平移变换矩阵T(z,-h1)，矩阵如下：

$T(y,-d1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -d1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],T(z,-h1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -h1\\ 0& 0& 0& 1\end{array}\right];$

S3-4，绕Z旋转矩阵的构造和推倒过程如下：全站仪所在的坐标系绕z轴旋转θ度，则全站仪所在的坐标系中的点旋转后在机械臂所在的坐标系中的坐标为： $\begin{array}{c}x1=x2\cos \mathrm{\θ}-y2sin\mathrm{\θ}+0+0\\ y1=x2\sin \mathrm{\θ}+y2cos\mathrm{\θ}+0+0\\ z1=0+0+z2+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right],$

即得到沿着z轴的旋转矩阵 $R(z,\mathrm{\θ})=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

该平移矩阵和旋转矩阵推算方法简单，准确，能快速得到全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵。

进一步的，所述步骤S4中得到的点P在机械臂所在的坐标系中的坐标为：q＝Q*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)；点Q在机械臂所在的坐标系中的坐标为：p＝P*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)。由于符号矩阵运算结果非常复杂，所以此处不给出最终的q和p点的坐标符号表示。在应用过程中，可以将具体的参数带入进行计算即可。

本发明公开的方法采用全站仪方式进行距离和角度测量和标定，并且通过转换矩阵将测量坐标和位置朝向转换到机械臂坐标系统中，核心内容包括：采用全站仪测量空间目标的两个位置点，利用全站仪所在的坐标系和机械臂所在的坐标系之间的距离和转角构建平移变换矩阵和旋转矩阵，将测量系统中的目标坐标转换到工作系统中，简单，直接、有效地得到了空间目标首尾两点在全站仪所在坐标系中的坐标及朝向，为机械臂反向运动学分析提供位置和朝向信息。

本发明的附加方面和优点将在下面的描述中部分给出，部分将从下面的描述中变得明显，或通过本发明的实践了解到。

附图说明

本发明的上述和/或附加的方面和优点从结合下面附图对实施例的描述中将变得明显和容易理解，其中：

图1是测量示意图。

具体实施方式

下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。

在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。

本发明提供了一种基于全站仪的空间目标的位置和朝向标定方法，包括以下步骤：

S1，如图1所示，构建机械臂所在的坐标系[x1，y1，z1]和全站仪所在的坐标系[x2，y2，z2]，其中z1轴和z2轴保持平行。

S2、确定空间目标首尾两点P、Q在全站仪所在的坐标系中的坐标。

具体确定方法为：

首先，由全站仪所在的坐标系原点O2向机械臂所在坐标系的x1-y1平面作垂直投影，得到投影点O2’，在投影点O2’处作平行于全站仪所在的坐标系y2轴的平行线y2’，确定空间目标首尾两点P、Q，由点P和点Q向机械臂所在的坐标系的x1-y1平面上作垂直投影，得到投影点P’和投影点Q’，由投影点P’和投影点Q’分别向平行线y2’作垂线，与平行线y2’相交得到点K和点H；

然后，测量全站仪所在的坐标系原点O2到与投影点O2’之间的距离h1，测量点P到投影点P’的距离h2，点Q到投影点Q’的距离h3，投影点P’到平行线y2’的距离d3，投影点Q’到平行线y2’的距离d4，点K到投影点O2’的距离d5，点H到投影点O2’的距离d6；

最后，利用测量值表示在全站仪所在的坐标系中空间目标首尾两点P、Q的坐标，其中，点P坐标为[-d3,d5,h2-h1]，点Q坐标为[-d4,d6,h3-h1]。

S3、构造全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵。

具体分为以下步骤：

第一步，测量全站仪所在的坐标系原点O2在机械臂所在的坐标系的x1-y1平面上的投影点O2’到x1轴的距离d1和到y1轴的距离d2，测量全站仪所在的坐标系原点O2与其在机械臂所在坐标系的x1-y1平面的投影点O2’之间的距离h1，测量得到两个坐标系的绕z轴的旋转的相对角度θ；

第二步，将全站仪所在的坐标系沿着x轴平移d2个单位，变换到机械臂所在的坐标系，全站仪所在的坐标系的点变换到机械臂所在的坐标系以后坐标变为： $\begin{array}{c}x1=x+0+0-d2\\ y1=0+y+0+0\\ z1=0+0+z+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right],$

得到沿着x轴的平移变换矩阵 $T(x,-d2)=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

第三步，同理，构造沿y轴的平移变换矩阵T(y,-d1)，和沿z轴的平移变换矩阵T(z,-h1)，矩阵如下：

$T(y,-d1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -d1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],T(z,-h1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -h1\\ 0& 0& 0& 1\end{array}\right];$

第四步，绕Z旋转矩阵的构造和推倒过程如下：全站仪所在的坐标系绕z轴旋转θ度，则全站仪所在的坐标系中的点旋转后在机械臂所在的坐标系中的坐标为： $\begin{array}{c}x1=x2\cos \mathrm{\θ}-y2sin\mathrm{\θ}+0+0\\ y1=x2\sin \mathrm{\θ}+y2cos\mathrm{\θ}+0+0\\ z1=0+0+z2+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right],$

即得到沿着z轴的旋转矩阵 $R(z,\mathrm{\θ})=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right].$

该平移矩阵和旋转矩阵推算方法简单，准确，能快速得到全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵。

S4、将点P和点Q在全站仪所在的坐标系中的坐标按平移矩阵和旋转矩阵向机械臂所在的坐标系进行转换，得到点P在机械臂所在的坐标系中的坐标为：q＝Q*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)；点Q在机械臂所在的坐标系中的坐标为：p＝P*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)。由于符号矩阵运算结果非常复杂，所以此处不给出最终的q和p点的坐标符号表示。在应用过程中，可以将具体的参数带入进行计算即可。

至此，点P和点Q在机械臂所在的坐标系中的位置标定完成，点q和点p就是测量目标孔在机械臂坐标系统的坐标点表示，通过点q和点p可以确定目标孔的位置点在机械臂坐标系统下的坐标为q(qx,qy,qz)，其朝向向量为至此，空间目标的位置和朝向标定完毕。将标定完成的目标位置和朝向作为机械臂的反向运动学分析的输入数据使用，为机械臂反向运动学分析提供了位置和朝向信息。

在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。

尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (4)

1.一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，包括以下步骤：

S1，构建机械臂所在的坐标系[x1，y1，z1]和全站仪所在的坐标系[x2，y2，z2]，其中z1轴和z2轴保持平行；

S2，确定空间目标首尾两点P、Q在全站仪所在的坐标系中的坐标；

S3，构造全站仪所在的坐标系到机械臂所在的坐标系的平移矩阵和旋转矩阵；

S4，将点P和点Q在全站仪所在的坐标系中的坐标按平移矩阵和旋转矩阵向机械臂所在的坐标系进行转换，得到点P和点Q在机械臂所在的坐标系中的坐标p(px,py,pz)和q(qx,qy,qz)；

S5，确定目标孔的位置点在机械臂坐标系统下的坐标为q(qx,qy,qz)，其朝向向量为至此空间目标的位置和朝向标定完毕。

2.根据权利要求1所述的一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，所述步骤S2包括以下步骤：

S2-1，由全站仪所在的坐标系原点O2向机械臂所在坐标系的x1-y1平面作垂直投影，得到投影点O2’，在投影点O2’处作平行于全站仪所在的坐标系y2轴的平行线y2’，确定空间目标首尾两点P、Q，由点P和点Q向机械臂所在的坐标系的x1-y1平面上作垂直投影，得到投影点P’和投影点Q’，由投影点P’和投影点Q’分别向平行线y2’作垂线，与平行线y2’相交得到点K和点H；

S2-2，测量全站仪所在的坐标系原点O2到与投影点O2’之间的距离h1，测量点P到投影点P’的距离h2，点Q到投影点Q’的距离h3，投影点P’到平行线y2’的距离d3，投影点Q’到平行线y2’的距离d4，点K到投影点O2’的距离d5，点H到投影点O2’的距离d6；

S2-3，利用测量值表示在全站仪所在的坐标系中空间目标首尾两点P、Q的坐标，其中，点P坐标为[-d3,d5,h2-h1]，点Q坐标为[-d4,d6,h3-h1]。

3.根据权利要求1所述的一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，所述步骤S3包括以下步骤：

S3-1，测量全站仪所在的坐标系原点O2在机械臂所在的坐标系的x1-y1平面上的投影点O2’到x1轴的距离d1和到y1轴的距离d2，测量全站仪所在的坐标系原点O2与其在机械臂所在坐标系的x1-y1平面的投影点O2’之间的距离h1，测量得到两个坐标系的绕z轴的旋转的相对角度θ；

S3-2，将全站仪所在的坐标系沿着x轴平移d2个单位，变换到机械臂所在的坐标系，全站仪所在的坐标系的点变换到机械臂所在的坐标系以后坐标变为：

x1＝x+0+0-d2

y1＝0+y+0+0

，

z1＝0+0+z+0

1＝0+0+0+1

将上式写成矩阵形式为：

$\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right],$

得到沿着x轴的平移变换矩阵 $T(x,-d2)=\left[\begin{array}{cccc}1& 0& 0& -d2\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

S3-3，同理，构造沿y轴的平移变换矩阵T(y,-d1)，和沿z轴的平移变换矩阵T(z,-h1)，矩阵如下：

$T(y,-d1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& -d1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right],T(z,-h1)=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& -h1\\ 0& 0& 0& 1\end{array}\right];$

S3-4，绕Z旋转矩阵的构造和推倒过程如下：全站仪所在的坐标系绕z轴旋转θ度，则全站仪所在的坐标系中的点旋转后在机械臂所在的坐标系中的坐标为： $\begin{array}{c}x1=x2\cos \mathrm{\θ}-y2\sin \mathrm{\θ}+0+0\\ y1=x2\sin \mathrm{\θ}+y2\cos \mathrm{\θ}+0+0\\ z1=0+0+z2+0\\ 1=0+0+0+1\end{array},$

将上式写成矩阵形式为： $\left[\begin{array}{c}x1\\ y1\\ z1\\ 1\end{array}\right]=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{c}x2\\ y2\\ z2\\ 1\end{array}\right],$

即得到沿着z轴的旋转矩阵 $R(z,\mathrm{\θ})=\left[\begin{array}{cccc}cos\mathrm{\θ}& sin\mathrm{\θ}& 0& 0\\ -sin\mathrm{\θ}& \cos \mathrm{\θ}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]\mathrm{..}$

4.根据权利要求3所述的一种基于全站仪的空间目标的位置和朝向标定方法，其特征在于，所述步骤S4中得到的点P在机械臂所在的坐标系中的坐标为：q＝Q*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)；点Q在机械臂所在的坐标系中的坐标为：p＝P*R(z,θ)*T(x,-d2)*T(y,-d1)*T(z，-h1)。