基于 RT-Thread的麦克纳姆轮小车循迹运动控制算法开发和研究

??我们采用了沁恒 WCH的 CH32V103R8T6芯片作为主控芯片。在图像采集传感器上，我们使用了 MT9V032摄像头来做循迹处理。考虑到 H车模中 Mecanum轮的全方位运行的特点和此次竞赛中要求在三岔路口切换行进姿态以及单片机定时器资源的受限，我们外加了一个舵机来控制摄像头的采集方位，从而实现全向的循迹运行。此外，我们还使用了 BMX055陀螺仪辅助车身的坡道判断以及进入三岔时车身旋转的精准把控。

??为便于调试，我们使用了五轴按键、拨码开关、蓝牙、 TFT18显示屏来构成人机交互模块。
??车模整体情况如下图所示：

3.2摄像头安装

??为了使得摄像头调试方便，对于摄像头的仰角的调整，我们的调整原则是能够看到尽量远的同时，也要保证看的够近，才有更多的空间调整车身姿态。摄像头固定的杆子采用了轻便坚固的碳纤维管作为安装摄像头的材料，再用底座与支架将摄像头固定下来。底座与一个舵机相连接。

3.4舵机的安装

??整体来说便是摄像头安装到碳素杆上，碳素杆通过底座安装到舵机的舵盘上，舵机通过 3D打印的支架安装到主控板上方，因为主控板本身是居中安装的，所以能保证摄像头居中安装。因为比赛规则中规定小车要在三岔道路内横向行驶，因此必须要让摄像头能转 180度，除了采用双摄的方案外，我们才用舵机转摄像头的方案，使小车能实现前后双向行驶，减少小车的转体的时间。

??初代车模挡板的设计理念主要是为了车轮防撞，因为 H车的轮胎可变因素较多，速度一旦快起来车模可能会冲出赛道，因此设计挡板避免车模受损。

??在设计第二代挡板的时候，主要考虑减少挡板的长度，避免挡板卡住路肩的情况，就形成了第二代车模挡板，即在最初的挡板上修改，我们将两侧的部分切割掉，这样即保留了碳素板的强度，同时不至于卡到凸台上，其最终实物如下图：

§04 硬件电路设计

??本车硬件设计主要分为以下几个模块：电源管理模块，声音采集模块，速度采集模块，单片机处理核心模块，电机驱动模块，交互模块。稳定性是硬件的基本要求，我们选取线性电源芯片给 MCU供电，减少主控的附件，按照车模外形设计电路板的外形，将驱动电路与 MCU分开，分成了主控和驱动部分电路。

4.1主控芯片

??本车模采用南京沁恒公司的 CH32V103R8T6芯片作为主控芯片，该芯片采用封装，方便焊接。此次竞赛我们采用了双核通信的方案，单片机及外围电路如下：

4.3调试模块

??在调试时必要的会需要对行进时实时数据进行监测，运用蓝牙可以很方便的实现， OLED对菜单显示可以方便调节参数。蓝牙和 OLED接口原理图如图 4.3。

4.5测速模块

??光电编码器是一种通过光电转换将输出轴上的机械几何位移量转换成脉冲或数字量的传感器 ,这也是目前应用最多的测速传感器之一。其获取信息准确、精度高、应用简单。

??采用 1024线光电编码器，供电电压为 3.3V-5V，输出为小幅值的方波信号。通过正交解码后获得的脉冲数，同时定时获取脉冲数，即可获得编码器的转速，通过计算编码器齿轮和车模齿轮的齿轮比，即可计算出车轮的转速，只需测量轮子的半径即可计算出实际的车速。测速模块为了减少车辆负载，采用体积较小的可以正交解码的 1024线逐飞 mini编码器。

4.7 SD卡电路

??SD卡槽电路原理图如下：

4.7.3软件设计

??SD卡使用时直接将需要存储的数据按顺序写入固定的扇区中，需要时再读固定的扇区将数据按顺序取出来。

4.8驱动板改进

??在我们传统使用的 IR2104驱动板中需要两路 PWM波控制一个电机，而对于我们竞赛要求使用的单片机资源受限，定时器资源不够，因此我们改用了 DRV驱动电路，只需要改变 GPIO的高低电平即可控制车轮的正反转，一路 PWM控制一个电机，这样控制四个电机只需要一个定时器。

4.9舵机供电电路

??采用较高的电压可以提高舵机的响应速度，但同时过高的电压容易导致舵机工作不稳定。综合各方面因素的考虑，舵机采用电池电压直接进行供电。为了防止舵机产生的反向电动势对主板的影响，在电池与舵机之间接一个肖特基二极管同时舵机的信号口和单片机的 IO之间使用光耦进行电气隔离。舵机模块电路的原理图如下。

5.2赛道中心线提取及优化处理

5.2.1原始图像的特点

??在单片机采集图像信号后需要对其进行处理以提取主要的赛道信息，今年使用总钻风，由于芯片 RAM相较于往年使用的芯片太小，我们采集图像时遇到了不少问题，图像会卡住，，一部分原因是我们时序原因，但是修改后仍然会出现问题，虽然概率非常小，但是不得不说对稳定性是有了一定影响，当然也有可能是我们软硬件设计上的原因导致。摄像头采集图像时，由于我们的图像小精度低，在十字、车库、三岔等赛道元素存在反光、阴影、远处图像不清晰等的干扰，使得采集到的图像质量大打折扣，加大了图像处理的难度。所以，我们必须想办法通过软件方式排除或是减弱这些干扰因素，尽可能减少误判的发生。采集到的图像中，我们主要信息有：赛道两侧边界，赛道宽度，赛道类型等。

??由于今年芯片资源的受限，图像信息被严重压缩，信息量减小了非常多。尤其是在 MT9V032的上下左右各个角落处，桶形失真和梯形失真会有叠加，导致图像畸变更加严重。因此，在数据采集时选择只采集半幅图像，也就是中心上下处的图像，这样采集所得的图像几乎不存在筒形失真。
??通过摄像头采集到几种典型赛道图像如图 5-1～图 5-3所示

(5) 圆环：圆环的识别是直接对圆环的入口进行识别的，不过由于图像压缩，所以看到的圆环图像和往年有蛮大不同，容易出现上下边界连通域

??看不到圆环的情况，不过圆环的处理难度感觉没有十字麻烦。只要能找准拐点，就可以根据拐点获得更多的信息，多和其他元素进行对比，并修改算法，就不会发生误判。

??（7）车库：车库的识别并没有什么难度，识别起跑线即可，进入车库是用的补线的方式，因为车库是个直角弯道，弯道接车库和长直接车库轨迹不太类似，而且摩擦力不同轨迹也不同。如何顺滑的进入车库是有一定难度的，稍有不慎，就会蹭到路肩上。这就需要我们在控制方案上下点心思了。

??图 5-12边界提取流程图

5.2.4偏差量的计算

??采集到的图像缩小较严重，且远处信息不可靠，所以为了增强循迹鲁棒性，我们限制了偏差计算的高度。而且由于双倍路肩对我们图像的影响，斜率算法感觉不是很可靠，经常出现超调，尤其是十字圆环处，所以我们放弃使用斜率算法，使用较为稳定的偏差算法。

??获得偏差前需要先标定赛道边沿，将小车放在直道上，标定出标准的左右边线。通过取到的左线和左标准边线作差，将取到的右边线和右标准线作差，得出前方一定距离处的偏差量，将此用作后期控制等算法中的偏差量。

??另外一个需要关注的问题是，选用多少距离范围的数据用作偏差量。当处于直道上时，由于小车的速度较快，因此要选取较远处的偏差，以防止小车来不及转向；当处于弯道内时，由于摄像头的视野有限，应尽量使用能采集到的最远处的数据段。这样能在连续弯的切换上较为流畅，以及在回环弯上较为稳定。

5.2.5路径选择

??我们知道小车能否以最短的时间完成比赛，与小车的速度和路径都有着密切的关系，因此，如何使赛车以一个最合理、最高效的路径完成比赛是提高小车速度的关键。

??由于今年时间有限，我们把所有元素解决并调试稳定后，已经接近比赛了，但我们知道，全向行进组使用匀速是无法取得较好成绩的，所以对于小车路径的进行优化，不能仅在匀速下调节转弯系数，而应该将变速考虑进去。调节路径的方法是在慢速匀速情况下调出一套较为切内弯的 PD系数，慢慢的提高匀速的速度。这是由于在弯道内前瞻量是有限的，因此在提速的过程中，不可避免地会出现反应迟钝的情况，此时就应当停止调节转向参数。在此基础上获得这套转向参数的可用速度范围，并添加变速程序，即可达到稳定并且高速的效果。

5.3控制算法设计

??在电机运行和 mecanum轮的方向控制上，需要采用合适的控制方法。这里我们采用了经典的 PID控制算法，以及一些与其相关的改进方法，如不完全微分 PID和抗积分饱和 PID等，并取得了良好的效果。以下简要介绍 mecanum轮的运动学分析和几种 PID控制算法，以及特殊元素三岔路口的策略。

5.3.1 mecanum轮的运动学分析

??假设小车底盘在行进过程中一直与地面平行，可以将小车系统的运动分为 3个独立自由度上的运动，即平面上的向和向运动，和小车本身围绕竖直轴的纯转动。也即，在研究过程中，小车可以由 2个平面移动自由度和 1个转动自由度分别控制。 2个平面自由度控制小车的位置， 1个转动自由度控制小车的姿态。所以小车所有的运行状况都可以用这三种情况耦合而成。故以小车的几何中心为原点，长为轴，宽为轴，小车底盘向上的法向量作为 z轴，建立直角坐标系。计小车在方向上的速度为，在方向的速度为，围绕轴的转动的角速度记为。

??上式中，可以根据小车在三个自由度上的状态倒推出四个轮毂的速度。只要控制电机的速度，就可以让小车调整到目的状态。

5.3.2经典 PID控制算法介绍

??比例、积分、微分控制，简称 PID控制。 PID控制器问世至今已有近 70年历史，其有着结构简单、稳定性好、工作可靠、调整方便等优点，现已成为工业控制领域的主要技术之一。

??当不能完全掌握被控对象的结构和参数、或得不到精确的数学模型时，其它的控制理论技术难以被采用，必须依靠经验和现场调试来确定控制器的结构和参数，此时应用 PID控制技术是最为方便的。
??PID控制在实际中也有 PI和 PD控制。 PID控制器是一种线性的控制器，其根据给定值与实际输出值构成控制偏差。将偏差的比例 §、积分 (I)和微分 (D)通过线性组合构成控制量，对被控对象进行控制，故称为 PID控制器，其原理框图如图 4.7所示。

??运用上述公式，可以写出 PID控制算法。在智能车的制作过程中，由于增量式 PID相对于位置式 PID有明显的优势，所以我们选择增量式 PID作为速度控制算法，用编码器获取速度作为反馈量，实现闭环控制。

5.3.3不完全微分 PID控制算法介绍

??在经典 PID的基础上进行的改进的不完全微分 PID，其优势是对反复跳变的被控量有着更好的控制。智能车的方向控制过程中，图像处理后的车模位置偏差在反复跳变。综上考虑，使用增量式不完全微分 PID控制电机转速。不完全微分 PID控制控制框图为：

??我们同样使用了增量式的不完全微分 PID，这个 PID主要用来智能车的方向控制。

5.3.4串级 PID

??串级控制系统是改善控制质量的有效方法之一，在过程控制中得到了广泛的应用。所谓串级控制，就是采用两个控制器串联工作，外环控制器的输出作为内环控制器的设定值，由内环控制器的输出去操纵控制阀，从而对外环被控量具有更好的控制效果。这样的控制系统被称为串级系统。PID串级控制就是串级控制中的两个控制器均为 PID控制器。

??根据比赛任务的分析，我们一定程度上做好了线程分配，灵活地使用了信号量、邮箱等组件，我们的切身感受是，不仅这些组件和开发模式是有趣的，而且任务调度器在上下文环境的切换非常快速，代码也变得相对裸机的代码而言，更加地精简且易懂。此次具体应用中，我们简单运用了几个 RTT的内核组件，详情将介绍如下。

6.1线程创建

??RTT的应用中，最核心的部分是线程的创建。我们依次根据比赛任务，将图像及参数屏幕显示、蜂鸣器响应、按键检测这 3个任务分配到 4个线程中，且均使用动态线程创建，省去手动创建控制块的代码。

??其中图像及参数屏幕显示和蜂鸣器响应这两个线程的优先级一样，但时间片图像显示设置得更长，这里使用了时间片的轮转调度算法。 4个线程的创建代码及其优先级和时间片展示如下，具体线程入口函数具体代码将于附录展示。

6.2邮箱使用

??在 RTT的线程间通信机制中，我们使用了比消息队列机制更高效的邮箱（mbox）。

??在邮箱中，一封邮件的长度固定为 4字节，我们此次在蜂鸣器的线程中使用，具体的使用方式为：在图像处理中，如若识别到不同的赛道元素，我们将设定发送不同的时间变量给蜂鸣器邮箱，使得蜂鸣器在遇到不同赛道元素的时候的响应时长有别。

??邮箱创建可见图 6-3。

6.3信号量使用

??在此次 RTT使用中，我们最大的应用之一就是将图像采集部分用 RTT替换。我们创建了图像采集的信号量，与之对应的是在 DMA结束场中断的时候用 rt_sem_release(camera_sem);的语句来释放信号量，我们可以在主函数的主循环内用

??rt_sem_take(camera_sem, RT_WAITING_FOREVER);函数来检测信号量确保一次图像采集完毕，以便随后进行图像处理。

6.4软件定时器使用

??软件定时器基于滴答定时器，以系统节拍的长度为定时单位（可修改，通常为 1ms）。

??尽管理论上我们可以创建并使用无限个定时器，但我们在实际使用的时候，还是选择了使用一个定时器。因为我们双核方案需要考虑时钟同步问题，构建多个定时器会带了工作量和代码繁杂，可何况一个定时中断已经可以帮助我们满足比赛任务。

??软件定时器的创建如图 6-5所示，函数中最后一个参数即设置为周期性超时，这样这个软件定时器结束进入的回调函数就相当于一次定时中断的中断服务函数。

7.2上位机调试

??为了获取车模在行驶时的运行数据，我们在车上装了蓝牙，可以将关键参数实时发送到上位机查看，上位机我们使用山外的多功能调试助手和匿名科创地面站，它支持查看图像、虚拟示波器、串口助手等功能，使用方便。

??我们编写了多个上位机，为了更加方便对车模运行信息的查看和存储，主要用途在数据的采集，把电感值和图像各种信息从 SD卡里读出来。

§08 总结

??本文主要介绍了浙江工业大学浙工大鸿泉全向 SOLO队在准备比赛时的基本思路，包括了基于 RT-Thread操作系统使用，机械、电路、电池以及图像算法等创新思想。

??1. ：图像因 RAM资源限制而精度不高，我们完全改变了对十字、圆环、起跑线的识别和处理方法。为了配合单片机的性能，我们在精简代码的同时保证了大程度上的识别稳定。

??2. ：我们都知道车速上限就是机械结构。由于我们车模很重，舵机负荷大，为了解决惯性大和转向打滑的问题，我们花了不少时间调整板子布局，同时对整车的重量和重心都进行了调整，整车的重量太轻太重都会影响车模的运行，而车模重心则容易影响整体车模的转向性能。

??3. ：以模块形式分类，对最小系统、主板、电机驱动、电池保护等模块分别设计，经过不断实验和改进，最后决定了最终的电路图。

??4. ：我们选用 MounRiver Studio作为开发工具调试程序，使用 C语言进行编程。经过长时间的编写修改程序和调试参数，最终实现了小车的直道加速,入弯减速，出弯加速。并能够在一定时间内完成比赛。