智能车竞赛技术报告 | 全向行进组 - 东北林业大学- 进取号E_麦轮陀螺仪pid

 

第一章 引言

---

  全国大学生智能汽车竞赛是以智能汽车为研究对象的创意性科技竞赛,是面向全国大学生的一种具有探索性工程实践活动，是教育部倡导的大学生科技竞赛之一。竞赛以“立足培养,重在参与,鼓励探索,追求卓越”为指导思想,旨在促进高等学校素质教育，培养大学生的综合知识运用能力、基本工程实践能力和创新意识，激发大学生从事科学研究与探索的兴趣和潜能,倡导理论联系实际、求真务实的学风和团队协作的人文精神，为优秀人才的脱颖而出创造条件。

  竞赛以竞速赛为基本竞赛形式，辅助以创意赛和技术方案赛等多种形式。竞速赛以统一规范的标准硬软件为技术平台，制作一部能够自主识别道路的模型汽车，按照规定路线行进，并符合预先公布的其他规则，以完成时间最短者为优胜。创意赛是在统一限定的基础平台上，充分发挥参赛队伍想象力，以创意任务为目标，完成研制作品；竞赛评判由专家组、现场观众等综合评定。技术方案赛是以学术为基准，通过现场方案交流、专家质疑评判以及现场参赛队员和专家投票等互动形式，针对参赛队伍的优秀技术方案进行评选,其目标是提高参赛队员创新能力，鼓励队员之间相互学习交流。

  竞赛过程包括理论设计、实际制作、整车调试、现场比赛等环节，要求学生组成团队，协同工作，初步体会一个工程性的研究开发项目从设计到实现的全过程。竞赛融科学性、趣味性和观赏性为一体，是以迅猛发展、前景广阔的汽车电子为背景，涵盖自动控制、模式识别、传感技术、电子、电气、计算机、机械与汽车等多学科专业的创意性比赛。本竞赛规则透明，评价标准客观，坚持公开、公平、公正的原则，保证竞赛向健康、普及、持续的方向发展。

  全国大学生智能汽车竞组织运行模式贯彻“政府倡导、专家主办、学生主体、社会参与”的 16 字方针，充分调动各方面参与的积极性。

 

第二章 系统设计总述

---

  今年限定了H车模作为全向型进组竞赛车模.麦克纳姆轮基于全向移动的特点,配合舵机与摄像头,可以轻松实现在三岔道路横向前进的任务.基于本次芯片采用沁恒推出的CH32V103的引脚数及性能限制.我们采用硬件二值化摄像头OV7725,有效减轻单核的数据处理负担.为了识别元素更为准确,我们又采用四个高精度电感获取更多赛道信息.基于全向移动的特点,车体采取对称式结构方便控制.

 

第三章 机械系统设计

---

3.1 防撞

  在调试过程中,经常会遇到车模冲出赛道的情况,为了防止撞击损坏电路板和传感器,使用质地较硬的漆包线与高硬度碳杆制作电磁前瞻.利用多层热缩管和热熔胶将电感固定在前瞻上.既保证了轻质,又保证了强度.经过测试,最终使用外径4.0*4.0mm,内径1.0mm的碳杆.

▲ 图3-1 电磁前瞻

3.2 电池固定

  由于车模运动速度较高,并且惯性大,所以电池的固定必须牢固,以防在运动过程中电池发生运动,影响参数的校正.并且由于比赛时间紧张,电池还要方便更换.车模采用专用打印件将电池固定在车模X轴中心.

▲ 图3-2-1 电池固定槽(左)(右)

▲ 图3-2-2 实际装配效果

3.3摄像头固定

  由于车模采用摄像头循迹,并需要在三岔元素内进行转向,摄像头与舵机的配合需要一定强度,在保证摄像头在车模运行过程中不会抖动的情况下,又能实现转动.采用了附加支架和轴承将摄像头固定.为了能使摄像头不随碳杆与舵盘发生相对滑动,采用了8.08.023mm碳杆.使用轴承为内径12mm,外径20mm的塑料材质轴承.为了使方杆与轴承内径有接触,使用黑色电工胶带将碳杆加厚.

▲ 图3-3-1 轴承与碳杆

  使用过程中发现附加支架极易拉扯车模底盘,因此将原本采用的机械结构用热熔胶代替.

▲ 图3-3-2 原有的附加支架

▲ 图3-3-3 改用热熔胶固定

  为了获得更好图像，我们增加了连接装置.

▲ 图3-3-4 增加装置后

3.4 舵机固定

  舵机质量尽量小,有利于调整车模重心.为了使摄像头角度在运行过程中可以转动共180度,舵机可控角度应为180度及以上.舵机虚位尽量小,可以尽量减小其抖动对摄像头识别造成的影响.摄像头转角速度也要尽量快.最终我们选用了FT2331M这款舵机.可控角度为240度,体积小,重量20g,转角速度0.09s/60°(6v).

▲ 图3-4-1 FT2331M舵机

  对于多舵机的安装,重点是做好定位孔,要使得舵机齿的位置处于车模中心.基于载板上预定打好的孔,用强度稍高的铝管可以达到很好的固定效果,纸垫片可以有效保护载板版面,也可以对载板和铝管进行绝缘.

▲ 图3-4-2 舵机支架

▲ 图 3-4-3 装配效果

  使用打印件连接舵盘和碳杆

▲ 图3-4-4 连接件

▲ 图3-4-5连接件装配效果

3.5 测距传感器固定

  车模采用tf-mini PLUS激光雷达传感器,用于配合处理坡道元素,分辨率为cm,串口通讯.但可惜受制于处理器,最终未能使用.利用车模上定位孔将其固定在摄像头正下方,调整角度不被电磁前瞻遮挡.

▲ 图3-5 测距传感器 tf-mini

3.6 编码器固定

  编码器的安装要考虑到齿轮的啮合问题，不能过松或过紧。并且编码器要尽 量安装在靠近车中心的位置，防止在碰撞时编码器的位置发生变化。编码器采用 螺丝和塑料固定件固定在车轮上方。为了防止编码器齿轮松动，直接采用 262 胶水将齿轮固定在编码器轴上。编码器配合齿轮容易滑丝,挡板质地坚硬,不易损坏,因此用于固定编码器的挡板,其定位孔设计为槽型,可以用调整挡板的形式调整编码器齿轮的啮合.

▲ 图3-6-1编码器安装

▲ 图3-6-2 编码器安装

▲ 图3-6-3 编码器安装架

3.7陀螺仪固定

  为了保证车体前后对称,且不占用舵机安装空间,将陀螺仪用锡焊固定在主板上,舵机的正下方.X轴Y轴方向与程序有紧密联系.这里选取X轴正向正对车模前方.

▲ 图3-7-1 陀螺仪

▲ 图3-7 实际安装效果

3.8 麦克纳姆轮处理

  本届为了使H车在pvc材质赛道上跑得更稳,没有使用往届的买麦克纳姆轮,而是采用了16届新版的”钻石轮”,提升车体防抖性能;小辊子上胶皮易与其发生相对滑动,胶皮的两侧也容易与轮毂边沿卡住导致横向前进无法稳定,选用厚度为9448A双面胶将胶皮粘在小辊子上.

▲ 图3-8麦克纳姆轮

3.9 PCB板固定

  本车模PCB板为矩形,长100mm,宽80mm.板子上打有舵机安装定位孔,陀螺仪安装定位孔.为了保证陀螺仪确实在车体正中心,同时不影响走线,使用了与车模定位孔相匹配的打印件.主板与打印件之间使用热熔胶连接.打印机件与车模底板间使用21.0mm的高强度铝管连接.

▲ 图3-9-1 打印件

▲ 图3-9-2装配效果(正面)

▲ 图3-9-3装配效果(反面)

  对于电机驱动板,由于不安装传感器,使用45mm尼龙柱*2即可固定,底部为防止对车底盘变形造型影响,使用热熔胶.

▲ 图3-9-4 驱动板

3.10 重心调整

  H车模依靠四个电机差速实现姿态矫正,重心不当容易造成控制逻辑混乱;重心靠上容易使车模跳轮.最终将重达220g的电池以及30g重的主板放置在车模中心位置,且电池紧贴车模底盘,主板与电池间距为3.0mm,选取舵机时特别选取了重量仅为20g的FT2331M舵机,保证了除麦轮外的重量集中在车体中心靠下.

▲ 图3-9-5 整体装配效果

3.11 机械结构选择与踩坑

  在正常的机械结构下，如果漂移甩尾，或是速度不高且提不上速度，不要迷信轮胎处理或是结构微调这些，这时候影响到你的大概率是程序和算法。轮胎处理和结构微调只会在高速时有些许的改善，陷入天天有问题就想调机械也是我们最开始做的问题。

  轮胎处理

• 其实我们认为最重要的是，把辊子上的胶套用3M薄双面胶粘死在了轮子中间。未作处理的话横向跑车的时候胶套会偏向轮毂一侧，影响辊子正常转动。虽然但是，闭环了确实会在一定程度上不会出现太大的问题。

• 我们也尝试了用软化剂涂抹胶套，具体的方法就是用刷子把每个辊子上的胶套都刷上软化剂，再用保鲜膜包起来密封八小时左右，一半就是偶尔在晚上调完车后处理第二天早上再拆封这样。如果不嫌拆装麻烦可以拆掉所有辊子上的胶套并浸泡在软化剂里，或是有充足资金买软化剂就直接把麦轮拿去泡。软化剂我们使用的是逐飞店铺的黄色小瓶，呆萌侠的也尝试过。不过因为胶套太薄面积太小，处理完之后没有明显的手感变化，上车测试也是一样。

  呆萌侠的厚胶套我们买过，感觉就是把长橡胶管截成一个个小段，并且是上下等宽等粗，感觉很不靠谱的样子。并且后面卓大公众号评论下面回复这种胶套厚度可能超标，为了安全起见我们就放弃用了，那个其实连装车都嫌累没装过

• 打磨轮胎的话，可以用砂轮机打磨，也可以找个粗螺丝放到胶套里固定，再卡到台式钻台上，手拿着砂纸在旁边磨。硅橡胶是为了防止打磨将轮胎磨破，可使用南大硅橡胶进行补胎，硅橡胶更软但摩擦力不如原胎。但是这么多辊子磨完会是个巨大的工程，看着就很累最终也没做处理。另外原装胶套本身就很薄，稍不留神就容易磨破。我们尝试处理了一个手感上也没有明显变化。其实跑到了最后胶套已经被自然磨损了一层，边缘甚至还会破损。破损只要不是特别严重我们也就没修，严重的就直接换掉了。

• 我们发现辊子和轮毂连接处在跑了很长一段时间后会出现黑色的东西，有些粘性但也不少油状的，猜测可能是胶套的碎屑卡在了慢慢积累到一起。这个黑色的东西会让辊子自转稍微受阻。这是我们在拆轮时才发现的，不拆轮单从外面看没有发现，拆之前并未察觉到异样，拆后也只有仔细观察才能发现提升之处。

  其他

• 卸下来车模前挡板，因为挡板会卡路肩，并且要撞也是先撞掉放在前面的电磁感.
• 轮胎以及赛道的干净很重要。建议擦轮胎使用微湿的抹布，擦赛道使用较干的抹布进行，擦拭完毕等二者都干掉再发车。也有推荐购买WD-40除锈剂来清洁轮胎。如果隔夜代码跑不了，请先不要怀疑自己，擦一遍赛道和轮子会有意外收获的。

 

第四章 硬件系统设计

---

4.1 传感器

  选择的传感器如下:

• 编码器: 360°位置传感器倒立摆角度绝对式迷你mini编码器.每个电机配备一个角度传感器来测速,共四个.使用SPI通讯接口,CPOL=0/CPHA=0通讯方式.通过测量角度变化读取电机速度.
• 摄像头: OV7725小钻风硬件二值化数字摄像头.可调帧率0-150,通过软件调节阈值,使图像更好地与环境相适应.相较于彩色摄像头,减轻单片机运算负担.镜头130°,对于调试无任何影响.
• 激光测距传感器: 北醒激光雷达传感器PIX避障室内版TF-mini-Plus.串口通信, 测距范围10-120cm,精度毫米级.通过850nm激光测量坡道距接收镜头距离,输出赛道起伏状况.配合摄像头准确输出赛道信息.
• 陀螺仪: ICM-20602传感器模块.SPI通讯,测量精度高,噪声小.负责圆环内及三岔内角度收集.

4.2 电源部分

  电源部分设计本着稳定,低成本,简单的原则,共设计五个电源部分:

• 作用 电源 使用芯片
• 舵机驱动 6V TPS5430
• 摄像头驱动 3V3_CAM LD29150-33
• 运放驱动 3V3_OP LD29150-33
• 测距驱动 5V LD29150-50
• 其他外设 3V3_1 LD29150-33

  为避免摄像头和电磁电源互相影响,特地使用两个电源进行供电,在设计中后考虑到陀螺仪使用较为频繁,应将陀螺仪也单独进行供电.可能这也是赛中,陀螺仪掉线车模于三岔口偶尔无法收到数据的原因.因此建议所有传感器单独供电.
  对于舵机电源,除负责给舵机供电外,还需要利用它的输出作为其他电源芯片的输入. TPS5430经典电路如下:

▲ 图 4-2-1 6V电源电路

  其中所有3V3电源皆使用LD29150-50对6V电源进行转压,实测该芯片稳压误差小于1%.5V电源用于测距模块使用.在转出时应使用0欧磁珠进行电源分割.

▲ 图4-2-2 5V电源\3V3电源

4.3 人机交互

  为方便观察摄像头获取信息是否正确,设定了SPI通讯的屏幕接口:

▲ 图4-3-1 屏幕接口

  蜂鸣器可以方便地判断元素是否识别到:

▲ 图4-3-2 蜂鸣器

  为方便设定速度档位,设定了按键,单核处理器没有多余的IO口,因此五轴按键只用了三个引脚:

▲ 图4-3-3 按键

4.4 通信接口

  共用到了三种通讯接口,分别是串口,SPI,I\O.如下表所示.

• 接口种类 使用外设
• SPI 陀螺仪,编码器(角度传感器),屏幕
• UART 摄像头,测距,无线串口
• I\O 电机驱动方向信号,按键,蜂鸣器,舵机

  为了避免接触不良,布局设计中应遵照越短越好的原则.其中陀螺仪用锡焊直接焊在载板信号接口处.编码器,电机驱动信号用12pin牛角头线连接,摄像头采用18pin FPC排线进行连接,测距采用xh1.25带锁扣排座连接, 避免接反.无线串口和屏幕采用排针排母直连.

4.5电机驱动

  电机驱动板由两个全桥电路构成,每个全桥电路由4个N-MOS组成.为节约PWM资源,采用Drv8701e芯片,一路IO,一路PWM驱动信号层:

▲ 图4-5-1 H桥驱动电路

  电机和单片机之间用SN74HC125PWR芯片进行隔离:

▲ 图 4-5-2 电机-单片机隔离电路

  驱动芯片和隔离芯片用LD29150-50和LD29150-33电路进行供电,为了节约成本及购买便捷性,采用LD29150同一封装的不同规格进行电源传递:

▲ 图4-5-3 芯片电源

  单片机提供过控制输出PWM占空比实现电机调速,通过改变与驱动板的IO接口高低电平,控制DRV8701e的使能端和输入端信号,进而控制电机正反转和停止.车模使用电机为RS-380电机,堵转以及正反转过渡过程中电机电流较大,最大堵转电流约为20A.因此,设计驱动板时对过电流较大导线进行覆铜处理,MOS管使用TPH1R403NL,足够承受10V以下电压和大电流.

 

第五章 摄像头处理

---

5.1简介

  我们使用的单片机存在较多限制，为了使得能提高处理速度，减少单片机负荷，压缩代码量，我们最终决定使用硬件二值化的摄像头和130°无畸变镜头来采集图像，得到原始图像。

5.2摄像头信息的提取和利用

• 原始信息

  使用硬件二值化摄像头解压缩后得到一幅只有黑和白的图像，实质上是60*80的一个二维数组。

• 二次信息提取

  在获取到原始图像的基础上，根据赛道特点，根据赛道边线是黑白分界线的特征来提取边线。

  在扫线上，传统的获取方法是在底部往上扫线，根据上一行中点来确定本次中点起始位置的算法存在遍历点次数过多，速度过慢的缺点。因此我们使用八邻域边缘提取算法，分别对左边线和右边线进行边缘提取，八邻域扫线特性和赛道特性契合，处理速度较快，同时利用八邻域扫线过程中，相邻两个像素点位置，可以大致得出赛道趋势，可以在后续判断中使用。获取的赛道具有几个特点：每一行直接具有连续性，每帧图像和上一帧图像边线形态上具有一定的关联性。可以根据实际摄像头角度和光照情况，根据本次图像上边线特点和像素点信息，对光照对原始图像是否严重进行验证，如果不满足条件就不处理本帧图像，采取下一帧图像进行处理。

  我们通过扫线得到了左右边线在一帧图像上的位置，通过对同一行所在列数进行相减，可以得到每一行的赛道宽度。在扫线完成后，根据赛道边线形态和八邻域得出的赛道趋势可以确定赛道上一个很重要的信息，该帧图像近处拐点是否存在和拐点所在坐标。至此，每帧赛道基础信息获取完毕

▲ 图5-1-1 正常赛道获取到的图像

• 二次信息的利用

  特殊元素判断 :

  本届智能车赛道需要处理的元素有：斑马线，十字路口，三叉，环岛，坡道。
  摄像头获取的图像和车身相对于元素的位置有较大关系。

  其中斑马线较为好判断，在左右两边线间存在大量黑白跳变点。

  坡道使用摄像头结合激光测距模块进行测量。

  三叉识别通过拐点和赛道趋势进行判断，它的特征比较明显，同时找到下方两个拐点，赛道趋势满足特定趋势，赛道宽度逐渐变
  宽即可识别。

  环岛使用摄像头和电磁的综合判断，根据场地上电磁线铺设情况，环岛处电磁值是是整个赛场上最大的，使用横向于车体的电感识别。之后用摄像头进行左右环岛识别和特殊补线处理。通过赛道信息更改赛道边线，从而更改摄像头规划出的路径。

• 十字，三叉特殊处理

  全向组比赛要求需要在三叉中心旋转，需要识别到以后直行到三叉中心点，而直行过程中如果正常循迹路径规划会出现三叉走向一侧的问题。

  因为十字和三叉近处特征相似，因此可以使用同一种补线思路来对边线进行重新设定使得路径保持直行。减少了代码量，增强了复用性

  重新设定的思路很多，我们最终选取使用根据近处两侧拐点下边线情况来进行重新设定。重新设定分为左侧单边设定和右侧单边设定两种并列情况，可以适应直入十字，斜入十字，直入三叉，斜入三叉四种情况。

• 中线拟合

  我们选用赛道左右两侧边界来拟合虚拟中线，对单边丢线情况进行特殊处理，算特定行数的虚拟中线和屏幕中线的误差作为转向环输入。

▲ 图5-1-2 特殊情况丢线

 

第六章 PID控制实现

---

6.1 经典PID控制：

  • 比例、积分、微分控制，简称PID控制，又称PID调节。PID控制的基本方法是根据系统输入与预定输出的偏差的大小运用比例、积分、微分计算出一个控制量，将这个控制量输入系统，获得输出量，通过反馈回路再次检测该输出量的偏差，循环上述过程，以使输出达到预定值。 PID控制器问世至今已有近 70年历史，其有着结构简单、稳定性好、工作可靠、调整方便等优点。
  当被控对象的结构和参数不能完全掌握，或得不到精确的数学模型时，控制理论的其它技术难以采用时，系统控制器的结构和参数必须依靠经验和现场调试来确定，这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象，或不能通过有效的测量手段来获得系统参数时，也适合用PID控制技术。

  • 连续型PID的公式：
  即对err(t)分别进行比例，积分，微分运算之后进行加权求和得到输出U(t)。
  • 由于单片机控制是采样控制，只能根据采样时刻的偏差控制输出量而不能计算连续输出量，所以要采用离散化的 PID算法。离散数字 PID分为位置式和增量式两种。

  而增量式PID算法对应可由位置式推出：

  具体的代码实现为：

float Realize_PID(PIDTypeDef_t *pid, float get, float set) {
pid->Now_Val = get;
pid->Ex_Val = set;
pid->Err_Now = pid->Ex_Val - pid->Now_Val;

   pid->Output_p = pid->Kp * pid->Err_Now;
   pid->Output_i += pid->Ki * pid->Err_Now;
   pid->Output_d = pid->Kd * (pid->Err_Now - pid->Err_Last);
   pid->Output = pid->Output_p + pid->Output_i + pid->Output_d;

   pid->Err_Last = pid->Err_Now;
   
   return pid->Output;
}

float Incremental_PID(PIDTypeDef_t *pid, float get, float set) {
pid->Now_Val = get;
pid->Ex_Val = set;
pid->Err_Now = pid->Ex_Val - pid->Now_Val;

   pid->Output_p = pid->Kp * (pid->Err_Now - pid->Err_Last);
   pid->Output_i = pid->Ki * pid->Err_Now;
   pid->Output_d = pid->Kd * (pid->Err_Now - 2.000f * pid->Err_Last + pid->Err_LastLast);
   pid->Output += pid->Output_p + pid->Output_i + pid->Output_d;

   pid->Err_Last = pid->Err_Now;
   pid->Err_LastLast = pid->Err_Last;
   
   return pid->Output;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

  • 公式中没有体现周期T，其实考虑到周期是一个常数，就直接合在了系数中。采样周期和控制周期都会影响系数。

  • 在PID算法中，比例环节 P 的作用是成比例地反映控制系统的偏差信号e(t)，一旦产生偏差，比例控制环节立即产生控制作用以减小偏差；积分环节 I 的作用是消除静差，提高系统的无差度。微分环节 D 的作用是反映偏差信号的变化趋势，能够在偏差信号变化之前先引入一个有效的早期修正信号来提前修正偏差，加快系统的动作速度，减少调节时间。

  • 所谓增量式PID 是指数字控制器的输出只是控制量的增量△u(k)。增量式PID优点在于，由于输出的是增量所以误动作时影响小，必要时可用逻辑判断的方法关掉。另外由于控制增量△u(k)的确定仅与最近k次的采样值有关，所以较容易通过加权处理而获得比较好的控制效果。

  • 在PID参数的选择上，公认的常规方案是，过程控制系统（如电机控制）因较慢的响应常采用PI控制，突出积分项的作用，而随动控制系统（如舵机控制）为了达到较快的响应常采用PD控制。
  大多智能车队会推荐速度环使用增量式PID，方向环使用位置式PID，这是由于增量PID由于也不容易造成电机的正反转切换。但我们认为，在合适的参数下，增量式PID与位置式PID应该是可以等价。

6.2 改进式PID：

  • 不完全微分PID：
  微分项的控制可以加快动态响应的同时，也可能带来极大的高频扰动，因而使用微分项常常采用串接低通滤波的方式，以克服高频干扰，抗微分扰动。

  将一低通滤波环节串接在PID控制器的输出后，或单独串接在微分项的输出之后。
  pid->Output = pid->Output * Lpf1Factor_Out + pid->Output_Last * (1.000f - Lpf1Factor_Out);
  pid->Output_d = pid->Kd * (pid->Err_Now - pid->Err_Last) * Lpf1Factor_d + pid->Output_dd * (1.000f - Lpf1Factor_d);

  • 微分先行PID：
  o 输出微分先行PID：将低通滤波串联在反馈环节（如编码器环节）
  pid->Now_Val = get * Lpf1Factor_val + pid->Last_Val * (1.000f - Lpf1Factor_val);
  o 偏差微分先行PID：将低通滤波串联在偏差环节
  pid->Err_Now = (pid->Ex_Val - pid->Now_Val) * Lpf1Factor_err + pid->Err_Last * (1.000f - Lpf1Factor_err);

  • 变积分PID：
  o 死区：
  死区即实际工程中可容忍的最大误差。实际应用中，小误差产生的原因很大可能是由于测量误差或其它不可避免因素造成而不是实际误差。在较小的偏差下弱调节或不调节，可以让控制避免抖动更加平滑。
  o 三段式积分分离：
  在普通PID控制中引入积分环节主要是为了消除净差和提高控制精度。但在控制系统启动或停车或大幅度增减目标值时，由于积分饱和作用，短时间内会造成系统有特别大的偏差，会造成PID运算的积分积累，致使输出量特别大，很容易导致超调，甚至会引起系统较大的震荡。
  当误差绝对值大于阈值时取消积分项避免过量超调，在大误差范围内减小积分项作用。当被控量接近目标值时（即误差正常时），引入积分控制，以消除净差，提高控制精度。

if (pid->Err_ABS > 50) {
   ki_index = 0.000f;
} else if(pid->Err_ABS > 30 & pid->Err_ABS < 50) {
ki_index = 0.600f;
} else if(pid->Err_ABS > 25 & pid->Err_ABS < 30) {
   ki_index = 0.900f;
} else {
   ki_index = 1.000f;
}

pid->Output = pid->Output_p + pid->Output_i * ki_index + pid->Output_d;
1
2
3
4
5
6
7
8
9
10
11

  积分分离也会导致启动时或是状态变化时PID上升时间边长，若要运用则应多次实验并谨慎来选择范围和系数。若阈值太大，达不到积分分离的目的，若太小又有可能因被控量无法跳出积分分离区，只进行PD控制，将会出现残差。
  o 变速积分：

  积分分离每一段的ki_index值都为常数，而变速积分则采用系数的形式，使得积分的分段更加线性化。

if (pid->Err_ABS > L_Err) {
   ki_index = 0.000f;
} else if (pid->Err_ABS < S_Err) {
   ki_index = 1.000f;
} else {
   ki_index = (L_Err - pid->Err_ABS) / (L_Err - S_Err);
}
1
2
3
4
5
6
7

  变速积分控制更为缓慢，实际应用体验还是积分分离效果更好。
  o 有效偏差法：
  当根据PID位置算法算出的控制量超出限制范围时，控制量实际上只能取边际值U=Umax,或U=Umin,有效偏差法是将相应的这一控制量的偏差值作为有效偏差值计入积分累计而不是将实际的偏差计入积分累计。
  o 遇限削弱积分：
  在计算U(k)时，若上一时刻的控制输出量U(k-1)超过限制范围，则根据偏差的符号决定是否将相应偏差计入积分项。

if (ABS(pid->Output_Last) > OutputLimit) {
if ((pid->Err_Sum > 0 && pid->Err_Now < 0) || (pid->Err_Sum < 0 && pid->Err_Now > 0)) {
       pid->Err_Sum += pid->Err_Now;
  }
} else {
   pid->Err_Sum += pid->Err_Now;
}
1
2
3
4
5
6
7

  o 实际工程中，如切换PID模式，则建议消除积分项以避免积分饱和。
  • 专家PID：
  专家PID依赖经验与规则的制定，同样也依赖调参。专家PID使用时多用于电机环。

if (pid->Err_ABS < S_Err) {
   //小偏差 较小误差, 调小比例
   pid->Output = Offine_K * pid->Output;
} else if (pid->Err_ABS > L_Err) {
//大偏差
   //串联BangBang-PID
pid->Output = (pid->Err_Now > 0) ? (9500) : (-9500);
} else {
//正常偏差
if ((pid->Err_Now * (pid->Err_Now - pid->Err_Last) > 0 && pid->Err_Last * (pid->Err_Last - pid->Err_LastLast) > 0) || (pid->Err_Now - pid->Err_Last) == 0) {
       //偏差在朝向偏差绝对值增大的方向变化(偏差越来越大), 或者偏差一直为某一固定值
       if (pid->Err_ABS > Mid_Err) {
           //控制器实施较强的控制作用
           pid->Output = Online_K * pid->Output;
      } else {
      //但是偏差绝对值本身并不是很大
           pid->Output = pid->Output + 0;
      }
} else if(pid->Err_Now * (pid->Err_Now - pid->Err_Last) < 0 && (pid->Err_Now - pid->Err_Last) * (pid->Err_Last - pid->Err_LastLast) > 0 || (pid->Err_Now == 0 && pid->Err_Last == 0) {
       //偏差的绝对值向减小的方向变化，或者已经达到平衡状态
       //此时可以保持控制器输出不变
} else if(pid->Err_Now * (pid->Err_Now - pid->Err_Last) < 0 && ((pid->Err_Now - pid->Err_Last) * (pid->Err_Last - pid->Err_LastLast) < 0)) {
       //偏差处于极值极限状态
       if (pid->Err_ABS > Mid_Err) {
           pid->Output = Online_K * pid->Output;
      } else {
           //但是偏差绝对值本身并不是很大
           pid->Output = pid->Output + 0;
      }
  } else {
       //正常情况
       pid->Output = pid->Output + 0;
  }
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34

  这是一套较为完整的专家PID，根据实际使用时的情况可以随意删减规则，如为了实际应用时减小静差则不使用小偏差弱调节，为了更为稳定不串联BangBang-PID等，较为依赖经验。

 

第七章 图像循迹方案

---

  摄像头程序处理后会给出一条中线，使用计算中线与标准值误差，赋予转向环PID。

• 三行巡线：

  类似于线性CCD的控制方案，是最早期使用的方法。不过使用的有效信息太少，中线一旦跳变整个车就会抖动，车速到了一米一左右尤为明显。
  aim_line取决于速度和有效行，速度越快aim_line预瞄距离越远，同时限幅在有效行之下。
  AngleErr = ((5 * middle_line[aim_line] +
  3 * middle_line[aim_line + 1] +
  2 * middle_line[aim_line + 2]) / (10.000f)) - middle_standard;

• 加权计算全图像：

  使用了整个图像，对于图像不同区域（行数）给与不同权重。为了防止跳变取近三次输出值加权计算误差。
  对于不同元素可以赋予不同权重，如坡道weight3权重整体关注区域更偏向上方，环岛weight2权重关注区域为补线区域，三叉内低速横向循迹weight4权重关注区域更偏下方。

  start_row 为补线起始行，valid_row为有效行最顶行，作用相当于限幅。

uint8   weight1[60] = { //0为图像最顶行
        0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
        1, 1, 3, 3, 5, 5,10,10,10,10,
       10,10,10,10,10,10,10,10,10,10,
       10,10,10,10,10,10,10,10,10,10,
       10,10,10,10,10, 5, 5, 5, 5, 5,
        3, 3, 3, 1, 1, 1, 0, 0, 0, 0,};    //基础   //注意斜率变化引起的跳变,要平滑

for(i = start_row; i > valid_row + 1; i --) {
   weight_sum += weight1[i];
   line_sum += weight1[i] * middle_line[i];
}   //从下往上扫

midlinenow  = (float)line_sum / weight_sum - middle_standard;
midline_fff = midline_ff;
midline_ff  = midline_f;
midline_f   = midlinenow;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17

  AngleErr = midline_fff * 0.50f + midline_ff * 0.30f + midline_f * 0.20f;
  在这些的基础上增加了最小二乘法对摄像头中线的再拟合，作用也是为了更为了使循迹平滑。
  Slope = Regression_cal(valid_row + 3, start_row);
  AngleErr = AngleErr * 0.80f + Slope * 0.20f;

• 用部分图像计算偏差：

memset(Part_Err, 0, sizeof(Part_Err));
Improper_Row_Count = 0;
memset(Improper_Row, 0, sizeof(Improper_Row));

if (Start_Part > start_row) Start_Part = start_row - 2; //起始行限幅
End_Part = Start_Part - PartUsed;
if (End_Part < valid_row) End_Part = valid_row + 2; //截至行限幅

for (i = Start_Part; i > End_Part; i--) { //从下往上扫
   Part_Err[Start_Part - i] = (float)(middle_line[i] - middle_standard) * 100.0f / ((float)img_real_width[i] / 2.0f);
   Part_Ave += middle_line[i];
//       if (i == End_Part - 2) {
//           Part_Err[Start_Part - i] = Part_Err[Start_Part - i] * 1.20f;
//       }   //放大截至行误差
} //相当于逆透视
//PartUsed 相当于前瞻, 10行适合低速, 高速可以取15, 区域越小越灵敏

Part_Ave = Part_Ave / Part_Used; //计算范围内中线平均值
for (i = Start_Part; i > End_Part; i--) {
   if (ABS(middle_line[i] - Part_Ave) > 10) {
       Improper_Row[Start_Part - i] = middle_line[i];
       Part_Used--;
       Improper_Row_Count++;
  }
}   //记录偏离中线均值10以上的点 滤掉跳变点

for (i = 0; i < (Part_Used / 5); i++) {
   Part_Err[i] = 0.000f;
}   //最底下1/5区域置零, 为了平滑

for (i = 0; i < Part_Used; i++) {
   if (Improper_Row[i] == 0) {
       Part_Err_Sum += Part_Err[i];
  }
}   //累加非跳变点

AngleErr  = Part_Err_Sum * Err_K / (float)Part_Used;
AngleErr2 = AngleErr1;
AngleErr1 = AngleErr0;
AngleErr0 = AngleErr;
AngleErr = AngleErr0 * 0.70f + AngleErr1 * 0.20f + AngleErr2 * 0.10f;
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41

• 弯道控制：

  弯道的速度可以由公式 = （F是摩擦力），这样V= ,其中 是摩擦系数，由地面和轮胎决定，R是转弯半径。由于地面和轮胎在过弯时是给定的，这样在比赛中我们为了保证V大，只能保证更大的转弯半径。R越大，速度V就越大。所以稳定沿着电磁线循迹并不是最优解，最好是采用外内外切弯。即入弯时贴弯道的内弯，出弯时贴外弯。这种情况下赛车通过整个弯道过程中行车线半径是固定的，即定曲率行车线。

  实践中发现通过调整纯跟踪算法的预瞄距离，可以有效提高路径规划的最优性。对于全向车也是一样，弯道的控制方案最好为，入弯减速避免打滑，出弯加速节约时间。

 

第八章 元素处理方案

---

环岛：

  • 我们对于环岛的识别使用的是稳定的电磁判断方案。内侧两个横向电感会在环岛切点处达到极值，以此来判断环岛。
  • 入环时使用摄像头补线，并开始计算陀螺仪z轴积分。陀螺仪大于60度入环补线结束，陀螺仪大于200度开始摄像头识别出环，若出环判断失败陀螺仪大于250度强制开始出环，陀螺仪大于355度出环完成。
  • 当陀螺仪大于100度小于225度时，累加环内转向环PID输出量，记录环内转角输出，目的是用环内计算环岛内平均打角。当出环预备标志位置1后，将计算值强制赋给转向环输出，实现出环使用稳定的环内姿态。出环不仅可以使用环内状态也可使用入环状态，我们测试发现使用环内姿态更为稳定。
  • 电感判断环岛的问题在于出环后会重复判断，所以需要写一个出环后短时间0.8s内不再识别第二个环岛。
  • 另外判断小环岛和大环岛 可以记录车身0-60度时间或是编码器脉冲数。
  • 对于环内巡线，由于我们是30cm摄像头限高，所以使用正常巡线也能有不错的效果。但使用正常的摄像头中值会偏向一侧，可以在左右环岛分别赋予不同的摄像头中值。
  • 环内属于稳定而又简单的元素，入环并判断车身稳定后可以环内加速。如果使用记录环内姿态用于出环，则出环速度需要与环内速度相同，相当于在环岛元素内只在入环减速其他时刻都在加速。

坡道：

  • 我们使用TFMiniPlus激光雷达测距来判断坡道。当距离降到一定阈值下即可判断坡道。
  • 我们将TFMiniPlus竖直倾斜安装，此时测距点在赛道上。水平安装同样可以，不过水平安装测距点不稳定，赛道铺设场地有限的情况下可能会与赛道挡板误判。
  • 在识别到坡道后开始计步（Vy轴向编码器数值累加），路程判断在上坡进行到三分之一稳定后时开启加速并在下坡时关闭加速。路程经过上坡三分之一后对Vx和w两个自由度的每次输出进行限幅防止过量打角抖动，上坡前三分之一不限幅是为了让车身修正姿态。
  • 判断坡道结束可以仅使用计步，当距离超过测定阈值后结束坡道。也可以当测距再次出现极小值时判断坡道结束。为了防止下坡时二次识别，可以加上下坡后0.5s内不再识别第二个坡道。另外还可以用走过一定距离后识别正常赛道结束坡道，判断条件为计步超过小阈值并且测距恢复为正常距离值一定次数后即可判断结束。

直道加速：

  全向组在赛道上匀速循迹很大可能不会有一个较高的速度，所以直道加速就显得很有必要。我们使用了五种直道加速的判断条件。
  • 中线判断加速：
  判断图像上半部分中线与赛道两边界无交点即可加速。

for (i = startrow; i < endrow; i++) {
   if ((endrow - startrow) < 5) {
       break;
  }   //行数过少取消判断
   
if (middle_standard - left_black[i] > 4 && right_black[i] - middle_standard > 4) {
       RowCount2++;
  } else if (middle_standard - left_black[i] > 2 && right_black[i] - middle_standard > 2) {
       RowCount++;
  } else {
       break;
  }

   if (RowCount == endrow - startrow) {
           Gear = 8;
  }
   if (RowCount2 == endrow - startrow) {
       Gear = 10;
  }
}   //判断中线上半部与赛道两边界无焦点即可加速
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

  • 顶部有效图像判断加速：
  在正常计算中线误差的 for 循环内 当计算完前三分之一使用面积时，计算有效图像前三分之一的误差，用于远处判断加速。这样可以使运算更为高效。

  • 顶行采样判断直道加速：
  分别计算speed_line为20, 25, 30这三行的误差，并取最大值用于判断加速。
  speedline_err_real = (ABS)(( 5 * middle_line[speed_line] +
  2 * middle_line[speed_line + 1] +
  3 * middle_line[speed_line - 1]) / (10) - middle_standard);

  • 有效行判断加速：
  当有效行在顶部时，给予加速。可以简单使用分段打表的方式，也可以拟合成线性关系。记得写限幅。

  • 距离判断加速：
  实际走过一定距离的直道, 就算是直道。有些时候小s不会判断加速，这时只是用车身姿态来判断，可以解决这些正常判断较为难以处理的情况。注意这样加有滞后性, 加速量不要给多。

if(ABS((int16)(fabsf(CarSpeed_a))) < SpeedZone) {
   Dis_Acc_Count ++;
} else {
Dis_Acc_Count = 0;
}

if (Dis_Acc_Count >= 500) Dis_Acc_Count = 500; //限幅

if (Dis_Acc_Count > CountZone) {
Gear = 3;
}
1
2
3
4
5
6
7
8
9
10
11

三岔：

  • 我们的控制方案三岔路口的一条路分为八步处理。

  在预判断三岔后加速降为加速计算量的一半。当完全判断为三岔时，因为摄像头判断区域与车身实际位置存在距离，且在摄像头高度和角度不变的情况下，距离也不会变。所以第一步为前进一定距离至三岔中心点，同时开始90度旋转舵机。

  第二步在前进完成后使车身旋转30度与入口赛道平行。这样可以防止仅摄像头补线输出值过大，进入三岔出现抖动的问题。
  因为舵机旋转需要时间，且旋转完成后摄像头处理速度较慢，旋转完成时的摄像头输出值不稳定，旋转后不能立刻切为摄像头循迹。所以第三步为车身横向平移一定距离等待摄像头稳定。

  第四步即为三岔内横向循迹。我们出现的情况是右三岔循迹良好左三岔严重偏向外侧，所以右三岔基础速度和加速最大值都会大于左三岔。另外对于三岔内偏向一侧也可以使用与环岛相同的处理办法，即左右分别赋予不同的摄像头中值。

  第五步在预判断出三叉后开启转角输出限幅，限幅至仅能小幅度修正姿态，防止补线补到三岔的另一侧致使出三岔内切。并且也将加速降为加速计算量的一半。

  第六步在判断到出三岔后前进一定距离至中心点，同时开始转正舵机，原因同第一步。

  第七步在前进完成后使车身旋转150度。

  第八步在旋转后前进一定距离，原因同第三步。至此三叉的一条边处理结束。左三叉也是相同的处理方式。

 

第九章 麦轮特色控制

---

麦轮安装

  • 麦克纳姆轮由两大部分组成：轮毂和辊子（roller）。轮毂是整个轮子的主体支架，辊子则是安装在轮毂上的鼓状物。车模上使用的麦轮轮毂轴与辊子转轴呈 45° 角，这个夹角理论上可以是任意值，根据不同的夹角可以制作出不同的轮子，车模上的45°也是最常见和最容易批量生产的角度。

  • 麦轮的安装：麦轮一般是四个一组使用，两个左旋轮，两个右旋轮。左旋轮和右旋轮呈手性对称。常用的有两种安装方式，分为O型和X型，其中X和O表示的是与四个轮子地面接触的辊子所形成的图形。又可以根据四个轮子与地面接触点所围成的形状分为正方形与长方形。传动力矩的长度上O型大于X长方形，且X正方形轮子转动产生的力矩会经过同一个点，yaw 轴无法主动旋转和保持角度，因此一般安装时采用O型。科宇的车模为正方形，四个轮子上滚轮的转动轴汇聚于底板中心，我们也选择了O型来安装。

  安装后通过速度分解，将底盘的运动期望解算至电机转速，分别控制四个轮子的转速，即可实现3个独立自由度上的运动运动。麦克纳姆轮驱动的H车属与于Holonomic，它的控制自由度等于整体自由度，因为它可以在平面坐标系内沿任意方向移动。小车可以由 2个平面移动自由度和 1个转动自由度分别控制。 2个平面自由度控制小车的位置， 1个转动自由度控制小车的姿态。所以小车所有的运行状况都可以用这三种情况耦合而成。

  麦轮的运动模型推算过程略过，可以阅读参考文献[1]，这里直接给出结论

  可以解算出四个轮子的转速为

  同理，根据逆运动学模型中的三个方程逆向运算可解得，正运动学模型方程组为

常规控制方案

  • 常规的速度控制方案为二自由度，即只使用Vy和w，不使用横向自由度Vx，控制思路类似于三轮车模的差速控制。对于正常的直道和弯道最终我们就是只使用了这种二自由度的方案。这种方案好处在于十分稳定，不过车模的物理重心需要尽量接近四个麦轮的的中心这点更是尤为重要。

麦轮特色控制方案

  所谓全向组的特色控制方案，即在常规三轮差速的二自由度控制上增加x轴向控制。具体我们尝试了全程三自由度循迹和元素内三自由度循迹。但因为全程三自由度循迹未能在一段时间内调参调整至传统二自由度的慢速档，再加上当时还有闭陀螺仪环的想法，最终仅在元素内使用三自由度。

  • 坡道处理： 常规二自由度若上坡位置不正，需要在坡道打角，容易造成在坡道上抖动并摔出坡道，并且高速过坡稳定性堪忧。麦轮车在识别到坡道后，可以切换控制模态，转为仅使用Vy和Vx这两个自由度，即在上坡时通过横向平移修正姿态。这样可以做到稳定过坡，并且即使开启坡道加速也能有不错的稳定性。
  注意在上坡后最好开始横向计步（即编码器Vx累加），开启限幅为坡道三分之一左右的宽度，避免过矫摔下坡道，限幅防摔这一点也是传统二自由度很难做到的。
  • 环岛处理： 四轮在处理环岛时会遇到内切或是外切环岛，此时可能需要赋予不同的摄像头中值或是切换摄像头补线等特殊处理。而麦轮车仅需要加上X轴向反方向的一点速度补偿即可实现环岛内稳定循迹。
  对于环岛内Vx方向速度的确定，因为我们并未整定出三自由度高速循迹的参数，所以仅使用凑试打表的方法赋予定值。但赋予定值的效果也能算得上可以，十分钟左右就凑试出了当前环内速度对应的定值，并在速度控制算法和摄像头补线均未变时该定值很稳定。在我们尝试三自由度低速循迹的方案，只需要稍加修改参数即可过环，不过高速情况与低速可能差异很大，我们的思路仅为抛砖引玉。

  对于左右环岛不对称问题，也可以赋予不同横向速度来修正偏移量。

  • 三叉路口：三叉内外切问题，以及左右不对称问题，我们的处理方案与环岛相同，在二自由度的基础上增加Vy方向的补偿即可。
  • 小s弯：对于识别到小s弯后，可以切换控制模态为Vy和Vx二自由度循迹，即平移过小s弯。不过常规控制小s弯内w打脚本身也不会很大。我们尝试当时速度并不快，二者区别并不大，反而因为小s弯内加速后横向速度稍微过大就会打滑漂移冲出路肩，使得参数整定变得困难，再加上摄像头也不能稳定判断出小s元素，最后线上赛大小s弯不占分数也没有设计进赛道，最终并没有一套稳定可行的方案。不过平移过小s很具观赏性，当时可加速量也高于传统控制，这种针对小s的控制也算是抛砖引玉吧。
  • 平移出库：平移出库胜在稳定性，编码器vx方向计步很稳定。对于高速出库平移可以单独拉出一个PID参数，不过即使使用正常三叉横向循迹的PID参数效果依旧可以。另外可以在平移之外再给y轴速度，实现斜对角出库。横向出库速度不输舵机打角出库，出库总共用时不长也不会过1s，反而受出库位置限制更小也不会撞路肩更加稳定。
  对于入库我们还是采用传统补线入库，三斤多的全向车定点刹车再平移对控制精度要求太高了，对于入库来说没有必要调整那么精细，传统二自由度足够了。
  • 三自由度循迹：将摄像头误差传递给w轴和Vx轴，使用两种不同模态PID控制，最终再输出给电机环。

---

● 相关图表链接:

• 图3-1 电磁前瞻
• 图3-2-1 电池固定槽(左)(右)
• 图3-2-2 实际装配效果
• 图3-3-1 轴承与碳杆
• 图3-3-2 原有的附加支架
• 图3-3-3 改用热熔胶固定
• 图3-3-4 增加装置后
• 图3-4-1 FT2331M舵机
• 图3-4-2 舵机支架
• 图 3-4-3 装配效果
• 图3-4-4 连接件
• 图3-4-5连接件装配效果
• 图3-5 测距传感器 tf-mini
• 图3-6-1编码器安装
• 图3-6-2 编码器安装
• 图3-6-3 编码器安装架
• 图3-7-1 陀螺仪
• 图3-7 实际安装效果
• 图3-8麦克纳姆轮
• 图3-9-1 打印件
• 图3-9-2装配效果(正面)
• 图3-9-3装配效果(反面)
• 图3-9-4 驱动板
• 图3-9-5 整体装配效果
• 图 4-2-1 6V电源电路
• 图4-2-2 5V电源\3V3电源
• 图4-3-1 屏幕接口
• 图4-3-2 蜂鸣器
• 图4-3-3 按键
• 图4-5-1 H桥驱动电路
• 图 4-5-2 电机-单片机隔离电路
• 图4-5-3 芯片电源
• 图5-1-1 正常赛道获取到的图像
• 图5-1-2 特殊情况丢线