Stanley Estimator 39-030 how to use

无人驾驶 算法 —— 使用 Stanley method 实现 无人 车 轨迹 追踪

无人驾驶 算法 —— 使用 Stanley method 实现 无人 车 轨迹 追踪


文章 已 搬至 知 乎 , 如果 对 你 有所 帮助 和 启发 , 欢迎 关注 , , 点 赞 , 评论 交流 , 感谢

Date: 2019/08/03
Editor: 萧潇子 (Jesse)
Contact: [email protected]

基于 几何 追踪 的 方法

关于 无人 车 追踪 轨迹 , 目前 的 主流 方法 分为 两类 : 基于 几何 追踪 的 方法 和 基于 模型 预测 的 方法 , 其中 几何 追踪 方法 主要 包含 纯 跟踪 跟踪 和 Stanley 两种 方法 , 纯 跟踪 方法 已经 广泛 应用于 移动机器人 的 路径 跟踪 中 , 网上 也 有 很多 详细 的 介绍 , 本文 主要 介绍 斯坦福 大学 无人 无人 使用 的 Stanley 方法。

Stanley method

Stanley 方法 是 一种 基于 横向 跟踪 误差 (cross-track error : eee 为 前轴 中心 到 最近 路径 点 (px, py) (p_x, p_y) (px, py) 的 距离) 的 非线性 反馈 函数 ,并且 能 实现 横向 跟踪 误差 指数 收敛 于 0。 根据 车辆 位 姿 与 给 定 路径 的 相对 几何 关系 可以 直观 的 获得 控制 车辆 方向盘 转角 的 控制 变量 , 其中 包含 横向 偏差 eee 和 航向 偏差 θ e \ theta_e θe 。e 。e
δ (t) = δ e (t) + δ θ e (t) \ delta (t) = \ delta_ {e} (t) + \ delta _ {\ theta_ {e}} (t) δ (t) = δe (T) + δθe (t)

  • 在 不 考虑 横向 跟踪 误差 的 情况 下 , 前轮 偏角 和 给 定 路径 切线 方向 一致 , 如图 所示。 其中 其中 θ e \ theta_e θe 表示 车辆 航向 与 最近 路径 点 切线 方向 之间 的 夹角 , 在没有 任何 横向 误差 的 情况 下 , 前轮 方向 与 所在 路径 点 的 方向 相同 :
    δ θ e (t) = θ e (t) \ delta _ {\ theta_ {e}} (t) = \ theta_e (t) δθe (t) = θe (t)
  • 在 不 考虑 航向 跟踪 偏差 的 情况 下 , 横向 跟踪 误差 越大 , 前轮 转向 角 越大 , 假设 车辆 预期 轨迹 在 距离 前轮 d (t) d (t) d (t) 处 与 给 定 路径 上 最近点 切线 相交 , 根据 几何 关系 得出 如下 非线性 比例 函数 :
    δe (t) = arctand (t) e (t) = arctanv (t) ke (t)
    其中 d (t) d (t) d (t) 与 车速 相关 , 最后 用 车速 v (t) v (t) v (t) , 增益 参数 kkk 表示。 随着 横向 误差 的 增加 , arctan arctan arctan 函数 产生一个 直接 指向 期望 路径 的 前轮 偏角 , 并且 收敛 受 车速 v (t) v (t) v (t) 限制。
    综合 两 方面 控制 因素 , 基本 转向 角 控制 率 如下 :
    δ (t) = θ e (t) + arctan ⁡ ke (t) v (t) \ delta (t) = \ theta_e (t) + \ arctan \ frac {ke (t)} {v (t)} δ (t) = θe (t) + arctanv (t) ke (t)
    使用 线性 自行车 运动 模型 , 可以 得到 横向 误差 的 变化 率 :
    e ˙ (t) = - v (t) sin ⁡ δ e (t) \ dot {e} (t) = -v (t) \ sin \ delta_ {e} (t) e˙ (t) = - v (t) sinδe (t)
    其中 sin ⁡ δ e (t) \ sin \ delta_ {e} (t) sinδe (t) 根据 几何 关系 可知 :
    sinδe (t) = d (t) 2+ (e (t)) 2 e (t) = v (t) 2+ (ke (t)) 2 ke (t)
    so:
    e˙ (t) = v (t) 2+ (ke (t)) 2 −v (t) ke (t) = 1 + (v (t) ke (t)) 2 −ke (t )

当 横向 跟踪 误差 e (t) e (t) e (t) 很 小时 , (ke (t) / v (t)) 2 → 0 (ke (t) / v (t)) ^ {2} \ to0 (ke (t) / v (t)) 2 → 0:
e ˙ (t) ≈ - k e (t) \ dot {e} (t) \ approx -ke (t) e˙ (t) ≈ − ke (t)
通过 积分 上 式:
e (t) = e (0) × exp ⁡ - k t e (t) = e (0) \ times \ exp ^ {- kt} e (t) = e (0) × exp − kt

因此 横向 误差 指数 收敛 于 e (t) = 0 e (t) = 0 e (t) = 0 , 参数 k k k 决定 了 收敛 速度。 对于 任意 横向 误差 , 微分方程 都 单调 的 收敛 到 0。

基于 Python 的 Stanley 算法 跟踪 直线 仿真
Stanley 控制 转向 角度 , 使用 一个 简单 的 P 控制器 控制 速度 , 首先 我们 定义 参数 数值 如下 :

这里 将 增益 参数 k k k 设置 为 0.3 , 速度 P 控制器 的 比例 系数 Kp 设置 为 1.0 , 时间 间隔 为 0.1 秒 , 车 的 轴距 我们 定 为 3.0 米。

定义 车辆 状态 类 , 在 简单 的 自行车 模型 中 , 我们 只 考虑 车辆 的 当前 位置 (x, y) (x, y) (x, y), 车辆 的 偏航 角度 yaw yaw yaw 以及 车辆 的 速度 vvv , 为了仿真 模拟 , 我们 定义 车辆 的 状态 更新 函数 来 模拟 真实 车辆 的 状态 更新 :

仿真 中 , 纵向 控制 使用 一个 简单 的 P 控制器 , 横向 控制 (即 转角 控制) 使用 Stanley 控制器 , 并且 限制 车轮 转角 范围 [-30, 30] , 这 两个 控制器 定义 如下 :

定义 函数 用于 搜索 最 临近 的 路 点 :

主 函数 :

仿真 结果 :

通过 仿真 结果 图 , 我们 看到 红 点 表示 实现 规划 好的 直线 路径 , 蓝 线 则 表示 我们 的 车辆 实际 运行 的 的 轨迹 , 绿 点 表示 距离 当前 位置 最近 路径 点 点 , 在 这段 代码 中 , 我们 设置 了 增益参数 kkk 为 0.3 , 通过 进一步 去 改变 kkk 的 大小 对比 结果 , 可以 减小 kkk 值得 到 更 平滑 的 跟踪 轨迹 , 但是 更加 平滑 的 结果 会 引起 某些 急剧 的 转角 处 会 存在 转向 不足 的 情况。

参考

  1. Automatic Steering Methods for Autonomous Automobile Path Tracking
  2. Stanley_ The robot that won the DARPA grand challenge
  3. 无人驾驶 汽车 系统 入门 (十八) —— 使用 pure pursuit 实现 无人 车 轨迹 追踪 (https://blog.csdn.net/adamshan/article/details/80555174)