CN115436955A - 室内外环境定位方法 - Google Patents

室内外环境定位方法 Download PDF

Info

Publication number
CN115436955A
CN115436955A CN202210934071.3A CN202210934071A CN115436955A CN 115436955 A CN115436955 A CN 115436955A CN 202210934071 A CN202210934071 A CN 202210934071A CN 115436955 A CN115436955 A CN 115436955A
Authority
CN
China
Prior art keywords
positioning
map
indoor
outdoor
robot
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.)
Withdrawn
Application number
CN202210934071.3A
Other languages
English (en)
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.)
Suzhou Hezhong Intelligent Technology Co ltd
Original Assignee
Suzhou Hezhong Intelligent Technology 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 Suzhou Hezhong Intelligent Technology Co ltd filed Critical Suzhou Hezhong Intelligent Technology Co ltd
Priority to CN202210934071.3A priority Critical patent/CN115436955A/zh
Publication of CN115436955A publication Critical patent/CN115436955A/zh
Withdrawn legal-status Critical Current

Links

Images

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/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
    • G01P—MEASURING LINEAR OR ANGULAR SPEED, ACCELERATION, DECELERATION, OR SHOCK; INDICATING PRESENCE, ABSENCE, OR DIRECTION, OF MOVEMENT
    • G01P15/00—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration
    • G01P15/02—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses
    • G01P15/08—Measuring acceleration; Measuring deceleration; Measuring shock, i.e. sudden change of acceleration by making use of inertia forces using solid seismic masses with conversion into electric or magnetic values
    • 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/42—Devices characterised by the use of electric or magnetic means
    • 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
    • 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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/10—Segmentation; Edge detection
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T2207/00—Indexing scheme for image analysis or image enhancement
    • G06T2207/20—Special algorithmic details
    • G06T2207/20112—Image segmentation details
    • G06T2207/20128—Atlas-based segmentation

Landscapes

  • Physics & Mathematics (AREA)
  • Engineering & Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Theoretical Computer Science (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种室内外环境定位方法，包括：S10、将室外地图切割成多个局部地图，在室内地图和室外地图之间设置切换点，机器人通过所述切换点进行室内外切换，以实现在室内定位时加载室内地图，在室外定位时实时加载机器人所在的局部地图；S20、使用误差状态卡尔曼滤波器融合安装于机器人上的惯性测量单元、轮式里程计和激光定位系统，对机器人进行定位。本发明可同时应用于室内和室外。根据机器人所在位置实时加载局部地图，提高了计算效率，降低了系统计算负载。本发明使用了误差状态卡尔曼滤波器进行融合定位，预测部分使用惯性测量单元，观测部分使用激光配准的位姿数据和轮式里程计的速度，提升了定位的精确性和鲁棒性。

Description

室内外环境定位方法
技术领域
本发明涉及机器人导航技术领域，特别涉及一种室内外环境定位方法。
背景技术
目前的自主导航机器人按应用场景主要可以分为室外机器人和室内机器人两种，室外机器人根据运动速度的不同，可以分为中低速机器人(<10m/s)和高速机器人。中低速机器人主要用于园区配送，安防巡检，草坪割草，而高速机器人主要面向自动驾驶行业。室内机器人又可以称做自动导引车(AGV)，主要用于工业，工厂中有大量物料配送的需求，而AGV可以在工厂环境完美的达到高效率，低成本的物流配送。
目前的机器人只能单独用于室外或者室内，无法同时应用于室内和室外，而且，目前的室外激光定位方案存在着大场景地图过大，计算效率低，难以实时定位和定位鲁棒性不够的问题。
发明内容
本发明要解决的技术问题是提供一种定位精度高、鲁棒性好的室内外环境定位方法。
为了解决上述问题，本发明提供了一种室内外环境定位方法，其包括以下步骤：
S10、将室外地图切割成多个局部地图，在室内地图和室外地图之间设置切换点，机器人通过所述切换点进行室内外切换，以实现在室内定位时加载室内地图，在室外定位时实时加载机器人所在的局部地图；
S20、使用误差状态卡尔曼滤波器融合安装于机器人上的惯性测量单元、轮式里程计和激光定位系统，对机器人进行定位。
作为本发明的进一步改进，在步骤S10中，将室外地图切割成若干局部地图，包括：
S111、计算室外地图的x坐标的最大值和最小值，以及y坐标的最大值和最小值；
S112、设置切换步长、激光测量范围和局部地图边长；
S113、遍历室外地图上的点，若该点的x坐标和y坐标都能够整除所述切换步长，则将该点作为局部地图的中心点，并将室外地图切割成若干局部地图。
作为本发明的进一步改进，所述切换步长、激光测量范围和局部地图边长满足：
size≥step+2*m
其中，size为局部地图边长；step为切换步长；m为激光测量范围。
作为本发明的进一步改进，在室内定位时使用2D激光定位系统，在室外定位时使用3D激光定位系统；在步骤S10中，所述机器人通过所述切换点进行室内外切换，包括：
当从室内到达所述切换点时，将2D激光定位系统的当前定位数据传输至3D激光定位系统，对3D激光定位系统进行初始化定位；
当从室外到达所述切换点时，将3D激光定位系统的当前定位数据传输至2D激光定位系统，对2D激光定位系统进行初始化定位。
作为本发明的进一步改进，通过以下步骤判断初始化定位时是否成功，步骤包括：
计算当前激光点云根据初始化定位位姿变换之后的点云中每一个点和地图点云之间的最短欧氏距离的平均值，然后将平均值通过指数函数得到定位的分数，如果分数大于阈值，则初始化定位成功，否则不成功。
作为本发明的进一步改进，步骤S20包括：
S21、状态初始化步骤；包括：进行位姿初始化，并进行误差状态卡尔曼滤波器初始化；
S22、模型预测步骤；包括：利用惯性测量单元测量的角速度和线性加速度积分来预测位姿，然后更新误差状态的雅可比矩阵和协方差矩阵；
S23、观测校正步骤；包括：先获取轮式里程计的速度观测值，并且根据NDT算法配准当前点云数据和地图得到位姿，然后将观测量传递到误差状态卡尔曼滤波器，计算误差状态卡尔曼增益，然后计算误差状态值和协方差矩阵，然后将误差状态值加到名义状态值中更新名义状态值，然后重置误差状态值。
作为本发明的进一步改进，S22模型预测步骤中，采用如下公式：
Figure BDA0003782788860000031
Figure BDA0003782788860000032
Figure BDA0003782788860000033
式中，p为机器人位置；r为机器人方向的旋转矩阵；dt为时间间隔；v代表机器人速度；am代表线加速度测量值；
Figure BDA0003782788860000034
代表线加速度偏置；g代表重力加速度；q代表旋转方向四元数；ωm代表角速度测量值；ε代表角速度偏置；下标k、k-1分别代表k、k-1时刻；上标T代表转置；
接着根据系统微分方程：
Figure BDA0003782788860000035
计算系统状态方程：Xk-1＝Fk-1Xk-1+Bk-1Uk-1关于误差状态的雅可比矩阵Fk-1；
其中，
Figure BDA0003782788860000037
为系统状态量的导数；Ft为系统微分方程状态雅可比矩阵；E为系统微分方程扰动量；Bt为系统微分方程扰动雅可比矩阵；Fk-1为系统状态方程状态雅可比矩阵；U为系统状态方程扰动量，Bk-1为系统状态方程扰动雅可比矩阵；之后系统状态方程状态使用雅可比矩阵Fk-1更新误差状态的协方差矩阵，公式如下：
Figure BDA0003782788860000036
其中，P是误差状态的协方差矩阵。
作为本发明的进一步改进，S23观测校正步骤中，采用如下公式：
Yk＝GkXk-1+CkWk
其中，Y是误差观测量，包括位置误差、角度误差和速度误差、X为误差状态量、W为观测扰动量、G和C分别是误差观测量关于误差状态和观测扰动的雅可比矩阵；
由卡尔曼滤波可知，矫正方程为：
Figure BDA0003782788860000041
Pk＝(I-KkGk)Pk
Xk＝Xk+Kk(Yk-GkXk)
其中，P是误差状态的协方差矩阵；R是观测扰动的协方差矩阵；I是单位矩阵；K为卡尔曼增益；下标k、k-1分别代表k、k-1时刻；上标T代表转置。
本发明还提供了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述任意一项所述方法的步骤。
本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述任意一项所述方法的步骤。
本发明的有益效果：
一、本发明室内外环境定位方法通过在室内地图和室外地图之间设置切换点，机器人通过切换点进行室内外切换，实现在室内定位时加载室内地图，在室外定位时实时加载机器人所在的局部地图，保证机器人可同时应用于室内和室外。
二、本发明针对室外大场景环境，机器人面临的激光点云地图占用内存大，计算效率低的问题，通过预先将室外地图分割成多个较小的局部地图，根据机器人所在位置实时加载局部地图，提高了计算效率，降低了系统计算负载。
三、本发明使用了误差状态卡尔曼滤波器进行融合定位，预测部分使用惯性测量单元，观测部分使用激光配准的位姿数据和轮式里程计的速度，提升了定位的精确性和鲁棒性。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明的技术手段，而可依照说明书的内容予以实施，并且为了让本发明的上述和其他目的、特征和优点能够更明显易懂，以下特举较佳实施例，并配合附图，详细说明如下。
附图说明
图1是本发明优选实施例中室内外环境定位方法的流程图；
图2是本发明优选实施例中室外地图切割的流程图；
图3是本发明优选实施例中实时加载局部地图的流程图；
图4是本发明优选实施例中2D区域切换到3D区域的流程图；
图5是本发明优选实施例中3D区域切换到2D区域的流程图；
图6是本发明优选实施例中室内外切换的示意图；
图7是本发明优选实施例中误差状态卡尔曼滤波器的流程图。
具体实施方式
下面结合附图和具体实施例对本发明作进一步说明，以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。
如图1所示，为本发明优选实施例中室内外环境定位方法，包括以下步骤：
步骤S10、将室外地图切割成多个局部地图，在室内地图和室外地图之间设置切换点，机器人通过所述切换点进行室内外切换，以实现在室内定位时加载室内地图，在室外定位时实时加载机器人所在的局部地图；
由于室外地图为大场景地图，占用的内存较大，以及激光点云和地图配准时的占用的cpu也会因此增加。因此，本发明将室外的大场景地图分割为多个具有一定重合区域的局部地图。
在已知需要切分出来的局部地图边界时，可以将大场景地图在局部地图边界以外的点云数据全部删除，从而得到的即为局部地图。边界的形式多种多样，可选地，本发明采用的立方体边界。
分割后所有的局部地图必须包含大场景地图的所有区域，将大场景地图分割为若干个立方体局部地图。因为假设机器人从1号局部地图切换到2号局部地图时，在1号局部地图的该切换位置必须有激光雷达测量范围的视野，换而言之，切换位置距离地图边界至少有激光雷达测量范围的距离，并且切换位置距离1号和2号地图边界均至少有激光雷达测量范围的距离。因此相邻的局部地图在该方向上有两个激光测量距离的重合区域。
参照图2，在其中一实施例中，在步骤S10中，将室外地图切割成若干局部地图，包括以下步骤：
S111、计算室外地图的x坐标的最大值和最小值，以及y坐标的最大值和最小值；可分别表示为max_x、min_x、max_y、min_y。
S112、设置切换步长、激光测量范围和局部地图边长；其中，所述切换步长、激光测量范围和局部地图边长满足：
size≥step+2*m
其中，局部地图xy平面为正方形，size为局部地图边长；step为切换步长；m为激光测量范围。
S113、遍历室外地图上的点，若该点的x坐标和y坐标都能够整除所述切换步长，则将该点作为局部地图的中心点，并将室外地图切割成若干局部地图。
具体地，若该点的x坐标和y坐标能够整除步长step，则将该点作为局部地图的中心点，其坐标为(x_center,y_center)。该局部地图xy平面的边界x轴最大值为中心点x坐标加上二分之一的局部地图大小(x_center+0.5*size)，x轴最小值为中心点x坐标减去二分之一的局部地图大小(x_center-0.5*size)，y轴最大值为中心点y坐标加上二分之一的局部地图大小(y_center+0.5*size)，y轴最小值为中心点y坐标减去二分之一的局部地图大小(x_center-0.5*size)。z轴不做切割，全部保留。以该局部边界切割大场景地图，保存生成的地图命名为中心点x坐标_中心点y坐标。
参照图3，在其中一实施例中，在步骤S10中，在室外定位时实时加载机器人所在的局部地图，包括以下步骤：
S121、设置切换步长step；
S122、获取当前机器人位置x,y,z；
S123、计算当前所在位置所在的局部地图。当前位置x坐标除以步长与0.5的和向下取整再乘以步长，得到的结果是中心点x坐标，当前位置y坐标除以步长与0.5的和向下取整再乘以步长，得到的结果是中心点y坐标，如果中心点x，y坐标有任何一个改变了说明局部地图已经改变，此时加载改变以后的局部地图，如果没有变化，则不做修改。处理完成后，一个周期结束，返回上一步。
参照图6，在其中一实施例中，在室内(2D区域)定位时使用2D激光定位系统，在室外(3D区域)定位时使用3D激光定位系统；在步骤S10中，所述机器人通过所述切换点进行室内外切换，包括：
当从室内到达所述切换点时，将2D激光定位系统的当前定位数据传输至3D激光定位系统，对3D激光定位系统进行初始化定位；参照图4。
当从室外到达所述切换点时，将3D激光定位系统的当前定位数据传输至2D激光定位系统，对2D激光定位系统进行初始化定位；参照图5。
具体地，通过以下步骤判断初始化定位时是否成功，步骤包括：
计算当前激光点云根据初始化定位位姿变换之后的点云中每一个点和地图点云之间的最短欧氏距离的平均值，然后将平均值通过指数函数得到定位的分数，分数体现了点云和地图之间配准的准确性，如果分数大于阈值，则初始化定位成功，否则不成功。
步骤S20、使用误差状态卡尔曼滤波器融合安装于机器人上的惯性测量单元、轮式里程计和激光定位系统，对机器人进行定位。
其中，机器人上安装有多种类型的测量传感器，包括激光雷达，惯性测量元件、轮式里程计。激光雷达可以扫描出周围环境的点云数据，在和预先建立的环境地图配准以后，可以得到机器人在环境中的位置，激光雷达包括2D激光雷达和3D激光雷达，2D激光雷达构成2D激光定位系统，3D激光雷达构成3D激光定位系统；惯性测量单元可以测量车体的三轴角速度和三轴线性加速度，这些数据积分处理后可以得到一段时间内机器人的位置变化量、速度变化量以及角速度变化量；轮式里程计可以测量机器人当前的运动速度。使用这些传感器融合可以得到定位，融合的方式主要有滤波的方法和优化的方法，滤波的方法计算量更小，为了达到实时性的目的，采用了滤波的框架。滤波的问题可以简单理解为一种预测和观测融合的结果，在本发明中，是惯性测量单元预测位姿速度变化，激光点云配准观测位姿，轮式里程计观测速度的结果。
误差状态卡尔曼滤波中有三种状态值：名义状态，误差状态和真实状态。真实状态由名义状态和误差状态组合而成(线性和，四元数乘积或者矩阵乘积)。高频率的惯性测量单元数据积分得到名义状态。在该名义状态中，没有包含噪声以及其他的模型缺陷。误差状态将会收集这些缺陷，并使用误差状态卡尔曼滤波进行估计，在观测数据到达后完成校正。
参照图7，在其中一实施例中，步骤S20包括：
S21、状态初始化步骤；包括：进行位姿初始化，并进行误差状态卡尔曼滤波器初始化；其中包含了预测噪声、观测噪声的设置。
S22、模型预测步骤；包括：利用惯性测量单元测量的角速度和线性加速度积分来预测位姿，然后更新误差状态的雅可比矩阵和协方差矩阵；
具体地，首先使用惯性测量单元测量的三轴角速度和三轴线加速度预测机器人位姿。当前时刻位置等于上一时刻位置，上一时刻速度乘以时间变化量，当前机器人方向的旋转矩阵与线加速度测量值减去线加速度偏置的结果的乘积与重力加速度的和再乘以时间变化量的平方除以2的值的和。当前速度等于上一时刻速度，当前机器人方向的旋转矩阵与线加速度测量值减去线加速度偏置的结果的乘积与重力加速度的和再乘以时间变化量的值的和。当前旋转方向四元数等于上一时刻旋转方向四元数和角速度减去角速度偏置的差值和时间变化量的乘积转化的四元数的乘积。采用如下公式：
Figure BDA0003782788860000081
Figure BDA0003782788860000082
Figure BDA0003782788860000083
式中，p为机器人位置；r为机器人方向的旋转矩阵；dt为时间间隔；v代表机器人速度；am代表线加速度测量值；
Figure BDA0003782788860000084
代表线加速度偏置；g代表重力加速度；q代表旋转方向四元数；ωm代表角速度测量值；ε代表角速度偏置；下标k、k-1分别代表k、k-1时刻；上标T代表转置；
接着根据系统微分方程：
Figure BDA0003782788860000085
计算系统状态方程：Xk-1＝Fk-1Xk-1+Bk-1Uk-1关于误差状态的雅可比矩阵Fk-1；
其中，
Figure BDA0003782788860000086
为系统状态量的导数；Ft为系统微分方程状态雅可比矩阵；E为系统微分方程扰动量；Bt为系统微分方程扰动雅可比矩阵；Fk-1为系统状态方程状态雅可比矩阵；U为系统状态方程扰动量，Bk-1为系统状态方程扰动雅可比矩阵；之后系统状态方程状态使用雅可比矩阵Fk-1更新误差状态的协方差矩阵，公式如下：
Figure BDA0003782788860000087
其中，P是误差状态的协方差矩阵。
S23、观测校正步骤；包括：先获取轮式里程计的速度观测值，并且根据NDT算法配准当前点云数据和地图得到位姿，然后将观测量传递到误差状态卡尔曼滤波器，计算误差状态卡尔曼增益，然后计算误差状态值和协方差矩阵，然后将误差状态值加到名义状态值中更新名义状态值，然后重置误差状态值。
具体地，首先需要通过传感器获取观测数据，在本发明中有两种传感器作为观测，分别是轮式里程计和激光雷达，轮式里程计可以得到机器人本体坐标系下的运动速度，激光雷达得到的激光点云数据和地图配准可以得到机器人的全局位姿，配准的算法有多种可以选择，常用的有NDT，GICP，FAST_GICP等，激光点云的配准算法通常需要设置目标点云，当前点云和初始估计位姿值。目标点云为地图点云，当前点云为激光雷达实时获取到的点云，而初始估计位姿值对配准具有重要的影响，更准确的初始估计位姿值可以避免配准算法进入局部最小值的计算，提高算法鲁棒性和效率，初始估计位姿值使用预测得到的位姿可以有较好的准确性。然后通过得到的速度和位姿计算误差状态测量，对于观测方程：
Yk＝GkXk-1+CkWk
其中，Y是误差观测量，包括位置误差、角度误差和速度误差、X为误差状态量、W为观测扰动量、G和C分别是误差观测量关于误差状态和观测扰动的雅可比矩阵；
由卡尔曼滤波可知，矫正方程为：
Figure BDA0003782788860000091
Pk＝(I-KkGk)Pk
Xk＝Xk+Kk(Yk-GkXk)
其中，P是误差状态的协方差矩阵；R是观测扰动的协方差矩阵；I是单位矩阵；K为卡尔曼增益；下标k、k-1分别代表k、k-1时刻；上标T代表转置。
其中，观测和预测有序地进行，预测传感器数据到达进行预测步骤，观测传感器数据到达执行观测步骤，不断地更新系统位姿数据，观测校正后的数据会发送给机器人进行定位。
在一具体实施例中，采用本发明的室内外环境定位方法对机器人进行定位，包括以下步骤：
一、地图分割步骤：
1、假设计算出大场景地图的x,y轴的最大值和最小值max_x,max_y,min_x,min_y分别为1200，700，0，300；
2、设置切换步长step为500，激光测量范围m为100和局部地图大小size为700(size>＝(step+2*m))；
3、遍历x坐标0到1200，遍历y坐标300到700，x和y坐标能够整除步长step(500)的点有3个，分别为(0，500)、(500，500)和(1000，500)。(0，500)为中心点的局部地图xy平面的四个边界点分别为(-350，150)、(-350，850)、(350，150)、(350，850)，生成的局部地图命名为0_500。(500，500)为中心点的局部地图xy平面的四个边界点分别为(150，150)、(150，850)、(850，150)、(850，850)，生成的局部地图命名为500_500。(1000，500)为中心点的局部地图xy平面的四个边界点分别为(650，150)、(650，850)、(1350，150)、(1350，850)，生成的局部地图命名为1000_500。
二、实时加载局部地图步骤：
1、设置切换步长step为500，step和地图分割时设置必须一致；
2、获取当前机器人位置x、y、z，假设分别为248、600、0；
3、计算中心点x坐标Floor(248/500+0.5)*500＝0；计算中心点y坐标Floor(600/500+0.5)*500＝500；
4、得到中心点x、y坐标为(0，500)，加载0_500地图，x坐标248+100<350(局部地图x轴最大值),激光雷达最大视野仍然在地图范围内；
5、获取当前机器人位置x、y、z，假设分别为249、600、0；
6、计算中心点x坐标Floor(249/500+0.5)*500＝0；计算中心点y坐标Floor(600/500+0.5)*500＝500；
7、得到中心点x、y坐标为(0，500)，地图没有改变，不加载地图；
8、获取当前机器人位置x,y,z，假设分别为251，600，0；
9、计算中心点x坐标Floor(251/500+0.5)*500＝500；计算中心点y坐标Floor(600/500+0.5)*500＝500；
10、得到中心点x、y坐标为(500，500)，加载500_500地图，x坐标350(0_500局部地图x轴最大值)<251+100<850(500_500局部地图x轴最大值)、150(500_500局部地图x轴最小值)<251-100，激光雷达最大视野不在0_500局部地图，在500_500局部地图范围内，地图切换成功。
三、融合惯性测量单元和轮式里程计和激光雷达的误差状态卡尔曼滤波定位步骤：
1、系统状态量X为
Figure BDA0003782788860000111
δP为位置误差，δV为速度误差，φ为失准角，ε为角速度偏置，
Figure BDA0003782788860000112
为线加速度偏置；
2、获取前一时刻系统名义状态值，位置pk-1，姿态qk-1和速度vk-1；
3、根据惯性测量单元数据和时间预测当前时刻系统位置pk，姿态qk和速度vk，下标k代表k时刻；
4、位置预测：
Figure BDA0003782788860000118
姿态预测：
Figure BDA0003782788860000113
速度预测：
Figure BDA0003782788860000114
5、系统微分方程：
Figure BDA0003782788860000115
6、计算系统微分方程状态雅可比矩阵
Figure BDA0003782788860000116
其中，
Figure BDA0003782788860000119
是base_link相对于n系(静止的导航坐标系)的变换的旋转部分；fn是加速度(n系)；fn×是fn的反对称矩阵；I3×3代表3乘3的单位矩阵；I代表单位矩阵；下标3x3代表3乘3，其余下标同理；
7、计算系统微分方程扰动雅可比矩阵
Figure BDA0003782788860000117
8、系统状态方程：Xk-1＝Fk-1Xk-1+Bk-1Uk-1；
9、计算Fk-1＝I+Ft×dt，Bk-1＝Bt×dt；
10、更新误差状态协方差矩阵：
Figure BDA0003782788860000121
11、获取激光雷达点云数据，设置点云，以预测的系统位姿作为初始值进行点云配准，得到配准后的位姿，作为观测值，获取轮式里程计速度值；
12、系统观测方程Yk＝GkXk-1+CkWk；系统观测量Y为
Figure BDA0003782788860000122
13、计算系统误差状态观测量，系统预测位置，姿态和速度和相应传感器计算得到的结果之间的差值，得到观测量Y；
14、计算系统观测方程关于误差状态的雅可比矩阵：
Figure BDA0003782788860000123
其中，
Figure BDA0003782788860000124
是n系相对于base_link的变换的旋转部分；Vn是速度(n系)；Vn×是Vn的反对称矩阵；系统观测方程关于噪声的雅可比矩阵Ck为单位矩阵。
15、计算卡尔曼增益
Figure BDA0003782788860000125
16、计算校正后协方差矩阵Pk＝(I-KkGk)Pk；
17、计算校正后误差状态值Xk＝Xk+Kk(Yk-GkXk)；
18、使用校正后的误差状态值更新名义状态值；
19、重置系统误差状态值。
本发明室内外环境定位方法通过在室内地图和室外地图之间设置切换点，机器人通过切换点进行室内外切换，实现在室内定位时加载室内地图，在室外定位时实时加载机器人所在的局部地图，保证机器人可同时应用于室内和室外。
本发明针对室外大场景环境，机器人面临的激光点云地图占用内存大，计算效率低的问题，通过预先将室外地图分割成多个较小的局部地图，根据机器人所在位置实时加载局部地图，提高了计算效率，降低了系统计算负载。
本发明使用了误差状态卡尔曼滤波器进行融合定位，预测部分使用惯性测量单元，观测部分使用激光配准的位姿数据和轮式里程计的速度，提升了定位的精确性和鲁棒性。
本发明优选实施例还公开了一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现上述实施例中所述方法的步骤。
本发明优选实施例还公开了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时实现上述实施例中所述方法的步骤。
以上实施例仅是为充分说明本发明而所举的较佳的实施例，本发明的保护范围不限于此。本技术领域的技术人员在本发明基础上所作的等同替代或变换，均在本发明的保护范围之内。本发明的保护范围以权利要求书为准。

Claims (10)

1.室内外环境定位方法，其特征在于，包括：
S10、将室外地图切割成多个局部地图，在室内地图和室外地图之间设置切换点，机器人通过所述切换点进行室内外切换，以实现在室内定位时加载室内地图，在室外定位时实时加载机器人所在的局部地图；
S20、使用误差状态卡尔曼滤波器融合安装于机器人上的惯性测量单元、轮式里程计和激光定位系统，对机器人进行定位。
2.如权利要求1所述的室内外环境定位方法，其特征在于，在步骤S10中，将室外地图切割成若干局部地图，包括：
S111、计算室外地图的x坐标的最大值和最小值，以及y坐标的最大值和最小值；
S112、设置切换步长、激光测量范围和局部地图边长；
S113、遍历室外地图上的点，若该点的x坐标和y坐标都能够整除所述切换步长，则将该点作为局部地图的中心点，并将室外地图切割成若干局部地图。
3.如权利要求2所述的室内外环境定位方法，其特征在于，所述切换步长、激光测量范围和局部地图边长满足：
size≥step+2*m
其中，size为局部地图边长；step为切换步长；m为激光测量范围。
4.如权利要求1所述的室内外环境定位方法，其特征在于，在室内定位时使用2D激光定位系统，在室外定位时使用3D激光定位系统；在步骤S10中，所述机器人通过所述切换点进行室内外切换，包括：
当从室内到达所述切换点时，将2D激光定位系统的当前定位数据传输至3D激光定位系统，对3D激光定位系统进行初始化定位；
当从室外到达所述切换点时，将3D激光定位系统的当前定位数据传输至2D激光定位系统，对2D激光定位系统进行初始化定位。
5.如权利要求4所述的室内外环境定位方法，其特征在于，通过以下步骤判断初始化定位时是否成功，步骤包括：
计算当前激光点云根据初始化定位位姿变换之后的点云中每一个点和地图点云之间的最短欧氏距离的平均值，然后将平均值通过指数函数得到定位的分数，如果分数大于阈值，则初始化定位成功，否则不成功。
6.如权利要求1所述的室内外环境定位方法，其特征在于，步骤S20包括：
S21、状态初始化步骤；包括：进行位姿初始化，并进行误差状态卡尔曼滤波器初始化；
S22、模型预测步骤；包括：利用惯性测量单元测量的角速度和线性加速度积分来预测位姿，然后更新误差状态的雅可比矩阵和协方差矩阵；
S23、观测校正步骤；包括：先获取轮式里程计的速度观测值，并且根据NDT算法配准当前点云数据和地图得到位姿，然后将观测量传递到误差状态卡尔曼滤波器，计算误差状态卡尔曼增益，然后计算误差状态值和协方差矩阵，然后将误差状态值加到名义状态值中更新名义状态值，然后重置误差状态值。
7.如权利要求6所述的室内外环境定位方法，其特征在于，S22模型预测步骤中，采用如下公式：
Figure FDA0003782788850000021
Figure FDA0003782788850000022
Figure FDA0003782788850000023
式中，p为机器人位置；r为机器人方向的旋转矩阵；dt为时间间隔；v代表机器人速度；am代表线加速度测量值；
Figure FDA0003782788850000024
代表线加速度偏置；g代表重力加速度；q代表旋转方向四元数；ωm代表角速度测量值；ε代表角速度偏置；下标k、k-1分别代表k、k-1时刻；上标T代表转置；
接着根据系统微分方程：
Figure FDA0003782788850000025
计算系统状态方程：Xk-1＝Fk-1Xk-1+Bk-1Uk-1关于误差状态的雅可比矩阵Fk-1；
其中，
Figure FDA0003782788850000026
为系统状态量的导数；Ft为系统微分方程状态雅可比矩阵；E为系统微分方程扰动量；Bt为系统微分方程扰动雅可比矩阵；Fk-1为系统状态方程状态雅可比矩阵；U为系统状态方程扰动量，Bk-1为系统状态方程扰动雅可比矩阵；之后系统状态方程状态使用雅可比矩阵Fk-1更新误差状态的协方差矩阵，公式如下：
Figure FDA0003782788850000031
其中，P是误差状态的协方差矩阵。
8.如权利要求6所述的室内外环境定位方法，其特征在于，S23观测校正步骤中，采用如下公式：
Yk＝GkXk-1+CkWk
其中，Y是误差观测量，包括位置误差、角度误差和速度误差、X为误差状态量、W为观测扰动量、G和C分别是误差观测量关于误差状态和观测扰动的雅可比矩阵；
由卡尔曼滤波可知，矫正方程为：
Figure FDA0003782788850000032
Pk＝(I-KkGk)Pk
Xk＝Xk+Kk(Yk-GkXk)
其中，P是误差状态的协方差矩阵；R是观测扰动的协方差矩阵；I是单位矩阵；K为卡尔曼增益；下标k、k-1分别代表k、k-1时刻；上标T代表转置。
9.一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现权利要求1-8中任意一项所述方法的步骤。
10.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该程序被处理器执行时实现权利要求1-8任意一项所述方法的步骤。
CN202210934071.3A 2022-08-04 2022-08-04 室内外环境定位方法 Withdrawn CN115436955A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210934071.3A CN115436955A (zh) 2022-08-04 2022-08-04 室内外环境定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210934071.3A CN115436955A (zh) 2022-08-04 2022-08-04 室内外环境定位方法

Publications (1)

Publication Number Publication Date
CN115436955A true CN115436955A (zh) 2022-12-06

Family

ID=84242827

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210934071.3A Withdrawn CN115436955A (zh) 2022-08-04 2022-08-04 室内外环境定位方法

Country Status (1)

Country Link
CN (1) CN115436955A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115797425A (zh) * 2023-01-19 2023-03-14 中国科学技术大学 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法
CN116929338A (zh) * 2023-09-15 2023-10-24 深圳市智绘科技有限公司 地图构建方法、设备及存储介质

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115797425A (zh) * 2023-01-19 2023-03-14 中国科学技术大学 一种基于点云鸟瞰图和由粗到精策略的激光全局定位方法
CN116929338A (zh) * 2023-09-15 2023-10-24 深圳市智绘科技有限公司 地图构建方法、设备及存储介质
CN116929338B (zh) * 2023-09-15 2024-01-09 深圳市智绘科技有限公司 地图构建方法、设备及存储介质

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN115436955A (zh) 室内外环境定位方法
CN110889808B (zh) 一种定位的方法、装置、设备及存储介质
CN112882053B (zh) 一种主动标定激光雷达和编码器外参的方法
CN110488818B (zh) 一种基于激光雷达的机器人定位方法、装置和机器人
KR20170088228A (ko) 다중로봇의 자기위치인식에 기반한 지도작성 시스템 및 그 방법
JP7153511B2 (ja) 自己位置推定装置
CN111487960A (zh) 一种基于定位能力估计的移动机器人路径规划方法
TW202036030A (zh) 資訊處理裝置以及移動機器人
Srinara et al. Performance analysis of 3D NDT scan matching for autonomous vehicles using INS/GNSS/3D LiDAR-SLAM integration scheme
Hu et al. 1d-lrf aided visual-inertial odometry for high-altitude mav flight
Gao et al. Localization of mobile robot based on multi-sensor fusion
CN117387604A (zh) 基于4d毫米波雷达和imu融合的定位与建图方法及系统
US20240077880A1 (en) Slope location correction method and apparatus, robot and readable storage medium
Huang et al. A real-time fast incremental SLAM method for indoor navigation
Kolu et al. A mapping method tolerant to calibration and localization errors based on tilting 2D laser scanner
Selkäinaho Adaptive autonomous navigation of mobile robots in unknown environments
Qin et al. Multi-stage perception, positioning and planning for automatic wireless charging of agvs
Yang et al. AGV robot for laser-SLAM based method testing in automated container terminal
CN113777615B (zh) 室内机器人的定位方法、系统及清洁机器人
WO2024004265A1 (ja) 自己位置推定装置、自己位置推定方法、およびプログラム
Bigaj et al. Low Time Complexity Collision Avoidance Method for Autonomous Mobile Robots
Liu et al. An AMR Mapping Method based on High Efficiency Recursive Filtering Fusion Algorithm
Arturo et al. Cooperative simultaneous localisation and mapping using independent rao–blackwellised filters

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
WW01 Invention patent application withdrawn after publication
WW01 Invention patent application withdrawn after publication

Application publication date: 20221206