Puresuit 轨迹跟踪

在网上看过了很多Puresuit的轨迹跟踪算法，看起来都写的差不多，用起来不会用。
套用一份demo,在C++转C语言的时候又深入理解了一些，在此整理成文档，供大家参考。

输入

1.输入量是什么;

	要知道车的长度，车的后轮位置以及下一个点的位置。车要求是前轮转向的。控制速度会比较慢。

2.计算结果是什么：

是车的前轮转角。
通过当前车的位置（后轮位置），下一个点的位置，车的航向，轴距。
计算在车的位置和下一个点的位置的横向偏差和纵向偏差，然后转到车体坐标系。

3.怎么使用

目前，假设已经知道在大地坐标系下的坐标及航向(和真北的夹角)（car_X,car_Y，yaw)和下一个点的坐标（target_X,target_Y）,可以计算出横向偏差和纵向偏差：

```cpp
delta_x = car_X - target_X ;
delta_y = car_Y - target_Y ;
//转到车体坐标系下
x = delta_x *sin(yaw) + delta_y *cos(yaw) ; //e 
y = delta_x *cos(yaw) - delta_y *sin(yaw) ; // 没有用到 
e = x ;
//同时，计算两点之间的直线距离 ld
ld =sqrt(pow(delta_x ,2),pow(delta_y ,2));
//tan(cruve) = 2L*e/(ld*ld) ,L为轴距，车长。
cruve = arctan(2L*e/(ld*ld))

//另外，其实tan(cruve) = 2L*sin(alp)/ld ,sin(alp) = e /ld
//如果知道alp 角，就可以直接用，不需要这么麻烦了

其中 δ 表示前轮的转角，L 为轴距（Wheelbase），R则为在给定的转向角下后轴遵循着的圆的半径。这个公式能够在较低速度的场景下对车辆运动做估计。