CN113587930B - 基于多传感融合的自主移动机器人室内外导航方法及装置 - Google Patents
基于多传感融合的自主移动机器人室内外导航方法及装置 Download PDFInfo
- Publication number
- CN113587930B CN113587930B CN202111168188.7A CN202111168188A CN113587930B CN 113587930 B CN113587930 B CN 113587930B CN 202111168188 A CN202111168188 A CN 202111168188A CN 113587930 B CN113587930 B CN 113587930B
- Authority
- CN
- China
- Prior art keywords
- robot
- current position
- point cloud
- moving process
- cloud data
- 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 133
- 230000004927 fusion Effects 0.000 title claims abstract description 24
- 230000008859 change Effects 0.000 claims abstract description 79
- 238000005259 measurement Methods 0.000 claims abstract description 72
- 238000012937 correction Methods 0.000 claims abstract description 41
- 230000008569 process Effects 0.000 claims description 84
- 230000033001 locomotion Effects 0.000 claims description 18
- 238000000605 extraction Methods 0.000 claims description 2
- 239000011159 matrix material Substances 0.000 description 11
- 230000001133 acceleration Effects 0.000 description 7
- 238000004590 computer program Methods 0.000 description 7
- 238000005516 engineering process Methods 0.000 description 6
- 238000004422 calculation algorithm Methods 0.000 description 5
- 238000004891 communication Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 5
- 230000009466 transformation Effects 0.000 description 5
- 238000012545 processing Methods 0.000 description 4
- 238000011161 development Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000004807 localization Effects 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000010295 mobile communication Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000008447 perception Effects 0.000 description 2
- SAZUGELZHZOXHB-UHFFFAOYSA-N acecarbromal Chemical compound CCC(Br)(CC)C(=O)NC(=O)NC(C)=O SAZUGELZHZOXHB-UHFFFAOYSA-N 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000003190 augmentative effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 230000006835 compression Effects 0.000 description 1
- 238000007906 compression Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 238000007667 floating Methods 0.000 description 1
- 230000006870 function Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Images
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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
-
- 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
- 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
- G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
-
- 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/20—Instruments for performing navigational calculations
-
- 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/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- 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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
-
- 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
- G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
- G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
- G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
- G01S19/42—Determining position
- G01S19/45—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement
- G01S19/47—Determining position by combining measurements of signals from the satellite radio beacon positioning system with a supplementary measurement the supplementary measurement being an inertial measurement, e.g. tightly coupled inertial
-
- 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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
-
- 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
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- 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/0255—Control of position or course in two dimensions specially adapted to land vehicles using acoustic signals, e.g. ultra-sonic singals
-
- 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
Abstract
本申请涉及一种基于多传感融合的自主移动机器人室内外导航方法及装置。所述方法包括：获取机器人在当前位置的惯性测量数据和三维点云数据；基于所述机器人在当前位置的惯性测量数据，确定所述机器人的位姿变化；基于所述机器人的位姿变化，对所述机器人在当前位置的三维点云数据进行畸变校正；将畸变校正后的三维点云数据与导航地图进行匹配，确定所述机器人的当前位置。采用本方法能够实现机器人准确定位。
Description
技术领域
本申请涉及导航技术领域，特别是涉及一种基于多传感融合的自主移动机器人室内外导航方法及装置。
背景技术
随着机器人技术的发展，出现了自主移动机器人（Autonomous Mobile Robots，AMR）。AMR是具备see-think-act（看见、思考、行动）能力的机器，可以感知环境，自主建立环境模型，从而获取自身在环境中的位姿信息，规划并控制机器人移动。
传统技术中，仅采用三维激光雷达进行SLAM（simultaneous localization andmapping，即时定位与地图构建）。由于LiDAR（激光雷达）数据的运动畸变、采样的地面数据稀疏等问题，存在重力矢量漂移现象和严重的高程估计误差，在结构单一或过于复杂的环境下，现有紧靠雷达匹配定位的方法容易发散，无法实现机器人的准确定位。
发明内容
基于此，有必要针对上述技术问题，提供一种能够实现机器人准确定位的基于多传感融合的自主移动机器人室内外导航方法及装置。
一种基于多传感融合的自主移动机器人室内外导航方法，所述方法包括：
获取机器人在当前位置的惯性测量数据和三维点云数据；
基于所述机器人在当前位置的惯性测量数据，确定所述机器人的位姿变化；
基于所述机器人的位姿变化，对所述机器人在当前位置的三维点云数据进行畸变校正；
将畸变校正后的三维点云数据与导航地图进行匹配，确定所述机器人的当前位置。
在其中一个实施例中，所述将畸变校正后的三维点云数据与导航地图进行匹配，确定所述机器人的当前位置，包括：
获取机器人在当前位置的GPS信息；
基于GPS信息与二维地图之间的对应关系，获取所述机器人在当前位置的GPS信息对应的二维地图，所述二维地图上任意两点对应的三维坐标的连线与水平面的夹角相同；
将畸变校正后的三维点云数据投影在所述机器人当前移动的路面上，得到所述机器人在当前位置的二维点云数据；
将所述机器人在当前位置的二维点云数据与所述机器人在当前位置的GPS信息对应的二维地图进行匹配，确定所述机器人的当前位置。
在其中一个实施例中，所述方法还包括：
控制所述机器人按照设定路线移动，并获取所述机器人在移动过程中的惯性测量数据、GPS信息和三维点云数据；
基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中的位姿变化；
将所述机器人在移动过程中的三维点云数据投影到所述机器人移动的路面上，建立与所述机器人在移动过程中的GPS信息对应的二维地图，直到所述机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度。
在其中一个实施例中，所述方法还包括：
若所述机器人在移动过程中的位姿变化对应的俯仰角度大于或等于所述设定角度，则将俯仰角度大于或等于所述设定角度后的三维点云数据单独投影到所述机器人移动的路面上，重新建立与所述机器人在移动过程中的GPS信息对应的二维地图，直到所述机器人在移动过程中的位姿变化对应的俯仰角度再次大于或等于所述设定角度。
在其中一个实施例中，所述基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中的位姿变化，包括：
基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中位姿变化的预估值；
基于所述机器人在移动过程中的三维点云数据，确定地面扫描点和物体扫描点；
对所述地面扫描点和所述物体扫描点进行特征提取，确定线特征点和面特征点；
将所述机器人在移动过程中位姿变化的预估值、所述线特征点的坐标和所述面特征点的坐标代入误差方程，确定所述误差方程的误差值最小时的位姿变化，作为所述机器人在移动过程中位姿变化的实际值。
在其中一个实施例中，所述方法还包括：
获取所述机器人的移动路径；
基于所述机器人的移动路径和当前位置，确定所述机器人的轮转角。
在其中一个实施例中，所述基于所述机器人的移动路径和当前位置，确定所述机器人的轮转角，包括：
按照如下公式计算所述机器人的轮转角：
其中，θ(t)为所述机器人的前轮转角，L为所述机器人的前后轮距离，α为所述机器人的目标位置、当前位置与转弯圆心连线夹角的一半，k为设定系数，vx为所述机器人在前行方向上的移动速度。
在其中一个实施例中，所述方法还包括：
获取所述机器人在当前位置的超声波测量距离；
若所述机器人在当前位置的超声波测量距离小于第一距离阈值，则控制所述机器人暂停移动。
在其中一个实施例中，所述方法还包括：
基于畸变校正后的三维点云数据，确定所述机器人在当前位置的激光测量距离；
若所述机器人在当前位置的激光测量距离小于第二距离阈值，则控制所述机器人降低移动速度。
一种基于多传感融合的自主移动机器人室内外导航装置，所述装置包括：
获取模块，用于获取机器人在当前位置的惯性测量数据和三维点云数据；
变化确定模块，用于基于所述机器人在当前位置的惯性测量数据，确定所述机器人的位姿变化；
校正模块，用于基于所述机器人的位姿变化，对所述机器人在当前位置的三维点云数据进行畸变校正；
位置确定模块，用于将畸变校正后的三维点云数据与导航地图进行匹配，确定所述机器人的当前位置。
上述基于多传感融合的自主移动机器人室内外导航方法及装置，通过获取机器人在当前位置的惯性测量数据和三维点云数据，基于机器人在当前位置的惯性测量数据，确定机器人的位姿变化，可以先基于机器人的位姿变化，对机器人在当前位置的三维点云数据进行畸变校正，再将畸变校正后的三维点云数据与导航地图进行匹配，确定机器人的当前位置。利用惯性测量数据确定机器人的位姿变化，对三维点云数据进行畸变校正，解决三维点云数据运动畸变导致的定位误差，实现机器人的准确定位。
附图说明
图1为一个实施例中基于多传感融合的自主移动机器人室内外导航方法的应用环境图；
图2为一个实施例中基于多传感融合的自主移动机器人室内外导航方法的流程示意图；
图3为一个实施例中三维点云数据畸变校正的示意图；
图4为另一个实施例中基于多传感融合的自主移动机器人室内外导航方法的流程示意图；
图5为又一个实施例中基于多传感融合的自主移动机器人室内外导航方法的流程示意图；
图6为一个实施例中地面扫描点判定的示意图；
图7为一个实施例中物体扫描点判定的示意图；
图8为一个实施例中二维地图建立的示意图；
图9为一个实施例中机器人运动状态的示意图；
图10为一个实施例中机器人路径跟踪的示意图；
图11为又一个实施例中基于多传感融合的自主移动机器人室内外导航装置的结构框图；
图12为一个实施例中机器人的内部结构图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行进一步详细说明。应当理解，此处描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。
近年来，5G（5th Generation Mobile Communication Technology，第五代移动通信技术）、AR（Augmented Reality，增强现实）、VR（Virtual Reality，虚拟现实）、云计算、大数据、物联网、人工智能和机器人等技术的飞速发展，为相关行业的变革飞跃提供了技术上的保障。在我国，随着配送需求的快速增长、用户体验与人力成本的提升，以及一些偏远地区和特殊环境下的末端配送需求难以满足，货物配送方式已发生显著变化。随着无人驾驶技术的成熟，无人配送车得到了进一步的发展。基于无人驾驶技术的无人配送车，车身布满各种高灵度传感器，实时感知周围实际道路情况。传感器的实时感知将帮助无人配送车避开可能发生的延误事故，规划更直接、快捷的配送路线，准时完成配送任务，提升货物配送的效率，已经成为大势所趋。
本申请提供的基于多传感融合的自主移动机器人室内外导航方法，可以应用于如图1所示的应用环境中。机器人102在区域104内自主移动。机器人102上布满各种高灵敏度传感器，可以实时感知周围情况，帮助机器人102避开区域104内的各种障碍物，并规划路线移动到指定位置，实现货物的无人配送。其中，移动到指定位置的过程中，需要先对机器人102进行定位，即确定机器人102在区域104内的位置，再基于机器人102在区域104内的位置，确定机器人102的移动方向，以按照规定路线移动。
具体地，机器人102采用如下方式进行定位：获取机器人在当前位置的惯性测量数据和三维点云数据；基于机器人在当前位置的惯性测量数据，确定机器人的位姿变化；基于机器人的位姿变化，对机器人在当前位置的三维点云数据进行畸变校正；将畸变校正后的三维点云数据与导航地图进行匹配，确定机器人的当前位置。
在一个实施例中，如图2所示，提供了一种基于多传感融合的自主移动机器人室内外导航方法，以该方法应用于图1中的机器人为例进行说明，包括以下步骤：
步骤S202，获取机器人在当前位置的惯性测量数据和三维点云数据。
其中，惯性测量数据是IMU（Inertial Measurement Unit，惯性测量单元）的测量数据，包括三个单轴的加速度和角速度。
三维点云数据是利用激光雷达发射激光得到的数据，包括各个激光反射点的坐标。
具体地，机器人上设有IMU和激光雷达。IMU在机器人的移动过程中实时检测，得到惯性测量数据。
激光雷达向周围发射光束，光束在物体表面反射后回到激光雷达，激光雷达根据反射光束的发射方向、以及光束在发射和接收之间的时间差，可以确定物体所在的方位、以及与激光雷达之间的距离，进而建立三维点云数据。
步骤S204，基于机器人在当前位置的惯性测量数据，确定机器人的位姿变化。
具体地，该步骤S204包括：
采用如下公式确定机器人的位姿变化：
其中，pj和pi分别为机器人的第j个位置和第i个位置，vk、vj和vi分别为机器人在第k个位置的速度、第j个位置的速度和第i个位置的速度，△t为机器人从第i个位置到第j个位置的时间差，gw为重力加速度，Rk为机器人在第k个位置从IMU坐标系转换至世界坐标系的旋转矩阵，ak为机器人在第k个位置的加速度，ba为IMU的加速度漂移，qj和qi分别为机器人在第j个位置的旋转姿态和在第i个位置的旋转姿态，wk为机器人在第k个位置的角速度，bw为IMU的角速度漂移。
步骤S206，基于机器人的位姿变化，对机器人在当前位置的三维点云数据进行畸变校正。
具体地，该步骤S206包括：基于机器人的位姿变化，确定机器人在两个时间点之间的位姿变换；将机器人在后一个时间点的三维点云数据，减去机器人在两个时间点之间的位姿变换。
例如，如图3所示，先确定激光雷达从A时刻点到B时刻点之间的位姿变换Rk，再将激光雷达在B时刻点的三维点云数据减去位姿变换Rk，完成畸变校正。
步骤S208，将畸变校正后的三维点云数据与导航地图进行匹配，确定机器人的当前位置。
上述基于多传感融合的自主移动机器人室内外导航方法中，通过获取机器人在当前位置的惯性测量数据和三维点云数据，基于机器人在当前位置的惯性测量数据，确定机器人的位姿变化，可以先基于机器人的位姿变化，对机器人在当前位置的三维点云数据进行畸变校正，再将畸变校正后的三维点云数据与导航地图进行匹配，确定机器人的当前位置。利用惯性测量数据确定机器人的位姿变化，对三维点云数据进行畸变校正，解决三维点云数据运动畸变导致的定位误差，实现机器人的准确定位。
在一个实施例中，如图4所示，步骤S208包括：
步骤S302，获取机器人在当前位置的GPS信息。
其中，GPS信息是利用GPS定位得到的信息，包括时间、纬度、纬度方向、经度、经度方向、解状态、参与计算卫星数、水平精度因子等。
具体地，机器人上设有GPS接收机。GPS接收机通过天线接收多个运行卫星发送的卫星信号、以及基准站发送的修正数，修正数是基准站将已知的准确坐标与基于卫星信号得到的计算坐标进行比较得到的，GPS接收机利用修正数消除基于卫星信号得到的计算坐标存在的误差，得到机器人的准确位置，即GPS信息。
在实际应用中，GPS接收机将收到的数据帧解析成经纬度、航向角及GPS定位求解状态等信息。GPS定位求解状态分为四个状态，当求解状态为固定解时，解析结果可用；当求解状态为浮动解、单点解、定位无效中的一个时，解析结果不采用。
步骤S304，基于GPS信息与二维地图之间的对应关系，获取机器人在当前位置的GPS信息对应的二维地图。
其中，二维地图上任意两点对应的三维坐标的连线与水平面的夹角相同。在实际应用中，二维地图上任意两点对应的三维坐标的连线与水平面的夹角变化小于设定角度，如5°、10°、15°等，则认为二维地图上任意两点对应的三维坐标的连线与水平面的夹角恒定，以容许地面不平整导致的夹角变化。
在本实施例中，机器人在同一二维地图上移动的位姿变化对应的俯仰角度小于设定角度，相当于机器人在平面上移动，机器人在这个范围内的三维点云数据投影到移动的路面上基本没有变形，得到的二维点云数据可以实现机器人的准确定位。
而且将二维地图与GPS信息对应，即使GPS信息确定的位置并不是机器人的准确位置，也会很接近机器人的准确位置，因此可以利用GPS信息准确确定出机器人当前位置的大致范围，即获取机器人在当前位置的GPS信息对应的二维地图，后续再在二维地图中找出机器人的准确位置。
步骤S306，将畸变校正后的三维点云数据投影在机器人当前移动的路面上，得到机器人在当前位置的二维点云数据。
具体地，采用机器人当前移动的路面作为投影平面，将机器人在当前位置的三维点云数据投影在投影平面上，并将自机器人向远离机器人的各个方向上距离机器人最近的点，组成机器人在当前位置的二维点云数据。
步骤S308，将机器人在当前位置的二维点云数据与机器人在当前位置的GPS信息对应的二维地图进行匹配，确定机器人的当前位置。
具体地，将机器人在当前位置的GPS信息对应的二维地图中各个位置的二维点云数据，与机器人在当前位置的二维点云数据进行匹配，选择匹配度最高的位置确定为机器人的当前位置。
本实施例中，先获取机器人在当前位置的GPS信息，基于GPS信息与二维地图之间的对应关系，获取机器人在当前位置的GPS信息对应的二维地图，二维地图上任意两点对应的三维坐标的连线与水平面的夹角恒定，可以利用机器人在当前位置的GPS信息获取对应区域的二维地图，准确限定出机器人当前位置所在的大致区域。再获取机器人在当前位置的三维点云数据，将机器人在当前位置的三维点云数据投影到机器人当前移动的路面上，得到机器人在当前位置的二维点云数据，并将机器人在当前位置的二维点云数据与机器人在当前位置的GPS信息对应的二维地图进行匹配，即可在限定出的大致范围内准确确定机器人的当前位置。机器人的当前位置是综合GPS信息和三维点云数据得到的，可以在室内应用时利用激光雷达弥补GPS受到干扰而产生的误差，也可以在室外应用时利用GPS避免激光雷达受到障碍物影响而产生误差，可以同时在室内和室外两种场景下实现自主移动机器人的准确定位。
在一个实施例中，如图5所示，该方法还包括：
步骤S502，控制机器人按照设定路线移动，并获取机器人在移动过程中的惯性测量数据、GPS信息和三维点云数据。
其中，设定路线可以是预先规划并输入机器人的路线，也可以是机器人在移动过程中基于实际情况选择的路线。
步骤S504，基于机器人在移动过程中的惯性测量数据，确定机器人在移动过程中的位姿变化。
具体地，该步骤S504包括：
第一步，基于机器人在移动过程中的惯性测量数据，确定机器人在移动过程中位姿变化的预估值。
具体地，第一步与步骤S204类似，在此不再详述。
第二步，基于机器人在移动过程中的三维点云数据，确定地面扫描点和物体扫描点。
示例性地，该方法还包括：基于机器人在移动过程中位姿变化的预估值，对机器人在移动过程中的三维点云数据进行畸变校正。
在实际应用中，先将激光雷达得到的无序点云数据转换为有序深度矩阵，方便后续的数据处理。具体地，有序深度矩阵的同一行数据与激光雷达在同一竖直平面上的线束一一对应，有序深度矩阵的同一列数据与激光雷达在同一水平面上的线束一对多。有序深度矩阵的同一列数据数量受到数据处理水平的限制，通常比激光雷达在同一水平面上的线束数量少，因此将激光雷达在同一水平面上的多个相邻线束与有序深度矩阵一列数据中的一个对应。有序深度矩阵中每个数据数值为对应的多个线束反射距离的统计值，统计值为最大值、最小值或者平均值。
在有序深度矩阵之后，先确定各水平方向的扫描点是否为地面扫描点，得到地面扫描点集。再从有序深度矩阵中剔除地面扫描点集之后，确定属于同一物体的扫描点集，并从各物体的扫描点集中剔除扫描点数量小于数量阈值的扫描点集，得到物体扫描点集。
示例性地，第二步包括：基于机器人在移动过程中的三维点云数据，确定同一水平方向的各相邻扫描点连线与水平面之间的夹角；若同一水平方向的各相邻扫描点连线与水平面之间的夹角均为0，则判定这一水平方向的各扫描点为地面扫描点；若相邻两个非地面扫描点连线与其中一个非地面扫描点的扫描方向之间的夹角在设定范围内，则判定两个非地面扫描点属于同一物体；若属于同一物体的非地面扫描点的数量大于或等于数量阈值，则判定属于同一物体的非地面扫描点为物体扫描点。
例如，如图6所示，确定同一水平方向的相邻两个扫描点连线在水平方向上的距离差dxy=sqrt（dx+dy）和在竖直方向上的距离差dz，进而计算两个扫描点的垂直角度θ=arctan（dz/dxy），即这个水平方向的一相邻扫描点连线与水平面的夹角。如果这个水平方向的各相邻扫描点连线与水平面的夹角均为0，则判定这个水平方向的各扫描点为地面扫描点；如果这个水平方向的至少一相邻扫描点连线与水平面的夹角大于0，则判定这个水平方向的各扫描点为非地面扫描点。
又如，如图7所示，确定激光雷达L0发射的激光束L的相邻两个扫描点Li、Lj的连线LiLj，与扫描点Li的扫描方向（即扫描点Li与激光雷达L0的连线）之间的夹角α，或者与扫描点Lj的扫描方向（即扫描点Lj与激光雷达L0的连线）之间的夹角β。如果α或者β在设定范围内，则判定扫描点Li、Lj属于同一物体；如果α或者β在设定范围外，则判定扫描点Li、Lj属于不同物体。
第三步，对地面扫描点和物体扫描点进行特征提取，确定线特征点和面特征点。
具体地，采用如下公式确定扫描点的特征值：
其中，P’i和P’j为两个扫描点的数值，
为P’i对应扫描点的特征值，S为P’i对应扫描点的邻域大小；|S|为求S绝对值，||P’i||为求向量P’i的模；
若非地面扫描点的特征值大于或等于特征阈值，则判定非地面扫描点为线特征点；
若扫描点的特征值小于特征阈值，则判定扫描点为面特征点。
第四步，将机器人在移动过程中位姿变化的预估值、线特征点的坐标和面特征点的坐标代入误差方程，确定误差方程的误差值最小时的位姿变化，作为机器人在移动过程中位姿变化的实际值。
具体地，第四步包括：
采用如下公式组计算误差方程的最小误差值：
其中，Tk L为机器人在移动过程中位姿变化的预估值，EL为激光点云数据进行特征
匹配的误差值，EB为惯性测量数据进行坐标变换的误差值，
为线特征点，
为面特征
点，
为
中的线特征点，
为以k时刻的
坐标系为基准、
在这个坐标系内的
投影，
，R为TkL中的旋转矩阵，p为TkL中的平移矩阵，
为
中最近邻同类特征点，
为
在相邻通道的最近邻点，
为
中的
面特征点，
为
最近邻同类特征点，
为
在相邻通道的最近
邻点，
为
在相同通道的最近邻点，
分别代表位置、速度、角度、加速度偏差、角速度偏差，pk和pk-1分别为机器人的第k个位置和
第k-1个位置，gw为重力加速度，△tk-1为机器人从第k-1个位置到第k个位置的时间差，vk和
vk-1分别为为机器人在第k个位置的速度和在第k-1个位置的速度，ak k-1为机器人在第k个位
置的加速度，ba，k和ba，k-1分别为IMU在第k个位置的加速度漂移和在第k-1个位置的加速度漂
移，bw，k和bw，k-1分别为IMU在第k个位置的的角速度漂移和在第k-1个位置的角速度漂移，
βk k-1为机器人从第k-1个位置到第k个位置的速度，θk k-1为机器人从第k-1个位置到第k个位
置的角度，qk-1和qk分别为机器人在第k-1个位置的姿态和在第k个位置的姿态。
在实际应用中，由于
只有位置和角度信息，因此
。
步骤S506，将机器人在移动过程中的三维点云数据投影到机器人移动的路面上，建立与机器人在移动过程中的GPS信息对应的二维地图，直到机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度。
具体地，激光雷达获取的三维点云数据通过在KD树（k-dimensional tree）搜索最近邻点进行配准，建立三维地图。对三维地图进行平面化压缩，建立二维地图。二维地图与GPS信息绑定，可以通过GPS信息获取对应的二维地图。
在一个实施例中，该方法还包括：若机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度，则将俯仰角度大于或等于设定角度后的三维点云数据单独投影到机器人移动的路面上，重新建立与机器人在移动过程中的GPS信息对应的二维地图，直到机器人在移动过程中的位姿变化对应的俯仰角度再次大于或等于设定角度。
在实际应用中，若机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度，重新建立一个新的二维地图，以适应新的路面坡度。与此同时，可以继续建立原来的二维地图，直到机器人的移动距离达到距离阈值，以确保二维地图的完整性。
示例性地，距离阈值为传感器探测距离的两倍。
例如，如图8所示，先建立二维地图A，当机器人移动到A1点时，IMU检测到路面坡度达到阈值，此时重新建立二维地图C，同时继续建立二维地图A。当机器人移动到A2点时，机器人的移动距离（即A1和A2之间的距离）达到距离阈值，此时停止建立二维地图A，继续建立二维地图C，二维地图C和二维地图A存在重合区域D。
在一个实施例中，该方法还包括：将机器人在移动过程中的三维点云数据划分到以机器人所在位置为圆心、直径不同的圆环中，并统计各个圆环中扫描点的数量；若机器人两次采集的三维点云数据得到的各个圆环中扫描点的数量相同，则判定机器人回到原点，结束建立二维地图。
具体地，采用如下公式将三维点云数据压缩成向量Ksc：
其中，Mri为三维点云数据中第i个圆环中扫描点的数量，Nn为三维点云数据在第i个圆环中扫描点数量的最大值；
利用向量Ksc在KD树中搜索相似度最高的多帧三维点云数据，并在相似度达到阈值时，判定检测到回环，即机器人回到原点，此时结束建立二维地图。
示例性地，采用如下公式计算相邻两个圆环的半径差：
其中，Wc为相邻两个圆环的半径差，Bmax为激光点的最远距离，Nc为圆环的数量。
在实际应用中，每次插入新的因子节点（惯性测量数据更新、三维点云数据更新和检测到闭环中的至少一个），优化一次数据。为保证实时性，可以在三个线程上并行运行。
本实施例中，通过控制机器人按照设定路线移动，并获取机器人在移动过程中的惯性测量数据、GPS信息和三维点云数据，以便后续使用。具体来说，基于机器人在移动过程中的惯性测量数据，确定机器人在移动过程中的位姿变化，可以判定机器人在移动过程中的位姿变化对应的俯仰角度与设定角度的大小关系。将机器人在移动过程中的三维点云数据投影到机器人移动的路面上，建立与机器人在移动过程中的GPS信息对应的二维地图，直到机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度，可以综合惯性测量数据、GPS信息和三维点云数据，准确建立机器人定位使用的二维地图，以对机器人进行准确定位。
在实际应用中，可以基于构建的二维地图实时进行路径规划，基于墙壁、家具等障碍物的位置信息，采用距离最近的障碍物距离信息设置代价值，在障碍物周围设置缓冲区，进行最优路径评价计算。全局规划可以建立到最终目标或一个远距离目标的路径，具体可以采用A*、Dijkstra、D*、LPA*、D* lite和RRT等算法。局部规划可以建立到近距离目标和为了临时躲避障碍物的路径，具体可以采用TEB、DWA、eband等算法。
在路径规划之后，可以根据算法传入的路径起点、途径点和终点信息，不断获取机器人的定位信息，采用基于几何追踪的跟踪算法计算分段跟踪轨迹控制量，实现机器人对目标途径点的路径跟踪。
在一个实施例中，该方法还包括：获取机器人的移动路径；基于机器人的移动路径和当前位置，确定机器人的轮转角。
根据阿克曼转向结构的对称性，机器人的运动模型可以简化为如图9所示的结构。其中，转弯半径为R，前轮转角为θ，前后轮距离为L。纯跟踪算法以车后轴为切点，车辆纵向车身为切线，通过控制前轮转角，使车辆可以沿着一条经过目标路点的圆弧行驶，如图10所示，根据正弦定理和几何原理，可得到前轮转角控制函数。
具体地，基于机器人的移动路径和当前位置，确定机器人的轮转角，包括：
按照如下公式计算机器人的轮转角：
其中，θ(t)为机器人的前轮转角，t为时间，L为机器人的前后轮距离，α为机器人的目标位置、当前位置与转弯圆心连线夹角的一半，k为设定系数，vx为机器人在前行方向上的移动速度。
在一个实施例中，该方法还包括：获取机器人在当前位置的超声波测量距离；若机器人在当前位置的超声波测量距离小于第一距离阈值，则控制机器人暂停移动。
示例性地，该方法还包括：基于畸变校正后的三维点云数据，确定机器人在当前位置的激光测量距离；若机器人在当前位置的激光测量距离小于第二距离阈值，则控制机器人降低移动速度。
激光雷达的测量距离可达100米，距离远，数据量大，处理时间较长，适用于长距离避障。而超声波的测量距离为5米，距离短，数据量小，处理迅速，适用于短距离避障。综上，可以实现机器人的紧急避障及突发状态的自我安全保护。
另外，机器人上设有人机交互模块，一方面可以显示机器人信息，如传感数据、地图及定位信息、所选路径、实时轨迹等，另一方面，也可以设置任务，如操控模式、轨迹录制与复现、地图编辑与修改等。
应该理解的是，虽然图2、4-5的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，图2、4-5中的至少一部分步骤可以包括多个步骤或者多个阶段，这些步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤中的步骤或者阶段的至少一部分轮流或者交替地执行。
在一个实施例中，如图11所示，提供了一种基于多传感融合的自主移动机器人室内外导航装置900，包括：获取模块901、变化确定模块902、校正模块903和位置确定模块904，其中：
获取模块901，用于获取机器人在当前位置的惯性测量数据和三维点云数据。
变化确定模块902，用于基于机器人在当前位置的惯性测量数据，确定机器人的位姿变化。
校正模块903，用于基于机器人的位姿变化，对机器人在当前位置的三维点云数据进行畸变校正。
位置确定模块904，用于将畸变校正后的三维点云数据与导航地图进行匹配，确定机器人的当前位置。
上述基于多传感融合的自主移动机器人室内外导航装置，通过获取机器人在当前位置的惯性测量数据和三维点云数据，基于机器人在当前位置的惯性测量数据，确定机器人的位姿变化，可以先基于机器人的位姿变化，对机器人在当前位置的三维点云数据进行畸变校正，再将畸变校正后的三维点云数据与导航地图进行匹配，确定机器人的当前位置。利用惯性测量数据确定机器人的位姿变化，对三维点云数据进行畸变校正，解决三维点云数据运动畸变导致的定位误差，实现机器人的准确定位。
在一个实施例中，获取模块901还用于，获取机器人在当前位置的GPS信息。位置确定模块904包括：地图获取单元、投影单元和匹配单元。其中：地图获取单元，用于基于GPS信息与二维地图之间的对应关系，获取机器人在当前位置的GPS信息对应的二维地图，二维地图上任意两点对应的三维坐标的连线与水平面的夹角相同。投影单元，用于将畸变校正后的三维点云数据投影在机器人当前移动的路面上，得到机器人在当前位置的二维点云数据。匹配单元，用于将机器人在当前位置的二维点云数据与机器人在当前位置的GPS信息对应的二维地图进行匹配，确定机器人的当前位置。
在一个实施例中，获取模块901还用于，控制机器人按照设定路线移动，并获取机器人在移动过程中的惯性测量数据、GPS信息和三维点云数据。相应地，该装置还包括：位姿确定模块和地图建立模块。其中：位姿确定模块，用于基于机器人在移动过程中的惯性测量数据，确定机器人在移动过程中的位姿变化。地图建立模块，用于将机器人在移动过程中的三维点云数据投影到机器人移动的路面上，建立与机器人在移动过程中的GPS信息对应的二维地图，直到机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度。
在一个实施例中，地图建立模块还用于，当机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度时，将俯仰角度大于或等于设定角度后的三维点云数据单独投影到机器人移动的路面上，重新建立与机器人在移动过程中的GPS信息对应的二维地图，直到机器人在移动过程中的位姿变化对应的俯仰角度再次大于或等于设定角度。
在一个实施例中，位姿确定模块包括位姿估计单元、扫描点确定单元、特征点确定单元和位姿确定单元。其中：位姿估计单元，用于基于机器人在移动过程中的惯性测量数据，确定机器人在移动过程中位姿变化的预估值。扫描点确定单元，用于基于机器人在移动过程中的三维点云数据，确定地面扫描点和物体扫描点。特征点确定单元，用于对地面扫描点和物体扫描点进行特征提取，确定线特征点和面特征点。位姿确定单元，用于将机器人在移动过程中位姿变化的预估值、线特征点的坐标和面特征点的坐标代入误差方程，确定误差方程的误差值最小时的位姿变化，作为机器人在移动过程中位姿变化的实际值。
在一个实施例中，该装置还包括路径获取模块和轮转角确定模块。其中：获取模块，用于获取机器人的移动路径。轮转角确定模块，用于基于机器人的移动路径和当前位置，确定机器人的轮转角。
在一个实施例中，轮转角确定模块用于，按照如下公式计算机器人的轮转角：
其中，θ(t)为机器人的前轮转角，L为机器人的前后轮距离，α为机器人的目标位置、当前位置与转弯圆心连线夹角的一半，k为设定系数，vx为机器人在前行方向上的移动速度。
在一个实施例中，数据获取模块901还用于，获取机器人在当前位置的超声波测量距离。相应地，该装置还包括控制模块。其中：控制模块，用于当机器人在当前位置的超声波测量距离小于第一距离阈值时，控制机器人暂停移动。
在一个实施例中，控制模块还用于，基于畸变校正后的三维点云数据，确定机器人在当前位置的激光测量距离；若机器人在当前位置的激光测量距离小于第二距离阈值，则控制机器人降低移动速度。
关于基于多传感融合的自主移动机器人室内外导航装置的具体限定可以参见上文中对于基于多传感融合的自主移动机器人室内外导航方法的限定，在此不再赘述。上述基于多传感融合的自主移动机器人室内外导航装置中的各个模块可全部或部分通过软件、硬件及其组合来实现。上述各模块可以硬件形式内嵌于或独立于计算机设备中的处理器中，也可以以软件形式存储于计算机设备中的存储器中，以便于处理器调用执行以上各个模块对应的操作。
在一个实施例中，提供了一种机器人，其内部结构图可以如图12所示。该机器人包括通过系统总线连接的处理器、存储器、通信接口、显示屏、输入装置、IMU、激光雷达和GPS接收机。其中，该机器人的处理器用于提供计算和控制能力。该机器人的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该机器人的通信接口用于与外部的终端进行有线或无线方式的通信，无线方式可通过WIFI、运营商网络、NFC（近场通信）或其他技术实现。该计算机程序被处理器执行时以实现一种基于多传感融合的自主移动机器人室内外导航方法。该机器人的显示屏可以是液晶显示屏或者电子墨水显示屏，该机器人的输入装置可以是显示屏上覆盖的触摸层，也可以是机器人外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。该机器人的IMU、激光雷达和GPS接收机分别用于获取惯性测量数据、激光点云数据和GPS信息。
本领域技术人员可以理解，图12中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备的限定，具体的计算机设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
在一个实施例中，还提供了一种计算机设备，包括存储器和处理器，存储器中存储有计算机程序，该处理器执行计算机程序时实现上述各方法实施例中的步骤。
在一个实施例中，提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述各方法实施例中的步骤。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和易失性存储器中的至少一种。非易失性存储器可包括只读存储器（Read-Only Memory，ROM）、磁带、软盘、闪存或光存储器等。易失性存储器可包括随机存取存储器（Random Access Memory，RAM）或外部高速缓冲存储器。作为说明而非局限，RAM可以是多种形式，比如静态随机存取存储器（Static Random Access Memory，SRAM）或动态随机存取存储器（Dynamic Random Access Memory，DRAM）等。
以上实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。
Claims (16)
1.一种基于多传感融合的自主移动机器人室内外导航方法，其特征在于，所述方法包括：
获取机器人在当前位置的GPS信息、惯性测量数据和三维点云数据；
基于所述机器人在当前位置的惯性测量数据，确定所述机器人的位姿变化；
基于所述机器人的位姿变化，对所述机器人在当前位置的三维点云数据进行畸变校正；
基于GPS信息与二维地图之间的对应关系，获取所述机器人在当前位置的GPS信息对应的二维地图，所述二维地图上任意两点对应的三维坐标的连线与水平面的夹角相同；
将畸变校正后的三维点云数据投影在所述机器人当前移动的路面上，得到所述机器人在当前位置的二维点云数据；
将所述机器人在当前位置的二维点云数据与所述机器人在当前位置的GPS信息对应的二维地图进行匹配，确定所述机器人的当前位置。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
控制所述机器人按照设定路线移动，并获取所述机器人在移动过程中的惯性测量数据、GPS信息和三维点云数据；
基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中的位姿变化；
将所述机器人在移动过程中的三维点云数据投影到所述机器人移动的路面上，建立与所述机器人在移动过程中的GPS信息对应的二维地图，直到所述机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度。
3.根据权利要求2所述的方法，其特征在于，所述方法还包括：
若所述机器人在移动过程中的位姿变化对应的俯仰角度大于或等于所述设定角度，则将俯仰角度大于或等于所述设定角度后的三维点云数据单独投影到所述机器人移动的路面上，重新建立与所述机器人在移动过程中的GPS信息对应的二维地图，直到所述机器人在移动过程中的位姿变化对应的俯仰角度再次大于或等于所述设定角度。
4.根据权利要求2或3所述的方法，其特征在于，所述基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中的位姿变化，包括：
基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中位姿变化的预估值；
基于所述机器人在移动过程中的三维点云数据，确定地面扫描点和物体扫描点；
对所述地面扫描点和所述物体扫描点进行特征提取，确定线特征点和面特征点；
将所述机器人在移动过程中位姿变化的预估值、所述线特征点的坐标和所述面特征点的坐标代入误差方程，确定所述误差方程的误差值最小时的位姿变化，作为所述机器人在移动过程中位姿变化的实际值。
5.根据权利要求1至3任一项所述的方法，其特征在于，所述方法还包括：
获取所述机器人的移动路径；
基于所述机器人的移动路径和当前位置，确定所述机器人的轮转角。
6.根据权利要求5所述的方法，其特征在于，所述基于所述机器人的移动路径和当前位置，确定所述机器人的轮转角，包括：
按照如下公式计算所述机器人的轮转角：
其中，θ(t)为所述机器人的前轮转角，L为所述机器人的前后轮距离，α为所述机器人的目标位置、当前位置与转弯圆心连线夹角的一半，k为设定系数，vx为所述机器人在前行方向上的移动速度。
7.根据权利要求1至3任一项所述的方法，其特征在于，所述方法还包括：
获取所述机器人在当前位置的超声波测量距离；
若所述机器人在当前位置的超声波测量距离小于第一距离阈值，则控制所述机器人暂停移动。
8.根据权利要求7所述的方法，其特征在于，所述方法还包括：
基于畸变校正后的三维点云数据，确定所述机器人在当前位置的激光测量距离；
若所述机器人在当前位置的激光测量距离小于第二距离阈值，则控制所述机器人降低移动速度。
9.一种基于多传感融合的自主移动机器人室内外导航装置，其特征在于，所述装置包括：
获取模块，用于获取机器人在当前位置的GPS信息、惯性测量数据和三维点云数据；
变化确定模块，用于基于所述机器人在当前位置的惯性测量数据，确定所述机器人的位姿变化；
校正模块，用于基于所述机器人的位姿变化，对所述机器人在当前位置的三维点云数据进行畸变校正；
位置确定模块，包括地图获取单元、投影单元和匹配单元；
所述地图获取单元，用于基于GPS信息与二维地图之间的对应关系，获取所述机器人在当前位置的GPS信息对应的二维地图，所述二维地图上任意两点对应的三维坐标的连线与水平面的夹角相同；
所述投影单元，用于将畸变校正后的三维点云数据投影在所述机器人当前移动的路面上，得到所述机器人在当前位置的二维点云数据；
所述匹配单元，用于将所述机器人在当前位置的二维点云数据与所述机器人在当前位置的GPS信息对应的二维地图进行匹配，确定所述机器人的当前位置。
10.根据权利要求9所述的装置，其特征在于，所述获取模块还用于，控制所述机器人按照设定路线移动，并获取所述机器人在移动过程中的惯性测量数据、GPS信息和三维点云数据；
所述装置还包括：
位姿确定模块，用于基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中的位姿变化；
地图建立模块，用于将所述机器人在移动过程中的三维点云数据投影到所述机器人移动的路面上，建立与所述机器人在移动过程中的GPS信息对应的二维地图，直到所述机器人在移动过程中的位姿变化对应的俯仰角度大于或等于设定角度。
11.根据权利要求10所述的装置，其特征在于，所述地图建立模块还用于，当所述机器人在移动过程中的位姿变化对应的俯仰角度大于或等于所述设定角度时，将俯仰角度大于或等于所述设定角度后的三维点云数据单独投影到所述机器人移动的路面上，重新建立与所述机器人在移动过程中的GPS信息对应的二维地图，直到所述机器人在移动过程中的位姿变化对应的俯仰角度再次大于或等于所述设定角度。
12.根据权利要求10或11所述的装置，其特征在于，所述位姿确定模块包括：
位姿估计单元，用于基于所述机器人在移动过程中的惯性测量数据，确定所述机器人在移动过程中位姿变化的预估值；
扫描点确定单元，用于基于所述机器人在移动过程中的三维点云数据，确定地面扫描点和物体扫描点；
特征点确定单元，用于对所述地面扫描点和所述物体扫描点进行特征提取，确定线特征点和面特征点；
位姿确定单元，用于将所述机器人在移动过程中位姿变化的预估值、所述线特征点的坐标和所述面特征点的坐标代入误差方程，确定所述误差方程的误差值最小时的位姿变化，作为所述机器人在移动过程中位姿变化的实际值。
13.根据权利要求9至11任一项所述的装置，其特征在于，所述装置还包括：
路径获取模块，用于获取所述机器人的移动路径；
轮转角确定模块，用于基于所述机器人的移动路径和当前位置，确定所述机器人的轮转角。
14.根据权利要求13所述的装置，其特征在于，所述轮转角确定模块用于，
按照如下公式计算所述机器人的轮转角：
其中，θ(t)为所述机器人的前轮转角，L为所述机器人的前后轮距离，α为所述机器人的目标位置、当前位置与转弯圆心连线夹角的一半，k为设定系数，vx为所述机器人在前行方向上的移动速度。
15.根据权利要求9至11任一项所述的装置，其特征在于，所述数据获取模块还用于，获取所述机器人在当前位置的超声波测量距离；
所述装置还包括：
控制模块，用于当所述机器人在当前位置的超声波测量距离小于第一距离阈值时，控制所述机器人暂停移动。
16.根据权利要求15所述的装置，其特征在于，所述控制模块还用于，基于畸变校正后的三维点云数据，确定所述机器人在当前位置的激光测量距离；若所述机器人在当前位置的激光测量距离小于第二距离阈值，则控制所述机器人降低移动速度。
Priority Applications (2)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111168188.7A CN113587930B (zh) | 2021-10-08 | 2021-10-08 | 基于多传感融合的自主移动机器人室内外导航方法及装置 |
| US17/569,949 US20230116869A1 (en) | 2021-10-08 | 2022-01-06 | Multi-sensor-fusion-based autonomous mobile robot indoor and outdoor navigation method and robot |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111168188.7A CN113587930B (zh) | 2021-10-08 | 2021-10-08 | 基于多传感融合的自主移动机器人室内外导航方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113587930A CN113587930A (zh) | 2021-11-02 |
| CN113587930B true CN113587930B (zh) | 2022-04-05 |
Family
ID=78242912
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111168188.7A Active CN113587930B (zh) | 2021-10-08 | 2021-10-08 | 基于多传感融合的自主移动机器人室内外导航方法及装置 |
Country Status (2)
| Country | Link |
|---|---|
| US (1) | US20230116869A1 (zh) |
| CN (1) | CN113587930B (zh) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115115702A (zh) * | 2022-05-24 | 2022-09-27 | 广东人工智能与先进计算研究院 | 自主定位方法、装置、设备及计算机可读存储介质 |
| CN115236714A (zh) * | 2022-05-24 | 2022-10-25 | 芯跳科技(广州)有限公司 | 多源数据融合定位方法、装置、设备及计算机存储介质 |
| CN116878488B (zh) * | 2023-09-07 | 2023-11-28 | 湘江实验室 | 一种建图方法、装置、存储介质及电子装置 |
| CN117761717B (zh) * | 2024-02-21 | 2024-05-07 | 天津大学四川创新研究院 | 一种自动回环三维重建系统及运行方法 |
Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106918830A (zh) * | 2017-03-23 | 2017-07-04 | 安科机器人有限公司 | 一种基于多导航模块的定位方法及移动机器人 |
| CN110001637A (zh) * | 2019-04-10 | 2019-07-12 | 吉林大学 | 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法 |
| CN111239758A (zh) * | 2020-03-13 | 2020-06-05 | 苏州天准科技股份有限公司 | 地图采集设备 |
| CN112070800A (zh) * | 2020-07-23 | 2020-12-11 | 武汉理工大学 | 一种基于三维点云极化地图表征的智能车定位方法及系统 |
| CN112985401A (zh) * | 2021-02-02 | 2021-06-18 | 上海华测导航技术股份有限公司 | 一种headline全路径规划和跟踪方法 |
| CN113219440A (zh) * | 2021-04-22 | 2021-08-06 | 电子科技大学 | 一种基于轮式里程计的激光雷达点云数据校正方法 |
Family Cites Families (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9014848B2 (en) * | 2010-05-20 | 2015-04-21 | Irobot Corporation | Mobile robot system |
| CN104143194B (zh) * | 2014-08-20 | 2017-09-08 | 清华大学 | 一种点云分割方法及装置 |
| CN112381919B (zh) * | 2019-07-29 | 2022-09-27 | 浙江商汤科技开发有限公司 | 信息处理方法、定位方法及装置、电子设备和存储介质 |
-
2021
- 2021-10-08 CN CN202111168188.7A patent/CN113587930B/zh active Active
-
2022
- 2022-01-06 US US17/569,949 patent/US20230116869A1/en active Pending
Patent Citations (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106918830A (zh) * | 2017-03-23 | 2017-07-04 | 安科机器人有限公司 | 一种基于多导航模块的定位方法及移动机器人 |
| CN110001637A (zh) * | 2019-04-10 | 2019-07-12 | 吉林大学 | 一种基于多点跟踪的无人驾驶汽车路径跟踪控制装置及控制方法 |
| CN111239758A (zh) * | 2020-03-13 | 2020-06-05 | 苏州天准科技股份有限公司 | 地图采集设备 |
| CN112070800A (zh) * | 2020-07-23 | 2020-12-11 | 武汉理工大学 | 一种基于三维点云极化地图表征的智能车定位方法及系统 |
| CN112985401A (zh) * | 2021-02-02 | 2021-06-18 | 上海华测导航技术股份有限公司 | 一种headline全路径规划和跟踪方法 |
| CN113219440A (zh) * | 2021-04-22 | 2021-08-06 | 电子科技大学 | 一种基于轮式里程计的激光雷达点云数据校正方法 |
Non-Patent Citations (1)
| Title |
|---|
| 激光雷达惯导耦合的里程计与建图方法研究;庞帆;《中国优秀硕士学位论文全文数据库信息科技辑》;20200815(第8期);摘要，正文第2，9-29，41行及图3-1，3-2 * |
Also Published As
| Publication number | Publication date |
|---|---|
| US20230116869A1 (en) | 2023-04-13 |
| CN113587930A (zh) | 2021-11-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113587930B (zh) | 基于多传感融合的自主移动机器人室内外导航方法及装置 | |
| Zou et al. | A comparative analysis of LiDAR SLAM-based indoor navigation for autonomous vehicles | |
| JP6827627B2 (ja) | ビークル環境マップを生成および更新するための方法およびシステム | |
| CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
| Zhang et al. | 2d lidar-based slam and path planning for indoor rescue using mobile robots | |
| JP7263630B2 (ja) | 無人航空機による３次元再構成の実行 | |
| JP5930346B2 (ja) | 自律移動システムおよび管制装置 | |
| US20200117220A1 (en) | Swarm path planner system for vehicles | |
| CN112183133B (zh) | 一种基于ArUco码引导的移动机器人自主充电方法 | |
| CN113607166B (zh) | 基于多传感融合的自主移动机器人室内外定位方法及装置 | |
| CN104764457A (zh) | 一种用于无人车的城市环境构图方法 | |
| CN111982114B (zh) | 一种采用imu数据融合估计三维位姿的救援机器人 | |
| CN112740274A (zh) | 在机器人设备上使用光流传感器进行vslam比例估计的系统和方法 | |
| JP2022027593A (ja) | 可移動機器の測位方法、装置及び可移動機器 | |
| Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
| Hoang et al. | 3D motion estimation based on pitch and azimuth from respective camera and laser rangefinder sensing | |
| JP7032062B2 (ja) | 点群データ処理装置、移動ロボット、移動ロボットシステム、および点群データ処理方法 | |
| Tuvshinjargal et al. | Hybrid motion planning method for autonomous robots using kinect based sensor fusion and virtual plane approach in dynamic environments | |
| US11561553B1 (en) | System and method of providing a multi-modal localization for an object | |
| CN116466712A (zh) | 基于cml-avg激光雷达小车的自主巡航方法及系统 | |
| Pereira et al. | Backward motion for estimation enhancement in sparse visual odometry | |
| Shioya et al. | Minimal Autonomous Mover-MG-11 for Tsukuba Challenge– | |
| JP6949417B1 (ja) | 車両操縦システムと車両操縦方法 | |
| CN113030960A (zh) | 一种基于单目视觉slam的车辆定位方法 | |
| Sun et al. | Detection and state estimation of moving objects on a moving base for indoor navigation |
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 | ||
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20220726 Address after: 528200 room 2203, 22 / F, building 13, Tian'an center, No. 31, Jihua East Road, Guicheng Street, Nanhai District, Foshan City, Guangdong Province (residence declaration) Patentee after: Jiutian Industrial Research (Foshan) equity investment partnership (L.P.) Address before: 510030 room 612, building 13, compound 100, Xianlie Middle Road, Yuexiu District, Guangzhou, Guangdong Patentee before: Institute of intelligent manufacturing, Guangdong Academy of Sciences |
|
| TR01 | Transfer of patent right | ||
| TR01 | Transfer of patent right |
Effective date of registration: 20221010 Address after: 528200 Room 2208, 22/F, Block 13, Tian'an Center, No. 31, Jihua East Road, Guicheng Street, Nanhai District, Foshan City, Guangdong Province (residence declaration) Patentee after: Jiutian innovation (Guangdong) Intelligent Technology Co.,Ltd. Address before: 528200 room 2203, 22 / F, building 13, Tian'an center, No. 31, Jihua East Road, Guicheng Street, Nanhai District, Foshan City, Guangdong Province (residence declaration) Patentee before: Jiutian Industrial Research (Foshan) equity investment partnership (L.P.) |