本文是学习自动驾驶控制算法第七讲 离散规划轨迹的误差计算的学习笔记。

上一节中，我们知道$e_{rr}$中元素的表达式如下：
$$\begin{array}{c}\{\begin{array}{l}{e}_d=d\\ e_{\phi }=\phi -{\theta }_r\\ {\dot{e}}_d=v\sin (\theta -{\theta }_r)\\ {\dot{e}}_{\phi }=\dot{\phi }-{\dot{\theta }}_r=\phi -k_r\dot{s}\end{array}\end{array}$$
接下来计算$e_{rr}$元素表达式中的未知量，可以参考自然坐标系和笛卡尔坐标系之间的转换关系。
第一步计算投影点，
首先遍历离散的轨迹点，与自车当前位置距离最近的点认为是匹配点，记为$${\mathbf{x}}_{\mathbf{m}}=(x_m,y_m)$$，理论上可以将匹配点作为投影点进行后续计算，这需要轨迹点特别密集，这里我们通过匹配点进一步寻找投影点：

如图所示
$$\begin{array}{c}{\mathbf{\tau }}_{\mathbf{m}}=(\cos {\theta }_m,\sin {\theta }_m)\end{array}$$
$$\begin{array}{c}{\mathbf{n}}_{\mathbf{m}}=(-\sin {\theta }_m,\cos {\theta }_m)\end{array}$$
近似有
$$\begin{array}{c}{e}_d=(\mathbf{x}-{\mathbf{x}}_{\mathbf{m}})\cdot {\mathbf{n}}_{\mathbf{m}}\end{array}$$
$$\begin{array}{c}\Delta s=(\mathbf{x}-{\mathbf{x}}_{\mathbf{m}})\cdot {\mathbf{\tau }}_{\mathbf{m}}\end{array}$$
这里$\mathbf{x}$表示自车位置，$\Delta s$为正表示投影点在匹配点前面，为负表示在后面。
另外，假设投影点处的曲率$k_r$与匹配点处的曲率$k_m$近似相等
$$\begin{array}{c}{k}_r=k_m\end{array}$$
$$\begin{array}{c}{\theta }_r={\theta }_m+k_m\Delta s\end{array}$$
根据自然坐标系和笛卡尔坐标系之间的转换关系可知
$$\begin{array}{c}\dot{s}=\frac{v\cos (\theta -{\theta }_r)}{1-k_r{e}_d}\end{array}$$
综上，带入到式1中就得到了$e_{rr}$中所有元素的值，结合上一节的计算，也就得到了控制量
$$\begin{array}{c}u=-K{e}_{rr}+{\delta }_f\end{array}$$
的值。