CN105973240B - 一种导航模块坐标系与机器人坐标系的转换方法 - Google Patents

Abstract

本发明涉及一种导航模块坐标系与机器人坐标系的转换方法，包括：S100,机器人自转，利用所述相机拍摄的第一预设路标计算出导航模块坐标系原点与机器人坐标系原点的偏移量；S200,机器人直线行走，利用所述相机拍摄的第二预设路标计算出导航模块坐标系与机器人坐标系的X轴旋转角；S300,计算导航模块坐标系与机器人坐标系的转换参数，确定转换关系矩阵。本发明通过机器人自转与直线行走，计算出导航模块坐标系与机器人坐标系之间的位移加旋转关系，就可以得到导航模块坐标系与机器人坐标系之间的转换关系，导航模块坐标系可与机器人坐标系进行转换，使得机器人能够在定位与导航过程中得到准确的基于自身的坐标与位姿，导航精度更高。

Description

一种导航模块坐标系与机器人坐标系的转换方法

技术领域

本发明属于机器人视觉导航领域，特别涉及一种导航模块坐标系与机器人坐标系的转换方法。

背景技术

为了实现机器人视觉导航，需要设置路标和导航模块，所述路标可以设置在室内的天花板上；所述导航模块为了拍摄与识别所述路标，一般包括相机、图像采集单元、图像处理单元与计算单元，所述相机设置在机器人头部的顶端，所述图像采集单元、图像处理单元、计算单元设置在机器人内部。

一般而言，所述导航模块坐标系是指所述相机的坐标系，更确切的说是指所述相机的感光芯片靶面上设定的坐标系；所述机器人坐标系是指基于机器人本身的坐标系，根据机器人的整体结构与导航精度要求进行设定，比如可以将所述机器人坐标系原点设定为机器人上表面的中心。

因而，由于所述导航模块中的相机所处位置、或者设计误差与安装误差，可能导致所述导航模块坐标系与所述机器人坐标系不相同，导航过程中机器人不能准确识别自身的坐标及姿态，得到的是所述导航模块的坐标及姿态。

发明内容

本发明的目的在于解决现有技术中的机器人不能准确识别自身坐标及姿态，得到的是所述导航模块的坐标及姿态的问题。

为实现上述目的，一种导航模块坐标系与机器人坐标系的转换方法，所述机器人包括导航模块，并通过导航模块和设置的路标进行视觉导航，所述导航模块包括相机，其特征在于，包括：

S100：机器人自转，利用所述相机拍摄的第一预设路标计算出导航模块坐标系原点与机器人坐标系原点的偏移量；

S200：机器人直线行走，利用所述相机拍摄的第二预设路标计算出导航模块坐标系与机器人坐标系的X轴旋转角；

S300：计算导航模块坐标系与机器人坐标系的转换参数，确定转换关系矩阵。

所述机器人坐标系原点为机器人的竖直轴线上任意一点，机器人坐标系所在的平面与所述机器人的竖直轴线互相垂直。

所述导航模块坐标系原点是指所述相机感光靶面上分辨率划分的第0行第0列的顶点。

所述S100包括：

S110:机器人自转,所述相机拍摄所述第一预设路标3幅或3幅以上的图像；

S120:所述第一预设路标在相机的感光芯片靶面上的成像形成对应的点，3个或3个以上的不在同一条直线上的对应的点经过计算拟合出所述第一预设路标的圆形运动轨迹；

S130：计算所述圆形运动轨迹的圆心与其在所述导航模块坐标系下的坐标，所述圆心就是所述机器人坐标系原点在所述导航模块坐标系中的投影点，所述坐标就是所述导航模块坐标系原点与机器人坐标系原点的偏移量。

所述S100中，机器人在能够拍摄到所述第一预设路标的任意位置自转。

进一步地，所述S100中，所述机器人在所述第一预设路标正下方自转。

所述机器人坐标系的X轴正方向是机器人向前直线行走的方向，经过所述机器人坐标系原点的为X轴；所述机器人的右侧与所述X轴垂直的指向所述机器人坐标系原点的反方向为Y轴的正方向，经过所述机器人坐标系原点的为Y轴。

所述导航模块坐标系的X轴是指所述相机的感光芯片靶面中第0行的边，指向所述导航模块坐标系原点的反方向为X轴的正方向；所述相机的感光芯片靶面中的第0列的边为所述导航模块坐标系的Y轴，指向所述导航模块坐标系原点的反方向为Y轴的正方向。

所述S200包括：

S210，机器人向前直线行走，所述相机拍摄2幅或2幅以上的所述第二预设路标的图像；

S220，所述第二预设路标在所述相机的感光芯片靶面上的成像具有对应的点，2个或2个以上的对应的点经过计算拟合出了所述第二预设路标的直线运动轨迹；

S230，所述直线运动轨迹与所述机器人坐标系X轴平行，计算所述直线运动轨迹与所述导航模块坐标系X轴的夹角，该夹角即为所述导航模块坐标系与所述机器人坐标系X轴的旋转角。

所述S200中，所述机器人在能够拍到所述第二预设路标的任意位置向前直线行走。

进一步地，所述S200中，所述机器人在所述第二预设路标正下方位置向前直线行走。

所述第一预设路标与第二预设路标可以为同一个路标。

所述转换参数包括导航模块坐标系与机器人坐标系原点的偏移量(tx,ty)，以及机器人坐标系X轴与导航模块坐标系X轴的旋转角θ，所述转换关系矩阵为：

其中，(x,y)是机器人坐标系中的一个坐标，(x′,y′)是(x,y)在导航模块坐标系中的坐标。

与现有技术相比，本技术方案具有以下优点：

本技术方案的一种导航模块坐标系与机器人坐标系的转换方法，通过机器人做自转与直线行走，计算出导航模块坐标系与机器人坐标系之间的位移加旋转关系，就可以得到导航模块坐标系与机器人坐标系之间的转换关系，导航模块坐标系可与机器人坐标系进行转换，使得机器人能够在定位与导航过程中得到准确的基于自身的坐标与位姿，导航精度更高。

附图说明

图1为本发明一种导航模块坐标系与机器人坐标系的转换方法的流程图。

图2为本发明机器人坐标系原点与导航模块坐标系原点的偏移量的示意图。

图3为本发明一种导航模块坐标系与机器人坐标系的转换方法的示意图。

(注意：附图中的所示结构只是为了说明本发明特征的示意，并非是要依据附图所示结构。)

具体实施方式

为使本发明的上述目的、特征和优点能够更为明显易懂，下面结合附图对本发明的具体实施例做详细的说明。

本发明提供了一种导航模块坐标系与机器人坐标系的转换方法。

所述机器人坐标系的原点位于机器人的竖直轴线上，机器人坐标系所在的平面与所述机器人的竖直轴线互相垂直。

所述机器人坐标系所在平面与所述机器人竖直轴线垂直。

所述导航模块坐标系是指所述导航模块中设置的相机的坐标系，所述导航模块坐标系位于所述相机的感光芯片靶面上，所述相机的感光芯片靶面与所述竖直轴线垂直。

所述机器人坐标系与所述导航模块坐标系位于同一平面或位于不同的互相平行的平面。

所述的导航模块坐标系与机器人坐标系转换方法中，还需利用为了导航而设置的若干路标。

本发明中，机器人在空间内的导航不涉及到机器人往上走或往下走等高度上的变化，因而只需考虑机器人在水平地面上的运动，也就是说，将所述导航模块坐标系与机器人坐标系转换到同一个平面上即可计算导航模块坐标系与机器人坐标系之间的转换。

参考图1，所述的导航模块坐标系与机器人坐标系的转换方法，包括如下步骤：

S100，机器人自转，利用所述相机拍摄的第一预设路标计算出导航模块坐标系原点与机器人坐标系原点的偏移量。

所述导航模块坐标系是指导航模块包括的所述相机的坐标系，所述相机用于机器人的视觉导航，现有技术中相机通常设置在机器人头部的顶端；所述导航模块坐标系的原点是指具有特定分辨率的相机感光芯片靶面中的点，所述分辨率数量与所述感光芯片靶面中的行列数数量相同。

在本实施例中，所述相机分辨率为640*480,即所述相机有640个像素点，所述感光芯片靶面分为640列与480行，所述感光芯片靶面的第0行第0列的顶点为导航模块坐标系的原点。

所述机器人坐标系原点，位于机器人的竖直轴线上任意一点，机器人坐标系所在的平面与所述机器人的竖直轴线互相处置。

所述机器人坐标系原点与所述机器人的车轮转速、方向以及车轮间的距离有关；所述机器人坐标系原点所在的竖直轴线穿过机器人自转时车轮运动轨迹的圆心。

在本实施例中，所述机器人有两个车轮，所述车轮相同，所述车轮转速设定相同，当所述车轮做顺时针或逆时针运动使得机器人自转时，车轮运动轨迹为一个圆，所述机器人的竖直轴线穿过所述车轮圆形运动轨迹的圆心。

在其他实施例中，所述机器人有三个车轮，所述车轮相同，所述车轮间的距离、方向、转速设定相同，当所述车轮做顺时针或逆时针运动驱动机器人自转时，车轮运动轨迹为一个圆，穿过圆心的竖直轴线上分布着所述机器人坐标系的原点，也就是说，所述机器人坐标系的原点可以是所述圆心、可以是所述竖直轴线与路标所在平面的交点、可以是所述轴线与所述导航模块坐标系所在平面的交点。

在本实施例中，参考图2，所述S100包括：

S110，所述机器人在所述第一预设路标正下方以经过机器人坐标系原点的竖直轴线为轴自转，在所述机器人自转时，如果以机器人作为参照物，则所述第一预设路标在相对于所述机器人与所述相机转动，所述第一预设路标在所述相机的感光芯片靶面1上成像，所述相机拍摄所述第一预设路标3幅或3幅以上的图像；

S120，所述第一预设路标在相机的感光芯片靶面上的成像具有对应的点，在所述导航模块坐标系O′X′Y′中，利用3个或3个以上的不在同一直线上的对应的点，经过最小二乘法拟合出所述第一预设路标的圆形运动轨迹；

S130，所述圆形运动轨迹的圆心正好是所述机器人坐标系原点在所述导航模块坐标系平面的投影点Q，将所述机器人坐标系转换到了所述导航模块坐标系平面上，根据数学中的已知圆求圆心，确定圆心的位置，并且计算出该圆心在所述导航模块坐标系O′X′Y′下的坐标，就可以得到所述机器人坐标系原点到所述导航模块坐标系原点的偏移量。

在其他实施例中，所述机器人在所述相机能够拍摄到所述预设路标的任意位置自转，只要在所述导航模块坐标系中的3幅或3幅以上的成像形成不在同一直线上的3个或3个以上的点即可，此时可以运用最小二乘法拟合出圆形轨迹；同样，计算出的所述第一预设路标圆形运动轨迹的圆心在所述导航模块坐标系下的坐标，即为所述机器人坐标系原点在所述导航模块坐标系下的坐标，即为所述机器人坐标系原点与所述导航模块坐标系原点的偏移量。

在其他实施例中，也可以通过增加相机分辨率来使所述机器人自转时所述相机采集到的尽可能多的图像形成的是所述第一预设路标相对运动的近圆形轨迹，这样可以减少最小二乘法或其他方法再计算圆心，也就是所述机器人坐标系原点在所述导航模块坐标系平面的投影点的坐标。

S200，机器人直线行走，利用所述相机拍摄的第二预设路标计算出导航模块坐标系与机器人坐标系的X轴的旋转角。

所述的第二预设路标与S100中所述的第一预设路标可以是同一个路标，也可以是不同的路标。

在本实施例中，S100中所述的第一预设路标与所述第二预设路标选择了同一个路标。

所述导航模块坐标系的X轴与Y轴可以是所述相机的感光芯片靶面上互相垂直的任意直线。

在本实施例中，所述导航模块坐标系的X轴，是指所述相机的感光芯片靶面中的第0行的边，指向所述导航模块坐标系原点的反方向为X轴的正方向；所述相机的感光芯片靶面中的第0列的的边为所述导航模块坐标系的Y轴，指向所述导航模块坐标系原点的反方向为Y轴的正方向。

在其他实施例中，所述导航模块坐标系的X轴与Y轴可以做其他的设定。比如，可以规定感光芯片靶面上中的第1行第1列的两条垂直边分别为所述导航模块坐标系的坐标轴，交点为所述导航模块坐标系原点。

所述机器人坐标系的X轴正方向是指机器人向前直线行走的方向。

在本实施例中，所述机器人坐标系的X轴正方向是指在机器人车轮速度相同时向前直线行走的方向，经过所述机器人坐标系原点的为X轴；所述机器人的右侧与所述X轴垂直的指向所述机器人坐标系原点的反方向为Y轴的正方向，且经过所述机器人坐标系原点的为Y轴。

在本实施例中，机器人停止自转后，所述机器人在第二预设路标正下方，两个车轮以相同的速度向前直线行走，即沿所述机器人坐标系OXY的X轴正方向行走。

在其他实施例中，所述机器人可以在能够拍摄到所述第二预设路标的任意位置行走，只要所述机器人是沿所述机器人坐标系OXY的X轴正方向行走即可。

在本实施例中，参考图3，所述S200包括：

S210，所述机器人向前直线行走，所述相机拍摄2幅或2幅以上的所述第二预设路标的图像；

S220,如果以所述机器人作为参照物，则所述第二预设路标相对于所述机器人与所述相机运动，所述第二预设路标在所述相机的感光芯片靶面1上成像具有对应的点，在所述导航模块坐标系O′X′Y′中，利用2个或2个以上的对应的点，采用最小二乘法拟合出所述第二预设路标的直线运动轨迹；

S230,所述直线运动轨迹与所述机器人坐标系OXY的X轴平行，可看做是所述机器人坐标系在所述导航模块坐标系中的投影线，所述直线运动轨迹与所述导航模块坐标系O′X′Y′的X′轴具有一个旋转角θ，因此，得到所述导航模块坐标系O′X′Y′的X′轴与所述机器人坐标系OXY的X轴的旋转角θ。

S300，计算导航模块坐标系与机器人坐标系的转换参数，确定转换关系矩阵。

通过S100与S200已经确定的所述导航模块坐标系与机器人坐标系之间的原点偏移量与旋转角θ，基于平面直角坐标系之间的转换关系确定所述导航模块坐标系与所述机器人坐标系之间的转换矩阵。

在本实施例中，已知偏移量与旋转角，基于平面直角坐标系之间的转换关系就可以确定所述导航模块坐标系与所述机器人坐标系的转换矩阵。

参考图3，OXY为机器人坐标系，(x,y)是机器人坐标系中的坐标；O′X′Y′是导航模块坐标系，(x′,y′)是导航模块坐标系中的坐标；机器人坐标系原点O在导航模块坐标系O′X′Y′中的坐标为(tx,ty),即坐标(tx,ty)是所述导航模块坐标系原点O′与所述机器人坐标系原点O的偏移量；机器人坐标系的X轴与导航模块坐标系X′轴的旋转角为θ；坐标转换公式是:x′＝xcosθ-ysinθ+tx,y′＝xsinθ+ycosθ+ty。

即，转换矩阵如下所示：

为了更进一步简化公式，便于坐标系之间的转换计算，引入齐次坐标形式，则得到以下公式：

将所述导航模块坐标系转换成所述机器人坐标系，则是在上述公式两边乘以一个相同的逆矩阵。

通过以上计算完成的转换矩阵，任意所述机器人坐标系下的坐标均可以转换为所述导航模块坐标系下的坐标；通过乘以逆矩阵所述导航模块坐标系下的坐标也可以转换为所述机器人坐标系下的坐标。

本技术方案的一种导航模块坐标系与机器人坐标系的转换方法，通过机器人做自转与直线行走，计算出导航模块坐标系与机器人坐标系之间的位移加旋转关系，就可以得到导航模块坐标系与机器人坐标系之间的转换关系，导航模块坐标系可与机器人坐标系进行转换，使得机器人能够在定位与导航过程中得到准确的基于自身的坐标与位姿，导航精度更高。

以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (12)

1.一种导航模块坐标系与机器人坐标系的转换方法，所述机器人包括导航模块，并通过导航模块和设置的路标进行视觉导航，所述导航模块包括相机，其特征在于，包括：

S100：机器人自转，利用所述相机拍摄的第一预设路标计算出导航模块坐标系原点与机器人坐标系原点的偏移量；

S200：机器人直线行走，利用所述相机拍摄的第二预设路标计算出导航模块坐标系与机器人坐标系的X轴旋转角；

S300：计算导航模块坐标系与机器人坐标系的转换参数，确定转换关系矩阵；

所述S100包括：

S110:机器人自转,所述相机拍摄所述第一预设路标3幅或3幅以上的图像；

S120:所述第一预设路标在相机的感光芯片靶面上的成像具有对应的点，3个或3个以上的不在同一条直线上的对应的点经过计算拟合出所述第一预设路标的圆形运动轨迹；

S130：计算所述圆形运动轨迹的圆心与其在所述导航模块坐标系下的坐标，所述圆心就是所述机器人坐标系原点在所述导航模块坐标系中的投影点，所述圆心坐标就是所述导航模块坐标系原点与机器人坐标系原点的偏移量。

2.根据权利要求1所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述机器人坐标系原点为机器人的竖直轴线上任意一点，机器人坐标系所在的平面与所述机器人的竖直轴线互相垂直。

3.根据权利要求1所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述导航模块坐标系原点是指所述相机感光靶面上分辨率划分的第0行第0列的点。

4.根据权利要求1所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述S100中，机器人在能够拍摄到所述第一预设路标的任意位置自转。

5.根据权利要求4所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述S100中，所述机器人在所述第一预设路标正下方自转。

6.根据权利要求1所述导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述机器人坐标系的X轴正方向是机器人向前直线行走的方向，经过所述机器人坐标系原点的为X轴；所述机器人的右侧与所述X轴垂直的指向所述机器人坐标系原点的反方向为Y轴的正方向，经过所述机器人坐标系原点的为Y轴。

7.根据权利要求1所述导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述导航模块坐标系的X轴是指所述相机的感光芯片靶面中第0行的边，指向所述导航模块坐标系原点的反方向为X轴的正方向；所述相机的感光芯片靶面中的第0列的边为所述导航模块坐标系的Y轴，指向所述导航模块坐标系原点的反方向为Y轴的正方向。

8.根据权利要求1或6或7所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述S200包括：

S210，机器人向前直线行走，所述相机拍摄2幅或2幅以上的所述第二预设路标的图像；

S220，所述第二预设路标在所述相机的感光芯片靶面上的成像具有对应的点，2个或2个以上的对应的点经过计算拟合出了所述第二预设路标的直线运动轨迹；

S230，所述直线运动轨迹与所述机器人坐标系X轴平行，计算所述直线运动轨迹与所述导航模块坐标系X轴的夹角，该夹角即为所述导航模块坐标系与所述机器人坐标系X轴的旋转角。

9.根据权利要求1所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述S200中，所述机器人在能够拍到所述第二预设路标的任意位置向前直线行走。

10.根据权利要求9所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述S200中，所述机器人在所述第二预设路标正下方位置向前直线行走。

11.根据权利要求1所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述第一预设路标与第二预设路标可以为同一个路标。

12.根据权利要求1所述的导航模块坐标系与机器人坐标系的转换方法，其特征在于，所述转换参数包括导航模块坐标系与机器人坐标系原点的偏移量(tx,ty)，以及机器人坐标系X轴与导航模块坐标系X轴的旋转角θ，所述转换关系矩阵为：

其中，(x,y)是机器人坐标系中的一个坐标，(x',y')是(x,y)在导航模块坐标系中的坐标。