CN112269187B - 机器人状态检测方法、装置及设备 - Google Patents

机器人状态检测方法、装置及设备 Download PDF

Info

Publication number
CN112269187B
CN112269187B CN202011042717.4A CN202011042717A CN112269187B CN 112269187 B CN112269187 B CN 112269187B CN 202011042717 A CN202011042717 A CN 202011042717A CN 112269187 B CN112269187 B CN 112269187B
Authority
CN
China
Prior art keywords
robot state
constraint
robot
laser
state detection
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
Application number
CN202011042717.4A
Other languages
English (en)
Other versions
CN112269187A (zh
Inventor
林燕龙
张伟
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Guangzhou Shirui Electronics Co Ltd
Original Assignee
Guangzhou Shiyuan Electronics Thecnology Co Ltd
Guangzhou Shirui Electronics Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Guangzhou Shiyuan Electronics Thecnology Co Ltd, Guangzhou Shirui Electronics Co Ltd filed Critical Guangzhou Shiyuan Electronics Thecnology Co Ltd
Priority to CN202011042717.4A priority Critical patent/CN112269187B/zh
Publication of CN112269187A publication Critical patent/CN112269187A/zh
Application granted granted Critical
Publication of CN112269187B publication Critical patent/CN112269187B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P3/00—Measuring linear or angular speed; Measuring differences of linear or angular speeds
    • G01P3/36—Devices characterised by the use of optical means, e.g. using infrared, visible, or ultraviolet light
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
    • G01S17/06—Systems determining position data of a target
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/88—Lidar systems specially adapted for specific applications
    • G01S17/89—Lidar systems specially adapted for specific applications for mapping or imaging

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Electromagnetism (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Power Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及一种机器人状态检测方法、装置及设备，所述机器人状态检测方法包括：将当前机器人状态节点加入预先创建的机器人状态检测窗口；获取机器人状态检测窗口内除当前机器人状态节点外的其余机器人状态节点的初始值以及机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；根据当前机器人状态节点的预测值、其余机器人状态节点的初始值以及预设的非线性优化算法，对激光相对约束、惯性预积分约束和轮计预积分约束进行非线性优化，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。相对于现有技术，本申请降低了机器人状态检测的误差，提高了机器人位姿估计的准确性。

Description

机器人状态检测方法、装置及设备
技术领域
本申请实施例涉及机器人领域，尤其涉及一种机器人状态检测方法、装置及设备。
背景技术
目前，机器人已经逐步应用于各行各业，例如安防巡逻、抗震救灾、农业植保、航空测绘、制造业、仓储物流等。在机器人工作的过程中，需要对机器人的状态进行检测，以保持机器人控制的稳定性，提高其作业的安全性。
通常对机器人状态进行检测的方法，是先根据机器人的上一状态以及传感器采集的数据各自预测机器人的当前状态，再把各个预测到的机器人当前状态组合成一个优化问题进行求解，利用上述方法检测到的机器人状态误差积累较快，难以对机器人位姿进行准确预估。
发明内容
本申请实施例提供了一种机器人状态检测方法、装置及设备，可以解决机器人状态检测结果的准确性较低的问题，所述技术方案如下:
第一方面，本申请实施例提供了一种机器人状态检测方法，包括：
接收当前激光测量时刻采集的机器人激光点云，获得当前激光测量时刻对应的当前机器人状态节点的预测值，将所述当前机器人状态节点加入预先创建的机器人状态检测窗口；其中，所述机器人状态检测窗口依次记录有多个激光测量时刻对应的机器人状态节点，且所述机器人状态检测窗口每加入一个新的机器人状态节点时，所述机器人状态检测窗口中最早加入的一个机器人状态节点被删除；
获取所述机器人状态检测窗口内除所述当前机器人状态节点外的其余机器人状态节点的初始值以及所述机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；其中，所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值；
根据所述当前机器人状态节点的预测值、所述其余机器人状态节点的初始值以及预设的非线性优化算法，对所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
可选的，所述获取所述机器人状态检测窗口内所有机器人状态节点的初始状态，包括步骤：
获取所述当前激光测量时刻与上一激光测量时刻之间的惯性数据；
根据所述惯性数据，获取所述当前机器人状态节点的预测值。
可选的，获取所述机器人状态检测窗口中每两个相邻机器人状态节点之间的激光相对约束，包括步骤：
获取所述机器人状态检测窗口内每个所述机器人状态节点对应的激光点云；
获取激光地图；其中，所述激光地图由所有已删除的机器人状态节点对应的激光点云拼接而成；
将所述激光地图和所述激光点云变换到目标坐标系下，计算每个所述激光点云与所述激光地图之间的激光点线约束和激光点面约束；其中，所述目标坐标系为所述激光点云对应的机器人状态节点的前一相邻机器人状态节点所对应的坐标系；
根据所述激光点线约束和激光点面约束，得到每两个相邻所述机器人状态节点之间的激光相对约束。
可选的，所述获取激光地图，包括步骤：
获取所有已删除的机器人状态节点对应的初始激光点云；
根据所述已删除的机器人状态节点的优化值，调整对应的所述初始激光点云的姿态和位置，得到调整后的激光点云；
拼接所述调整后的激光点云，得到所述激光地图。
可选的，所述将所述激光地图和所述激光点云变换到目标坐标系下之前，还包括步骤：对所述激光点云进行点云去畸变。
可选的，获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束，包括步骤：
获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束；其中，所述第一距离约束、第一速度约束和第一旋转四元数约束通过两个相邻激光测量时刻之间采集的惯性数据获取；
根据所述其余机器人状态节点的初始值以及所述第一距离约束、所述第一速度约束和所述第一旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束。
可选的，获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束，包括步骤：
获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束；其中，所述第二距离约束和第二旋转四元数约束通过两个相邻激光测量时刻之间采集的轮计数据获取；
根据所述其余机器人状态节点的初始值以及所述第二距离约束和第二旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束。
第二方面，本申请实施例提供了一种机器人状态检测装置，包括：
窗口滑动单元，用于接收当前激光测量时刻采集的机器人激光点云，获得当前激光测量时刻对应的当前机器人状态节点的预测值，将所述当前机器人状态节点加入预先创建的机器人状态检测窗口；其中，所述机器人状态检测窗口依次记录有多个激光测量时刻对应的机器人状态节点，且所述机器人状态检测窗口每加入一个新的机器人状态节点时，所述机器人状态检测窗口中最早加入的一个机器人状态节点被删除；
第一获取单元，用于获取所述机器人状态检测窗口内除所述当前机器人状态节点外的其余机器人状态节点的初始值以及所述机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；其中，所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值；
检测单元，用于根据所述当前机器人状态节点的预测值、所述其余机器人状态节点的初始值以及预设的非线性优化算法，对所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
第三方面，本申请实施例提供了一种设备，包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如第一方面所述的机器人状态检测方法的步骤。
第四方面，本申请实施例提供了一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，所述计算机程序被处理器执行时实现如上述第一方面所述的机器人状态检测方法的步骤。
本申请实施例通过预先创建一个机器人状态检测窗口，在所述机器人状态检测窗口内依次记录多个激光测量时刻对应的机器人状态节点，在接收到当前激光测量时刻采集的机器人激光点云时，滑动所述机器人状态检测窗口，加入当前激光测量时刻对应的当前机器人状态节点，删除最早加入的一个机器人状态节点，之后通过获取当前机器人状态节点的预测值、机器人状态检测窗口内除当前机器人状态节点外的其余机器人状态节点的初始值以及预设的非线性优化算法，对机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束进行非线性优化，使得每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取机器人状态检测窗口内所有机器人状态节点的优化值，然后当接收到下一个激光测量时刻采集的机器人激光点云时，再重复执行上述步骤。上述机器人状态检测方法解决了现有方法中仅根据机器人上一状态以及传感器采集数据各自预测机器人的当前状态再进行优化，而造成的机器人状态误差累积过快的技术问题，降低了机器人状态检测的误差，提高了机器人位姿估计的准确性。
为了更好地理解和实施，下面结合附图详细说明本申请的技术方案。
附图说明
图1为本申请一个实施例提供的机器人状态检测方法的流程示意图；
图2为本申请一个实施例中提供的机器人状态检测窗口的结构示意图；
图3为本申请一个实施例中提供的机器人激光点云和惯性数据的采集过程示意图；
图4为本申请一个实施例提供的机器人状态检测方法S102中获取激光相对约束的流程示意图；
图5为本申请一个实施例提供的机器人状态检测方法S1022的流程示意图；
图6为本申请一个实施例中激光点云的结构示意图；
图7为本申请一个实施例提供的机器人状态检测方法S102中获取惯性预积分约束的流程示意图；
图8为本申请一个实施例提供的机器人状态检测方法S102中获取轮计预积分约束的流程示意图；
图9为本申请一个实施例提供的机器人状态检测装置的结构示意图；
图10为本申请一个实施例提供的机器人状态检测设备的结构示意图。
具体实施方式
这里将详细地对示例性实施例进行说明，其示例表示在附图中。下面的描述涉及附图时，除非另有表示，不同附图中的相同数字表示相同或相似的要素。以下示例性实施例中所描述的实施方式并不代表与本申请相一致的所有实施方式。相反，它们仅是与如所附权利要求书中所详述的、本申请的一些方面相一致的装置和方法的例子。
在本申请使用的术语是仅仅出于描述特定实施例的目的，而非旨在限制本申请。在本申请和所附权利要求书中所使用的单数形式的“一种”、“所述”和“该”也旨在包括多数形式，除非上下文清楚地表示其他含义。还应当理解，本文中使用的术语“和/或”是指并包含一个或多个相关联的列出项目的任何或所有可能组合。
应当理解，尽管在本申请可能采用术语第一、第二、第三等来描述各种信息，但这些信息不应限于这些术语。这些术语仅用来将同一类型的信息彼此区分开。例如，在不脱离本申请范围的情况下，第一信息也可以被称为第二信息，类似地，第二信息也可以被称为第一信息。取决于语境，如在此所使用的词语“如果”/“若”可以被解释成为“在……时”或“当……时”或“响应于确定”。
在详细阐述本申请实施例提供的机器人状态检测方法之前，先对需要用到的3个传感器进行说明：
(1)激光传感器：本申请实施例中使用的激光传感器为3D激光传感器，可以为机械激光或固态激光，通过激光传感器发射激光测量能够得到机器人真实物理点在激光坐标系下的位置，输出激光点云。激光点云包括多个点，每个点对应一个三维坐标。
(2)惯性传感器：本申请实施例中使用的惯性传感器包括惯性测量单元(Inertialmeasurement unit，IMU)，惯性测量单元可输出机器人的惯性数据，也即IMU数据，如三轴角速度和三轴加速度。
(3)轮速计传感器：本申请实施例中使用的轮速传感器可根据内部编码器以及轮径等输出机器人的轮计数据，如左右轮的行进距离。
请参阅图1，为本申请一个实施例提供的机器人状态检测方法的流程示意图，所述方法包括如下步骤：
S101：接收当前激光测量时刻采集的机器人激光点云，获得当前激光测量时刻对应的当前机器人状态节点的预测值，将所述当前机器人状态节点加入预先创建的机器人状态检测窗口；其中，所述机器人状态检测窗口依次记录有多个激光测量时刻对应的机器人状态节点，且所述机器人状态检测窗口每加入一个新的机器人状态节点时，所述机器人状态检测窗口中最早加入的一个机器人状态节点被删除。
在本申请实施例中，机器人状态检测设备(以下简称状态检测设备)先分别与激光传感器、惯性传感器以及轮速计传感器建立通信连接，接收由上述3个传感器采集的机器人激光点云、惯性数据以及轮计数据。
在到达当前激光测量时刻之时，激光传感器发射激光测量机器人真实物理点在激光坐标系下的位置，输出机器人激光点云。状态检测设备接收当前激光测量时刻采集的机器人激光点云，生成当前激光时刻对应的当前机器人状态节点，将所述当前机器人状态节点加入所述预先创建的机器人状态检测窗口。
其中，所述机器人状态检测窗口是预先创建在状态检测设备内的虚拟窗口，该虚拟窗口能够依次记录多个激光测量时刻对应的机器人状态节点。
并且，所述机器人状态检测窗口是一个滑动窗口，其内部记录的机器人状态节点的个数是预先设置且保持不变的，在每加入一个新的机器人状态节点时，最早加入的一个机器人状态节点会被删除，相当于进行了一次窗口的滑动。
具体地，请参阅图2，图2为本申请一个实施例中提供的机器人状态检测窗口的结构示意图，其中，矩形框表示机器人状态检测窗口，圆形表示机器人状态节点，图2所示的机器人状态检测窗口中机器人状态节点的个数为5个，可以看出，机器人检测窗口向前滑动加入新的机器人状态节点old时，最早加入的一个机器人状态节点new被删除。
在本申请实施例中，激光传感器、惯性传感器和轮速计传感器分别按照一定的频率采集数据，其中，惯性传感器和轮速计传感器的采集频率较高，而激光传感器的采集频率要远低于两者。具体请参阅图3，图3为本申请一个实施例中提供的机器人激光点云和惯性数据的采集过程示意图，由图3可以看出，在采集第i个激光测量时刻采集的机器人激光点云(第i 帧激光点云)和第j个激光测量时刻采集的机器人激光点云(第j帧激光点云)之间，这里j＝i+1，惯性传感器会采集多次惯性数据(IMU数据)，而状态检测设备每接收一次所述惯性数据之后，就会根据机器人的上一预测状态和所述惯性数据，预测机器人的下一状态。
具体地，状态检测设备为了更准确地获取当前激光测量时刻对应的当前机器人状态节点的预测值，需先获取所述当前激光测量时刻与上一激光测量时刻之间的所有惯性数据，根据惯性数据获取当前机器人状态节点的预测值。
其中，机器人状态节点的预测值可表示为x＝(p,v,q,ba,bg)，p为当前机器人的三自由度位置，v为当前机器人的三自由度速度，q为当前机器人的姿态的四元数表示，ba为三轴加速度计的偏置，bg为三轴陀螺仪的偏置。
假设机器人在上一激光测量时刻对应的机器人状态节点的预测值为xi＝(pi,vi,qi,bai,bgi)，直至当前激光测量时刻到来之前，状态检测设备会接收到多个惯性数据，每接到一个新的惯性数据，就使用欧拉积分公式预测机器人的下一状态，其中，所述欧拉积分公式如下：
这里，Δt为惯性数据采集的时间间隔，a为加速度，w为角速度， xi+1＝(pi+1,vi+1,qi+1,bai+1,bgi+1)表示机器人下一状态的预测值。
在当前激光测量时刻到来之时，状态检测设备会同时接收到一个新的惯性数据，那么就再使用上述欧拉积分公式预测机器人的下一状态的预测值，此时，该预测值即是当前机器人状态节点的预测值。
S102：获取所述机器人状态检测窗口内除所述当前机器人状态节点外的其余机器人状态节点的初始值以及所述机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；其中，所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值。
在机器人状态检测窗口内除所述当前机器人状态节点之外，其余机器人状态节点在窗口滑动之前已经进行了一次优化，因此，可以理解的是，在未加入当前机器人状态节点前，其余机器人状态节点都有对应的一个优化值，该优化值即所述其余机器人状态节点的初始值。
在机器人状态检测窗口内，每两个相邻的机器人状态节点之间存在状态的约束，该状态的约束包括激光相对约束、惯性预积分约束和轮计预积分约束，下面将对激光相对约束、惯性预积分约束和轮计预积分约束展开详细的说明：
(1)激光相对约束：在本申请实施例中，激光相对约束包括激光点线相对约束和激光点面相对约束。
为准确获取机器人状态检测窗口中每两个相邻机器人状态节点之间的激光相对约束，请参阅图4，图4为本申请一个实施例提供的机器人状态检测方法S102中获取激光相对约束的流程示意图，具体地，步骤S102包括S1021～S1024，如下所示：
S1021：获取所述机器人状态检测窗口内每个所述机器人状态节点对应的激光点云。
每个激光测量时刻，状态检测设备都会接收到一帧机器人的激光点云，同时也会将当前激光测量时刻对应的当前机器人状态节点加入至机器人状态检测窗口，因此，每个机器人状态节点均有其对应的激光点云。状态检测设备获取到机器人状态检测窗口内每个所述机器人状态节点对应的激光点云。
对于每一帧激光点云来说，由于机器人的运动会导致每个激光点的发出时间不一样，因而会造成激光点的坐标系不一致，所以需要根据激光发射期间机器人的相对运动把激光点云变换到同一坐标系下，即对激光点云进行点云去畸变。
激光点云去畸变的方式多种多样，在本申请实施例，利用惯性数据对激光点云进行去畸变，具体过程如下：
状态检测设备将最新的惯性数据加入固定长度的队列中。接收到激光点云时，解析激光点云中每一个激光扫描点的时间戳。在队列中查找在时间上相邻的两帧惯性数据，通过惯性数据估计机器人在激光点云第一个点与当前点之间的相对运动变换关系为R0,Δt，p0,Δt，R0,Δt为用姿态变换，p0,Δt用于位置变换，此时，若l为畸变校正前的激光点，l'为畸变校正后的激光点，则可以得到l'＝R0,Δt*l+p0,Δt，完成点云去畸变。
S1022：获取激光地图；其中，所述激光地图由所有已删除的机器人状态节点对应的激光点云拼接而成。
在本申请实施例中，激光地图是由从机器人状态检测窗口中所有已删除的机器人状态节点对应的激光点云拼接而成的。
由于这些机器人状态节点已经从机器人状态检测窗口中删除，因而，状态节点的优化值不会再发生改变，可以理解为这些已删除的机器人状态节点的优化值已经为节点的最优状态。
所述已删除的机器人状态节点的优化值能够用于对激光地图进行位置和姿态的变换，从而不仅能够提高激光相对约束计算的准确性，还能够使激光地图更准确地反应出机器人行进路径。
在一个可选的实施例中，请参阅图5，为更准确地获取激光地图，对激光地图进行位置和姿态的变换，步骤S1022可以包括S10221～S10223，具体如下：
S10221：获取所有已删除的机器人状态节点对应的初始激光点云。
状态检测设备获取所有已删除的机器人状态节点对应的初始激光点云，所述初始激光点云是指未经过位置和姿态调整的激光点云。
S10222：根据所述已删除的机器人状态节点的优化值，调整对应的所述初始激光点云的姿态和位置，得到调整后的激光点云。
所述已删除的机器人状态节点的优化值包括机器人状态节点的位置优化值和姿态四元数优化值等。
根据每个已删除的机器人状态节点的优化值调整该机器人状态节点对应的初始激光点云的姿态和位置，从而得到调整后的激光点云。
S10223：拼接所述调整后的激光点云，得到所述激光地图。
状态检测设备根据采集所述初始激光点云的时间顺序，将对应的调整后的激光点云进行拼接，得到所述激光地图。
通过对激光点云进行位置和姿态的调整，能够得到更为准确的激光地图，该激光地图不仅能够反应出机器人在运动过程中的行进路径，还能够用对机器人进行定位导航。
S1023：将所述激光地图和所述激光点云变换到目标坐标系下，计算每个所述激光点云与所述激光地图之间的激光点线约束和激光点面约束；其中，所述目标坐标系为所述激光点云对应的机器人状态节点的前一相邻机器人状态节点所对应的坐标系。
在本申请实施例中，据激光点云的曲率可以提取到激光点云中的线点和面点，线点为高曲率的点，面点为低曲率的点，请参阅图6，图6为本申请一个实施例中激光点云的结构示意图，图6中直线h上的点为线点，其他点为面点。
同样地，在激光地图中也存在线点和面点，在建立激光点线约束时，在激光点云的附近搜索激光地图中线点，用这些线点拟合一条直线，计算激光点云与这条直线间的距离，作为激光点线约束。
在建立激光点面约束时，在激光点云的附近搜索激光地图中的面点，用这些面点拟合一个平面，计算激光点云与这个平面的距离，作为激光点面约束。
具体地，由于激光地图和各个机器人状态节点对应的激光点云所在的坐标系可能不同，因而，在计算每个激光点云与激光地图之间的激光相对约束之前，需要将激光地图和激光点云的坐标系变换到所述激光点云对应的机器人状态节点的前一相邻机器人状态节点所对应的坐标系下，以保证激光地图和各个机器人状态节点对应的激光点云所在的坐标系相同。
之后，再计算各个激光点云与激光地图之间的激光点线约束和激光点面约束。
在本申请实施例中，以将当前机器人状态节点为例，说明建立当前机器人状态节点与其前一相邻机器人状态节点之间的激光点线约束和激光点面约束的过程，具体如下：
d1＝D1(ΔR*li+Δp,m)
d2＝D2(ΔR*li+Δp,m)
其中，d1表示激光点线约束，d2表示激光点面约束，m表示激光地图，假设当前机器人状态节点的优化值为xi＝(pi,vi,qi,ba(i),bg(i))，其前一相邻机器人状态节点的优化值为xi-1＝(pi-1,vi-1,qi-1,ba(i-1),bg(i-1))，利用Ri-1表示qi-1对应的的旋转矩阵，ΔR表示对应的旋转矩阵，用于调整当前激光点云的姿态，/>用于调整当前激光点云的位置，符号T表示矩阵的转置，通过ΔR和Δp把当前机器人状态节点对应的激光点云的坐标系变换到前一相邻机器人状态节点所对应的坐标系下。
对于其他两个相邻机器人状态节点之间的激光点线约束和激光点面约束的过程与上述过程相同，在此不再赘述。
S1024：根据所述激光点线约束和激光点面约束，得到每两个相邻所述机器人状态节点之间的激光相对约束。
状态检测设备根据所述激光点线约束和激光点面约束，得到每两个相邻所述机器人状态节点之间的激光相对约束，激光相对约束用公式表示为eL(l,X)＝d1+d2。
(2)惯性预积分约束(也可以称为IMU预积分约束)：
惯性预积分约束是使用IMU预积分理论计算两个相邻激光测量时刻采集的激光点云之间的预积分量，作为两个相邻机器人状态节点之间的惯性预积分约束。
请参阅图7，图7为本申请一个实施例提供的机器人状态检测方法S102中获取惯性预积分约束的流程示意图，具体地，步骤S102包括S1025～S1026，如下所示：
S1025：获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束；其中，所述第一距离约束、第一速度约束和第一旋转四元数约束通过两个相邻激光测量时刻之间采集的惯性数据获取。
在本申请实施例中，状态检测设备获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束。
其中，所述两个相邻机器人节点之间的第一距离约束为两个相邻激光测量时刻之间机器人的行进距离；所述两个相邻机器人节点之间的第一速度约束为两个相邻激光测量时刻之间机器人的移动速度；所述两个相邻机器人节点之间的第一旋转四元数为两个相邻激光测量时刻之间机器人的姿态改变值。
上述第一距离约束、第一速度约束和第一旋转四元数约束均通过两个相邻激光测量时刻之间采集的惯性数据获取，所述惯性数据是由IMU惯性测量单元采集到的数据。
以当前机器人状态节点与其前一相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束为例，说明第一距离约束、第一速度约束和第一旋转四元数约束的计算过程，具体计算公式如下：
αbibj＝∫∫t∈[i,j](qbibtabt)δt2
βbibj＝∫t∈[i,j](qbibtabt)δt
其中，j表示当前激光测量时刻，i表示j的前一相邻激光测量时刻，qbibt为i时刻到t时刻 IMU积分得到的旋转四元数约束，abt和wbt均为惯性数据，abt为IMU在t时刻测量得到的加速度，wbt为IMU在t时刻测量得到的角速度，αbibj为当前机器人状态节点与其前一相邻机器人状态节点之间的第一距离约束，βbibj为当前机器人状态节点与其前一相邻机器人状态节点之间的第一速度约束，qbibj为当前机器人状态节点与其前一相邻机器人状态节点之间的第一旋转四元数约束。
对于其他两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束的计算过程与上述过程相同，在此不再赘述。
但是，需要说明的是，在本次非线性优化过程中，除当前机器人状态节点与其前一相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束需要计算之外。对于机器人状态窗口内其他两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束，其实已经在进行上一次非线性优化时计算完成，因此在本次非线性优化时无需再重复计算，以节省状态检测时间。
S1026：根据所述其余机器人状态节点的初始值以及所述第一距离约束、所述第一速度约束和所述第一旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束。
状态检测设备根据所述其余机器人状态节点的初始值以及所述第一距离约束、所述第一速度约束和所述第一旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束。
所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值，具体包括：机器人状态节点的位置优化值、速度优化值、姿态四元数优化值、加速度计偏置优化值和角速度计偏置优化值。
下面以建立当前机器人状态节点与其前一相邻机器人状态节点之间的惯性预积分约束为例，对惯性预积分约束的建立过程进行说明，其中，惯性预积分约束可以表示为：
根据惯性预积分约束的表示可以了解到，下标15×1表示所述惯性预积分约束实质上为15 维的数据，这是由于位置p、速度v、姿态四元数q以及加速度计偏置ba、角速度计偏置bg均是三维三个方向上的，一共是15维，所以对应的惯性预积分约束也是15维的。
由S1025的阐述能够得知，αbibj为当前机器人状态节点与其前一相邻机器人状态节点之间的第一距离约束，βbibj为当前机器人状态节点与其前一相邻机器人状态节点之间的第一速度约束，qbibj为当前机器人状态节点与其前一相邻机器人状态节点之间的第一旋转四元数约束，计算过程可以详见S1025中的说明。
表示当前机器人状态节点的位置优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的位置初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到位置优化值，其为已知量。
表示当前机器人状态节点的姿态四元数优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的姿态四元数初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到姿态四元数优化值，其为已知量；/>表示/>的四元数逆。
表示当前机器人状态节点的速度优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的速度初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到速度优化值，其为已知量；gw为重力加速度；Δt当前激光测量时刻与前一相邻激光测量时刻之间的时间间隔；[q]xyz表示取姿态四元数实部组成的三维向量，/>为四元数乘法。
表示当前机器人状态节点的加速度计偏置优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的加速度计偏置初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到加速度计偏置优化值，其为已知量。
表示当前机器人状态节点的角速度计偏置优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的角速度计偏的初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到角速度计偏置优化值，其为已知量。
对于建立所述机器人状态检测窗口内其他两个相邻机器人状态节点之间的惯性预积分约束的过程与上述过程相同，不再赘述。
(2)轮计预积分约束(也可以称为IMU预积分约束)：
轮计预积分约束是通过机器人模型计算得到每两个相邻激光测量时刻采集的激光点云之间的机器人运动的距离和旋转的偏航角，进而得到每两个相邻机器人状态节点之间的轮计预积分约束。
请参阅图8，图8为本申请一个实施例提供的机器人状态检测方法S102中获取轮计预积分约束的流程示意图，具体地，步骤S102包括S1027～S1028，如下所示：
S1027：获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束；其中，所述第二距离约束和第二旋转四元数约束通过两个相邻激光测量时刻之间采集的轮计数据获取。
在本申请实施例中，获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束。
其中，所述两个相邻机器人节点之间的第二距离约束为两个相邻激光测量时刻之间机器人的行进距离；所述两个相邻机器人节点之间的第二旋转四元数为两个相邻激光测量时刻之间机器人的姿态改变值，即机器人旋转的偏航角。
上述第二距离约束和第二旋转四元数约束均通过两个相邻激光测量时刻之间采集的轮计数据获取。
以当前机器人状态节点与其前一相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束为例，说明第二距离约束和第二旋转四元数约束的计算过程，具体计算公式如下：
其中，j表示当前激光测量时刻，i表示j的前一相邻激光测量时刻，qOiOt为i时刻到t时刻轮计的旋转四元数约束，wbt为轮计在t时刻测量得到的角速，qOiOj为当前机器人状态节点与其前一相邻机器人状态节点之间的第二旋转四元数约束，为轮计在时刻k-1到时刻k 的新进距离，Δpij为当前机器人状态节点与其前一相邻机器人状态节点之间的第二距离约束。
对于其他两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束的计算过程与上述过程相同，在此不再赘述。
但是，需要说明的是，在本次非线性优化过程中，除当前机器人状态节点与其前一相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束需要计算之外。对于机器人状态窗口内其他两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束，其实已经在进行上一次非线性优化时计算完成，因此在本次非线性优化时无需再重复计算，以节省状态检测时间。
S1028：根据所述其余机器人状态节点的初始值以及所述第二距离约束和第二旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束。
状态检测设备根据所述其余机器人状态节点的初始值以及所述第二距离约束和第二旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束。
所述其余机器人状态节点的初始值已经在步骤S1026中进行阐述，在此不再赘述。
同样地，以建立当前机器人状态节点与其前一相邻机器人状态节点之间的轮计预积分约束为例，对轮计预积分约束的建立过程进行说明，其中，轮计预积分约束可以表示为：
/>
qOiOj为当前机器人状态节点与其前一相邻机器人状态节点之间的第二旋转四元数约束；表示当前机器人状态节点的姿态四元数优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的姿态四元数初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到姿态四元数优化值，其为已知量；/>表示/>的四元数逆；[q]xyz表示取姿态四元数实部组成的三维向量；/>为四元数乘法。
Δpij为当前机器人状态节点与其前一相邻机器人状态节点之间的第二距离约束；表示当前机器人状态节点的位置优化值，在对机器人状态检测窗口进行本次非线性优化之前，其是一个未知量；/>表示前一相邻机器人状态节点的位置初始值，其是为未加入当前机器人状态节点前对所述机器人状态检测窗口内机器人状态节点进行非线性优化后，所得到位置优化值，其为已知量。
对于建立所述机器人状态检测窗口内其他两个相邻机器人状态节点之间的轮计预积分约束的过程与上述过程相同，不再赘述。
S103：根据所述当前机器人状态节点的预测值、所述其余机器人状态节点的初始值以及预设的非线性优化算法，对所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
状态检测设备以所述当前机器人状态节点的预测值和其余机器人状态节点的初始值作为收敛条件，通过预设的非线性优化算法对机器人状态检测窗口内每两个相邻所述机器人状态节点之间的所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
由于在机器人状态检测窗口内除所述当前机器人状态节点之外，其余机器人状态节点在窗口滑动之前已经进行了一次优化，因此，可以理解的是，在未加入当前机器人状态节点前，其余机器人状态节点都有对应的一个优化值。在本申请实施例中，将该优化值作为所述其余机器人状态节点的初始值，使用其余机器人状态节点的初始值与当前机器人状态节点的预测值一同加快所述预设的非线性优化算法的收敛。
在本申请实施例中，所述预设的非线性优化算法包括并不限于高斯牛顿，梯度下降，马夸尔特，狗腿算法等，对于非线性优化算法在此不做限定。
在一个可选的实施例中，所述激光相对约束、惯性预积分约束和轮计预积分约束之和是指对激光相对约束、惯性预积分约束和轮计预积分约束分别取绝对值后再累加的和。
在另一个可选的实施例中，所述激光相对约束、惯性预积分约束和轮计预积分约束之和是指对激光相对约束、惯性预积分约束和轮计预积分约束分别取绝对值再平方后累加的和，具体地，可以用表示，其中， eL(l,X)为激光相对约束，eI(gβ,β+1,X)为惯性预积分约束，eO(Oβ,β+1,X)为轮计预积分约束， j表示机器人状态检测窗口中机器人状态节点的个数，X(x1,...,xj)表示机器人状态检测窗口中机器人状态节点的集合。
可以理解的是，当i＝j时，为当前激光测量时刻采集机器人激光点云，eL(l,X)表示的是当前机器人状态节点与其前一相邻机器人状态节点之间的激光相对约束；当β＝j-1 时，eI(gβ,β+1,X)表示的就是当前机器人状态节点与其前一相邻机器人状态节点资产按的惯性预积分约束；当β＝j-1时，eI(gβ,β+1,X)表示的就是当前机器人状态节点与其前一相邻机器人状态节点之间的轮计预积分约束。
本申请实施例通过预先创建一个机器人状态检测窗口，在所述机器人状态检测窗口内依次记录多个激光测量时刻对应的机器人状态节点，在接收到当前激光测量时刻采集的机器人激光点云时，滑动所述机器人状态检测窗口，加入当前激光测量时刻对应的当前机器人状态节点，删除最早加入的一个机器人状态节点，之后通过获取当前机器人状态节点的预测值、机器人状态检测窗口内除当前机器人状态节点外的其余机器人状态节点的初始值以及预设的非线性优化算法，对机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束进行非线性优化，使得每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取机器人状态检测窗口内所有机器人状态节点的优化值，然后当接收到下一个激光测量时刻采集的机器人激光点云时，再重复执行上述步骤。上述机器人状态检测方法解决了现有方法中仅根据机器人上一状态以及传感器采集数据各自预测机器人的当前状态再进行优化，而造成的机器人状态误差累积过快的技术问题，降低了机器人状态检测的误差，提高了机器人位姿估计的准确性。
请参见图9，为本申请一个实施例提供的机器人状态检测装置的结构示意图。该装置可以通过软件、硬件或两者的结合实现成为机器人状态检测设备的全部或一部分。该装置9包括窗口滑动单元91、第一获取单元92和检测单元93：
窗口滑动单元91，用于接收当前激光测量时刻采集的机器人激光点云，获得当前激光测量时刻对应的当前机器人状态节点的预测值，将所述当前机器人状态节点加入预先创建的机器人状态检测窗口；其中，所述机器人状态检测窗口依次记录有多个激光测量时刻对应的机器人状态节点，且所述机器人状态检测窗口每加入一个新的机器人状态节点时，所述机器人状态检测窗口中最早加入的一个机器人状态节点被删除；
第一获取单元92，用于获取所述机器人状态检测窗口内除所述当前机器人状态节点外的其余机器人状态节点的初始值以及所述机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；其中，所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值；
检测单元93，用于根据所述当前机器人状态节点的预测值、所述其余机器人状态节点的初始值以及预设的非线性优化算法，对所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
本申请实施例通过预先创建一个机器人状态检测窗口，在所述机器人状态检测窗口内依次记录多个激光测量时刻对应的机器人状态节点，在接收到当前激光测量时刻采集的机器人激光点云时，滑动所述机器人状态检测窗口，加入当前激光测量时刻对应的当前机器人状态节点，删除最早加入的一个机器人状态节点，之后通过获取当前机器人状态节点的预测值、机器人状态检测窗口内除当前机器人状态节点外的其余机器人状态节点的初始值以及预设的非线性优化算法，对机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束进行非线性优化，使得每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取机器人状态检测窗口内所有机器人状态节点的优化值，然后当接收到下一个激光测量时刻采集的机器人激光点云时，再重复执行上述步骤。上述机器人状态检测方法解决了现有方法中仅根据机器人上一状态以及传感器采集数据各自预测机器人的当前状态再进行优化，而造成的机器人状态误差累积过快的技术问题，降低了机器人状态检测的误差，提高了机器人位姿估计的准确性。
可选的，所述第一获取单元92包括：
第二获取单元，用于获取所述当前激光测量时刻与上一激光测量时刻之间的惯性数据；
预测单元，用于根据所述惯性数据，获取所述当前机器人状态节点的预测值。
可选的，所述第一获取单元92包括：
第三获取单元，用于获取所述机器人状态检测窗口内每个所述机器人状态节点对应的激光点云；
第四获取单元，用于获取激光地图；其中，所述激光地图由所有已删除的机器人状态节点对应的激光点云拼接而成；
第一运算单元，用于将所述激光地图和所述激光点云变换到目标坐标系下，计算每个所述激光点云与所述激光地图之间的激光点线约束和激光点面约束；其中，所述目标坐标系为所述激光点云对应的机器人状态节点的前一相邻机器人状态节点所对应的坐标系；
激光相对约束建立单元，用于根据所述激光点线约束和激光点面约束，得到每两个相邻所述机器人状态节点之间的激光相对约束。
可选的，第四获取单元包括：
初始激光点云获取单元，用于获取所有已删除的机器人状态节点对应的初始激光点云；
调整单元，用于根据所述已删除的机器人状态节点的优化值，调整对应的所述初始激光点云的姿态和位置，得到调整后的激光点云；
拼接单元，用于拼接所述调整后的激光点云，得到所述激光地图。
可选的，所述第一获取单元92还包括：
去畸变单元，用于对所述激光点云进行点云去畸变。
可选的，所述第一获取单元92还包括：
第五获取单元，用于获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束；其中，所述第一距离约束、第一速度约束和第一旋转四元数约束通过两个相邻激光测量时刻之间采集的惯性数据获取；
惯性预积分约束建立单元，用于根据所述其余机器人状态节点的初始值以及所述第一距离约束、所述第一速度约束和所述第一旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束。
可选的，所述第一获取单元92还包括：
第六获取单元，用于获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束；其中，所述第二距离约束和第二旋转四元数约束通过两个相邻激光测量时刻之间采集的轮计数据获取；
轮计预积分约束建立单元，用于根据所述其余机器人状态节点的初始值以及所述第二距离约束和第二旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束。
请参见图10，为本申请一个实施例提供的机器人状态检测设备的结构示意图。如图10 所示，所述机器人状态检测设备10可以包括：处理器1000、存储器1001以及存储在所述存储器1001并可以在所述处理器1000上运行的计算机程序1002，例如：机器人状态检测程序；所述处理器1000执行所述计算机程序1002时实现上述各方法实施例中的步骤，例如图1所示的步骤S101至S103。或者，所述处理器1000执行所述计算机程序1002时实现上述各装置实施例中各模块/单元的功能，例如图9所示模块91至93的功能。
其中，所述处理器1000可以包括一个或多个处理核心。处理器1000利用各种接口和线路连接所述控制设备100内的各个部分，通过运行或执行存储在存储器1001内的指令、程序、代码集或指令集，以及调用存储器1001内的数据，执行控制设备100的各种功能和处理数据，可选的，处理器1000可以采用数字信号处理(Digital Signal Processing,DSP)、现场可编程门阵列(Field-Programmable Gate Array，FPGA)、可编程逻辑阵列(Programble Logic Array，PLA)中的至少一个硬件形式来实现。处理器1000可集成中央处理器(Central Processing Unit， CPU)、图像处理器(Graphics Processing Unit，GPU)和调制解调器等中的一种或几种的组合。其中，CPU主要处理操作系统、用户界面和应用程序等；GPU用于负责触摸显示屏所需要显示的内容的渲染和绘制；调制解调器用于处理无线通信。可以理解的是，上述调制解调器也可以不集成到处理器1000中，单独通过一块芯片进行实现。
其中，存储器1001可以包括随机存储器(Random Access Memory，RAM)，也可以包括只读存储器(Read-Only Memory)。可选的，该存储器1001包括非瞬时性计算机可读介质(non-transitory computer-readable storage medium)。存储器1001可用于存储指令、程序、代码、代码集或指令集。存储器1001可包括存储程序区和存储数据区，其中，存储程序区可存储用于实现操作系统的指令、用于至少一个功能的指令(比如触控指令等)、用于实现上述各个方法实施例的指令等；存储数据区可存储上面各个方法实施例中涉及到的数据等。存储器 1001可选的还可以是至少一个位于远离前述处理器1000的存储装置。
本申请实施例还提供了一种计算机存储介质，所述计算机存储介质可以存储有多条指令，所述指令适用于由处理器加载并执行上述图1、图4、图5以及图7至图8所示实施例的方法步骤，具体执行过程可以参见图1、图4、图5以及图7至图8所示实施例的具体说明，在此不进行赘述。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，仅以上述各功能单元、模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能单元、模块完成，即将所述装置的内部结构划分成不同的功能单元或模块，以完成以上描述的全部或者部分功能。实施例中的各功能单元、模块可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中，上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。另外，各功能单元、模块的具体名称也只是为了便于相互区分，并不用于限制本申请的保护范围。上述系统中单元、模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述或记载的部分，可以参见其它实施例的相关描述。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的实施例中，应该理解到，所揭露的装置/终端设备和方法，可以通过其它的方式实现。例如，以上所描述的装置/终端设备实施例仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通讯连接可以是通过一些接口，装置或单元的间接耦合或通讯连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的模块/单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明实现上述实施例方法中的全部或部分流程，也可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一计算机可读存储介质中，该计算机程序在被处理器执行时，可实现上述各个方法实施例的步骤。其中，所述计算机程序包括计算机程序代码，所述计算机程序代码可以为源代码形式、对象代码形式、可执行文件或某些中间形式等。
本发明并不局限于上述实施方式，如果对本发明的各种改动或变形不脱离本发明的精神和范围，倘若这些改动和变形属于本发明的权利要求和等同技术范围之内，则本发明也意图包含这些改动和变形。

Claims (9)

1.一种机器人状态检测方法，其特征在于，包括步骤：
接收当前激光测量时刻采集的机器人激光点云，获得当前激光测量时刻对应的当前机器人状态节点的预测值，将所述当前机器人状态节点加入预先创建的机器人状态检测窗口；其中，所述机器人状态检测窗口依次记录有多个激光测量时刻对应的机器人状态节点，且所述机器人状态检测窗口每加入一个新的机器人状态节点时，所述机器人状态检测窗口中最早加入的一个机器人状态节点被删除；
获取所述机器人状态检测窗口内除所述当前机器人状态节点外的其余机器人状态节点的初始值以及所述机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；其中，所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值；获取所述机器人状态检测窗口内每个所述机器人状态节点对应的激光点云；获取激光地图；其中，所述激光地图由所有已删除的机器人状态节点对应的激光点云拼接而成；将所述激光地图和所述激光点云变换到目标坐标系下，计算每个所述激光点云与所述激光地图之间的激光点线约束和激光点面约束；其中，所述目标坐标系为所述激光点云对应的机器人状态节点的前一相邻机器人状态节点所对应的坐标系；根据所述激光点线约束和激光点面约束，得到每两个相邻所述机器人状态节点之间的激光相对约束；
根据所述当前机器人状态节点的预测值、所述其余机器人状态节点的初始值以及预设的非线性优化算法，对所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
2.根据权利要求1所述的机器人状态检测方法，其特征在于，获取所述机器人状态检测窗口内所有机器人状态节点的初始状态，包括步骤：
获取所述当前激光测量时刻与上一激光测量时刻之间的惯性数据；
根据所述惯性数据，获取所述当前机器人状态节点的预测值。
3.根据权利要求1所述的机器人状态检测方法，其特征在于，所述获取激光地图，包括步骤：
获取所有已删除的机器人状态节点对应的初始激光点云；
根据所述已删除的机器人状态节点的优化值，调整对应的所述初始激光点云的姿态和位置，得到调整后的激光点云；
拼接所述调整后的激光点云，得到所述激光地图。
4.根据权利要求1所述的机器人状态检测方法，其特征在于，所述将所述激光地图和所述激光点云变换到目标坐标系下之前，还包括步骤：
对所述激光点云进行点云去畸变。
5.根据权利要求1所述的机器人状态检测方法，其特征在于，获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束，包括步骤：
获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第一距离约束、第一速度约束和第一旋转四元数约束；其中，所述第一距离约束、第一速度约束和第一旋转四元数约束通过两个相邻激光测量时刻之间采集的惯性数据获取；
根据所述其余机器人状态节点的初始值以及所述第一距离约束、所述第一速度约束和所述第一旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的惯性预积分约束。
6.根据权利要求1所述的机器人状态检测方法，其特征在于，获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束，包括步骤：
获取所述机器人状态检测窗口内每两个相邻机器人状态节点之间的第二距离约束和第二旋转四元数约束；其中，所述第二距离约束和第二旋转四元数约束通过两个相邻激光测量时刻之间采集的轮计数据获取；
根据所述其余机器人状态节点的初始值以及所述第二距离约束和第二旋转四元数约束，建立所述机器人状态检测窗口内每两个相邻机器人状态节点之间的轮计预积分约束。
7.一种机器人状态检测装置，其特征在于，包括：
窗口滑动单元，用于接收当前激光测量时刻采集的机器人激光点云，获得当前激光测量时刻对应的当前机器人状态节点的预测值，将所述当前机器人状态节点加入预先创建的机器人状态检测窗口；其中，所述机器人状态检测窗口依次记录有多个激光测量时刻对应的机器人状态节点，且所述机器人状态检测窗口每加入一个新的机器人状态节点时，所述机器人状态检测窗口中最早加入的一个机器人状态节点被删除；
第一获取单元，用于获取所述机器人状态检测窗口内除所述当前机器人状态节点外的其余机器人状态节点的初始值以及所述机器人状态检测窗口内每两个相邻机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束；其中，所述其余机器人状态节点的初始值为未加入当前机器人状态节点前所述机器人状态检测窗口内机器人状态节点的优化值；获取所述机器人状态检测窗口内每个所述机器人状态节点对应的激光点云；获取激光地图；其中，所述激光地图由所有已删除的机器人状态节点对应的激光点云拼接而成；将所述激光地图和所述激光点云变换到目标坐标系下，计算每个所述激光点云与所述激光地图之间的激光点线约束和激光点面约束；其中，所述目标坐标系为所述激光点云对应的机器人状态节点的前一相邻机器人状态节点所对应的坐标系；根据所述激光点线约束和激光点面约束，得到每两个相邻所述机器人状态节点之间的激光相对约束；
检测单元，用于根据所述当前机器人状态节点的预测值、所述其余机器人状态节点的初始值以及预设的非线性优化算法，对所述激光相对约束、所述惯性预积分约束以及所述轮计预积分约束进行非线性优化，使所述机器人状态检测窗口内每两个相邻所述机器人状态节点之间的激光相对约束、惯性预积分约束和轮计预积分约束之和最小，获取所述机器人状态检测窗口内所有机器人状态节点的优化值。
8.一种机器人状态检测设备，其特征在于，包括：处理器、存储器以及存储在所述存储器中并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至6任一项所述方法的步骤。
9.一种计算机可读存储介质，所述计算机可读存储介质存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至6任一项所述方法的步骤。
CN202011042717.4A 2020-09-28 2020-09-28 机器人状态检测方法、装置及设备 Active CN112269187B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011042717.4A CN112269187B (zh) 2020-09-28 2020-09-28 机器人状态检测方法、装置及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011042717.4A CN112269187B (zh) 2020-09-28 2020-09-28 机器人状态检测方法、装置及设备

Publications (2)

Publication Number Publication Date
CN112269187A CN112269187A (zh) 2021-01-26
CN112269187B true CN112269187B (zh) 2024-05-14

Family

ID=74349344

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011042717.4A Active CN112269187B (zh) 2020-09-28 2020-09-28 机器人状态检测方法、装置及设备

Country Status (1)

Country Link
CN (1) CN112269187B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115560744A (zh) * 2022-08-22 2023-01-03 深圳市普渡科技有限公司 机器人以及基于多传感器的三维建图方法、存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107516326A (zh) * 2017-07-14 2017-12-26 中国科学院计算技术研究所 融合单目视觉和编码器信息的机器人定位方法和系统
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
WO2019195270A1 (en) * 2018-04-03 2019-10-10 Kaarta, Inc. Methods and systems for real or near real-time point cloud map data confidence evaluation
CN110954100A (zh) * 2019-12-30 2020-04-03 广东省智能制造研究所 一种基于激光与惯导融合的足式机器人本体状态估计方法
CN111337947A (zh) * 2020-05-18 2020-06-26 深圳市智绘科技有限公司 即时建图与定位方法、装置、系统及存储介质
CN111402339A (zh) * 2020-06-01 2020-07-10 深圳市智绘科技有限公司 一种实时定位方法、装置、系统及存储介质
CN111427061A (zh) * 2020-06-15 2020-07-17 北京云迹科技有限公司 一种机器人建图方法、装置，机器人及存储介质
CN111552757A (zh) * 2020-04-30 2020-08-18 上海商汤临港智能科技有限公司 生成电子地图的方法、装置、设备及存储介质

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107516326A (zh) * 2017-07-14 2017-12-26 中国科学院计算技术研究所 融合单目视觉和编码器信息的机器人定位方法和系统
WO2019195270A1 (en) * 2018-04-03 2019-10-10 Kaarta, Inc. Methods and systems for real or near real-time point cloud map data confidence evaluation
CN109945856A (zh) * 2019-02-18 2019-06-28 天津大学 基于惯性/雷达的无人机自主定位与建图方法
CN110954100A (zh) * 2019-12-30 2020-04-03 广东省智能制造研究所 一种基于激光与惯导融合的足式机器人本体状态估计方法
CN111552757A (zh) * 2020-04-30 2020-08-18 上海商汤临港智能科技有限公司 生成电子地图的方法、装置、设备及存储介质
CN111337947A (zh) * 2020-05-18 2020-06-26 深圳市智绘科技有限公司 即时建图与定位方法、装置、系统及存储介质
CN111402339A (zh) * 2020-06-01 2020-07-10 深圳市智绘科技有限公司 一种实时定位方法、装置、系统及存储介质
CN111427061A (zh) * 2020-06-15 2020-07-17 北京云迹科技有限公司 一种机器人建图方法、装置，机器人及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Scara机器人关节摩擦建模与补偿的实验与仿真;李琳 等;《系统仿真学报》;第31卷(第8期);第1572-1581页 *
基于多传感器融合的室内SLAM算法研究与实现;纪嘉文;《中国优秀硕士学位论文全文数据库 信息科技辑》(第4期);第19-28页 *

Also Published As

Publication number Publication date
CN112269187A (zh) 2021-01-26

Similar Documents

Publication Publication Date Title
US10852139B2 (en) Positioning method, positioning device, and robot
CN107748569B (zh) 用于无人机的运动控制方法、装置及无人机系统
JP5987823B2 (ja) 画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム
CN110197615B (zh) 用于生成地图的方法及装置
CN106814753B (zh) 一种目标位置矫正方法、装置及系统
CN110956665B (zh) 车辆拐弯轨迹双向计算方法、系统、装置
CN110470333B (zh) 传感器参数的标定方法及装置、存储介质和电子装置
CN108684203A (zh) 使用基于学习的模型预测控制来确定自动驾驶车辆的道路摩擦的方法和系统
CN113034594A (zh) 位姿优化方法、装置、电子设备及存储介质
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN110068326B (zh) 姿态计算方法、装置、电子设备以及存储介质
CN113960622A (zh) 融合激光雷达及imu传感器信息的实时定位方法及装置
CN114018274A (zh) 车辆定位方法、装置及电子设备
CN114323033A (zh) 基于车道线和特征点的定位方法、设备及自动驾驶车辆
CN110770664A (zh) 一种导航路径跟踪控制方法、设备、移动机器人及系统
CN112269187B (zh) 机器人状态检测方法、装置及设备
CN109903367B (zh) 构建地图的方法、装置和计算机可读存储介质
CN113759347B (zh) 坐标关系的标定方法、装置、设备及介质
CN106535132A (zh) 用于定位终端的方法和装置
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN114147717B (zh) 机器人运动轨迹估计方法、装置、控制器及存储介质
CN115560744A (zh) 机器人以及基于多传感器的三维建图方法、存储介质
CN115900697A (zh) 对象运动轨迹信息处理方法、电子设备以及自动驾驶车辆
CN109074407A (zh) 多源数据建图方法、相关装置及计算机可读存储介质
CN113946151A (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