ros平台下基于激光slam的室内导航机器人搭建（三）

第三篇：底层驱动板开发

前言

这篇博客主要讲述底层驱动板的开发，底层驱动板的作用是控制机器人的移动，首先给大家看一下成品图。

这块板子的原理图和PCB我放在第一篇博客了，大家可以自行下载。
这块板子主要的任务分为三大部分，一是控制四个电机的转速，二是通过电机转速计算出实时速度，三是通过串口将实时速度上传至运行ros的上位机。下面我将分别讲述各部分的开发。

一、资源分配

主控芯片我采用的是STM32F407ZET6
定时器1用于输出四轮PWM,分别控制四个轮子转速
定时器2用于产生rosserial通信的时间戳
定时器3用于读取A轮编码器
定时器4用于读取B轮编码器
定时器5用于读取C轮编码器
定时器8用于读取D轮编码器
定时器6用于定时往上位机发布机器人的实时速度

串口1用于rosserial通信，同时开启对应的DMA
IIC1引出来预留着，方便以后扩展IMU，但是这次我没有用上IMU

以上资源配置的具体参数参考我发布的工程文件里面的wyu_robot.ioc文件

二、基本参数

底盘的基本参数写在config.h文件里，如果切换机器人底盘，只需要修改一下参数就可以很方便的移植代码。

三、电机控制

这个机器人一共有四个电机，我们需要分别计算出每个电机的转速，然后用PID算法控制转速。

TB6612

电机驱动板是TB6612

TB6612可以通过PWM波的占空比不同来控制输出电压的线性变化，因此可以通过PID算法计算出对应的占空比来控制电机的转速。
定时器1可以输出4路PWM波，刚好可以控制四个轮子。
下面是cubemx的定时器1设置，可以看到我设置了四路PWM输出端，它们分别引到两块TB6612的四个PWM引脚。

运动学计算Kinematics

这个文件的作用是速度分解，就是将底盘的移动速度拆解成四个轮子的转速，或者通过四个的转速计算出底盘的移动速度。

PID算法

对于没使用过PID算法的人来说，这里可能是个难点，所以我这里给大家讲解一下这个算法的思路。
PID算法是经典的控制算法，在这里我使用它控制轮子的转速。
首先我们先明确控制量，那就是PWM波的占空比，占空比越大，电机转的越快，占空比越小，则电机转的越慢。因此不难想到，当需要增速时，我们需要增加电压（增大占空比），当需要减速时，我们需要减小电压（减小真空比）。
P I D三个参数分别代表比例 积分 微分

P:比例参数，假设目标值与实际值相差X个单位，则给输出补偿P * X个单位，由于P是个固定的常数，因此P是比例调节。
I:积分参数，设X的积分为Y，给输出补偿I * Y个单位，用于消除P无法消除的稳态误差，也叫滞后调节。
D：微分参数，设当前X与上一次X的差值为Z，则给输出补偿D * Z个单位，这个差值反应了误差的加速度，根据误差增减的趋势提前调节，因此也叫超前调节。

因此不难看出PID算法的思想就是补偿，少了就补多一点，多了就补偿负值。这个慢慢体会。

电机转速设置

为了提高代码的可移植行，我使用了C++来开发STM32，因此我使用了C++的面向对象特性来开发电机，首先将电机封装成一个类，然后实例化出四个电机对象，分别为motorA motorB motorC motorD，
每个对象控制一个电机。同时可以看到我还将PID算法封装成了类，和电机一样实例化了四个PID对象，分别用于四个电机的PID计算。设置电机的转速只需要调用对应的电机对象来进行操作就可以了。
例如设置电机motorA 的转速：

motorA.spin(motorA_pwm);
 

四、计算底盘速度

通过运动学计算，将四个轮子转速转换为底盘移动速度

Kinematics::velocities Kinematics::rpm_to_Velocities(double motor_a_rpm, double motor_b_rpm, double motor_c_rpm, double motor_d_rpm)
 {Kinematics::velocities vel;vel.linear_x = (float)(motor_a_rpm + motor_b_rpm + motor_c_rpm + motor_d_rpm) * circumference / 4 / 60 ; vel.linear_y = (float)(-motor_a_rpm + motor_b_rpm + motor_c_rpm - motor_d_rpm) * circumference / 4 / 60 ; vel.angular_z  = (float)(-motor_a_rpm + motor_b_rpm - motor_c_rpm + motor_d_rpm) * circumference / 4 / 60 / ((base_width / 2) + (base_long / 2));return vel;
 }
 

五、rosserial协议的移植

由于上位机运行了ros系统，因此可以使用rosserial协议进行串口传输，这样做的好处是，我们STM32的开发可以使用ros的一些特性，比如发布话题和订阅话题，由于该步骤比较复杂，因此我专门写了一个协议的移植教程。具体的步骤大家可以看我另一篇博客rosserial协议移植教程。

六、订阅和发布话题

在mainpp.cpp文件里我们需要使用rosserial建立两个节点，如上图所示
一个节点用于订阅上位机发布的指令速度话题（这个话题的作用是上位机给驱动板发布速度指令，驱动板需要控制机器人的移动速度与指令速度一样，说白了就是上位机告诉底层驱动板，机器人需要跑这么快）。在这个工程中，我使用定时器6设置每经过100ms，STM32读取一次速度信息。
第二个节用于发布速度信息，就是底层驱动板告诉上位机我现在的速度是多少，这个数据的作用是构建里程计数据，上位机接收到这个数据后，会进行积分，然后可以推断出机器人的位移，这样就可以配合雷达数据构建地图。在这个工程中，我使用定时器6设置每经过20ms，STM32就往上位机发布一次速度信息，并且更新电机转速。

七、总结

1、每20ms发布一次底盘速度信息并更新电机转速。
2、每100ms读取一次指令速度。

xt_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L3FxXzM3NDE2MjU4,size_16,color_FFFFFF,t_70)