CN108614552B - 一种基于离散位姿的路径规划方法 - Google Patents

一种基于离散位姿的路径规划方法 Download PDF

Info

Publication number
CN108614552B
CN108614552B CN201810307947.5A CN201810307947A CN108614552B CN 108614552 B CN108614552 B CN 108614552B CN 201810307947 A CN201810307947 A CN 201810307947A CN 108614552 B CN108614552 B CN 108614552B
Authority
CN
China
Prior art keywords
key frame
path
norm
containing information
vertex
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
CN201810307947.5A
Other languages
English (en)
Other versions
CN108614552A (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.)
Zhejiang Dacheng Robot Technology Co ltd
Original Assignee
Zhejiang Dacheng Robot 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 Zhejiang Dacheng Robot Technology Co ltd filed Critical Zhejiang Dacheng Robot Technology Co ltd
Priority to CN201810307947.5A priority Critical patent/CN108614552B/zh
Publication of CN108614552A publication Critical patent/CN108614552A/zh
Application granted granted Critical
Publication of CN108614552B publication Critical patent/CN108614552B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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/0217—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with energy consumption, time reduction or distance reduction criteria
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Management, Administration, Business Operations System, And Electronic Commerce (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于离散位姿的路径规划方法，包括以下步骤：步骤1：将关键帧转化为顶点，将关键帧集合SetK中的每一个关键帧Ki加入到顶点集合SetV中；步骤2：计算边集合SetE({E1...,Ei...,Em})，Ei包含信息{Vc1,Vc2,C}，其中，Vc1与Vc2为SetK中的任意两个不相同的顶点，C表示两个顶点相连的代价；步骤3：给定目标位置，并进行路径规划。本发明基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧，解决了基于离散位姿的路径规划，保证机器人使用本发明可以运行的稳定可靠。

Description

一种基于离散位姿的路径规划方法
技术领域
本发明涉及路径规划方法技术领域，具体涉及一种基于离散位姿的路径规划方法。
背景技术
视觉SLAM系统可以对环境进行地图构建，得到的地图形式通常为一系列关键帧和空间点的集合。该地图可以保存并进一步应用于机器人定位导航。机器人导航则需要实时的基于地图进行路径规划。
基于视觉SLAM构图算法得到的地图有一定的特殊性，即在关键帧位置附近的区域，定位精度和对环境变化的抗干扰能力比较好，因此，在机器人进行自主导航时，为了保证高可靠性，需要保证机器人在关键帧附近工作。本发明解决了基于离散位姿地图的路径规划问题，任意指定机器人目标位置，可以快速得到从当前位置到目标位置的路径，并且使得路径尽量在关键帧位置上，从而在满足路径规划需求的同时，使机器人尽量稳定可靠的运行。
发明内容
本发明提供了一种基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧。
本发明所述地图包括关键帧集合
Figure BDA0001621543540000011
与三维点集合SetM
Figure BDA0001621543540000012
Nk，NM分别表示两个集合的元素数目，Nk为关键帧集合的元素数目，NM为三维点集合SetM的元素数目。本发明只关注关键帧集合SetK。Ki表示第i个关键帧，Ki包含下列信息{ti,Ri,Ii}，ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，Ii为采集到的图像。
本发明所述图Graph包括顶点集合SetV({V1...,Vi...,Vn})，Vi由上述地图中的Ki得到，Vi包含信息{ti,Ri}；边集合SetE({E1...,Ei...,Em})，Ei包含信息{Vc1,Vc2,C}，其中Vc1与Vc2为SetK中的任意两个不相同的顶点，C表示两个顶点相连的代价，计算方式如下所示。
本发明采用的技术方案如下：
一种基于离散位姿的路径规划方法，包括以下步骤：
步骤1：将关键帧转化为顶点，将关键帧集合SetK中的每一个关键帧Ki加入到顶点集合SetV中。
步骤1中，通过现有的视觉SLAM系统可以对环境进行地图构建，得到的地图包括：关键帧集合
Figure BDA0001621543540000021
Nk为关键帧集合的元素数目，K1表示第1个关键帧，
Figure BDA0001621543540000023
表示第Nk个关键帧，Ki表示第i个关键帧，Ki包含下列信息{ti,Ri,Ii}，ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，Ii为采集到的图像；
顶点集合SetV({V1...,Vi...,Vn})，Vi包含信息{ti,Ri}，Vi中的{ti,Ri}和Ki中的{ti,Ri}具有相同含义。
步骤2：计算边集合SetE({E1...,Ei...,Em})，Ei包含信息{Vc1,Vc2,C}，其中，Vc1与Vc2为SetK中的任意两个不相同的顶点，C表示两个顶点相连的代价，采用下列步骤计算任意{Vc1,Vc2}之间的代价C。
步骤2.1：Vc1包含信息{tc1,Rc1}，Vc2包含信息{tc2,Rc2}，计算tc1与tc2之间的距离Distc1_c2：
Distc1_c2＝norm(tc1-tc2)；
其中，norm(v)为计算向量v的模，norm(tc1-tc2)为计算向量tc1-tc2的模，norm为计算函数；
如果Distc1_c2大于阈值DisThres，则C设置为+∞，将{Vc1,Vc2,C}加入到SetE，否则执行步骤2.2(即如果Distc1_c2小于等于阈值DisThres，则执行步骤2.2)；
步骤2.2：计算Vc1,Vc2之间的朝向误差DistAnglec1_c2：
Figure BDA0001621543540000022
该步骤计算Vc1,Vc2之间的朝向误差DistAnglec1_c2。继续执行步骤2.3.
步骤2.3：计算Vc2在Vc1中的位置，并得到路径角度误差DistPathc1_c2。
Oc＝Rc1×tc2+tc1
DistPathc1_c2＝1-Oc(3)/norm(Oc)
其中Oc表示Vc2相对于Vc1的位置，为3×1矩阵，Oc(i)表示获取Oc的第i个元素，Oc(3)表示获取Oc的第3个元素，norm为函数。
步骤2.4：计算C.
C＝(DistAnglec1_c2+DistPathc1_c2+α)×Distc1_c2
其中，α为人为设定阈值，将{Vc1,Vc2,C}加入到SetE。
步骤3：给定目标位置，并进行路径规划。
步骤3.1：给定目标位置{td,Rd}，在SetV中查找距离目标位置最近的顶点Vend，即遍历SetV中的元素，找到Vend使得下列公式最小：
distmin＝norm(tend-td)；
Vend包含信息{tend,Rend}。
步骤3.2：获取当前位置{tc,Rc}，在SetV中查找距离当前位置最近的顶点Vstart，即遍历SetV中的元素，找到Vstart使得下列公式最小：
distmin＝norm(tstart-tc)；
Vstart包含信息{tstart,Rstart}。
步骤3.3：对SetV和SetE使用Dijkstra算法(迪杰斯特拉算法)在地图中查找最短路径，使得Vstart与Vend之间距离最小，得到顶点集合Listpath＝{Vstart...Vpi...Vend}，其中Vpi∈SetV。
步骤3.4：将当前位置和目标位置加入到Listpath，最终得到路径Listpath＝{{tc,Rc},Vstart...Vpi...Vend,{td,Rd}}。
综上所述，由于采用了上述技术方案，本发明的有益效果是：
本发明基于离散位姿的路径规划方法，给定基于关键帧的地图，在该地图进行快速路径规划，并尽可能使规划后的路径靠近关键帧，解决了基于离散位姿的路径规划，保证机器人使用本发明可以运行的稳定可靠。
附图说明
图1为本发明基于离散位姿的路径规划方法的流程示意图。
具体实施方式
本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任意方式组合。
实施例一：
步骤1：将关键帧转化为顶点。将地图中的每一个关键帧Ki加入到顶点集合SetV中。
步骤1中，通过现有的视觉SLAM系统可以对环境进行地图构建，得到的地图包括：关键帧集合
Figure BDA0001621543540000041
Nk为关键帧集合的元素数目，K1表示第1个关键帧，
Figure BDA0001621543540000042
表示第Nk个关键帧，Ki表示第i个关键帧，Ki包含下列信息{ti,Ri,Ii}，ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，Ii为采集到的图像；
顶点集合SetV({V1...,Vi...,Vn})，Vi包含信息{ti,Ri}，Vi中的{ti,Ri}和Ki中的{ti,Ri}具有相同含义。
步骤2：计算边集合SetE({E1...,Ei...,Em})，Ei包含信息{Vc1,Vc2,C}，其中，Vc1与Vc2为SetK中的任意两个不相同的顶点，C表示两个顶点相连的代价，采用下列步骤计算任意{Vc1,Vc2}之间的代价C。
步骤2.1：Vc1包含信息{tc1,Rc1}，Vc2包含信息{tc2,Rc2}，计算tc1与tc2之间的距离Distc1_c2。
Distc1_c2＝norm(tc1-tc2)
其中，norm(v)为计算向量v的模，norm(tc1-tc2)为计算向量tc1-tc2的模，norm为计算函数；
如果Distc1_c2大于阈值DisThres，则C设置为+∞，将{Vc1,Vc2,C}加入到SetE，否则执行步骤2.2。
步骤2.2：计算Vc1,Vc2之间的朝向误差DistAnglec1_c2。
Figure BDA0001621543540000043
该步骤计算Vc1,Vc2之间的朝向误差DistAnglec1_c2，继续执行步骤2.3.
步骤2.3：Vc1包含信息{tc1,Rc1}，Vc2包含信息{tc2,Rc2}，计算Vc2在Vc1中的位置，并得到路径角度误差。
Oc＝Rc1×tc2+tc1
DistPathc1_c2＝1-Oc(3)/norm(Oc)；
其中Oc表示Vc2相对于Vc1的位置，为3×1矩阵，Oc(i)表示获取Oc的第i个元素，Oc(3)表示获取Oc的第3个元素，norm为函数。
步骤2.4：计算C.
C＝(DistAnglec1_c2+DistPathc1_c2+α)×Distc1_c2
其中α为人为设定阈值。将{Vc1,Vc2,C}加入到SetE。
步骤3：给定目标位置，并进行路径规划。
步骤3.1：给定目标位置{td,Rd}，在SetV中查找距离目标位置最近的顶点Vend，即遍历SetV中的元素，找到Vend使得下列公式最小：
distmin＝norm(tend-td)
Vend包含信息{tend,Rend}。
步骤3.2：获取当前位置{tc,Rc}，在SetV中查找距离目标位置最近的顶点Vstart，如步骤3.1中所述。即遍历SetV中的元素，找到Vstart使得下列公式最小：
distmin＝norm(tstart-tc)；
Vstart包含信息{tstart,Rstart}。
步骤3.3：对SetV和SetE使用Dijkstra算法在构建的地图查找最短路径，使得Vstart与Vend之间距离最小。得到顶点集合Listpath＝{Vstart...Vpi...Vend}，其中Vpi∈SetV。
步骤3.4：将当前位置和目标位置加入到Listpath，最终得到路径Listpath＝{{tc,Rc},Vstart...Vpi...Vend,{td,Rd}}。

Claims (2)

1.一种基于离散位姿的路径规划方法，其特征在于，包括以下步骤：
步骤1：将关键帧转化为顶点，将关键帧集合SetK中的每一个关键帧Ki加入到顶点集合SetV中；
步骤2：计算边集合SetE({E1...,Ei...,Em})，Ei表示边集合SetE中的第i个元素，Ei包含信息{Vc1,Vc2,C}，其中，Vc1与Vc2为SetK中的任意两个不相同的顶点，C表示两个顶点相连的代价；
采用下列步骤计算任意{Vc1,Vc2}之间的代价C，具体包括：
步骤2.1：Vc1包含信息{tc1,Rc1}，Vc2包含信息{tc2,Rc2}，计算tc1与tc2之间的距离Distc1_c2：
Distc1_c2＝norm(tc1-tc2)；
其中，norm(tc1-tc2)为计算向量tc1-tc2的模，norm为计算函数；
如果Distc1_c2大于阈值DisThres，则C设置为+∞，将{Vc1,Vc2,C}加入到SetE，否则执行步骤2.2；
步骤2.2：计算Vc1,Vc2之间的朝向误差DistAnglec1_c2：
Figure FDA0002748735290000011
该步骤计算Vc1,Vc2之间的朝向误差DistAnglec1_c2，继续执行步骤2.3；
步骤2.3：计算Vc2在Vc1中的位置，并得到路径角度误差DistPathc1_c2；
Oc＝Rc1×tc2+tc1
DistPathc1_c2＝1-Oc(3)/norm(Oc)
其中Oc表示Vc2相对于Vc1的位置，为3×1矩阵，Oc(i)表示获取Oc的第i个元素，Oc(3)表示获取Oc的第3个元素，norm为函数；
步骤2.4：计算C；
C＝(DistAnglec1_c2+DistPathc1_c2+α)×Distc1_c2
其中，α为人为设定阈值，将{Vc1,Vc2,C}加入到SetE；
步骤3：给定目标位置，并进行路径规划，具体包括：
步骤3.1：给定目标位置{td,Rd}，在SetV中查找距离目标位置最近的顶点Vend，即遍历SetV中的元素，找到Vend使得下列公式最小：
distmin＝norm(tend-td)；
Vend包含信息{tend,Rend}；
步骤3.2：获取当前位置{tc,Rc}，在SetV中查找距离当前位置最近的顶点Vstart，即遍历SetV中的元素，找到Vstart使得下列公式最小：
distmin＝norm(tstart-tc)；
Vstart包含信息{tstart,Rstart}；
步骤3.3：对SetV和SetE使用Dijkstra算法在地图中查找最短路径，使得Vstart与Vend之间距离最小，得到顶点集合Listpath＝{Vstart...Vpi...Vend}，其中Vpi∈SetV；
步骤3.4：将当前位置和目标位置加入到Listpath，最终得到路径Listpath＝{{tc,Rc},Vstart...Vpi...Vend,{td,Rd}}。
2.根据权利要求1所述的基于离散位姿的路径规划方法，其特征在于，步骤1中，通过视觉SLAM系统对环境进行地图构建，得到的地图包括：关键帧集合
Figure FDA0002748735290000021
Nk为关键帧集合的元素数目，K1表示第1个关键帧，
Figure FDA0002748735290000022
表示第Nk个关键帧，Ki表示第i个关键帧，Ki包含下列信息{ti,Ri,Ii}，ti表示关键帧Ki在世界坐标系中的位置，为3×1矩阵，Ri表示关键帧Ki的朝向，为3×3矩阵，Ii为采集到的图像；
顶点集合SetV({V1...,Vi...,Vn})，Vi包含信息{ti,Ri}，Vi中的{ti,Ri}和Ki中的{ti,Ri}具有相同含义。
CN201810307947.5A 2018-04-08 2018-04-08 一种基于离散位姿的路径规划方法 Active CN108614552B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810307947.5A CN108614552B (zh) 2018-04-08 2018-04-08 一种基于离散位姿的路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810307947.5A CN108614552B (zh) 2018-04-08 2018-04-08 一种基于离散位姿的路径规划方法

Publications (2)

Publication Number Publication Date
CN108614552A CN108614552A (zh) 2018-10-02
CN108614552B true CN108614552B (zh) 2021-01-12

Family

ID=63659611

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810307947.5A Active CN108614552B (zh) 2018-04-08 2018-04-08 一种基于离散位姿的路径规划方法

Country Status (1)

Country Link
CN (1) CN108614552B (zh)

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2008013568A2 (en) * 2005-12-30 2008-01-31 Irobot Corporation Autonomous mobile robot
WO2008070208A2 (en) * 2006-07-05 2008-06-12 Battelle Energy Alliance, Llc Multi-robot control interface
CN103500447A (zh) * 2013-09-18 2014-01-08 中国石油大学(华东) 基于增量式高次布尔能量最小化的视频前后景分割方法
CN103914685A (zh) * 2014-03-07 2014-07-09 北京邮电大学 一种基于广义最小团图和禁忌搜索的多目标跟踪方法
CN105772905A (zh) * 2016-03-16 2016-07-20 南京工业大学 一种基于弧焊机器人系统的斜交偏置管轨迹规划方法
CN105867381A (zh) * 2016-04-25 2016-08-17 广西大学 一种基于概率地图的工业机器人路径搜索优化算法
CN107063258A (zh) * 2017-03-07 2017-08-18 重庆邮电大学 一种基于语义信息的移动机器人室内导航方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101619076B1 (ko) * 2009-08-25 2016-05-10 삼성전자 주식회사 모바일 플랫폼의 동적 물체 검출 및 추적 방법

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2008013568A2 (en) * 2005-12-30 2008-01-31 Irobot Corporation Autonomous mobile robot
WO2008070208A2 (en) * 2006-07-05 2008-06-12 Battelle Energy Alliance, Llc Multi-robot control interface
CN103500447A (zh) * 2013-09-18 2014-01-08 中国石油大学(华东) 基于增量式高次布尔能量最小化的视频前后景分割方法
CN103914685A (zh) * 2014-03-07 2014-07-09 北京邮电大学 一种基于广义最小团图和禁忌搜索的多目标跟踪方法
CN105772905A (zh) * 2016-03-16 2016-07-20 南京工业大学 一种基于弧焊机器人系统的斜交偏置管轨迹规划方法
CN105867381A (zh) * 2016-04-25 2016-08-17 广西大学 一种基于概率地图的工业机器人路径搜索优化算法
CN107063258A (zh) * 2017-03-07 2017-08-18 重庆邮电大学 一种基于语义信息的移动机器人室内导航方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
A Path Planning Framework for Indoor Low-cost Mobile Robots ;Sixiang Zuo等;《Proceedings of the 2017 IEEE International Conference on Information and Automation (ICIA)》;20170731;第18-23页 *
机器人视觉伺服研究进展: 视觉系统与控制策略;贾丙西，等;《自动化 学报》;20150531;第41卷(第5期);第861-873页 *

Also Published As

Publication number Publication date
CN108614552A (zh) 2018-10-02

Similar Documents

Publication Publication Date Title
CN106840148B (zh) 室外作业环境下基于双目摄像机的可穿戴式定位与路径引导方法
KR20170088228A (ko) 다중로봇의 자기위치인식에 기반한 지도작성 시스템 및 그 방법
CN107300382B (zh) 一种用于水下机器人的单目视觉定位方法
CN112965063B (zh) 一种机器人建图定位方法
JP7300550B2 (ja) 視覚標識に基づき標識マップを構築する方法、装置
WO2021016806A1 (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN105096341A (zh) 基于三焦张量和关键帧策略的移动机器人位姿估计方法
JP6922348B2 (ja) 情報処理装置、方法、及びプログラム
CN110930444B (zh) 一种基于双边优化的点云匹配方法、介质、终端和装置
CN116592897A (zh) 基于位姿不确定性的改进orb-slam2定位方法
WO2021016803A1 (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN114370878A (zh) 一种基于stackf的多auv协同定位方法
CN108614552B (zh) 一种基于离散位姿的路径规划方法
CN110853098B (zh) 机器人定位方法、装置、设备及存储介质
WO2021051361A1 (zh) 高精度地图定位方法、系统、平台及计算机可读存储介质
CN111444467A (zh) 基于实时定位轨迹数据进行局部线性插值和预测的方法
CN110926405A (zh) 一种基于单目视觉灭点检测的arv姿态测量方法
CN113091743B (zh) 机器人的室内定位方法及装置
JPH07146121A (ja) 視覚に基く三次元位置および姿勢の認識方法ならびに視覚に基く三次元位置および姿勢の認識装置
Lee et al. Robust Robot Navigation using Polar Coordinates in Dynamic Environments
CN113324550A (zh) 一种基于改进航迹积分的航迹修正方法及存储介质
Silveira et al. Visual servoing over unknown, unstructured, large-scale scenes
CN111435244A (zh) 一种回环闭合方法、装置及机器人
CN113074729B (zh) 一种基于地标识别的agv导航定位方法
JP2022138036A (ja) 情報処理装置、情報処理方法及びプログラム

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