手把手教用matlab做无人驾驶（四）-pure pursuit

如上图汽车的自行车模型，上面轻灰色显示的是车，这个车坐标系是红色，这个世界坐标系是蓝色，轮子转向角是,两个轴扩展虚线然后相交于ICR。ICR到后轮和到前轮的距离分别为R1，R2。

运动等式：

对于上面的公式车身角度增量与轮子转向的关系的推导：

轮子的约束条件：

纯追踪算法对于路径跟随而言是简单和有效的方法，这里给出跟踪圆的路径，

在matlab/simulink中搭建仿真模型：

上面的自行车模型如下：

上面圆的路径如下：

仿真图形如下：

控制角度也可以用曲率：

tf::Vector3 p1(path.poses.at(i).pose.position.x, path.poses.at(i).pose.position.y, 0.0);

tf::Vector3 p2(path.poses.at(max(0,i-1)).pose.position.x, path.poses.at(max(0,i-1)).pose.position.y, 0.0);

pos_sp_ = tf_.inverse() * ((p1 + p2) / 2.0);

float curvature = 2*abs(pos_sp_.getY())/(LOOK_AHEAD_DIST * LOOK_AHEAD_DIST);

// float ang_diff=atan((path.poses.at(i).pose.position.y-path.poses.at(max(0,i-1)).pose.position.y)/(path.poses.at(i).pose.position.x-path.poses.at(max(0,i-1)).pose.position.x));

// publish drive cmds

float steering_cmd = P_GAIN * curvature;

// float steering_cmd = 0.15* (ang_diff- car_pose_.position.z);

if (pos_sp_.getY()<0){steering_cmd *= -1.0;}

外国电子书籍下载地址：

http://b-ok.org/