CN112506200B - 机器人定位方法、装置、机器人及存储介质 - Google Patents
机器人定位方法、装置、机器人及存储介质 Download PDFInfo
- Publication number
- CN112506200B CN112506200B CN202011466991.4A CN202011466991A CN112506200B CN 112506200 B CN112506200 B CN 112506200B CN 202011466991 A CN202011466991 A CN 202011466991A CN 112506200 B CN112506200 B CN 112506200B
- Authority
- CN
- China
- Prior art keywords
- state
- robot
- coordinate system
- local
- representing
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 238000003860 storage Methods 0.000 title claims abstract description 22
- 238000005259 measurement Methods 0.000 claims abstract description 237
- 238000005457 optimization Methods 0.000 claims abstract description 50
- 230000006870 function Effects 0.000 claims description 61
- 239000011159 matrix material Substances 0.000 claims description 58
- 238000013507 mapping Methods 0.000 claims description 35
- 238000013519 translation Methods 0.000 claims description 29
- 230000010354 integration Effects 0.000 claims description 22
- 230000001133 acceleration Effects 0.000 claims description 18
- 230000009466 transformation Effects 0.000 claims description 16
- 230000036961 partial effect Effects 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 9
- 238000012545 processing Methods 0.000 claims description 8
- 238000010276 construction Methods 0.000 claims description 7
- 230000002441 reversible effect Effects 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 18
- 238000010586 diagram Methods 0.000 description 18
- 230000008569 process Effects 0.000 description 12
- 238000006243 chemical reaction Methods 0.000 description 11
- 230000008859 change Effects 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 5
- 230000002829 reductive effect Effects 0.000 description 5
- 238000004422 calculation algorithm Methods 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000005452 bending Methods 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000001131 transforming effect Effects 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000002776 aggregation Effects 0.000 description 1
- 238000004220 aggregation Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 238000004140 cleaning Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 235000012054 meals Nutrition 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 239000012466 permeate Substances 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 230000000452 restraining effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Length Measuring Devices By Optical Means (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请实施例公开了一种机器人定位方法、装置、机器人及存储介质，涉及机器人技术领域，其包括：获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态；根据激光点云数据绘制机器人的第一局部地图；根据第一局部地图、激光点云数据、第一状态及机器人在前一帧的第二状态确定局部测量残差；根据全局地图、激光点云数据及第一状态确定全局测量残差；根据第一状态、第二状态及惯性测量数据确定惯性测量残差；将第一状态加入滑动窗口并根据滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数；根据优化函数对机器人进行定位。采用上述方法可以解决现有技术中当前环境发生变化时机器人定位准确性低的技术问题。
Description
技术领域
本申请实施例涉及机器人技术领域，尤其涉及一种机器人定位方法、装置、机器人及存储介质。
背景技术
机器人是一种自动化的机器，其可以通过编程和自动控制来执行如作业或移动等任务。随着人们对机器人智能化技术的加深，机器人开始源源不断地向人类活动的各个领域渗透，例如，在餐饮领域出现了送餐机器人、在清洁领域出现了扫地机器人，在驾驶领域出现了自动驾驶的汽车。对于可移动的机器人而言，机器人定位是机器人进行自主移动时的关键技术。其中，机器人定位是指确定机器人在当前环境中的位置，通过该位置可以确定机器人后续的移动路线。
一些技术中，预先在机器人中输入当前环境的地图数据，且地图数据输入后不会变化。之后，机器人在移动过程中利用3D激光传感器等装置来获取周围环境的激光数据，进而根据激光数据和预先存储的地图数据确定机器人在当前环境中的位置。如果当前环境发生变化，如构建地图数据时道路两旁没有车辆，而机器人移动过程中道路两旁停有汽车。此时，已有的地图数据与当前环境的实际情况不相符，所以，机器人获取周围环境的激光数据后，根据激光数据和地图数据对机器人进行定位时，会导致定位失败或者得到错误的位置，进而降低了机器人定位的准确性。
发明内容
本申请实施例提供了一种机器人定位方法、装置、机器人及存储介质，以解决现有技术中当前环境发生变化时机器人定位准确性低的技术问题。
第一方面，本申请实施例提供了一种机器人定位方法，包括：
获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态；
根据所述激光点云数据绘制所述机器人在当前帧识别到的第一局部地图；
根据所述第一局部地图、所述激光点云数据、所述第一状态以及所述机器人在前一帧的第二状态确定局部测量残差；
根据预存的全局地图、所述激光点云数据以及所述第一状态确定全局测量残差；
根据所述第一状态、所述第二状态以及两帧之间的惯性测量数据确定惯性测量残差；
将所述第一状态加入滑动窗口，并根据所述滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数；
根据所述优化函数确定当前帧下所述机器人在所述全局地图中的位置。
第二方面，本申请实施例还提供了一种机器人定位装置，包括：
数据获取模块，用于获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态；
地图绘制模块，用于根据所述激光点云数据绘制所述机器人在当前帧识别到的第一局部地图；
局部残差确定模块，用于根据所述第一局部地图、所述激光点云数据、所述第一状态以及所述机器人在前一帧的第二状态确定局部测量残差；
全局残差确定模块，用于根据预存的全局地图、所述激光点云数据以及所述第一状态确定全局测量残差；
惯性残差确定模块，用于根据所述第一状态、所述第二状态以及两帧之间的惯性测量数据确定惯性测量残差；
函数构建模块，用于将所述第一状态加入滑动窗口，并根据所述滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数；
位置确定模块，用于根据所述优化函数确定当前帧下所述机器人在所述全局地图中的位置。
第三方面，本申请实施例还提供了一种机器人，包括：
激光传感器，用于采集激光点云数据；
惯性测量单元，用于采集惯性测量数据；
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如第一方面所述的机器人定位方法。
第四方面，本申请实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现如第一方面所述的机器人定位方法。
上述机器人定位方法、装置、机器人及存储介质，通过获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态，并根据激光点云数据绘制机器人在当前帧识别到的第一局部地图，之后，根据第一局部地图、激光点云数据、第一状态、机器人在前一帧的第二状态、预存的全局地图以及两帧之间的惯性测量数据确定局部测量残差、全局测量残差以及惯性测量残差，之后，采用滑动窗口优化的方式，根据滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数，并根据优化函数对机器人进行定位的技术手段，解决了一些技术中当前环境发生变化时机器人定位准确性低的技术问题。通过激光传感器和IMU采集的数据确定局部测量残差、全局测量残差以及惯性测量残差，并将上述各残差作为机器人状态的约束，以得到机器人状态，实现了紧耦合优化，降低了机器人定位的计算难度，且结合滑动窗口优化，即考虑一段时间内的机器人状态，可以提高机器人定位的精度，增强了机器人对环境改变的鲁棒性，提高机器人对空间环境的适应能力。
附图说明
图1为本申请实施例提供的激光点云数据和惯性测量数据的采集频率示意图；
图2为本申请实施例提供的一种机器人定位方法的流程图；
图3为本申请实施例提供的一种滑动窗口示意图；
图4为本申请实施例提供的一种优化函数因子图；
图5为本申请实施例提供的另一种机器人定位方法的流程图；
图6为本申请实施例提供一帧激光点云数据示意图；
图7为本申请实施例提供的一种机器人定位方法流程框图；
图8为本申请实施例提供的一种机器人定位装置的结构示意图；
图9为本申请实施例提供的一种机器人的结构示意图。
具体实施方式
下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例用于解释本申请，而非对本申请的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与本申请相关的部分而非全部结构。
需要说明的是，在本文中，诸如第一和第二之类的关系术语仅仅用来将一个实体或操作或对象与另一个实体或操作或对象区分开来，而不一定要求或者暗示这些实体或操作或对象之间存在任何这种实际的关系或顺序。例如，第一状态和第二状态的“第一”和“第二”用来区分两个不同的机器人状态。
本申请实施例提供的机器人定位方法可以由机器人执行，该机器人中安装有激光传感器和惯性测量单元(Inertial Measurement Unit，IMU)。其中，激光传感器为3D激光传感器。具体的，3D激光传感器在工作过程中按照一定频率向一定角度内的空间区域发射激光波束，每个激光波束对应一个发射角度，此时，在没有遮挡物的前提下，激光波束所经过的空间区域为3D激光传感器的扫描区域。可理解，扫描区域是机器人所在当前环境中的局部区域。当前环境的空间区域可认为是全局区域。发射激光波束后，若激光波束的传播路径上存在遮挡物，则由于遮挡物的遮挡，会使得激光波束返回至3D激光传感器。之后，机器人在激光波束返回时确定该激光波束的发射角度，进而确定遮挡物的方向，同时，根据返回时间确定出遮挡物的具体位置以及高度，进而得绘制出激光点云数据。激光点云数据可体现发射到遮挡物表面的激光波束的激光点在三维坐标系中的集合。该三维坐标系是根据机器人所处空间区域构建的局部坐标系，三维坐标系也理解为根据机器人当前所在位置(世界坐标系下的位置)确定的物体坐标系，物体坐标系是以机器人所在位置为原点构建的相对坐标系，即机器人位置发生变化时，其绘制激光点云数据时所依托的局部坐标系同步发生变化。可理解，机器人向扫描区域内的各角度每发射一次激光波束后生成一帧激光点云数据。
IMU包括三个单轴的加速度计和三个单轴的陀螺仪。通过IMU可测量机器人的三轴加速度和三轴角速度，实施例中，将IMU采集的三轴加速度和三轴角速度称为一组惯性测量数据，通过惯性测量数据可以确定机器人的姿态。可理解，IMU按照一定频率采集惯性测量数据，可选的，IMU的采集频率大于3D激光传感器的采集频率。即两帧激光点云数据之间包含多个惯性测量数据。举例而言，图1为本申请实施例提供的激光点云数据和惯性测量数据的采集频率示意图。参考图1，上一行表示两帧相邻的激光点云数据，下一行表示采集两帧激光点云数据时所采集的惯性测量数据，由图1可知，采集两帧点云数据时可以采集六组惯性测量数据。实施例中，以帧为单位描述3D激光传感器采集的激光点云数据，以时刻为单位描述IMU采集的惯性测试数据，以图1为例，相邻两帧的激光点云数据对应六组相邻时刻的惯性测量数据。
实施例中，结合3D激光传感器和IMU实现在当前环境中对机器人进行定位。
图2为本申请实施例提供的一种机器人定位方法的流程图。参考图2，该机器人定位方法包括：
步骤110、获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态。
激光点云数据描绘出3D激光传感器发射一帧激光波束后返回的全部激光点，每个激光点对应一束被遮挡物返回的激光波束。
第一状态是指当前帧下预测得到的机器人状态。机器人状态表示为(p、v、q、ba、bg)，其中，p表示机器人在全局区域中的三自由度位置，也可以理解为机器人在世界坐标系下的三维坐标，v表示机器人当前的三自由度速度，也可以理解为机器人在世界坐标系下三个坐标方向(x、y、z)上的速度，q表示机器人当前姿态的四元数表示以及表示机器人当前的局部坐标系转换到世界坐标系的旋转变换四元数，其中，四元数由实数加上三个虚数单位组成，ba表示IMU在当前帧下三轴加速度计的偏置，bg表示IMU在当前帧下三轴陀螺仪的偏置，其中，偏置也可以理解为测量误差。进一步的，第一状态可通过前一帧的机器人状态以及前一帧至当前帧内IMU采集的惯性测量数据预测得到，可理解，前一帧的机器人状态是指前一帧下对机器人定位后得到的机器人状态。即前一帧的机器人状态为计算得到的最终值，第一状态为计算得到的预测值，之后，通过第一状态可计算得到当前帧下机器人状态的预测值。可理解，前一帧到当前帧之间会接收到多组惯性测量数据，此时，每接收到一组惯性测量数据，便会预测得到一个机器人状态，且将当前帧下预测得到的机器人状态作为第一状态。举例而言，前一帧和当前帧之间共接收到三组惯性测量数据，那么第一状态的计算过程为：接收到第一组惯性测量数据时，根据前一帧机器人状态和惯性测量数据预测得到一个机器人状态，接收到第二组惯性测量数据时，根据前一时刻预测的机器人状态和惯性测量数据再得到一个机器人状态，接收到第三组惯性数据时，再根据前一时刻预测的机器人状态和惯性测量数据再次预测新的机器人状态。由于最新预测的机器人状态为当前帧下的机器人状态，因此，最新预测的机器人状态为第一状态。其中，预测机器人状态时，假设前一时刻得到的机器人状态为(p'、v'、q'、ba'、bg')，当前时刻接收的惯性测量数据为(a、w)，a表示当前时刻的三轴加速度，w表示当前时刻的三轴角速度，当前时刻预测的机器人状态为(p”、v”、q”、ba”、bg”)，那么，p”＝p'+v'Δt+0.5aΔt2，v”＝v'+aΔt，ba”＝ba'，bg”＝bg'。其中，Δt表示前一时刻和当前时刻的时间间隔，/>表示四元数乘法。
步骤120、根据激光点云数据绘制机器人在当前帧识别到的第一局部地图。
第一局部地图用于描述当前帧下3D激光传感器扫描区域内的遮挡物信息。其中，遮挡物信息包括遮挡物的高度以及遮挡物在扫描区域内的位置等内容。第一局部地图根据激光点云数据绘制。可理解，激光点云数据中各激光点的三维坐标是当前帧下局部坐标系内的三维坐标，在构建第一局部地图时，需要先将各激光点由局部坐标系映射到世界坐标系下。具体的，先确定局部坐标系转换到世界坐标系所需的旋转矩阵和平移向量，之后，根据旋转矩阵和平移向量对各激光点在局部坐标系下的三维坐标进行转换，以得到各激光点在世界坐标系下的三维坐标。之后，将世界坐标系下的各激光点拼接后，便可以得到第一局部地图。可理解，对于同一遮挡物而言，其表面的激光点应为近邻关系，即激光点间距离较近，因此，拼接各激光点时，只拼接距离较近的激光点，以将位于同一遮挡物表面的各激光点拼接起来。此时，第一局部地图中连接的各激光点可以绘制出遮挡物的表面。
需说明，实施例中，每帧激光点云数据均会构建对应的第一局部地图。
步骤130、根据第一局部地图、激光点云数据、第一状态以及机器人在前一帧的第二状态确定局部测量残差。
局部测量残差用于体现机器人在前一帧描述的扫描区域和当前帧描述的扫描区域之间的差别，上述扫描区域为当前帧下3D激光传感器的扫描区域。可理解，机器人状态准确时，前一帧描述的扫描区域和当前帧描述的扫描区域应该是相同的，但是由于IMU测量误差、当前环境变化或3D激光传感器扫描误差等因素，会使机器人状态不够准确，进而使得前一帧描述的扫描区域和当前帧描述的扫描区域存在差异，该差异便可用局部测量残差表示。
示例性的，先获取机器人在前一帧的第二状态。第二状态是指前一帧下计算得到最终的机器人状态。具体的，机器人通过第二状态下的三自由度位置和三自由度速度可得到其在前一帧的物体坐标系(即局部坐标系)。同时，根据第二状态下的旋转变换四元数和第一状态下的旋转变换四元数可以确定第一状态下的局部坐标系转换到第二状态下的局部坐标系所需要的旋转矩阵，根据第一机器人在第一状态下的三自由度位置和第二状态下的三自由度位置可以确定第一状态下的局部坐标系转换到第二状态下的局部坐标系所需要的平移向量。之后，根据该旋转矩阵和平移向量便可以将当前帧的激光点云数据映射到前一帧的局部坐标系中。之后，将第一局部地图也转换到前一帧的局部坐标系中，以获取第一局部地图在前一帧下的描述。其中，根据第二状态下的旋转变换四元数可以确定前一帧的局部坐标系转换到世界坐标系的旋转矩阵，根据第二状态下的三自由度位置可以确定前一帧的局部坐标系转换到世界坐标系所需要的平移向量，根据旋转矩阵和平移向量，便可以将第一局部地图转换到前一帧的局部坐标系中。之后，计算对应激光点在前一帧局部坐标系中三维坐标间的距离，其中对应激光点是指激光点云数据中激光点以及该激光点映射到世界坐标系后在第一局部地图中的激光点。之后，根据各激光点的距离得到局部测量残差。可理解，对应激光点间的距离越小，说明局部测量残差越小，第一状态越准确。因此，计算局部测量残差时使用的距离也可以认为是为了得到准确的第一状态而构建的局部距离约束。
一个实施例中，上述计算过程可以具体为：根据激光点云数据中各激光点的曲率，识别出激光点云数据中的每个线点和每个面点。其中，面点具有较低的曲率，线点具有较高的曲率，面点是指其对应的激光波束扫描到遮挡物的表面的曲率较低，该表面可以认为是一平面或者趋近于一平面，线点是指其对应的激光波束扫描到遮挡物的表面曲率较高，该表面可能为两个平面间的转折线或者趋近于转折线。之后，在第一局部地图中，查找各线点对应的直线以及各面点对应的平面，可理解，构建第一局部地图时，已经将激光点云数据映射到世界坐标系下，因此，本步骤中，可以直接在世界坐标系中确定各线点对应的激光点，并根据查找到的激光点确定对应的直线，以及直接在世界坐标系中确定各面点对应的激光点，并根据查找到的激光点确定对应的平面。一般而言，遮挡物中同一平面对应的激光点在第一局部地图中也会组成一个平面，遮挡物中同一转折线对应的激光点在第一局部地图中也会组成一条直线。因此，在第一局部地图中先查找各线点附近的线点，并将查找到的线点拼接成一条直线，以及查找各面点附近的面点，并将查找到的面点拼接成一个平面。之后，将激光点云数据中各线点以及各面点映射到前一帧局部坐标系下，以得到前一帧局部坐标系下的线点和面点，并将第一局部地图映射到前一帧局部坐标系下，以得到前一帧局部坐标下的直线和平面。之后，计算前一帧局部坐标系下的线点到前一帧局部坐标系下对应直线的距离，以及，计算前一帧局部坐标下的面点到前一帧局部坐标系下对应平面的距离，之后，将各距离相加以得到局部测量残差。或者是，先将第一局部地图映射到前一帧局部坐标系下，之后，在前一帧局部坐标系下第一局部地图内查找位于前一帧局部坐标系下线点附近的线点，将查找到的线点拼接成一条直线，同样的，在前一帧局部坐标系下第一局部地图内查找位于前一帧局部坐标系下面点附近的面点，将查找到的面点拼接成一个平面。可理解，上述距离越小，说明局部测量误差越小，即前一帧下对扫描区域的描述与当前帧下对扫描区域的描述越相同。
步骤140、根据预存的全局地图、激光点云数据以及第一状态确定全局测量残差。
全局地图用于描述机器人所在当前环境的全局区域内的障碍物信息。全局地图是世界坐标系下的地图。全局地图在机器人移动前存入机器人中。可理解，全局地图是描述全局区域的地图，第一局部地图是描述3D激光传感器扫描区域的地图，全局地图的范围大于第一局部地图的范围。示例性的，机器人工作前，先通过3D激光传感器扫描全局区域，并根据扫描结果绘制全局区域中各障碍物表面的高度以及位置，进而得到全局地图，并由人工将全局地图保存至机器人中。一般而言，机器人的移动过程中全局地图不会发生变化。
具体的，全局测量残差用于体现当前帧激光点云数据描述的遮挡物信息与全局地图中对应局部区域中描述的遮挡物信息之间的差别。可理解，机器人状态准确时，激光点云数据中任一激光点的扫描结果与全局地图中同一位置激光点的扫描结果相同。但是由于IMU测量误差、当前环境变化或3D激光传感器扫描误差等因素，会使机器人状态不够准确，进而使激光点云数据和全局地图中同一位置激光点的扫描结果存在差异，该差异便可用全局测量残差表示。
一个实施例中，全局测量残差的确定方式为：在激光点云数据中查找出每个线点和每个面点，可理解，确定局部测量残差时已经确定了激光点云数据中的线点和面点，本步骤可以直接使用前述步骤确定的线点和面点。之后，将各线点和各面点映射到世界坐标系下。可理解，构建第一局部地图时，已经将激光点云数据映射到世界坐标系下，因此，本步骤中，可以直接在世界坐标系中确定各线点对应的激光点以及各面点对应的激光点。之后，在全局地图中查找各线点附近的线点并拼接成该线点对应的直线，以及查找各面点附近的面点并拼接成该面点对应的平面，其中，全局地图中的线点和面点同样根据曲率确定。之后，计算各线点到对应直线的距离以及各面点到对应平面的距离。之后，将各线点和各面点的距离相加以得到全局测量残差，可理解，上述距离越小，说明全局测量误差越小，当前帧下对扫描区域的描述与全局地图中对扫描区域的描述越相同。因此，计算全局测量残差时使用的距离便可以认为是为了得到准确第一状态而构建的全局距离约束。
步骤150、根据第一状态、第二状态以及两帧之间的惯性测量数据确定惯性测量残差。
惯性测量残差是指由IMU测量误差所带来的残差。惯性测量残差也可以理解为惯性测量预积分残差，惯性测量残差可以由惯性测量数据的预积分结果确定。其中，惯性测量数据的预积分结果是指对两个时刻下的惯性测量数据进行预积分后得到结果，实施例中，两个时刻是指第一状态和第二状态对应的两个时刻。其中，通过计算两帧之间的惯性测量数据预积分，可以使得机器人只关心两帧之间机器人状态的相对变化，降低了计算复杂度。
可理解，局部测量残差、全局测量残差和惯性测量残差可同时计算。
步骤160、将第一状态加入滑动窗口，并根据滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数。
示例性的，构建优化函数时采用滑动窗口优化。其中，滑动窗口优化是指将一段时间内的机器人状态(一段时间内每帧对应的机器人状态)作为一个优化变量合计，把这段时间内激光点云数据和惯性测量数据作为观测数据集，根据观测数据集优化滑动窗口内的机器人状态，进而根据优化后机器人状态确定当前帧下机器人在世界坐标系下的三自由度位置。可理解，滑动窗口内机器人状态的最大数量可以根据实际情况设定，当得到新的第一状态后，将新的第一状态加入滑动窗口中，并当滑动窗口中机器人状态的数量超过最大数量时，将最老的机器人状态边缘化掉，此时，边缘化掉的机器人状态对应的数据(如全局测量残差、惯性测量残差以及机器人状态等)通过边缘因子保存，并加入新的滑动窗口中一起优化。可理解，滑动窗口中原有的机器人状态是已经优化过的机器人状态，即已经计算后得到的准确的机器人状态。举例而言，图3为本申请实施例提供的一种滑动窗口示意图。参考图3，滑动窗口中机器人状态11的最大数量为5，得到新的第一状态时，滑动窗口右移，以得到新的滑动窗口12，旧的滑动窗口13中最老的机器人状态移出滑动窗口，其对应的数据通过边缘因子保存。
实施例中，构建优化函数时，考虑滑动窗口内各机器人状态对应的局部测量残差、全局测量残差和惯性测量残差。优化函数表示为：
其中，i表示当前帧，σ表示滑动窗口内机器人状态的总数量，[1-σ,i]表示滑动窗口中的各帧，X表示滑动窗口中各帧下机器人状态的状态集合，ο表示滑动窗口中的第ο帧，eLο(l,X)表示第ο帧的局部测量残差，表示第o帧的第一局部地图，eMο(M,X)表示第ο帧的全局测量残差，eIο(gο-1,ο,X)表示第ο帧的惯性测量残差。||.||2表示L2范数，/>表示滑动窗口内各局部测量残差的范数和值，l表示第ο帧时各线点到对应直线的距离及各面点到对应平面的距离的和值，/>表示滑动窗口内各全局测量残差的范数和值，其包括被丢弃的机器人状态对应的全局测量残差，M表示全局地图，表示滑动窗口内各惯性测量残差的范数和值，其包括被丢弃的机器人状态对应的惯性测量残差，gο-1,ο为计算得到的第ο-1帧到第ο帧的惯性测量残差。通过上述公式可知，优化函数中同时加入了惯性测量参数对应的惯性测量残差和激光点云数据对应的局部测量残差和全局测量残差，例如，图4为本申请实施例提供的一种优化函数因子图。参考图4，各帧计算得到的机器人状态可通过IMU预积分因子、全局地图因子、激光点线因子和激光点面因子获得，其中，IMU预积分因子对应惯性测量残差、全局地图因子对应全局测量残差，激光点线因子和激光点面因子对应局部测量残差，可理解，两帧机器人状态间包含一个IMU预积分因子，多个激光点线因子和激光点面因子。
需说明，通过优化函数进行优化时，可以认识是紧耦合优化。其中，紧耦合优化可以认为是将传感器(实施例中的3D激光传感器和IMU)采集的数据集合到一个优化问题中，以进行求解。
步骤170、根据优化函数确定当前帧下机器人在全局地图中的位置。
具体的，根据优化函数进行计算，以得到机器人的位置。其中，计算过程具体为：通过不断调整滑动窗口中各机器人状态，以使得滑动窗口内各残差(局部测量残差、全局测量残差以及惯性测量残差)的和值最小，进而使得优化函数最小。可选的，计算优化函数时，可以采用梯度下降算法、马夸尔特算法、狗腿算法、高斯牛顿算法等。可理解，优化函数越小，说明计算得到的各机器人状态与对应的实际机器人状态越贴近。因此，实施例中，获取优化函数最小时滑动窗口内各机器人状态，并将当前帧的机器人状态作为计算得到的机器人状态，进而将机器人状态中的三自由度位置作为机器人在全局地图中的位置。
上述，通过获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态，并根据激光点云数据绘制机器人在当前帧识别到的第一局部地图，之后，根据第一局部地图、激光点云数据、第一状态、机器人在前一帧的第二状态、预存的全局地图以及两帧之间的惯性测量数据确定局部测量残差、全局测量残差以及惯性测量残差，之后，采用滑动窗口优化的方式，根据滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数，并根据优化函数对机器人进行定位的技术手段，解决了一些技术中当前环境发生变化时机器人定位准确性低的技术问题。通过激光传感器和IMU采集的数据确定局部测量残差、全局测量残差以及惯性测量残差，并将上述各残差作为机器人状态的约束，以得到机器人状态，实现了紧耦合优化，降低了机器人定位的计算难度，且结合滑动窗口优化，即考虑一段时间内的机器人状态，可以提高机器人定位的精度，增强了机器人对环境改变的鲁棒性，提高机器人对空间环境的适应能力。
图5为本申请实施例提供的另一种机器人定位方法的流程图。本实施例是在上述实施例的基础上进行具体化。具体的，参考图5，该机器人定位方法具体包括：
步骤210、获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态。
步骤220、将激光点云数据中的各激光点映射到世界坐标系下。
具体的，根据第一状态确定当前帧的局部坐标系转换成世界坐标系所需要的旋转矩阵和平移向量。一个实施例中，局部坐标系的原点为第一状态中机器人的三自由度位置，根据该三自由度位置便可以确定当前帧局部坐标系转换成世界坐标系时所需要的平移向量。根据第一状态中旋转变换四元数便可以确定当前帧局部坐标系转换成世界坐标系所需要的旋转矩阵。确定旋转矩阵和平移向量后，便可以根据旋转矩阵和平移向量将激光点云数据中的各激光点由当前帧的局部坐标系变换到世界坐标系下。
可选的，3D激光传感器发射一帧激光波束时具体是向各发射角度依次发射激光数据，以达到扫描的效果。若机器人当前正在移动，那么，3D激光传感器发射一帧激光波束时，各激光波束对应的局部坐标系会因为机器人的移动而发生变化。这样会导致激光点云数据中各激光点所在的局部坐标系不同。举例而言，3D激光传感器发射当前帧的第一束激光波束时，机器人在A位置，那么，第一束激光波束对应的激光点所在的局部坐标系是以A位置为原点构建的局部坐标系。随着机器人的移动，3D激光传感器发射当前帧的最后一束激光波束时，机器人移动到B位置，那么，最后一束激光波束对应的激光点所在的局部坐标系是以B位置为原点构建的局部坐标系。此时，为了避免同一帧的激光点云数据使用的局部坐标系不同会影响后续处理的准确性，实施例中，将同一帧的激光点云数据转换到相同的局部坐标系中。可选的，以转换到第一个激光点对应的局部坐标系为例进行描述，此时，将激光点云数据中的各激光点映射到世界坐标系下之前，包括：获取激光点云数据中的第一个激光点；以第一个激光点为基准，将激光点云数据中的其他激光点映射到第一个激光点所在的第一局部坐标系中。具体的，第一个激光点是指激光点云数据中第一个被3D激光传感器接收到的激光波束所生成的激光点。第一局部坐标系是指发射第一个激光点对应的激光波束时机器人所在的局部坐标系。可选的，3D激光传感器每发射一束激光波束，均会同步记录机器人对应的局部坐标系。之后，获取激光点云数据中除第一个激光点外的其他激光点对应的局部坐标系，并计算其他激光点的局部坐标系变换到第一局部坐标系所需要的旋转矩阵和平移向量，之后，根据旋转矩阵和平移向量将其他激光点映射到第一局部坐标系下，进而实现将激光点云数据中的各激光点转换到第一局部坐标系中。举例而言，第一点云数据中某个激光点A1对应的局部坐标系变换到第一局部坐标系时的旋转矩阵为R1△t、平移向量为T1△t、其中，△t表示A1与第一个激光点之间的时间间隔。此时，将A1映射到第一局部坐标系后得到激光点A2，且A2＝R1△t*A1+T1△t。按照上述方式便可以将激光点云数据中的全部激光点转换到第一局部坐标系下。可理解，旋转矩阵和平移向量的计算过程实施例步骤限定，例如，对机器人的移动距离进行积分，以得到的旋转矩阵和平移向量。可理解，后续过程中提及的激光点云数据均是指已经转换到第一局部坐标系中的激光点云数据。
步骤230、拼接世界坐标系下的各激光点，以得到机器人在当前帧识别到的第一局部地图。
具体的，世界坐标系下的各激光点拼接后便可以得到机器人在当前帧的第一局部地图。可选的，拼接世界坐标系下的各激光点时可以是将一定距离内的激光点拼接到一起，或者是将同一高度内的近邻激光点拼接到一起。一般而言，同一遮挡物上的激光点在世界坐标系下近邻，因此，通过拼接近邻的激光点，可以将同一遮挡物上的激光点连接在一起，而不同遮挡物的激光点由于高度、位置存在差异，其激光点在世界坐标系下不近邻，因此，不会被拼接到一起。可理解，第一局部地图中拼接在一起的激光点便是对遮挡物表面位置、高度以及形状的描述。
步骤240、根据激光点云数据中各激光点的曲率，识别出激光点云数据中的每个第一线点和每个第一面点。
具体的，计算激光点云数据中各激光点的曲率。其中，曲率可体现曲线的弯曲程度，激光点云数据中每个激光点均可计算得到对应的曲率。一个实施例中，曲率计算的方式是：针对当前计算曲率的激光点而言，在激光点云数据中查找其邻域的激光点，并拟合成一个局部曲面，之后，计算该局部曲面的曲率并作为该激光点的曲率。可理解，曲率越大，说明激光点拟合的曲面弯曲程度越高，该激光点处遮挡物表面的弯折程度越高，该激光点为线点的可能越大。曲率越低，说明激光点拟合的曲面弯曲程度越低，该激光点处遮挡物表面越平整，该激光点为面点的概率越大。实施例中，将曲率低的激光点作为线点，曲率高的激光点作为面点。线点和面点可以通过对应的曲率阈值来确定，例如，预先设定一曲率阈值，当激光点的曲率高于该曲率阈值时，将该激光点识别为线点，并且，预先设定另一曲率阈值，且另一曲率阈值小于或等于前述的曲率阈值，当激光点的曲率低于另一曲率阈值时，将该激光点识别为面点。例如，图6为本申请实施例提供一帧激光点云数据示意图。参考图6，白色的激光点21是面点，其与邻域激光点拟合的曲面的曲率低，黑色的激光点22是线点，其与邻域激光点拟合的曲面的曲率高。以遮挡物为墙面为例，那么，图6中的面点可以认为是激光波束扫描到到墙面所得到的激光点，图6中的线点可以认为是激光波速扫描到两片墙面之间的拐角线所得到的激光点。进一步的，为了便于与其他线点和面点进行区分，实施例中，将当前帧的激光点云数据中的线点记为第一线点，将当前帧的激光点云数据中的面点记为第一面点。可理解，计算当前帧激光点云数据中各激光点的曲率后，便可以得到当前帧的激光点云数据中的全部第一线点和全部第一面点。
步骤250、将第一线点和第一面点分别映射到第二局部坐标系中，分别得到第二线点和第二面点，每个第一线点对应一个第二线点，每个第一面点对应一个第二面点，第二局部坐标系为前一帧机器人第二状态下的局部坐标系。
具体的，第二局部坐标系是指机器人在第二状态下的局部坐标系(物体坐标系)。第二局部坐标系通过第二状态确定，一个实施例中，获取第二状态中的三自由度位置以及三自由度速度，根据三自由度位置确定第二局部坐标系中的原点位置，并根据三自由度速度确定第二局部坐标系中x轴、y轴和z轴的方向，进而得到第二局部坐标系。
进一步的，将激光点云数据中的第一线点和第一面点对分别映射到第二局部坐标系中，实施例中，将映射到第二局部坐标系的第一线点记为第二线点，此时，每个第一线点对应一个第二线点。将映射到第二局部坐标系的第一面点记为第二面点，此时，每个第一面点对应一个第二面点。可理解，由于激光点云数据位于第一局部坐标系中，因此，将第一线点和第一面点映射到第二局部坐标系时，需要先确定第一局部坐标系和第二局部坐标系之间的旋转矩阵和平移向量。其中，旋转矩阵和平移向量由第一状态和第二状态获得。具体的，旋转矩阵的确定方式为：获取第一状态中局部坐标系转换到世界坐标系下的旋转变换四元数，以及第二状态中局部坐标系转换到世界坐标系下的旋转变换四元数。实施例中，将第一状态对应的旋转变换四元数记为qi，其中，i表示当前帧，将第二状态对应的旋转变换四元数记为qi-1，其中，i-1表示前一帧。之后，利用四元数乘法得到第一局部坐标系转换到第二局部坐标系的旋转矩阵，其中，该旋转矩阵的计算方式为：平移矩阵的确定方式为：获取第一状态中机器人的三自由度位置以及第二状态中机器人的三自由度位置，实施例中，将第一状态中机器人的三自由度位置记为pi，第二状态中机器人的三自由度位置记为pi-1。那么，机器人的移动距离可表示为pi-pi-1。由于三自由度位置属于世界坐标系下，因此，该移动距离是机器人在世界坐标系下的移动距离。之后，将移动距离转换到第二局部坐标系下，其中，可根据第二状态中局部坐标系转换到世界坐标系下的旋转变换四元数可以得到第二局部坐标系转换到世界坐标系中的旋转矩阵，实施例中，将该旋转矩阵记为Ri-1。之后，根据Ri-1将移动距离转换到第二局部坐标系中，此时，第二局部坐标系中的移动距离便可以表示为/>即平移向量为/>之后，利用旋转矩阵和平移向量对第一线点和第一面点进行坐标转换，以得到相应的第二线点和第二面点。
步骤260、将第一局部地图映射到第二局部坐标系下，得到第二局部地图。
具体的，将第一局部地图映射到第二局部坐标系下，具体的，第一局部地图为世界坐标系下的地图，将其映射到第二局部坐标系时需要先确定世界坐标系变换成第二局部坐标系时的旋转矩阵和平移向量。实施例中，旋转矩阵和平移向量均通过第二状态确定。其中，旋转矩阵的确定方式为：根据第二状态中局部坐标系转换到世界坐标系下的旋转变换四元数得到第二局部坐标系转换到世界坐标系中的旋转矩阵，进而根据该旋转矩阵得到世界坐标系变换成第二局部坐标系的旋转矩阵。平移向量的确定方式为：将第二状态中机器人的三自由度位置作为世界坐标系变换成第二局部坐标系的平移向量。之后，将第一局部地图中各激光点映射到第二局部坐标系下，并按照第一局部地图中各激光点的拼接顺序对第二局部坐标系下的各激光点进行拼接，以得到第二局部坐标系下的第一局部地图，实施例中，将第二局部坐标系下的第一局部地图记为第二局部地图。可理解，第二局部地图可以体现机器人在第二局部坐标系下对激光点云数据对应的障碍物信息的描述。
步骤270、在第二局部地图中，查找每个第二线点对应的第一直线以及每个第二面点对应的第一平面。
示例性的，在第二局部地图中同样查找出线点和面点，且线点和面点的查找方式与激光点云数据中查找第一线点和第一面点的方式相同，在此不做赘述。具体的，根据第二线点在第二局部坐标系下的三维坐标，查找第二局部地图中位于第二线点附近的线点，并将查找到的线点拟合成一条直线以作为第二线点对应的直线。同样的，根据第二面点在第二局部坐标系下的三维坐标，查找第二局部地图中位于第二面点附近的面点，并将查找到的面点拟合成一个平面以作为第二面点对应的平面。实施例中，将第二局部地图中与第二线点对应的直线记为第一直线，与第二面点对应的平面记为第一平面。此时，每个第二线点均有对应的第一直线，每个第二面点均有对应的第一平面。需说明，每条第一直线可以对应多个第二线点，每个第一平面可以对应多个第二面点。
可理解，各第一线点在第一局部地图中也存在对应的直线，且各第一线点在第一局部地图中位于对应的直线上，各第一面点在第一局部地图中也存在对应的平面，且各第一面点在第一局部地图中位于对应的平面上。
步骤280、构建每个第二线点到对应第一直线的第一距离约束以及每个第二面点到对应第一平面的第二距离约束。
示例性的，构建第二线点与第一直线的距离约束以及第二面点与第一平面的距离约束。实施例中，将第二线点对应的距离约束记为第一距离约束，每个第二线点对应一个第一距离约束，将第二面点对应的距离约束记为第二距离约束，每个第二面点对应一个第二距离约束。第一距离约束和第二距离约束的计算方式相同，实施例中，以构建第一距离约束为例进行描述。
具体的，第一距离约束是指第二线点在第二局部坐标系下的三维坐标到第一直线的距离。可理解，第一状态预测的越准确，根据第一状态将激光点云数据映射到第二局部坐标系以及将第一局部地图映射到第二局部坐标系后，第二线点到对应第一直线的距离越近(包含第二线点位于第一直线上的情况)，前一帧和当前帧对于同一位置的障碍物信息描述越相同。一个实施例中，第一距离约束表示为：dN＝D1(ΔR*lN+Δp，m)，其中lN表示激光点云数据中第N个第一线点的三维坐标，dN表示第N个第二线点的第一距离约束，D1表示构建第一距离约束时使用的函数(即第N个第二线点到对应第一直线的距离计算函数)，ΔR表示第一旋转矩阵，所述第一旋转矩阵是第三局部坐标系转换到第二局部坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，m表示第一局部地图，Δp表示第三局部坐标系转换到第二局部坐标系时的平移向量，Ri-1表示第二旋转矩阵，所述第二旋转矩阵是第二局部坐标系转换到世界坐标系时的旋转矩阵，pi-1表示第二状态中机器人的三自由度位置，pi表示第一状态中机器人的三自由度位置，i表示当前帧。由上述表示可知，pi-pi-1表示机器人由前一帧到当前帧的移动距离，该移动距离为世界坐标系下的移动距离，通过Ri-1可将世界坐标系下的移动距离转换到第二局部坐标系中以作为平移向量。ΔR为第一状态下的第三局部坐标系转换到第二状态下的第二局部坐标系的旋转矩阵，可理解，/>其中，qi为第一状态对应的旋转变换四元数，其中，i表示当前帧，qi-1为第二状态对应的旋转变换四元数，其中，i-1表示前一帧，/>表示四元数乘法。ΔR*lN+Δp为将激光点云数据中第N个第一线点映射到第二局部坐标系后得到的第N个第二线点。通过D1(ΔR*lN+Δp，m)便可以得到第N个第二线点到第二局部地图中对应第一直线的距离，即第一距离约束。每个第一线点对应一个第一距离约束。可理解，由前述内容可知，激光点云数据中的各激光点均在第一局部坐标系中，因此，第一状态下的第三局部坐标系与上述第一局部坐标系为同一局部坐标系。
同样的，第二距离约束表示为：dM＝D2(ΔR*lM+Δp，m)，其中，lM表示激光点云数据中第M个第一面点的三维坐标，dM表示第M个第二面点的第二距离约束，D2表示构建第二距离约束时使用的函数(即第二面点到对应第一平面的距离计算函数)，ΔR表示第一旋转矩阵，m表示第一局部地图，Δp表示映射到第二局部坐标系内的移动距离，Ri-1表示第二旋转矩阵，pi-1表示第二状态中机器人的三自由度位置，pi表示第一状态中机器人的三自由度位置，i表示当前帧。
步骤290、根据各第一距离约束和各第二距离约束确定局部测量残差。
具体的，将每个第一距离约束和每个第二距离约束相加后，便可以得到局部测量残差。
步骤2100、根据激光点云数据中各激光点的曲率，识别出激光点云数据中的每个第一线点和每个第一面点。
该步骤与步骤的240过程相同，在此不做赘述。实际应用中，也可以直接获取步骤240中得到的第一线点和第一面点。
步骤2110、根据第一状态，确定各第一线点在世界坐标系下的第三线点以及各第一面点在世界坐标系下的第三面点，每个第一线点对应一个第三线点，每个第一面点对应一个第三面点。
具体的，将激光点云数据中各激光点映射到世界坐标系后，除了可以得到第一局部地图外，还可以确定各第一线点在世界坐标系中对应的激光点，以及各第一面点在世界坐标系中对应的激光打。实施例中，将全局地图中的第一线点记为第三线点，每个第一线点对应一个第三线点。将全局地图中的第一面点记为第三面点，每个第一面点对应一个第三面点。
步骤2120、在预存的全局地图中，查找每个第三线点对应的第二直线以及每个第三面点对应的第二平面。
示例性的，根据世界坐标系中的第三线点，在全局地图中确定对应的直线，以及，根据世界坐标系中的第三面点，在全局地图中确定对应的平面。实施例中，将全局地图中与第三线点对应的直线记为第二直线，与第三面点对应的平面记为第二平面。可理解，由于全局地图也是根据3D激光传感器构建的，所以，全局地图中各激光点也可以分为线点和面点。此时，确定第二直线的具体过程为：在全局地图中，查找第三线点附近的各线点，并将各线点拟合成一条直线，以作为第三线点对应的第二直线。同样的，在全局地图中，查找第三面点附近的各面点，并将各面点拟合成一个平面，以作为第三面点对应的第二平面。其中，第二直线和第二平面的构建依据与第一直线和第一平面的构建依据相同，在此不做赘述。
步骤2130、构建每个第三线点到对应第二直线的第三距离约束以及每个第三面点到对应第二平面的第四距离约束。
示例性的，构建第三线点与第二直线的距离约束以及第三面点与第二平面的距离约束。实施例中，将第三线点对应的距离约束记为第三距离约束，每个第三线点对应一个第三距离约束，将第三面点对应的距离约束记为第四距离约束，每个第三面点对应一个第四距离约束。第三距离约束和第四距离约束的计算方式相同，实施例中，以构建第三距离约束为例进行描述。
具体的，第三距离约束是指第三线点在世界坐标系下的三维坐标到对应第二直线的距离。可理解，第一状态预测的越准确，根据第一状态将激光点云数据映射到世界坐标系后，世界坐标系下的各第三线点到对应第二直线的距离越近(包含第三线点位于第二直线上的情况)，当前帧局部坐标系和世界坐标系中对于同一位置的障碍物信息描述越相同。一个实施例中，第三距离约束为：d'N＝D'1(Ri*lN+pi,M)，其中，lN表示激光点云数据中第N个第一线点的三维坐标，d'N表示第N个第三线点的第三距离约束，D'1表示构建第三距离约束时使用的函数(第N个第三线点到对应第二直线的距离计算函数)，Ri表示第三旋转矩阵，所述第三旋转矩阵是第三局部坐标系转换到世界坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，M表示全局地图，pi表示第一状态中机器人的三自由度位置，i表示当前帧。由上述表示可知，Ri通过第一状态对应的旋转变换四元数确定，其作为第三局部坐标系转换到世界坐标系的旋转矩阵，pi为第三局部坐标系转换到世界坐标系的平移向量，Ri*lN+pi为将激光点云数据中第N个第一线点映射到世界坐标系后对应的第N个第三线点。通过D'1(Ri*lN+pi,M)便可以得到第N个第三线点到全局地图中对应第二直线的距离，即第三距离约束。可理解，由前述内容可知，激光点云数据中的各激光点均在第一局部坐标系中，因此，第一状态下的第三局部坐标系与上述第一局部坐标系为同一局部坐标系。
同样的，第四距离约束表示为：d'M＝D'2(Ri*lM+pi,M)，其中，lM表示所述激光点云数据中第M个第一面点的三维坐标，d'M表示第M个第三面点的第四距离约束，D2表示构建第四距离约束时使用的函数(即第三面点到对应第二平面的距离计算函数)，ΔR表示第三旋转矩阵，所述第三旋转矩阵是第三局部坐标系转换到世界坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，M表示全局地图，pi表示第一状态中机器人的三自由度位置，i表示当前帧。
步骤2140、根据各第三距离约束和各第四距离约束确定全局测量残差。
具体的，将每个第三距离约束和每个第四距离约束相加后，便可以得到全局测量残差。
步骤2150、根据第一状态、第二状态以及两帧之间的惯性测量数据确定惯性测量预积分参数。
示例性的，惯性测量预积分参数是根据IMU的预积分模型对惯性测量数据进行预积分后得到的数据。预积分模型仅仅跟IMU的惯性测量数据有关,它将一段时间内的惯性测量数据直接积分起来就得到了预积分量(即惯性测量预积分参数)。一个实施例中，惯性测量预积分参数表示为：
其中，αbjbi表示第二状态到第一状态时机器人的第五距离约束，i表示当前帧，j表示前一帧，qbjbt表示第二状态对应的第二局部坐标系转换到第t时刻对应的第四局部坐标系时的第一旋转变换四元数约束，t∈[j,i]，abt表示第t时刻采集的惯性测量数据中的三轴加速度，βbjbi表示第二状态到第一状态时机器人的速度约束，qbjbi表示第二状态到第一状态时机器人的第二旋转变换四元数约束，wbt表示第t时刻采集的惯性测量数据中的三轴角速度，表示四元数乘法。通过上述公式，便可以得到任意两个时刻的惯性测量预积分参数。
步骤2160、根据惯性测量预积分参数确定第二状态到第一状态的惯量测量残差。
通过将一段时间内IMU构建的惯性测量预积分参数作为测量值,对两帧之间的机器人状态(即第一状态和第二状态)进行约束，便可以得到第二状态到第一状态的惯性测量残差。
一个实施例中，惯性测量残差表示为：
其中，gj,i表示惯性测量残差，rp表示第二状态到第一状态的三自由度位置残差，rq表示第二状态到第一状态的位姿四元数残差，rv表示第二状态到第一状态的三自由度速度残差，rba表示第二状态到第一状态的加速度偏置残差，rbg表示第二状态到第一状态的角速度偏置残差，表示第二状态中机器人的位姿四元数，/>表示/>的逆，/>表示第一状态中机器人的位姿四元数，/>表示第一状态中机器人的三自由度位置，/>表示第二状态中机器人的三自由度位置，/>表示第二状态中机器人的三自由度速度，/>表示第一状态中机器人的三自由度速度，gw表示重力加速度，Δt表示第二状态对应时刻与第t时刻的时间间隔，αbjbi表示所述第五距离约束，qbjbi表示所述第二旋转变换四元数约束，qbibj表示qbjbi的逆，βbjbi表示所述速度约束，/>表示第一状态时采集的惯性测量残差的加速度偏置，/>表示第二状态时采集的惯性测量残差的加速度偏置，/>表示第一状态时采集的惯性测量残差的角速度偏置，/>表示第二状态时采集的惯性测量残差的角速度偏置，[.]xyz表示取四元数的实部组成的三维向量。通过上述表示，便可以得到惯性测量残差。
需说明，由于惯性测量预积分参数和惯性测量残差的计算方式均为现有的计算方式，因此，实施例中不另做描述。
步骤2170、将第一状态加入滑动窗口，并根据滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数。
步骤2180、根据优化函数确定当前帧下机器人在全局地图中的位置。
图7为本申请实施例提供的一种机器人定位方法流程框图，参考图7，其为上述机器人定位方法中各数据流向关系。由图7可知，优化函数通过局部测量残差、全局测量残差以及惯性测量参数得到，点云去畸变是指将激光点云数据中各个激光点映射到同一第一局部坐标系中，其需要依靠第一状态进行。第一状态可以通过惯性测量数据获取，并根据优化函数进行更新。
上述，通过将激光点云数据中各个激光点映射到同一第一局部坐标系中，可以防止由于机器人移动使得各激光点不在同一局部坐标系中对后续计算准确性的影响。进一步的，通过将当前帧的激光点云数据和第一局部地图分别映射到第二局部坐标系中，进而通过第二局部坐标系内第二线点到第一直线的距离约束以及第二面点到第一平面的距离约束得到局部测量残差，以明确预测第一状态时不同帧下对局部区域的测量误差。进一步的，通过将当前帧的激光点云数据映射到世界坐标系中，进而通过第三线点到全局地图中对应第二直线的距离约束以及第三面点到全局地图中对应第二平面的距离约束得到全局测量残差，以明确预测第一状态时局部区域和全局区域间的测量误差。进一步的，通过将局部测量残差、全局测量残差以及惯性测量残差作为机器人状态的约束，以得到机器人状态，实现了紧耦合优化，降低了机器人定位的计算难度，且结合滑动窗口优化，即考虑一段时间内的机器人状态，可以提高机器人定位的精度，增强了机器人对环境改变的鲁棒性，提高机器人对空间环境的适应能力。
图8为本申请实施例提供的一种机器人定位装置的结构示意图。参考图8，该机器人定位装置包括：数据获取模块301、地图绘制模块302、局部残差确定模块303、全局残差确定模块304、惯性残差确定模块305、函数构建模块306以及位置确定模块307。
其中，数据获取模块301，用于获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态；地图绘制模块302，用于根据激光点云数据绘制机器人在当前帧识别到的第一局部地图；局部残差确定模块303，用于根据第一局部地图、激光点云数据、第一状态以及机器人在前一帧的第二状态确定局部测量残差；全局残差确定模块304，用于根据预存的全局地图、激光点云数据以及第一状态确定全局测量残差；惯性残差确定模块305，用于根据第一状态、第二状态以及两帧之间的惯性测量数据确定惯性测量残差；函数构建模块306，用于将第一状态加入滑动窗口，并根据滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数；位置确定模块307，用于根据优化函数确定当前帧下机器人在全局地图中的位置。
在上述实施例的基础上，地图绘制模块302包括：第一映射单元，用于将激光点云数据中的各激光点映射到世界坐标系下；拼接单元，用于拼接世界坐标系下的各激光点，以得到机器人在当前帧识别到的第一局部地图。
在上述实施例的基础上，还包括：激光点获取模块，用于将激光点云数据中的各激光点映射到世界坐标系下之前，获取激光点云数据中的第一个激光点；激光点映射模块，用于以第一个激光点为基准，将激光点云数据中的其他激光点映射到第一个激光点所在的第一局部坐标系中。
在上述实施例的基础上，局部残差确定模块303包括：第一识别单元，用于根据激光点云数据中各激光点的曲率，识别出激光点云数据中的每个第一线点和每个第一面点；第二映射单元，用于将第一线点和第一面点分别映射到第二局部坐标系中，分别得到第二线点和第二面点，每个第一线点对应一个第二线点，每个第一面点对应一个第二面点，第二局部坐标系为前一帧机器人第二状态下的局部坐标系；第三映射单元，用于将第一局部地图映射到第二局部坐标系下，得到第二局部地图；第一查找单元，用于在第二局部地图中，查找每个第二线点对应的第一直线以及每个第二面点对应的第一平面；第一约束单元，用于构建每个第二线点到对应第一直线的第一距离约束以及每个第二面点到对应第一平面的第二距离约束；第一残差确定单元，用于根据各第一距离约束和各第二距离约束确定局部测量残差。
在上述实施例的基础上，第一距离约束为：dN＝D1(ΔR*lN+Δp,m),其中，lN表示激光点云数据中第N个第一线点的三维坐标，dN表示第N个第二线点的第一距离约束，D1表示构建第一距离约束时使用的函数，ΔR表示第一旋转矩阵，所述第一旋转矩阵是第三局部坐标系转换到第二局部坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，m表示第一局部地图，Δp表示第三局部坐标系转换到第二局部坐标系时的平移向量，Ri-1表示第二旋转矩阵，所述第二旋转矩阵是第二局部坐标系转换到世界坐标系时的旋转矩阵，pi-1表示第二状态中机器人的三自由度位置，pi表示第一状态中机器人的三自由度位置，i表示当前帧。
在上述实施例的基础上，全局残差确定模块304包括：第二识别单元，用于根据激光点云数据中各激光点的曲率，识别出激光点云数据中的每个第一线点和每个第一面点；第四映射单元，用于根据第一状态，确定各第一线点在世界坐标系下的第三线点以及各第一面点在世界坐标系下的第三面点，每个第一线点对应一个第三线点，每个第一面点对应一个第三面点；第二查找单元，用于在预存的全局地图中，查找每个第三线点对应的第二直线以及每个第三面点对应的第二平面；第二约束单元，用于构建每个第三线点到对应第二直线的第三距离约束以及每个第三面点到对应第二平面的第四距离约束；第二残差确定单元，用于根据各所述第三距离约束和各所述第四距离约束确定全局测量残差。
在上述实施例的基础上，第三距离约束为：d'N＝D'1(Ri*lN+pi,M)，其中，lN表示激光点云数据中第N个第一线点的三维坐标，d'N表示第N个第三线点的第三距离约束，D'1表示构建第三距离约束时使用的函数，Ri表示第三旋转矩阵，所述第三旋转矩阵是第三局部坐标系转换到世界坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，M表示全局地图，pi表示第一状态中机器人的三自由度位置，i表示当前帧。
在上述实施例的基础上，惯性残差确定模块305包括：预积分单元，用于根据第一状态、第二状态以及两帧之间的惯性测量数据确定惯性测量预积分参数；第三残差确定单元，用于根据惯性测量预积分参数确定第二状态到第一状态的惯量测量残差。
在上述实施例的基础上，惯性测量预积分参数为：
其中，αbjbi表示第二状态到第一状态时机器人的第五距离约束，i表示当前帧，j表示前一帧，qbjbt表示第二状态对应的第二局部坐标系转换到第t时刻对应的第四局部坐标系时的第一旋转变换四元数约束，t∈[j,i]，abt表示第t时刻采集的惯性测量数据中的三轴加速度，βbjbi表示第二状态到第一状态时机器人的速度约束，qbjbi表示第二状态到第一状态时机器人的第二旋转变换四元数约束，wbt表示第t时刻采集的惯性测量数据中的三轴角速度，表示四元数乘法。
在上述实施例的基础上，惯性测量残差为：
其中，gj,i表示惯性测量残差，rp表示第二状态到第一状态的三自由度位置残差，rq表示第二状态到第一状态的位姿四元数残差，rv表示第二状态到第一状态的三自由度速度残差，rba表示第二状态到第一状态的加速度偏置残差，rbg表示第二状态到第一状态的角速度偏置残差，表示第二状态中机器人的位姿四元数，/>表示/>的逆，/>表示第一状态中机器人的位姿四元数，/>表示第一状态中机器人的三自由度位置，/>表示第二状态中机器人的三自由度位置，/>表示第二状态中机器人的三自由度速度，/>表示第一状态中机器人的三自由度速度，gw表示重力加速度，Δt表示第二状态对应时刻与第t时刻的时间间隔，αbjbi表示所述第五距离约束，qbjbi表示所述第二旋转变换四元数约束，qbibj表示qbjbi的逆，βbjbi表示所述速度约束，/>表示第一状态时采集的惯性测量残差的加速度偏置，/>表示第二状态时采集的惯性测量残差的加速度偏置，/>表示第一状态时采集的惯性测量残差的角速度偏置，/>表示第二状态时采集的惯性测量残差的角速度偏置。
在上述实施例的基础上，所述优化函数表示为：
其中，i表示当前帧，σ表示滑动窗口内机器人状态的总数量，[1-σ,i]表示滑动窗口中的各帧，X表示滑动窗口中各帧下机器人状态的状态集合，ο表示滑动窗口中的第ο帧，eLο(l,X)表示第ο帧的局部测量残差，表示第o帧的第一局部地图，eMο(M,X)表示第ο帧的全局测量残差，eIο(gο-1,ο,X)表示第ο帧的惯性测量残差。
上述提供的机器人定位装置可用于执行上述任意实施例提供的机器人定位方法，具备相应的功能和有益效果。
值得注意的是，上述机器人定位装置的实施例中，所包括的各个单元和模块只是按照功能逻辑进行划分的，但并不局限于上述的划分，只要能够实现相应的功能即可；另外，各功能单元的具体名称也只是为了便于相互区分，并不用于限制本发明的保护范围。
图9为本申请实施例提供的一种机器人的结构示意图。如图9所示，该机器人包括处理器40、存储器41、激光传感器42、移动装置43、输入装置44、输出装置45、惯性测量单元46；机器人中处理器40的数量可以是一个或多个，图9中以一个处理器40为例。机器人中处理器40、存储器41、激光传感器42、移动装置43、输入装置44、输出装置45、惯性测量单元46可以通过总线或其他方式连接，图9中以通过总线连接为例。
存储器41作为一种计算机可读存储介质，可用于存储软件程序、计算机可执行程序以及模块，如本发明实施例中的机器人定位方法对应的程序指令/模块(例如，机器人定位装置中的数据获取模块301、地图绘制模块302、局部残差确定模块303、全局残差确定模块304、惯性残差确定模块305、函数构建模块306以及位置确定模块307)。处理器40通过运行存储在存储器41中的软件程序、指令以及模块，从而执行机器人的各种功能应用以及数据处理，即实现上述的机器人定位方法。
存储器41可主要包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需的应用程序；存储数据区可存储根据机器人的使用所创建的数据等。此外，存储器41可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实例中，存储器41可进一步包括相对于处理器40远程设置的存储器，这些远程存储器可以通过网络连接至机器人设备。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。
激光传感器42用于采集激光点云数据。移动装置43用于根据处理器40的指令进行移动。输入装置44可用于接收输入的数字或字符信息，以及产生与机器人的用户设置以及功能控制有关的键信号输入。输出装置45可包括显示屏等显示设备。惯性测量单元46用于采集惯性测量数据。机器人还可包括通信装置，以与其他设备进行数据通信。
上述机器人包含机器人定位装置，可以用于执行任意机器人定位方法，具备相应的功能和有益效果。
此外，本发明实施例还提供一种包含计算机可执行指令的存储介质，所述计算机可执行指令在由计算机处理器执行时用于执行本申请任意实施例所提供的机器人定位方法中的相关操作，且具备相应的功能和有益效果。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。
因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。存储器可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。存储器是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括要素的过程、方法、商品或者设备中还存在另外的相同要素。
注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。
Claims (13)
1.一种机器人定位方法，其特征在于，包括：
获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态，所述激光点云数据中各激光点的三维坐标是当前帧下局部坐标系内的三维坐标；
根据所述激光点云数据绘制所述机器人在当前帧识别到的第一局部地图，所述第一局部地图的激光点的三维坐标为世界坐标系下的三维坐标；
根据所述第一局部地图、所述激光点云数据、所述第一状态以及所述机器人在前一帧的第二状态确定局部测量残差，局部测量残差用于体现机器人在前一帧描述的扫描区域和当前帧描述的扫描区域之间的差别；
根据预存的全局地图、所述激光点云数据以及所述第一状态确定全局测量残差，全局地图是世界坐标系下的地图，全局测量残差用于体现当前帧激光点云数据描述的遮挡物信息与全局地图中对应局部区域中描述的遮挡物信息之间的差别；
根据所述第一状态、所述第二状态以及两帧之间的惯性测量数据确定惯性测量残差；
将所述第一状态加入滑动窗口，并根据所述滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数；
根据所述优化函数确定当前帧下所述机器人在所述全局地图中的位置。
2.根据权利要求1所述的机器人定位方法，其特征在于，所述根据所述激光点云数据绘制所述机器人在当前帧识别到的第一局部地图包括：
将所述激光点云数据中的各激光点映射到世界坐标系下；
拼接所述世界坐标系下的各激光点，以得到机器人在当前帧识别到的第一局部地图。
3.根据权利要求2所述的机器人定位方法，其特征在于，所述将所述激光点云数据中的各激光点映射到世界坐标系下之前，包括：
获取所述激光点云数据中的第一个激光点；
以所述第一个激光点为基准，将所述激光点云数据中的其他激光点映射到所述第一个激光点所在的第一局部坐标系中。
4.根据权利要求1所述的机器人定位方法，其特征在于，所述根据所述第一局部地图、所述激光点云数据、所述第一状态以及所述机器人在前一帧的第二状态确定局部测量残差包括：
根据所述激光点云数据中各激光点的曲率，识别出所述激光点云数据中的每个第一线点和每个第一面点；
将所述第一线点和所述第一面点分别映射到第二局部坐标系中，分别得到第二线点和第二面点，每个所述第一线点对应一个第二线点，每个所述第一面点对应一个第二面点，所述第二局部坐标系为前一帧机器人第二状态下的局部坐标系；
将所述第一局部地图映射到所述第二局部坐标系下，得到第二局部地图；
在所述第二局部地图中，查找每个所述第二线点对应的第一直线以及每个所述第二面点对应的第一平面；
构建每个所述第二线点到对应第一直线的第一距离约束以及每个所述第二面点到对应第一平面的第二距离约束；
根据各所述第一距离约束和各所述第二距离约束确定局部测量残差。
5.根据权利要求4所述的机器人定位方法，其特征在于，所述第一距离约束为：dN＝D1(ΔR*lN+Δp，m)，
其中，lN表示激光点云数据中第N个第一线点的三维坐标，dN表示第N个第二线点的第一距离约束，D1表示构建第一距离约束时使用的函数，ΔR表示第一旋转矩阵，所述第一旋转矩阵是第三局部坐标系转换到第二局部坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，m表示第一局部地图，Δp表示第三局部坐标系转换到第二局部坐标系时的平移向量，Ri-1表示第二旋转矩阵，所述第二旋转矩阵是第二局部坐标系转换到世界坐标系时的旋转矩阵，pi-1表示第二状态中机器人的三自由度位置，pi表示第一状态中机器人的三自由度位置，i表示当前帧。
6.根据权利要求1所述的机器人定位方法，其特征在于，所述根据预存的全局地图、所述激光点云数据以及所述第一状态确定全局测量残差包括：
根据所述激光点云数据中各激光点的曲率，识别出所述激光点云数据中的每个第一线点和每个第一面点；
根据所述第一状态，确定各所述第一线点在世界坐标系下的第三线点以及各所述第一面点在所述世界坐标系下的第三面点，每个所述第一线点对应一个第三线点，每个所述第一面点对应一个第三面点；
在预存的全局地图中，查找每个所述第三线点对应的第二直线以及每个所述第三面点对应的第二平面；
构建每个所述第三线点到对应第二直线的第三距离约束以及每个所述第三面点到对应第二平面的第四距离约束；
根据各所述第三距离约束和各所述第四距离约束确定全局测量残差。
7.根据权利要求6所述的机器人定位方法，其特征在于，所述第三距离约束为：d'N＝D'1(Ri*lN+pi,M)，
其中，lN表示激光点云数据中第N个第一线点的三维坐标，d'N表示第N个第三线点的第三距离约束，D'1表示构建第三距离约束时使用的函数，Ri表示第三旋转矩阵，所述第三旋转矩阵是第三局部坐标系转换到世界坐标系时的旋转矩阵，所述第三局部坐标系为所述第一状态下机器人的局部坐标系，M表示全局地图，pi表示第一状态中机器人的三自由度位置，i表示当前帧。
8.根据权利要求1所述的机器人定位方法，其特征在于，所述根据所述第一状态、所述第二状态以及两帧之间的惯性测量数据确定惯性测量残差包括：
根据所述第一状态、所述第二状态以及两帧之间的惯性测量数据确定惯性测量预积分参数；
根据惯性测量预积分参数确定所述第二状态到所述第一状态的惯量测量残差。
9.根据权利要求8所述的机器人定位方法，其特征在于，所述惯性测量预积分参数为：
其中，αbjbi表示第二状态到第一状态时机器人的第五距离约束，i表示当前帧，j表示前一帧，qbjbt表示第二状态对应的第二局部坐标系转换到第t时刻对应的第四局部坐标系时的第一旋转变换四元数约束，t∈[j,i]，abt表示第t时刻采集的惯性测量数据中的三轴加速度，βbjbi表示第二状态到第一状态时机器人的速度约束，qbjbi表示第二状态到第一状态时机器人的第二旋转变换四元数约束，wbt表示第t时刻采集的惯性测量数据中的三轴角速度，表示四元数乘法；
所述惯性测量残差为：
其中，gj,i表示惯性测量残差，rp表示第二状态到第一状态的三自由度位置残差，rq表示第二状态到第一状态的位姿四元数残差，rv表示第二状态到第一状态的三自由度速度残差，rba表示第二状态到第一状态的加速度偏置残差，rbg表示第二状态到第一状态的角速度偏置残差，表示第二状态中机器人的位姿四元数，/>表示/>的逆，/>表示第一状态中机器人的位姿四元数，/>表示第一状态中机器人的三自由度位置，/>表示第二状态中机器人的三自由度位置，/>表示第二状态中机器人的三自由度速度，/>表示第一状态中机器人的三自由度速度，gw表示重力加速度，Δt表示第二状态对应时刻与第t时刻的时间间隔，αbjbi表示所述第五距离约束，qbjbi表示所述第二旋转变换四元数约束，qbibj表示qbjbi的逆，βbjbi表示所述速度约束，/>表示第一状态时采集的惯性测量残差的加速度偏置，/>表示第二状态时采集的惯性测量残差的加速度偏置，/>表示第一状态时采集的惯性测量残差的角速度偏置，/>表示第二状态时采集的惯性测量残差的角速度偏置。
10.根据权利要求1所述的机器人定位方法，其特征在于，所述优化函数为：
其中，i表示当前帧，σ表示滑动窗口内机器人状态的总数量，[1-σ,i]表示滑动窗口中的各帧，X表示滑动窗口中各帧下机器人状态的状态集合，o表示滑动窗口中的第o帧，eLo(l,X)表示第o帧的局部测量残差，表示第o帧的第一局部地图，eMo(M,X)表示第o帧的全局测量残差，eIo(go-1,o,X)表示第o帧的惯性测量残差。
11.一种机器人定位装置，其特征在于，包括：
数据获取模块，用于获取机器人在当前帧采集的激光点云数据以及当前帧的第一状态，所述激光点云数据中各激光点的三维坐标是当前帧下局部坐标系内的三维坐标；
地图绘制模块，用于根据所述激光点云数据绘制所述机器人在当前帧识别到的第一局部地图，所述第一局部地图的激光点的三维坐标为世界坐标系下的三维坐标；
局部残差确定模块，用于根据所述第一局部地图、所述激光点云数据、所述第一状态以及所述机器人在前一帧的第二状态确定局部测量残差，局部测量残差用于体现机器人在前一帧描述的扫描区域和当前帧描述的扫描区域之间的差别；
全局残差确定模块，用于根据预存的全局地图、所述激光点云数据以及所述第一状态确定全局测量残差，全局地图是世界坐标系下的地图，全局测量残差用于体现当前帧激光点云数据描述的遮挡物信息与全局地图中对应局部区域中描述的遮挡物信息之间的差别；
惯性残差确定模块，用于根据所述第一状态、所述第二状态以及两帧之间的惯性测量数据确定惯性测量残差；
函数构建模块，用于将所述第一状态加入滑动窗口，并根据所述滑动窗口内各帧对应的局部测量残差、全局测量残差和惯性测量残差构建优化函数；
位置确定模块，用于根据所述优化函数确定当前帧下所述机器人在所述全局地图中的位置。
12.一种机器人，其特征在于，包括：
激光传感器，用于采集激光点云数据；
惯性测量单元，用于采集惯性测量数据；
一个或多个处理器；
存储器，用于存储一个或多个程序；
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-10中任一所述的机器人定位方法。
13.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现如权利要求1-10中任一所述的机器人定位方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011466991.4A CN112506200B (zh) | 2020-12-14 | 2020-12-14 | 机器人定位方法、装置、机器人及存储介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202011466991.4A CN112506200B (zh) | 2020-12-14 | 2020-12-14 | 机器人定位方法、装置、机器人及存储介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN112506200A CN112506200A (zh) | 2021-03-16 |
| CN112506200B true CN112506200B (zh) | 2023-12-08 |
Family
ID=74973875
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202011466991.4A Active CN112506200B (zh) | 2020-12-14 | 2020-12-14 | 机器人定位方法、装置、机器人及存储介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN112506200B (zh) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113313765B (zh) * | 2021-05-28 | 2023-12-01 | 上海高仙自动化科技发展有限公司 | 定位方法、装置、电子设备和存储介质 |
| CN113503883B (zh) * | 2021-06-22 | 2022-07-19 | 北京三快在线科技有限公司 | 采集用于构建地图的数据的方法、存储介质及电子设备 |
| CN114018269B (zh) * | 2021-11-22 | 2024-03-26 | 阿波罗智能技术(北京)有限公司 | 定位方法、装置、电子设备、存储介质以及自动驾驶车辆 |
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107438752A (zh) * | 2016-12-23 | 2017-12-05 | 深圳前海达闼云端智能科技有限公司 | 定位方法、终端和服务器 |
| CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
| CN109900280A (zh) * | 2019-03-27 | 2019-06-18 | 浙江大学 | 一种基于自主导航的畜禽信息感知机器人与地图构建方法 |
| CN110428467A (zh) * | 2019-07-30 | 2019-11-08 | 四川大学 | 一种相机、imu和激光雷达联合的机器人定位方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN111337947A (zh) * | 2020-05-18 | 2020-06-26 | 深圳市智绘科技有限公司 | 即时建图与定位方法、装置、系统及存储介质 |
| CN111402339A (zh) * | 2020-06-01 | 2020-07-10 | 深圳市智绘科技有限公司 | 一种实时定位方法、装置、系统及存储介质 |
| CN111427061A (zh) * | 2020-06-15 | 2020-07-17 | 北京云迹科技有限公司 | 一种机器人建图方法、装置，机器人及存储介质 |
| CN111508021A (zh) * | 2020-03-24 | 2020-08-07 | 广州视源电子科技股份有限公司 | 一种位姿确定方法、装置、存储介质及电子设备 |
| CN111895989A (zh) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | 一种机器人的定位方法、装置和电子设备 |
| CN112067007A (zh) * | 2020-11-12 | 2020-12-11 | 湖北亿咖通科技有限公司 | 地图生成方法、计算机存储介质及电子设备 |
-
2020
- 2020-12-14 CN CN202011466991.4A patent/CN112506200B/zh active Active
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107438752A (zh) * | 2016-12-23 | 2017-12-05 | 深圳前海达闼云端智能科技有限公司 | 定位方法、终端和服务器 |
| CN108917759A (zh) * | 2018-04-19 | 2018-11-30 | 电子科技大学 | 基于多层次地图匹配的移动机器人位姿纠正算法 |
| CN109900280A (zh) * | 2019-03-27 | 2019-06-18 | 浙江大学 | 一种基于自主导航的畜禽信息感知机器人与地图构建方法 |
| CN110428467A (zh) * | 2019-07-30 | 2019-11-08 | 四川大学 | 一种相机、imu和激光雷达联合的机器人定位方法 |
| CN111045017A (zh) * | 2019-12-20 | 2020-04-21 | 成都理工大学 | 一种激光和视觉融合的巡检机器人变电站地图构建方法 |
| CN111508021A (zh) * | 2020-03-24 | 2020-08-07 | 广州视源电子科技股份有限公司 | 一种位姿确定方法、装置、存储介质及电子设备 |
| CN111337947A (zh) * | 2020-05-18 | 2020-06-26 | 深圳市智绘科技有限公司 | 即时建图与定位方法、装置、系统及存储介质 |
| CN111402339A (zh) * | 2020-06-01 | 2020-07-10 | 深圳市智绘科技有限公司 | 一种实时定位方法、装置、系统及存储介质 |
| CN111427061A (zh) * | 2020-06-15 | 2020-07-17 | 北京云迹科技有限公司 | 一种机器人建图方法、装置，机器人及存储介质 |
| CN111895989A (zh) * | 2020-06-24 | 2020-11-06 | 浙江大华技术股份有限公司 | 一种机器人的定位方法、装置和电子设备 |
| CN112067007A (zh) * | 2020-11-12 | 2020-12-11 | 湖北亿咖通科技有限公司 | 地图生成方法、计算机存储介质及电子设备 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN112506200A (zh) | 2021-03-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112506200B (zh) | 机器人定位方法、装置、机器人及存储介质 | |
| CN109084732B (zh) | 定位与导航方法、装置及处理设备 | |
| CN111402339B (zh) | 一种实时定位方法、装置、系统及存储介质 | |
| Panahandeh et al. | Vision-aided inertial navigation based on ground plane feature detection | |
| CN110275540A (zh) | 用于扫地机器人的语义导航方法及其系统 | |
| US8510039B1 (en) | Methods and apparatus for three-dimensional localization and mapping | |
| CN106339001A (zh) | 地图生成方法、移动机器人以及地图生成系统 | |
| CN111121754A (zh) | 移动机器人定位导航方法、装置、移动机器人及存储介质 | |
| CN111487960A (zh) | 一种基于定位能力估计的移动机器人路径规划方法 | |
| WO2018154579A1 (en) | Method of navigating an unmanned vehicle and system thereof | |
| CN111263308A (zh) | 定位数据采集方法及系统 | |
| KR20200028210A (ko) | 모바일 맵핑 또는 자율 주행용 플랫폼과 관측 데이터의 구조화를 위한 시스템 | |
| CN112068152A (zh) | 使用3d扫描仪同时进行2d定位和2d地图创建的方法和系统 | |
| Adam et al. | Fusion of fixation and odometry for vehicle navigation | |
| CN116608847A (zh) | 基于面阵激光传感器和图像传感器的定位和建图方法 | |
| EP4258078A1 (en) | Positioning method and apparatus, and vehicle | |
| CN112652001B (zh) | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 | |
| JP5105595B2 (ja) | 自律走行移動体の走行経路決定用地図作成装置及び走行経路決定用地図作成方法 | |
| CN116202509A (zh) | 一种面向室内多层建筑的可通行地图生成方法 | |
| AU2021273605B2 (en) | Multi-agent map generation | |
| KR20200080598A (ko) | 이동 로봇의 주행 평가 방법 | |
| Hasler et al. | Implementation and first evaluation of an indoor mapping application using smartphones and AR frameworks | |
| CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
| JP5953393B2 (ja) | ロボットシステム及び地図更新方法 | |
| CN114964204A (zh) | 地图构建方法、地图使用方法、装置、设备和存储介质 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |