在网上看过了很多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则为在给定的转向角下后轴遵循着的圆的半径。这个公式能够在较低速度的场景下对车辆运动做估计。