激光导航机器人及其机器人的里程计校准方法

技术领域

本申请涉及激光导航系统技术领域，尤其涉及激光导航机器人及其机器人的里程计校准方法。

背景技术

现有的机器人上的激光导航方案均涉及到对轮式里程计的应用。通常来说，里程计的标定是在出厂时完成的。然而随着时间的增加，由于轮子的磨损，机械结构的变换等会使得里程计的标定精度下降。当里程计的精度下降到一定程度的时候，激光导航的效果会被严重影响。

申请内容

因此，有必要提供激光导航机器人及其机器人的里程计校准方法，以解决里程计的轮子磨损或机械结构变换导致里程计的标定精度下降影响激光导航的技术问题。

为实现上述目的，本申请提出机器人的激光里程计校准方法，所述机器人用于激光导航，所述里程计用于统计行进轮的运动，所述机器人还包括用于检测机器人移动位置的水平扫描激光雷达，所述方法包括：

所述里程计和激光雷达同时检测机器人从第一位置到第二位置的移动，分别得到里程计数据和激光雷达数据；

根据所述里程计和激光雷达的位置关系信息和所述里程计数据和激光雷达数据构建残差并求解得到里程计参数的最小的解；

以所述最小的解校准里程计参数的初始值。

在一些实施例中，所述方法还包括：

建立里程计坐标系，建立激光雷达坐标系，所述里程计参数包括行进轮参数、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标；

所述里程计和激光雷达同时检测机器人从第一位置到第二位置的移动，分别得到里程计数据和激光雷达数据的步骤，以及所述根据所述里程计和激光雷达的位置关系信息和所述里程计数据和激光雷达数据构建残差并求解得到里程计参数的最小的解的步骤具体包括：

控制机器人从第一位置移动到第二位置，通过所述激光雷达拼接计算出两个位置的相对位姿，并根据所述位置关系信息将所述相对位姿转换到里程计坐标系下得到第一估值；同时所述里程计检测计算机器人从第一位置移动到第二位置的相对位置的第二估值；计算所述第一估值和第二估值及其差值；

所述机器人多次移动产生多组第一位置和第二位置，计算得到多组第一估值和第二估值及其差值；

合并求解多个差值并得到一组使优化目标最小的解。

在一些实施例中，所述合并求解多个差值并得到一组使优化目标最小的解的步骤之后，所述方法还包括：

将所述里程计参数的初始值与所述最小的解比较，若比较结果在预设范围内则以所述最小的解校准所述激光里程计的行进轮参数、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标。

在一些实施例中，所述建立所述里程计坐标系具体包括：

所述里程计坐标系原点是两轮中心的位置o，x轴正方向是垂直于两轮之间的连线的垂线指向轮子正向滚动的方向；y轴方向是两轮直线的连线的方向，y轴正方向是x轴方向绕原点o逆时针旋转90°的方向，z轴方向与地面垂直，z轴正方向是垂直于地面向上的方向；

所述建立激光雷达坐标系具体包括：

所述激光雷达坐标系的原点是激光雷达的激光器测量的原点O，Z轴正方向方向和所述里程计坐标系z轴正方向重合，所述激光雷达坐标系的X轴正方向为所述激光器的正方向，Y轴正方向通过右手法则推导出来，右手拇指方向为所述激光雷达坐标系的X轴正方向，右手四指所指的方向为Y轴正方向。

在一些实施例中，

所述行进轮包括左轮和右轮，所述行进轮参数包括左轮半径、右轮半径、两轮之间的轴距，定义所述左轮半径初始值α＝1，右轮半径初始值β＝1，两轮之间的轴距γ＝1；所述激光雷达坐标系的X轴相对于所述里程计坐标系x轴的旋转角度θ、激光雷达坐标系原点O在里程计坐标系的坐标(x,y)均根据所述激光雷达的安装数据得到；求解所述最小的解使用的优化函数为：

式中，N是总共观测的次数，C是激光拼接的协方差矩阵。

在一些实施例中，

所述优化函数使用高斯牛顿法或线性近似法求解。

在一些实施例中，

所述方法还包括：

对每一组第一估值和第二估值计算差值k

根据k

在一些实施例中，

所述最小的解校准所述激光里程计的左轮半径、右轮半径、两轮之间的轴距、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标之前，所述方法还包括：

以预设检查标准筛选所述最小的解校准所述激光里程计的左轮半径、右轮半径、两轮之间的轴距、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标，以保证所述激光雷达的激光点的距离都在此激光雷达的高精度区域之内。

在一些实施例中，

所述方法还包括：

使用惯性测量单元积分计算所述里程计的双轮的第一旋转参数；同时在同一时间内计算里程计的第二旋转参数；比较所述第一旋转参数和第二旋转参数，若不一致，则删除对应时段内的里程计数据以及激光雷达数据。

为实现上述目的，本申请还提出所述机器人上安装处理器、激光里程计和水平扫描激光雷达，所述处理器用于执行如上述的机器人的激光里程计校准方法。

在一些实施例中，所述机器人上安装惯性测量单元，使用所述惯性测量单元积分计算所述里程计的双轮的第一旋转参数；同时在同一时间内计算单位时间内里程计的第二旋转参数；比较所述第一旋转参数和第二旋转参数，若不一致，则删除对应时段内的里程计数据以及激光雷达数据。

为实现上述目的，本申请还提出计算机可读存储介质，所述计算机可读存储介质上存储有屏幕显示程序，所述屏幕显示程序被所述处理器执行时实现如上述的机器人的激光里程计校准方法的步骤。

本申请实施例提供的激光导航机器人及其机器人的里程计校准方法，里程计和激光雷达同时检测机器人从第一位置到第二位置的移动，分别得到里程计数据和激光雷达数据；根据所述里程计和激光雷达的位置关系信息和所述里程计数据和激光雷达数据构建残差并求解得到里程计参数的最小的解；以所述最小的解校准里程计参数的初始值，从而解决里程计的轮子磨损或机械结构变换导致里程计的标定精度下降的问题。

附图说明

为了更清楚地说明本申请实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本申请的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图示出的结构获得其他的附图。

图1是本申请实施例的机器人结构示意图；

图2是本申请实施例的机器人的激光里程计校准方法的流程图；

图3是本申请另一个实施例的机器人的激光里程计校准方法的流程图；

图4是本申请又一个实施例的机器人的激光里程计校准方法的流程图；

图5是本申请实施例的机器人的激光里程计校准方法的里程计坐标系示意图；

图6是本申请实施例的机器人的激光里程计校准方法的激光雷达坐标系示意图；

图7是本申请实施例的激光导航机器人的结构框图。

具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请的一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明，本申请实施例中所有方向性指示(诸如上、下、左、右、前、后……)仅用于解释在某一特定姿态(如附图所示)下各部件之间的相对位置关系、运动情况等，如果该特定姿态发生改变时，则该方向性指示也相应地随之改变。

另外，在本申请中涉及“第一”、“第二”等的描述仅用于描述目的，而不能理解为指示或暗示其相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括至少一个该特征。另外，各个实施例之间的技术方案可以相互结合，但是必须是以本领域普通技术人员能够实现为基础，当技术方案的结合出现相互矛盾或无法实现时应当认为这种技术方案的结合不存在，也不在本申请要求的保护范围之内。

在本申请中，除非另有明确的规定和限定，术语“连接”、“固定”等应做广义理解，例如，“固定”可以是固定连接，也可以是可拆卸连接，或成一体；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系，除非另有明确的限定。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本申请中的具体含义。

本方法适用于大部分现有的激光导航系统，只要它们具有下列组件：(1)里程计1005及带有里程计的双轮差速电机，(2)水平扫描的单线激光雷达1004，(3)通过SLAM机器人(SLAM(simultaneous localization and mapping),即时定位与地图构建)能够实时估计自己在场景中的位姿，从而能够实现自主导航。场景可以是室内，也可以是室外，如图1示意出机器人的一个例子。机器人100上还包括惯性测量单元1006。

实施例一

本申请实施例提出的机器人的激光里程计校准方法，所述机器人用于激光导航，所述里程计用于统计行进轮的运动。如图1所示，如果机器人是双轮的，则会设置双轮差速电机。实际上本申请实施例能够标定的不止是双轮里程计，也能够标定其他类型的轮子，比如单轮(输出旋转量和平移量)的里程计，只要是能够通过里程计推导出一个位姿变换就可以标定里程计。在下述实施例中的是双轮里程计，行进轮参数包括左轮半径、右轮半径和两轮之间的轴距，但别的类型的里程计可能是其它参数，也一样能够标定，本申请对此不作限制。

所述机器人还包括用于检测机器人移动位置的水平扫描激光雷达，所述方法包括：

可选地，在一些实施例中，事先建立里程计坐标系和激光雷达坐标系。

如图2所示，所述方法包括：

步骤1、所述里程计和激光雷达同时检测机器人从第一位置到第二位置的移动，分别得到里程计数据和激光雷达数据；

具体地，第一位置可以定义为机器人的初始位置，第二位置定义为机器人的目标位置。

步骤2、根据所述里程计和激光雷达的位置关系信息和所述里程计数据和激光雷达数据构建残差并求解得到里程计参数的最小的解；

步骤3、以所述最小的解校准里程计参数的初始值。

进一步地，如图3和图4所示，所述方法还包括：

步骤4、建立里程计坐标系，建立激光雷达坐标系，里程计参数包括行进轮参数、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标；

所述步骤2和步骤3具体包括：

步骤20、控制机器人从第一位置移动到第二位置，通过水平扫描激光雷达拼接计算出两个位置的相对位姿，并根据所述位置关系信息将所述相对位姿转换到里程计坐标系下得到第一估值；同时所述里程计检测计算机器人从第一位置移动到第二位置的相对位置的第二估值；计算所述第一估值和第二估值及其差值；

激光雷达拼接计算出第一位置和第二位置这两个位置的相对位姿可使用现有的计算方法得到。

具体地，激光雷达与所述机器人的位置关系信息可根据激光导航系统的安装图纸得到。图1中示例出激光雷达在机器人上的安装位置。

里程计检测计算机器人从第一位置移动到第二位置，需要结合里程计的双轮差速电机的各种参数设置。

进一步地，所述方法还包括：

步骤5、所述机器人多次移动产生多组第一位置和第二位置，计算得到多组第一估值和第二估值及其差值；

具体地，第一估值也可称为第一计算估值，第二估值也可称为第二计算估值。

步骤6、合并求解多个差值并得到一组使优化目标最小的解；

计算出最小的解之后，以所述最小的解校准所述激光里程计的行进轮参数、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标。

具体地，建立所述里程计坐标系具体包括：所述里程计坐标系原点是两轮中心的位置o，x轴正方向是垂直于两轮之间的连线的垂线指向轮子正向滚动的方向；y轴方向是两轮直线的连线的方向，y轴正方向是x轴方向绕原点o逆时针旋转90°的方向，z轴方向与地面垂直，z轴正方向是垂直于地面向上的方向；

具体地，建立激光雷达坐标系具体包括：所述激光雷达坐标系的原点是激光雷达的激光器测量的原点O，Z轴正方向方向和所述里程计坐标系z轴正方向重合，所述激光雷达坐标系的X轴正方向为所述激光器的正方向，Y轴正方向通过右手法则推导出来，右手拇指方向为所述激光雷达坐标系的X轴正方向，右手四指所指的方向为Y轴正方向。

里程计坐标系图见图5，激光雷达坐标系图见图6。

本申请实施例提供的机器人的里程计校准方法，通过激光里程计自身对机器人移动的检测计算得到的第二估值，以及借助水平扫描激光雷达对机器人从第一位置移动到第二位置的拼接计算，并换算到里程计坐标系下得到第一估值，机器人的一次移动得到两个计算估值，通过优化计算多组第一估值和第二估值及其差值并合并求解，得到一组使优化目标最小的解，最小的解包括所述里程计参数；以所述最小的解校准所述行进轮参数、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标，由于重新标定了里程计参数，从而解决里程计的轮子磨损或机械结构变换导致里程计的标定精度对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标，由于重新标定/校准了里程计参数，从而解决里程计的轮子磨损或机械结构变换导致里程计的标定精度下降的问题。

进一步地，所述合并求解多个差值并得到一组使优化目标最小的解的步骤之后，所述方法还包括：

步骤31、将所述里程计参数的初始值与所述最小的解比较，若比较结果在预设范围内则以所述最小的解校准所述激光里程计的行进轮参数、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标。

即，并不直接将最小的解来校准里程计参数，还是先与左轮半径、右轮半径、两轮之间的轴距、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标的初始值做比较，比较结果在预设范围内才使用最小的解来校准。

所述预设范围包括：

最小的解中的左轮半径是左轮半径初始值alpha的0.9-1.1倍；

最小的解中的右轮半径是右轮半径初始值beta的0.9-1.1倍；

最小的解中的两轮之间的轴距是两轮之间的轴距的初始值gamma的0.9-1.1倍；

最小的解中的激光雷达坐标系X轴相对于里程计坐标系x轴的旋转角度是激光雷达坐标系X轴相对于里程计坐标系x轴的旋转角度的初始值θ的上下10度；

最小的解中的激光雷达坐标系原点O在里程计坐标系下的坐标，是激光雷达坐标系原点O在里程计坐标系下的坐标的初始值(x,y)中x的上下5厘米，y的上下5厘米。

里程计包括以下三个参数：(1)左轮半径alpha*l，(2)右轮半径beta*r，(3)两轮之间的轴距gamma*d。l,r,d这三个参数是由物理模型决定的。但由于磨损等原因，每一个参数并不是准确的，在本申请实施例中使用alpha，beta，gamma描述这种误差。

所述里程计的左轮半径初始值α＝1，右轮半径初始值β＝1，两轮之间的轴距γ＝1；所述激光雷达坐标系的X轴相对于所述里程计坐标系x轴的旋转角度θ、激光雷达坐标系原点O在里程计坐标系的坐标(x,y)均根据所述激光雷达的安装数据得到；

即，本方法要标定(校准)的里程计参数有：

(1)α；

(2)β；

(3)γ；

(4)激光雷达坐标系X轴相对于里程计坐标系x轴的旋转θ(Z/z轴重合所以只有1个维度的旋转)；

(5)激光雷达坐标系原点O在里程计坐标系下的x与y(Z/z轴重合所以只有x，y)。

其中，定义里程计参数的初始值为：

α＝1；

β＝1；

γ＝1；

θ＝从安装图纸(激光雷达的安装数据得到)得出；

x：从安装图纸得出初始值；

y：从安装图纸得出；

本申请实施例构建一个包含上述参数的残差，然后用优化算法解出残差最小时对应的各物理量。

构建残差：当机器人从一个第一位置(Pi)移动到一个第二位置(Pi

式中，N是总共观测的次数，C是激光拼接的协方差矩阵；实际上，通过合并多次测量寻求一组使优化目标最小的解。由于初始值一般较为精确，这个优化函数可以用通用的优化算法比如高斯牛顿法等求解，也可以用线性近似法求解。

在一些实施例中，考虑到实际运行中，本发明实施例机器人的激光里程计校准方法还需要考虑众多实际发生的问题，下面将针对可能遇到的问题提出相应的解决方法：

问题(1)里程计打滑问题：在现实场景中，对于某些地面和轮子的组合，轮子会出现打滑的情况。反应在数学模型中，打滑造成了一个离群点，而离群点对于最小二乘的构建方式是致命的，所以我们需要精确的祛除发生打滑的数据。我们提出增加一个IMU(惯性测量单元)来解决此问题。由于轮子打滑，在单位时间内里程计计算出的旋转和IMU在同一时间内积分出的旋转并不一致，此时间周期内的里程计数据和激光雷达数据不应该被加入到优化中。

所述方法还包括：

使用惯性测量单元积分计算所述里程计的双轮的第一旋转参数；同时在同一时间内计算里程计的第二旋转参数；比较所述第一旋转参数和第二旋转参数，若不一致，则删除该时间内检测的里程计数据。

里程计通过里程计的参数设置来计算旋转。

问题(2)激光拼接不准确以及拼接错误问题。C作为协方差矩阵反应了激光拼接的优劣程度，在数据筛选中选择那些激光拼接较好的数据。但是协方差矩阵C无法反映错误的拼接。也就说错误的拼接也可能有较好的协方差矩阵C。为解决问题(2)提出：

所述方法还包括：

对每一组第一估值和第二估值计算差值k

然后计算k

问题(3)激光的测量误差：市面上的激光器在特定距离会有比较大误差。比如基于TOF原理的激光雷达在10厘米以内的误差相对较大，小型三角测距的激光雷达在10m之外的误差较大。这些误差的大小不会反应到C矩阵之中，所以需要对记录的标定数据做一次附加的检查，保证里面的激光点的距离都在此激光雷达的高精度区域之内。

为解决这个问题，所述方法还包括：

所述最小的解校准所述激光里程计的左轮半径、右轮半径、两轮之间的轴距、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标之前，所述方法还包括：

以预设检查标准筛选所述最小的解校准所述激光里程计的左轮半径、右轮半径、两轮之间的轴距、激光雷达坐标系X轴相对于里程计坐标系x轴的旋转和激光雷达坐标系原点O在里程计坐标系下的坐标，以保证所述激光雷达的激光点的距离都在此激光雷达的高精度区域之内。

由于市面上的激光器在特定距离会有比较大误差。比如TOF激光雷达的激光在10厘米以内的误差相对较大(其10厘米-20米内精度最高)，小型三角测距的激光雷达在10m之外的误差较大(其在0-8m内精度最高)。这些误差的大小不会反应到激光拼接的协方差矩阵C中，因此需要对记录的标定数据做一次附加的检查，保证里面的激光点的距离都在此激光雷达的高精度区域之内。

以三角法的激光雷达为例，其在0-8m内精度最高，所以如果某一段数据中包含了测量出的10m距离，通过判断某一段数据是否包含超出精度范围的测量，那么这段数据(里程计数据以及激光雷达数据)就不加入到优化之中，因为这些数据不够准确。具体要结合具体的激光雷达型号来判断，故预设检查标准也根据具体情况设定。

在一些实施例中，提出激光导航系统的机器人的里程计校准办法，所述激光导航系统包括机器人，所述机器人上安装激光里程计和水平扫描激光雷达，所述方法包括：

启动机器人自动导航；

在行程过程中记录预设时间的里程计数据以及激光雷达数据，检测机器人的停止状态，在所述停止状态下暂停记录；所述预设时间的范围为2-4秒记录阈值次数后停止记录数据；所述阈值次数的范围为100-10000次。

对记录的数据进行筛选剔除(避免由于机构雷达的精度问题带来的测量误差)，若数据量少于100条则继续记录。

开始计算所述第一估值和第二估值及其差值；

所述机器人多次移动产生多组第一位置和第二位置，计算得到多组第一估值和第二估值及其差值；

计算完成后对多组第一估值和第二估值及其差值和初始值进行对比，相差不大则接受标定(校准里程计的六个参数α,β,γ,θ,x,y)，否则判定标定失败。

每相隔一段时间后重复上述步骤，间隔时间可根据机器人使用率调整。以最小的解校准激光里程计参数，由于持续标定了里程计参数，从而解决里程计的轮子磨损或机械结构变换导致里程计的标定精度下降的问题。

实施例二

如图7所示，本发明实施例还提供激光导航机器人200，所述机器人上安装处理器201、激光里程计202和水平扫描激光雷达203，所述处理器201用于执行如实施例一的机器人的激光里程计校准方法。

具体地，所述机器人上安装惯性测量单元，使用所述惯性测量单元积分计算所述里程计的双轮的第一旋转参数；同时在同一时间内计算单位时间内里程计的第二旋转参数；比较所述第一旋转参数和第二旋转参数，若不一致，则删除对应时段内的里程计数据以及激光雷达数据。

需要说明的是，本实施例的激光导航机器人，与第一实施例的方法属于同一构思，其具体实现过程详细见方法实施例，且方法实施例中的技术特征在本实施例中均对应适用，这里不再赘述。

本申请实施例提供的激光导航机器人，里程计和激光雷达同时检测机器人从第一位置到第二位置的移动，分别得到里程计数据和激光雷达数据；根据所述里程计和激光雷达的位置关系信息和所述里程计数据和激光雷达数据构建残差并求解得到里程计参数的最小的解；以所述最小的解校准里程计参数的初始值。由于重新标定了里程计参数，从而解决里程计的轮子磨损或机械结构变换导致里程计的标定精度下降的问题。

实施例三

本申请第三实施例提供计算机可读存储介质，所述计算机可读存储介质上存储有机器人的里程计校准的程序，所述机器人的里程计校准的程序被处理器执行时用于实现第一实施例所述的机器人的里程计校准的方法的步骤。

需要说明的是，本实施例的计算机可读存储介质，与第一实施例的方法属于同一构思，其具体实现过程详细见方法实施例，且方法实施例中的技术特征在本实施例中均对应适用，这里不再赘述。

本领域普通技术人员可以理解，上文中所公开方法中的全部或某些步骤、系统、装置中的功能模块/单元可以被实施为软件、固件、硬件及其适当的组合。在硬件实施方式中，在以上描述中提及的功能模块/单元之间的划分不一定对应于物理组件的划分；例如，一个物理组件可以具有多个功能，或者一个功能或步骤可以由若干物理组件合作执行。某些物理组件或所有物理组件可以被实施为由处理器，如中央处理器、数字信号处理器或微处理器执行的软件，或者被实施为硬件，或者被实施为集成电路，如专用集成电路。这样的软件可以分布在计算机可读介质上，计算机可读介质可以包括计算机存储介质(或非暂时性介质)和通信介质(或暂时性介质)。如本领域普通技术人员公知的，术语计算机存储介质包括在用于存储信息(诸如计算机可读指令、数据结构、程序模块或其他数据)的任何方法或技术中实施的易失性和非易失性、可移除和不可移除介质。计算机存储介质包括但不限于RAM、ROM、EEPROM、闪存或其他存储器技术、CD-ROM、数字多功能盘(DVD)或其他光盘存储、磁盒、磁带、磁盘存储或其他磁存储装置、或者可以用于存储期望的信息并且可以被计算机访问的任何其他的介质。此外，本领域普通技术人员公知的是，通信介质通常包含计算机可读指令、数据结构、程序模块或者诸如载波或其他传输机制之类的调制数据信号中的其他数据，并且可包括任何信息递送介质。

以上仅为本申请的优选实施例，并非因此限制本申请的专利范围，凡是在本申请的申请构思下，利用本申请说明书及附图内容所作的等效结构变换，或直接/间接运用在其他相关的技术领域均包括在本申请的专利保护范围内。