自动驾驶仿真：carla纯跟踪控制

自动驾驶仿真：carla纯跟踪控制

附赠自动驾驶最全的学习资料和量产经验：链接

完成前面三节以后，我相信很多小伙伴迫不及待的想去尝试一下在carla中加入自己的算法去控制车辆的运动了吧？这一节我们就来通过纯跟踪控制来实现这个目标。

1. 一个简单的Routing

要想实现对车辆的控制，得先需要有一条平滑的路径，这样我们才知道我们的汽车该什么时候左转，什么时候刹车。在自动驾驶中这是Planning做的事情，但是如果从规划层一步一步做到控制层，这太麻烦了，Planning的内容我们后面再做。这里我就用了一个比较简单的办法，就是直接从地图中选取出一条路径，然后控制我们的汽车去跟随这条路径。

1.1 从地图中选取出一条路径

carla的地图格式采用的是opendrive的地图格式，在创建地图的时候，其内部就已经提供了很多完成的路径。至于怎么提供的，其实就是对各个节点的链接。在apollo中，是将map转换成topomap，然后利用A*去进行搜索的。但是这里我们可以直接取：

geom::Transformtransform=spawn_point[200];std::vector<SharedPtr<client::Waypoint>>route;SharedPtr<client::Waypoint>waypoint=map->GetWaypoint(transform.location);for(inti=0;i<1000;++i){route.push_back(waypoint);std::vector<SharedPtr<client::Waypoint>>next_point=waypoint->GetNext(5);waypoint=next_point.front();}

OK，这样我们就有了一条路径route。我们可以试着将这条路径画出来大家看一下，

这里我们使用的地图依然是"Town04"。这里大家就能看到，我们已经取出了一条比较平滑的路径了，这里选取的规则是每个点之间的距离为5m，一共往前选取了1000个点，总距离也就是5km。

2. 纯跟踪控制汽车跟踪轨迹

当我们有了路径以后接下来我们就可以去进行控制了。纯跟踪控制的理论知识我就不在这里赘述了，网上各路大神讲的也很清晰，我自己也是在网上一些公开资料里学习的，所以我就不做搬运的工作了。简单总结一下就是，在轨迹上选取一个目标点作为我们跟踪的目标，然后通过二自由度模型中车辆后轮与目标点的几何关系求出汽车前轮转角与误差的几何关系从而实现对目标点的跟踪控制。不了解的建议去学习一下，自己动手算一算还是很简单的。我们还是直接看代码吧：

头文件：

structPoint{doublex;// x坐标
doubley;// y坐标
doubleyaw;};structPose{Pointpoint;doubleheading;// 航向角
};structControlCommand{doubledelta;Pointpoint;};structPpVehicle{doublewheelbase=2.5;// 轴距
doublemaxSteeringAngle=(70/180)*M_PI;// 最大转角限制
};classPurePursuitController{public:PurePursuitController(PpVehiclevehicle,std::vector<Point>path);ControlCommandcalculateControl(constPose&currentPos);doubleld(doublekv,doublev,doublel0){lookAheadDistance_=kv*v+l0;returnlookAheadDistance_;}private:PpVehiclevehicle_;doublelookAheadDistance_;intclosestIndex_=0;inttarget_index_;std::vector<Point>path_;};

源文件：

ControlCommandPurePursuitController::calculateControl(constPose&currentPos){if(path_.empty()){std::cout<<"path is empty!"<<std::endl;}doubleclosestDistance=std::numeric_limits<double>::max();// Find the closest point on the path
for(size_ti=0;i<path_.size();i++){doubledistance=std::hypot(path_[i].x-currentPos.point.x,path_[i].y-currentPos.point.y);if(distance<closestDistance){closestDistance=distance;closestIndex_=i;}}Pointtarget;target=path_[closestIndex_+3];ControlCommandcommand;doublealpha=atan2(target.y-currentPos.point.y,target.x-currentPos.point.x)-(currentPos.heading);command.delta=atan((2*vehicle_.wheelbase*std::sin(alpha))/lookAheadDistance_);command.delta=normalize_angle(command.delta);command.delta=(command.delta*180.0)/(70*M_PI);command.point=target;returncommand;}

调用函数：

voidPpControl(PurePursuitController&pp_controller,PosecurrentPose,SharedPtr<client::Vehicle>&vehicle){doublev=6.0;// velocity（1 m/s）
doublekv=0.1;doublel0=10;doubleepsilon=1.0e-6;doublethrottle=0.5;pp_controller.ld(kv,v,l0);ControlCommandcommand=pp_controller.calculateControl(currentPose);//    std::cout << "delta=" << command.delta << std::endl;
if(std::hypot(command.point.x-currentPose.point.x,command.point.y-currentPose.point.y)<epsilon){std::cout<<"reached the endpoint!"<<std::endl;v=0;throttle=0;}//    client::Vehicle::AckermannControl ackermannControl;
//    ackermannControl.speed = v;
//    ackermannControl.steer = command.delta;
//    vehicle->ApplyAckermannControl(ackermannControl);
client::Vehicle::Controlcontrol;control.steer=command.delta;control.throttle=throttle;vehicle->ApplyControl(control);}

主函数部分我就不粘贴过来了，前面有。你们直接在主函数的循环里调用这个函数就可以了。主要就是这里有一个路径格式的转化和一个汽车的初始参数：

std::vector<Point>path;for(inti=0;i<route.size();++i){Pointpoint_temp;point_temp.x=route[i]->GetTransform().location.x;point_temp.y=route[i]->GetTransform().location.y;point_temp.yaw=(route[i]->GetTransform().rotation.yaw/180)*M_PI;path.push_back(point_temp);}PpVehiclevehicle_param{3,(70/180)*M_PI};

因为我是直接复制的我之前写的代码，所以在这里做了个简单的转化。当然你们也可以直接使用carla提供的格式，是一样的。

我写的比较简单，但是基本的功能是可以实现的，但是要想控制的好，在这个过程中还是要加入很多其他的决策的。比喻纵向速度的控制，在过一些弯道比较大的弯的时候肯定是要减速的。但是我这里主要是针对横向上的控制，所以就没有去考虑纵向速度，大家在自己尝试的时候可以添加进去。还有就是尾部判断，我这里只是加了个简单的判断，但实际在使用的时候好像会直接失控而不是停下来，这里我就没有改了，留给大家自己去做，算是我留的一个作业吧。还有路口的红绿灯，这个我在前面就已经介绍过了，大家可以直接复制粘贴进去。

这里面需要特别注意的是弧度和角度。carla这仿真器它没有统一使用哪一种，而是两种都有使用。比喻说路径里面输入的rotation.yaw就是一个角度，所以你得去把它转成弧度。而且，最恶心的是他控制输入的steer，steer的范围是[-1，1]。但是这里的1它不是弧度的1，而是(70/180)*pi，也就是70度，也就是说当我们steer输入1的时候对应的表示前轮转角为70度，但是我分别打印出前面两个轮子的转角吧，他又不是。但是咱不管它是不是，我们在算完弧度后，再输入steer的时候，都需要对他进行一下转化。也就是这行代码：

command.delta=(command.delta*180.0)/(70*M_PI);

最后大家一起来看一下控制效果吧！

01:44