- 1:序言
- 2:前置知识
- 3:物理环境
- 3.1:传感器
- 3.2:执行器
- 4:程序框架
- 4.1:程序框架基本要求
- 4.2:控制思路
- 5:题外话
序言
该文章基于机器人创新协会的第四代机器人控制器进行撰写。主要用于引导初次接触相关设计任务的开发者产生宏观的智能小车编程设计思路。
在过去几年的观察中,我看到协会的很多开发者往往会在初期受阻,并且大多数人还只是刚学习了C语言,甚至C语言也了解不多(亦如我大一的时候一样)。因此大家在第一次涉及这种相对较大的项目工程时容易手足无措,而且编程思路和程序结构都较为混乱。希望本文可以给大家带来一些帮助,尽量少走弯路。有基础的读者可以根据目录跳转至感兴趣的部分。
本人才疏学浅,若有纰漏,还望海涵,也希望可以积极指出以便更正,避免误人子弟。
实际上,基于现在越来越卷的教学体系,循迹小车看起来似乎是一个非常low的任务。毕竟B站上有很多初中生乃至小学二年级都在进行循迹小车的设计,还发了很多视频。(不愧是毕导口中的小学二年级啊),但是,如果希望设计一个健壮稳定可控的循迹小车,依然需要一定的宏观设计思路。
在开始本文之前,请先尝试设想一下一个智能小车的实现过程,倘若觉得有些茫然无措,那么请不必随便搜一些教程,代码一段一段的copy,一段一段地硬啃,然后急着上手,结果发现一堆问题,最后代码没看懂,东西没做好,心态先崩溃,转身就劝退。充足的思考和准备,明确一下方向后有的放矢,希望能给读者带来一些帮助。
前置知识
通常,开始本文的设计目标,需要先掌握以下前置知识:
开发环境
请确保您已经或者能够完成Keil开发环境的安装和第四代机器人控制器底层框架的顺利编译。
C语言基础
请确保您具备头文件、函数调用、宏定义等C语言基础知识,否则建议先学习掌握C语言基础,可以看一些菜鸟教程或者相关B站视频。
硬件基础
请确保您了解什么电机、什么是传感器、什么是单片机。
控制算法基础
请确保您了解基本的PID控制算法,控制周期等思想。
其实不了解也不是什么太大的问题,本文会尽可能地浅显易懂一些,也尽量地减少专业性术语的使用。
物理环境
通常,机器人设计或者说控制系统设计,最先关注的点(或者说较为优先关注)是系统运行的物理环境,这并非是系统运行的载体,如计算机、单片机之类,而是机器人系统去产生交互的周围物理环境。比如本文的循迹小车的物理环境就是小车运动的机体、地面、和待循迹的线,如下图所示。
当然,实验室的循迹物理环境并非任意曲线而是网格地图,如下图所示。
为了实现在循迹线上的移动,我们所设计的小车至少应当具备辨认黑线或者白线的能力,并且具备稳定的车轮结构使得小车不至于跌倒且可以进行符合预期的移动。
机器人的核心就在于传感器(检测黑白线)、执行器(车轮实现移动)和控制器(单片机、计算机等大脑实现控制的思想)。这也是完备机器人所具备的基本结构,进行任何简易单体机器人的设计都可以从这三个方面入手。
传感器
传感器提供了环境感知的能力,让机器人可以判断环境。本设计中的小车为了给机器人提供检测黑白线的能力,通常可以选择的传感器有:RGB相机、红外传感器、灰度传感器。
红外发射接收传感器具有不受灯光影响的优点。其工作原理是通过发射红外线,并利用接收头感知是否有红外线反射回来从而判断目标点是黑色(吸收光)还是白色(反射光),因此即便在昏暗的环境下也可以保证工作,而RGB相机和灰度传感器都是利用了物体的反射可见光,需要额外补光,无法在昏暗环境下工作。
本协会采用的是集成多个红外传感器实现的自研循迹传感器,如下图所示。其基本原理是通过一排的红外传感器从而实现一个区域的黑白检测。这很容易可以理解,亦如眼睛囊括了多少区域,才有可能获取多少范围内的信息。
倘若您使用的是本协会提供的循迹小车底层代码,则可以在“Application/amt1450_uart.h”头文件中看到如下函数的声明:
void get_AMT1450Data_UART(uint8_t *begin_Color,uint8_t *jump_Count,uint8_t *jump_Location);
亦如您在windows平台编程时包含了头文件“stdio.h”后便可以使用printf()函数一样,您也可以在包含头文件“amt1450_uart.h”后读取循迹传感器的数据,你可以不用关系具体的原理,就像printf函数一样,知道如何使用以及使用后会产生什么效果即可。
通常,在使用任意库(即头文件)时,应当仔细阅读头文件开头的总体注释和具体所声明函数的注释,这通常包含了函数的使用方法、限制条件和注意事项等等。对于不够标准的头文件,例如用户自行编写的头文件,则应当结合头文件所对应的源文件进行分析,并非所有的代码都会规范的写注释,而且也可能会存在BUG。
关于amt1450函数调用的基础知识
头文件math.h内存在如下声明:
double sqrt(double x);
这告诉了开发者可以按照声明的格式使用函数,例如该函数表示传入一个数字x,则可以计算返回它的二次方根结果。
同样的,get_AMT1450Data_UART函数中的参数表为:*begin_Color,*jump_Count,*jump_Location。
这三个变量均以指针的形式传入该函数,类似于scanf("%d",&a);中的"&a"。这是函数参数的“引用调用”,可以实现数据的传出。调用该函数后,begin_Color变量内读取到了“传感器开头的颜色”,jump_Count变量内读取到了“区域内的颜色改变次数”,jump_Location数组内读取到了“颜色改变的位置”。
例如,begin_Color=0(假如0代表白色),jump_Count=2,jump_Location[0]=60,jump_Location[1]=120。
我们就可以知道循迹传感器检测到的物理环境如下图所示:0的位置是白色,颜色发生了两次改变,分别在60和120的位置。
这是一种非常适合描述线性区域内黑白情况的一种方式,当然你也可以用一个144大小的数组存下这条线上的黑白情况,或者压缩一点可以是144/8即18个char型数组。
amt1450_Test_UART函数内给出了应用该函数的一种典型方式,此处不再赘述。
上述是典型的利用头文件实现函数调用的一个例子。在这个例子中,我们可以确定在路径无误的情况下,任意源文件包含目标头文件后就可以“使用”目标头文件内声明的函数、变量和宏定义。
头文件内是函数的声明,而并没有包含函数是如何实现的,其对应的函数定义通常在源文件内,例如本例中amt1450_Test_UART函数在“Application/amt1450_uart.h”内声明,在“Application/amt1450_uart.c”内定义,后缀分别为“.h”和“.c”。
本设计的小车还有诸如MPU姿态传感器、电机旋转编码器等传感器。
关于电机旋转编码器,其类似于智能设备的旋钮,都是一种旋转编码器(传感器),可以检测物体旋转的位置。与旋钮不同的时,由于电机转速较快,因此并非使用容易产生摩擦损耗的机械接触式旋转编码器,而是非接触式的磁编码器或者光电编码器,此处不赘述原理,倘若感兴趣可以自行学习。
旋转编码器提供了旋转位置信号,根据编码器的精度,有一圈分辨率4、100、250、500之分。其被称为线数,例如一个500线的旋转编码器则表示可以它检测的最小单位是1/500圈($=\frac{1}{250}\pi$)。假设 $t_1$ 时刻编码器输出为1000, $t_2$ 时刻编码器输出为2000,我们可以知道这段时间,目标被转动了 $\frac{2000-1000}{500}=2$ 圈,也就是 $4\pi$。
循迹小车底层在“Hardware/motor_controller.h”有关于获取编码器计数值的函数声明如下:
/** * @brief 获取编码器累计计数值 * @param nEncoder 编码器编号,nEncoder=1返回编码器1,以此类推。 * @return 编码器的累计计数值 32位带符号整形 * @note 返回值为32位带符号整形,注意长时间运行的溢出。一般情况下,数小时没有问题。 * 通常可以间隔一定时间进行两次调用,将两次的返回值作差运算得到增量结果,最后通过增量结果计算电机速度。 * */ extern int32_t Encoder_GetEncCount(uint8_t nEncoder);
执行器
执行器提供了影响环境的能力,使得一个系统拥有了输出。本设计中的小车所拥有的执行器就是电机,通过改变电机的转速实现各种复杂的运动:减速、加速、刹车、转弯、曲线行驶等等。
循迹小车底层代码所提供的“Hardware/motor_controller.h”头文件中看到如下函数的声明:
//设置轮子转速,nMotor电机编号,nSpeed轮子线速度,单位:mm/s void MotorController_SetSpeed(uint8_t nMotor, int16_t nSpeed);
通过包含“motor_controller.h”头文件即可使用该函数设置电机的转速,例如MotorController_SetSpeed(1,100);
则表示设置1号电机的转速为100mm/s。
关于电机函数调用的基础知识
对于amt1450,本文并未过多涉及获取传感器数据的基本原理,即底层代码所提供的get_AMT1450Data_UART函数是如何实现循迹传感器数据的获取的,其主要是利用UART串口进行数据通信比进行数据的解析。
对于电机,倘若期望小车控制的更好则建议了解电机的控制过程。
众所周知,电机是一种具有两个端口的执行器件,对这个端口施加电压(通电)则可以使得电机旋转,施加反向电压则实现电机的反转。施加电压越高,则电机旋转越快。通常单片机(CPU)输出的电流能力是有限的,会皆由电机驱动芯片,实现电机的大功率驱动(输出高电压高电流)。
假设,我们期望让电机转速适中,此使需要通电6V,那么我们有三种方法:
- (持续稳定地)通电6V
- (在较短的单位时间内)1/2的时间通电12V,1/2的时间通电0V
- (在较短的单位时间内)2/3的时间通电12V,1/3的时间通电-12V
这叫做伏秒平衡原理,他们在功率输出上是等效的,电机被驱动所达到的转速也就是一样的。单片机也是通过这种方式控制向电机施加的电压大小,这中间的变量就是单位周期内各部分电压的时间比例。
同样的,电压的施加本质上也是一种广义的通信——电压本身的大小就是信号,因此基于这种等效,我们也把这种信号传递叫做脉冲宽度调制。
上图就是一个典型的PWM波(脉宽调制信号波形),而且是双极性PWM波(双极性指同时存在正负,相应的,上文中的方法b只有一种极性,就是单极性调制)。其中 $T_0$ 就是一个较短的周期, $T_1$ 是12V的时间宽度(脉宽), $\frac{T1}{T0}$ 就是12V通电时间所占的比例(也被称为占空比)。我们很容易可以得到它的等效电压就是:
$$U=\frac{T1}{T0}\times 12 + \frac{T2}{T0}\times (-12)$$
该调制方法的控制函数在循迹底层中的“Hardware/motor_driver.h”声明:
/** * @brief 设置电机驱动的PWM占空比 * @param nMotor 电机编号,可选值1-4 * @param nDuty PWM占空比,0 ~ MOTOR_DRIVER_PWM_DUTY_LIMIT 对应 0 ~ 100% * @details 该函数通过设置PWM的占空比,控制电机驱动H桥的开关周期,从而实现电机的降压控制。 * 占空比 0 ~ 100% 对应电压 -VCC ~ +VCC。 */ extern void MotorDriver_SetPWMDuty(uint8_t nMotor, uint16_t nDuty);
假如您使用过电机,可以尝试给电机装上车轮,然后给电机通较小的电压,用手(注意安全,确保电机转速不至于过快)或者将车轮切面与地面接触施加压力,会发现车轮的转速收到了影响。
这是因为在存在外部负载或干扰时,电机的转速与电压并不是一一对应的关系,倘若需要确保车轮转速保持不变,应当给电机增加电压,直到电机转速达到预期。
这一“不断地调节电压直到使得电机转速达到预期设定”的过程就是闭环控制过程。每一次判断和控制的周期,就是控制周期。在一定范围内,控制周期越短,反应越灵敏,控制效果越好,但对CPU的运算压力越大。(这里加上一定范围是因为物理对象是一个复杂过程,在设计过程中切勿陷入唯一论,例如在系统响应受限或者硬件控制信号受限的情况下,控制周期过短反而容易产生灾难性后果,这一点在模拟舵机的控制上也有例子。)
典型的工业控制方法就是PID控制,其具有算法简单但控制效果和性价比极为优异的特点,占据了工业领域的半壁江山,或者作为另外半壁江山控制算法的内在核心。电机转速控制也是一个典型的用到了PID三个全部环节的控制系统。
循迹底层代码在“Hardware/motor_controller.c”源文件中实现了电机的转速检测、PID闭环控制和电压输出。
通常,您只需要使用该文件定义的MotorController_SetSpeed函数实现电机转速控制即可,而无需关心其他内容。
假如您所设计的小车为如上图所示的两轮差速式驱动底盘,则可以很容易分析得到小车运动的模型。驱动车轮等距安装于小车中轴线的左右两侧,前后辅助安装从动车轮用以支撑小车的前后平衡。其中通过两节对装的铜锣柱调节从动轮的高低,以确保从动轮只是刚刚好接触地面,使得车自重产生的绝大部分压力落在驱动轮上,以利于驱动轮产生足够的有效转动摩擦推动小车前进。
通过定性的分析我们可知,两轮同速正转,小车前进,同速反转,小车后退。两轮转速不一发生转弯,两轮等速率反转,小车原地转弯。
通过定量计算分析我们可知,小车的速度可以分解为线速度(影响前进)和角速度(影响转动)的叠加,其公式如下所示:
$$\begin{equation} \begin{cases} \begin{aligned} v&=\frac{v_\text{R}+v_\text{L}}{2} \\ \omega&=\frac{v_\text{R}-v_\text{L}}{2} \end{aligned} \end{cases} \end{equation} $$
因此,我们可以通过合理设置左右轮的速度实现小车的速度控制,上述公式也可以将 $v_\text{L}$ 和 $v_\text{R}$ 转换为因变量,$v$ 和 $\omega$ 转换为自变量,读者可以自行推导。
以上便是小车物理环境的部分内容,但希望读者可以明确,以上并非物理环境的全部,例如还存在“地面脏污对循迹传感器的影响”、“小车轮速的误差”、“微小压力变化、电机发热和地面空间不一致导致的两轮速度波动”、“传感器波动、时空离散导致的控制波动” 、“重心偏移导致两轮压力差,使得摩擦力不一致,从而引起两轮加速度不一致”、“电机出场的非绝对一致导致的加速度不一致”等问题,这些环境、控制算法、传感器和执行器的误差让一切充满了不确定性。同样的,我们在分析过程中其实也对诸多环境和问题进行了简略近似。
希望读者阅读到这里可以明确我们拥有什么,并且能做什么,并且如何做到。
例如我们可以用传感器发现循迹线,用电机控制小车移动,从而实现智能小车的循线。
程序框架
程序框架也可以归入机器人的控制器部分。芯片和电路板是控制器的物理载体,软件程序是控制器的内在灵魂。算法的设计水平和程序的实现质量是影响控制器控制效果的极大变数,也是方便维护升级的部分。通常合理的控制系统设计是软件与硬件相互配合的,硬件设计为软件的设计作预先准备,软件可以在合理化的硬件设计上提高效率和质量。
程序框架基本要求
通常一个合格的程序框架至少应当具备如下特点:
合理的文件结构
即合理的模块拆分,将函数和变量无比混乱地堆放在一起容易导致思路的混乱、更容易写出BUG,当程序规模逐渐增大也越难修改和补充。以写长篇小说为例,假如您著有一篇50个章节20万字的小说,结果50个章节一共有100个场景,全部以“场景444”、“场景b”、“场景a”、“场景1”、“场景1(1)”、“场景1(1)(1)”作为文件名乱序堆在同一个文件夹中,而且一个主角A与主角B共同参加一个会议的场景涉及到了50个句子,同时存在“场景a”、“场景444”、“场景2(1)(2)”里面,每当你想要继续修改的时候一定是一场灾难。
对于程序而言,当刚开始还好,当内容越来越多之后呢?
所以,合理的前瞻性分块是开始的基础。以本文所设计的循迹底层框架为例,其以HAL库为基础,文件内分别为“Application”、“Core”、“Drivers”、“Hardware”、“MDK-ARM”。
其中“MDK-ARM”是编译器文件,包含了keil的工程入口和程序的编译结果。“Drivers”是芯片(本例为STM32F407VET6)的底层驱动代码包含HAL库、CMSIS驱动库等。“Core”是使用STM32CubeMX生成的基本代码框架。以上内容通常使用STM32CubeMX工具直接生成,非特殊情况下不需要更改,除了“Core”内的程序入口文件“main.c”,里面包含了main函数,程序开始的地方。
“Application”和“Hardware”是额外增加的用于放置用户代码的文件夹,分别是“应用程序文件夹”和“硬件控制程序文件夹”。
倘若交由读者来设计应用于上文所述物理环境的智能小车,会如何设计程序框架。
可能一:在main.c里堆砌代码,后面发现有点麻烦了,新建了一个"control.c"和“"control.h”,移动部分,然后来回编写。然后某一次太多了,就整理一下,多出了个“move.c”“move.h”,写了一些“zhuanwan()”、“zuozhuan()”、“lukou()”等函数。(!这里的转弯、左转、路口函数是错误的命名例子。)
该循迹底层框架考虑到实际的任务规模,先划分为Application和Hardware两大类,其中Hardware包含并提供了:
beep.c / .h | 蜂鸣器控制 |
delay.c / .h | 延时功能 |
keys.c / .h | 按键控制 |
led.c / .h | led控制 |
motor_controller.c / .h | 电机闭环控制算法 |
motor_driver.c / .h | 电机基本驱动 |
mpu6500dmp.c / .h | 姿态传感器数据获取 |
vcc_sense.c / .h | 电压检测功能 |
Application内包含并提供了:
amt1450_uart.c / .h | 循迹传感器数据获取 |
backend_loop.c / .h | 后台控制循环 |
config.h | 系统总体配置 |
(PS:实际上我认为amt1450_uart应该放在Hardware里)
Hardware基本上都涉及到了与硬件沟通的部分,也可以理解为是控制的基础驱动,在Hardware里面分别实现了各类传感器和执行器的控制,提供了各类实用函数,例如:获取循迹数据、设置电机速度、控制led灯等等。
Application则基于实现的底层函数实现更加复杂的功能,目前基本为空,是为了便于协会的开发者在此基础上拓展出个性化的控制方案。目前只提供了backend_loop和config,backend_loop提供了一个被定期循环执行的函数,config用于配置系统的各项参数,包括但不限于PID控制器的参数、车轮的直径(用于将车轮转速的单位从圈数转换为mm/s)等。
关于Backend_Loop()函数的设计目的
backend_loop.h头文件内的循环函数声明如下:
/** * @brief 后台循环函数 * @note 可以将需要周期性调用的函数放在本函数内统一执行,注意避免使用长时间的阻塞性函数。 * 需要注意的是,该周期性不一定严格正确,需要准确时间间隔的函数建议单独处理。 * 本程序默认情况下,在 stm32f4xx_it.c 文件中被 TIM7 的 20ms中断 循环调用。 */ void Backend_Loop(void);
提供该底层框架的原因是因为我在过去几年发现协会的新生在设计小车控制方法的时候欠缺控制周期的概念,书写的程序基本上是线性的。当涉及到循迹线检测、十字路口判断时基本上是在main函数中的while循环里跑死,而涉及到机械臂和舵机的控制时,基本上是直接给一个目标信号,舵机和机械臂在移动中生硬突兀。而对于转弯的常见写法是:
可以发现,所有的过程冗杂在一个线性过程里,循迹的时候不能控制舵机、控制舵机的时候不能循迹、不能转弯等等。
因此,当然,这个问题是可以通过使用操作系统解决的,但是使用操作系统涉及到了框架更加复杂不利于代码都刚开始学的初学者,性能开销较大,且对于该相对简单的任务没有必要。此外本库还设计了一个宏函数CYCLE_OK(time);
,在Backend_Loop()里调用时将time设定为100则可以得到以100ms为周期的定时代码段。例如下述代码实现了1s变化一次的LED闪烁。
void Backend_Loop(void){ /* 每1000ms执行一次 */ if(CYCLE_OK(1000)){ FnLED1_SHIFT(); } }
如果是第一次尝试多文件编程建议先利用搜索引擎学习,或者参照一下本文所涉及框架内的标准写法。关于头文件的标准固定开头读者也可以自行学习。
#ifndef __XXX_H #define __XXX_H /* some code */ #endif
合理的命名和注释
我已经无数次看到初学者写代码存在大段的奇怪命名和无注释情况。
例如直行函数用“zhixing()”,左转函数用“zuozhuan()”,变量命名为“a”,“b”,“cishu”,“jvli”等,拼音命名会造成极大的阅读困难,假如我给你一个函数叫“wuhang()”你猜是什么功能。
本框架中的代码均遵循了易读的命名规则,有时辅以前缀区分归属的模块。例如amt1450函数中的begin_Color、jump_Count即是如此,分别可以直译为“开始的颜色”、“跳变的次数”,其中jump取了“突然改变”之意。倘若使用了支持utf-8的现代编译器,使用中文命名变量也是可以的,可以省去取名困扰,但处于兼容性的考虑,目前(截至2023年10月)各行各业还是以英文命名为主。
当然,必须要承认的是,在一个项目中,命名风格统一是一件比较困难的事情,但至少不应该出现一些影响阅读的命名方式。
关于注释,我的意见是尽可能在必要的地方增加适当的注释,当然也有一个说法叫在合理的命名规则之下,代码本身就是注释。
但不论如何建议合理规划模块、函数、变量的命名。
控制思路
以上内容确保了你有一个扎实的基础,和清晰代码结构的保障,接下来我们则需要思考控制思路的实现方法。我们现在拥有了读取传感器数据的手段、控制电机转速的手段、获取电机编码器数据的手段,main函数里的前台while循环和backend里的后台定时循环。
在开始以下内容之前,请确保读者了解:在遇到的每一个场景时先思考代码的构成和控制的思路,而避免直接阅读我后文提供代码,这样有利于您的印证学习。
通常,对于一般控制系统,我们会这样设计main函数:
void main(){ /* 一些初始化代码 */ while(1){ /* 一些循环的控制过程 */ if(xxx()){ /* 一些特定条件下执行的代码 */ } Delay(10); /* 粗略的循环定时 */ } return 0; }
其中,我们会在while前执行一些固定的初始化配置部分,这些内容仅执行一次。while循环内,会固定的执行一些控制目标,例如循线。而特定的条件则可以实现一些流程或逻辑控制,例如:如果我当前处于第三个十字路口则左转,如果我到达了第十个路口则右转等等以实现路线的控制。
典型的线性控制思路:
首先,我们假设一个场景,我们需要沿着地面上的白线行驶,那么循迹传感器可以告诉我白线所处的位置。我们通过get_AMT1450Data_UART函数获取了循迹数据,例如,begin_Color=1(假如1代表黑色),jump_Count=2,jump_Location[0]=60,jump_Location[1]=120。则可以知道60和120之间是白色,那么按照取中点原则,白线的坐标就是(60+120)/2=90,由于循迹传感器的坐标范围是0~144,所以可知白线更靠近循迹传感器坐标为144的那一端。那么我们就可以通过设置小车的角速度来矫正这一偏差,使得小车回到正中(白线位于循迹传感器的正中位置72)。
由此我们可以实现如下程序:
程序实现代码
void TracingControl(void){ /* 定义变量 */ /* 因为库函数中提供的get_AMT1450Data_UART最大返回六个跳变,所以count数组大小为6 */ uint8_t begin, jump, count[6]; uint8_t position; get_AMT1450Data_UART(&begin, &jump, count); if (jump == 2) position = 0.5f * (count[0] + count[1]); /* 判断白线位置 */ if(position>72){ SetCarRotationSpeed(-20); }else{ SetCarRotationSpeed(20); } } void main(){ /* 一些初始化代码 */ while(1){ /* 一些循环的控制过程 */ TracingControl(); Delay(10); /* 粗略的循环定时 */ } return 0; }
这样我们就基本实现了循线的过程,当然上述代码中采用的控制方案过于生硬,属于开关控制,因为角速度只会在-20到20之间变化,那么他有较大的概率会在白线的左右振荡。因此我们很容易可以设计出控制量随偏移量变化的代码,偏差越大角速度绝对值越大,偏差越小角速度绝对值越小,这也就是最基本的P控制,比例控制器。代码如下。
程序实现代码
void TracingControl(void){ uint8_t begin, jump, count[6]; uint8_t position; get_AMT1450Data_UART(&begin, &jump, count); if (jump == 2) position = 0.5f * (count[0] + count[1]); /* 更新的部分:P控制器 */ uint16_t error = position - 72; float Kp = 0.1f; SetCarRotationSpeed(Kp*error); }
如果你仔细阅读就会发现,那如果偏差过大会不会控制量太大了,那么你可以加一个if判断,如果Kp*error大于或小于一定限度则等于该限度,这样就避免了控制量过大,这被称为幅限。当然,关于幅限环节和幅限的大小需要您仔细地分析如何设置,设置多大合适。
特别的,请注意偏差和角速度的方向,矫正角速度的方向应该是误差减小的方向,即负反馈。如果写成相反的矫正角速度,会导致系统发现偏差那么控制使得偏差更大,即正反馈。通常您可以通过观察循迹传感器的数据判断0-144的位置。
实际上,对于本例所设计的简单低速系统,小车行驶速度小于等于500mm/s,单位控制周期10ms内,小车移动的距离不超过5mm,而且循迹的线全部是直线,没有急转弯,比例控制器可能是完全足够的,但需要您通过调试确定合适的比例系统Kp。请结合具体情况适当的使用PID控制器,并确保您理解PID三个部分的具体作用以免产生不可预期的控制结果。您也可以游玩循迹小游戏感受调试PID控制器的过程。
此处赘述一处内容,如果您游玩了上述循迹游戏就会发现波形曲线的重要性,这也是代码设计和控制调试的重要手段,基于数据分析的调试让控制思路跟为顺畅,而非茫然的尝试,请在以后的代码设计和调试中灵活捕捉和运用系统运行产生的数据。
典型的PID控制器代码
完整代码位于第四代机器人控制器扩展库内,/Ext-Library-code/Grid_Position_move_controller/pid_controller.c & .h
/** * @brief: 增量式PID */ float INCPID_Update(INCPIDController *PID,float target,float input){ //误差值计算 float error=target-input; //误差值存储 PID->prevError=PID->lastError; PID->lastError=error; //PID输出计算 float output =( PID->output + PID->pidParam.kp*( error-PID->lastError) + PID->pidParam.ki*( error+PID->lastError)*0.5f + PID->pidParam.kd*((error-PID->lastError)-(PID->lastError-PID->prevError)) ); //PID输出幅限 output = Constrain(output,PID->outMINLimit,PID->outMAXLimit); //PID输出更新 PID->output = output; return output; } /** * @brief: 位置式PID */ float POSPID_Update(POSPIDController *PID,float target,float input,float dt){ //误差值计算 float error=target-input; //比例项 float pTerm=PID->pidParam.kp*error; //积分项 PID->iTerm+=(PID->pidParam.ki*(error+PID->lastError)*0.5f*dt); PID->iTerm=Constrain(PID->iTerm,0-PID->integrationLimit,PID->integrationLimit); //微分项 float dTerm=PID->pidParam.kd*(error-PID->lastError)/dt; //误差值存储 PID->lastError=error; //PID输出计算 float output = pTerm + PID->iTerm + dTerm; //输出值滤波 output = PID->FilterPercent * output + (1 - PID->FilterPercent)* PID->output; //PID输出幅限 output = Constrain(output,PID->outMINLimit,PID->outMAXLimit); //PID输出更新 PID->output = output; return output; }
假如,现在增加一个环节,要求你判断十字路口,并且在十字路口处右转。
我们首先分析十字路口的特点,从而利于判断。
倘若我们沿着白线行驶,当接触到横着的白线时则意味着为遇到了十字路口,那么我们很容易可以想到其核心判断依据便是循迹传感器数据的开头颜色为白,且跳变次数为零。
倘若我需要实现在第二个十字路口右转,那么就需要增加一个十字路口计数的变量,并且在合适的时候每遇到一个十字路口就增加一。此处留一个悬念,倘若读者采用的十字路口计数增加一的条件是“循迹传感器数据的开头颜色为白,且跳变次数为零”则会发生一个异常的错误——计数不准且结果偏大。希望读者自行思考产生的原因并进行解决。
当综合十字路口检测后,如果要实现第三个路口右转,读者可能会得到如下程序:
程序流程框图
这样的代码存在几个问题。
1、右转的时刻:是cross_count(十字路口计数)等于3的时候吗?那么在第三个路口到第四个路口之间的很长一段时间cross_count都等于3,而我们并不需要这些路段都右转。
2、这个时候读者可能会想到,那就是当正在十字路口且cross_count==3的时候,那么这个时候有第二个问题,你的转弯方案是原地不动自转还是像轿车行驶一样一边直行一边右转。
3、循迹传感器的安装位置:这决定了当小车检测到十字路口时,小车所处的坐标,那么它的转弯运动轨迹应当如何?
原地不动自转会更加稳定且简单,但像轿车行驶一样一边直行一边右转会更快(因为不用停止),小车速度的动态过程如何(因为速度不能突变,哪怕你在程序里设置了小车速度为零,它的速度也无法瞬间达到零),这些都会影响最终的运动情况。
还有一个问题就是,读者计划如何实现转弯的这一过程。
假设循迹传感器在小车的正前方,当探测到十字路口时,倘若采用停到十字路口正中央再转弯的方案,按照“过程思维”来书写,可能的程序如下:
程序流程框图
上述程序中,220和420被称为魔数,是一种在编程中直接出现的数字,在某些情况下应当适当避免。这里是为了使得小车在探测到十字路口后,再运行一段时间而刚好停在十字路口正中间,220ms是一个多次调试得到的值。420ms则是转弯刚好90度的值。(这是一个不好的办法,因为该参数会随着地面和轮胎摩擦力,小车重量,小车速度等多种因素的变化而改变,您可能需要反复调试。)
当然,也有读者可能会想到,我完全可以用循迹传感器发现白线在正中间来作为转弯到90度的标志啊,但这里又将出现两个问题:
1、开始转弯的瞬间,白线就在正中,所以如果直接判断可能会导致还没转90度就认为转弯完成。
2、于是您可能需要增加一个出弯的延迟,确保直行方向上的白线已经离开视线。
3、因为小车的停止需要时间(速度不可突变),所以如果当“白线在正中间”时设置速度为零,最终的结果可能其实转弯了110度。所以实际上还不到正中时就应当准备停止。
细心的读者可能会发现,上述代码并未采用position == 72,请尝试思考。
以上内容也是我在作为初学者时一步一步累计代码形成的一段简单的循迹程序,我模拟了这一过程。当然,我们也会发现这里面存在几个非常明显的问题:
1、运动过程缺少预期性,依靠低容错率的while条件判断。
2、运动过程的维持高度依赖delay延时,这些片段被硬编码在各个过程里,给后续的调试(倘若要修改转弯速度)带来极大的麻烦。
3、获取循迹传感器数据在多个地方需要调用:循迹、判断十字路口、转弯。
基于数学模型的控制思路:
需要说明的是,我并非说前述代码就一定不好,更重要的是,我们需要在适合的时候学会使用适合的控制方案。
首先,让我们重新回顾一下,我们所具有的设备:控制电机转速、读取循迹传感器数据和读取电机编码器数据。
电机编码器数据即电机转动的圈数,在不考虑滑动摩擦的情况下,通过乘以车轮的周长我们即可得到小车行驶的距离,利用这一数据我们即可实现更加随意的距离控制。例如:特定地控制小车行驶500mm。利用编码器可以得到一个小范围内相对准确得到数据,比起使用距离除以速度得到期望行驶的时间来说,编码器不存在舍去误差。
但特别的,请注意物理总是不完美的,您很难使得小车的车轮时完全滚动的,它必然存在滑动,这导致了编码器的数据在长时间积累下也并非绝对准确。
那么我们还有什么途径可以解决这一问题?
通常,我们将采用多传感器数据融合技术来解决单一传感器的误差问题。
通过观察外部环境可以发现,本目标所所涉及的地图是400mm x 400mm 形成的网格地图,每个十字路口的间距为400mm,白线的宽度为3mm,这些信息我们均未利用。
另外,我们也可以通过测量循迹模块到车旋转中心的距离来确定循迹模块探测点相对车的坐标。
结合这两点信息我们可以确定,当循迹传感器检测到十字路口时,小车理论上所处的位置是可以推算出来的,于是我们可以利用这一信息校准编码器的数据,从而达到传感器融合的目的。
例如,小车循迹传感器在前方10mm,小车从0mm处出发,根据编码器可知小车行驶了320mm,但此时小车检测到了十字路口,因此可以认为小车实际上是处于300mm的位置,于是根据这一数据重置小车的位置,则实现了位置的校准。
至此,我们利用编码器得到了相对准确的短距离位移,利用循迹传感器得到了准确的网格位置坐标,两者结合,我们可以较为准确的判断小车的位置,从而实现更加复杂的控制效果。
同样的,我们也会发现,当要实现的功能越来越多,这些过程冗杂在一起会显得没有条理,此外,所有的过程缺少一个耗时的概念,我们无法确定每一次控制,小车可能会行驶的距离,所有的微小误差都可能会放大出一些难以预料的问题。
计算电机位置的相关函数实现
/** * @brief: 更新电机当前位置 * @retval: None */ void UpdateMotorCurPostion(void){ int64_t cnt; for(uint8_t i=0;i<MOTOR_NUM;i++){ cnt = (int64_t)Encoder_GetEncCount(i); motorController[i].curPosition = (int32_t)((PI * CAR_WHEEL_DIAMETER) * nCnt * ( 1000 / 4 ) / ENCODER_RESOLUTION); } }
这是一个计算电机旋转位置的示例函数,其与/Hardware/motor_controller.c 内的速度计算函数极为相似,区别在于去掉了时间,且计数值从差值变为了绝对值。这是因为电机速度的计算本就是利用两次编码器的插值除以时间得到的。其中的curPosition以mm为单位。
倘若您深究其计算过程,会发现其同样存在舍入误差,因为最后面除以的编码器线数是一个非常大的数,我在另一个框架的代码中计算了不舍入的微米旋转距离,从而最终实现了毫米级的车辆定位。
基于前后台周期控制的控制思路:
我们通过数学的模型的引入可以解决控制模糊的问题,但也会使得程序越来越复杂,也无法解决上文提出的第三个问题。
因此我们可以将任务并行化。
1、通过查阅手册我们可以知道AMT1450传感器的串口数据汇报速度约为12ms,所以在12ms以内多余的获取数据都是多余的,同时,我们也可以视为在12ms的极短时间内,循迹数据基本不变,通过分析小车的速度,我们也可以确定10ms内小车最大的移动距离为5mm(假设速度500mm/s)。因此,我么可以将get_AMT1450Data_UART函数放在后台定时循环Backend_Loop内,不断地刷新全局变量begin_Color,jump_Count和jump_Location。
且由于该函数在后台定时循环内调用,我们可以确定不论何时,该数据都会被定时刷新。至此,我们可以视为“抛弃”了这段代码,我们只需要在需要的时候随心所欲地使用这三个变量即可。
2、我们可以重新写一个小车位置计算函数,并放在后台中不断地更新,他会通过编码器的数据和循迹传感器的数据不断地更新小车的位置Car_Position从而再一次解耦小车位置和角度的计算。
3、我们可以将循迹函数也放在后台循环内,并辅助以条件,从而将循线过程再次“抛弃”。
请尝试构思代码结构。
程序流程框图
void Backend_Loop(void){ Car_UpdateAMT1450Data(); Car_TracingController(); Car_MovePositionController(); /* 每1000ms执行一次 */ if(CYCLE_OK(1000)){ FnLED1_SHIFT(); } }
前文所述循迹函数的条件是指,并非所有时刻都需要循线,例如转弯过程。所以至少应当提供一个小车状态变量car_state用于控制循线的启动或停止。
于是我们可以在main函数内聚焦我们核心的路线控制任务。
同样的,对于路线控制任务(例如直走三格,左走五格)我们也不必写成这样一条一条的函数:
Car_Go(3); while(is_arrive()==false); Car_TurnLeft(); while(is_arrive()==false); Car_Go(5);
而可以采用表驱动的写法,例如:
switch(task[current_target]){ case GO: Car_Go(); case TurnLeft: Car_TurnLeft(); case TurnRight: Car_TurnRight(); default: break; }
这样我只需要把每个节点要做的事情,存到task数组内,利用自增current_target变量一件一件的执行下去。
本文其实还有非常多的内容没有涉及,例如:
1、位置控制如何实现。读者可以尝试思考,我们当前已经具备了检测位置的手段,如何运用PID实现位置控制。
2、表驱动任务的具体实现。
3、还有很多宏定义、自定义结构体、自定义枚举类型等等,这些都是方便编程,方便控制实现的有利工具。
希望本文可以给大家带来一点帮助。
题外话
我大一的时候,只懂得一点C语言编程(刚开始学),对于STM32完全不了解,刚开始的时候我也不懂什么配置IO口、读取传感器数据,是跟着下面这几篇教程来的。
教程链接
不过我刚刚检查,这几篇文件基本变成VIP文章了,如果有会员的可以尝试阅读,另外如果读者是本协会的成员,需要注意该教程的代码可能不适用于基于HAL库的循迹底层框架代码。
基于STM32F103C8T6的循迹避障小车完整制作过程(详细)----上篇
文章写到一半的时候我突然想去看看我当初是怎么学的,感怀一下当时的心境,以及当时作为初学者对哪些地方是产生困惑的。结果很无奈地发现五个变成VIP文章,一个无法访问。我无意反对知识付费,相反我支持知识付费。但不得不说,中文互联网的环境质量是在下降的,知识论坛的逐渐消亡,大厂私域流量的割据对立,博客和搜索引擎的衰落,再过一段时间,中文互联网上还会剩下什么呢?
我希望做一个逆行者。
没有人,单机博客wuwu  ̄﹃ ̄
tql,orz
太精彩 了
可以的,有用!
感谢支持!
写的泰裤辣!!!