底层驱动板与上层控制板的通信以及里程计实现
在前面介绍了底层驱动板的设计以及其所具有的功能。其中，底层控制板最重要的功能之一就是负责底层信息的采集以及电机的驱动，在采集到传感器的信息之后需要与上层控制板进行信息交互，同时上层控制板也需要下发一些控制指令，在本篇中将详细介绍如何实现两者之间的通信。另外，我们可以在此基础上使用IMU信息和编码器信息来实现一个里程计的功能。-
本文所涉及的代码可以在github仓库中找到，其链接为：https://github.com/softdream/robot_projects/tree/master/odometry
1. 通信方式
首先要明确的一点是机器人的底层驱动板与上层控制板的通信方式为串口通信，在X3派中可以直接使用串口3，如图1所示：-
将控制板的串口与x3派的串口引脚相连接之后，可以在/dev目录下面发现串口设备：ttyS3，这里要注意地线也要相连接。
2. 通信程序设计
通信的程序设计是本篇文章的重点，这里会侧重介绍x3派上的代码设计与编写，即我们如何在x3派上编写代码实现串口的数据接收以及发送功能。由于x3派上跑的有操作系统，因此我们可以直接在操作系统的系统接口进行封装就可以了，而不需要关心具体的串口驱动。同时我们采用了一种高效的IO复用模型EPOLL来提高通信效率。
2.1 串口设备操作接口封装
我们知道，在linux下对各种外设的操作都是统一成文件的操作形式，特别是对于串口这种字符设备，只要获取到它的文件描述符，即可食用相应的api对其进行使用。我们的封装代码可以参考odometry/include/uart.h这个文件里的代码。-
首先要使用open函数获取串口3的设备描述符，然后设置串口的波特率、停止位等一系列参数，这些操作封装在initUart()函数中了。然后再实现一个串口发送数据操作和一个串口读取数据操作：
/*
* 从串口中读取长度为len的数据并放在以buffer为起始地址的空间上
*/
int readData( char* buffer, int len )
{
return read( fd, buffer, len );
}
/*
* 往串口中写入任意类型的数据
*/
template<typename T>
int writeData( T&& data )
{
return write( fd, &data, sizeof( data ) );
}
可以看出这里也是直接调用了read和write系统调用。
2.2 IO复用模型
搞过socket或者其它类型的通信程序的小伙伴都知道，在接收数据的时候，可以在一个while循环里调用recv函数来轮询的以阻塞或者非阻塞的方式读取对端发过来的数据。当发送方数量增多的时候，轮询查询的方式效率显然不太高，因此在linux下面，诞生了IO多路复用模型，即可以在一个线程中实现多路IO操作。虽然在我们这个项目中，数据的发送方始终只有一个(即底层控制板)，但还是选择使用EPOLL来管理数据的交互，具体的来说：-
我们在odometry/include/EpollEvent.h，odometry/include/IEvent.h和/odometry/src/EpollEvent.cpp这三个文件里实现了最基本的epoll操作封装。首先要知道EPOLL是“事件驱动型”的，即当事件到来的时候，就会立即触发某一个操作。-
在IEvent.h文件中，我们定义了事件的类型如下：
typedef std::function<void*(int, void*)> FUNC;
typedef struct{
int fd;// the fd want to monitor
short event;// the event want to monitor
FUNC callback; // the callback function
void *arg;// the parameters of the callback function
}Event;
然后在EpollEvent.h和EpollEvent.cpp文件中实现了三个最重要的操作函数：-
initEvent()，addEvent(), dispatcher();-
其作用分别是：-
initEvent()： 初始化EPOLL设备，在这里会调用系统函数epoll_create(),可以得到epoll设备的文件描述符。-
addEvent()：添加一个事件到EPOLL设备中，具体的事件即结构体Event中所定义的这些。这里会调用系统函数epoll_ctl(),设置epoll要监听的事件的文件描述符以及触发方式。事件的触发方式有两种：边沿触发和水平触发。边沿触发是指事件到来的一瞬间只触发一次，而水平触发是指在事件存在的那段时间内一直会保持触发状态。可以类比单片机中的外部中断的触发方式。-
dispatcher():开始监听事件。这里会调用系统函数epoll_wait(), epoll会在一个线程中持续监听事件的到来情况。如果事件触发，就会调用相应的回调函数对数据进行处理。
2.3 使用EPOLL实现串口通信
使用EPOLL实现底层驱动板与上层控制板的串口通信的代码在odometry/include-
/odometry.h文件中，具体流程如图2所示：-
可以直观的理解为：一旦X3派收到了底层驱动板发过来的一帧数据，就会立刻触发odomRecvCallback()这个回调函数对其进行处理。那么在这个函数里，我们首先调用上面所封装好的串口数据读取函数readData()将数据放在数组recv_buffer_中。然后再调用parseData()函数来对数据进行一个解析，具体解析方式这里就不多做介绍了，各位小伙伴也可以根据自己的情况进行更改。
2.4 回调函数中的处理
为了实现里程计的功能，我在这里所做的操作重点是对接收到的IMU和编码器数据做一个预处理，然后调用EKF的接口来计算机器人的位姿。-
其中，底层控制板往x3派上发过来的IMU和编码器数据是打包在一起的一个字符串，其格式为：millis(), velocity, delta_s, delta_angle, imu.gz, l_rpm, r_rpm；即：毫秒级时间戳，线速度，位移增量，旋转角度增量，角速度，左轮转速，右轮转速。-
至于EKF的原理和代码实现，则在下一节中详细介绍。-
这里EKF的预测过程中的控制输入为(delta_s, delta_angle)组成的向量，而EKF的观测更新中，注意需要在得到机器人旋转的角速度后，使用了一个中值积分来计算单位时间内旋转的角度，把这个角度作为EKF的观测输入。
3. 使用扩展卡尔曼滤波实现里程计功能
EKF的实现代码在odometry/include/ekf_fusion.h文件中，我们的基本思想是使用编码器的测量数据来执行EKF的预测步骤，然后使用IMU的测量数据来执行EKF的更新步骤。
3.1 预测步骤
假设机器人在k时刻的状态为：
Xk=(xk,yk,θk)T(1)X_k = (x_k, y_k, \theta_k)^T \quad(1)Xk=(xk,yk,θk)T(1)
机器人在k时刻的运动增量为：
(Δsk,Δθk)T(2)(\Delta s_k, \Delta \theta_k )^T \quad(2)(Δsk,Δθk)T(2)
其中，Δs\Delta sΔs为机器人运动位移增量，Δθ\Delta \thetaΔθ为机器人运动角度增量， 这两个值可以由编码器测量得到，具体来说，假设机器人车轮转一圈编码器的输出脉冲数为NencoderN_encoderNencoder，车轮周长为circumferencecircumferencecircumference,并且在k时刻左右轮编码器的输出分别为lkl_klk和rkr_krk，那么就有：
{Δskl=(lk−lk−1)/Nencoder⋅circumferenceΔskl=(rk−rk−1)/Nencoder⋅circumference(3)\begin{equation} \begin{cases} \Delta s_k^l = (l_k - l_{k-1}) / N_{encoder} \cdot circumference\\ \Delta s_k^l = (r_k - r_{k-1}) / N_{encoder} \cdot circumference\\ \end{cases} \end{equation} \quad(3){Δskl=(lk−lk−1)/Nencoder⋅circumferenceΔskl=(rk−rk−1)/Nencoder⋅circumference(3)
其中，Δskl\Delta s_k^lΔskl表示左轮位移量，Δskl\Delta s_k^lΔskl表示右轮位移量，根据左右轮的位移量可以算出机器人的位移增量和旋转增量：
{Δsk=(Δskr+Δskl)/2Δθk=(Δskr−Δskl)/width(4)\begin{equation} \begin{cases} \Delta s_k = (\Delta s_k^r + \Delta s_k^l) / 2 \\ \Delta \theta_k = (\Delta s_k^r - \Delta s_k^l) / width \\ \end{cases} \end{equation} \quad(4){Δsk=(Δskr+Δskl)/2Δθk=(Δskr−Δskl)/width(4)
使用两轮差速底盘的割线运动模型对机器人状态进行预测：
{xk=xk−1+Δskcos(θk+12Δθk))yk=yk−1+Δsksin(θk+12Δθk))θk=θk−1+Δθk(5)\begin{equation} \begin{cases} x_k = x_{k-1} + \Delta s_k cos(\theta_k + \frac{1}{2}\Delta \theta_k)) \\ y_k = y_{k-1} + \Delta s_k sin(\theta_k + \frac{1}{2}\Delta \theta_k)) \\ \theta_k = \theta_{k-1} + \Delta \theta_k \end{cases} \end{equation} \quad(5)⎩⎨⎧xk=xk−1+Δskcos(θk+21Δθk))yk=yk−1+Δsksin(θk+21Δθk))θk=θk−1+Δθk(5)
3.2 预测方程的雅可比矩阵
由于预测方程是非线性的，因此需要对其状态向量求导，得到雅可比矩阵为：
F3×3=[10−Δsksin(θk+12Δθk)01Δskcos(θk+12Δθk)001](6)F_{3\times3}= \left[ {\begin{array}{cc} 1 & 0 & -\Delta s_k sin( \theta_k + \frac{1}{2} \Delta \theta_k ) \\ 0 & 1 & \Delta s_k cos( \theta_k + \frac{1}{2} \Delta \theta_k ) \\ 0 & 0 & 1 \end{array} } \right] \quad(6)F3×3=⎣⎡100010−Δsksin(θk+21Δθk)Δskcos(θk+21Δθk)1⎦⎤(6)
3.3 观测更新步骤
在观测更新步骤中，我们只使用imu测出的角度增量作为观测值。观测方程可写为：
h(Xk)=HXk=θk(7)h(X_k) = HX_k = \theta_k \quad(7)h(Xk)=HXk=θk(7)
其中，
H1×3=(0,0,1)(8)H_{1\times3} = (0, 0, 1) \quad(8)H1×3=(0,0,1)(8)
具体的代码实现可参考ekf_fusion.h这个文件，这里就不细讲了。如果想详细的了解卡尔曼滤波器在融合编码器和IMU数据这块的原理的，可以参考本人的知乎专栏：https://zhuanlan.zhihu.com/p/536392146
4. 实验结果
在工程-
https://github.com/softdream/robot_projects/tree/master/odometry-
中，新建build目录，执行：
cmake ..
make
然后执行：
sudo ./robot
运行结果如图所示：
里程计功能演示