4 运动动力学约束下的路径搜索

已于 2023-11-30 21:03:04 修改
阅读量2.2k 收藏 16
点赞数 1
分类专栏： Motion Planning 文章标签： 自动驾驶 动态规划求解
于 2021-04-15 14:02:09 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/qq_37087723/article/details/113806063
版权

文章目录

Introduction

T: 考虑机器人的动力学约束
Kinodynamic: kinematic + Dynamic

运动学规划问题是在同时受到运动学约束(如避障)和动力学约束(如速度、加速度和力的模量界限)的情况下综合机器人运动。动力学解是从时间到广义力或加速度的映射。
——《动力学运动规划》，Bruce Donald, Patrick Xavier, John Canny, John Reif

  • 不同的限制
  • 最大力(加速度)
    Y:
    Straight-line connections between pairs of states are typically not valid trajectories due to the system’s differential constraints.
    由于系统的微分约束，状态对之间的直线连接通常不是有效的轨迹。
    在这里插入图片描述
  • coarse-to-fine process(由粗到精）
  • 轨迹只会局部规划
  • 不可行路径对非完整系统没有意义
    如大部分车不能平移，如下图所示，不能够走下面
    在这里插入图片描述
    在这里插入图片描述
    无人机的一开始的速度是在向右，但是按路径规划左边路径更短，其实应该沿着速度方向向右飞

单轮

在这里插入图片描述

差动轮

在这里插入图片描述

简化的汽车模型

在这里插入图片描述
r应该标记为v

Workflow

basic idea

We have many weapons to attack graph search.
•假设机器人的质量点不再令人满意。
•我们现在需要一个具有可行运动连接的图。

  • 我们手动创建(构建)一个所有边都由机器人可执行的图。
    • 反向，离散机器人的状态空间，连接，如栅格地图
    • 正向。离散机器人的控制空间，如PRM
  • 这是所有动动力规划的基本动机。
  • 状态点阵规划（State lattice planning）是最直接的一种。

build the graph,sample in control vs state space

在这里插入图片描述
说明：s为状态变量，u 为控制变量

如果在控制空间没有任务导向性,在已知被控对象的条件下，固定输入的控制量u uu和积分时间T，由此进行前向积分可以得到被控对象从任意的初始状态 s_0过度到终止状态s f，前向积分可以对被控对象在任意时间T之后的状态进行预测，进而得到一系列的状态集合，这就是控制空间中的采样。由于u 和T是任意给定的，在控制空间中的采样往往没有明确的目的性，采样结果只能分布在某些状态附近，无法精确采样到某一个给定状态，如下图所示：
在这里插入图片描述在这里插入图片描述

选择一个𝑢，固定一个时间段𝑇，正向模拟系统(数值积分)。

  • 正向模拟
  • 确定的u,T
  • 无任务导向（缺点）
  • 容易实施
  • 低效率

选择状态s_f,寻找从s_0到s_f的轨迹

  • 反向计算:
  • 需要知道T,u(或者估计假设等)
  • 很难实施
  • 具有任务导向性

sample in control space

在这里插入图片描述
选择加速度作为控制输入，紫色的线代表(feasible motion)T=1s（假设）时间内机器人的状态轨迹,为啥会呈现抛物线形状,这是由于物体匀加速运动导致的；jerk:加速度求导 x求三次导
A nilpotent:幂零矩阵(会在A的某次方为零,方便计算)

在这里插入图片描述
e^At:状态转移矩阵
F(t):零输入响应
G(t):零状态响应
Um:给定输入
S(t):状态响应
其实就是匀加速运动在高维状态的叠加
对采样结果进行进一步的离散化，控制空间中的最终采样结果如下图：
在这里插入图片描述
The lattice graph obtained by searching
搜索得到的点阵图
需要要有目标性,不需要完整的搜索
在搜索过程中，可以根据需要建立图形。
•当节点被新发现时 ，创建节点(状态)和边(初始运动)。
•节省计算时间/空间。

在这里插入图片描述
步骤:

  • 搜索树按某种规则找到一个周期T内的状态S
  • 选择一个控制的输入
  • 固定一个较短的时间,堆等式进行积分
  • 加入没有碰撞的动作到搜索树

在这里插入图片描述

构建一个点阵图:
• 给定一个原点。
• 对于原点周围的8个邻居节点，找到可行路径。
• 向外扩展到24个邻居。
• 完整点阵
参考：Generating Near Minimal Spanning Control Sets for Constrained Motion Planning in Discrete State Spaces，Mihail Pivtoraiko and Alonzo Kelly

两层Lattice graph
第一层不同,不同的初始状态

comparison

论文:State Space Sampling of Feasible Motions for High-Performance Mobile Robot Navigation in Complex Environments,
Thomas M. Howard, Colin J. Green, and Alonzo Kelly
在这里插入图片描述

  • 在初始角速度方向上轨迹密度更大，受初始化影响很大
  • 不同输入的输出非常相似。
  • 从图中可以看出，控制空间中的采样没有目的性，离散控制量得到的运动路径是不合格的，会使机器人运动到合理的运动环境之外，得到的局部轨迹仅仅有一部分可以使用；状态空间中的采样可以保证局部轨迹可用。

Boundary Value Problem(BVP)

  • BVP是状态采样点阵规划的基础。
  • 没有通用的解决方案。只能逐个设计。
  • 经常进行复杂的数值优化。
    Y：需要求解状态空间采样的问题。
    根据边界条件求解方程
    在这里插入图片描述

optimal BVP

问题：从一个状态（初始）到另一个状态（结束），如何最小化jerk平方和的积分:
（jerk加加速度）
在这里插入图片描述
k表示某个轴，
根据庞特里亚的极小值原理，我们首先引入协态：根据system model来定义
定义哈密尔顿函数在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
首先定义cost function ，然后定义Hamilton中间变量 λ \lambda λ，使用极小值原理选取u使得H函数最小

参考
A Computationally Efficient Motion Primitive for Quadrocopter Trajectory Generation, Mark W. Mueller, Markus Hehn, and Raffaello D’Andrea
Dynamic Programming and Optimal Control, D. P. Bertsekas

example optimal state

先简化为1轴的问题，最小化无人机的jerk
在这里插入图片描述
这里计算关于 λ \lambda λ导数时可能会下意识地直接求H关于 λ \lambda λ,但其实应该求关于s的导数
在这里插入图片描述
这样便可以得到一组可行解 α , β , γ \alpha, \beta, \gamma α,β,γ为待定系数
将 λ \lambda λ代入到Hamilton公式中即可得到最优J
这里需要注意输入为J ，而v,a的最优值确定（s(t)=(p,v*,a*))**
此时关注
在这里插入图片描述
即可，求导极值为0，计算得到最优的输入,通过积分求得相应最优状态。
如果没有边界条件，没有final state，但有一定到达的硬约束。根据s(T)=s_f来求解
在这里插入图片描述
最后转换为多项式求根
如果有一些变量不需要给定，求最优也能行
同时，可以将代价函数J 转变为关于时间T 的函数，对代价函数求极小值可以求得最优的时间T 如下：
在这里插入图片描述
将两点的边界状态代入状态方程，可以求得α , β , γ 的值，OBVP问题求解完成：
在这里插入图片描述
最终求多项式的根T
在这里插入图片描述
reference

example

Optimal Rough Terrain Trajectory Generation for Wheeled Mobile Robots, Thomas M. Howard Alonzo Kelly
A Computationally Efficient Motion Primitive for Quadrocopter Trajectory。
Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments , Dmitri Dolgov , Sebastian Thrun,
Michael Montemerlo , James Diebel。
https://pythonrobotics.readthedocs.io/en/latest/modules/path_planning.html#optimal-trajectory-in-a-frenet-frame。
参考论文Generation, Mark W. Mueller, Markus Hehn , and Raffaello D’Andrea
Dynamic Programming and Optimal Control, D. P. Bertseka

轨迹库

根据评分函数来确定那个轨迹较合适

Heuristic heuristic

原则:只解决一个简单的问题，
两种做法
•假定不存在障碍
•假设不考虑动力学

Planning in Frenet-serret Frame

参考文献
Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame, Moritz Werling, Julius Ziegler, Sören Kammel, and Sebastian Thrun。
Optimal trajectories for time-critical street scenarios using discretized terminal manifolds , Moritz Werling, Sören Kammel , Julius Ziegler and Lutz Gröll。

作业

OBVP 问题求解

ref

https://zhuanlan.zhihu.com/p/483647239
https://www.shenlanxueyuan.com/course/633

明码
关注
  • 1
    点赞
  • 踩
  • 16
    收藏
    觉得还不错? 一键收藏
  • 3
    评论
专栏目录
基于Frenet坐标系的无人车轨迹规划详解与实现
robinvista的专栏
04-28 2万+
　目的 　　本文通俗地介绍Frenet坐标系下的无人车轨迹规划方法，随后探讨在ROS下的代码实现细节，以及如何调节各项参数，需要重点注意的地方。 　前言 　　Frenet坐标系下的无人车轨迹规划方法是由Moritz Werling在2010年提出的，由于其简单有效的特点被广泛采用，甚至MATLAB都设计了相应的函数：trajectoryOptimalFrenet。一些开源的项目虽然不直接使用这种方法，但是也大量借鉴了方法中包含的思想，例如百度的Apollo无人驾驶项目，其中有很多Frenet坐标系的影子。
移动机器人运动规划(四)--动力学约束下的路径规划
weixin_43619346的博客
07-16 8346
本节课
Path Planning for Autonomous Vehicles in Unknown Semi-structured Environments
02-26
本文分2部分，第2-3节介绍free space下的路径规划，目标是规划出一条平滑且满足车辆non-holonomic约束的最短路径。第4节介绍semi-structured环境的路径规划。 free space的方法分为2部分，第1部分是采用启发式搜索方法获得一条满足车辆运动学约束的可行驶轨迹，比如Hybrid A*方法，通常结果不是最优、但接近最优。第2部分是在此基础上，采用数值优化产生更平滑的轨迹。
Apollo学习笔记（13）Lattice Planner与OBVP问题
qq_24649627的博客
11-29 1911
本文是学习了深蓝科技相关课程与阅读了相关文献后，整理所得。 论文为A computationally efficient motio primitive for quadrocopter trajectory generation。 Lattice Planner算法的根本目标就是从车辆当前的位置，结合地图、定位、障碍物等信息，通过采样并搜索出一条设定的目标 cost function 值最小的一组坐标。这就和BVP问题很相像了，都是知道起始状态信息和最终信息（部分也可以），求解出满足要求的多阶曲线。如.
弗莱纳公式（Frenet–Serret formulas）
CCC的专栏
07-09 1万+
在向量微积分中，弗莱纳公式用来描述欧几里得空间R中的粒子在连续可微曲线上的运动。更具体的说，弗莱纳公式描述了曲线的切向、法向、副法方向之间的关系。弗莱纳公式（弗莱纳坐标系）在自动驾驶轨迹规划等领域应用广泛。 单位切向量 T，单位法向量 N，单位副法向量 B，被称作弗莱纳标架（Frenet–Serret frame or TNB frame，他们的具体定义如下： T 是单位切向量，方向指向粒子运动的方向； N 是单位法向量 T对弧长参数的微分单位化得到的向量； B 是 T和 N的外积。 弗莱纳公式如下：
庞特里亚金最小值原理求解能耗最小化的时间最优OBVP问题
gophae的博客
08-10 6979
本文依据论文A computationally efficient motion primitive for quadrocopter trajectory generation中所述的方法，整理出其中对OBVP问题的求解方法。所谓的OBVP即为optimal boundary value problem。 OBVP是特殊的BVP问题，最简单的BVP问题就是state sampled lattice planning,这其实也是一种lattice planner方法。常见的例子比如：给定初始位置和终点位置，
使用离散化终端流形的时间临界街道场景的最优轨迹
tony_513的博客
03-28 618
Optimal trajectories for time-critical street scenarios using discretized terminal manifolds 流形空间：流形是局部具有欧几里得空间性质的空间，在数学中用于描述几何形体 semi-reactive planning strategy：半反应规划策略 maneuver tasks：机动任务 lane-...
4-2路径规划HybridA*&Kinodynamic RRT*
qq_37087723的博客
05-30 6145
Hybrid A* Workflow T: 将栅格地图的路径搜索与lattice grid 结合起来 ，保证在每个栅格里面只有一个状态（节点），如下图所示 Y:在线生成密集的栅格花费太多的时间。 Practical Search Techniques in Path Planning for Autonomous Driving , Dmitri Y:Dolgov , Sebastian Thrun Path Planning for Autonomous Vehicles in Unknown Se
规划控制 | 详解自动驾驶轨迹规划Hybrid A*算法
CV_Autobot的博客
03-06 2032
作者|涅索斯衬衫 编辑|汽车人原文链接：https://zhuanlan.zhihu.com/p/610206756?点击下方卡片，关注“自动驾驶之心”公众号ADAS巨卷干货，即可获取点击进入→自动驾驶之心技术交流群后台回复【规划控制综述】获取自动驾驶、智能机器人规划控制最新综述论文！传统的A*算法是一种基于图搜索(Graph Search)的启发式搜索算法，启发式搜索在一定程度上避免了...
Lattice Planner从学习到放弃（一）.额不....到实践
热门推荐
王不二的路
07-26 2万+
到昨天，第一次实现了lattice集成后（第一阶段：无障碍物）在实车的初次运行，过程一言蔽之：基于现有的地图实现循迹功能。但是这个过程实现...累却赤激，真的是站在前辈工作上一点点进步。然后，CSDN和知乎也算同性交友平台不？本想只是记录下自己的学习过程，然后同几个同性私信分享交流经验了，哈哈，世界不孤单～总结一下这两个月的结果，希望能有更多小伙伴指点和交流。 参考源 参考大佬们的工作是不可少的，除去之前文章里的那些参考， 近期看了Moritz Werling经典的...
【路径规划】 Optimal Trajectory Generation in Frenet阅读记录 (附python代码实例)
这里是张聪明，正在学习中，请稍后
07-06 2806
参考与前言 2010年，论文 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame 地址：https://www.researchgate.net/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame Python代码示意地址：https://gitee.com/
Boundary Value Problem (BVP) 两点边界最优控制问题
世界仙境与冷酷尽头's Blog
06-05 3468
OBVP在路径规划当中的应用
自动驾驶の泊车算法
最新发布
whaosoft143ai的博客
10-30 656
我以为测试结果会很差，结果还是有点出乎意料的，虽然有的时候会出现无解，但是只要初始线段给的好一点就不会出问题，优化的速度也挺快平均时间大概是0.3s， 路径的质量的评价我觉得主要是看平滑度和曲率大小，只要曲率不会超过车辆最小转弯半径就可以了， 这里我设置的曲率最大是0.155， 列了几组测试图，第一个是直线段参考线和优化后的路径图，第二幅是对应的曲率和航向。路径点的间隔不能小，否则优化时间和质量都会受到影响，这个我觉得后期可以参考上述论文，先优化出一条大概得解，然后再密集插值进一步平滑。whaosoft
文献阅读之自主泊车APA的路径规划
silver bullet
12-06 6456
目录 The future of parking A survey on automated valet parking with an outlook on high density parking.pdf Sampling Based Vehicle Motion Planning for Autonomous Valet Parking with Moving Obstacles....
【frenet frame】在frenet坐标系中的轨迹规划
jja223的博客
06-12 4594
中文参考文章：https://blog.csdn.net/adamshan/article/details/80779615 论文：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frene´t Frame，Moritz Werling, Julius Ziegler, So¨ren Kammel, and Sebas...
Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame-翻译解读
真诚的灰灰的博客
07-04 3937
论文名：Frenet框架中动态街区场景的最优轨迹生成 作者：Moritz Werling,Julius Ziegler,Soren Kammel,and Sebastian Thrun 本文为个人学习和同大家交流，希望共同交流学习，文中有翻译不对的地方，请在评论区指出，谢谢！ 原文链接：Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame 摘要 自动驾驶汽车对动态高速公路和内部城市场景的安全处理涉及到生成适应
蓝桥ROS扩展笔记CppRobotics编译
zhangrelay的专栏
07-29 1434
为了方便下载和使用，选用gitcode：git clone https://gitcode.net/ZhangRelay/cpp_robotics.git 解压缩：unzip cpp_robotics/CppRobotics.zip编译常见过程：mkdir buildcmake ..make一次成功^_^选个A*看看效果？＃ 目录 * [定位](#localization) * [扩展卡尔曼滤波器](#extended-kalman-filter-localization) * [粒子过滤器
Matlab自动驾驶学习笔记【自动驾驶工具箱】——基于Frenet参考路径的高速公路轨迹规划
m0_56396498的博客
04-14 1722
说明：本次学习主要是参考官方的帮助文档，网址如下所示 Automated Driving ToolboxDocumentation- MathWorks 中国 一、负载驱动场景 这部分需要定义当前工作区中的车辆和道路特性，使用的是自动驾驶工具箱中的驾驶场景设计器 1.1以编程方式创建驾驶场景变体 这样可以满足在多种情况下快速测试驾驶算法。 具体步骤如下： 使用驾驶场景设计器应用程序 导出生成与此方案等效的MATLAB代码的MATLAB函数。 在MATLAB编辑器中，修改
如何在路径规划时考虑动力学约束？
sinat_38182444的博客
04-13 1100
像基础路径规划那样，只需知道位置状态，在考虑下一步可选择的点位时，只考虑运动学约束下可到达的点位，再除去障碍点位，进而确定下一步的点位。
高速插补算法研究：基于机床动力学特性
其次，论文构建了小线段连续插补动力学约束条件，包括直线段内S曲线加减速的性能约束、转接点的误差约束、最大加速度约束以及机床进给伺服驱动系统的动力学约束。这些条件共同决定了转接点速度的计算方式，确保了...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论 3
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值