,
如上四个公式,误差的计算要用到投影点的信息、与曲率k,以及定位到的车的实际点的信息、与横摆角速度。
误差是基于车在连续的Frenet坐标系上的投影点算出的,但是一般规划模块规划出的是一系列离散的点,所以要计算基于离散的Frenet坐标系的规划点的误差。
计算离散轨迹的误差
(1)找投影点
先找匹配点,就是从离散的轨迹点中找到与车的真实位置最近的点,就是匹配点(match_point),找到匹配点就可以用匹配点的信息和车真实位置的信息近似的算出投影点。
车真实位置信息,匹配点信息。
假设匹配点与投影点之间用一段连续的圆弧代替,即认为匹配点和投影点的曲率一样都是。由匹配点的得出切线向量和法向量。匹配点到车真实位置的向量为。
(2)计算投影点的信息
横向误差,在直线段没有误差,在曲线段误差正比于规划点间隔的平方。
弧长,匹配点与投影点之间的弧长。
或者由上图得出。
,
总结:
通过离散的规划点来近似计算出投影点的信息,然后代入误差的计算公式得到的值,代入控制公式算出使误差趋于0的控制量。