CN104571129B - 一种巡视器航向到位控制轨迹确定方法 - Google Patents
一种巡视器航向到位控制轨迹确定方法 Download PDFInfo
- Publication number
- CN104571129B CN104571129B CN201410827873.XA CN201410827873A CN104571129B CN 104571129 B CN104571129 B CN 104571129B CN 201410827873 A CN201410827873 A CN 201410827873A CN 104571129 B CN104571129 B CN 104571129B
- Authority
- CN
- China
- Prior art keywords
- arc
- points
- track
- point
- course
- 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
Links
Landscapes
- Processing Or Creating Images (AREA)
Abstract
本发明公开了一种巡视器航向到位控制轨迹确定方法,实现步骤包括:航向到位初始轨迹判断,选取平滑轨迹中途点,确定最终轨迹。本发明将点到位和航向控制分开,点到位后再通过原地转向方式调整航向,降低了探测效率,增加了控制复杂性。
Description
技术领域
本发明涉及一种巡视器航向到位控制轨迹确定方法,属于移动机器人导航控制领域。
背景技术
以指定航向到达某个指定位置,是巡视器导航控制要求之一。在当前位置和航向已知的情况下,需设计合理运动轨迹,使其移动至指定目标位置,且到达该位置时航向角为指定值。控制轨迹应满足:第一,完整运动轨迹由一个或多个弧形轨迹(或直线)组成;第二,弧形轨迹(或直线)数量尽可能少;第三,轨迹运动平滑,避免曲率过大的情况。
已有移动机器人的运动控制方法大多采用点对点运动控制方式,保证点到位精度,没有考虑到位后的航向问题。
发明内容
本发明要解决的技术问题是:为克服现有技术的不足,提出一种巡视器航向到位控制轨迹确定方法,以使得巡视器在满足航向要求的同时,能够以一到两条弧形轨迹或直线平滑运动至目标位置。
本发明的技术解决方案:
一种巡视器航向到位控制轨迹确定方法,具体步骤如下:
(1)航向到位初始轨迹判断
记巡视器当前点为A点,目标点为B点;
过A、B两点做弧,使其与A点偏航方向相切,记为弧1,对应曲率为CA;过A、B两点做弧,使其与B点偏航方向相切,记为弧2,对应曲率为CB;
若CA=CB,弧1与弧2重合,则A、B两点之间的运动轨迹为一条弧形或直线,该轨迹的期望曲率为C=CA=CB,期望弧长即为弧1或弧2的弧长,轨迹确定完成;
若CA≠CB,则转入步骤(2);
(2)选取平滑轨迹中途点
过A、B两点做弧,使其曲率为CM=(CA+CB)/2,记为弧3,取弧3的中点为轨迹中途点,记为点M;
(3)确定最终轨迹
比较|CA|和|CB|,
若|CA|≤|CB|,则过B、M两点做圆,使其与B点偏航方向相切,记其圆点为OB,过A点做圆,使其与圆OB-B及A点偏航方向相切,记其圆点为OA,圆OA-A与圆OB-B的切点记为P,OA、P、OB共线;
若|CA|>|CB|,则过A、M两点做圆,使其与A点偏航方向相切,记其圆点为OA,过B点做圆,使其与圆OA-A及B点偏航方向相切,记其圆点为OB,圆OA-A与圆OB-B的切点记为P,OA、P、OB共线;
此时A、B两点之间的运动轨迹由两条弧形或直线组成,分别为AP和BP,轨迹确定完成。
曲率的正负符号定义为:若弧形轨迹圆点位于当前点右侧,此时弧形对应巡视器右转,曲率为正;若弧形轨迹圆点位于当前点左侧,此时弧形对应巡视器左转,曲率为负;若弧形轨迹圆点为无限远点,轨迹变为直线,对应巡视器直行,曲率为零。
本发明与现有技术相比的优点在于:
(1)本发明将点到位和航向控制分开,点到位后再通过原地转向方式调整航向,降低了探测效率,增加了控制复杂性;
(2)本发明的完整运动轨迹由一个或多个弧形轨迹或直线组成、轨迹运动平滑,不会出现曲率过大导致控制偏差增大的情况。
附图说明
图1为巡视器当前位置航向与目标位置航向关系示意图;
图2为确定轨迹中途点M示意图;
图3为确定移动轨迹示意图;
图4为确定运动轨迹示意图。
具体实施方式
下面结合附图及具体实施例对本发明进行详细描述。
已知巡视器起始点位置及航向为(x1,y1,ψ1)=(0,0,0°),目标点位置及航向为(x2,y2,ψ2)=(-1,5,30°)。确定巡视器的期望运动轨迹,步骤如下:
(1)计算边界曲率
记巡视器当前点为A点,目标点为B点。
过A、B两点做弧,使其与A点偏航方向相切,记为弧1,对应曲率CA为-0.0769;过A、B两点做弧,使其与B点偏航方向相切,记为弧2,对应曲率CB为0.2589。
(2)确定轨迹中途点M
过A、B两点做弧,曲率CM为0.0910,记为弧3,取弧3的中点为轨迹中途点,记为点M,坐标为(2.4412,-0.7940)。
(3)确定移动轨迹
比较|CA|和|CB|,有|CA|≤|CB|,则过B、M两点做圆,使其与B点偏航方向相切,圆点OB为(3.8699,0.9574),过A点做圆,使其与圆OB-B及A点偏航方向相切,圆点OA为(0,-4.1387),圆OA-A与圆OB-B的切点P为(2.5030,-0.8426),OA、P、OB共线。
此时A、B两点之间的运动轨迹由两条弧形组成,分别为弧AP和弧BP,AP的曲率为-0.2416,弧长为2.6880,BP的曲率为0.4424,弧长为2.6514。轨迹确定完成。运动轨迹如图4所示。
具体要求可明确为:已知巡视器当前点的位置坐标及偏航方向(x1,y1,ψ1)及目标点的位置坐标及偏航方向(x2,y2,ψ2),如图1所示,计算连接当前点和目标点的一个或多个弧形轨迹(或直线)的曲率及弧长。
其中曲率的正负符号定义为:若弧形轨迹圆点位于当前点右侧,此时弧形对应巡视器右转,曲率为正;若弧形轨迹圆点位于当前点左侧,此时弧形对应巡视器左转,曲率为负;若弧形轨迹圆点为无限远点,轨迹变为直线,对应巡视器直行,曲率为零。
Claims (2)
1.一种巡视器航向到位控制轨迹确定方法,其特征在于,具体步骤如下:
(1)航向到位初始轨迹判断
记巡视器当前点为A点,目标点为B点;
过A、B两点做弧,使其与A点航向相切,记为弧1,对应曲率为CA;过A、B两点做弧,使其与B点航向相切,记为弧2,对应曲率为CB;
若CA=CB,弧1与弧2重合,则A、B两点之间的运动轨迹为一条弧形或直线,该轨迹的期望曲率为C=CA=CB,期望弧长即为弧1或弧2的弧长,轨迹确定完成;
若CA≠CB,则转入步骤(2);
(2)选取平滑轨迹中途点
过A、B两点做弧,使其曲率为CM=(CA+CB)/2,记为弧3,取弧3的中点为轨迹中途点,记为点M;
(3)确定最终轨迹
比较|CA|和|CB|,
若|CA|≤|CB|,则过B、M两点做圆,使其与B点航向相切,记其圆点为OB,过A点做圆,使其与圆OB-B及A点航向相切,记其圆点为OA,圆OA-A与圆OB-B的切点记为P,OA、P、OB共线;
若|CA|>|CB|,则过A、M两点做圆,使其与A点航向相切,记其圆点为OA,过B点做圆,使其与圆OA-A及B点航向相切,记其圆点为OB,圆OA-A与圆OB-B的切点记为P,OA、P、OB共线;
此时A、B两点之间的运动轨迹由两条弧形或直线组成,分别为AP和BP,轨迹确定完成。
2.如权利要求1所述的一种巡视器航向到位控制轨迹确定方法,其特征在于,曲率的正负符号定义为:若弧形轨迹圆点位于当前点右侧,此时弧形对应巡视器右转,曲率为正;若弧形轨迹圆点位于当前点左侧,此时弧形对应巡视器左转,曲率为负;若弧形轨迹圆点为无限远点,轨迹变为直线,对应巡视器直行,曲率为零。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410827873.XA CN104571129B (zh) | 2014-12-26 | 2014-12-26 | 一种巡视器航向到位控制轨迹确定方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410827873.XA CN104571129B (zh) | 2014-12-26 | 2014-12-26 | 一种巡视器航向到位控制轨迹确定方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104571129A CN104571129A (zh) | 2015-04-29 |
CN104571129B true CN104571129B (zh) | 2017-05-31 |
Family
ID=53087454
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410827873.XA Active CN104571129B (zh) | 2014-12-26 | 2014-12-26 | 一种巡视器航向到位控制轨迹确定方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104571129B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106598051B (zh) * | 2016-12-13 | 2019-04-12 | 浙江嘉蓝海洋电子有限公司 | 一种基于动力矢量的航迹控制方法 |
CN108594852B (zh) * | 2018-04-23 | 2021-03-30 | 成都信息工程大学 | 一种已知起点、终点和运动方向的移动路径计算方法 |
Family Cites Families (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP3412272B2 (ja) * | 1994-08-24 | 2003-06-03 | 株式会社デンソー | 車両走行位置表示装置 |
CN100491084C (zh) * | 2007-07-03 | 2009-05-27 | 北京控制工程研究所 | 一种基于二元环境信息的移动机器人局部路径规划方法 |
CN103760908B (zh) * | 2014-01-03 | 2015-09-23 | 北京控制工程研究所 | 一种巡视器闭环跟踪控制方法 |
CN103869820B (zh) * | 2014-03-18 | 2015-10-21 | 北京控制工程研究所 | 一种巡视器地面导航规划控制方法 |
CN103970139B (zh) * | 2014-05-09 | 2017-01-11 | 上海交通大学 | 一种机器人连续点位运动规划方法 |
-
2014
- 2014-12-26 CN CN201410827873.XA patent/CN104571129B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN104571129A (zh) | 2015-04-29 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106945662B (zh) | 一种垂直自动泊车路径规划方法及*** | |
CN108594815B (zh) | 一种分阶段的轮式机器人移动路径规划方法 | |
Liang et al. | Automatic parking path optimization based on bezier curve fitting | |
CN107615194B (zh) | 加工轨迹平滑转接的方法及加工装置 | |
CN111196271A (zh) | 自动泊车方法、装置、设备及存储介质 | |
CN108940759B (zh) | 连续加工路径的恒定胶量的控制方法及*** | |
CN109933057B (zh) | 拖拉机自动驾驶***的局部引导轨迹规划方法及装置 | |
CN103760908B (zh) | 一种巡视器闭环跟踪控制方法 | |
CN105867372B (zh) | 全方位移动机器人狭窄直角弯下转向运动规划方法及*** | |
CN107966907B (zh) | 一种应用于冗余度机械臂的障碍物躲避解决方法 | |
CN108534777A (zh) | 一种平行泊车路径规划方法 | |
CN104571129B (zh) | 一种巡视器航向到位控制轨迹确定方法 | |
CN108640040A (zh) | 一种智能叉车的位姿调整路径规划方法 | |
CN108759829B (zh) | 一种智能叉车的局部避障路径规划方法 | |
CN102996137A (zh) | 一种基于s型曲线的盾构机轨迹控制方法 | |
CN109976321A (zh) | 一种用于智能驾驶***的轨迹规划方法及智能驾驶*** | |
CN111746525B (zh) | 泊车路径规划方法、装置、设备及存储介质 | |
CN111531536A (zh) | 机器人控制方法、机器人、电子设备和可读存储介质 | |
CN112141091A (zh) | 解决车位偏移和定位偏移的二次泊车方法、***及车辆 | |
CN111216738A (zh) | 车辆的控制方法、装置、电子设备及车辆 | |
CN104589348A (zh) | 一种仿人机器人多模态运动转换方法 | |
CN109959383A (zh) | 一种自动泊车路径规划方法 | |
CN107168338A (zh) | 基于毫米波雷达的惯性导引车导航方法及惯性导引车 | |
CN105563483A (zh) | 一种用于蛇形仿生机器人转弯运动的组合控制方法 | |
CN109556623A (zh) | 融合触须算法与切线图法的迭代式路径规划算法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |