CN114383621B - 基于网格地图的轨迹纠偏方法、电子设备和存储介质 - Google Patents

基于网格地图的轨迹纠偏方法、电子设备和存储介质 Download PDF

Info

Publication number
CN114383621B
CN114383621B CN202111480927.6A CN202111480927A CN114383621B CN 114383621 B CN114383621 B CN 114383621B CN 202111480927 A CN202111480927 A CN 202111480927A CN 114383621 B CN114383621 B CN 114383621B
Authority
CN
China
Prior art keywords
time
navigation path
projection
real
representing
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
CN202111480927.6A
Other languages
English (en)
Other versions
CN114383621A (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.)
Hangzhou Yixian Advanced Technology Co ltd
Original Assignee
Hangzhou Yixian Advanced 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 Hangzhou Yixian Advanced Technology Co ltd filed Critical Hangzhou Yixian Advanced Technology Co ltd
Priority to CN202111480927.6A priority Critical patent/CN114383621B/zh
Publication of CN114383621A publication Critical patent/CN114383621A/zh
Application granted granted Critical
Publication of CN114383621B publication Critical patent/CN114383621B/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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/28—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network with correlation of data from several navigational instruments
    • G01C21/30—Map- or contour-matching
    • 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/26—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 specially adapted for navigation in a road network
    • G01C21/34—Route searching; Route guidance
    • G01C21/3407—Route searching; Route guidance specially adapted for specific applications
    • G01C21/3415—Dynamic re-routing, e.g. recalculating the route when the user deviates from calculated route or after detecting real-time traffic data or accidents
    • 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/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Abstract

本申请涉及基于网格地图的轨迹纠偏方法、电子设备和存储介质，属于导航技术领域，该方法包括：获取用户的初始位置和目标位置，基于网格地图生成导航路径；获取用户的实时位置，判断用户是否偏离所述导航路径；若否，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果。根据本申请实施例，既可以对室外的运动轨迹进行纠偏，也可以对室内的运动轨迹进行纠偏；既可以应用于车辆导航的轨迹纠偏，也可以应用于行人导航的轨迹纠偏，适用性广；而且，采用投影算法和因子图优化算法进行纠偏，极大的提高了纠偏结果的准确性。

Description

基于网格地图的轨迹纠偏方法、电子设备和存储介质
技术领域
本申请涉及导航技术领域，特别是涉及基于网格地图的轨迹纠偏方法、电子设备和存储介质。
背景技术
如今，导航成为一个热门的研究领域，其主要涉及监测和控制工艺、车辆从一个地方移动到另一个地方的过程。导航领域的类别包括陆地导航、海洋导航、航空导航和空间导航。从广泛的意义来说，导航涉及确定位置和方向的技能或研究，可包括定向运动和行人导航。在导航场景下经常需要做轨迹纠偏。
通常情况下，轨迹纠偏是将时序定位的结果(即轨迹)映射到路网(即拓扑地图)的道路(即拓扑地图的边)上，本质是对定位序列进行平滑。但是，在传统拓扑地图上，定位结果必须落在拓扑地图的边上，纠偏后的结果准确性较低。
例如，在行人导航场景下，若行人遇到空旷的广场，传统拓扑地图有两种表达方式：(1)将广场用一个点表达；(2)将广场用开放区域表达(不属于拓扑地图的元素)，传统拓扑地图的轨迹纠偏，需要把GPS的实时位置，投影到拓扑地图的边(即道路)上，上述两种表达都无法做轨迹纠偏。
发明内容
本申请实施例提供了一种基于网格地图的轨迹纠偏方法、电子设备和存储介质，以至少解决相关技术中轨迹纠偏的结果准确性低的问题、和某些场景下，传统拓扑地图无法轨迹纠偏的问题。
第一方面，本申请实施例提供了一种基于网格地图的轨迹纠偏方法，包括:获取用户的初始位置和目标位置，基于网格地图生成导航路径；获取用户的实时位置，判断用户是否偏离所述导航路径；若否，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果。
在其中一些实施例中，在用户偏离所述导航路径的情况下，所述方法还包括：重新获取初始位置，根据重新获取的初始位置和所述目标位置重新生成导航路径。
在其中一些实施例中，所述根据投影算法分析出实际位置包括：将所述实时位置投影到所述导航路径周围一定范围内所有可能的路径上，得到投影点；若所述投影点为多个，则根据时序约束对多个所述投影点进行过滤，将剩下的投影点作为实际位置。
在其中一些实施例中，所述时序约束的定义如下：将所述导航路径上的初始位置用P0表示；将任意时刻t的从实时位置投影到所述导航路径上的投影点用Pt表示；将任意时刻t+1的从实时位置投影到所述导航路径上的投影点用Pt+1表示；将从P0沿着所述导航路径到Pt的距离用Dt表示；将从P0沿着所述导航路径到Pt+1的距离用Dt+1表示；Dt+1≥Dt。
在其中一些实施例中，在所述剩下的投影点为多个的情况下，所述根据投影算法分析出实际位置包括：计算每个剩下的投影点到上一时刻投影得到的投影点之间的距离，保留距离值最小的投影点，将保留的投影点作为实际位置。
在其中一些实施例中，在所述实时位置落在所述网格地图之外的情况下，所述根据投影算法分析出实际位置包括：将所述实时位置投影到所述导航路径与所述网格地图的交点上，得到投影点，将所述投影点作为实际位置。
在其中一些实施例中，所述通过因子图优化算法进行轨迹纠偏包括：定义因子图的节点，所述节点包括所述实时位置、所述实际位置和因子图优化位置；根据各所述节点计算因子图的边；根据各所述边和预先定义的协方差矩阵，计算马氏距离，并通过梯度下降算法求解。
在其中一些实施例中，所述通过因子图优化算法进行轨迹纠偏包括：
通过如下公式计算所述因子图的边：
UR＝p_loct-pt
BR＝(pt+1-pt)-(p_loct+1-p_loct)
NR＝(p_projt-pt)
对如下公式采用梯度下降算法求解：
其中，UR表示一元残差；BR表示二元残差；NR表示导航仪残差；t取值为1到N；p_loct表示时刻t的实时位置，p_loct+1表示时刻t+1的实时位置；p_projt表示时刻t的实际位置；pt表示时刻t的因子图优化位置，pt+1表示时刻t+1的因子图优化位置；T表示向量的转置；∑_loc-1和∑_nav-1表示协方差矩阵的逆；URT∑_loc-1UR、BRT(2∑_loc)-1BR和NRT∑_nav-1NR表示马氏距离。
第二方面，本申请实施例提供一种电子设备，包括存储器和处理器，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行上述任一项所述的方法。
第三方面，本申请实施例提供一种存储介质，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行上述任一项所述的方法。
相比于相关技术，本申请实施例提供的基于网格地图的轨迹纠偏方法，包括：获取用户的初始位置和目标位置，基于网格地图生成导航路径；获取用户的实时位置，判断用户是否偏离所述导航路径；若否，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果。根据本申请实施例，既可以对室外的运动轨迹进行纠偏，也可以对室内的运动轨迹进行纠偏；既可以应用于车辆导航的轨迹纠偏，也可以应用于行人导航的轨迹纠偏，适用性广；而且，采用投影算法和因子图优化算法进行纠偏，极大的提高了纠偏结果的准确性。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1是根据本申请实施例的基于多边形网格地图表达的2D界面示意图；
图2是根据本申请实施例的基于网格地图的轨迹纠偏方法的流程图；
图3是根据本申请实施例的应用环境中的多个硬件间的关系示意图；
图4是根据本申请实施例的投影点的表达示意图；
图5是根据本申请实施例的因子图的表达示意图；
图6是根据本申请实施例的举例的基于网格地图的轨迹纠偏方法的流程图；
图7是根据本申请实施例的纠偏后的结果的表达示意图；
图8是根据本申请实施例的电子设备的内部结构示意图。
具体实施方式
为了使本申请的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本申请进行描述和说明。应当理解，此处所描述的具体实施例仅仅用以解释本申请，并不用于限定本申请。基于本申请提供的实施例，本领域普通技术人员在没有作出创造性劳动的前提下所获得的所有其他实施例，都属于本申请保护的范围。此外，还可以理解的是，虽然这种开发过程中所作出的努力可能是复杂并且冗长的，然而对于与本申请公开的内容相关的本领域的普通技术人员而言，在本申请揭露的技术内容的基础上进行的一些设计，制造或者生产等变更只是常规的技术手段，不应当理解为本申请公开的内容不充分。
在本申请中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域普通技术人员显式地和隐式地理解的是，本申请所描述的实施例在不冲突的情况下，可以与其它实施例相结合。
除非另作定义，本申请所涉及的技术术语或者科学术语应当为本申请所属技术领域内具有一般技能的人士所理解的通常意义。本申请所涉及的“一”、“一个”、“一种”、“该”等类似词语并不表示数量限制，可表示单数或复数。本申请所涉及的术语“包括”、“包含”、“具有”以及它们任何变形，意图在于覆盖不排他的包含；例如包含了一系列步骤或模块(单元)的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可以还包括没有列出的步骤或单元，或可以还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。本申请所涉及的“连接”、“相连”、“耦接”等类似的词语并非限定于物理的或者机械的连接，而是可以包括电气的连接，不管是直接的还是间接的。本申请所涉及的“多个”是指大于或者等于两个。“和/或”描述关联对象的关联关系，表示可以存在三种关系，例如，“A和/或B”可以表示：单独存在A，同时存在A和B，单独存在B这三种情况。本申请所涉及的术语“第一”、“第二”、“第三”等仅仅是区别类似的对象，不代表针对对象的特定排序。
电子导航地图(Electronic map)，主要是用于路径的规划和导航功能上的实现。电子导航地图从组成形式来看，由道路、背景、注记和POI组成，还可包括3D路口实景放大图、三维建筑物等；从功能角度来看，电子导航地图需要有定位显示、索引、路径计算、引导的功能。
电子导航地图分为离散空间地图和连续空间地图两大类，其中离散空间地图包括网格地图、四叉树地图、体素地图和八叉树地图等；连续空间地图包括网格地图，拓扑地图等。上述网格地图既可以表达2D空间，也可以表达3D空间，其中三角网格最为常见，在图形学和建模中用来模拟复杂物体的表面。
优选的，本申请实施例采用多边形网格地图，不仅需要的储存空间小、计算量小，而且表达准确。例如，图1是根据本申请实施例的基于多边形网格地图表达的2D界面示意图，如图1所示，将场景可划分为多个可通行区域，这些可通行区域为凸多边形，而凸多边形的特点是多边形内部任意两点之间的连线依旧在多边形内部，该性质可以保证凸多边形形成的路径尽可能保持直线，从而解决了路径绕折问题。此外，将场景中可通行区域分割为多块凸多边形方式能够避免网格地图内存占比过大的问题，还具备网格地图中环境信息失真度低的优点，不仅能提高用户体验感，还能将本申请的导航方法移植到内存相对较小的移动端，使用不受限制。
基于此，本申请提供一种基于网格地图的轨迹纠偏方法，图2是根据本申请实施例的基于网格地图的轨迹纠偏方法的流程图，如图2所示，该方法包括以下步骤：
S201：获取用户的初始位置和目标位置，基于网格地图生成导航路径；
S202：获取用户的实时位置，判断用户是否偏离导航路径；
S203：若否，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果。
需要说明的是，上述实时位置是指高噪声定位，即有歧义的定位，因为只有在有歧义的定位的情况下做轨迹纠偏才有意义。
在其中一些实施例中，上述基于网格地图的轨迹纠偏方法可以应用于如图3所示的应用环境中，图3是根据本申请实施例的应用环境中的多个硬件间的关系示意图，其中，终端设备32包括但不限于各种个人计算机、笔记本电脑、智能手机、平板电脑和便携式可穿戴设备，服务器31可以用独立的服务器或者是多个服务器组成的服务器集群来实现。
作为一个示例，服务器31可获取终端设备32的定位信息，该定位信息可代表用户的位置。因此，服务器31可获取用户的初始位置和用户在终端设备32中设置的目标位置，然后，基于网格地图生成导航路径。由于用户携带终端设备32前行，所述服务器31可获取用户的实时位置(指高噪声定位)，判断用户是否偏离导航路径；若否，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果。
值得说明的是，上述定位信息可根据GPS、蓝牙或WIFI等定位模块得到，其定位技术为本领域技术人员所熟知，故此处不做赘述。
因此，本申请实施例的方案既可以对室外的运动轨迹进行纠偏，也可以对室内的运动轨迹进行纠偏；既可以应用于车辆导航的轨迹纠偏，也可以应用于行人导航的轨迹纠偏，适用性广。
本申请实施例依据导航路径先验，进行在线计算，属于一种半约束道路绑定，优化后的结果更加精准，优化得到的轨迹不一定跟导航路径重合。
另外，本申请实施例的方案可以在不增加额外设备(例如传感器)的前提下对导航路径进行二次优化，降低了经济成本。
为了更清楚的对本申请实施例进行说明，下文对上述步骤进行详细阐述。
步骤S201：获取用户的初始位置和目标位置，基于网格地图生成导航路径。
例如，在获取到用户的初始位置和目标位置之后，本申请实施例采用A*算法基于网格地图生成导航路径。由于网格地图只有可行区域，没有道路，所以更贴合现实情况。上述A*算法由搜索函数和启发式函数两部分构成，其中，搜索函数可以使用经典算法(如Dijkstra算法)；启发式函数可根据使用场景进行适配(如“使用两点间的欧式距离”)。因此，可以有效的求解出一条最短路径，而且精确度较高。
步骤S202：获取用户的实时位置，判断用户是否偏离导航路径。
例如，获取用户的实时位置后，计算实时位置到导航路径的距离，若连续M次大于X米，就判定为偏航，其中，X取2σ，σ是定位结果的标准差；M是大于等于1的整数。可选的，如果判定为偏航，则重新获取用户的初始位置(即获取最新的实时位置)，根据重新获取的初始位置和目标位置重新生成导航路径。
步骤S203：若用户未偏离导航路径，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果。
例如，将实时位置投影到导航路径周围一定范围内所有可能的路径(网格地图上可能的路径)上，得到投影点；若投影点为多个，则根据时序约束对多个投影点进行过滤，将剩下的投影点作为实际位置。如果剩下的投影点为多个，则计算每个剩下的投影点到上一时刻投影得到的投影点之间的距离，保留距离值最小的投影点，将保留的投影点作为实际位置。如果实时位置落在网格地图之外，则将实时位置投影到导航路径与网格地图的交点上，得到投影点，将投影点作为实际位置。
作为一个示例，上述时序约束的定义包括：将所述导航路径上的初始位置用P0表示；将任意时刻t的从实时位置投影到所述导航路径上的投影点用Pt表示；将任意时刻t+1的从实时位置投影到所述导航路径上的投影点用Pt+1表示；将从P0沿着所述导航路径到Pt的距离用Dt表示；将从P0沿着所述导航路径到Pt+1的距离用Dt+1表示；Dt+1≥Dt。该定义假设了用户“不走回头路”，也“允许用户停止”。
举例而言，图4是根据本申请实施例的投影点的表达示意图，如图4所示，用户沿着导航路径从位置0向终点(即目标位置)行走，GPS观测点(即实时位置)包括图4中的标记1-8(标记大小按时序依次递增)，投影点用a-k表示，其中，位置4有两个投影点f和g，经过筛选，将投影点f作为实际位置，即D4是指用户从位置0沿着导航路径到投影点f的距离；位置5有两个投影点e和f，若将投影点e作为实际位置，那么从位置0沿着导航路径到投影点e的距离D5小于D4，表示用户走了回头路，违背了时序约束，所以根据时序约束，将投影点f作为实际位置，此时D5＝D4，表示用户停止。位置6有三个投影点d、h、i，若将投影点d作为实际位置，那么从位置0沿着导航路径到投影点d的距离D6小于D5，违背了时序约束，故过滤掉投影点d，又由于上一时刻投影得到是投影点f，从投影点f到投影点h的距离小于从投影点f到投影点i的距离，所以将投影点h作为位置6的实际位置。通过上述方式，投影得到的结果更加细致、精确。
另外，本申请实施例需要定义因子图，因子图的节点(Node)包括实时位置、实际位置和因子图优化位置，从而根据各节点定义因子图的边(Edge)。
作为一个示例，图5是根据本申请实施例的因子图的表达示意图，如图5所示，因子图的边包括UR(unary residual，一元残差)、BR(binary residual，二元残差)和NR(navigator residual，导航仪残差)，其中GPS观测点是指上文所述的实时位置，GPS估计点是指因子图优化位置。
本申请实施例还需要定义协方差矩阵(∑)，UR的协方差矩阵由定位模块给出；BR的协方差矩阵由定位模块二次计算得到；NR的协方差矩阵由算法设计者设置，代表用户有多大的置信度完全按照导航路径来走。
本申请实施例可通过如下公式计算所述因子图的边：
UR＝p_loct-pt (公式1)
BR＝(pt+1-pt)-(p_loct+1-p_loct) (公式2)
NR＝(p_projt-pt) (公式3)
对如下公式采用梯度下降算法求解：
其中，公式1表示“高噪声定位结果(即实时位置)”与“因子图优化位置”之差；公式2表示“t+1时刻因子图优化位置与t时刻因子图优化位置之差得到的向量”与“t+1时刻高噪声定位结果与t时刻高噪声定位结果之差得到的向量”之差；公式3表示“t时刻投影点”与“因子图优化位置”之差。
具体的，t取值为1到N；p_loct表示时刻t的实时位置，p_loct+1表示时刻t+1的实时位置；p_projt表示时刻t的实际位置；pt表示时刻t的因子图优化位置，pt+1表示时刻t+1的因子图优化位置；T表示向量的转置；∑-loc-1是定位结果的协方差矩阵的逆，由定位模块给出，如手机GPS模块可以输出经纬度和不确定度，其中不确定度就可以构造协方差矩阵；∑-nav-1表示用户跟着导航路径走的协方差矩阵的逆，一般使用定位模块的不确定度，增加不确定度，相当于更相信定位结果；减少不确定度，相当于更相信导航路径；URT∑_loc-1UR、BRT(2∑_loc)-1BR和NRT∑_nav-1NR均表示马氏距离。
可选的，上述梯度下降算法可采用高斯牛顿法或LM(Levenberg-Marquardt)算法等等。
根据上述内容，本申请实施例可举例一种基于网格地图的轨迹纠偏方法，图6是根据本申请实施例的举例的基于网格地图的轨迹纠偏方法的流程图，如图6所示，该流程包括以下步骤：
步骤S601：获取用户的高噪声定位结果(即实时位置)；
步骤S602：判断是否偏航；
步骤S603：若偏航，重新获取初始位置，进入步骤S604；若未偏航，进入步骤S606；
步骤S604：调用导航算法；
步骤S605：重新生成导航路径，返回步骤S602；
步骤S606：调用投影算法；
步骤S607：通过因子图优化算法进行轨迹纠偏；
步骤S608：得到纠偏后的结果。
图7是根据本申请实施例的纠偏后的结果的表达示意图，如图7所示，估计路径为纠偏后的结果，显然，估计路径比导航路径更加贴近真实路径，本申请实施例的轨迹纠偏方法没有“生硬”地将高噪声定位位置拉到导航路径上，而是进行“约束性优化”，得到的路径尽可能还原用户的真实行走的路径，提高了纠偏结果的准确性。
综上，本申请实施例既可以对室外的运动轨迹进行纠偏，也可以对室内的运动轨迹进行纠偏；既可以应用于车辆导航的轨迹纠偏，也可以应用于行人导航的轨迹纠偏，适用性广；而且，采用投影算法和因子图优化算法进行纠偏，极大的提高了纠偏结果的准确性。
需要说明的是，本实施例中的具体示例可以参考上述实施例及可选实施方式中所描述的示例，本实施例在此不再赘述。
另外，结合上述实施例中的基于网格地图的轨迹纠偏方法，本申请实施例可提供一种存储介质来实现。该存储介质上存储有计算机程序；该计算机程序被处理器执行时实现上述实施例中的任意一种基于网格地图的轨迹纠偏方法。
本申请的一个实施例中还提供了一种电子设备，该电子设备可以是终端。该电子设备包括通过系统总线连接的处理器、存储器、网络接口、显示屏和输入装置。其中，该电子设备的处理器用于提供计算和控制能力。该电子设备的存储器包括非易失性存储介质、内存储器。该非易失性存储介质存储有操作系统和计算机程序。该内存储器为非易失性存储介质中的操作系统和计算机程序的运行提供环境。该电子设备的网络接口用于与外部的终端通过网络连接通信。该计算机程序被处理器执行时以实现一种基于网格地图的轨迹纠偏方法。该电子设备的显示屏可以是液晶显示屏或者电子墨水显示屏，该电子设备的输入装置可以是显示屏上覆盖的触摸层，也可以是电子设备外壳上设置的按键、轨迹球或触控板，还可以是外接的键盘、触控板或鼠标等。
在一个实施例中，图8是根据本申请实施例的电子设备的内部结构示意图，如图8所示，提供了一种电子设备，该电子设备可以是服务器，其内部结构图可以如图8所示。该电子设备包括通过内部总线连接的处理器、网络接口、内存储器和非易失性存储器，其中，该非易失性存储器存储有操作系统、计算机程序和数据库。处理器用于提供计算和控制能力，网络接口用于与外部的终端通过网络连接通信，内存储器用于为操作系统和计算机程序的运行提供环境，计算机程序被处理器执行时以实现一种基于网格地图的轨迹纠偏方法，数据库用于存储数据。
本领域技术人员可以理解，图8中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的电子设备的限定，具体的电子设备可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，该计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本申请所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
本领域的技术人员应该明白，以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各个技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。
以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (5)

1.一种基于网格地图的轨迹纠偏方法，其特征在于，包括：
获取用户的初始位置和目标位置，基于网格地图生成导航路径；
获取用户的实时位置，判断用户是否偏离所述导航路径；
若否，根据投影算法分析出实际位置，并通过因子图优化算法进行轨迹纠偏，得到纠偏后的结果；
在用户偏离所述导航路径的情况下，所述方法还包括：重新获取初始位置，根据重新获取的初始位置和所述目标位置重新生成导航路径；
所述根据投影算法分析出实际位置包括：
将所述实时位置投影到所述导航路径周围一定范围内所有可能的路径上，得到投影点；
若所述投影点为多个，则根据时序约束对多个所述投影点进行过滤，将剩下的投影点作为实际位置；
在所述实时位置落在所述网格地图之外的情况下，所述根据投影算法分析出实际位置包括：将所述实时位置投影到所述导航路径与所述网格地图的交点上，得到投影点，将所述投影点作为实际位置；
所述通过因子图优化算法进行轨迹纠偏包括：
定义因子图的节点，所述节点包括所述实时位置、所述实际位置和因子图优化位置；
根据各所述节点计算因子图的边；
根据各所述边和预先定义的协方差矩阵，计算马氏距离，并通过梯度下降算法求解；
所述通过因子图优化算法进行轨迹纠偏包括：
通过如下公式计算所述因子图的边：
对如下公式采用梯度下降算法求解：
其中，表示一元残差；/>表示二元残差；/>表示导航仪残差，NR 的协方差矩阵由算法设计者设置，代表用户有多大的置信度完全按照导航路径来走；t取值为1到N；/>表示时刻t的实时位置，/>表示时刻t+1的实时位置；/>表示时刻t的实际位置；/>表示时刻t的因子图优化位置，/>表示时刻t+1的因子图优化位置；T表示向量的转置；和/>表示协方差矩阵的逆；/>、/>和表示马氏距离。
2.根据权利要求1所述的方法，其特征在于，所述时序约束的定义如下：
将所述导航路径上的初始位置用P0表示；
将任意时刻t的从实时位置投影到所述导航路径上的投影点用Pt表示；
将任意时刻t+1的从实时位置投影到所述导航路径上的投影点用Pt+1表示；
将从P0沿着所述导航路径到Pt的距离用Dt表示；
将从P0沿着所述导航路径到Pt+1的距离用Dt+1表示；
Dt+1≥Dt。
3.根据权利要求1所述的方法，其特征在于，在所述剩下的投影点为多个的情况下，所述根据投影算法分析出实际位置包括：
计算每个剩下的投影点到上一时刻投影得到的投影点之间的距离，保留距离值最小的投影点，将保留的投影点作为实际位置。
4.一种电子设备，包括存储器和处理器，其特征在于，所述存储器中存储有计算机程序，所述处理器被设置为运行所述计算机程序以执行权利要求1至3中任一项所述的方法。
5.一种存储介质，其特征在于，所述存储介质中存储有计算机程序，其中，所述计算机程序被设置为运行时执行权利要求1至3中任一项所述的方法。
CN202111480927.6A 2021-12-06 2021-12-06 基于网格地图的轨迹纠偏方法、电子设备和存储介质 Active CN114383621B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111480927.6A CN114383621B (zh) 2021-12-06 2021-12-06 基于网格地图的轨迹纠偏方法、电子设备和存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111480927.6A CN114383621B (zh) 2021-12-06 2021-12-06 基于网格地图的轨迹纠偏方法、电子设备和存储介质

Publications (2)

Publication Number Publication Date
CN114383621A CN114383621A (zh) 2022-04-22
CN114383621B true CN114383621B (zh) 2024-04-09

Family

ID=81196311

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111480927.6A Active CN114383621B (zh) 2021-12-06 2021-12-06 基于网格地图的轨迹纠偏方法、电子设备和存储介质

Country Status (1)

Country Link
CN (1) CN114383621B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103438897A (zh) * 2013-09-04 2013-12-11 杭州东信捷峻科技有限公司 基于栅格地图的路径轨迹校正方法
US11048277B1 (en) * 2018-01-24 2021-06-29 Skydio, Inc. Objective-based control of an autonomous unmanned aerial vehicle
CN113138401A (zh) * 2021-03-25 2021-07-20 厦门才茂通信科技有限公司 一种运动过程中对轨迹纠偏技术进行计算的方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US10018474B2 (en) * 2015-12-21 2018-07-10 Invensense, Inc. Method and system for using offline map information aided enhanced portable navigation
CN111694287B (zh) * 2020-05-14 2023-06-23 阿波罗智能技术(北京)有限公司 无人驾驶仿真场景中的障碍物模拟方法和装置

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103438897A (zh) * 2013-09-04 2013-12-11 杭州东信捷峻科技有限公司 基于栅格地图的路径轨迹校正方法
US11048277B1 (en) * 2018-01-24 2021-06-29 Skydio, Inc. Objective-based control of an autonomous unmanned aerial vehicle
CN113138401A (zh) * 2021-03-25 2021-07-20 厦门才茂通信科技有限公司 一种运动过程中对轨迹纠偏技术进行计算的方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
SALAS-MORENO R F 等.SLAM++:Simultaneous localisation and mapping at the level of objects.IEEE Conference on Computer Vision and Pattern Recognition,IEEE Computer Society.2013,第1352-1359页. *
基于动态加权的城市路网地图匹配算法;樊娜;尉青青;康军;葛建东;唐蕾;;测控技术(第01期);第154-158页 *
面向车辆监控导航的地图匹配算法研究;陈嘉;胡继华;张飞舟;;北京大学学报(自然科学版);第45卷(第02期);第299-305页 *

Also Published As

Publication number Publication date
CN114383621A (zh) 2022-04-22

Similar Documents

Publication Publication Date Title
JP7166383B2 (ja) 高精度マップの作成方法及び装置
US20200132476A1 (en) Method and apparatus for producing a lane-accurate road map
US10288437B2 (en) Routing with data version stitching
US6982712B2 (en) Three-dimensional model generating system and method, and computer program therefor
US8935096B2 (en) Apparatus for fast path search by learning heuristic function and method thereof
JP2019512668A (ja) ルート逸脱認識方法、端末、および記憶媒体
CN110221600B (zh) 路径规划方法、装置、计算机设备和存储介质
WO2021115143A1 (zh) 一种运动轨迹处理方法、介质、装置及设备
US20230042968A1 (en) High-definition map creation method and device, and electronic device
CN109685898B (zh) 点云数据的分层方法、装置、计算机设备和存储介质
CN105469445B (zh) 一种步长可变地图生成方法
CN102779165A (zh) 栅格地图图片库的建立方法
CN114440916A (zh) 一种导航方法、装置、设备及存储介质
CN114554395A (zh) 定位精度预测方法、装置、信号源布局确定方法及装置
CN113658342A (zh) Ar导航路径生成的方法、系统、电子装置和存储介质
CN114383621B (zh) 基于网格地图的轨迹纠偏方法、电子设备和存储介质
CN113096251A (zh) 基于cim的模型处理方法、装置、设备及存储介质
Jo et al. Construction process of a three-dimensional roadway geometry map for autonomous driving
US20150127302A1 (en) Method and apparatus for optimized routing
Qin et al. Dynamic detection of topological information from grid-based generalized Voronoi diagrams
Shashidharan et al. FUTURES-AMR: towards an adaptive mesh refinement framework for geosimulations
CN112685531A (zh) 车辆匹配方法和装置、计算设备以及计算机可读存储介质
CN111882494B (zh) 位姿图处理方法、装置、计算机设备和存储介质
US20240027223A1 (en) Determining motion in indoor space using probabilistic representation of motion transition
CN114323017B (zh) Ar导航内容生成方法和系统

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