CN113483762A - 一种位姿优化方法及设备 - Google Patents

一种位姿优化方法及设备 Download PDF

Info

Publication number
CN113483762A
CN113483762A CN202110755390.3A CN202110755390A CN113483762A CN 113483762 A CN113483762 A CN 113483762A CN 202110755390 A CN202110755390 A CN 202110755390A CN 113483762 A CN113483762 A CN 113483762A
Authority
CN
China
Prior art keywords
pose
weighting factor
odometer
preliminary
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.)
Pending
Application number
CN202110755390.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.)
Henan University of Technology
Original Assignee
Henan University of Technology
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 Henan University of Technology filed Critical Henan University of Technology
Priority to CN202110755390.3A priority Critical patent/CN113483762A/zh
Publication of CN113483762A publication Critical patent/CN113483762A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • 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
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers

Landscapes

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

Abstract

本申请的目的是提供一种位姿优化方法及设备，本申请利用扩展卡尔曼融合算法和激光里程计方法分别进行机器人姿态估计、利用自适应加权和卡尔曼增益思想的修正算法，通过分析调整最优加权因子和局部动态加权因子，构建修正算法对机器人初步位姿进行修正，得到机器人准确位姿即最优位姿，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。

Description

一种位姿优化方法及设备
技术领域
本申请涉及计算机领域，尤其涉及一种位姿优化方法及设备。
背景技术
随着移动机器人被要求在复杂多变的环境下执行越来越严峻的任务，机器人实现对自身位姿的精确估计就显着尤为重要。目前对于在室内的未知条件下机器人位姿累计误差较大，单一传感器精度不够的问题，大多数采取的方法是加入视觉相机和激光雷达等传感器来进一步估计位姿。然而，视觉相机易受光照条件限制，不利于保证机器人位姿估计的精确度。虽然激光雷光的测量精度很高，并且可以在无光环境工作，但因激光数据是对外部环境的观测，所以激光雷达构建的激光里程计极其容易受环境影响，故需要结合其他传感器实现对机器人位姿精确估计。当使用不同传感器在室内未知环境下定位时，仅使用单一的位姿估计方法会造成鲁棒性低的问题。
因此，实现对机器人位姿进行修正，得到更精确的机器人位姿信息，减少位姿累计误差，提高姿态估计鲁棒性是本领域需要继续研究的方向。
发明内容
本申请的一个目的是提供一种位姿优化方法及设备，以解决现有技术中环卫车如何减少位姿累计误差，提高姿态估计精确度和鲁棒性的问题。
根据本申请的一个方面，提供了一种位姿优化方法，包括：
获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；
基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；
计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述位姿偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿。
进一步地，所述位姿优化方法还包括：
若所述偏差值小于等于所述位姿偏差阈值，则计算所述激光里程计位姿和所述初步位姿的位姿平均值作为所述最优位姿。
进一步地，上述位姿优化方法中，所述基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，包括：
分别计算所述激光里程计位姿和所述初步位姿的方差，并结合多元函数极值方法得到激光里程计位姿整体最优加权因子和初步位姿整体最优加权因子；
分别计算所述激光里程计位姿和所述初步位姿与上一时刻修正后位姿的偏差信息，基于所述偏差信息通过平方差计算方法得到激光里程计位姿局部动态权重因子和初步位姿局部动态权重因子；
将所述激光里程计位姿整体最优加权因子、所述初步位姿整体最优加权因子、所述激光里程计位姿局部动态权重因子、所述初步位姿局部动态权重因子进行融合得到修正权重因子，通过所述修正权重因子进行当前时刻的位姿矫正得到最优位姿。
进一步地，上述位姿优化方法中，所述获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿，包括：
选择PL-ICP的点云匹配的方法构建激光里程计，并计算得到所述激光里程计位姿。
进一步地，上述位姿优化方法中，所述基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿，包括：
惯性传感器测量得到机器人角速度和加速度后，通过Mahony姿态结算算法计算得到姿态结算位姿数据；
通过轮式里程计方法得到轮式里程计位姿数据；
利用所述扩展卡尔曼滤波将所述姿态结算位姿数据和所述轮式里程计位姿数据进行融合得到所述初步位姿。
根据本申请的另一方面，还提供了一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行时，使所述处理器实现如上述任一项所述的方法。
根据本申请的另一方面，还提供了一种位姿优化设备，该设备包括：
一个或多个处理器；
计算机可读介质，用于存储一个或多个计算机可读指令，
当所述一个或多个计算机可读指令被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述中任一项所述的方法。
与现有技术相比，本申请通过获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，即提出一种扩展卡尔曼融合算法和激光里程计方法分别进行机器人姿态估计、利用自适应加权和卡尔曼增益思想的修正算法，通过分析调整最优加权因子和局部动态加权因子，构建修正算法对机器人初步位姿进行修正，得到机器人准确位姿即最优位姿，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。
附图说明
通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：
图1示出根据本申请一个方面的一种位姿优化方法的流程示意图；
图2示出根据本申请一个方面的一种位姿方法中融合结构流程示意图。
附图中相同或相似的附图标记代表相同或相似的部件。
具体实施方式
下面结合附图对本申请作进一步详细描述。
在本申请一个典型的配置中，终端、服务网络的设备和可信方均包括一个或多个处理器(例如中央处理器(Central Processing Unit，CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RandomAccess Memory，RAM)和/或非易失性内存等形式，如只读存储器(Read Only Memory，ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(Phase-Change RAM，PRAM)、静态随机存取存储器(Static Random Access Memory，SRAM)、动态随机存取存储器(Dynamic RandomAccess Memory，DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory，EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(Compact Disc Read-Only Memory，CD-ROM)、数字多功能光盘(Digital Versatile Disk，DVD)或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括非暂存电脑可读媒体(transitorymedia)，如调制的数据信号和载波。
图1示出根据本申请一个方面的一种位姿优化方法的流程示意图，该方法适用于移动机器人的任意运动场景，该方法包括步骤S11、步骤S12及步骤S13，其中，具体包括：
获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；在此，激光雷达不受光照影响，在光照不稳定下鲁棒性比较高，定位比较准确。
基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；在此，卡尔曼滤波是常见的多传感器融合算法，通过机器人当前状态以及前一时刻状态进行估计运算，不断迭代，最终估计机器人的最优状态。在机器人移动的过程中，机器人的运动是非线性的，本文选择扩展卡尔曼滤波对机器人进行融合处理。扩展卡尔曼滤波是对卡尔曼滤波的非线性描述，通过泰勒公式把非线性转换为线性，继而进行预测估计。
计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿。
上述步骤S11至步骤S13，首先，获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；然后，基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；最后，计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，即提出一种扩展卡尔曼融合算法和激光里程计方法分别进行机器人姿态估计、利用自适应加权和卡尔曼增益思想的修正算法，通过分析调整最优加权因子和局部动态加权因子，构建修正算法对机器人初步位姿进行修正，得到机器人准确位姿即最优位姿，不仅利用多种传感器进行位姿检测获取位姿信息，同时还融合多种位姿估计算法进行融合矫正，实现了机器人位姿精确估计，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。
例如，在幻宇移动机器人平台进行机器人位姿优化，搭配的雷达为YDLIDAR G4，激光雷达扫描角度为360度，最小角度分辨率0.26度，扫描频率10HZ，测距半径为0.10-16m。获取机器人当前状态信息a1、a2、a3...an，并利用激光里程计方法得到激光里程计位姿
Figure BDA0003147107310000061
然后，基于所述当前状态信息a1、a2、a3...an，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿
Figure BDA0003147107310000062
然后，计算所述激光里程计位姿和所述初步位姿的位姿偏差值。当所述激光里程计位姿和所述初步位姿之间的偏差值k大于位姿偏差阈值α即k＞α时，基于所述激光里程计位姿
Figure BDA0003147107310000063
和所述初步位姿
Figure BDA0003147107310000064
计算得到激光里程计位姿
Figure BDA0003147107310000065
和初步位姿
Figure BDA0003147107310000066
整体最优加权因子μ。整体最优加权因子μ包括激光里程计位姿最优加权因子μ2和初步位姿最优加权因子μ1。基于所述激光里程计位姿
Figure BDA0003147107310000067
和所述初步位姿
Figure BDA0003147107310000068
计算得到初步位姿局部动态加权因子λ1、激光里程计位姿局部动态加权因子λ2，结合整体和局部不同的权重因子调整所述机器人位姿累计误差，最后得到最优位姿
Figure BDA0003147107310000069
即利用扩展卡尔曼融合算法和激光里程计方法分别进行机器人姿态估计、利用自适应加权和卡尔曼增益思想的修正算法，通过分析调整最优加权因子和局部动态加权因子，构建修正算法对机器人初步位姿进行修正，得到机器人准确位姿即最优位姿，不仅利用多种传感器进行位姿检测获取位姿信息，同时还融合多种位姿估计算法进行修正，实现了机器人位姿精确估计，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。
本申请的又一实施例，上述方法还包括：
若所述偏差值小于等于所述位姿偏差阈值即数据匹配成功，则计算所述激光里程计位姿和所述初步位姿的位姿平均值作为所述最优位姿。在此，数据匹配成功后修正公式如下：
Figure BDA0003147107310000071
接着本申请的上述实施例，所述步骤S13中基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，包括：
步骤S131，分别计算所述激光里程计位姿和所述初步位姿的方差，并结合多元函数极值方法得到激光里程计位姿整体最优加权因子和初步位姿整体最优加权因子。在此，分别把初步位姿和激光里程计位姿看作两种不同传感器的测量数据，利用多次测量同一段轨迹的方法计算两者的总方差。利用自适应融合思想，为保证得到整体最优加权因子，需使得初步位姿和激光里程计总均方误差最小，所以结合多元函数求极值理论进行计算。此时对应加权因子μ为：
Figure BDA0003147107310000072
权重因子满足：
Figure BDA0003147107310000073
步骤S132，分别计算所述激光里程计位姿和所述初步位姿与上一时刻修正后位姿的偏差信息，基于所述偏差信息通过平方差计算方法得到激光里程计位姿局部动态权重因子和初步位姿局部动态权重因子。通常所述激光里程计位姿和所述初步位姿相对于上一时刻位姿会有很大偏差，基于上一时刻融合后位姿来计算局部加权因子，能避免将错误的信息带入下一时刻累积的位姿，减少累积误差。
步骤S133，将所述激光里程计位姿整体最优加权因子、所述初步位姿整体最优加权因子、所述激光里程计位姿局部动态权重因子、所述初步位姿局部动态权重因子进行融合得到修正权重因子，通过所述修正权重因子进行当前时刻的位姿矫正得到最优位姿。在此，修正加权因子w的公式如下：
Figure BDA0003147107310000081
w1表示初步位姿修正权重因子，w2表示激光里程计位姿修正权重因子。得到所述最优位姿的具体公式如下：
Figure BDA0003147107310000082
该方法通过分析两者位姿总均方差以及前后时刻位姿误差，调整最优加权因子和局部动态加权因子，从而实现累积误差自适应校正，得到机器人更精确位姿估计。
例如，首先，分别把初步位姿和激光里程计位姿看作两种不同传感器的测量数据，利用多次测量同一段轨迹的方法计算两者的总方差，记为
Figure BDA0003147107310000083
Figure BDA0003147107310000084
结合多元函数求极值，保证初步位姿
Figure BDA0003147107310000085
和激光里程计
Figure BDA0003147107310000086
总均方误差最小，计算得到激光里程计位姿整体最优加权因子μ2和初步位姿整体最优加权因子μ1。接着，分别计算初步位姿
Figure BDA0003147107310000087
和激光里程计位姿
Figure BDA0003147107310000088
与上一时刻修正后位姿
Figure BDA0003147107310000089
的偏差信息，取偏差信息平方差的方法对局部进行权重因子计算，来进一步估计位姿：
Figure BDA00031471073100000810
Figure BDA00031471073100000811
Figure BDA00031471073100000812
为前一帧修正后位姿，δ1、δ2分别为两者偏差。基于此计算局部动态权重因子λ：
Figure BDA0003147107310000091
最后，将所述激光里程计位姿整体最优加权因子μ2、所述初步位姿整体最优加权因子μ1、所述激光里程计位姿局部动态权重因子λ2、所述初步位姿局部动态权重因子λ1进行融合得到修正权重因子w1和w2，通过所述修正权重因子进行当前时刻的位姿矫正得到最优位姿
Figure BDA0003147107310000092
实现了机器人位姿精确估计，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。轮式里程计频率高，局部精度高，激光里程计绝对精度高，但计算时间长频率低。无论是单独扩展卡尔曼融合后的初步位姿
Figure BDA0003147107310000093
还是PL-ICP算法构建激光里程计的位姿
Figure BDA0003147107310000094
都会随时间的延长产生累计误差导致机器人位姿精度低，一般为了解决激光里程计出现累计误差，会使用回环优化的环节消除，但回环检测比较繁琐。在发明通过分析两者位姿总均方差以及前后时刻位姿误差确定所述整体最优加权因子和所述局部动态权重因子，最终得到修正权重因子，来融合
Figure BDA0003147107310000095
以及
Figure BDA0003147107310000096
位姿的策略来降低累计误差。
接着本申请的上述实施例，所述步骤S11获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿，包括：
选择PL-ICP的点云匹配的方法构建激光里程计，并计算得到所述激光里程计位姿。在此，选定PL-ICP的点云匹配的方法来构建激光里程计，PL-ICP相比于ICP和NICP等算法，不同点在于利用点到线之间的距离寻找最近邻点，使得匹配精度得到了明显的提高，且原理简单，容易实现。其数学描述为：
Figure BDA0003147107310000097
sref是参考平面，ni T表示参考表面法向量，pi是点集，
Figure BDA0003147107310000098
是旋转平移运算符，k为迭代次数，i表示帧数，qk+1表示坐标变换。
Figure BDA0003147107310000099
是t+1时刻经过旋转矩阵R和平移矩阵T变化到t时刻的当前帧，
Figure BDA0003147107310000101
表示前一时刻参考帧在sref面的投影。
PL-ICP匹配算法流程：
当前激光帧的数据转换到参考帧坐标系下。
当前激光帧中的每一个点，找到其最近的两个点
Figure BDA0003147107310000102
和
Figure BDA0003147107310000103
计算误差，去除误差过大的点。
构建最小误差方程：
Figure BDA0003147107310000104
求解变换矩阵qk+1，用于下次迭代累加计算。
接着本申请的上述实施例，所述步骤S12基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿，包括：
惯性传感器测量得到机器人角速度和加速度后，通过Mahony姿态结算算法计算得到姿态结算位姿数据；
通过轮式里程计方法得到轮式里程计位姿数据；
利用所述扩展卡尔曼滤波将所述姿态结算位姿数据和所述轮式里程计位姿数据进行融合得到所述初步位姿。
在此，惯性传感器(Inertial Measurement Unit，IMU)一般有三轴陀螺仪、三轴加速度计组成，分别测量机器人角速度和加速度。陀螺仪是通过积分得到角度值，这就会使得误差大量累计。本方法利用Mahony算法对IMU姿态解算，Mahony滤波器的基本思想是通过融合陀螺仪积分的旋转角度和加速度计测量的加速度值来估计姿态。基于差分地盘模型构建轮式里程计，但因在实际应用中受到地面影响，轮子会发生形变、侧滑等使轮式里程计的精度降低，在机器人长距离运动时误差逐步扩大。故该方法通过融合轮式里程计和IMU的方法来实现机器人初步位姿预测，如图2所示。
根据本申请的另一个方面，还提供了一种位姿优化设备，其特征在于，该设备包括：
一个或多个处理器；
计算机可读介质，用于存储一个或多个计算机可读指令，
当所述一个或多个计算机可读指令被所述一个或多个处理器执行，使得所述一个或多个处理器实现如上述在设备上控制用户对垒方法。
在此，所述设备的各实施例的详细内容，具体可参见上述设备端的控制用户对垒方法实施例的对应部分，在此，不再赘述。
综上所述，本申请通过获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，即提出一种扩展卡尔曼融合算法和激光里程计方法分别进行机器人姿态估计、利用自适应加权和卡尔曼增益思想的修正算法，通过分析调整最优加权因子和局部动态加权因子，构建修正算法对机器人初步位姿进行修正，得到机器人准确位姿即最优位姿，不仅利用多种传感器进行位姿检测获取位姿信息，同时还融合多种位姿估计算法进行融合矫正，实现了机器人位姿精确估计，减少了位姿累计误差，提高了姿态估计准确度和鲁棒性。
需要注意的是，本申请可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路(ASIC)、通用目的计算机或任何其他类似硬件设备来实现。在一个实施例中，本申请的软件程序可以通过处理器执行以实现上文所述步骤或功能。同样地，本申请的软件程序(包括相关的数据结构)可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本申请的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。
另外，本申请的一部分可被应用为计算机程序产品，例如计算机程序指令，当其被计算机执行时，通过该计算机的操作，可以调用或提供根据本申请的方法和/或技术方案。而调用本申请的方法的程序指令，可能被存储在固定的或可移动的记录介质中，和/或通过广播或其他信号承载媒体中的数据流而被传输，和/或被存储在根据所述程序指令运行的计算机设备的工作存储器中。在此，根据本申请的一个实施例包括一个装置，该装置包括用于存储计算机程序指令的存储器和用于执行程序指令的处理器，其中，当该计算机程序指令被该处理器执行时，触发该装置运行基于前述根据本申请的多个实施例的方法和/或技术方案。
对于本领域技术人员而言，显然本申请不限于上述示范性实施例的细节，而且在不背离本申请的精神或基本特征的情况下，能够以其他的具体形式实现本申请。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本申请的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。

Claims (7)

1.一种位姿优化方法，其特征在于，所述方法包括：
获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿；
基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿；
计算所述激光里程计位姿和所述初步位姿的位姿偏差值，若所述位姿偏差值大于位姿偏差阈值，则基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿。
2.根据权利要求1所述的方法，其特征在于，所述位姿优化方法还包括：
若所述偏差值小于等于所述位姿偏差阈值，则计算所述激光里程计位姿和所述初步位姿的位姿平均值作为所述最优位姿。
3.根据权利要求2所述的方法，其特征在于，所述基于所述激光里程计位姿和所述初步位姿，计算整体最优加权因子和局部动态权重因子，调整所述机器人位姿累计误差，得到最优位姿，包括：
分别计算所述激光里程计位姿和所述初步位姿的方差，并结合多元函数极值方法得到激光里程计位姿整体最优加权因子和初步位姿整体最优加权因子；
分别计算所述激光里程计位姿和所述初步位姿与上一时刻修正后位姿的偏差信息，基于所述偏差信息通过平方差计算方法得到激光里程计位姿局部动态权重因子和初步位姿局部动态权重因子；
将所述激光里程计位姿整体最优加权因子、所述初步位姿整体最优加权因子、所述激光里程计位姿局部动态权重因子、所述初步位姿局部动态权重因子进行融合得到修正权重因子，通过所述修正权重因子进行当前时刻的位姿矫正得到最优位姿。
4.根据权利要求1所述的方法，其特征在于，所述获取机器人当前状态信息，并利用激光里程计方法得到激光里程计位姿，包括：
选择PL-ICP的点云匹配的方法构建激光里程计，并计算得到所述激光里程计位姿。
5.根据权利要求1所述的方法，其特征在于，所述基于所述当前状态信息，利用扩展卡尔曼滤波融合通过轮式里程计方法和姿态解算方法得到的位姿数据，得到初步位姿，包括：
惯性传感器测量得到机器人角速度和加速度后，通过Mahony姿态结算算法计算得到姿态结算位姿数据；
通过轮式里程计方法得到轮式里程计位姿数据；
利用所述扩展卡尔曼滤波将所述姿态结算位姿数据和所述轮式里程计位姿数据进行融合得到所述初步位姿。
6.一种计算机可读介质，其上存储有计算机可读指令，所述计算机可读指令可被处理器执行时，使所述处理器实现如权利要求1至5中任一项所述的方法。
7.一种位姿优化设备，其特征在于，该设备包括：
一个或多个处理器；
计算机可读介质，用于存储一个或多个计算机可读指令，
当所述一个或多个计算机可读指令被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1至5中任一项所述的方法。
CN202110755390.3A 2021-07-05 2021-07-05 一种位姿优化方法及设备 Pending CN113483762A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110755390.3A CN113483762A (zh) 2021-07-05 2021-07-05 一种位姿优化方法及设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110755390.3A CN113483762A (zh) 2021-07-05 2021-07-05 一种位姿优化方法及设备

Publications (1)

Publication Number Publication Date
CN113483762A true CN113483762A (zh) 2021-10-08

Family

ID=77939935

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110755390.3A Pending CN113483762A (zh) 2021-07-05 2021-07-05 一种位姿优化方法及设备

Country Status (1)

Country Link
CN (1) CN113483762A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114663496A (zh) * 2022-03-23 2022-06-24 北京科技大学 一种基于卡尔曼位姿估计网络的单目视觉里程计方法

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108036792A (zh) * 2017-12-11 2018-05-15 苏州中德睿博智能科技有限公司 一种用于移动机器人的里程计与测量位姿的数据融合方法
CN110263675A (zh) * 2019-06-03 2019-09-20 武汉联一合立技术有限公司 一种社区安保机器人的垃圾目标识别***及其识别方法
CN111121768A (zh) * 2019-12-23 2020-05-08 深圳市优必选科技股份有限公司 一种机器人位姿估计方法、装置、可读存储介质及机器人
CN111521195A (zh) * 2020-04-10 2020-08-11 广州铁路职业技术学院（广州铁路机械学校） 一种智能机器人
CN112014849A (zh) * 2020-07-15 2020-12-01 广东工业大学 一种基于传感器信息融合的无人车定位修正方法
CN112729321A (zh) * 2020-12-28 2021-04-30 上海有个机器人有限公司 一种机器人的扫图方法、装置、存储介质和机器人

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108036792A (zh) * 2017-12-11 2018-05-15 苏州中德睿博智能科技有限公司 一种用于移动机器人的里程计与测量位姿的数据融合方法
CN110263675A (zh) * 2019-06-03 2019-09-20 武汉联一合立技术有限公司 一种社区安保机器人的垃圾目标识别***及其识别方法
CN111121768A (zh) * 2019-12-23 2020-05-08 深圳市优必选科技股份有限公司 一种机器人位姿估计方法、装置、可读存储介质及机器人
CN111521195A (zh) * 2020-04-10 2020-08-11 广州铁路职业技术学院（广州铁路机械学校） 一种智能机器人
CN112014849A (zh) * 2020-07-15 2020-12-01 广东工业大学 一种基于传感器信息融合的无人车定位修正方法
CN112729321A (zh) * 2020-12-28 2021-04-30 上海有个机器人有限公司 一种机器人的扫图方法、装置、存储介质和机器人

Non-Patent Citations (7)

* Cited by examiner, † Cited by third party
Title
周阳: "基于多传感器融合的移动机器人SLAM算法研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 9, 15 September 2019 (2019-09-15), pages 1 *
姜祚鹏 等: "一种基于PL-ICP 及NDT 点云匹配的单线激光里程计", 激光杂志, vol. 41, no. 3, pages 21 - 24 *
姜祚鹏 等: "一种基于PL-ICP及NDT点云匹配的单线激光里程计", 激光杂志, vol. 41, no. 3, pages 21 - 24 *
孙安顺: "多传感器融合的移动机器人室内定位***研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 6, pages 24 - 37 *
郭超: "基于激光雷达的室内移动机器人导航研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 2, pages 17 - 28 *
金鸥翔: "基于激光雷达的移动机器人同步定位与建图算法研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 6, 15 June 2021 (2021-06-15), pages 1 *
靳东: "室内复杂环境下移动机器人激光视觉融合SLAM 及导航研究", 中国优秀硕士学位论文全文数据库信息科技辑, no. 1, pages 25 - 36 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114663496A (zh) * 2022-03-23 2022-06-24 北京科技大学 一种基于卡尔曼位姿估计网络的单目视觉里程计方法

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及***
US7747348B2 (en) Method and apparatus for using rotational movement amount of mobile device and computer-readable recording medium for storing computer program
CN113074739A (zh) 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法
CN112987728A (zh) 一种机器人的环境地图更新方法、***、设备及存储介质
KR101909953B1 (ko) 라이다 센서를 이용한 차량의 자세 추정 방법
CN108332752B (zh) 机器人室内定位的方法及装置
CN112229405B (zh) 一种基于图像跟踪与激光测距的无人机目标运动估计方法
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
US20130131984A1 (en) Rapid lidar image correlation for ground navigation
CN106370178B (zh) 移动终端设备的姿态测量方法及装置
CN113763548B (zh) 基于视觉-激光雷达耦合的贫纹理隧洞建模方法及***
CN115200572B (zh) 三维点云地图构建方法、装置、电子设备及存储介质
CN114179825A (zh) 多传感器融合获取量测值置信度方法及自动驾驶车辆
CN114429432B (zh) 一种多源信息分层融合方法、装置及存储介质
CN117824667B (zh) 一种基于二维码和激光的融合定位方法及介质
CN113483762A (zh) 一种位姿优化方法及设备
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam***
CN116429090A (zh) 基于线激光的同步定位和建图方法、装置、移动机器人
US20240134033A1 (en) Method for determining a movement state of a rigid body
CN116203579A (zh) 基于点云残差的激光雷达slam退化检测方法及***
CN114019954B (zh) 航向安装角标定方法、装置、计算机设备和存储介质
CN115540854A (zh) 一种基于uwb辅助的主动定位方法、设备和介质
CN115727871A (zh) 一种轨迹质量检测方法、装置、电子设备和存储介质
CN112987010B (zh) 一种用于机器人的多雷达建图的***及方法
CN114061573A (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