自动驾驶控制算法---离散规划轨迹的误差

Be No.1

已于 2023-11-23 11:31:29 修改

阅读量95

点赞数 1

文章标签：自动驾驶人工智能机器学习

于 2023-11-03 21:36:25 首次发布

本文链接：https://blog.csdn.net/weixin_46072670/article/details/134192227

版权

$e_{d}=(\vec{x}-\vec{x_{r}})\cdot \vec{n_{r}}$

$\dot{e_{d}}=\left | \vec{v} \right |\sin (\theta -\theta _{r})$

$e_{\varphi }=\varphi -\theta _{r}$

$\dot{e_{\varphi }}=\dot{\varphi }-\dot{\theta _{r}}=\dot{\varphi }-k\dot{s}$ ， $\dot{s}=\frac{\left | \vec{v} \right |\cos (\theta -\theta _{r})}{1-ke_{d}}$

　　如上四个公式，误差 $e_{rr}$ 的计算要用到投影点的信息 $\vec{x_{r}}$ 、 $\theta _{r}$ 与曲率k，以及定位到的车的实际点的信息 $\vec{x}$ 、 $\vec{v}$ 与横摆角速度 $\dot{\varphi }$ 。

　　误差 $e_{rr}$ 是基于车在连续的Frenet坐标系上的投影点算出的，但是一般规划模块规划出的是一系列离散的点，所以要计算基于离散的Frenet坐标系的规划点的误差 $e_{rr}$ 。

计算离散轨迹的误差

　　（1）找投影点

　　先找匹配点，就是从离散的轨迹点中找到与车的真实位置 $\vec{x}$ 最近的点，就是匹配点(match_point)，找到匹配点就可以用匹配点的信息和车真实位置的信息近似的算出投影点。

　　车真实位置信息 $\left ( x,y,v,\theta \right )$ ，匹配点信息 $\left ( x_{m},y_{m},\theta _{m},k_{m} \right )$ 。

　　假设匹配点与投影点之间用一段连续的圆弧代替，即认为匹配点和投影点的曲率一样都是 $k_{m}$ 。由匹配点的 $\theta _{m}$ 得出切线向量 $\vec{\tau _{m}}=(\cos \theta _{m},\sin \theta _{m})$ 和法向量 $\vec{n_{m}}=(-\sin \theta _{m},\cos \theta _{m})$ 。匹配点到车真实位置的向量为 $\vec{x}-\vec{x_{m}}=(x-x_{m},y-y_{m})$ 。

　　（2）计算投影点的信息

　　横向误差 $e_{d}\approx(\vec{x}-\vec{x_{m}})\cdot \vec{n_{m}}$ ，在直线段没有误差，在曲线段误差正比于规划点间隔的平方。

　　弧长 $e_{s}\approx(\vec{x}-\vec{x_{m}})\cdot \vec{\tau _{m}}$ ，匹配点与投影点之间的弧长。

　　 $\theta _{r}= \theta _{m}$ 或者由上图得出 $\theta _{r}= \theta _{m}+k_{m}\cdot e_{s}=\theta _{m}+k_{m}\cdot(\vec{x}-\vec{x_{m}})\cdot \vec{\tau _{m}}$ 。

　　 $k_{r}=k_{m}$ ， $\dot{s}=\frac{\left | \vec{v} \right |\cos (\theta -\theta _{r})}{1-k_{m}e_{d}}$

总结：

$e_{d}=(\vec{x}-\vec{x_{r}})\cdot \vec{n_{m}}$

$\dot{e_{d}}=\left | \vec{v} \right |\sin (\theta -\theta _{r})$

$e_{\varphi }=\varphi -\theta _{r}$

$\dot{e_{\varphi }}=\dot{\varphi }-\dot{\theta _{r}}=\dot{\varphi }-k_{m}\dot{s}=\dot{\varphi }-k_{m}\frac{\left | \vec{v} \right |\cos (\theta -\theta _{r})}{1-k_{m}e_{d}}$