【自动驾驶轨迹规划之最优控制】

本文详细介绍了自动驾驶轨迹规划中的最优控制问题,包括车辆动力学约束、边值约束、碰撞躲避约束和代价函数。文章讨论了二自由度车辆模型、控制变量的选择以及如何构建代价函数以实现最优轨迹。同时,提到了不同约束的处理方式,为自动驾驶轨迹规划提供了理论基础。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

欢迎大家关注我的B站:

偷吃薯片的Zheng同学的个人空间-偷吃薯片的Zheng同学个人主页-哔哩哔哩视频 (bilibili.com)

目录

1 车辆动力学约束

1.1 模型的采取

1.2 模型的假设

1.3 运动学模型的微分方程组

1.4 控制变量的选择

2 边值约束

2.1 简单的约束

2.2 较复杂的约束

3 碰撞躲避约束

4 代价函数

5 完整的最优控制问题


1 车辆动力学约束

1.1 模型的采取

车辆的运动状态改变源自于轮胎受到的合力,由于轮胎受到的力沿接触面分布,需要用到材料力学的知识,有大量的复杂方程,并且在研究车辆低速情况的轨迹规划时,搭建如此复杂的物理模型并不实用,所以我们采用简洁的车辆二自由度模型。

1.2 模型的假设

(1)车辆行驶于平坦路面,忽略车辆在垂直于地面方向上的运动;

(2)忽略车辆受到的空气阻力以及地面侧向摩擦力;

(3)车辆与地面始终保持良好的滚动摩擦;

(4)车辆为刚体,忽略车身悬架结构的影响。

1.3 运动学模型的微分方程组

二自由度模型忽略左右两轮的转动角速度与角加速度差异,将车辆的两只前轮与后轮以车辆的纵轴方向合并为虚拟单轮,因此二自由度模型也称为 bicycle 模型。运动学微分方程组如下

L_%7Bw%7D%20%5Cend%7Bbmatrix%7D%20%5Cqquad%20t%5Cin%20%5Cbegin%7Bbmatrix%7D%200%20%2Ct_%7Bf%7D%20%5Cend%7Bbmatrix%7D

x(t) 与 y(t) 为车辆后轴中点坐标,v(t) 与 a(t) 表示沿车体纵轴方向的速度及加速度,gif.latex?%5Cphi%20%28t%29 代表车辆前轮偏转角,gif.latex?%5Ctheta%20%28t%29 代表车辆的航向角,gif.latex?L_%7Bw%7D 代表车辆前后轴距离。根据理论力学的知识,上述方程易得,若有疑问可参考http://t.csdn.cn/QPoru

1.4 控制变量的选择

一般来说,可被微分的变量是状态变量,因为未被微分的变量可以通过积分得到那些变量并且可被微分的变量一定连续,未被微分的不一定连续。因此在1.3的微分方程组中,我们选择未被微分的gif.latex?a%28t%29 与 gif.latex?%5Comega%20%28t%29 作为控制变量,通过积分来确定剩下的状态变量,gif.latex?a%28t%29 与 gif.latex?%5Comega%20%28t%29 体现着掌控车辆动力学 的意味,假如遇到对车辆行驶平稳性要求较高的场景,则应引入变量  gif.latex?j%28t%29%3D%5Cfrac%7B%5Cmathrm%7Bd%7Da%28t%29%20%7D%7B%5Cmathrm%7Bd%7D%20t%7D  ,同时控制变量的选择也发生变化。轨迹规划的本质其实就是找到一组控制变量使得在人为设定的代价函数下这一组控制变量使系统最优。

2 边值约束

2.1 简单的约束

车辆的初始状态由车载传感器实时记录推算着,而车辆的最终状态也就是控制的目标状态有一定限制。例如在自主泊车中,车子必须停下来则车速为零,车子不能有运动趋势则加速度为零(包括角加速度),同时车辆最后车轮应该回正,所以前轮偏转角应该为零。

2.2 较复杂的约束

watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2x5bm4xNTYwMDY5Mzk5OA==,size_16,color_FFFFFF,t_70

车辆停到某一个车位,相当于一个矩形落在另一个矩形中首先根据状态变量算出车辆的四个顶点坐标

gif.latex?A%3D%5Bx+%28L_%7Bw%7D+L_%7Bf%7D%29*cos%28%5Ctheta%20%29-0.5*L_%7Bb%7D*sin%28%5Ctheta%20%29%2Cy+%28L_%7Bw%7D+L_%7Bf%7D%29*sin%28%5Ctheta%20%29+0.5*L_%7Bb%7D*cos%28%5Ctheta%20%29%5C%5C%5C%5CB%3D%5Bx+%28L_%7Bw%7D+L_%7Bf%7D%29*cos%28%5Ctheta%20%29+0.5*L_%7Bb%7D*sin%28%5Ctheta%20%29%2Cy+%28L_%7Bw%7D+L_%7Bf%7D%29*sin%28%5Ctheta%20%29-0.5*L_%7Bb%7D*cos%28%5Ctheta%20%29%5C%5C%5C%5CC%3D%5Bx-L_%7Br%7D*cos%28%5Ctheta%20%29+0.5*L_%7Bb%7D*sin%28%5Ctheta%20%29%2Cy-L_%7Br%7D*sin%28%5Ctheta%20%29-0.5*L_%7Bb%7D*cos%28%5Ctheta%20%29%5C%5C%5C%5CD%3D%5Bx-L_%7Br%7D*cos%28%5Ctheta%20%29-0.5*L_%7Bb%7D*sin%28%5Ctheta%20%29%2Cy-L_%7Br%7D*sin%28%5Ctheta%20%29+0.5*L_%7Bb%7D*cos%28%5Ctheta%20%29

Lw为前后轴距,Lf为车辆前悬距离,Lr为车辆后悬距离,Lb为车宽。根据车辆后轴中心坐标与车辆航向角即可以计算出车辆的四个顶点,目标泊车区域可以通过四条直线方程来表示,想要泊车目的达到,只需要建立四个点在泊车区域内的点约束方程组即可。

3 碰撞躲避约束

两辆汽车不相撞近似为两个矩形不重叠,也就可以看作第一辆车的四个顶点在第二辆车构成的矩形之外,第二辆车的四个顶点也在第一辆车构成的矩形之外。当点在凸边形外,这个点到凸边形某两个点构成的多个三角形面积之和大于凸边形的面积。

20191020211253581.png

 三角形面积可以通过行列式得到20191020211346949.png

矩形的面积也就是凸边形的面积

20191020211422673.png

  此外,碰撞躲避约束还可以体现为很多形式,根据自动驾驶的不同场景而改变。

4 代价函数

前面的车辆动力学约束、边值约束、碰撞躲避约束都是必须严格满足的硬性约束,但在满足硬约束的情况下,轨迹还有很多条,所以我们需要设定一个代价函数去人为设定指标得到我们期望的最优轨迹。

(1)如果期待运动尽块完成,

 gif.latex?J_%7B1%7D%3Dt_%7Bf%7D

(2)如果期待车速变化小,

gif.latex?%5C%5C%5C%5CJ_%7B2%7D%3D%5Cint_%7B0%7D%5E%7Bt_%7Bf%7D%7Da%28t%29%5E%7B2%7D*dt

(3)如果期待转动速度变化小

gif.latex?%5C%5C%5C%5CJ_%7B3%7D%3D%5Cint_%7B0%7D%5E%7Bt_%7Bf%7D%7D%5Comega%20%28t%29%5E%7B2%7D*dt

(4)如果期待适当远离障碍物

gif.latex?J_%7B4%7D%3D%5Csum_%7Bj%3D1%7D%5E%7Bn%7D%5Cint_%7B0%7D%5E%7Bt_%7Bf%7D%7De%5E%7B-d_%7Bj%7D%28t%29%5E%7B2%7D%7D*dt

n为障碍物的数量,d函数代表车辆与障碍物的距离

除了上述例子,我们可以自行根据轨迹规划的目标自行设定代价函数。再将目标函数汇总,就可以得到完整的代价函数

gif.latex?J%3D%5Comega%20_%7B1%7D*J_%7B1%7D+%5Comega%20_%7B2%7D*J_%7B2%7D+%5Comega%20_%7B3%7D*J_%7B3%7D+%5Comega%20_%7B4%7D*J_%7B4%7D+...

gif.latex?%5Comega 为权重系数,意味着这个代价在整个代价中的重要性,我们可以进行灵活调参。

5 完整的最优控制问题

将上述的内容汇总就得到了车辆轨迹规划的最优控制框架

         min   J

s.t. 车辆动力学约束

      边值约束

      碰转躲避约束

下一篇文章将开始讲述路径规划中常见的算法。

### 基于插值与微分平坦的轨迹优化方法 #### 插值在路径规划中的应用 通过B样条插值实现平滑路径生成,能够有效处理复杂环境下的路径规划需求。B样条具备良好的凸包特性,这意味着控制点形成的多边形会包围整个曲线,从而简化了安全性验证过程并有助于施加动力学约束[^1]。 对于高维空间内的连续运动规划而言,采用B样条表示法可以确保各节段之间的光滑过渡而无需额外增加边界条件来维持Ck级数连续性。这种性质特别适用于机器人手臂末端效应器或其他移动平台的位置跟踪任务中,在这些应用场景里保持轨迹的一致性和稳定性至关重要。 ```python import numpy as np from scipy.interpolate import BSpline, splprep, splev # 定义一些示例数据点用于创建B样条 tck, u = splprep([x_data_points, y_data_points], s=0) # 计算新的均匀分布的时间向量 u_new = np.linspace(u.min(), u.max(), num=100) # 使用BSpline函数评估新时间上的位置 new_positions = splev(u_new, tck) ``` #### 微分平坦理论的应用 微分平坦是一种强大的工具,允许将复杂的非线性系统映射到一组易于操作的输入变量上。具体来说,如果一个动态系统的输出可以通过有限次求导得到所有状态,则称其为微分平坦。这一概念被广泛应用于现代航空航天工程以及地面车辆等领域内高效能控制器的设计当中。 当涉及到轮式四足机器人的轨迹优化时,利用ZMP(零力矩点)作为平衡指标并通过线性化的方式将其转化为简单的不等式形式成为可能。这种方法不仅提高了计算效率而且增强了算法鲁棒性;同时由于预先考虑到了角位移成分的影响因此可以直接获得满足物理规律的结果而不必依赖迭代求解NLP问题[^2]。 ```matlab % MATLAB伪代码展示如何设置和解决带有ZMP约束的最优控制问题 syms x(t) y(t) theta(t); % 笛卡尔坐标系下质心位置及朝向角度 eqns = [ diff(x,t,2) == f_x(...), ... diff(y,t,2) == f_y(...), ... diff(theta,t,2) == tau/Iz]; % 添加关于ZMP坐标的附加限制条件 constraints = [... zmp_x >= lb_zmp_x & zmp_x <= ub_zmp_x,... zmp_y >= lb_zmp_y & zmp_y <= ub_zmp_y]; sol = solve([eqns; constraints],[x,y,theta]); ```
评论 3
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

无意2121

创作不易,多多支持

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付:¥1
获取中
扫码支付

您的余额不足,请更换扫码支付或充值

打赏作者

实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

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

余额充值