CN110207699B - 一种定位方法和装置 - Google Patents

一种定位方法和装置 Download PDF

Info

Publication number
CN110207699B
CN110207699B CN201810167085.0A CN201810167085A CN110207699B CN 110207699 B CN110207699 B CN 110207699B CN 201810167085 A CN201810167085 A CN 201810167085A CN 110207699 B CN110207699 B CN 110207699B
Authority
CN
China
Prior art keywords
basic data
determining
positioning
following expression
coordinate
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
CN201810167085.0A
Other languages
English (en)
Other versions
CN110207699A (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.)
Beijing Jingdong Century Trading Co Ltd
Beijing Jingdong Shangke Information Technology Co Ltd
Original Assignee
Beijing Jingdong Century Trading Co Ltd
Beijing Jingdong Shangke Information 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 Beijing Jingdong Century Trading Co Ltd, Beijing Jingdong Shangke Information Technology Co Ltd filed Critical Beijing Jingdong Century Trading Co Ltd
Priority to CN201810167085.0A priority Critical patent/CN110207699B/zh
Publication of CN110207699A publication Critical patent/CN110207699A/zh
Application granted granted Critical
Publication of CN110207699B publication Critical patent/CN110207699B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • 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
    • G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface

Abstract

本发明公开了一种定位方法和装置，涉及计算机技术领域。该方法的一具体实施方式包括：获取基础数据集，所述基础数据集包括：定位对象与环境特征之间的测量距离、所述环境特征的坐标；基于所述基础数据集，通过加权线性最小二乘法确定所述定位对象的第一坐标估计值；将所述第一坐标估计值作为迭代初值，基于所述基础数据集，通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值，以通过所述第二坐标估计值定位所述定位对象。该实施方式能够快速且精确的进行定位。

Description

一种定位方法和装置
技术领域
本发明涉及计算机技术领域，尤其涉及一种定位方法和装置。
背景技术
目前，自动导引运输车(Automated Guided Vehicle，AGV)已经广泛应用于无人化生产系统中。在AGV的运行过程中，一个重要环节是对AGV进行实时的定位。其中，对于AGV的定位是基于AGV坐标系和AGV所处环境的全局坐标系实现的，AGV坐标系是以AGV车体中心为原点，AGV前进方向为坐标轴构建的坐标系，AGV车体中心设置有激光扫描仪，激光扫描仪对周围环境特征进行扫描，每次扫描后，激光扫描仪都会获得一个环境特征与激光扫描仪间的距离值ρ。通过激光扫描仪扫描AGV周围环境的n个环境特征获得的n组数据ρi，和每个环境特征在全局坐标系中的坐标(xi,yi)，即可求得AGV在全局坐标系下的位姿(xL,yL)，从而实现对于AGV的定位。
在实现本发明过程中，发明人发现现有技术中至少存在如下问题：由于在实际应用中，激光扫描仪存在测量误差和估计误差，因此距离值ρi可能不是真实的距离值，传统的计算位姿方法是通过线性最小二乘法或非线性最小二乘法来计算AGV的坐标估计值
Figure BDA0001584767920000011
其中，线性最小二乘法计算速度快，但估计值
Figure BDA0001584767920000012
的精度不够。非线性最小二乘法通常涉及到一些迭代搜索技术，需要以不同的迭代初值进行反复迭代，计算代价高，速度慢。
因此，亟需一种快速且精确的定位方法和装置。
发明内容
有鉴于此，本发明实施例提供一种定位方法和装置，能够快速且精确的进行定位。
为实现上述目的，根据本发明实施例的一个方面，提供了一种定位方法，包括：
获取基础数据集，所述基础数据集包括：定位对象与环境特征之间的测量距离、所述环境特征的坐标；
基于所述基础数据集，通过加权线性最小二乘法确定所述定位对象的第一坐标估计值；
将所述第一坐标估计值作为迭代初值，基于所述基础数据集，通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值，以通过所述第二坐标估计值定位所述定位对象。
进一步的，所述测量距离是由定位设备测量得到的，所述加权线性最小二乘法的权重根据所述定位设备在不同测距范围的测量精度确定。
进一步的，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，i为小于或等于n的正整数；
基于预定义的定位模型：
Figure BDA0001584767920000021
和所述基础数据集，通过加权线性最小二乘法确定所述第一坐标估计值为(ATWA)-1ATWB，其中，εi为第i组基础数据的测量距离ρi的测量误差，di为第i组基础数据对应的环境特征与定位对象的实际距离，(xL,yL)为定位对象的实际坐标，
Figure BDA0001584767920000022
对角矩阵W＝diag{w1,w2,...,wn}，其中，第i组基础数据的权重系数wi与定位设备在第i组基础数据的测量距离ρi处的测量精度正相关。
可选的，所述测量误差服从正态分布N(0,h2(di))，h(di)为正态分布的尺度参数函数，
Figure BDA0001584767920000031
或
Figure BDA0001584767920000032
可选的，所述定位设备在距离其h0处测量精度最高，若ρi≥h0，则
Figure BDA0001584767920000033
若ρi＜h0，则
Figure BDA0001584767920000034
可选的，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，以及所述环境特征的方位角θi，i为小于或等于n的正整数；
所述方法在确定所述第二坐标估计值之后，还包括：
根据下述表达式确定第i组基础数据对所述定位对象的方向角的估计值
Figure BDA0001584767920000035
Figure BDA0001584767920000036
其中，
Figure BDA0001584767920000037
为所述第二坐标估计值；
根据下述表达式确定所述定位对象的方向角的估计值
Figure BDA0001584767920000038
Figure BDA0001584767920000039
可选的，所述方法在确定所述定位对象的方向角的估计值
Figure BDA00015847679200000310
之后，还包括通过下述步骤确定定位对象的位姿估计值
Figure BDA00015847679200000311
的协方差矩阵：
确定协方差矩阵
Figure BDA00015847679200000312
和方差
Figure BDA00015847679200000313
其中，
Figure BDA00015847679200000314
根据下述表达式确定函数D(bi)：
Figure BDA00015847679200000315
其中，
Figure BDA00015847679200000316
根据下述表达式确定方差Cov(Bi,Bi)：
Figure BDA00015847679200000317
其中，r为小于或等于n的正整数；
根据下述表达式确定方差Cov(Bi,Bj)：
Figure BDA00015847679200000318
其中，j为小于或等于n的正整数，i≠j；
根据下述表达式确定
Figure BDA00015847679200000319
的近似协方差矩阵
Figure BDA00015847679200000320
Figure BDA0001584767920000041
其中，矩阵B＝[B1,B2,...,Bn]T；
根据下述表达式确定方差
Figure BDA0001584767920000042
Figure BDA0001584767920000043
根据下述表达式确定方差
Figure BDA0001584767920000044
Figure BDA0001584767920000045
根据下述表达式确定方差
Figure BDA0001584767920000046
Figure BDA0001584767920000047
进一步的，所述通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值包括：
步骤1，定义迭代初始值
Figure BDA0001584767920000048
当迭代次数k＝0时，迭代值z(0)＝z0，k为正整数；
步骤2，根据所述基础数据集为迭代值z(k)、梯度函数
Figure BDA0001584767920000049
函数S(z(k))以及梯度函数
Figure BDA00015847679200000410
赋值，其中，迭代值
Figure BDA00015847679200000411
Figure BDA00015847679200000412
Figure BDA00015847679200000413
Figure BDA0001584767920000051
步骤3，若
Figure BDA0001584767920000052
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4，否则令
Figure BDA0001584767920000053
然后执行步骤3，其中，
Figure BDA0001584767920000054
γ和β为预定义参数；
步骤4，判断||γΔz(k)||∞≤ε是否成立，若是，则确定第二坐标估计值
Figure BDA0001584767920000055
否则令k＝k+1，然后执行步骤2，其中，ε为预设精度。
为实现上述目的，根据本发明实施例的另一个方面，提供了一种定位装置，包括：
获取模块，用于获取基础数据集，所述基础数据集包括：定位对象与环境特征之间的测量距离、所述环境特征的坐标；
第一确定模块，用于基于所述基础数据集，通过加权线性最小二乘法确定所述定位对象的第一坐标估计值；
第二确定模块，用于将所述第一坐标估计值作为迭代初值，基于所述基础数据集，通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值，以通过所述第二坐标估计值定位所述定位对象。
进一步的，所述测量距离是由定位设备测量得到的，所述第一确定模块进一步用于根据所述定位设备在不同测距范围的测量精度确定所述加权线性最小二乘法的权重。
进一步的，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，i为小于或等于n的正整数；
所述第一确定模块进一步用于基于预定义的定位模型：
Figure BDA0001584767920000056
和所述基础数据集，通过加权线性最小二乘法确定所述第一坐标估计值为(ATWA)-1ATWB，其中，εi为第i组基础数据的测量距离ρi的测量误差，di为第i组基础数据对应的环境特征与定位对象的实际距离，(xL,yL)为定位对象的实际坐标，
Figure BDA0001584767920000061
对角矩阵W＝diag{w1,w2,...,wn}，其中，第i组基础数据的权重系数wi与定位设备在第i组基础数据的测量距离ρi处的测量精度正相关。
可选的，所述测量误差服从正态分布N(0,h2(di))，h(di)为正态分布的尺度参数函数，
Figure BDA0001584767920000062
或
Figure BDA0001584767920000063
可选的，所述定位设备在距离其h0处测量精度最高，若ρi≥h0，则
Figure BDA0001584767920000064
若ρi＜h0，则
Figure BDA0001584767920000065
可选的，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，以及所述环境特征的方位角θi，i为小于或等于n的正整数；
所述装置还包括：
第三确定模块，用于根据下述表达式确定第i组基础数据对所述定位对象的方向角的估计值
Figure BDA0001584767920000066
Figure BDA0001584767920000067
其中，
Figure BDA0001584767920000068
为所述第二坐标估计值；
根据下述表达式确定所述定位对象的方向角的估计值
Figure BDA0001584767920000069
Figure BDA00015847679200000610
可选的，所述装置还包括：
第四确定模块，用于通过执行下述步骤确定定位对象的位姿估计值
Figure BDA00015847679200000611
的协方差矩阵：
确定协方差矩阵
Figure BDA00015847679200000612
和方差
Figure BDA00015847679200000613
其中，
Figure BDA00015847679200000614
根据下述表达式确定函数D(bi)：
Figure BDA0001584767920000071
其中，
Figure BDA0001584767920000072
根据下述表达式确定方差Cov(Bi,Bi)：
Figure BDA0001584767920000073
其中，r为小于或等于n的正整数；
根据下述表达式确定方差Cov(Bi,Bj)：
Figure BDA0001584767920000074
其中，j为小于或等于n的正整数，i≠j；
根据下述表达式确定
Figure BDA0001584767920000075
的近似协方差矩阵
Figure BDA0001584767920000076
Figure BDA0001584767920000077
其中，矩阵B＝[B1,B2,...,Bn]T；
根据下述表达式确定方差
Figure BDA0001584767920000078
Figure BDA0001584767920000079
根据下述表达式确定方差
Figure BDA00015847679200000710
Figure BDA00015847679200000711
根据下述表达式确定方差
Figure BDA00015847679200000712
Figure BDA00015847679200000713
进一步的，所述第二确定模块进一步用于执行下述步骤：
步骤1，定义迭代初始值
Figure BDA00015847679200000714
当迭代次数k＝0时，迭代值z(0)＝z0，k为正整数；
步骤2，根据所述基础数据集为迭代值z(k)、梯度函数
Figure BDA00015847679200000715
函数S(z(k))以及梯度函数
Figure BDA00015847679200000716
赋值，其中，迭代值
Figure BDA00015847679200000717
Figure BDA00015847679200000718
Figure BDA0001584767920000081
Figure BDA0001584767920000082
步骤3，若
Figure BDA0001584767920000083
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4，否则令
Figure BDA0001584767920000084
然后执行步骤3，其中，
Figure BDA0001584767920000085
γ和β为预定义参数；
步骤4，判断||γΔz(k)||∞≤ε是否成立，若是，则确定第二坐标估计值
Figure BDA0001584767920000086
否则令k＝k+1，然后执行步骤2，其中，ε为预设精度。
为实现上述目的，根据本发明实施例的另一个方面，提供了一种定位电子设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现本发明实施例提供的定位方法。
为实现上述目的，根据本发明实施例的另一个方面，提供了一种计算机可读介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现本发明实施例提供的定位方法。
本发明实施例提供的定位方法和装置，先通过加权线性最小二乘法计算定位对象的坐标估计值，然后将该坐标估计值作为后续步骤非线性最小二乘法求解的迭代初值。将加权线性最小二乘法的解作为非线性最小二乘法求解的迭代初值，相当于给出了定位对象的真实坐标值附近的迭代初值，在保证计算精度的同时，非线性最小二乘法不用再以不同的迭代初值进行反复迭代，有效降低了计算代价，减少了计算时间，提高了计算效率。
本发明实施例提供的定位方法和装置，在通过加权线性最小二乘法的求解过程中，充分考虑到定位设备在不同测距范围的测量精度，并以此为依据为不同范围内的基础数据分配权重，从而使得非线性最小二乘法的迭代初值更加接近定位对象的真实坐标值，提高非线性最小二乘法的计算精度。
本发明实施例提供的定位方法和装置，同时还提供了定位对象方向角和位姿估计值的不确定性矩阵的计算方法，使得对于定位对象的定位更加全面完善，位姿估计值的不确定性矩阵能够得知位姿的估计值之间在概率意义下的关系，并且能够对于后续的其他算法提供支持。
上述的非惯用的可选方式所具有的进一步效果将在下文中结合具体实施方式加以说明。
附图说明
附图用于更好地理解本发明，不构成对本发明的不当限定。其中：
图1是本发明实施例提供的定位方法的主要流程的示意图；
图2是本发明实施例提供的定位装置的主要模块的示意图；
图3是本发明实施例可以应用于其中的示例性系统架构图；
图4是适于用来实现本发明实施例的电子设备的计算机系统的结构示意图。
具体实施方式
以下结合附图对本发明的示范性实施例做出说明，其中包括本发明实施例的各种细节以助于理解，应当将它们认为仅仅是示范性的。因此，本领域普通技术人员应当认识到，可以对这里描述的实施例做出各种改变和修改，而不会背离本发明的范围和精神。同样，为了清楚和简明，以下的描述中省略了对公知功能和结构的描述。
本发明实施例提供一种定位方法，方法用于根据定位对象与其所处环境的环境特征之间的测量距离以及环境特征的坐标，确定定位对象的坐标，测量距离是由定位设备测量得到的。
在本发明的一种应用场景中，定位对象可以为自动导引运输车(AutomatedGuided Vehicle,AGV)，定位设备可以为AGV设备上配置的激光扫描仪，AGV移动时，激光扫描仪对周围环境的环境特征(反光板)进行扫描。每次扫描后，激光扫描仪都会获得一个环境特征与激光器间的距离值，在该应用场景中，激光扫描仪可以认定为是AGV车体的中心点，因此，环境特征与激光器间的距离值可以认定为是定位对象与环境特征之间的距离。在本发明中，环境特征的坐标以及定位对象的坐标是指基于定位对象所处环境所建立的全局坐标系的坐标。当然，结合不同的应用场景，定位对象和定位设备也可以为其他类型的设备，例如定位设备还可以是声呐参测器等。
如图1所示，本发明提供的定位方法包括步骤S101至步骤S103，其中，在步骤S101中，获取基础数据集，基础数据集包括：定位对象与环境特征之间的测量距离和环境特征的坐标。
然后在步骤S102中，基于上述获取的基础数据集，通过加权线性最小二乘法确定定位对象的第一坐标估计值，其中，加权线性最小二乘法的权重根据定位设备在不同测距范围的测量精度确定。
由于定位对象与环境特征之间的测量距离是通过定位设备测量获得的，因此测量距离会存在测量误差，因此，需要使用最小二乘法计算定位对象的坐标估计值。本发明方法在本步骤中，先通过加权线性最小二乘法计算定位对象的坐标估计值，然后将该坐标估计值作为后续步骤非线性最小二乘法求解的迭代初值。由于当非线性最小二乘法的迭代初值在定位对象的真实坐标值附近时，非线性最小二乘法求得的坐标估计值将会有较高精度。将加权线性最小二乘法的解作为非线性最小二乘法求解的迭代初值，相当于给出了定位对象的真实坐标值附近的迭代初值，在保证计算精度的同时，非线性最小二乘法不用再以不同的迭代初值进行反复迭代，有效降低了计算代价，减少了计算时间，提高了计算效率。
由于定位设备通常具有一定的测量范围，以激光扫描仪为例，假设激光扫描仪的测量范围为5米到10米，那么，当一个环境特征距离激光扫描仪大于10米或小于5米，则激光扫描仪测量出的距离的误差就会比较大。并且定位设备通常会在一定距离具有最高的测量精度，例如，假设激光扫描仪在8米距离的测量精度最高，那么对于离激光扫描仪8米左右的环境特征，激光测量得到的距离值最精确，对于其他距离的环境特征的测量误差就有所上升。本发明在通过加权线性最小二乘法的求解过程中，充分考虑到定位设备在不同测距范围的测量精度，并以此为依据为不同范围内的基础数据分配权重，从而使得非线性最小二乘法的迭代初值更加接近定位对象的真实坐标值，提高非线性最小二乘法的计算精度。
进而在步骤S103中，将第一坐标估计值作为迭代初值，基于基础数据集，通过非线性最小二乘法迭代获得定位对象的第二坐标估计值，以通过第二坐标估计值定位该定位对象。
本发明方法在计算定位对象的坐标估计值的过程中，利用加权线性最小二乘法获得迭代初值，然后再利用该迭代初值使用非线性最小二乘法计算定位对象的坐标估计值，充分利用了线性最小二乘法计算简单速度快的优点和非线性最小二乘法计算精度高的优点，相对于现有技术单独使用线性最小二乘法或非线性最小二乘法计算定位对象的坐标估计值的方式，在保证了计算效率的同时也具有较高的计算精度。
在本发明的一种实施方式中，基础数据集包括n个环境特征所对应的n组基础数据，其中的一组基础数据对应于一个环境特征，n为正整数，n组基础数据中的第i组基础数据包括：环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，i为小于或等于n的正整数。
在本发明步骤S102中，基于预定义的定位模型：
Figure BDA0001584767920000121
和基础数据集，通过加权线性最小二乘法确定第一坐标估计值为(ATWA)-1ATWB。
其中，εi为第i组基础数据的测量距离ρi的测量误差，di为第i组基础数据对应的环境特征与定位对象的实际距离，(xL,yL)为定位对象的实际坐标，
Figure BDA0001584767920000122
对角矩阵W＝diag{w1,w2,...,wn}，矩阵AT是矩阵A的转置，其中，第i组基础数据的权重系数wi与定位设备在第i组基础数据的测量距离ρi处的测量精度正相关，即在精度高的测距距离范围内权重大，在精度低的测距距离范围内权重小。
在本发明的一种实施方式中，测量误差εi服从正态分布N(0,h2(di))，h(di)为预设的正态分布的尺度参数函数，
Figure BDA0001584767920000123
或
Figure BDA0001584767920000124
在本发明的一种实施方式中，定位设备在距离其h0处测量精度最高，若ρi≥h0，则
Figure BDA0001584767920000131
若ρi＜h0，则
Figure BDA0001584767920000132
在本发明中步骤S103通过非线性最小二乘法迭代获得定位对象的第二坐标估计值的过程具体包括下述步骤：
步骤1，定义迭代初始值
Figure BDA0001584767920000133
当迭代次数k＝0时，迭代值z(0)＝z0，k为正整数。
步骤2，根据基础数据集为迭代值z(k)、梯度函数
Figure BDA0001584767920000134
函数S(z(k))以及梯度函数
Figure BDA0001584767920000135
赋值，其中，迭代值
Figure BDA0001584767920000136
Figure BDA0001584767920000137
Figure BDA0001584767920000138
Figure BDA0001584767920000139
Figure BDA00015847679200001310
Figure BDA0001584767920000141
步骤3，利用改进的高斯-牛顿法，进行迭代。
若
Figure BDA0001584767920000142
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4。
若
Figure BDA0001584767920000143
则令
Figure BDA0001584767920000144
然后执行步骤3。
其中，
Figure BDA0001584767920000145
γ和β为预定义参数，可以根据应用场景和需求进行选取，例如，在本发明的一种实施方式中，令γ＝1,β＝10-5。
步骤4，判断||γΔz(k)||∞≤ε是否成立，若是，则确定第二坐标估计值
Figure BDA0001584767920000146
否则令k＝k+1，然后执行步骤2，其中，ε为预设精度，其表示最后得到的估计值
Figure BDA0001584767920000147
与真实坐标(xL,yL)的偏差(在范数意义下)，可以根据应用场景和需求进行选取。
在本发明的另一种实施方式中，在步骤3之前可以加入下述步骤：
判断条件|γΔz(k)||∞≤ε是否成立。若该条件成立，则确定第二坐标估计值
Figure BDA0001584767920000148
若该条件不成立，执行步骤3。
在本发明的另一种实施方式中，步骤3可以替换为，若
Figure BDA0001584767920000149
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4。若
Figure BDA00015847679200001410
则令
Figure BDA00015847679200001411
判断条件||γΔz(k)||∞≤ε是否成立，若该条件成立，则确定第二坐标估计值
Figure BDA00015847679200001412
若该条件不成立，则执行步骤3。
本发明方法还提供定位对象的方向角的计算过程。基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，n组基础数据中的第i组基础数据包括：环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，以及该环境特征的方位角θi，i为小于或等于n的正整数。
在本发明中，定位对象配置有对象坐标系，在该对象坐标系中，定位对象前进方向为坐标系的纵轴方向，垂直于该纵轴，定位对象的水平向右方向为坐标系的横轴方向。环境特征的方位角θi是在该坐标系中，环境特征与横轴之间的夹角。环境特征方位角θi可以根据定位设备的旋转速度估算得到。
当定位对象的定位设备收集到周围环境的n个环境特征的n组数据(ρi,θi)后，通过下述步骤计算定位对象在所处环境的全局坐标系下的的位姿(xL,yL,α)。其中，定位对象的方向角α为定位对象的前进方向，即对象坐标系的纵轴方向，与定位对象所处环境的全局坐标系的x轴方向的夹角，该夹角在全局坐标系的一和二象限为正角，在三和四象限为负角，方向角α在-180度到180度之间。
本发明方法在步骤S103确定第二坐标估计值之后，还包括下述步骤：
首先，根据下述表达式确定第i组基础数据对定位对象的方向角的估计值
Figure BDA0001584767920000151
Figure BDA0001584767920000152
其中，
Figure BDA0001584767920000153
为第二坐标估计值。这里atan2(y,x)是一个函数，该函数用来计算原点至点(x,y)的方位角，即计算(x,y)与x轴的夹角。
然后，根据下述表达式确定定位对象的方向角的估计值
Figure BDA0001584767920000154
Figure BDA0001584767920000155
结合定位对象的坐标估计值
Figure BDA0001584767920000156
和定位对象的方向角的估计值
Figure BDA0001584767920000157
即获得定位对象的位姿估计值
Figure BDA0001584767920000158
本发明方法还提供定位对象的位姿的协方差矩阵的计算过程。本发明方法在确定定位对象的方向角的估计值
Figure BDA0001584767920000161
之后，还包括下述步骤：
确定协方差矩阵
Figure BDA0001584767920000162
和方差
Figure BDA0001584767920000163
其中，
Figure BDA0001584767920000164
Figure BDA0001584767920000165
根据下述表达式确定函数D(bi)：
Figure BDA0001584767920000166
其中，
Figure BDA0001584767920000167
在本发明的一种实施方式中，上述表达式可以替换为：
D(bi)＝4ρi 2h2(ρi)
根据下述表达式确定方差Cov(Bi,Bi)：
Figure BDA0001584767920000168
其中，r为小于或等于n的正整数。
根据下述表达式确定方差Cov(Bi,Bj)：
Figure BDA0001584767920000169
其中，j为小于或等于n的正整数，i≠j。
根据下述表达式确定
Figure BDA00015847679200001610
的近似协方差矩阵
Figure BDA00015847679200001611
Figure BDA00015847679200001612
其中，矩阵B＝[B1,B2,...,Bn]T。
根据下述表达式确定方差
Figure BDA00015847679200001613
Figure BDA00015847679200001614
根据下述表达式确定方差
Figure BDA00015847679200001615
Figure BDA00015847679200001617
在本发明的一种实施方式中，上述表达式可以替换为：
Figure BDA00015847679200001616
根据下述表达式确定方差
Figure BDA0001584767920000171
Figure BDA0001584767920000172
在本发明的一种实施方式中，上述表达式可以替换为：
Figure BDA0001584767920000173
在上述步骤计算完成后，即确定定位对象的位姿估计值
Figure BDA0001584767920000174
的协方差矩阵：
Figure BDA0001584767920000175
本发明实施例还提供一种定位的装置，如图2所示，该装置200包括：获取模块201、第一确定模块202和第二确定模块203。
获取模块201用于获取基础数据集，基础数据集包括：定位对象与环境特征之间的测量距离和环境特征的坐标。
第一确定模块202用于基于基础数据集，通过加权线性最小二乘法确定定位对象的第一坐标估计值。
第二确定模块203用于将第一坐标估计值作为迭代初值，基于基础数据集，通过非线性最小二乘法迭代获得定位对象的第二坐标估计值，以定位定位对象。
在本发明中，测量距离是由定位设备测量得到的，第一确定模块进一步用于根据定位设备在不同测距范围的测量精度确定加权线性最小二乘法的权重。
在本发明中，基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，n组基础数据中的第i组基础数据包括：环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，i为小于或等于n的正整数。
第一确定模块进一步用于基于预定义的定位模型：
Figure BDA0001584767920000181
和基础数据集，通过加权线性最小二乘法确定第一坐标估计值为(ATWA)-1ATWB，其中，εi为第i组基础数据的测量距离ρi的测量误差，di为第i组基础数据对应的环境特征与定位对象的实际距离，(xL,yL)为定位对象的实际坐标，
Figure BDA0001584767920000182
对角矩阵W＝diag{w1,w2,...,wn}，其中，第i组基础数据的权重系数wi与定位设备在第i组基础数据的测量距离ρi处的测量精度正相关。
在本发明中，测量误差服从正态分布N(0,h2(di))，h(di)为正态分布的尺度参数函数，
Figure BDA0001584767920000183
或
Figure BDA0001584767920000184
在本发明中，定位设备在距离其h0处测量精度最高，若ρi≥h0，则
Figure BDA0001584767920000185
若ρi＜h0，则
Figure BDA0001584767920000186
在本发明中，基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，n组基础数据中的第i组基础数据包括：环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，以及环境特征的方位角θi，i为小于或等于n的正整数；
本发明实施例提供的定位的装置还包括：
第三确定模块，用于根据下述表达式确定第i组基础数据对定位对象的方向角的估计值
Figure BDA0001584767920000187
Figure BDA0001584767920000188
其中，
Figure BDA0001584767920000189
为第二坐标估计值；
根据下述表达式确定定位对象的方向角的估计值
Figure BDA0001584767920000191
Figure BDA0001584767920000192
本发明实施例提供的定位的装置还包括：
第四确定模块，用于执行下述步骤：
确定协方差矩阵
Figure BDA0001584767920000193
和方差
Figure BDA0001584767920000194
其中，
Figure BDA0001584767920000195
根据下述表达式确定函数D(bi)：
Figure BDA0001584767920000196
其中，
Figure BDA0001584767920000197
根据下述表达式确定方差Cov(Bi,Bi)：
Figure BDA0001584767920000198
其中，r为小于或等于n的正整数；
根据下述表达式确定方差Cov(Bi,Bj)：
Figure BDA0001584767920000199
其中，j为小于或等于n的正整数，i≠j；
根据下述表达式确定
Figure BDA00015847679200001910
的近似协方差矩阵
Figure BDA00015847679200001911
Figure BDA00015847679200001912
其中，矩阵B＝[B1,B2,...,Bn]T；
根据下述表达式确定方差
Figure BDA00015847679200001913
Figure BDA00015847679200001914
根据下述表达式确定方差
Figure BDA00015847679200001915
Figure BDA00015847679200001916
根据下述表达式确定方差
Figure BDA00015847679200001917
Figure BDA00015847679200001918
在本发明中，第二确定模块进一步用于执行下述步骤：
步骤1，定义迭代初始值
Figure BDA00015847679200001919
当迭代次数k＝0时，迭代值z(0)＝z0，k为正整数；
步骤2，根据基础数据集为迭代值z(k)、梯度函数
Figure BDA00015847679200001920
函数S(z(k))以及梯度函数
Figure BDA0001584767920000201
赋值，其中，迭代值
Figure BDA0001584767920000202
Figure BDA0001584767920000203
Figure BDA0001584767920000204
Figure BDA0001584767920000205
步骤3，若
Figure BDA0001584767920000206
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4，否则令
Figure BDA0001584767920000207
然后执行步骤3，其中，
Figure BDA0001584767920000208
γ和β为预定义参数；
步骤4，判断||γΔz(k)||∞≤ε是否成立，若是，则确定第二坐标估计值
Figure BDA0001584767920000209
否则令k＝k+1，然后执行步骤2，其中，ε为预设精度。
本发明实施例提供的定位方法和装置，先通过加权线性最小二乘法计算定位对象的坐标估计值，然后将该坐标估计值作为后续步骤非线性最小二乘法求解的迭代初值。将加权线性最小二乘法的解作为非线性最小二乘法求解的迭代初值，相当于给出了定位对象的真实坐标值附近的迭代初值，在保证计算精度的同时，非线性最小二乘法不用再以不同的迭代初值进行反复迭代，有效降低了计算代价，减少了计算时间，提高了计算效率。
本发明实施例提供的定位方法和装置，在通过加权线性最小二乘法的求解过程中，充分考虑到定位设备在不同测距范围的测量精度，并以此为依据为不同范围内的基础数据分配权重，从而使得非线性最小二乘法的迭代初值更加接近定位对象的真实坐标值，提高非线性最小二乘法的计算精度。
本发明实施例提供的定位方法和装置，同时还提供了定位对象方向角和位姿估计值的不确定性矩阵的计算方法，使得对于定位对象的定位更加全面完善，位姿估计值的不确定性矩阵能够得知位姿的估计值之间在概率意义下的关系，并且能够对于后续的其他算法提供支持。
图3示出了可以应用本发明实施例的定位方法或定位装置的示例性系统架构300。
如图3所示，系统架构300可以包括终端设备301、302、303，网络304和服务器305。网络304用以在终端设备301、302、303和服务器305之间提供通信链路的介质。网络304可以包括各种连接类型，例如有线、无线通信链路或者光纤电缆等等。
用户可以使用终端设备301、302、303通过网络304与服务器305交互，以接收或发送消息等。
终端设备301、302、303可以是具有显示屏并且支持网页浏览的各种电子设备，包括但不限于智能手机、平板电脑、膝上型便携计算机和台式计算机等等。
服务器305可以是提供各种服务的服务器。
需要说明的是，本发明实施例所提供的定位方法一般由服务器305执行，相应地，定位装置一般设置于服务器305中。
应该理解，图3中的终端设备、网络和服务器的数目仅仅是示意性的。根据实现需要，可以具有任意数目的终端设备、网络和服务器。
下面参考图4，其示出了适于用来实现本发明实施例的终端设备的计算机系统400的结构示意图。图4示出的终端设备仅仅是一个示例，不应对本发明实施例的功能和使用范围带来任何限制。
如图4所示，计算机系统400包括中央处理单元(CPU)401，其可以根据存储在只读存储器(ROM)402中的程序或者从存储部分408加载到随机访问存储器(RAM)403中的程序而执行各种适当的动作和处理。在RAM 403中，还存储有系统400操作所需的各种程序和数据。CPU 401、ROM 402以及RAM 403通过总线404彼此相连。输入/输出(I/O)接口405也连接至总线404。
以下部件连接至I/O接口405：包括键盘、鼠标等的输入部分406；包括诸如阴极射线管(CRT)、液晶显示器(LCD)等以及扬声器等的输出部分407；包括硬盘等的存储部分408；以及包括诸如LAN卡、调制解调器等的网络接口卡的通信部分409。通信部分409经由诸如因特网的网络执行通信处理。驱动器410也根据需要连接至I/O接口405。可拆卸介质411，诸如磁盘、光盘、磁光盘、半导体存储器等等，根据需要安装在驱动器410上，以便于从其上读出的计算机程序根据需要被安装入存储部分408。
特别地，根据本发明公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本发明公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信部分409从网络上被下载和安装，和/或从可拆卸介质411被安装。在该计算机程序被中央处理单元(CPU)401执行时，执行本发明的系统中限定的上述功能。
需要说明的是，本发明所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本发明中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本发明中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
附图中的流程图和框图，图示了按照本发明各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，上述模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图或流程图中的每个方框、以及框图或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
描述于本发明实施例中所涉及到的模块可以通过软件的方式实现，也可以通过硬件的方式来实现。所描述的模块也可以设置在处理器中，例如，可以描述为：一种处理器包括获取模块、第一确定模块、第二确定模块、第三确定模块和第四确定模块。其中，这些模块的名称在某种情况下并不构成对该模块本身的限定，例如，第二确定模块还可以被描述为“定义迭代初始值的模块”。
作为另一方面，本发明还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被一个该设备执行时，使得该设备包括：
获取基础数据集，所述基础数据集包括：所述定位对象与一个或多个所述环境特征之间的测量距离和所述每个环境特征的坐标；
基于所述基础数据集，通过加权线性最小二乘法确定所述定位对象的第一坐标估计值，其中，所述加权线性最小二乘法的权重根据所述定位设备在不同测距范围的测量精度确定；
将所述第一坐标估计值作为迭代初值，基于所述基础数据集，通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值，以定位所述定位对象。
上述具体实施方式，并不构成对本发明保护范围的限制。本领域技术人员应该明白的是，取决于设计要求和其他因素，可以发生各种各样的修改、组合、子组合和替代。任何在本发明的精神和原则之内所作的修改、等同替换和改进等，均应包含在本发明保护范围之内。

Claims (14)

1.一种定位方法，其特征在于，包括：
获取基础数据集，所述基础数据集包括：定位对象与环境特征之间的测量距离、所述环境特征的坐标；
基于所述基础数据集，通过加权线性最小二乘法确定所述定位对象的第一坐标估计值；
将所述第一坐标估计值作为迭代初值，基于所述基础数据集，通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值，以通过所述第二坐标估计值定位所述定位对象；
其中，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，i为小于或等于n的正整数；wi为第i组基础数据的权重系数；
基于预定义的定位模型：
Figure FDA0003471694990000011
和所述基础数据集，通过加权线性最小二乘法确定所述第一坐标估计值为(ATWA)-1ATWB，其中，εi为第i组基础数据的测量距离ρi的测量误差，di为第i组基础数据对应的环境特征与定位对象的实际距离，(xL,yL)为定位对象的实际坐标，
Figure FDA0003471694990000012
对角矩阵W＝diag{w1,w2,...,wn}，其中，第i组基础数据的权重系数wi与定位设备在第i组基础数据的测量距离ρi处的测量精度正相关；
所述通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值包括：
步骤1，定义迭代初始值
Figure FDA0003471694990000013
当迭代次数k＝0时，迭代值z(0)＝z0，k为正整数；
步骤2，根据所述基础数据集为迭代值z(k)、梯度函数▽f(z(k))、函数S(z(k))以及梯度函数▽S(z(k))赋值，其中，迭代值
Figure FDA0003471694990000021
Figure FDA0003471694990000022
Figure FDA0003471694990000023
Figure FDA0003471694990000024
步骤3，若
Figure FDA0003471694990000025
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4，否则令
Figure FDA0003471694990000026
然后执行步骤3，其中，
Figure FDA0003471694990000027
γ和β为预定义参数；
步骤4，判断||γΔz(k)||∞≤ε是否成立，若是，则确定第二坐标估计值
Figure FDA0003471694990000028
否则令k＝k+1，然后执行步骤2，其中，ε为预设精度。
2.根据权利要求1所述的方法，其特征在于，所述测量距离是由定位设备测量得到的，所述加权线性最小二乘法的权重根据所述定位设备在不同测距范围的测量精度确定。
3.根据权利要求2所述的方法，其特征在于，所述测量误差服从正态分布N(0,h2(di))，h(di)为正态分布的尺度参数函数，
Figure FDA0003471694990000029
或
Figure FDA0003471694990000031
4.根据权利要求2所述的方法，其特征在于，所述定位设备在距离其h0处测量精度最高，若ρi≥h0，则
Figure FDA0003471694990000032
若ρi＜h0，则
Figure FDA0003471694990000033
5.根据权利要求3所述的方法，其特征在于，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，以及所述环境特征的方位角θi，i为小于或等于n的正整数；
所述方法在确定所述第二坐标估计值之后，还包括：
根据下述表达式确定第i组基础数据对所述定位对象的方向角的估计值
Figure FDA0003471694990000034
Figure FDA0003471694990000035
其中，
Figure FDA0003471694990000036
为所述第二坐标估计值；
根据下述表达式确定所述定位对象的方向角的估计值
Figure FDA0003471694990000037
Figure FDA0003471694990000038
6.根据权利要求5所述的方法，其特征在于，所述方法在确定所述定位对象的方向角的估计值
Figure FDA0003471694990000039
之后，还包括通过下述步骤确定定位对象的位姿估计值
Figure FDA00034716949900000310
的协方差矩阵：
确定协方差矩阵
Figure FDA00034716949900000311
和方差
Figure FDA00034716949900000312
其中，
Figure FDA00034716949900000313
根据下述表达式确定函数D(bi)：
Figure FDA00034716949900000314
其中，
Figure FDA00034716949900000315
根据下述表达式确定方差Cov(Bi,Bi)：
Figure FDA00034716949900000316
其中，r为小于或等于n的正整数；
根据下述表达式确定方差Cov(Bi,Bj)：
Figure FDA00034716949900000317
其中，j为小于或等于n的正整数，i≠j；
根据下述表达式确定
Figure FDA00034716949900000318
的近似协方差矩阵
Figure FDA00034716949900000319
Figure FDA00034716949900000320
其中，矩阵B＝[B1,B2,...,Bn]T；
根据下述表达式确定方差
Figure FDA0003471694990000041
Figure FDA0003471694990000042
根据下述表达式确定方差
Figure FDA0003471694990000043
Figure FDA0003471694990000044
根据下述表达式确定方差
Figure FDA0003471694990000045
Figure FDA0003471694990000046
7.一种定位装置，其特征在于，包括：
获取模块，用于获取基础数据集，所述基础数据集包括：定位对象与环境特征之间的测量距离、所述环境特征的坐标；
第一确定模块，用于基于所述基础数据集，通过加权线性最小二乘法确定所述定位对象的第一坐标估计值；
第二确定模块，用于将所述第一坐标估计值作为迭代初值，基于所述基础数据集，通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值，以通过所述第二坐标估计值定位所述定位对象；
其中，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，i为小于或等于n的正整数；wi为第i组基础数据的权重系数；
基于预定义的定位模型：
Figure FDA0003471694990000047
和所述基础数据集，通过加权线性最小二乘法确定所述第一坐标估计值为(ATWA)-1ATWB，其中，εi为第i组基础数据的测量距离ρi的测量误差，di为第i组基础数据对应的环境特征与定位对象的实际距离，(xL,yL)为定位对象的实际坐标，
Figure FDA0003471694990000048
对角矩阵W＝diag{w1,w2,...,wn}，其中，第i组基础数据的权重系数wi与定位设备在第i组基础数据的测量距离ρi处的测量精度正相关；
所述通过非线性最小二乘法迭代获得所述定位对象的第二坐标估计值包括：
步骤1，定义迭代初始值
Figure FDA0003471694990000051
当迭代次数k＝0时，迭代值z(0)＝z0，k为正整数；
步骤2，根据所述基础数据集为迭代值z(k)、梯度函数
Figure FDA0003471694990000052
函数S(z(k))以及梯度函数
Figure FDA0003471694990000053
赋值，其中，迭代值
Figure FDA0003471694990000054
Figure FDA0003471694990000055
Figure FDA0003471694990000056
Figure FDA0003471694990000057
步骤3，若
Figure FDA0003471694990000058
则令z(k+1)＝z(k)+γΔz(k)，然后执行步骤4，否则令
Figure FDA0003471694990000059
然后执行步骤3，其中，
Figure FDA00034716949900000510
γ和β为预定义参数；
步骤4，判断||γΔz(k)||∞≤ε是否成立，若是，则确定第二坐标估计值
Figure FDA00034716949900000511
否则令k＝k+1，然后执行步骤2，其中，ε为预设精度。
8.根据权利要求7所述的装置，其特征在于，所述测量距离是由定位设备测量得到的，所述第一确定模块进一步用于根据所述定位设备在不同测距范围的测量精度确定所述加权线性最小二乘法的权重。
9.根据权利要求8所述的装置，其特征在于，所述测量误差服从正态分布N(0,h2(di))，h(di)为正态分布的尺度参数函数，
Figure FDA0003471694990000061
或
Figure FDA0003471694990000062
10.根据权利要求8所述的装置，其特征在于，所述定位设备在距离其h0处测量精度最高，若ρi≥h0，则
Figure FDA0003471694990000063
若ρi＜h0，则
Figure FDA0003471694990000064
11.根据权利要求9所述的装置，其特征在于，所述基础数据集包括n个环境特征所对应的n组基础数据，n为正整数，所述n组基础数据中的第i组基础数据包括：所述环境特征的坐标(xi,yi)，定位对象与该环境特征之间的测量距离ρi，以及所述环境特征的方位角θi，i为小于或等于n的正整数；
所述装置还包括：
第三确定模块，用于根据下述表达式确定第i组基础数据对所述定位对象的方向角的估计值
Figure FDA0003471694990000065
Figure FDA0003471694990000066
其中，
Figure FDA0003471694990000067
为所述第二坐标估计值；
根据下述表达式确定所述定位对象的方向角的估计值
Figure FDA0003471694990000068
Figure FDA0003471694990000069
12.根据权利要求11所述的装置，其特征在于，所述装置还包括：
第四确定模块，用于通过执行下述步骤确定定位对象的位姿估计值
Figure FDA00034716949900000610
的协方差矩阵：
确定协方差矩阵
Figure FDA00034716949900000611
和方差
Figure FDA00034716949900000612
其中，
Figure FDA00034716949900000613
根据下述表达式确定函数D(bi)：
Figure FDA00034716949900000614
其中，
Figure FDA00034716949900000615
根据下述表达式确定方差Cov(Bi,Bi)：
Figure FDA00034716949900000616
其中，r为小于或等于n的正整数；
根据下述表达式确定方差Cov(Bi,Bj)：
Figure FDA0003471694990000071
其中，j为小于或等于n的正整数，i≠j；
根据下述表达式确定
Figure FDA0003471694990000072
的近似协方差矩阵
Figure FDA0003471694990000073
Figure FDA0003471694990000074
其中，矩阵B＝[B1,B2,...,Bn]T；
根据下述表达式确定方差
Figure FDA0003471694990000075
Figure FDA0003471694990000076
根据下述表达式确定方差
Figure FDA0003471694990000077
Figure FDA0003471694990000078
根据下述表达式确定方差
Figure FDA0003471694990000079
Figure FDA00034716949900000710
13.一种定位电子设备，其特征在于，包括：
一个或多个处理器；
存储装置，用于存储一个或多个程序，
当所述一个或多个程序被所述一个或多个处理器执行，使得所述一个或多个处理器实现如权利要求1-6中任一所述的方法。
14.一种计算机可读介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1-6中任一所述的方法。
CN201810167085.0A 2018-02-28 2018-02-28 一种定位方法和装置 Active CN110207699B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810167085.0A CN110207699B (zh) 2018-02-28 2018-02-28 一种定位方法和装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810167085.0A CN110207699B (zh) 2018-02-28 2018-02-28 一种定位方法和装置

Publications (2)

Publication Number Publication Date
CN110207699A CN110207699A (zh) 2019-09-06
CN110207699B true CN110207699B (zh) 2022-04-12

Family

ID=67778933

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810167085.0A Active CN110207699B (zh) 2018-02-28 2018-02-28 一种定位方法和装置

Country Status (1)

Country Link
CN (1) CN110207699B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111533051B (zh) * 2020-05-08 2021-12-17 三一机器人科技有限公司 托盘位姿检测方法、装置、叉车和货运系统

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101000507A (zh) * 2006-09-29 2007-07-18 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
US7376688B1 (en) * 2001-01-09 2008-05-20 Urbain A. von der Embse Wavelet multi-resolution waveforms
CN102033236A (zh) * 2010-10-22 2011-04-27 浙江大学 一种卫星导航位置速度联合估计方法
CN102062587A (zh) * 2010-12-13 2011-05-18 上海大学 基于激光传感器的多移动机器人位姿测定方法
CN103808316A (zh) * 2012-11-12 2014-05-21 哈尔滨恒誉名翔科技有限公司 室内飞行智能体惯性系统与激光测距仪组合导航改进方法
CN104866462A (zh) * 2015-05-08 2015-08-26 同济大学 基于附有约束的总体最小二乘法的空间数据精度提高方法
CN106483496A (zh) * 2016-10-19 2017-03-08 河南城建学院 基于chan算法与改进牛顿迭代的联合时差定位方法
CN106767815A (zh) * 2016-11-30 2017-05-31 天津大学 基于相位差欧氏距离测距的加权最小二乘室内定位算法
WO2017197433A1 (en) * 2016-05-20 2017-11-23 Myriota Pty Ltd Position estimation in a low earth orbit satellite communications system
CN107738852A (zh) * 2017-11-30 2018-02-27 歌尔科技有限公司 定位方法、定位地图构建方法及机器人

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7376688B1 (en) * 2001-01-09 2008-05-20 Urbain A. von der Embse Wavelet multi-resolution waveforms
CN101000507A (zh) * 2006-09-29 2007-07-18 浙江大学 移动机器人在未知环境中同时定位与地图构建的方法
CN102033236A (zh) * 2010-10-22 2011-04-27 浙江大学 一种卫星导航位置速度联合估计方法
CN102062587A (zh) * 2010-12-13 2011-05-18 上海大学 基于激光传感器的多移动机器人位姿测定方法
CN103808316A (zh) * 2012-11-12 2014-05-21 哈尔滨恒誉名翔科技有限公司 室内飞行智能体惯性系统与激光测距仪组合导航改进方法
CN104866462A (zh) * 2015-05-08 2015-08-26 同济大学 基于附有约束的总体最小二乘法的空间数据精度提高方法
WO2017197433A1 (en) * 2016-05-20 2017-11-23 Myriota Pty Ltd Position estimation in a low earth orbit satellite communications system
CN106483496A (zh) * 2016-10-19 2017-03-08 河南城建学院 基于chan算法与改进牛顿迭代的联合时差定位方法
CN106767815A (zh) * 2016-11-30 2017-05-31 天津大学 基于相位差欧氏距离测距的加权最小二乘室内定位算法
CN107738852A (zh) * 2017-11-30 2018-02-27 歌尔科技有限公司 定位方法、定位地图构建方法及机器人

Also Published As

Publication number Publication date
CN110207699A (zh) 2019-09-06

Similar Documents

Publication Publication Date Title
CN111896914A (zh) 一种协同定位方法、装置、设备和存储介质
CN106912105B (zh) 基于pso_bp神经网络的三维定位方法
CN110602736B (zh) 场强预测方法、装置和计算机设备
CN112218306B (zh) 基站覆盖性能的预测方法、装置和计算机设备
JP2016161570A (ja) デバイスのロケーションを求める方法およびデバイス
US20140274128A1 (en) Indoor localization method and system
CN116182878B (zh) 道路曲面信息生成方法、装置、设备和计算机可读介质
CN111238507A (zh) 小区地理位置的确定方法、系统、电子设备及存储介质
CN113936046A (zh) 一种物体定位方法、装置、电子设备及计算机可读介质
CN110207699B (zh) 一种定位方法和装置
CN109489658B (zh) 一种运动目标定位方法、装置及终端设备
CN113761090B (zh) 一种基于点云地图的定位方法和装置
US20160223675A1 (en) Mobile terminal, position identification method, and position identification device
CN113870600B (zh) 车道线信息显示方法、装置、电子设备和计算机可读介质
CN110634159A (zh) 一种目标检测方法和装置
CN113034562B (zh) 用于优化深度信息的方法和装置
CN113970754A (zh) 一种自主可行驶设备的定位方法和装置
Miao et al. Dynamic base stations selection method for passive location based on GDOP
CN114812576A (zh) 地图匹配方法、装置及电子设备
CN108490389B (zh) 基于多径反射的定位方法、装置、电子设备及存储介质
CN109116342B (zh) 定位方法、装置、介质和计算设备
CN110389349B (zh) 定位方法和装置
CN110033088B (zh) 估计gps数据的方法和装置
CN114399555B (zh) 数据在线标定方法、装置、电子设备和计算机可读介质
CN113038357B (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