CN106323286A - 一种机器人坐标系与三维测量坐标系的变换方法 - Google Patents

Abstract

本发明提出一种机器人坐标系与三维测量坐标系的变换方法，该方法通过公共点提取、变换矩阵计算和坐标转换步骤实现测量数据的坐标到机器人坐标的变换。具体的，该方法首先在机器人工作环境中选取4个位置不变的角点作为公共点；然后，建立标准坐标系，并计算公共点到标准坐标系的正变换和逆变换矩阵，从而推算出三维测量系统到机器人运动系统的坐标变换矩阵；最后，根据变换矩阵将三维测量数据变换到机器人坐标系下。本发明的优点在于只需通过4个公共点，无需复杂的迭代计算即可计算出坐标转换所需的旋转平移矩阵，并对公共点误差带来的变换误差进行了一定的修正。因此，本方法可解决智能机器人与三维测量系统坐标系转换问题，并降低由于公共点误差而带来的转换误差。

Description

一种机器人坐标系与三维测量坐标系的变换方法

技术领域

本发明涉及信息技术处理领域，更具体地，涉及一种三维测量数据到机器人运动坐标系的变换方法。

背景技术

智能机器人借助激光扫描仪等三维测量装置，能获取目标位置和工作环境的3维数据，从而自动规划机器人移动路径，规避环境中的障碍移动到目标位置。但实际中由于机器人运动坐标值和激光扫描仪的坐标值都是在各坐标系下得到的并不统一。因此需要先将测量得到的3维数据转换到机器人的运动坐标系下，使测量数据的3维坐标等于运动系统下的3维坐标。

目前，所有的坐标转换方法都是通过在两个坐标系中找若干个公共点，然后通过建立方程组求解旋转平移参数。例如，七参数模型求解，将3个平移量、3个旋转量和1个尺度因子作为待求解的未知量，然后通过联立方程求解；十三参数模型求解，将3个旋转矩阵用一个3×3矩阵表示，则有9个未知参数，加上3个平移量和1个尺度因子合计13个未知量，然后通过联立方程求解。但将这些方法应用在智能机器人时存在以下问题：(1)难以寻找大量的公共点用于求解转换矩阵参数；(2)实际中所有的公共点是存在误差的。因此，需要一种坐标转换方法，只需要少量的公共点就能计算出坐标转换矩阵，同时能修正由于公共点误差带来的转换误差。

发明内容

本发明为克服上述现有技术所述的公共点难以寻找，公共点存在误差等问题，提供一种机器人坐标系与三维测量坐标系的变换方法，适用于各种智能机器人运动坐标系和三维测量系统坐标系的坐标变换。

为解决上述技术问题，本发明的技术方案如下：

一种机器人坐标系与三维测量坐标系的变换方法，所述方法包括以下步骤：

S1：公共点坐标提取：在机器人工作环境中选定4个公共点，然后在机器人运动坐标系下和三维测量坐标系下，分别提取这4个公共点的三维空间坐标作为坐标系变换的依据；

S2：变换矩阵计算：将4个公共点划分成4个特征三角形，并建立一个标准坐标系；计算运动坐标系下和扫描坐标系下的特征三角形转换到标准坐标系的正变换矩阵和逆变换矩阵；根据正变换矩阵和逆变换矩阵，将4个公共点的扫描坐标转换到运动坐标系下，并求取每个公共点转换后的均值坐标；最后计算公共点的扫描坐标到均值坐标的旋转平移矩阵；

S3：扫描数据坐标系转换：根据S2计算得到的变换矩阵，对测量系统得到的三维数据进行三维旋转和平移，使测量数据从测量系统的坐标系变换到机器人运动系统的坐标系中。

在一种优选的方案中，步骤S1中，公共点坐标提取具体步骤为：

S1.1：公共点选取：在机器人工作环境中，选取4个位置不同，且绝对位置不会发生改变的角点作为公共点。

S1.2：公共点的运动坐标提取：通过机器人的示教器或点动控制器，将机器人的末端执行机构依次移动到4个公共点的位置，并按顺序记录4个公共点在运动坐标系下的三维空间坐标。

S1.3：公共点的测量坐标提取：对三维测量系统测量得到的三维数据进行处理，利用角点识别方法，得到4个公共点在测量系统下的三维空间坐标。

在一种优选的方案中，步骤S2中，变换矩阵具体计算步骤为：

S2.1：特征三角形划分：对4个公共点进行编号(编号为1、2、3、4)；然后抽取一个点作为特征三角形的顶点，选取另一个点与顶点连线形成参考边，最后再选取一个点与另外两点连线形成一个三角形；

S2.2：标准坐标系建立：以特征三角形的顶点作为坐标系的原点，以特征三角形参考边的方向作为X轴方向，以特征三角形平面内垂直于参考边的方向作为Y轴方向，以垂直于三角形平面的方向为Z轴方向建立一个标准坐标系；

S2.3：标准坐标系变换矩阵计算：计算机器人运动坐标系和测量坐标系的特征三角形到标准坐标系的正变换矩阵和逆变换矩阵。令特征三角形三个点的坐标为：(x₁,y₁,z₁)，(x₂,y₂,z₂)，(x₃,y₃,z₃)，则正变换矩阵为：

$\left[\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\\ z_i^{\′}\end{array}\right]={\left[\begin{array}{c}{x}_i-x_1\\ y_i-y_1\\ z_i-z_1\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{cos\γ}}_1& {\mathrm{sin\γ}}_1& 0\\ -{\mathrm{sin\γ}}_1& {\mathrm{cos\γ}}_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\β}}_1& 0& -{\mathrm{sin\β}}_1\\ 0& 1& 0\\ {\mathrm{sin\β}}_1& 0& {\mathrm{cos\β}}_1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\α}}_1& {\mathrm{sin\α}}_1\\ 0& -{\mathrm{sin\α}}_1& {\mathrm{cos\α}}_1\end{array}\right]$

$\begin{array}{ccc}{\mathrm{\γ}}_1=-arctan\left(\frac{y_2-y_1}{x_2-x_1}\right)& {\mathrm{\β}}_1=-arctan\left(\frac{z_2-z_1}{x_2-x_1}\right)& {\mathrm{\α}}_1=-arctan\left(\frac{z_3-z_1}{y_3-y_1}\right)\end{array}$

式中，i＝1、2、3。逆变换矩阵为：

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]={\left[\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\\ z_i^{\′}\end{array}\right]}^T\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\α}}_2& {\mathrm{sin\α}}_2\\ 0& -{\mathrm{sin\α}}_2& {\mathrm{cos\α}}_2\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\β}}_2& 0& -{\mathrm{sin\β}}_2\\ 0& 1& 0\\ {\mathrm{sin\β}}_2& 0& {\mathrm{cos\β}}_2\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\γ}}_2& {\mathrm{sin\γ}}_2& 0\\ -{\mathrm{sin\γ}}_2& {\mathrm{cos\γ}}_2& 0\\ 0& 0& 1\end{array}\right]+\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$

$\begin{array}{ccc}{\mathrm{\α}}_2=arctan\left(\frac{z_3-z_1}{y_3-y_1}\right)& {\mathrm{\β}}_2=arctan\left(\frac{z_2-z_1}{x_2-x_1}\right)& {\mathrm{\γ}}_2=arctan\left(\frac{y_2-y_1}{x_2-x_1}\right)\end{array}$

S2.4：公共点坐标变换：将S2.3得到的测量坐标系到标准坐标系的4个正变换矩阵和机器人坐标系到标准坐标系的4个逆变换矩阵，根据特征三角形的对应关系一一对应后形成4对变换矩阵。然后，依次对测量系统下公共点的坐标乘上测量系统坐标系到标准坐标系的正变换矩阵，再乘上对应的逆变换矩阵；

S2.5：均值化处理：将S2.4得到的4种变换结果的坐标值进行均值计算，从而降低公共点坐标误差导致的坐标转换误差；

S2.6：变换矩阵计算：任意提取3个均值化处理后的公共点坐标，形成一个特征三角形，计算该特征三角形到标准坐标系的逆变换矩阵。然后在计算扫描坐标系下对应特征三角形到标准坐标系的正变换矩阵。将上述正变换矩阵和逆变换矩阵作为坐标变换矩阵组；然后将测量数据先乘上正变换矩阵，再乘上逆变换矩阵，实现测量坐标系到运动坐标系的变换。

与现有技术相比，本发明技术方案的有益效果是：(1)本发明中需通过4个公共点，且公共点的确定方式以及坐标提取方式简单可行；(2)本发明中变换矩阵只需通过计算简单的三角函数即可得到，无需复杂的迭代计算；(3)本发明通过均值化处理方式，在一定程度上修正了公共点误差带来的变换误差。

附图说明

图1为本发明机器人坐标系与三维测量坐标系变换方法的流程图。

图2为特征三角形到标准坐标系的变换示意图。

图3为扫描数据到机器人坐标系的变换示意图。

具体实施方式

附图仅用于示例性说明，不能理解为对本专利的限制；

为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；

对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。

下面结合附图和实施例对本发明的技术方案做进一步的说明。

实施例1

以水火弯板机器人运动坐标系和三维激光扫描仪坐标系的坐标变换为例，如图1所示，本发明方法包括以下步骤：

S1：公共点坐标提取：在机器人工作环境中选定4个公共点，然后在机器人运动坐标系下和三维测量坐标系下，分别提取这4个公共点的三维空间坐标作为坐标系变换的依据。在实例1中，公共点坐标提取具体步骤为：

S1.1：公共点选取：选取机器人工作平台的4个角点作为公共点。

S1.2：公共点的运动坐标提取：通过水火弯板机器人运动控制系统的点动控制按钮将机器人末端的火枪头移动到公共点的位置，并按顺序记录4个公共点在运动坐标系下的三维空间坐标。

S1.3：公共点的测量坐标提取：对扫描仪测量得到的点云数据进行处理，提取工作平台4个角点的三维空间坐标。

S2：变换矩阵计算：将4个公共点划分成4个特征三角形，并建立一个标准坐标系；计算运动坐标系下和扫描坐标系下的特征三角形转换到标准坐标系的正变换矩阵和逆变换矩阵；根据正变换矩阵和逆变换矩阵，将4个公共点的扫描坐标转换到运动坐标系下，并求取每个公共点转换后的均值坐标；最后计算公共点的扫描坐标到均值坐标的旋转平移矩阵。具体计算步骤为：

S2.1：特征三角形划分：对4个公共点进行编号(编号为1、2、3、4)；然后抽取一个点作为特征三角形的顶点，选取另一个点与顶点连线形成参考边，最后再选取一个点与另外两点连线形成一个三角形；

S2.2：标准坐标系建立：以特征三角形的顶点作为坐标系的原点，以特征三角形参考边的方向作为X轴方向，以特征三角形平面内垂直于参考边的方向作为Y轴方向，以垂直于三角形平面的方向为Z轴方向建立一个标准坐标系；

S2.3：标准坐标系变换矩阵计算：计算机器人运动坐标系和测量坐标系的特征三角形到标准坐标系的正变换矩阵和逆变换矩阵。令特征三角形三个点的坐标为：(x₁,y₁,z₁)，(x₂,y₂,z₂)，(x₃,y₃,z₃)，则正变换矩阵为：

$\left[\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\\ z_i^{\′}\end{array}\right]={\left[\begin{array}{c}{x}_i-x_1\\ y_i-y_1\\ z_i-z_1\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{cos\γ}}_1& {\mathrm{sin\γ}}_1& 0\\ -{\mathrm{sin\γ}}_1& {\mathrm{cos\γ}}_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\β}}_1& 0& -{\mathrm{sin\β}}_1\\ 0& 1& 0\\ {\mathrm{sin\β}}_1& 0& {\mathrm{cos\β}}_1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\α}}_1& {\mathrm{sin\α}}_1\\ 0& -{\mathrm{sin\α}}_1& {\mathrm{cos\α}}_1\end{array}\right]$

$\begin{array}{ccc}{\mathrm{\γ}}_1=-arctan\left(\frac{y_2-y_1}{x_2-x_1}\right)& {\mathrm{\β}}_1=-arctan\left(\frac{z_2-z_1}{x_2-x_1}\right)& {\mathrm{\α}}_1=-arctan\left(\frac{z_3-z_1}{y_3-y_1}\right)\end{array}$

式中，i＝1、2、3。逆变换矩阵为：

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]={\left[\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\\ z_i^{\′}\end{array}\right]}^T\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\α}}_2& {\mathrm{sin\α}}_2\\ 0& -{\mathrm{sin\α}}_2& {\mathrm{cos\α}}_2\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\β}}_2& 0& -{\mathrm{sin\β}}_2\\ 0& 1& 0\\ {\mathrm{sin\β}}_2& 0& {\mathrm{cos\β}}_2\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\γ}}_2& {\mathrm{sin\γ}}_2& 0\\ -{\mathrm{sin\γ}}_2& {\mathrm{cos\γ}}_2& 0\\ 0& 0& 1\end{array}\right]+\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$

$\begin{array}{ccc}{\mathrm{\α}}_2=arctan\left(\frac{z_3-z_1}{y_3-y_1}\right)& {\mathrm{\β}}_2=arctan\left(\frac{z_2-z_1}{x_2-x_1}\right)& {\mathrm{\γ}}_2=arctan\left(\frac{y_2-y_1}{x_2-x_1}\right)\end{array}$

具体的变换结果如图2所示。

S2.4：公共点坐标变换：将S2.3得到的测量坐标系到标准坐标系的4个正变换矩阵和机器人坐标系到标准坐标系的4个逆变换矩阵，根据特征三角形的对应关系一一对应后形成4对变换矩阵。然后，依次对测量系统下公共点的坐标乘上测量系统坐标系到标准坐标系的正变换矩阵，再乘上对应的逆变换矩阵；

S2.5：均值化处理：将S2.4得到的4种变换结果的坐标值进行均值计算，从而降低公共点坐标误差导致的坐标转换误差；

S2.6：变换矩阵计算：任意提取3个均值化处理后的公共点坐标，形成一个特征三角形，计算该特征三角形到标准坐标系的逆变换矩阵。然后在计算扫描坐标系下对应特征三角形到标准坐标系的正变换矩阵。将上述正变换矩阵和逆变换矩阵作为坐标变换矩阵组。

S3：扫描数据坐标系转换：根据S2计算得到的变换矩阵，对测量系统得到的三维数据进行三维旋转和平移，首先将测量数据先乘上坐标变换矩阵组的正变换矩阵，然后再乘上坐标变换矩阵组的逆变换矩阵，实现测量坐标系到运动坐标系的变换。使测量数据从测量系统的坐标系变换到机器人运动系统的坐标系中。具体的变换过程如图3所示。

附图中描述位置关系的用语仅用于示例性说明，不能理解为对本专利的限制；

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (3)

1.一种机器人坐标系与三维测量坐标系的变换方法，其特征在于，所述方法包括以下步骤：

S1：公共点坐标提取：在机器人工作环境中选定4个公共点，然后在机器人运动坐标系下和三维测量坐标系下，分别提取这4个公共点的三维空间坐标作为坐标系变换的依据；

S2：变换矩阵计算：将4个公共点划分成4个特征三角形，并建立一个标准坐标系；计算运动坐标系下和扫描坐标系下的特征三角形转换到标准坐标系的正变换矩阵和逆变换矩阵；根据正变换矩阵和逆变换矩阵，将4个公共点的扫描坐标转换到运动坐标系下，并求取每个公共点转换后的均值坐标；最后计算公共点的扫描坐标到均值坐标的旋转平移矩阵；

S3：扫描数据坐标系转换：根据S2计算得到的变换矩阵，对测量系统得到的三维数据进行三维旋转和平移，使测量数据从测量系统的坐标系变换到机器人运动系统的坐标系中。

2.根据权利要求1所述的一种机器人坐标系与三维测量坐标系的变换方法，其特征在于，步骤S1中，公共点坐标提取的具体过程为：

S1.1：公共点选取：在机器人工作环境中，选取4个位置不同，且绝对位置不会发生改变的角点作为公共点；

S1.2：公共点的运动坐标提取：通过机器人的示教器或点动控制器，将机器人的末端执行机构依次移动到4个公共点的位置，并按顺序记录4个公共点在运动坐标系下的三维空间坐标；

S1.3：公共点的测量坐标提取：对三维测量系统测量得到的三维数据进行处理，利用角点识别方法，得到4个公共点在测量系统下的三维空间坐标。

3.根据权利要求1所述的一种机器人坐标系与三维测量坐标系的变换方法，其特征在于，步骤S2中，包含以下子步骤：

S2.1：特征三角形划分：对4个公共点进行编号，编号为1、2、3、4；然后抽取一个点作为特征三角形的顶点，选取另一个点与顶点连线形成参考边，最后再选取一个点与另外两点连线形成一个三角形；

S2.2：标准坐标系建立：以特征三角形的顶点作为坐标系的原点，以特征三角形参考边的方向作为X轴方向，以特征三角形平面内垂直于参考边的方向作为Y轴方向，以垂直于三角形平面的方向为Z轴方向建立一个标准坐标系；

S2.3：标准坐标系变换矩阵计算：计算机器人运动坐标系和测量坐标系的特征三角形到标准坐标系的正变换矩阵和逆变换矩阵；令特征三角形三个点的坐标为：(x₁,y₁,z₁)，(x₂,y₂,z₂)，(x₃,y₃,z₃)，则正变换矩阵为：

$\left[\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\\ z_i^{\′}\end{array}\right]={\left[\begin{array}{c}{x}_i-x_1\\ y_i-y_1\\ z_i-z_1\end{array}\right]}^T\left[\begin{array}{ccc}{\mathrm{cos\γ}}_1& {\mathrm{sin\γ}}_1& 0\\ -{\mathrm{sin\γ}}_1& {\mathrm{cos\γ}}_1& 0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\β}}_1& 0& -{\mathrm{sin\β}}_1\\ 0& 1& 0\\ {\mathrm{sin\β}}_1& 0& {\mathrm{cos\β}}_1\end{array}\right]\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\α}}_1& {\mathrm{sin\α}}_1\\ 0& -{\mathrm{sin\α}}_1& {\mathrm{cos\α}}_1\end{array}\right]$

$\begin{array}{ccc}{\mathrm{\γ}}_1=-\arctan \left(\frac{y_2-y_1}{x_2-x_1}\right)& {\mathrm{\β}}_1=-\arctan \left(\frac{z_2-z_1}{x_2-x_1}\right)& {\mathrm{\α}}_1=-\arctan \left(\frac{z_3-z_1}{y_3-y_1}\right)\end{array}$

逆变换矩阵为：

$\left[\begin{array}{c}{x}_i\\ y_i\\ z_i\end{array}\right]={\left[\begin{array}{c}{x}_i^{\′}\\ y_i^{\′}\\ z_i^{\′}\end{array}\right]}^T\left[\begin{array}{ccc}1& 0& 0\\ 0& {\mathrm{cos\α}}_2& {\mathrm{sin\α}}_2\\ 0& -{\mathrm{sin\α}}_2& {\mathrm{cos\α}}_2\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\β}}_2& 0& -{\mathrm{sin\β}}_2\\ 0& 1& 0\\ {\mathrm{sin\β}}_2& 0& {\mathrm{cos\β}}_2\end{array}\right]\left[\begin{array}{ccc}{\mathrm{cos\γ}}_2& {\mathrm{sin\γ}}_2& 0\\ -{\mathrm{sin\γ}}_2& {\mathrm{cos\γ}}_2& 0\\ 0& 0& 1\end{array}\right]+\left[\begin{array}{c}{x}_1\\ y_1\\ z_1\end{array}\right]$

$\begin{array}{ccc}{\mathrm{\α}}_2=\arctan \left(\frac{z_3-z_1}{y_3-y_1}\right)& {\mathrm{\β}}_2=\arctan \left(\frac{z_2-z_1}{x_2-x_1}\right)& {\mathrm{\γ}}_2=\arctan \left(\frac{y_2-y_1}{x_2-x_1}\right)\end{array}$

S2.4：公共点坐标变换：将S2.3得到的测量坐标系到标准坐标系的4个正变换矩阵和机器人坐标系到标准坐标系的4个逆变换矩阵，根据特征三角形的对应关系一一对应后形成4对变换矩阵；然后，依次对测量系统下公共点的坐标乘上测量系统坐标系到标准坐标系的正变换矩阵，再乘上对应的逆变换矩阵；

S2.5：均值化处理：将S2.4得到的4种变换结果的坐标值进行均值计算，从而降低公共点坐标误差导致的坐标转换误差；

S2.6：变换矩阵计算：任意提取3个均值化处理后的公共点坐标，形成一个特征三角形，计算该特征三角形到标准坐标系的逆变换矩阵；然后在计算扫描坐标系下对应特征三角形到标准坐标系的正变换矩阵；最后将上述的正变换矩阵和逆变换矩阵作为一组坐标变换矩阵组，用于实现测量坐标系到运动坐标系的转换。