CN103926611A - 一种实时室内定位数据优化方法 - Google Patents

一种实时室内定位数据优化方法 Download PDF

Info

Publication number
CN103926611A
CN103926611A CN201410189205.9A CN201410189205A CN103926611A CN 103926611 A CN103926611 A CN 103926611A CN 201410189205 A CN201410189205 A CN 201410189205A CN 103926611 A CN103926611 A CN 103926611A
Authority
CN
China
Prior art keywords
location point
judge
current location
coordinate
math
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.)
Granted
Application number
CN201410189205.9A
Other languages
English (en)
Other versions
CN103926611B (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.)
Chengdu Information Technology Co Ltd of CAS
Original Assignee
Chengdu Information Technology Co Ltd of CAS
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 Chengdu Information Technology Co Ltd of CAS filed Critical Chengdu Information Technology Co Ltd of CAS
Priority to CN201410189205.9A priority Critical patent/CN103926611B/zh
Publication of CN103926611A publication Critical patent/CN103926611A/zh
Application granted granted Critical
Publication of CN103926611B publication Critical patent/CN103926611B/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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种实时室内定位数据优化方法，在实时室内定位过程中，当定位结果出现在障碍物区域内时，通过求取上一位置点与当前位置点的连线与障碍物区域的交点，用优化后的交点位置代替当前位置，实现了定位结果优化，有效避免了由于定位结果出现在障碍物区域内引起的定位结果缺失，使得定位结果更加符合现实逻辑。

Description

一种实时室内定位数据优化方法
技术领域
本发明涉及一种实时室内定位数据优化方法，尤其涉及一种当定位结果出现在障碍物区域内时的处理方法。
背景技术
目前人们获取室外位置信息的方式主要是通过全球定位系统(GlobalPositioning System，GPS)，但由于卫星信号容易受建筑物等阻挡，GPS等卫星定位技术并不适合室内或高楼林立的场合。室内定位技术快速发展，已成为GPS的有力补充。室内定位在医疗卫生、消防救援、军事应用等领域都有广阔的应用前景。
室内定位的主要原理是在室内环境下利用无线传感信号获取信号强度，信号发送时间等信息，再利用这些信息，通过一定的定位算法来取得移动终端的精准位置信息。
而在实时定位过程中，由于室内环境复杂，存在桌子、柜子等障碍物，再加上定位误差的存在，会出现定位到的人员出现在桌子，柜子等区域里的情况，这是不符合现实逻辑的。
针对这种情况，可以采用忽略出现在障碍物区域内的点的办法，但是这种方法会引起定位位置点的缺失，特别是在障碍物分布较密集区域，可能造成连续定位点的缺失，影响定位效果。
发明内容
为了克服现有技术的上述缺点，本发明提供了一种实时室内定位数据优化方法。
本发明解决其技术问题所采用的技术方案是：一种实时室内定位数据优化方法，包括如下步骤：
步骤一、判断定位是否结束：如果否，则进入步骤二，如果是，则进入步骤七；
步骤二、获取当前位置点的位置信息；
步骤三、判断当前位置点是否在障碍物区域内：如果否，则进入步骤六，如果是，则进入步骤四；
步骤四、求出上一位置点和当前位置点的连线与障碍区域多边形的所有交点；
步骤五、对交点进行优化处理，将优化结果作为当前位置点的坐标；
步骤六、将上一位置点的坐标替换为当前位置点的坐标，转步骤一；
步骤七、结束优化。
与现有技术相比，本发明的积极效果是：本发明解决了在实时室内定位过程中，定位结果出现在障碍物区域内的问题，通过求取上一位置点与当前位置点的连线与障碍物区域的交点，用优化后的交点位置代替当前位置，实现了定位结果优化，有效避免了由于定位结果出现在障碍物区域内引起的定位结果缺失，使得定位结果更加符合现实逻辑。
附图说明
本发明将通过例子并参照附图的方式说明，其中：
图1是本发明的算法流程图。
具体实施方式
一种实时室内定位数据优化方法，如图1所示，包括如下步骤：
步骤一、判断定位是否结束：如果否，则进入步骤二，如果是，则进入步骤七；
步骤二、获取当前位置点的位置信息；
步骤三、判断当前位置点是否在障碍物区域内：
采用角度和的算法来判断当前位置点是否位于障碍物区域内，具体方法如下：
(1)将障碍物区域抽象成二维平面内的多边形(以下简称“障碍区域多边形”)；
(2)计算当前位置点与障碍区域多边形所有相邻顶点组成的角的角度和；
(3)判断角度和是否等于360度：
如果否，则判定当前位置点在障碍区域多边形边界上或者障碍区域多边形外部，进入步骤六，如果是，则判定当前位置点在障碍区域多边形内部(不包含边界)，进入步骤四；
步骤四、求出上一位置点和当前位置点的连线与障碍区域多边形的所有交点：
遍历多边形的每一条边，判断当前位置点与上一位置点的连线与该条边是否相交，若相交则求出两条线段的交点：
假设已知线段P1P2和线段Q1Q2，我们可以分三步来求出这两条线段的交点：
(1)进行快速排斥实验：设以线段P1P2为对角线的矩形为R，设以线段Q1Q2为对角线的矩形为T，如果R和T相交，则进行跨立实验；否则，则判定两线段不相交。
如果R和T相交，需同时满足以下四个条件：
1)Math.Min(P1.x,P2.x)≤Math.Max(Q1.x,Q2.x)
2)Math.Min(Q1.x,Q2.x)≤Math.Max(P1.x,P2.x)
3)Math.Min(P1.y,P2.y)≤Math.Max(Q1.y,Q2.y)
4)Math.Min(Q1.y,Q2.y)≤Math.Max(P1.y,P2.y)
其中：Math.Min()为返回较小值函数；Math.Max()为返回较大值函数；P1.x、P2.x、Q1.x、Q2.x分别表示点P1、P2、Q1、Q2的x坐标；P1.y、P2.y、Q1.y、Q2.y分别表示点P1、P2、Q1、Q2的y坐标。
(2)进行跨立实验：
若线段P1P2与线段Q1Q2相互跨立对方，则判定两线段相交，进入第(3)步计算交点坐标；否则，则判定两线段不相交。
根据向量叉积的性质，我们可以通过它的符号判断两矢量相互之间的顺逆时针关系，假设两个向量
若则在的顺时针方向；
若则在的逆时针方向；
若则与共线；
如果两条线段相交，则两线段必然相互跨立对方。若线段P1P2跨立线段Q1Q2，则可得到矢量和将位于矢量的两侧，根据叉积判断法，可以得到： ( P 1 Q 1 &RightArrow; &times; Q 2 Q 1 &RightArrow; ) * ( P 2 Q 1 &RightArrow; &times; Q 2 Q 1 &RightArrow; ) < 0 ; 若线段Q1Q2跨立线段P1P2，则矢量和Q2P1将位于矢量的两侧，即有 ( Q 1 P 1 &RightArrow; &times; P 2 P 1 &RightArrow; ) * ( Q 2 P 1 &RightArrow; &times; P 2 P 1 &RightArrow; ) < 0 .
(3)计算两条线段的交点坐标：
当判定两条线段相交后，可以进行交点的求解，求交点可以用平面几何方法，列点斜式方程来完成。但由于点斜式方程难以处理斜率为0的特殊情况，不方便求解。因而，采用向量法求解交点。设交点为(x0,y0)，则下列方程组成立：
x 0 - x 1 = k 1 ( x 2 - x 1 ) y 0 - y 1 = k 1 ( y 2 - y 1 ) x 0 - x 3 = k 2 ( x 4 - x 3 ) y 0 - y 3 = k 2 ( y 4 - y 3 )
根据以上方程组，消除参数k1和k2，得到如下方程：
x0(y2-y1)-x1(y2-y1)＝y0(x2-x1)-y1(x2-x1)
x0(y4-y3)-x3(y4-y3)＝y0(x4-x3)-y3(x4-x3)
然后求解(x0,y0)，结果如下所示：
b1＝(y2-y1)x1+(x1-x2)y1
b2＝(y4-y3)x3+(x3-x4)y3
|D|＝(x2-x1)(y4-y3)-(x4-x3)(y2-y1)
|D1|＝b2(x2-x1)-b1(x4-x3)
|D2|＝b2(y2-y1)-b1(x4-x3)
x0＝|D1|/|D|,y0＝|D2|/|D|
求出的x0与y0即为要求的交点坐标。
步骤五、对交点进行优化处理，将优化结果作为当前位置点的坐标：
待求出所有交点后，获取离上一位置点最近的交点，再将该交点向上一位置点方向移动一定距离a得到新的位置点，a为人靠近障碍物区域行走时，人到障碍物区域边缘的距离，一般为0.1到0.5米，然后将当前位置点的坐标替换为新的位置点的坐标，即完成了位于障碍区域多边形内部的点的优化处理。转步骤六。
步骤六、将上一位置点的坐标替换为当前位置点的坐标，转步骤一。
步骤七、结束优化。

Claims (6)

1.一种实时室内定位数据优化方法，其特征在于：包括如下步骤：
步骤一、判断定位是否结束：如果否，则进入步骤二，如果是，则进入步骤七；
步骤二、获取当前位置点的位置信息；
步骤三、判断当前位置点是否在障碍物区域内：如果否，则进入步骤六，如果是，则进入步骤四；
步骤四、求出上一位置点和当前位置点的连线与障碍区域多边形的所有交点；
步骤五、对交点进行优化处理，将优化结果作为当前位置点的坐标；
步骤六、将上一位置点的坐标替换为当前位置点的坐标，转步骤一；
步骤七、结束优化。
2.根据权利要求1所述的一种实时室内定位数据优化方法，其特征在于：步骤三所述判断当前位置点是否在障碍物区域内的方法为：
(1)将障碍物区域抽象成二维平面内的多边形得到障碍区域多边形；
(2)计算当前位置点与障碍区域多边形所有相邻顶点组成的角的角度和；
(3)判断角度和是否等于360度：如果否，则判定当前位置点在障碍区域多边形边界上或者障碍区域多边形外部；如果是，则判定当前位置点在障碍区域多边形内部。
3.根据权利要求1所述的一种实时室内定位数据优化方法，其特征在于：步骤四所述求上一位置点和当前位置点的连线与障碍区域多边形的交点的方法为：
假设待判断是否相交的两条线段分别为P1P2和Q1Q2，
(1)进行快速排斥实验：设以线段P1P2为对角线的矩形为R，设以线段Q1Q2为对角线的矩形为T，判断R和T是否相交，如果是，则进入下一步骤，进行跨立实验；
(2)进行跨立实验：判断线段P1P2与线段Q1Q2是否相互跨立对方，如果是，则判定线段P1P2与线段Q1Q2相交，进入下一步骤，计算两条线段的交点坐标；
(3)按如下公式计算两条线段的交点坐标(x0,y0)：
x0＝|D1|/|D|,y0＝|D2|/|D|，其中：
|D|＝(x2-x1)(y4-y3)-(x4-x3)(y2-y1)
|D1|＝b2(x2-x1)-b1(x4-x3)
|D2|＝b2(y2-y1)-b1(x4-x3)
其中：点P1、P2、Q1、Q2的坐标分别为(x1,y1)、(x2,y2)、(x3,y3)、(x4,y4)，b1＝(y2-y1)x1+(x1-x2)y1，b2＝(y4-y3)x3+(x3-x4)y3。
4.根据权利要求3所述的一种实时室内定位数据优化方法，其特征在于：判断矩形R和矩形T相交的方法是：当下列4个条件全部满足时，则判定矩形R和矩形T相交：
1)Math.Min(P1.x,P2.x)≤Math.Max(Q1.x,Q2.x)
2)Math.Min(Q1.x,Q2.x)≤Math.Max(P1.x,P2.x)
3)Math.Min(P1.y,P2.y)≤Math.Max(Q1.y,Q2.y)
4)Math.Min(Q1.y,Q2.y)≤Math.Max(P1.y,P2.y)
其中：Math.Min()为返回较小值函数；Math.Max()为返回较大值函数；P1.x、P2.x、Q1.x、Q2.x分别表示点P1、P2、Q1、Q2的x坐标；P1.y、P2.y、Q1.y、Q2.y分别表示点P1、P2、Q1、Q2的y坐标。
5.根据权利要求3所述的一种实时室内定位数据优化方法，其特征在于：判断线段P1P2与线段Q1Q2是否相互跨立对方的方法是：当如下条件同时满足时，则线段P1P2和线段Q1Q2相互跨立对方：
( 1 ) ( P 1 Q 1 &RightArrow; &times; Q 2 Q 1 &RightArrow; ) * ( P 2 Q 1 &RightArrow; &times; Q 2 Q 1 &RightArrow; ) < 0 ;
( 2 ) ( Q 1 P 1 &RightArrow; &times; P 2 P 1 &RightArrow; ) * ( Q 2 P 1 &RightArrow; &times; P 2 P 1 &RightArrow; ) < 0 .
6.根据权利要求1所述的一种实时室内定位数据优化方法，其特征在于：步骤五所述对交点进行优化处理的方法是：选取离上一位置点最近的交点，再将该交点向上一位置点方向移动0.1米到0.5米的距离得到新的位置点，然后将当前位置点的坐标替换为新的位置点的坐标。
CN201410189205.9A 2014-05-07 2014-05-07 一种实时室内定位数据优化方法 Active CN103926611B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201410189205.9A CN103926611B (zh) 2014-05-07 2014-05-07 一种实时室内定位数据优化方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201410189205.9A CN103926611B (zh) 2014-05-07 2014-05-07 一种实时室内定位数据优化方法

Publications (2)

Publication Number Publication Date
CN103926611A true CN103926611A (zh) 2014-07-16
CN103926611B CN103926611B (zh) 2016-08-17

Family

ID=51144899

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201410189205.9A Active CN103926611B (zh) 2014-05-07 2014-05-07 一种实时室内定位数据优化方法

Country Status (1)

Country Link
CN (1) CN103926611B (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104853315A (zh) * 2015-04-22 2015-08-19 北京智慧图科技有限责任公司 一种室内定位的地图匹配方法和装置
CN105547299A (zh) * 2015-12-29 2016-05-04 哈尔滨工业大学 基于轨迹匹配的radio map转角矩阵自动生成的WLAN室内定位方法
CN110647141A (zh) * 2018-06-27 2020-01-03 西安合众思壮导航技术有限公司 避障路径的生成方法、装置和系统

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101634699A (zh) * 2008-12-31 2010-01-27 中国科学院计算技术研究所 一种在传感器网络中的定位方法及定位装置
CN103369466A (zh) * 2013-07-10 2013-10-23 哈尔滨工业大学 一种地图匹配辅助室内定位方法
CN103428275A (zh) * 2013-07-30 2013-12-04 苏州两江科技有限公司 基于wsn的室内移动目标行动路线追踪方法

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101634699A (zh) * 2008-12-31 2010-01-27 中国科学院计算技术研究所 一种在传感器网络中的定位方法及定位装置
CN103369466A (zh) * 2013-07-10 2013-10-23 哈尔滨工业大学 一种地图匹配辅助室内定位方法
CN103428275A (zh) * 2013-07-30 2013-12-04 苏州两江科技有限公司 基于wsn的室内移动目标行动路线追踪方法

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104853315A (zh) * 2015-04-22 2015-08-19 北京智慧图科技有限责任公司 一种室内定位的地图匹配方法和装置
CN104853315B (zh) * 2015-04-22 2018-07-10 北京智慧图科技有限责任公司 一种室内定位的地图匹配方法和装置
CN105547299A (zh) * 2015-12-29 2016-05-04 哈尔滨工业大学 基于轨迹匹配的radio map转角矩阵自动生成的WLAN室内定位方法
CN110647141A (zh) * 2018-06-27 2020-01-03 西安合众思壮导航技术有限公司 避障路径的生成方法、装置和系统

Also Published As

Publication number Publication date
CN103926611B (zh) 2016-08-17

Similar Documents

Publication Publication Date Title
US20210109537A1 (en) Autonomous exploration framework for indoor mobile robotics using reduced approximated generalized voronoi graph
Bosché et al. The value of integrating Scan-to-BIM and Scan-vs-BIM techniques for construction monitoring using laser scanning and BIM: The case of cylindrical MEP components
CN108830897B (zh) 一种道路中心线提取方法
US20210162599A1 (en) Robot navigation using 2d and 3d path planning
Van Toll et al. Navigation meshes for realistic multi-layered environments
CN104238560B (zh) 一种非线性路径规划方法及系统
CN106980633B (zh) 室内地图数据的生成方法及装置
Zhang et al. Multiagent approach for real-time collision avoidance and path replanning for cranes
CN103576686B (zh) 一种机器人自主导引及避障的方法
KR20190019897A (ko) 로봇의 경로　계획　시스템, 방법, 로봇 및 매체
KR20210053323A (ko) 동적 확률 모션 계획
KR20190039438A (ko) 자재 핸들링 차량 경로 검증 및 동적 경로 수정
CN108241369B (zh) 机器人躲避静态障碍的方法及装置
CN109540136A (zh) 一种多无人艇协同路径规划方法
CN102169347A (zh) 基于协作协进化和多种群遗传算法的多机器人路径规划系统
Lei et al. Algorithm for mobile crane walking path planning in congested industrial plants
Du et al. Group surfing: A pedestrian-based approach to sidewalk robot navigation
CN107393330B (zh) 人车汇合的路线规划方法及系统、车载终端、智能终端
Narazaki et al. Vision-based navigation planning for autonomous post-earthquake inspection of reinforced concrete railway viaducts using unmanned aerial vehicles
KR20190024467A (ko) 타겟 지향 로컬 경로를 생성하는 방법 및 이를 구현하는 로봇
CN103926611A (zh) 一种实时室内定位数据优化方法
JP2016149090A (ja) 自律移動装置、自律移動システム、自律移動方法、およびプログラム
CN114281076B (zh) 机器人的覆盖移动作业方法
CN105046052A (zh) 一种受限空间内的移动对象最短距离计算方法
Zhang et al. Feature extraction for outdoor mobile robot navigation based on a modified Gauss–Newton optimization approach

Legal Events

Date Code Title Description
C06 Publication
PB01 Publication
C10 Entry into substantive examination
SE01 Entry into force of request for substantive examination
C53 Correction of patent of invention or patent application
CB02 Change of applicant information

Address after: 1803, room 18, building 1, building 360, crystal Road, No. 610017, Hui Lu, Chengdu hi tech Zone, Sichuan

Applicant after: Chengdu Information Technology of Chinese Academy of Sciences Co., Ltd.

Address before: 610041, No. 11, building 5, high tech building, East Road, Chengdu hi tech Zone, Sichuan

Applicant before: Chengdu Information Technology of Chinese Academy of Sciences Co., Ltd.

C14 Grant of patent or utility model
GR01 Patent grant