4-2路径规划HybridA*&Kinodynamic RRT*

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 Semi-structured Environments, Dmitri Dolgov , Sebastian

Detail

维护一个优先队列来存储所有要扩展的节点
•所有节点的启发式函数h(n)是预先定义的
•优先队列初始化为启动状态X S
•对图中所有其他节点赋值g(X S)=0, g(n)=infinite
•循环
在这里插入图片描述
与A四面八方寻找不同,hybrid A是控制输入演化的邻居节点
在Hybrid A Star就考虑很多其他因素,比如说转向角转向角的改变,是否倒车,车辆行驶方向是否改变等。如果用 δ \delta δ表示车辆的转向角,D表示车辆的行驶方向,s表示每次搜索所走的路程。下图是该计算实际花费的过程, P 1 − P 4 P_1-P_4 P1P4是参数。
在这里插入图片描述

Heuristic design

(a) 二维欧式距离
(b)考虑车辆模型,运动学(不能够侧滑)
(c)走入死胡同
(d)考虑障碍物,在2D地图上找最短路径
在这里插入图片描述

trick

one-shot
分析扩展:一次启发式加上状态驱动对搜索过程的偏重
尝试去查找树,探寻到终点的路径

在这里插入图片描述

Application

在这里插入图片描述

Practical Search Techniques in Path Planning for Autonomous Driving , Dmitri Dolgov , Sebastian
高飞老师的工作
在这里插入图片描述

Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight , Boyu Zhou, Fei Gao
源码

Kinodynamic RRT*

Review:RRT*
在这里插入图片描述
在这里插入图片描述OBVP问题求解
在这里插入图片描述
O
在这里插入图片描述
在这里插入图片描述

RRT vs RRT*

在这里插入图片描述

Problems when it comes to motion constraints(运动约束问题)

How to sample

状态空间扩展,系统变量包含位置和速度

How to define “Near”

在这里插入图片描述

如果没有运动约束,可以使用欧几里得距离或曼哈顿距离。
而在有运动约束的状态空间中,引入最优控制。
如果引入最优控制,我们可以得到状态间转移的代价函数。
通常考虑能量与时间最优,如果从一种状态转移到另一种状态的成本很小,则两种状态是接近的。(注意,如果反向转移,成本可能不同),如果我们知道到达时间( τ \tau τ)和控制策略 u ( t ) u\left( t \right) u(t)的转移,我们可以计算成本。这些都是经典的最优控制解。
在这里插入图片描述

  • 固定最终状态,最后时间
    最优控制策略 u ∗ ( t ) u^*\left( t \right) u(t)
    u ∗ ( t ) = R − 1 B T exp ⁡ [ A T ( τ − t ) ] G ( τ ) − 1 [ x 1 − x ˉ ( τ ) ] u^*\left( t \right) =R^{-1}B^T\exp \left[ A^T\left( \tau -t \right) \right] G\left( \tau \right) ^{-1}\left[ x_1-\bar{x}\left( \tau \right) \right] u(t)=R1BTexp[AT(τt)]G(τ)1[x1xˉ(τ)]

G(t)为可加权可控格兰姆行列式
G ˙ ( t ) = A G ( t ) + G ( t ) A T + B R − 1 B T , G ( 0 ) = 0. \dot{G}\left( t \right) =\mathrm{AG}\left( \mathrm{t} \right) +\mathrm{G}\left( \mathrm{t} \right) \mathrm{A}^{\mathrm{T}}+\mathrm{BR}^{-1}\mathrm{B}^{\mathrm{T}},\mathrm{G}\left( 0 \right) =0. G˙(t)=AG(t)+G(t)AT+BR1BT,G(0)=0.
也就是李雅普诺夫方程的解
G ˙ ( t ) = A G ( t ) + G ( t ) A T + B R − 1 B T , G ( 0 ) = 0. \dot{G}\left( t \right) =\mathrm{AG}\left( \mathrm{t} \right) +\mathrm{G}\left( \mathrm{t} \right) \mathrm{A}^{\mathrm{T}}+\mathrm{BR}^{-1}\mathrm{B}^{\mathrm{T}},\mathrm{G}\left( 0 \right) =0. G˙(t)=AG(t)+G(t)AT+BR1BT,G(0)=0.
x ˉ ( t ) \bar{x}\left(t\right) xˉ(t)在t时刻没有控制输入作用下的状态 x x x
在这里插入图片描述
微分方程的解为
在这里插入图片描述

  • 固定最终状态x1,对最终时间τ不加限制
    如果我们想要找到最佳的到达时间( t t t),为此,我们确定控制策略 u ∗ ( t ) u^*\left(t\right) u(t)的成本函数( c [ π ] c\left[\pi\right] c[π])和评估积分
    在这里插入图片描述
    找到最优( τ \tau τ)通过( c [ τ ] c\left[\tau\right] c[τ])对 τ \tau τ的微分
    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    注意,函数( c [ τ ] c\left[\tau\right] c[τ])可能有多个局部最小值。双积分器系统,这是一个( 4 t h 4^{th} 4th)阶多项式。
    给出最优到达时间 τ ∗ \tau^* τ正如上面定义的,又变成了一个固定的最终状态。修正了最终时间问题。

How to “chooseParent”

现在如果我们对一个随机状态进行抽样,我们就可以从树中的这些状态节点计算出控制策略和到抽样状态代价,选择一个以最小的成本和检查( x ( t ) x(t) x(t)]和( u ( t ) u(t) u(t)范围。
如果没有找到合格的父节点,则对另一个状态进行取样。

How to find near nodes efficiently

查找一部分空间
在这里插入图片描述
如果我们设置一个成本容忍度r,我们可以计算实际状态的界(前向可达集),其能通过x_rand和状态界(反向可达集)得到。以低于r的成本到达x_rand。
如果我们以kd树的形式存储节点,我们可以在树中进行范围查询。

c [ τ ] = τ + [ x 1 − x ˉ ( τ ) ] T G ( t ) − 1 [ x 1 − x ˉ ( τ ) ] \mathrm{c}\left[ \mathrm{\tau} \right] =\mathrm{\tau}+\left[ \mathrm{x}_1-\mathrm{\bar{x}}\left( \mathrm{\tau} \right) \right] ^{\mathrm{T}}\mathrm{G}\left( \mathrm{t} \right) ^{-1}\left[ \mathrm{x}_1-\mathrm{\bar{x}}\left( \mathrm{\tau} \right) \right] c[τ]=τ+[x1xˉ(τ)]TG(t)1[x1xˉ(τ)]
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

How to “rewire”

计算前向可达集
在这里插入图片描述

演示

在这里插入图片描述

绿色曲线不考虑障碍;
红色曲线是考虑动力kinodynamic轨迹规划器的结果;
蓝色曲线为运动学轨迹规划器找到的第一个可行轨迹;
黄线是每个控制点的控制输入。

Homework

  • 假设位置是固定的,速度和加速度在这里是不加限制的,如何扩展邻居?
    根据前面提到的OBVP,求得部分自由最终状态情况的最优解(控制、状态、时间)。
  • 建立一个线性机器人模型的ego-graph(由唯一一个中心节点(ego),以及这个节点的邻居(alters)组成)编程生成许多个轨迹,设计一个打分函数,选择最接近规划目标的最佳轨迹,如演示图那样。
### Hybrid A* Algorithm Cost Function Implementation and Explanation The Hybrid A* algorithm extends the traditional A* algorithm specifically for pathfinding problems involving vehicles with non-holonomic constraints (such as cars). The cost function plays a crucial role in guiding the search process towards feasible paths that not only reach the goal but also adhere to vehicle dynamics. #### Definition of Cost Components In Hybrid A*, the total cost \( f(n) \) associated with each node can be defined by combining two main components: - **g-value**: Represents the actual cost from the start position to the current node. - **h-value**: Estimates the heuristic cost from the current node to the target/goal location. \[ f(n) = g(n) + h(n) \] For hybrid systems like robotic cars, these costs need careful consideration due to kinematic restrictions such as turning radius limitations or orientation requirements at waypoints[^1]. #### Calculation Details To calculate the `g` value accurately within Hybrid A*, one must account for factors including distance traveled along valid arcs between poses while respecting steering angles and speed limits. This often involves integrating over time using models derived from bicycle geometry equations which describe how changes in steering angle affect lateral displacement during motion planning tasks. Regarding the calculation of the `h` component, it typically employs heuristics tailored toward minimizing deviations from ideal trajectories considering both positional error relative to straight-line distances plus angular differences concerning heading directions required upon arrival at intermediate points throughout planned routes. ```python def compute_g_value(current_pose, previous_pose): """ Computes the true cost based on movement parameters. Args: current_pose (tuple): Current pose coordinates (x,y,theta). previous_pose (tuple): Previous pose coordinates (x',y',theta'). Returns: float: Calculated g-value representing travel expense. """ # Implement specific logic here according to your application's needs def estimate_h_value(target_pose, candidate_pose): """ Provides an admissible estimation of remaining effort needed until reaching destination. Args: target_pose (tuple): Goal configuration details (gx,gy,g_theta). candidate_pose (tuple): Potential next step info (cx,cy,c_theta). Returns: float: Predicted future expenditure under evaluation criteria. """ dx = abs(candidate_pose[0] - target_pose[0]) dy = abs(candidate_pose[1] - target_pose[1]) dtheta = min(abs(candidate_pose[2]-target_pose[2]), 2*math.pi-abs(candidate_pose[2]-target_pose[2])) return math.sqrt(dx*dx + dy*dy) + w*dtheta # Add weight 'w' factor adjusting importance given to orientation alignment vs linear proximity ``` Here, weights may vary depending on problem specifics; thus tuning might become necessary when applying this framework across diverse scenarios where different aspects could hold varying significance levels regarding overall performance metrics desired out of implemented solutions.
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值