自动驾驶运动规划-Dubins曲线
1、Simple Car模型
如下图所示,Simple Car模型是一个表达车辆运动的简易模型。Simple Car模型将车辆看做平面上的刚体运动,刚体的原点位于车辆后轮的中心;x轴沿着车辆主轴方向,与车辆运动方向相同;车辆在任意一个时刻的姿态可以表述为(x, y, )。车辆的运动速度为s;方向盘的转角为,它与前轮的转角相同;前轮和后轮中心的距离为L;如果方向角的转角固定,车辆会在原地转圈,转圈的半径为。
在一个很短的时间内,可以认为车辆沿着后轮指向的方向前进,当趋于0时,有:
根据数学定义:
将2) 和3)代入1)中,得到:
显然,和是5)式的一个解,两侧乘以速度s等式仍然满足。因此有:
用表示车辆前进的距离,则有:
根据三角几何,有:
将9)式代入8)式,得到:
8)式两侧同除以dt, 并根据,得到:
至此得到了车辆的运动模型(Motion Model)。
然后引入Action变量,假设车辆运动速度s和方向盘转角由Action变量和指定,得到:
2、Dubins曲线
假设车辆按照常量速度运行: ,最大转向角度为,最小转弯半径,起点为, 终点为,我们目标是求解从起点到终点的最短行驶距离。求解最短距离的过程就是优化如下Cost的过程。
是到达所需的时间,,当不可达时,。
由于速度是恒定的,根据前面提到的车辆的运动模型:
其中:。将13)和14)代入12),可看到,最短行驶距离只与时间有关。
令S为车辆直行的Motion Primitive,L和R分别为车辆左转和右转的Motion Primitive,可以证明,任意起点到终点的Dubins最短路径可以由不超过三个Motion Primitives构成。由三个Motion Primitives构成的序列称为一个Word。由于两个连续的、相同的Motion Primitive可以合并为一个Motion Primitive,因此所有可能的Word有10中组合,Dubins证明最优的Word组合只能是如下6个组合之一:
其中,,,这里注意,大于,如果小于,一定有其它的序列优于该序列。
3、Dubins计算过程推导
3.1 基于向量的切点计算
假设两个最小转弯半径构成的Circle为 和,半径分别为和,圆心分别为和。
1)首先构造C1和C2的圆心到的向量。
2)构造C1和C2的外切线切点构成的向量。
3)构造垂直于的单位法向量n,修改的使其平行于。
根据法向量的定义:,得到:
根据单位向量的定义:,代入上式得到:
- 16)式两侧同除以D,得到:
注意,这里实际是将向量单位化。
根据向量点乘的数学定义:
因此:
等于向量与法向量n的夹角的余弦。为了方便书写,定义一个常量。
等式17)中只有n是未知数。
5)将向量旋转角度C就得到向量n。假设,根据向量旋转的数学定义:
6)计算出n之后,就可以很方便的计算出外切线的切点和。从C1的圆心出发,沿着向量n的方向,距离为的位置即为切点,亦然。
3.2 计算CSC类型的行驶曲线
RSR、LSL、RSL、LSR是CSC类型的行驶曲线,该类型曲线首先计算两个圆的切点,然后车辆沿着最小转弯半径构成的圆周行驶到第一个圆的切点,然后直行到第二个圆的切点,再沿着最小转弯半径构成的圆周行驶到目的地。下面我们以RSR轨迹为例看看如何计算行驶曲线。
假设起点和终点,最小转弯半径为。然后我们计算起点和终点的圆心。
起点的圆心为:
终点的圆心为:
得到起点和终点的圆心之后,可以利用3.1小节的切点计算方法,得到切点和。然后就可以得到车辆的行驶轨迹,该轨迹分为三段:start到的圆周弧;和的直线距离;到Goal的圆周弧。至此我们得到了RSR的行驶曲线。
3.3 计算CCC类型的行驶曲线
如下图所示,和的圆心为和,是与和相切的圆,圆心为。
根据数学关系,可得到:
记为与的夹角,已知三角形的三个边的长度,根据余弦定理,有:
最终可得到:
注意此处为LRL模式时,需要加上;为RLR模式时,需要减去。
然后计算和计算就变得很容易。定义向量,将向量缩放到。
最后可以得到交点。按照同样的过程可以计算得到。然后就可以得到start到的圆周弧;和的圆周弧;到Goal的圆周弧的三段轨迹组成的行驶曲线。
参考文章
1、A Comprehensive, Step-by-Step Tutorial on Computing Dubin’s Curves (https://gieseanw.files.wordpress.com/2012/10/dubins.pdf)
2、Planning Algorithm (http://planning.cs.uiuc.edu/node1.html)
转自:https://zhuanlan.zhihu.com/p/120272035
自动驾驶运动规划-Dubins曲线的更多相关文章
- 自动驾驶运动规划-Reeds Shepp曲线
自动驾驶运动规划-Reeds Shepp曲线 相比于Dubins Car只允许车辆向前运动,Reeds Shepp Car既允许车辆向前运动,也允许车辆向后运动. Reeds Shepp Car运动规 ...
- Google Waymo 2017自动驾驶安全技术报告(一)
2017年10月Google Waymo向美国交通部提交了一份43页的安全报告,报告中详细说明了Waymo如何装备和训练自动驾驶车辆,从而避免驾驶中的一般和意外情况发生.这份报告对Waymo的自动驾驶 ...
- zz自动驾驶中轨迹规划的探索和挑战
大家好,今天我们主要介绍一下轨迹规划的探索和挑战,我主要从四个方面介绍: 轨迹规划的概念 决策 横向规划 纵向规划 轨迹规划的概念: 轨迹规划的核心就是要解决车辆该怎么走的问题.比如我们知道了附近有行 ...
- <转载> 从算法上解读自动驾驶是如何实现的?
科技新闻小鹏汽车2016-03-28 10:42 [摘要]车辆路径规划问题中路网模型.路径规划算法和交通信息的智能预测为关键点. 由于驾驶员的驾驶工作繁重,同时随着汽车拥有量的增加,非职业驾驶员的数 ...
- 自动驾驶缺人才?听听David Silver怎么说!
如今自动驾驶在全球范围内的发展势头愈发“凶猛”,该领域人才也一度被视为“香饽饽”. 即使在美国,自动驾驶工程师的起薪也已经突破了25万美元,我国‘“开价”之高更是令人咋舌. 人才.人才.还是人才!重要 ...
- zz自动驾驶复杂环境下高精度定位技术
今天为大家分享下,自动驾驶在复杂环境下的高精度定位技术. 定位/导航负责实时提供载体的运动信息,包括载体的:位置.速度.姿态.加速度.角速度等信息. 自动驾驶对定位系统的基本要求: 1. 高精度:达到 ...
- zz高精地图和定位在自动驾驶的应用
本次分享聚焦于高精地图在自动驾驶中的应用,主要分为以下两部分: 1. 高精地图 High Definition Map 拓扑地图 Topological Map / Road Graph 3D栅格地图 ...
- Reeds-Shepp曲线和Dubins曲线
转载:https://www.cnblogs.com/huyanan/articles/6243694.html 什么是Reeds-Shepp曲线? 想象你下班开车回家,到了小区后想把车停 ...
- 第三期 第三期 搜索——1.运动规划(motion_planing)
运动规划的根本问题在于机器人可能存在于一个这样的世界中, 它可能想找到一条到达这个目标的路径,那么就需要指定一个到达那里的计划, 自动驾驶汽车也会遇到这个问题.他可能处于高速公路的附近的街道网络中,他 ...
随机推荐
- 如何给远程桌面发送“Ctrl+Alt+Delete”命令
转至:https://www.yunweiku.com/forum.php?mod=viewthread&tid=925 当我们远程桌面需要是用ctrl+alt+delete组合键时无法使用, ...
- yum报错 , yum相关配置信息,yum重装
docker源的问题 yum有很多错,比如网络问题,dns问题,timeout 错,还有不知道什么错误 网上有很多,网络问题,dns问题,但是我ping www.baidu.com通,所以不是这个问题 ...
- k8s dashboard 安装和证书更新
1.k8s 搭建 参见https://blog.51cto.com/lizhenliang/2325770 [root@VM_0_48_centos ~]# kubectl get cs NAME ...
- 【python画圆】pip安装库时出现Read timed out.解决办法
昨天第一次用python画圆,当时并没有安装numpy库(导入数据包)和matplotlib库(导入图形包),于是尝试用pip安装库 首先,我先更新了pip,如下图: 顺便附上成功截图: 然后安装nu ...
- LeetCode-096-不同的二叉搜索树
不同的二叉搜索树 题目描述:给你一个整数 n ,求恰由 n 个节点组成且节点值从 1 到 n 互不相同的 二叉搜索树 有多少种?返回满足题意的二叉搜索树的种数. 二叉搜索树(Binary Search ...
- tensorflow源码解析之framework-op
目录 什么是op op_def定义 op注册 op构建与注册辅助结构 op重写 关系图 涉及的文件 迭代记录 1. 什么是op op和kernel是TF框架中最重要的两个概念,如果一定要做一个类比的话 ...
- DataFrame计算corr()函数计算相关系数时,出现返回值为空或NaN的情况+np.log1p()
- Spring data jpa 报错: java.sql.SQLSyntaxErrorExceptoion: Table ' test.hibernate_sequence' doesn`t exists
错误信息如图: 错误原因:在自定义主键是没有注明自增策略 解决办法:明确主键自增策略,比如 @Entity @Table(name = "t_order") @Data @AllA ...
- SQL Server2012安装教程
什么是SQL? 结构化查询语言(Structured Query Language)简称SQL,是一种特殊目的的编程语言,是一种数据库查询和程序设计语言,用于存取数据以及查询.更新和管理关系数据库系统 ...
- 如何使用Java AWT 创建一个简易计算器
摘要:手把手教你使用 Java AWT 创建一个简易计算器. 本文分享自华为云社区<手把手教你使用 Java AWT 创建一个简易计算器>,作者:海拥 . 关于AWT AWT (抽象窗口工 ...