深入理解如何不费吹灰之力搭建一个无人驾驶车（一）2D-小车底盘部分

一、搭建综述

无人驾驶最新很流行，但是很多人都觉得这东西蛮高大上的，因为CSDN还没有一个完整的介绍无人驾驶车如何做的博文，都很零散或者简略。其实有了ROS，这东西一个小学生都可以搭，但是对真正研究这个的人来说，光用开源代码搭建出一个能跑的机器是永远不够的，因为现实世界是复杂的，我们的假设往往不能很好的覆盖所有情况，比如如何解决动态场景下的SLAM问题，大场景下如何平衡内存消耗和计算效率的问题（比如如何更快更好地找到回环，局部更新等），以及如何用更少的计算消耗得到更大的计算精度的问题等。

所以本文的叙述体系是 简述到深入学习。
要讲的内容有2D-小车无人驾驶 2D汽车无人驾驶 3D汽车无人驾驶。

二、2D-小车无人驾驶

2.1 轮式里程计编写

因为是第一次叙述，故会将一些基础写出来，之后就不重复了。然后安装ros自己去看官网吧，这个不难，你要做无人驾驶，你首先需要一个车，这个车不管怎样，要能前进后退转向，车类型无所谓，就是运动模型差别，你只要能让他做到给他发一个位姿增量(△x,△y,△θ)，比如增量是(+1,+1,+60度)，那么它原本比如位姿是(2,2,120度)，之后他能到(3,3,180度)即可，这个θ是转向，可以见《概率机器人》的运动模型，如果车同时有恒定的角速度和线速度，那么它的轨迹理想下是一个圆，大家可以想象一下中学算天体运动的时候，为什么一个天体会绕另一个天体做圆周运动。那如果你想到达指定位姿增量，你可以先转一个角度然后前进，到达后再转一个角度，这就是里程计运动模型，也可以按速度运动模型及要机器人运动的时间间隔△t算出所需的v和w，此时轨迹为一个圆。
好了说白了就是，你要有一个小车，小车有轮子（要么差分驱动，要么有转向电机），你还要有个编码器能控制小车轮子（和转向电机，差分驱动不用），stm32也可以，对于某些不那么智能的编码器你可能需要连一个stm32（单片机），之后，编码器需通过485转USB，或者232转USB或者CAN转USB模块 连入电脑。 这时你需要写一个驱动（C++或python），并且专门开一个线程给监听IO信息，当你选择用C++串口与这个编码器通信步骤如下：你也可以直接搜C++串口通信

（1） 打开串口
（2） 配置串口
（3） 读写串口
（4） 关闭串口

然后查编码器手册，一般是你的电脑给编码器发一个读速度信息，然后编码器会给你一个速度（各个轮子的速度），你就读取这个速度，配合时间间隔△t，你就可以用它来计算轮式里程计了（累计里程计就是记录你离原点有多远的东西），以差分二轮驱动为例,当然这种不是很典型

其实就是说，左轮前进，右轮后退，他就向右转，两轮都前进他就前进，这个的轨迹是一个圆，旋转线速度即（左轮轮速-右轮轮速），前进速度v就是（左轮轮速+右轮轮速）/2，这个速度可为负，那有了这个，机器人转了多少度，我们就可以用高中的公式算出来，前进了多少也可以算出来，其中时间间隔为△t。
这样简单的轮式历程计就做完了，你可以发布odom信息（里程计信息）。(这里把odom名字改成wheel_odom)如下定义。你要把它发布出去，百度搜如何发布odom，自己去看。（不要直接复制，要分别放在特定位置）
当然有些编码器可以直接给出你两个轮子的总里程计信息。

//要先声明（一般写在class的某个运行函数里）
odom_pub_ = node.advertise("wheel_odom", 10);
//之后赋值（一般写在回调函数里）
	odom_.header.frame_id = wheel_odom;
    odom_.child_frame_id = base_link;
    odom_.header.stamp = now_;
    odom_.pose.pose.position.x = accumulation_x_;  //t从0到现在位姿增量的累加和
    odom_.pose.pose.position.y = accumulation_y_;
    odom_.pose.pose.position.z = 0;
    odom_.pose.pose.orientation.x = q.getX();
    odom_.pose.pose.orientation.y = q.getY();
    odom_.pose.pose.orientation.z = q.getZ();
    odom_.pose.pose.orientation.w = q.getW();
    odom_.twist.twist.linear.x = v_dis;
    odom_.twist.twist.linear.y = 0;
    odom_.twist.twist.angular.z = v_theta;
 //之后发布（一般在写回调函数最后）
 odom_pub_.publish(odom_);

然后发布一个odom到base_link的tf变换，在launch里写的

之后就可以在rviz里看到了，（记得加入一个axis）

2.2 对小车进行控制----cmd_vel订阅

这个东西一般是键盘控制节点或者导航节点发出的，现要求你能写一个节点使之能相应从键盘或导航节点发来的cmd_vel，先订阅cmd_vel

ros::Subscriber cmd_sub = node.subscribe("/cmd_vel", 10, &ChassisDriver::twist_callback, this);

然后再回调函数里解析

twist_mutex_.lock();
current_twist_ = *msg.get();
twist_mutex_.unlock();

这个mutex是互斥锁，你也可以用信号量，这个是为了进行对资源的互斥访问，详见操作系统原理。我们要解析下列东西：

double linear_speed = current_twist_.linear.x;
double angular_speed = current_twist_.angular.z;

一个是线速度，一个是角速度，那么我们的目的就是再用一个ros定时器：

ros::Timer timer = nh.createTimer(ros::Duration(0.1), timerCallback); 

每隔一段时间发给编码器信号控制轮子转速，注意不是cmd_vel一发过来就发送控制信号，这里是为了时序协调。这个在ros中是十分关键的，之后会讲到， 当然你也可以选择单独开个线程做这件事，这样整个底盘部分就OK啦吗？没有，还有个PID呢

2.3 对小车进行控制----根据cmd_vel及读取的速度进行PID控制
引用自一文读懂PID控制。

但原文没有提出怎么用，现在教大家写一个，如果想深入了解，或者比如ADRC，自行百度。

注：此时控制频率是读取频率的1/3
v[0] = wheelread(); //从编码器读速度
Vc = cmd_velV(); //cmd_vel的速度
v[3] = v[2];//t-2时速度
v[2] = v[1];//t-1时速度
v[1] = v[0];//t时速度
Upid = p*(Vc-V[0])+i*(3Vc-v[2]-v[1]-v[0])+d*(v[0]-v[1]);//输出的控制电压是这个，p比例系数，i积分系数，d微分系数。

实际上不需要很强的控制算法或根据自动控制原理给此系统建模进行最优的控制（注：不是最优控制），因为机器人能通过其他传感器进行修正，但是不能太差，比方说最优的控制是100分，你起码要80分。
这样我们底盘就完成了，但是汽车不同，汽车不仅运动模型不同，还需要考虑动力学模型。