CN116753963B - 一种基于信赖域算法的室内移动机器人定位方法 - Google Patents

一种基于信赖域算法的室内移动机器人定位方法 Download PDF

Info

Publication number
CN116753963B
CN116753963B CN202311032737.7A CN202311032737A CN116753963B CN 116753963 B CN116753963 B CN 116753963B CN 202311032737 A CN202311032737 A CN 202311032737A CN 116753963 B CN116753963 B CN 116753963B
Authority
CN
China
Prior art keywords
mobile robot
optimal
odometer
coordinates
value
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
CN202311032737.7A
Other languages
English (en)
Other versions
CN116753963A (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.)
Hunan University
Original Assignee
Hunan University
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 Hunan University filed Critical Hunan University
Priority to CN202311032737.7A priority Critical patent/CN116753963B/zh
Publication of CN116753963A publication Critical patent/CN116753963A/zh
Application granted granted Critical
Publication of CN116753963B publication Critical patent/CN116753963B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
    • 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
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G01S5/0268—Hybrid positioning by deriving positions from different combinations of signals or of estimated positions in a single positioning system
    • G01S5/02685—Hybrid positioning by deriving positions from different combinations of signals or of estimated positions in a single positioning system involving dead reckoning based on radio wave measurements
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G06F17/12—Simultaneous equations, e.g. systems of linear equations
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/18—Complex mathematical operations for evaluating statistical data, e.g. average values, frequency distributions, probability functions, regression analysis
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/029—Location-based management or tracking services
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开了一种基于信赖域算法的室内移动机器人定位方法，包括：在初始化阶段，先通过多组里程计数据和UWB测距值构建最小二乘优化问题及对应的误差函数；再通过图优化的方法求解最小二乘优化问题，得出移动机器人当前时刻在全局坐标系下的最优坐标，作为后续跟踪步骤的初值；在跟踪阶段，通过里程计增量数据和最优坐标对UWB的测距值进行卡尔曼滤波预处理，获取最优距离值；结合最优距离值通过信赖域算法求解移动机器人的最优坐标，实现对机器人的持续鲁棒跟踪。通过图优化的方法求解移动机器人的初始全局坐标，提高了定位系统的初值精度，通过卡尔曼滤波和信赖域算法，提高了移动机器人在全局坐标下定位的精度，提高了定位系统的可用性。

Description

一种基于信赖域算法的室内移动机器人定位方法
技术领域
本发明属于室内环境下融合定位技术领域，特别是涉及一种基于信赖域算法的室内移动机器人定位方法。
背景技术
近年来，移动机器人应用快速发展，例如送餐机器人、巡检机器人及扫地机器人等。这些移动机器人都需要实现定点导航功能，因此定位系统在这些应用中扮演着重要的角色。在室外可以使用全球定位系统（Global Positioning System ,GPS）实现高精度、稳定可靠的定位，而要在室内环境实现高精度的定位仍是一项具有挑战的任务。当前稳定成熟的室内定位系统技术主要包括：无线局域网定位、射频识别定位、超声波定位、蓝牙定位、惯性定位以及超宽带（Ultra Wideband, UWB）定位。这些技术主要利用指纹、三角法和航位推测法进行位置估计，但是基于惯性传感器的里程计定位系统存在传感器漂移，所产生的累计误差将导致无法提供长时间的定位；基于射频或者UWB的定位系统存在多径效应和非视距等因素的影响，导致定位误差增大。因此将UWB与里程计进行融合可实现高精度的稳定定位。
为了实现低成本的UWB定位系统，有学者提出了一种基于粒子滤波的低成本UWB定位方法，仅使用一个UWB基站就可确定并跟踪目标位置。与传统的多基站定位系统相比，当只使用稀疏分布的UWB基站进行距离测量时，可能会更频繁地遇到失轨问题，针对粒子丢失的问题，有学者通过引入粒子重启，将粒子重置在可能位置的矩形区域中，解决了失轨问题。尽管如此，基于滤波算法的UWB定位系统仍难以实现高精度的持续跟踪定位。
针对仅有单基站的UWB定位系统，本发明提出了一种基于信赖域算法的室内移动机器人定位方法，可实现对移动机器人的高精度持续跟踪定位。
发明内容
针对以上技术问题，本发明提供一种基于信赖域算法的室内移动机器人定位方法。
本发明解决其技术问题采用的技术方案是：
一种基于信赖域算法的室内移动机器人定位方法，方法包括以下步骤：
S100：通过多组里程计数据和UWB测距值构建最小二乘优化问题及对应的误差函数；
S200：通过图优化的方法求解最小二乘优化问题，得出移动机器人当前时刻在全局坐标系下的最优坐标，并将最优坐标作为后续跟踪步骤的初值；
S300：通过里程计增量数据和最优坐标对UWB测距值进行卡尔曼滤波预处理，获取最优距离值；
S400：结合最优距离值通过信赖域算法求解移动机器人的最优坐标，实现对移动机器人的持续鲁棒跟踪；
S100包括：
S110：定义全局坐标系的原点为基站位置，里程计坐标系原点为移动机器人上电位置；
S120：每当获取一个UWB测距值时，输出一次里程计数据；
S130：假设里程计坐标系与全局坐标系重合，在移动机器人上电时，通过人为的方式获取移动机器人在全局坐标系下的头部朝向；
S140：使移动机器人在室内环境下随机运动一定距离，判断是否存在三个不共线的里程计坐标，若存在，将/>之间的所有里程计坐标作为状态变量/>，将/>之间的所有UWB测距值作为观测变量/>；
S150：假设基站在里程计坐标系下的坐标为，该坐标即为待优化变量；
S160：状态变量到基站的距离与观测变量/>之间的差定义为误差函数：
；
存在一个最优解可满足误差函数最小，因此构建最小二乘优化问题：
；
其中表示最优解，满足误差函数的平方和最小；
S200包括：
S210：将待优化变量作为顶点，UWB的测距值作为一元边，并使用Levenberg-Marquardt方法进行求解，得到基站在里程计坐标系下的坐标值/>；
S220：将基站在里程计坐标系下的坐标值通过坐标变换公式得到移动机器人在全局坐标系下的位置：
；
其中为移动机器人在全局坐标系下的最优坐标；
S230：将当前时刻的最优坐标作为跟踪步骤的初值。
优选地，S140还包括若不存在三个不共线的里程计坐标，则移动机器人继续在室内环境下随机运动一定距离，直至存在三个不共线的里程计坐标/>。
优选地，S140中判断是否存在三个不共线的里程计坐标，具体为：
令；
当移动机器人运动距离大于时，即/>时，令；
当移动机器人运动距离再次大于，且与/>之间的夹角大于/>时，即，且时，令。
优选地，S300包括：
S310：通过里程计增量数据构建预测方程，估计出移动机器人在当前时刻的状态：
；
其中为里程计增量，/>为上一时刻输出的最优坐标，当时该坐标为初始化步骤求得的初值；
S320：估计状态与基站之间的距离即为先验估计距离/>：
；
其中为预测噪声；
S330：构建先验估计协方差：
；
其中为过程噪声协方差，/>为上一时刻输出的后验估计协方差；
S340：构建卡尔曼增益：
；
其中为测量噪声协方差；
S350：后验估计为：
；
S360：构建后验估计协方差：
；
S370：通过对预测值与观测值进行滤波，输出基站与移动机器人之间的最优距离值；其中，预测值为移动机器人在当前时刻估计状态与基站之间的距离/>，观测值为UWB的测距值。
优选地，S400包括：
S410：假设移动机器人当前时刻的坐标为，在不存在误差的情况下，估计坐标/>与基站之间的距离应等于最优距离值/>，估计坐标/>与上一时刻的最优坐标之间应满足里程计约束，由此可构建二元二次方程组：
观测方程可表示为：
；
预测方程可表示为：
；
其中，表示为移动机器人当前时刻的坐标的/>值，/>表示为移动机器人当前时刻的坐标的/>值；
S420：将预测坐标作为方程求解的迭代初值，其中，预测坐标为上一时刻的最优坐标与里程计增量的和；
S430：对二元二次方程组增加里程计约束条件：
；
S440：通过信赖域算法求解增加了里程计约束条件后的二元二次方程组，得到二元二次方程组的最优解；
S450：将二元二次方程组的最优解作为当前时刻的最优坐标，并循环持续跟踪步骤，实现对机器人的持续鲁棒跟踪。
优选地，S440包括：
S441：在二元二次方程组的预测坐标预设区域内构造一个近似模型，近似模型的边界区域称为信赖域，每次迭代调整信赖域直至收敛，则信赖域可表示为：
；
其中为第/>次迭代时的信赖域半径，用于确定信赖域的大小；/>为方程组第/>次迭代时的解；
S442：在信赖域中构造一个二次模型来近似目标函数/>，二次模型可表示为：
；
其中为极小值，/>是/>的雅可比矩阵，/>是/>的黑塞矩阵；
S443：若，即不满足收敛条件，定义信赖域的模型子问题为：
；
其中，表示子问题，/>表示实数域；
S444：求解出子问题的解后，通过信赖域保真度评估模型子问题的最优解/>；
S445：定义预测下降量为：
；
S446：定义实际下降量为：
；
S447：预测下降量与实际下降量的比值为；
S448：根据的值确定/>的值并校正信赖域半径；
；
；
其中，，当/>时，/>；
S449：令，返回S442，直至/>时结束迭代，此时满足收敛条件，得到二元二次方程组的最优解。
上述一种基于信赖域算法的室内移动机器人定位方法，仅依靠单基站可求得移动机器人的全局坐标，大大降低了定位系统的成本；通过图优化的方法对状态变量进行优化，通过卡尔曼滤波和信赖域算法，提高了所求的移动机器人在全局坐标下定位的精度，并提高了定位系统的可用性。
附图说明
图1为本发明一实施例中一种基于信赖域算法的室内移动机器人定位方法的流程图；
图2为本发明一实施例中一种基于信赖域算法的室内移动机器人定位方法的整体框架示意图；
图3为本发明一实施例中信赖域算法的求解流程示意图。
具体实施方式
为了使本技术领域的人员更好地理解本发明的技术方案，下面结合附图对本发明作进一步的详细说明。
在一个实施例中，如图1和图2所示，一种基于信赖域算法的室内移动机器人定位方法，方法包括以下步骤：
S100：通过多组里程计数据和UWB测距值构建最小二乘优化问题及对应的误差函数。
在一个实施例中，S100包括：
S110：定义全局坐标系的原点为基站位置，里程计坐标系原点为移动机器人上电位置；
S120：每当获取一个UWB测距值时，输出一次里程计数据；
S130：假设里程计坐标系与全局坐标系重合，在移动机器人上电时，通过人为的方式获取移动机器人在全局坐标系下的头部朝向；
S140：使移动机器人在室内环境下随机运动一定距离，判断是否存在三个不共线的里程计坐标，若存在，将/>之间的所有里程计坐标作为状态变量/>，将/>之间的所有UWB测距值作为观测变量/>；
S150：假设基站在里程计坐标系下的坐标为，该坐标即为待优化变量；
S160：状态变量到基站的距离与观测变量/>之间的差定义为误差函数：
；
存在一个最优解可满足误差函数最小，因此构建最小二乘优化问题：
；
其中表示最优解，满足误差函数的平方和最小。
在一个实施例中，S140还包括：若不存在三个不共线的里程计坐标，则移动机器人继续在室内环境下随机运动一定距离，直至存在三个不共线的里程计坐标。
在一个实施例中，S140中判断是否存在三个不共线的里程计坐标，具体为：
令；
当移动机器人运动距离大于时，即/>时，令；
当移动机器人运动距离再次大于，且与/>之间的夹角大于/>时，即，且时，令。
S200：通过图优化的方法求解最小二乘优化问题，得出移动机器人当前时刻在全局坐标系下的最优坐标，并将最优坐标作为后续跟踪步骤的初值。
在一个实施例中，S200包括：
S210：将待优化变量作为顶点，UWB的测距值作为一元边，并使用Levenberg-Marquardt方法进行求解，得到基站在里程计坐标系下的坐标值/>；
S220：将基站在里程计坐标系下的坐标值通过坐标变换公式得到移动机器人在全局坐标系下的位置：
；
其中为移动机器人在全局坐标系下的最优坐标；
S230：将当前时刻的最优坐标作为跟踪步骤的初值。
具体地，将基站坐标作为顶点（优化变量），UWB的测距值作为一元边（观测方程），并使用Levenberg-Marquardt方法进行求解。Levenberg-Marquardt算法是一种非线性最小二乘问题的优化算法，它将高斯-牛顿算法和阻尼最小二乘算法进行了结合。该算法在解决非线性最小二乘问题时，具有收敛速度快、精度高等优点，可有效求解图优化算法中的误差函数，快速精确的计算出最优解。
S300：获取里程计增量数据，通过里程计增量数据和最优坐标对UWB的测距值进行卡尔曼滤波，获取最优距离值。
具体地，卡尔曼滤波采用递归的方法进行求解，只需要当前时刻的观测值和预测值就能进行状态估计。卡尔曼滤波具有算法简单、计算量小、精度高的优点。因此本发明使用卡尔曼滤波求解最优距离值。
在一个实施例中，S300包括：
S310：通过里程计增量数据构建预测方程，估计出移动机器人在当前时刻的状态：
；
其中为里程计增量，/>为上一时刻输出的最优坐标，当时该坐标为初始化步骤求得的初值；
S320：估计状态与基站之间的距离即为先验估计距离/>：
；
其中为预测噪声；
S330：构建先验估计协方差：
；
其中为过程噪声协方差，/>为上一时刻输出的后验估计协方差；
S340：构建卡尔曼增益：
；
其中为测量噪声协方差；
S350：后验估计为：
；
S360：构建后验估计协方差：
；
S370：通过对预测值与观测值进行滤波，输出基站与移动机器人之间的最优距离值；其中，预测值为移动机器人在当前时刻估计状态与基站之间的距离/>，观测值为UWB的测距值。
S400：结合最优距离值通过信赖域算法求解移动机器人的最优坐标，实现对机器人的持续鲁棒跟踪。
具体地，信赖域算法与Levenberg-Marquardt算法类似，都是一种非线性最小二乘问题的优化算法，但在跟踪阶段中需要对带约束的非线性方程进行求解，信赖域算法通过信赖域半径则可以有效地计算出该问题的最优解。
在一个实施例中，S400包括：
S410：假设移动机器人当前时刻的坐标为，在不存在误差的情况下，估计坐标/>与基站之间的距离应等于最优距离值/>，估计坐标/>与上一时刻的最优坐标之间应满足里程计约束，由此可构建二元二次方程组：
观测方程可表示为：
；
预测方程可表示为：
；
其中，表示为移动机器人当前时刻的坐标的/>值，/>表示为移动机器人当前时刻的坐标的/>值；
S420：将预测坐标作为方程求解的迭代初值，其中，预测坐标为上一时刻的最优坐标与里程计增量的和；
S430：对二元二次方程组增加里程计约束条件：
；
S440：通过信赖域算法求解增加了里程计约束条件后的二元二次方程组，得到二元二次方程组的最优解；
S450：将二元二次方程组的最优解作为当前时刻的最优坐标，并循环持续跟踪步骤，实现对机器人的持续鲁棒跟踪。
具体地，预测方程的第一个方程的意义为估计状态与上一时刻最优坐标之间的距离等于里程计测得的运动距离，预测方程中第二个方程的意义为估计状态/>与上一时刻最优坐标之间的斜率等于里程计测得的斜率；另外，上述方程组的求解为非线性优化问题，由于非线性优化问题需要通过迭代的方式进行求解，且里程计的增量数据在短期内具有较高的精度，因此将预测坐标作为方程求解的迭代初值；由于观测误差会较大程度的影响求解精度，因此需要对方程组增加约束条件/>，通过/>保证求解过程仅在具有较高精度的预测位置附近进行求解。
在一个实施例中，如图3所示，S440包括：
S441：在二元二次方程组的预测坐标预设区域内构造一个近似模型，近似模型的边界区域称为信赖域，每次迭代调整信赖域直至收敛，则信赖域可表示为：
；
其中为第/>次迭代时的信赖域半径，用于确定信赖域的大小；/>为方程组第/>次迭代时的解；
S442：在信赖域中构造一个二次模型来近似目标函数/>，二次模型可表示为：
；
其中为极小值，/>是/>的雅可比矩阵，/>是/>的黑塞矩阵；
S443：若，即不满足收敛条件，定义信赖域的模型子问题为：
；
其中，表示子问题，/>表示实数域；
S444：求解出子问题的解后，通过信赖域保真度评估模型子问题的最优解/>是否可接受；
S445：定义预测下降量为：
；
S446：定义实际下降量为：
；
S447：预测下降量与实际下降量的比值为；
S448：根据的值确定/>的值并校正信赖域半径；
；
；
其中，，当/>时，/>；
S449：令，返回S442，直至/>时结束迭代，此时满足收敛条件，得到二元二次方程组的最优解。
具体地，信赖域的模型子问题为二次规划问题，即求解具有线性约束的二次目标函数；另外，预测下降量与实际下降量的比值用来衡量近似模型与目标函数的一致性程度，从而决定是否调整新的信任域半径。进一步地，常数的典型值为/>，。
上述一种基于信赖域算法的室内移动机器人定位方法，在定位系统初始化阶段，首先通过多组里程计数据和UWB测距值构建最小二乘优化问题及对应的误差函数；接着通过图优化的方法求解最小二乘优化问题，得出移动机器人当前时刻在全局坐标系下的最优坐标，并将该最优坐标作为后续跟踪步骤的初值；在定位系统跟踪阶段，首先通过里程计增量数据和最优坐标对UWB的测距值进行卡尔曼滤波预处理，获取最优距离值；结合最优距离值通过信赖域算法求解移动机器人的最优坐标，实现对机器人的持续鲁棒跟踪。
与现有技术相比，本发明具有以下优点：
本发明仅依靠单基站可求得移动机器人的全局坐标，大大降低了定位系统的成本。在初始化步骤中，目前基于单基站的UWB定位系统均是通过三边定位法求解初值，三边定位法仅依靠三组距离值求解出移动机器人的全局坐标，而在本发明中将三组距离值之间的所有状态变量一起进行优化，所求得移动机器人全局坐标的精度更高。其次，传统定位方法中均使用基于滤波的定位方法，而本发明中使用优化的方法进行求解，在求解精度上高于滤波算法，提高了定位系统的可用性。
以上对本发明所提供的一种基于信赖域算法的室内移动机器人定位方法进行了详细介绍。本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

Claims (6)

1.一种基于信赖域算法的室内移动机器人定位方法，其特征在于，所述方法包括以下步骤：
S100：通过多组里程计数据和UWB测距值构建最小二乘优化问题及对应的误差函数；
S200：通过图优化的方法求解所述最小二乘优化问题，得出移动机器人当前时刻在全局坐标系下的最优坐标，并将所述最优坐标作为后续跟踪步骤的初值；
S300：通过里程计增量数据和所述最优坐标对所述UWB测距值进行卡尔曼滤波预处理，获取最优距离值；
S400：结合所述最优距离值通过信赖域算法求解所述移动机器人的最优坐标，实现对所述移动机器人的持续鲁棒跟踪；
S100包括：
S110：定义全局坐标系的原点为基站位置，里程计坐标系原点为移动机器人上电位置；
S120：每当获取一个UWB测距值时，输出一次里程计数据；
S130：假设所述里程计坐标系与所述全局坐标系重合，在所述移动机器人上电时，通过人为的方式获取所述移动机器人在所述全局坐标系下的头部朝向；
S140：使所述移动机器人在室内环境下随机运动一定距离，判断是否存在三个不共线的里程计坐标，若存在，将/>之间的所有里程计坐标作为状态变量/>，将/>之间的所有UWB测距值作为观测变量/>；
S150：假设所述基站在所述里程计坐标系下的坐标为，该坐标即为待优化变量；
S160：所述状态变量到所述基站的距离与所述观测变量/>之间的差定义为误差函数：
；
存在一个最优解可满足所述误差函数最小，因此构建最小二乘优化问题：
；
其中表示最优解，满足误差函数的平方和最小；
S200包括：
S210：将所述待优化变量作为顶点，UWB的测距值作为一元边，并使用Levenberg-Marquardt方法进行求解，得到所述基站在所述里程计坐标系下的坐标值；
S220：将所述基站在所述里程计坐标系下的坐标值通过坐标变换公式得到所述移动机器人在全局坐标系下的位置：
；
其中为所述移动机器人在所述全局坐标系下的最优坐标；
S230：将当前时刻的最优坐标作为跟踪步骤的初值。
2.根据权利要求1所述的方法，其特征在于，S140还包括若不存在三个不共线的里程计坐标，则所述移动机器人继续在室内环境下随机运动一定距离，直至存在三个不共线的里程计坐标/>。
3.根据权利要求2所述的方法，其特征在于，S140中判断是否存在三个不共线的里程计坐标，具体为：
令；
当所述移动机器人运动距离大于时，即/>时，令；
当所述移动机器人运动距离再次大于，且与/>之间的夹角大于/>时，即，且时，令。
4.根据权利要求3所述的方法，其特征在于，S300包括：
S310：通过里程计增量数据构建预测方程，估计出所述移动机器人在当前时刻的状态：
；
其中为里程计增量，/>为上一时刻输出的最优坐标，当/>时该坐标为初始化步骤求得的初值；
S320：估计所述状态与所述基站之间的距离即为先验估计距离/>：
；
其中为预测噪声；
S330：构建先验估计协方差：
；
其中为过程噪声协方差，/>为上一时刻输出的后验估计协方差；
S340：构建卡尔曼增益：
；
其中为测量噪声协方差；
S350：后验估计为：
；
S360：构建后验估计协方差：
；
S370：通过对预测值与观测值进行滤波，输出所述基站与所述移动机器人之间的最优距离值；其中，所述预测值为所述移动机器人在当前时刻估计状态与基站之间的距离，所述观测值为UWB的测距值。
5.根据权利要求4所述的方法，其特征在于，S400包括：
S410：假设所述移动机器人当前时刻的坐标为，在不存在误差的情况下，坐标/>与所述基站之间的距离应等于所述最优距离值/>，坐标/>与上一时刻的最优坐标之间应满足里程计约束，由此可构建二元二次方程组：
观测方程可表示为：
；
预测方程可表示为：
；
其中，表示为所述移动机器人当前时刻的坐标的/>值，/>表示为所述移动机器人当前时刻的坐标的/>值；
S420：将预测坐标作为方程求解的迭代初值，其中，所述预测坐标为上一时刻的最优坐标与里程计增量的和；
S430：对所述二元二次方程组增加里程计约束条件：
；
S440：通过信赖域算法求解增加了所述里程计约束条件后的二元二次方程组，得到所述二元二次方程组的最优解；
S450：将所述二元二次方程组的最优解作为当前时刻的最优坐标，并返回S300和S400进行循环持续跟踪，实现对机器人的持续鲁棒跟踪。
6.根据权利要求5所述的方法，其特征在于，S440包括：
S441：在所述二元二次方程组的预测坐标预设区域内构造一个近似模型，所述近似模型的边界区域称为信赖域，每次迭代调整信赖域直至收敛，则信赖域可表示为：
；
其中为第/>次迭代时的信赖域半径，用于确定信赖域的大小；/>为方程组第/>次迭代时的解；
S442：在所述信赖域中构造一个二次模型来近似目标函数/>，所述二次模型可表示为：
；
其中为极小值，/>是/>的雅可比矩阵，/>是的黑塞矩阵；
S443：若，即不满足收敛条件，定义信赖域的模型子问题为：
；
其中，表示子问题，/>表示n维实数空间；
S444：求解出子问题的解后，通过信赖域保真度评估所述模型子问题的最优解/>；
S445：定义预测下降量为：
；
S446：定义实际下降量为：
；
S447：所述预测下降量与所述实际下降量的比值为；
S448：根据所述的值确定/>的值并校正信赖域半径；
；
；
其中，，当/>时，/>；
S449：令，返回S442，直至/>时结束迭代，此时满足收敛条件，得到所述二元二次方程组的最优解。
CN202311032737.7A 2023-08-16 2023-08-16 一种基于信赖域算法的室内移动机器人定位方法 Active CN116753963B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202311032737.7A CN116753963B (zh) 2023-08-16 2023-08-16 一种基于信赖域算法的室内移动机器人定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202311032737.7A CN116753963B (zh) 2023-08-16 2023-08-16 一种基于信赖域算法的室内移动机器人定位方法

Publications (2)

Publication Number Publication Date
CN116753963A CN116753963A (zh) 2023-09-15
CN116753963B true CN116753963B (zh) 2023-12-01

Family

ID=87953563

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202311032737.7A Active CN116753963B (zh) 2023-08-16 2023-08-16 一种基于信赖域算法的室内移动机器人定位方法

Country Status (1)

Country Link
CN (1) CN116753963B (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107659945A (zh) * 2017-09-07 2018-02-02 西安电子科技大学 一种toa条件下最优联合时间同步与定位的定位方法
CN111948602A (zh) * 2020-08-17 2020-11-17 南京工程学院 基于改进Taylor级数的二维UWB室内定位方法
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112584306A (zh) * 2020-11-27 2021-03-30 巢湖学院 一种基于卡尔曼滤波的室内机器人定位算法
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
WO2022151794A1 (zh) * 2021-01-15 2022-07-21 珠海一微半导体股份有限公司 基于无线测距传感器的移动机器人定位方法、系统及芯片
WO2022150904A1 (en) * 2021-01-12 2022-07-21 John Zelek System and method of hybrid scene representation for visual simultaneous localization and mapping
CN115728710A (zh) * 2022-10-28 2023-03-03 宁波大学 一种基于变中心最大熵准则的鲁棒tdoa定位方法
CN115915004A (zh) * 2022-12-11 2023-04-04 广州城市理工学院 一种超市分拣机器人的定位方法
CN116222583A (zh) * 2023-05-10 2023-06-06 北京航空航天大学 一种基于滑动窗口的协同导航快速计算方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108445750B (zh) * 2017-02-16 2022-04-08 法拉第未来公司 用于车辆运动规划的方法和系统

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107659945A (zh) * 2017-09-07 2018-02-02 西安电子科技大学 一种toa条件下最优联合时间同步与定位的定位方法
CN111948602A (zh) * 2020-08-17 2020-11-17 南京工程学院 基于改进Taylor级数的二维UWB室内定位方法
CN112378396A (zh) * 2020-10-29 2021-02-19 江苏集萃未来城市应用技术研究所有限公司 基于抗差lm视觉惯性里程计与uwb混合高精度室内定位方法
CN112584306A (zh) * 2020-11-27 2021-03-30 巢湖学院 一种基于卡尔曼滤波的室内机器人定位算法
CN112833876A (zh) * 2020-12-30 2021-05-25 西南科技大学 一种融合里程计与uwb的多机器人协作定位方法
WO2022150904A1 (en) * 2021-01-12 2022-07-21 John Zelek System and method of hybrid scene representation for visual simultaneous localization and mapping
WO2022151794A1 (zh) * 2021-01-15 2022-07-21 珠海一微半导体股份有限公司 基于无线测距传感器的移动机器人定位方法、系统及芯片
CN115728710A (zh) * 2022-10-28 2023-03-03 宁波大学 一种基于变中心最大熵准则的鲁棒tdoa定位方法
CN115915004A (zh) * 2022-12-11 2023-04-04 广州城市理工学院 一种超市分拣机器人的定位方法
CN116222583A (zh) * 2023-05-10 2023-06-06 北京航空航天大学 一种基于滑动窗口的协同导航快速计算方法

Non-Patent Citations (5)

* Cited by examiner, † Cited by third party
Title
Low-ComplexityLeader-FollowingFormation ControlofMobileRobotsUsingOnly FOV-ConstrainedVisualFeedback;张辉;《IEEETRANSACTIONSONINDUSTRIALINFORMATICS》;第18卷(第7期);4665-4673 *
信赖域算法的单目视觉惯性里程计;王靖淇，修春娣;《第十四届全国信号和智能信息处理与应用学术会议论文集》;309-313 *
基于RSS/AOA混合的室内可见光定位算法;董文杰;王旭东;吴楠;;激光与光电子学进展(第05期);56-61 *
基于高维状态空间的移动机器人定位;庄燕滨;徐则中;;电路与系统学报(第01期);56+111-115 *
改进人工势场法的移动机器人路径规划;于振中;闫继宏;赵杰;陈志峰;朱延河;;哈尔滨工业大学学报(第01期);88-93 *

Also Published As

Publication number Publication date
CN116753963A (zh) 2023-09-15

Similar Documents

Publication Publication Date Title
US7696894B2 (en) Method for determining a relative position of a mobile unit by comparing scans of an environment and mobile unit
CN108307301B (zh) 基于rssi测距和轨迹相似性的室内定位方法
US8983490B2 (en) Locating a mobile device
CN108638062B (zh) 机器人定位方法、装置、定位设备及存储介质
CN111912409B (zh) 可编程智能反射面辅助的多移动设备定位方法及装置
CN111044050B (zh) 一种基于粒子滤波和卡尔曼滤波的蓝牙定位方法
EP2476012A1 (en) Concurrent wireless transmitter mapping and mobile station positioning
CN107132504B (zh) 基于粒子滤波的定位追踪装置、方法及电子设备
CN110895408B (zh) 一种自主定位方法、装置及移动机器人
CN112702699B (zh) 一种融合UWB和LiDAR的室内定位方法
US20150106009A1 (en) Apparatus and method for integrated positioning
KR101268564B1 (ko) 파티클 필터 기반의 단말의 위치 측정 장치 및 방법과 이를 기록한 기록 매체
CN116753963B (zh) 一种基于信赖域算法的室内移动机器人定位方法
WO2013096209A1 (en) System and method for probablistic wlan positioning
CN109541541B (zh) 一种室内三角定位精度修正方法及装置
US20200033439A1 (en) Multi-algorithm trilateration system
CN116465393A (zh) 基于面阵激光传感器的同步定位和建图方法及装置
CN112835020B (zh) 面向非视距参数估计的刚体定位方法
CN115226027A (zh) 一种WiFi室内指纹定位方法及装置
CN107250831B (zh) 提供移动装置的前进方向的指示
CN111954158A (zh) 基于rss地图的联合滤波室内单目标跟踪方法、装置及介质
CN107389068B (zh) 基于tdoa的二分查找定位方法
KR20200140184A (ko) 무선 신호세기 기반의 파티클 필터를 이용한 실내 위치추적 방법 및 장치
Nistico et al. Temporal smoothing particle filter for vision based autonomous mobile robot localization
CN112788528B (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