CN108594852A - 一种已知起点、终点和运动方向的移动路径计算方法 - Google Patents

一种已知起点、终点和运动方向的移动路径计算方法 Download PDF

Info

Publication number
CN108594852A
CN108594852A CN201810371335.2A CN201810371335A CN108594852A CN 108594852 A CN108594852 A CN 108594852A CN 201810371335 A CN201810371335 A CN 201810371335A CN 108594852 A CN108594852 A CN 108594852A
Authority
CN
China
Prior art keywords
line
terminal
entered step
starting point
point
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
CN201810371335.2A
Other languages
English (en)
Other versions
CN108594852B (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 University of Information Technology
Original Assignee
Chengdu University of Information Technology
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 University of Information Technology filed Critical Chengdu University of Information Technology
Priority to CN201810371335.2A priority Critical patent/CN108594852B/zh
Publication of CN108594852A publication Critical patent/CN108594852A/zh
Application granted granted Critical
Publication of CN108594852B publication Critical patent/CN108594852B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • 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
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种已知起点、终点和运动方向的移动路径计算方法，其特征在于，包括以下步骤：当起点A的速度和终点B的速度共线时，进入步骤S2，否则进入步骤S5；当起点A的速度和终点B的速度同向时，进入步骤S3，否则进入步骤S4；当终点B在line(A，Va)直线上时，做异常处理；当终点B不在line(A，Va)直线上时，作运行圆弧轨迹；作出line(A，Va)和line(B，Vb)的交点C和运行圆弧轨迹。根据各种情况判断是否存在由一条直线和一段圆弧构成的移动路径，并找到该移动路径，从起点按照给定方向运动，在飞机、汽车、搬运机器人等自主巡航中构成完整的移动过程。

Description

一种已知起点、终点和运动方向的移动路径计算方法
技术领域
本发明涉及固定翼无人机技术领域，具体涉及一种已知起点、终点和运动方向的移动路径计算方法。
背景技术
自固定翼无人机飞行路径规划、汽车无人驾驶路径规划等应用场合中，为了实现无人驾驶自主巡航的功能，通常需要解决一种基础情况：给定两点以及两点的运动方向，希望找到由一条直线和一段圆弧组组成的路径，使得从起点按照给定方向研制路径运动能够到达第二点，其运动方向与给定方向一致。在飞机、汽车、搬运机器人等自主巡航中，通过一系列的点对对应的这种路径构成巡航的完整移动过程。但在目前的现有技术中，还不能很好的解决此情况。
发明内容
本发明所要解决的技术问题是提供一种已知起点、终点和运动方向的移动路径计算方法，解决无人驾驶中已知起点、终点和运动方向时，移动路径无法确定的问题。
本发明解决上述技术问题的技术方案如下：一种已知起点、终点和运动方向的移动路径计算方法，其特征在于，包括以下步骤：
S1、当起点A的速度和终点B的速度共线时，进入步骤S2，否则进入步骤S5；
S2、当起点A的速度和终点B的速度同向时，进入步骤S3，否则进入步骤S4；
S3、当终点B在line(A，Va)直线上时，做异常处理；
S4、当终点B不在line(A，Va)直线上时，作运行圆弧轨迹；
S5、作出line(A，Va)和line(B，Vb)的交点C和运行圆弧轨迹。
本发明的有益效果是：在本发明中，根据终点位置与起点直线的位置关系、起点运动方向和终点运动方向的关系、起点和终点构成方向与起点运动方向的关系，根据各种情况判断是否存在由一条直线和一段圆弧构成的移动路径，并找到该移动路径，从起点按照给定方向运动，在飞机、汽车、搬运机器人等自主巡航中构成完整的移动过程。
附图说明
图1为本发明总步骤流程图；
图2为本发明步骤S4的流程图；
图3为本发明步骤S5的流程图；
图4为本发明实施例中终点B点在投影点A＇前方时的工作原理图；
图5为本发明实施例中终点B点在投影点A＇后方时的工作原理图；
图6为本发明实施例中在的右边、在的左边、与同向且时的工作原理图；
图7为本发明实施例中在的右边、在的左边、与同向且时的工作原理图；
图8为本发明实施例中在的右边、在的左边、与反向且时的工作原理图；
图9为本发明实施例中在的右边、在的右边、与同向时的工作原理图；
图10为本发明实施例中在的右边、在的右边、与反向时的工作原理图；
图11为本发明实施例中在的左边、在的左边、与同向时的工作原理图；
图12为本发明实施例中在的左边、在的左边、与反向且时的工作原理图；
图13为本发明实施例中在的左边、在的左边、与反向且时的工作原理图；
图14为本发明实施例中在的左边、在的右边、与同向且时的工作原理图；
图15为本发明实施例中在的左边、在的右边、与同向且时的工作原理图；
图16为本发明实施例中在的左边、在的右边、与反向时的工作原理图。
具体实施方式
以下结合附图对本发明的原理和特征进行描述，所举实例只用于解释本发明，并非用于限定本发明的范围。
如图1所示，一种已知起点、终点和运动方向的移动路径计算方法，其特征在于，包括以下步骤：
S1、当起点A的速度和终点B的速度共线时，进入步骤S2，否则进入步骤S5；
S2、当起点A的速度和终点B的速度同向时，进入步骤S3，否则进入步骤S4；
S3、当终点B在line(A，Va)直线上时，做异常处理，line(A，Va)为过A点且方向与共线的直线；
S4、当终点B不在line(A，Va)直线上时，作运行圆弧轨迹；
S5、作出line(A，Va)和line(B，Vb)的交点C和运行圆弧轨迹，line(B，Vb)为过B点且方向与共线的直线。
如图2所示，所述步骤S4具体包括以下步骤：
S41、如图4所示，作起点A点在line(B，Vb)的投影点A＇，当终点B点在投影点A＇前方时，进入步骤S42，否则进入步骤S43；
S42、设起点A为切点，连接AA＇，取AA＇的中点O，以O点为圆心，OA为半径，画圆弧AA＇，并连接A＇B，作运行圆弧轨迹AA＇B；
S43、如图5所示，作终点B点在line(A，Va)的投影点B＇，设终点B为切点，连接BB＇，取BB＇的中点O＇，以O＇为圆心，O＇B为半径，画圆弧BB＇，并连接AB＇，作运行圆弧轨迹AB＇B。
如图3所示，所述步骤S5具体包括以下步骤：
S51、当在的右边时，进入步骤S52，否则进入步骤S53；
S52、当在的左边时，进入步骤S54，否则进入步骤S55；
S53、当在的左边时，进入步骤S56，否则进入步骤S57；
S54、当与同向时，进入步骤S58，否则进入步骤S510；
S55、当与同向时，进入步骤S510，否则进入步骤S511；
S56、当与同向时，进入步骤S510，否则进入步骤S59；
S57、当与同向时，进入步骤S58，否则进入步骤S511；
S58、当时，进入步骤S510，否则进入步骤S511；
S59、当时，进入步骤S511，否则进入步骤S510；
S510、设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB；
S511、设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
在本发明实施例中，如图6所示，当在的右边、在的左边、与同向且时，设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB。
在本发明实施例中，如图7所示，当在的右边、在的左边、与同向且时，设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
在本发明实施例中，如图8所示，当在的右边、在的左边、与反向且时，设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
在本发明实施例中，如图9所示，当在的右边、在的右边、与同向时，设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB。
在本发明实施例中，如图10所示，当在的右边、在的右边、与反向时，设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
在本发明实施例中，如图11所示，当在的左边、在的左边、与同向时，设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB。
在本发明实施例中，如图12所示，当在的左边、在的左边、与反向且时，设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
在本发明实施例中，如图13所示，当在的左边、在的左边、与反向且时，设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB。
在本发明实施例中，如图14所示，当在的左边、在的右边、与同向且时，设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB。
在本发明实施例中，如图15所示，当在的左边、在的右边、与同向且时，设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
在本发明实施例中，如图16所示，当在的左边、在的右边、与反向时，设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (3)

1.一种已知起点、终点和运动方向的移动路径计算方法，其特征在于，包括以下步骤：
S1、当起点A的速度和终点B的速度共线时，进入步骤S2，否则进入步骤S5；
S2、当起点A的速度和终点B的速度同向时，进入步骤S3，否则进入步骤S4；
S3、当终点B在line(A，Va)直线上时，做异常处理；
S4、当终点B不在line(A，Va)直线上时，作运行圆弧轨迹；
S5、作出line(A，Va)和line(B，Vb)的交点C和运行圆弧轨迹。
2.根据权利要求1所述的已知起点、终点和运动方向的移动路径计算方法，其特征在于，所述步骤S4具体包括以下步骤：
S41、作起点A点在line(B，Vb)的投影点A＇，当终点B点在投影点A＇前方时，进入步骤S42，否则进入步骤S43；
S42、设起点A为切点，连接AA＇，取AA＇的中点O，以O点为圆心，OA为半径，画圆弧AA＇，并连接A＇B，作运行圆弧轨迹AA＇B；
S43、作终点B点在line(A，Va)的投影点B＇，设终点B为切点，连接BB＇，取BB＇的中点O＇，以O＇为圆心，O＇B为半径，画圆弧BB＇，并连接AB＇，作运行圆弧轨迹AB＇B。
3.根据权利要求1所述的已知起点、终点和运动方向的移动路径计算方法，其特征在于，所述步骤S5具体包括以下步骤：
S51、当在的右边时，进入步骤S52，否则进入步骤S53；
S52、当在的左边时，进入步骤S54，否则进入步骤S55；
S53、当在的左边时，进入步骤S56，否则进入步骤S57；
S54、当与同向时，进入步骤S58，否则进入步骤S510；
S55、当与同向时，进入步骤S510，否则进入步骤S511；
S56、当与同向时，进入步骤S510，否则进入步骤S59；
S57、当与同向时，进入步骤S58，否则进入步骤S511；
S58、当时，进入步骤S510，否则进入步骤S511；
S59、当时，进入步骤S511，否则进入步骤S510；
S510、设起点A点为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作起点A关于矢量线的对称点Tb，在Tb点作垂直于并过圆心的矢量线与的交点为圆心O，以OA为半径画圆弧ATb，并连接TbB，运行圆弧轨迹ATbB；
S511、设终点B为切点，作与夹角的角平分线作垂直于并过圆心的矢量线作终点B关于矢量线的对称点Ta，在Ta点作垂直于并过圆心的矢量线与的交点为圆心O，连接ATa，并以OB为半径画圆弧TaB，作运行圆弧轨迹ATaB。
CN201810371335.2A 2018-04-23 2018-04-23 一种已知起点、终点和运动方向的移动路径计算方法 Expired - Fee Related CN108594852B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810371335.2A CN108594852B (zh) 2018-04-23 2018-04-23 一种已知起点、终点和运动方向的移动路径计算方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810371335.2A CN108594852B (zh) 2018-04-23 2018-04-23 一种已知起点、终点和运动方向的移动路径计算方法

Publications (2)

Publication Number Publication Date
CN108594852A true CN108594852A (zh) 2018-09-28
CN108594852B CN108594852B (zh) 2021-03-30

Family

ID=63614781

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810371335.2A Expired - Fee Related CN108594852B (zh) 2018-04-23 2018-04-23 一种已知起点、终点和运动方向的移动路径计算方法

Country Status (1)

Country Link
CN (1) CN108594852B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110000792A (zh) * 2019-04-26 2019-07-12 西湖大学 船壁清洗机器人的路径调整方法、装置、设备及存储介质
US20210190531A1 (en) * 2020-05-28 2021-06-24 Beijing Baidu Netcom Science Technology Co., Ltd. Ar navigation method and apparatus

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080154490A1 (en) * 2006-12-22 2008-06-26 Thales Method and device for calculating a path which is laterally offset with respect to a reference path
CN104571129A (zh) * 2014-12-26 2015-04-29 北京控制工程研究所 一种巡视器航向到位控制轨迹确定方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20080154490A1 (en) * 2006-12-22 2008-06-26 Thales Method and device for calculating a path which is laterally offset with respect to a reference path
CN104571129A (zh) * 2014-12-26 2015-04-29 北京控制工程研究所 一种巡视器航向到位控制轨迹确定方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
胡倩菲: "有向物体的路径规划研究", 《中国优秀硕士学位论文全文数据库（电子期刊）》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110000792A (zh) * 2019-04-26 2019-07-12 西湖大学 船壁清洗机器人的路径调整方法、装置、设备及存储介质
US20210190531A1 (en) * 2020-05-28 2021-06-24 Beijing Baidu Netcom Science Technology Co., Ltd. Ar navigation method and apparatus

Also Published As

Publication number Publication date
CN108594852B (zh) 2021-03-30

Similar Documents

Publication Publication Date Title
CN110262548B (zh) 一种考虑抵达时间约束的无人机航迹规划方法
CN107085938B (zh) 基于车道线与gps跟随的智能驾驶局部轨迹容错规划方法
CN106647814A (zh) 一种基于二维码地标识别的无人机视觉辅助定位与飞控系统及方法
Park et al. Feedback motion planning via non-holonomic RRT* for mobile robots
CN108594852A (zh) 一种已知起点、终点和运动方向的移动路径计算方法
Faigl et al. On solution of the Dubins touring problem
CN110989682B (zh) 基于单基站的无人机精准降落方法
US20210216073A1 (en) Vehicle control system, vehicle control method, and program
CN111006667A (zh) 高速场景下的自动驾驶轨迹生成系统
CN111397604A (zh) 一种分析轨迹数据的方法、相关设备及存储介质
Zhuge et al. A novel dynamic obstacle avoidance algorithm based on collision time histogram
CN113805597B (zh) 基于粒子群算法的障碍物自我保护人工势场法局部路径规划方法
CN111207752A (zh) 一种基于动态切点调整的无人机航迹规划方法
CN112415999B (zh) 自动驾驶路径点的处理方法、装置、设备及存储介质
CN109623825A (zh) 一种移动轨迹规划方法、装置、设备和存储介质
Huang et al. Monocular vision-based autonomous navigation system on a toy quadcopter in unknown environments
JP2017010292A (ja) Ａｇｖ軌道計算装置及び方法
CN114771551B (zh) 自动驾驶车辆轨迹规划方法、装置和自动驾驶车辆
CN114550571A (zh) 一种车道线标注方法、装置、存储介质及计算机程序产品
Farrugia et al. Swarm robotics for object transportation
CN109353339A (zh) 一种车辆导航方法以及系统
Lewis et al. A framework for visual return-to-home capability in GPS-denied environments
Salaris et al. Shortest paths for wheeled robots with limited field-of-view: introducing the vertical constraint
Khalifa et al. Vehicles platooning in urban environments: Integrated consensus-based longitudinal control with gap closure maneuvering and collision avoidance capabilities
Jang et al. Navigation-assistant path planning within a mav team

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
CF01 Termination of patent right due to non-payment of annual fee
CF01 Termination of patent right due to non-payment of annual fee

Granted publication date: 20210330