CN108890640A - 一种基于同步定位与地图构建技术的机器人设备校准方法 - Google Patents

Abstract

本发明中提出的一种基于同步定位与地图构建技术的机器人设备校准方法，其主要内容包括：获取姿势转换、运动与求解参数之间的关系、离线机器人‑同步定位与地图构建(SLAM)设备校准、在线位置调整，其过程为，先获取机器人和SLAM装置之间的相对位置和方向，然后分别考虑水平旋转、垂直旋转、向前运动与求解参数之间的关系，接着使用双向旋转校准和使用水平移动和SLAM设备的高度的辅助信息校准，最后用在线位置校准方法校准机器人和SLAM设备之间的相对位置。本发明通过调整机器人和设备的相对位置和方向，从而保持设备‑机器人外部环境信息的一致性，减少导航期间发生的动态错误，提高了导航定位的精度。

Description

一种基于同步定位与地图构建技术的机器人设备校准方法

技术领域

本发明涉及机器人导航领域，尤其是涉及了一种基于同步定位与地图构建技术的机器人设备校准方法。

背景技术

随着科学技术的进步和智能控制***的发展，机器人技术取得了巨大的进步。机器人技术的快速提升为自动化工业、智能化家用电器、未来超市等高新技术产业的发展带来更大的可能。然而，机器人导航技术是现在机器人发展的瓶颈之一。许多地面机器人常常需要在各种场景中自主执行任务，如家庭机器人(扫地机器人等)、物流仓储机器人(用于在仓库拣货和分区放置货物)、送餐机器人等，这些机器人都需要在完全未知环境中创建地图，同时利用地图进行自主定位和导航。现有的机器人导航技术没有考虑对机器人动作自由的限制，使得机器人导航时误差逐渐增大，无法在离线状态下进行自主定位和校准，因此限制了它们的应用。

本发明提出了一种基于同步定位与地图构建技术的机器人设备校准方法，先获取机器人和SLAM装置之间的相对位置和方向，然后分别考虑水平旋转、垂直旋转、向前运动与求解参数之间的关系，接着使用双向旋转校准和使用水平移动和SLAM设备的高度的辅助信息校准，最后用在线位置校准方法校准机器人和SLAM设备之间的相对位置。本发明通过调整机器人和设备的相对位置和方向，从而保持设备-机器人外部环境信息的一致性，减少导航期间发生的动态错误，提高了导航定位的精度。

发明内容

针对无法在离线状态下进行自主定位和校准的问题，本发明的目的在于提供一种基于同步定位与地图构建技术的机器人设备校准方法，先获取机器人和SLAM装置之间的相对位置和方向，然后分别考虑水平旋转、垂直旋转、向前运动与求解参数之间的关系，接着使用双向旋转校准和使用水平移动和SLAM设备的高度的辅助信息校准，最后用在线位置校准方法校准机器人和SLAM设备之间的相对位置。

为解决上述问题，本发明提供一种基于同步定位与地图构建技术的机器人设备校准方法，其主要内容包括：

(一)获取姿势转换；

(二)运动与求解参数之间的关系；

(三)离线机器人-同步定位与地图构建设备校准；

(四)在线位置调整。

其中，所述的同步定位与地图构建(SLAM)技术，将实时三维感测装置称为“SLAM装置”；SLAM设备具有传感、映射外部环境和实时估计自身位置和姿态的功能；由于机器人本身只知道内部状态，并且必须通过SLAM设备获取环境中的位置和方向；照相机和机器人手臂之间的校准被称为手眼校准；

在***中，使用了地图坐标系、“SLAM设备”坐标系、SLAM设备所安装的机器人框架的“头部”坐标系以及接触地板的机器人部分的“脚部”坐标系；为方便起见，SLAM设备所连接的机器人框架的坐标***称为“头部”；

处理一个未知旋转和平移的6-DoF(自由度)参数；将机器人前进的方向设为x，将机器人的垂直方向设为z；t_x，t_y，t_z是沿x、y、z轴的平移参数，并且滚动、倾斜、旋转分别是围绕轴x、y、z的旋转参数。

其中，所述的获取姿势转换，首先，解释用于计算校准参数的机器人和SLAM设备的转换的方法；为了获得机器人和SLAM装置之间的相对位置和方向，需要进行位置和方向的转换，并且取得转换前后两个位置和方向参数之间的差异；令姿势1成为头部和SLAM设备的初始姿势，并令姿势2成为它们过渡后的姿势；令M_1head和M_1sd分别为4×4矩阵，分别表示每个机器人和SLAM设备在姿态1处的位置和姿态；M_2head和M_2sd分别为在姿态2的局部坐标中的4×4位置和姿势矩阵；这些矩阵是观察值；

A表示头部从姿势1到姿势2的转换，A＝M_1head ^-1M_2head；B表示SLAM设备的转换，B＝M_1sd ^-1M_2sd；设X是一个未知的4×4矩阵，表示机器人和传感器之间的相对位置和方向，通过姿势2处的“头部”和姿势1处的“SLAM设备”有两个转换；前者由矩阵计算AX表示，后者由XB表示；结果在两个坐标转换中都是相同的，因此AX＝XB成立。

进一步地，所述的运动与求解参数之间的关系，考虑水平旋转、垂直旋转、向前运动的三种类型的运动；令R_A和R_B分别为A和B中的3×3旋转矩阵分量，k_A和k_B分别为R_A和R_B中表示旋转轴的单位矢量；设t_A和t_B为三维平移向量分量；根据AX＝XB有：

k_A＝Rk_B (1)

R_At+t_A＝Rt_B+t (2)

首先，考虑从方程(1)得到的约束；给定一组k_A和k_B，确定包含在R中的3-DoF旋转参数中，除了k_A矢量周围之外的2-DoF旋转参数；随后，考虑从方程(2)中获得的约束；方程(2)可以如下转换：

(1-R_A)t＝t_A-Rt_B (3)

旋转机器人时，并考虑将t分解k_A分量和两个单位向量t₁和t₂的情况，它与k_A正交，且满足相互正交，方程(3)中的t在k_A方向上具有一定的自由度，并且由于(1-R_A)k_A＝0，所以在t₁和t₂方向上有约束。

进一步地，所述的三种类型的运动，包括水平旋转、垂直旋转和向前运动；

(1)水平旋转：当机器人水平旋转时，旋转轴指向z方向；在旋转的情况下，方程(1)中的k_A指导z轴，可以获得绕z轴旋转以外的参数，即滚动和倾斜；在平移参数的情况下，基于方程(3)的约束主要应用于t_x和t_y；

(2)垂直旋转：在类似于水平旋转的情况下，在机器人中垂直摆动颈部的垂直方向(这里假设绕y轴旋转)进行旋转运动的情况下，可以从方程(1)中获得滚动和旋转的两个参数，也可以从方程(3)获得转化参数t_x和t_z；

(3)向前运动：直线移动机器人R_A≈I时，方程(2)变成t_A＝Rt_B；因此，当机器人前进时，约束被应用于旋转参数倾斜和旋转。

其中，所述的离线机器人-同步定位与地图构建设备校准，对两种类型的机器人进行校准，分别为：使用双向旋转进行校准和使用水平移动和SLAM设备的高度的辅助信息进行校准；同时考虑执行包括旋转和平移的复杂位置和姿态转换的复杂情况。

进一步地，所述的SLAM设备的高度的辅助信息进行校准，在使用难以获得t_z的机器人的情况下，对于剩余z方向上的分量，不能通过单独的水平位置和姿态转换来约束它，因此使用来自地面的SLAM设备的高度来计算z参数；设n是地板的法向矢量，h是SLAM设备离地面的高度；n和h可以从SLAM设备的环境地图中观察到；设b是从“头”到“脚”的向量，设o是从“SLAM设备”到“头部”的矢量的平行分量；o表示平行于地面的平面中的“SLAM设备”和“脚”的移位，o＝hn-R^-1t+b；因此n·o＝n·(hn-R^-1t+b)＝0可以限制地板垂直方向的剩余时间t_z。

进一步地，所述的复杂情况，根据方程(3)，t使用机器人头部的平移分量t_A与SLAM设备的平移运动分量Rt_B之间的差异来获得；现在，当在位置和姿态转换中同时旋转和平移时，会出现SLAM设备的测距和机器人之间的累积误差；这个累积误差直接与根据t_A和Rt_B之差计算的t的误差有关；因此，为了防止这种累积误差并执行精确的校准，有必要使机器人平移运动(即，t_A的长度)最小化。

其中，所述的在线位置调整，在线位置校准方法用于校准机器人和SLAM设备之间的相对位置，使外部环境的信息、机器人的形状和设备位置一致；即使校准是提前离线执行的，但有几个动态因素会导致定位基准在导航期间的位置不正确；在导航运行时，SLAM设备可能会错位；惯性测量单元(IMU)或机器人编码器的精度会导致错误，或者移动关节时编码器的时间滞后也会导致较大的误差。

进一步地，所述的误差，在由上述原因引起的误差中，尤其是当装置安装在高位置时，滚动轴和倾斜轴旋转中的误差将导致较大的定位误差；在线校准方法中，SLAM装置和机器人之间的参数是基于机器人垂直于地面的前提下进行修改的；在具体计算中，首先根据SLAM设备获取的外部环境信息，求出校准前由SLAM设备定位的机器人的地面法线矢量和垂直于机器人地平面的矢量；然后对机器人和设备之间的坐标转换应用额外的校准旋转，以便两个矢量匹配。

附图说明

图1是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的***框架图。

图2是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的现实世界与坐标系之间的关系。

图3是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的获取姿势转换。

图4是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的运动与受限参数之间的关系。

图5是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的在线位置调整。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互结合，下面结合附图和具体实施例对本发明作进一步详细说明。

图1是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的***框架图。主要包括获取姿势转换，运动与求解参数之间的关系，离线机器人-同步定位与地图构建设备校准，在线位置调整。

离线机器人-同步定位与地图构建设备校准，是对两种类型的机器人进行校准，分别为：使用双向旋转进行校准和使用水平移动和SLAM设备的高度的辅助信息进行校准；同时考虑执行包括旋转和平移的复杂位置和姿态转换的复杂情况。

在使用难以获得t_z的机器人的情况下，对于剩余z方向上的分量，不能通过单独的水平位置和姿态转换来约束它，因此使用来自地面的SLAM设备的高度来计算z参数；设n是地板的法向矢量，h是SLAM设备离地面的高度；n和h可以从SLAM设备的环境地图中观察到；设b是从“头”到“脚”的向量，设o是从“SLAM设备”到“头部”的矢量的平行分量；o表示平行于地面的平面中的“SLAM设备”和“脚”的移位，o＝hn-R^-1t+b；因此n·o＝n·(hn-R^-1t+b)＝0可以限制地板垂直方向的剩余时间t_z。

t使用机器人头部的平移分量t_A与SLAM设备的平移运动分量Rt_B之间的差异来获得；现在，当在位置和姿态转换中同时旋转和平移时，会出现SLAM设备的测距和机器人之间的累积误差；这个累积误差直接与根据t_A和Rt_B之差计算的t的误差有关；因此，为了防止这种累积误差并执行精确的校准，有必要使机器人平移运动(即，t_A的长度)最小化。

图2是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的现实世界与坐标系之间的关系。将实时三维感测装置称为“SLAM装置”；SLAM设备具有传感、映射外部环境和实时估计自身位置和姿态的功能；由于机器人本身只知道内部状态，并且必须通过SLAM设备获取环境中的位置和方向；照相机和机器人手臂之间的校准被称为手眼校准；

在***中，使用了地图坐标系、“SLAM设备”坐标系、SLAM设备所安装的机器人框架的“头部”坐标系以及接触地板的机器人部分的“脚部”坐标系；为方便起见，SLAM设备所连接的机器人框架的坐标***称为“头部”；

处理一个未知旋转和平移的6-DoF(自由度)参数；将机器人前进的方向设为x，将机器人的垂直方向设为z；t_x，t_y，t_z是沿x、y、z轴的平移参数，并且滚动、倾斜、旋转分别是围绕轴x、y、z的旋转参数。

图3是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的获取姿势转换。首先，解释用于计算校准参数的机器人和SLAM设备的转换的方法；为了获得机器人和SLAM装置之间的相对位置和方向，需要进行位置和方向的转换，并且取得转换前后两个位置和方向参数之间的差异；令姿势1成为头部和SLAM设备的初始姿势，并令姿势2成为它们过渡后的姿势；令M_1head和M_1sd分别为4×4矩阵，分别表示每个机器人和SLAM设备在姿态1处的位置和姿态；M_2head和M_2sd分别为在姿态2的局部坐标中的4×4位置和姿势矩阵；这些矩阵是观察值；

A表示头部从姿势1到姿势2的转换，A＝M_1head ^-1M_2head；B表示SLAM设备的转换，B＝M_1sd ^-1M_2sd；设X是一个未知的4×4矩阵，表示机器人和传感器之间的相对位置和方向，通过姿势2处的“头部”和姿势1处的“SLAM设备”有两个转换；前者由矩阵计算AX表示，后者由XB表示；结果在两个坐标转换中都是相同的，因此AX＝XB成立。

图4是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的运动与受限参数之间的关系。考虑水平旋转、垂直旋转、向前运动的三种类型的运动；令R_A和R_B分别为A和B中的3×3旋转矩阵分量，k_A和k_B分别为R_A和R_B中表示旋转轴的单位矢量；设t_A和t_B为三维平移向量分量；根据AX＝XB有：

k_A＝Rk_B (1)

R_At+t_A＝Rt_B+t (2)

首先，考虑从方程(1)得到的约束；给定一组k_A和k_B，确定包含在R中的3-DoF旋转参数中，除了k_A矢量周围之外的2-DoF旋转参数；随后，考虑从方程(2)中获得的约束；方程(2)可以如下转换：

(1-R_A)t＝t_A-Rt_B (3)

旋转机器人时，并考虑将t分解k_A分量和两个单位向量t₁和t₂的情况，它与k_A正交，且满足相互正交，方程(3)中的t在k_A方向上具有一定的自由度，并且由于(1-R_A)k_A＝0，所以在t₁和t₂方向上有约束。

其中，所述的三种类型的运动包括水平旋转、垂直旋转和向前运动；

(1)水平旋转：当机器人水平旋转时，旋转轴指向z方向；在旋转的情况下，方程(1)中的k_A指导z轴，可以获得绕z轴旋转以外的参数，即滚动和倾斜；在平移参数的情况下，基于方程(3)的约束主要应用于t_x和t_y；

(2)垂直旋转：在类似于水平旋转的情况下，在机器人中垂直摆动颈部的垂直方向(这里假设绕y轴旋转)进行旋转运动的情况下，可以从方程(1)中获得滚动和旋转的两个参数，也可以从方程(3)获得转化参数t_x和t_z；

(3)向前运动：直线移动机器人R_A≈I时，方程(2)变成t_A＝Rt_B；因此，当机器人前进时，约束被应用于旋转参数倾斜和旋转。

图5是本发明一种基于同步定位与地图构建技术的机器人设备校准方法的在线位置调整。在线位置校准方法用于校准机器人和SLAM设备之间的相对位置，使外部环境的信息、机器人的形状和设备位置一致；即使校准是提前离线执行的，但有几个动态因素会导致定位基准在导航期间的位置不正确；在导航运行时，SLAM设备可能会错位；惯性测量单元(IMU)或机器人编码器的精度会导致错误，或者移动关节时编码器的时间滞后也会导致较大的误差。

在由上述原因引起的误差中，尤其是当装置安装在高位置时，滚动轴和倾斜轴旋转中的误差将导致较大的定位误差；在线校准方法中，SLAM装置和机器人之间的参数是基于机器人垂直于地面的前提下进行修改的；在具体计算中，首先根据SLAM设备获取的外部环境信息，求出校准前由SLAM设备定位的机器人的地面法线矢量和垂直于机器人地平面的矢量；然后对机器人和设备之间的坐标转换应用额外的校准旋转，以便两个矢量匹配。

对于本领域技术人员，本发明不限制于上述实施例的细节，在不背离本发明的精神和范围的情况下，能够以其他具体形式实现本发明。此外，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围，这些改进和变型也应视为本发明的保护范围。因此，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

Claims (10)

1.一种基于同步定位与地图构建技术的机器人设备校准方法，其特征在于，主要包括获取姿势转换(一)；运动与求解参数之间的关系(二)；离线机器人-同步定位与地图构建设备校准(三)；在线位置调整(四)。

2.基于权利要求书1所述的同步定位与地图构建(SLAM)技术，其特征在于，将实时三维感测装置称为“SLAM装置”；SLAM设备具有传感、映射外部环境和实时估计自身位置和姿态的功能；由于机器人本身只知道内部状态，并且必须通过SLAM设备获取环境中的位置和方向；照相机和机器人手臂之间的校准被称为手眼校准；

在***中，使用了地图坐标系、“SLAM设备”坐标系、SLAM设备所安装的机器人框架的“头部”坐标系以及接触地板的机器人部分的“脚部”坐标系；为方便起见，SLAM设备所连接的机器人框架的坐标***称为“头部”；

处理一个未知旋转和平移的6-DoF(自由度)参数；将机器人前进的方向设为x，将机器人的垂直方向设为z；t_x，t_y,t_z是沿x、y、z轴的平移参数，并且滚动、倾斜、旋转分别是围绕轴x、y、z的旋转参数。

3.基于权利要求书1所述的获取姿势转换(一)，其特征在于，首先，解释用于计算校准参数的机器人和SLAM设备的转换的方法；为了获得机器人和SLAM装置之间的相对位置和方向，需要进行位置和方向的转换，并且取得转换前后两个位置和方向参数之间的差异；令姿势1成为头部和SLAM设备的初始姿势，并令姿势2成为它们过渡后的姿势；令M_1head和M_1sd分别为4×4矩阵，分别表示每个机器人和SLAM设备在姿态1处的位置和姿态；M_2head和M_2sd分别为在姿态2的局部坐标中的4×4位置和姿势矩阵；这些矩阵是观察值；

A表示头部从姿势1到姿势2的转换，A＝M_1head ^-1M_2head；B表示SLAM设备的转换，B＝M_1sd ^{- 1}M_2sd；设X是一个未知的4×4矩阵，表示机器人和传感器之间的相对位置和方向，通过姿势2处的“头部”和姿势1处的“SLAM设备”有两个转换；前者由矩阵计算AX表示，后者由XB表示；结果在两个坐标转换中都是相同的，因此AX＝XB成立。

4.基于权利要求书1所述的运动与求解参数之间的关系(二)，其特征在于，考虑水平旋转、垂直旋转、向前运动的三种类型的运动；令R_A和R_B分别为A和B中的3×3旋转矩阵分量，k_A和k_B分别为R_A和R_B中表示旋转轴的单位矢量；设t_A和t_B为三维平移向量分量；根据AX＝XB有：

k_A＝Rk_B (1)

R_At+t_A＝Rt_B+t (2)

首先，考虑从方程(1)得到的约束；给定一组k_A和k_B，确定包含在R中的3-DoF旋转参数中，除了k_A矢量周围之外的2-DoF旋转参数；随后，考虑从方程(2)中获得的约束；方程(2)可以如下转换：

(1-R_A)t＝t_A-Rt_B (3)

旋转机器人时，并考虑将t分解k_A分量和两个单位向量t₁和t₂的情况，它与k_A正交，且满足相互正交，方程(3)中的t在k_A方向上具有一定的自由度，并且由于(1-R_A)k_A＝0，所以在t₁和t₂方向上有约束。

5.基于权利要求书4所述的三种类型的运动，其特征在于，包括水平旋转、垂直旋转和向前运动；

(1)水平旋转：当机器人水平旋转时，旋转轴指向z方向；在旋转的情况下，方程(1)中的k_A指导z轴，可以获得绕z轴旋转以外的参数，即滚动和倾斜；在平移参数的情况下，基于方程(3)的约束主要应用于t_x和t_y；

(2)垂直旋转：在类似于水平旋转的情况下，在机器人中垂直摆动颈部的垂直方向(这里假设绕y轴旋转)进行旋转运动的情况下，可以从方程(1)中获得滚动和旋转的两个参数，也可以从方程(3)获得转化参数t_x和t_z；

(3)向前运动：直线移动机器人R_A≈I时，方程(2)变成t_A＝Rt_B；因此，当机器人前进时，约束被应用于旋转参数倾斜和旋转。

6.基于权利要求书1所述的离线机器人-同步定位与地图构建设备校准(三)，其特征在于，对两种类型的机器人进行校准，分别为：使用双向旋转进行校准和使用水平移动和SLAM设备的高度的辅助信息进行校准；同时考虑执行包括旋转和平移的复杂位置和姿态转换的复杂情况。

7.基于权利要求书6所述的SLAM设备的高度的辅助信息进行校准，其特征在于，在使用难以获得t_z的机器人的情况下，对于剩余z方向上的分量，不能通过单独的水平位置和姿态转换来约束它，因此使用来自地面的SLAM设备的高度来计算z参数；设n是地板的法向矢量，h是SLAM设备离地面的高度；n和h可以从SLAM设备的环境地图中观察到；设b是从“头”到“脚”的向量，设o是从“SLAM设备”到“头部”的矢量的平行分量；o表示平行于地面的平面中的“SLAM设备”和“脚”的移位，o＝hn-R^-1t+b；因此n·o＝n·(hn-R^-1t+b)＝0可以限制地板垂直方向的剩余时间t_z。

8.基于权利要求书6所述的复杂情况，其特征在于，根据方程(3)，t使用机器人头部的平移分量t_A与SLAM设备的平移运动分量Rt_B之间的差异来获得；现在，当在位置和姿态转换中同时旋转和平移时，会出现SLAM设备的测距和机器人之间的累积误差；这个累积误差直接与根据t_A和Rt_B之差计算的t的误差有关；因此，为了防止这种累积误差并执行精确的校准，有必要使机器人平移运动(即，t_A的长度)最小化。

9.基于权利要求书1所述的在线位置调整(四)，其特征在于，在线位置校准方法用于校准机器人和SLAM设备之间的相对位置，使外部环境的信息、机器人的形状和设备位置一致；即使校准是提前离线执行的，但有几个动态因素会导致定位基准在导航期间的位置不正确；在导航运行时，SLAM设备可能会错位；惯性测量单元(IMU)或机器人编码器的精度会导致错误，或者移动关节时编码器的时间滞后也会导致较大的误差。

10.基于权利要求书9所述的误差，其特征在于，在由上述原因引起的误差中，尤其是当装置安装在高位置时，滚动轴和倾斜轴旋转中的误差将导致较大的定位误差；在线校准方法中，SLAM装置和机器人之间的参数是基于机器人垂直于地面的前提下进行修改的；在具体计算中，首先根据SLAM设备获取的外部环境信息，求出校准前由SLAM设备定位的机器人的地面法线矢量和垂直于机器人地平面的矢量；然后对机器人和设备之间的坐标转换应用额外的校准旋转，以便两个矢量匹配。