从零开发一款四轮智能小车
以下是本次课程的大纲
| 课程大纲 | 内容 |
| 一、如何设计一款四轮智能小车 | 1. 总体设计思路 |
| 2. 机械结构设计 |
| 3. 驱动系统设计 |
| 4. 传感系统设计 |
| 5. 控制系统设计 |
| 二、如何开发智能小车的驱动器软件 | 1. 驱动器软件总体架构 |
| 2. FreeRTOS 快速上手 |
| 3. 运动控制与PID使用 |
| 4. 人机显示屏交互 |
| 5. 串口通信 |
| 三、如何实现智能小车的通信协议 | 1. 通信协议设计 |
| 2. 通信协议的下位机实现 |
| 3. 通信协议的上位机实现 |
| 4. 构建机器人ROS 驱动 |
| 四、如何开发智能小车的应用功能 | 1. 运动控制 |
| 2. 视觉巡线 |
| 3. 人体跟随 |
| 4. 目标检测 |
本课程将通过一系列实例来给大家介绍关于机器人开发的一些方法，并给出一些演示过程带大家实际操作智能机器人人，希望通过这门课程的学习，大家能够对智能小车有一个明确的认识和开发思路。
https://class.guyuehome.com/p/t_pc/goods_pc_detail/goods_detail/p_663d85c2e4b023c0667fe87c?product_id=p_663d85c2e4b023c0667fe87c
如何设计一款四轮智能小车
此部分共分为五个小节，通过这个部分的学习，我们将了解机器人的概念和组成，对机器人的整体设计有一个明确的思路，然后通过机械结构、驱动系统、传感系统、控制系统四大组成部分，展开介绍设计一款智能小车的基本思路。
通过这一部分的学习，我们希望大家脑海中能够对智能小车有一个明确的认识和开发思路，在之后的介绍中我们再逐个击破。
总体设计思路
从控制的角度来讲，机器人可以划分为图中四大组成部分，分别是：传感系统、执行机构、驱动系统和控制系统。
先来看执行机构，这是机器人要动起来的重要装置，比如移动机器人是需要移动的，那如何带动轮子旋转或者转弯呢，就是类似这样的电机、舵机来执行运动的。但并不是所有运动部位都会安装电机，比如一辆真实的汽车，一般只有一个电机或者发动机，那如何让两个轮子，甚至四个轮子都转起来呢，这就需要一个完成动力分配的传动系统，比如转弯的时候动态调整左右两个轮子的速度，这需要严密的机械设计，也就是差速器的功能。除了移动机器人，在一些工业机器人中，驱动机器人的关节电机、抓取物体的吸盘夹爪，也可以看做是执行机构。总之，执行机构就是执行运动的一套装置。
为了让执行机构准确的执行动作，还需要在执行机构前连接一套驱动系统，比如我们要让机器人的电机按照1m/s的速度旋转，如何动态的调整电压、电流，来达到准确的运动目的呢，这就是由电机驱动器来实现的。如果是电动执行机构的话，其配套的驱动系统一般都是由驱动板卡+控制软件组成，也是嵌入式系统应用的重要领域，我们在学校中学习的单片机、PID、数字电路等概念，都和这个部分紧密相连。驱动系统的选择是根据执行机构来的，比如普通的直流电机，用类似这样的电机驱动板就行，工业上常用的伺服电机，都会用到220V甚者380V电压，就得用专业的伺服驱动器了，还有类似吸盘的气压驱动，外接键盘鼠标一样的外设驱动，以及各种各样的传感器驱动，总之，驱动系统的职责就是保证机器人各种设备的正常运行。
机器人光动是不行的，还需要具备感知能力，这就得靠传感系统了。传感系统一般分为内部传感和外部传感，内部传感是用来感知机器人自身状态的，比如通过里程计计算自己轮子旋转的速度，从而计算累积位移，通过陀螺仪感知机器人自身的角加速度，判断转弯时的状态，通过加速度计，感知机器人在各个运动方向上的加速度，可以用来判断运动趋势或者上下坡，还有力传感器，可以用来感知机器人自身与外部的相互作用力度，比如抓一个鸡蛋，但又不至于抓破。
与内部传感器相反，外部传感器帮助机器人感知外部信息，类似人眼一样，使用摄像头来看到外部的彩色图像，不过机器人可以通过多种外部传感器超越人类的极限，比如可以使用红外传感器，在没有光线的情况下，也可以看到外部环境，类似夜视仪一样，还可以利用激光雷达、声纳、超声波等距离传感器，感知在某个角度范围内的障碍物距离，还有麦克风和喇叭，方便我们与机器人语音交流。
传感系统是智能机器人的重要组成，很多机器人甚至装备了几十上百个传感器，感知自身与环境的各种信息，比如自动驾驶汽车就是如此。
在这些系统的上层，就是机器人的大脑——控制系统了。控制系统一般也是硬件+软件组成，硬件大多采用计算资源丰富的处理器，比如我们常用的笔记本电脑、树莓派、英伟达板卡等；其中运行的软件就是各种丰富的应用程序了，比如让机器人建立未知环境的地图，或者让机器人运动到送餐地点，再或者是让机器人识别人脸。
智能机器人的核心算法体现，都是在控制系统中完成，这也是我们未来做机器人软件开发的主要位置。
机器人实例
机械结构设计
以上述机器人为例，直观看上去，似乎这个小车并不复杂。
底盘是整个小车的底座，通过螺丝安装了四个个车轮和配套的电机以及舵机，用来驱动小车运动。
底盘上第一层是电池，相对比较重，尽量放在下边，可以让小车的重心靠下，第二层则用来放置主要的传感器和控制器板卡，这样可以很好的隐藏两层板的叠加同时让板卡更加安全。
小车底盘的前端，为了安装相机，我们要设计一个安装的支架，一侧用螺丝固定在底盘上，另外一侧可以固定相机，最好还可以调节相机可视角度。
第三层则有一个LED显示屏以及一些通孔，可以用来放置一些方便调试以及需要一定高度的传感器如雷达等。
此外在车体两侧还放置了RGB灯带，可以让小车看起来更加炫酷。
这就是智能小车的机械结构啦，大家可以根据自己选择的各种零部件尺寸。
接下来，我们把焦点放到小车的执行结构上，也就是这两个电机和四个轮子，它们如何控制小车运动呢？
其实对于智能小车而言，常见的有三种形态运动方式，接下来一一介绍。
差速运动模式
什么叫差速？简单来说，就是通过两侧运动机构的速度差，来驱动机器人前进或转弯。
平衡车就是典型的差速驱动。大家想象一下平衡车的运动方式，如果两个轮子的速度相同，一起往前转，平衡车整体就向前走，一起往后转，平衡车整体就向后走。如果左边轮子的速度比右边快，平衡车就会向右转，反之则是向左转。这就是差速运动最基本的运动方式。
从这张图中，大家也可以看到，差速运动的重点是两侧轮子的速度差，像我们家里用的扫地机器人，大家有兴趣可以翻起来看一下，一般左右两侧都有一个驱动轮，一共是两个轮子，可以叫做两轮差速。大家可能还看到过稍微大一些的车子，比如美团京东的自动物流机器人，很多都是有四个驱动轮的，原理和两轮差速类似，但是负载和越障能力都更强，这就类似汽车中的两驱和四驱的差别，四驱的动力性能会更加强劲。
全向轮运动模式
如果要让机器人向前运动，就得让左右两侧的轮子把这里的横向分力抵消掉，所以两个轮子是对称的，如果不对称，那横向分力都是朝一个方向，机器人就走偏了。
如果要让机器人横着向左运动，那就得把纵向分力抵消掉，一个轮子向后转，一个轮子向前转。
如果要让机器人斜着走怎么办，对侧的轮子不转就可以了，运动的这两个轮子合力就是斜向前45度的。
可见，麦轮平台的运动就是各个轮子之间“力” 的较量。
阿克曼运动模式
这个是阿克曼运动的原理图，大家仔细看两个前轮和两个后轮的状态，像不像两个并驾齐驱的自行车呢，大家手拉手一起转弯。没错，我们一般性的把阿克曼模型等效简化为自行车模型，这两种模型在运动机理方面基本是一致的。大家再想想我们平时骑自行车的画面，车把控制前轮转向，但没有动力，脚蹬通过一系列齿轮，传送动力到后轮上，驱动自行车运动，这些齿轮就相当于是差速器。
驱动系统设计
之前我们有介绍过，机器人的驱动系统可以分为这几种方式，大家想一下，在智能小车中，我们需要用到哪些呢。
电机驱动肯定是需要的，我们得让小车的电机转起来；
传感驱动也是需要的，小车里的里程计、IMU都要驱动之后才能获取数据。
还有外设驱动，作为下位机的控制器板卡和上位机应用处理器之间，需要通过串口传输数据，这需要串口驱动，我们还希望了解小车的一些状态，一般会在控制器板卡上设计显示屏或者蜂鸣器来做人机交互，这也需要IO的驱动。
这样看来，驱动系统需要支持的设备不少，它是智能小车的底层基础，强壮稳定的驱动系统是未来上层应用开发的必要保障，大家在设计开发中千万不要忽视。
[OriginCar主板原理图.pdf]
传感系统设计
智能小车还需要去感知自身和环境的状态，这个部分我们来看一下智能小车需要哪些传感器，它们的原理又是什么样的？传感系统分为内部和外部之分；
内部传感器主要是电机上的编码器，和控制板卡上的IMU。
外部传感器主要是前端的相机。
里程计
里程计，是移动机器人普遍使用的传感器。
类似于汽车记录行驶公里数的码表，可以通过轮子的旋转圈数记录里程，机器人一般也会在轮子或者电机上安装一个传感器，通过检测轮子的旋转速度，再对时间积分，得到机器人的实时位置和速度，这项功能所使用的设备就叫做里程计，而实现这种功能的设备也并不是唯一的。
比如大家在某些小车上，会看到电机旁边安装有这样一个码盘，上边有不少开缝，电机旋转带动码盘旋转，光电管发射的光线就会以某种频率穿过缝隙，被接收端采集到，通过这个频率我们就可以计算得到电机的旋转速度，从而得到机器人走了多远、旋转了多少度等自身状态信息。
我们这里设计的智能小车采用的里程计是另外一种，叫做霍尔传感器，安装在电机尾部，当电机旋转时，霍尔传感器通过感应周边磁场产生的信号，测量出电机的旋转速度，继而得到机器人的状态信息。
所以无论是光电码盘还是霍尔传感器，都是根据采样单位时间内产生的脉冲数计算出轮子旋转的圈数，再通过轮子的周长计算出机器人的运动速度，速度对时间积分后，就得到里程信息啦，这就是里程计的基本原理。
不过里程计也有一个问题，那就是每次测量会有误差，不断积分后，误差必然会被放大，也就是我们常说的里程计累积误差。
相机成像
https://blog.csdn.net/qq_45488834/article/details/136447423?spm=1001.2014.3001.5501
控制系统设计
硬件选择
智能小车的身体/肌肉都有了，接下来我们来看一下智能小车的大脑，之前我们介绍的驱动系统、传感系统，最终都会和控制系统这个大脑中枢产生联系。
一般情况下，我们会在控制系统中完成这些功能，这也是我们进行机器人开发的主战场，后边使用的ROS就是这个战场上我们使用的主要武器。
在智能小车的开发中，直接放一个电脑在机器人身上肯定是不合适的，常用的方式是使用更加小型的嵌入式应用处理器，比如常见的树莓派、Jetson Nano、RDK X3，都可以满足智能小车的开发需求。
我们就拿这三款板卡做一个参数对比，CPU和内存两项都大差不差，智能小车的应用开发我们更加关注算力和框架的支持，在算力方面，理论上数值越高，我们能够实现的应用就会越流畅，比如人体识别之类的，RDK X3有5T的算力，相比其他板卡优势较为明显。在框架方面，三者都支持我们后续使用的ROS，RDK X3中还提供了一套深度优化的ROS2系统，名为TogetheROS，包含很多智能化的应用功能。
综合而言，我们这里选择RDK X3作为控制系统的硬件平台，当然，大家在自己智能小车的设计中，使用其他平台也都可以，里边的软件基本都是通用的，只是在算力要求较高的应用上会有一些差别。
控制系统中的软件部门该如何实现呢？
我们通过这张图来明确一下。
运动控制器作为驱动系统的核心，负责控制电机，完成对机器人自身状态的检测。这其中各项功能的实现，都是嵌入式开发的过程，会用到定时器、IO、PWM、PID等很多概念的原理和实现。
控制系统是智能小车的大脑，运行自主导航、地图构建、图像识别等功能，同时也会兼具一小部分传感器驱动的任务，通过USB采集外部相机和雷达的信息。
这个控制系统和运动控制器之间的通讯连接，通过串口来完成的。
为了方便机器人的操控，我们还会在机器人之外，使用自己的笔记本连接机器人进行编码和控制。
在整个这个大框架中，虚线框中的应用功能都是基于ROS2和TogetheROS环境进行开发实现，运动控制器中的功能是基于嵌入式开发来实现，从这张图中，大家也可以更好的理解ROS开发与嵌入式开发的关系，两者各司其职，一个偏向于上层应用，另一个偏向于底层控制，共同实现机器人的智能化功能。
好啦，讲到这里，相信大家已经对智能小车的设计思路有了清晰的理解。
回到我们最初的问题，如何从零设计并开发一款智能小车呢？
我们的目标是要做出这样一台智能小车，拆解之后由这些部件组成，可以划分为执行机构、驱动系统、传感系统、控制系统四大组成部分，未来课程，我们就会继续沿着这个思路，详细展开每一个部分的设计与实现。
如何开发智能小车的驱动器软件
驱动器软件总体架构
先来看到这样一张图，这张图是我基于过往的一些开发经验总结而来的针对机器人系统，我们需要从哪些部分进行开发。抛开机械结构等外形，从底向上看是硬件部分，硬件我们可以分成两个部分，分别是嵌入式板卡和外设。嵌入式板卡又可以分为MCU和开发板，外设则是接到两个板卡上的一些机构例如电机、相机、雷达等；往上看是驱动单元，驱动单元是针对硬件板卡进行设计的，它需要使能MCU和开发板的各个外设，并为后续的开发提供坚实稳定的基础。再往上是中间件，ROS/ROS2就是基于Linux下进行操作的一个中间件，我们可以基于这样的中间件更加便捷的开发机器人的各种功能，而工具很好理解，就是将一系列的功能打包起来，变成便捷易操作同时功能十分强大的面板或者指令了；回过头来看，其实驱动器软件的上下游是硬件外设和中间件软件，那想要开发驱动器软件就需要对上下游非常熟悉吗？不一定，但是作为一个嵌入式工程师需要懂得如何和对应开发者进行交流。
首先对于硬件开发者，他们会给我们提供一份原理图，以及一般性的我们需要知道芯片的使用手册，这样才能辅助我们去正确的使用硬件的各个IO，避免如最低级的正负级接反等问题；对于中间件软件，我们则需要和他们一起制定一个协议，这个协议约定了双方如何进行正确的数据交互。
说到这里，大家就很明确对于此处的嵌入式开发者而言需要做什么事情了。一方面要正确的使用板卡资源和外设资源，并对其数据进行加工处理来做一些开发者企图实现的功能。另一方面要将处理的数据与上游进行交互从而达到更高效的数据利用，当然了必要时也需要接收来自上游数据的指令。
此处举一个例子来看
FreeRTOS 快速上手
搭建软件环境
以Keil为例
https://www.keil.com/download/product
Stm32配置keil5编译版本
FreeRTOS使用
RTOS全称为 Real Time Operation System，即实时操作系统。RTOS强调的是实时性，又分为硬实时和软实时。硬实时要求在规定的时间内必须完成操作，不允许超时；而软实时里对处理过程超时的要求则没有很严格。RTOS的核心就是任务调度。
FreeRTOS是RTOS的一种，尺寸非常小，可运行于微控制器上。微控制器是尺寸小，资源受限的处理器，它在单个芯片上包含了处理器本身、用于保存要执行的程序的只读存储器（ROM或Flash）、所执行程序需要的随机存取存储器（RAM），一般情况下程序直接从只读存储器执行。
[FreeRTOS实时内核实用指南.pdf]
任务创建
关于任务创建，FreeRTOS 提供了 API 给我们使用。格式如下
| Plaintext
/* 创建 DemoTaskCreate 任务 */
DemoTaskCreate_Handle = xTaskCreateStatic((TaskFunction_t )DemoTaskCreate, //任务函数
(const char* )"DemoTaskCreate", //任务名称
(uint32_t )128, //任务堆栈大小
(void* )NULL, //传递给任务函数的参数
(UBaseType_t )3, //任务优先级
(StackType_t* )DemoTaskCreate_Stack, //任务堆栈
(StaticTask_t* )&DemoTaskCreate_TCB); //任务控制块
if(NULL != DemoTaskCreate_Handle)/* 创建成功 */
vTaskStartScheduler(); /* 启动任务，开启调度 */ |
代码实例
| C
//Task priority //任务优先级
#define START_TASK_PRIO 1
//Task stack size //任务堆栈大小
#define START_STK_SIZE 256
//Task handle //任务句柄
TaskHandle_t StartTask_Handler;
//Task function //任务函数
void start_task(void *pvParameters);
//Main function //主函数
int main(void)
{
systemInit(); //Hardware initialization //硬件初始化
//Create the start task //创建开始任务
xTaskCreate((TaskFunction_t )start_task, //Task function //任务函数
(const char* )"start_task", //Task name //任务名称
(uint16_t )START_STK_SIZE, //Task stack size //任务堆栈大小
(void* )NULL, //Arguments passed to the task function //传递给任务函数的参数
(UBaseType_t )START_TASK_PRIO, //Task priority //任务优先级
(TaskHandle_t* )&StartTask_Handler); //Task handle //任务句柄
vTaskStartScheduler(); //Enables task scheduling //开启任务调度
}
//Start task task function //开始任务任务函数
void start_task(void *pvParameters)
{
taskENTER_CRITICAL(); //Enter the critical area //进入临界区
//Create the task //创建任务
xTaskCreate(Balance_task, "Balance_task", BALANCE_STK_SIZE, NULL, BALANCE_TASK_PRIO, NULL); //Vehicle motion control task //小车运动控制任务
xTaskCreate(MPU6050_task, "MPU6050_task", MPU6050_STK_SIZE, NULL, MPU6050_TASK_PRIO, NULL); //IMU data read task //IMU数据读取任务
xTaskCreate(show_task, "show_task", SHOW_STK_SIZE, NULL, SHOW_TASK_PRIO, NULL); //The OLED display displays tasks //OLED显示屏显示任务
xTaskCreate(led_task, "led_task", LED_STK_SIZE, NULL, LED_TASK_PRIO, NULL); //LED light flashing task //LED灯闪烁任务
xTaskCreate(pstwo_task, "PSTWO_task", PS2_STK_SIZE, NULL, PS2_TASK_PRIO, NULL); //Read the PS2 controller task //读取PS2手柄任务
xTaskCreate(data_task, "DATA_task", DATA_STK_SIZE, NULL, DATA_TASK_PRIO, NULL); //Usartx3, Usartx1 and CAN send data task //串口3、串口1、CAN发送数据任务
vTaskDelete(StartTask_Handler); //Delete the start task //删除开始任务
taskEXIT_CRITICAL(); //Exit the critical section//退出临界区
} |
运行状态
运动控制与PID使用
PID 原理介绍
为了更好的控制机器人行走，电机控制算法通常使用PID算法，PID(proportion integration differentiation)其实就是指比例，积分，微分控制。当我们得到系统的输出后，将输出经过比例，积分，微分3种运算方式，重新叠加到输入中，从而控制系统的行为，让它能精确的到达我们指定的状态。基本形态如下图所示：
比例环节是对偏差瞬间作出反应，偏差只要产生，控制器立即产生控制作用， 使控制量向减少偏差的方向变化。
控制作用的强弱数值表示为误差值与比例系数Kp的乘积，取决于比例系数Kp， 比例系数Kp越大，控制作用越强， 则过渡过程越快， 控制过程的静态偏差也就越小； 但Kp越大，也越容易产生振荡， 就会破坏系统的稳定性。
所以， 比例系数Kp选择须恰当， 以期达到过渡时间少、静态偏差小而又稳定的效果。
积分部分的表达式为误差积分值与比例系数Ki的乘积， 从式中可看出，只要存在偏差， 则它的控制作用就不断的增加。 只有在偏差e(t)＝0时， 它的积分才是一个常数，控制作用才是一个不会增加的常数。 可见，积分部分可以消除系统的偏差。
积分环节的调节作用虽然会消除静态误差，但也会降低系统的响应速度，增加系统的超调量。积分常数Ti越大，积分的积累作用越弱，这时系统在过渡时不会产生振荡； 但是增大积分常数Ti会减慢静态误差的消除过程，消除偏差所需的时间也较长， 但可以减少超调量，提高系统的稳定性。
实际的控制系统中除了消除静态误差外，还要求加快调节过程。在偏差出现的瞬间，或在偏差变化的瞬间， 不但要对偏差量做出立即响应（比例环节的作用）， 而且要根据偏差的变化趋势预先适当的纠正。为了实现这一功能作用，须在 PI 控制器的基础上加入微分环节，形成 PID 控制器。
微分环节的作用是阻止偏差的变化。它是根据偏差的变化趋势（变化速度）进行控制。偏差变化的越快，微分控制器的输出就越大，并能在偏差值变大之前进行修正。微分作用的引入， 将有助于减小超调量， 克服振荡， 使系统趋于稳定， 它加快了系统的跟踪速度。
大家可以通过以下案例更加直观性的通过ROS的视角来看PID的原理和作用。
https://gitee.com/xiaobairisk/ros-pid-controller
参考链接：https://zhuanlan.zhihu.com/p/406496635
工程实例
在电机控制中，我们一般性的会将轮速转变为PWM去计算，那么如何获取PWM以及发布目标PWM变成了一个更加直接的问题。
origincar_controller 中使用了PI运算。通过获取编码器值（当前PWM值），以及设定的目标PWM值进行PI调速。
首先需要解决当前PWM值获取。
| C
// 获取当前编码器值
//编码器原始数据转换为车轮速度，单位m/s
MOTOR_A.Encoder= Encoder_A_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_B.Encoder= Encoder_B_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_C.Encoder= Encoder_C_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision;
MOTOR_D.Encoder= Encoder_D_pr*CONTROL_FREQUENCY*Wheel_perimeter/Encoder_precision; |
- MOTOR_A.Encoder：这表示电机A的编码器读数，可能是以编码器的脉冲数或者计数器的形式表示。
- Encoder_A_pr：这个变量可能是与编码器A相关的参数，可能是用来校正或者缩放编码器读数的系数。
- CONTROL_FREQUENCY：这是控制系统的频率，表示控制系统更新的速度，通常以赫兹（Hz）为单位。
- Wheel_perimeter：这是车轮的周长，用来将编码器的脉冲数转换为实际的行驶距离。
- Encoder_precision：这是编码器的精度，表示编码器每个脉冲对应的行驶距离
对于阿克曼结构车辆而言，该如何获取目标位置呢？其中需要对目标速度进行轮速解算，也就是计算出最终需要达到的PWM值。
| C
// 结算目标PWM值
{
float R, Ratio=636.56, AngleR, Angle_Servo;
//对于阿克曼小车Vz代表右前轮转向角度
AngleR=Vz;
R=Axle_spacing/tan(AngleR)-0.5f*Wheel_spacing;
//前轮转向角度限幅(舵机控制前轮转向角度)，单位：rad
AngleR=target_limit_float(AngleR,-0.49f,0.32f);
//运动学逆解
if(AngleR!=0)
{
MOTOR_A.Target = Vx*(R-0.5f*Wheel_spacing)/R;
MOTOR_B.Target = Vx*(R+0.5f*Wheel_spacing)/R;
}
else
{
MOTOR_A.Target = Vx;
MOTOR_B.Target = Vx;
}
//舵机PWM值，舵机控制前轮转向角度
Angle_Servo = -0.628f*pow(AngleR, 3) + 1.269f*pow(AngleR, 2) - 1.772f*AngleR + 1.573f;
Servo=SERVO_INIT + (Angle_Servo - 1.572f)*Ratio;
//车轮(电机)目标速度限幅
MOTOR_A.Target=target_limit_float(MOTOR_A.Target,-amplitude,amplitude);
MOTOR_B.Target=target_limit_float(MOTOR_B.Target,-amplitude,amplitude);
MOTOR_C.Target=0; //没有使用到
MOTOR_D.Target=0; //没有使用到
Servo=target_limit_int(Servo,800,2200); //舵机PWM值限幅
} |
PI计算
| C
int Incremental_PI (float Encoder,float Target)
{
static float Bias,Pwm,Last_bias;
Bias=Target-Encoder; //计算偏差
Pwm+=Velocity_KP*(Bias-Last_bias)+Velocity_KI*Bias;
if(Pwm>16700)Pwm=16700;
if(Pwm<-16700)Pwm=-16700;
Last_bias=Bias; //保存上一次偏差
return Pwm;
} |
人机显示屏交互
人机交互在驱动板中的体现很多，比如LED等、蜂鸣器、用户按键、继电器等，但是其中最为直观的外设当为OLED显示屏。
首先介绍为什么要引入OLED显示屏，从设计之初的考虑来看，最重要的一个原因是方便大家调试MCU代码，如大家可以看到当前状态下关键传感器信息，如陀螺仪等；此外也可以显示一些控制器传下来的信息，如WIFI0的IP地址。
接下来介绍一下OLED的主要优点：
- OLED 显示屏具备主动发光的特点，几乎没有视角限制，视角一般可达到 170 度，具有较宽的视角，从侧面也不会失真；
- OLED 显示屏低温特性好，在零下 40 摄氏度都能正常显示；
- OLED 显示屏的响应时间很快，大约是几微秒到几十微秒；
SSD1306驱动芯片实例介绍
以OriginCar使用的0.96寸SSD1306 OLED显示屏为例，以下为其原理图
如何开发显示屏
此处给出参考链接：SSD1306开发介绍，更关键的一点是在目前的调试开发中，大家该如何做二次开发。
在origincar_controller工程中，已经给大家封装好了使用函数
| C
//oled.h
void OLED_WR_Byte(u8 dat,u8 cmd);
void OLED_Display_On(void);
void OLED_Display_Off(void);
void OLED_Refresh_Gram(void);
void OLED_Init(void);
void OLED_Clear(void);
void OLED_DrawPoint(u8 x,u8 y,u8 t);
void OLED_ShowChar(u8 x,u8 y,u8 chr,u8 size,u8 mode);
void OLED_ShowNumber(u8 x,u8 y,u32 num,u8 len,u8 size);
void OLED_ShowString(u8 x,u8 y,const u8 *p); |
接下来给出显示实例
| C
// show.c
void oled_show(void)
{
...
//显示屏第1行显示内容//
OLED_ShowString(0,0,"Akm ");
//阿克曼、差速、四驱、履带车显示陀螺仪零点
OLED_ShowString(55,0,"BIAS");
if( Deviation_gyro[2]<0) OLED_ShowString(90,0,"-"),OLED_ShowNumber(100,0,-Deviation_gyro[2],3,12); //Zero-drift data of gyroscope Z axis
else OLED_ShowString(90,0,"+"),OLED_ShowNumber(100,0, Deviation_gyro[2],3,12); //陀螺仪z轴零点漂移数据
...
OLED_Refresh_Gram();
} |
实际开发中大家需要在类似本例中oled_show中使用OLED_ShowString/OLED_ShowNumber即可，注意在最后需要刷新一次OLED内容OLED_Refresh_Gram;
串口通信
串口通讯(Serial Communication)是一种设备间非常常用的串行通讯方式，因为它简单便捷，因此大部分电子设备都支持该通讯方式，其通讯协议可分层为协议层和物理层。物理层规定通信协议中具有机械、电子功能的特性，从而确保原始数据在物理媒体的传播；协议层主要规定通讯逻辑，统一双方的数据打包、解包标准。
物理层
RS232是一种串行数据传输形式，称其为串行连接，最经典的标志就是 9 针孔的 DB9 电缆RS232电压表示逻辑 1 ，0的范围大极大的增强了容错率，主要用于工业设备直接通信。
两个通讯设备的“DB9 接口”之间通过串口信号线建立起连接，串口信号线中使用“RS-232 标准”传输数据信号。由于 RS-232 电平标准的信号不能直接被控制器直接识别，所以这些信号会经过一个“电平转换芯片”转换成控制器能识别的“TTL 标准”的电平信号，才能实现通讯。
USB转串口：主要用于设备(如STM32)与其他设备通信。
电平转换芯片一般有CH340、PL2303、CP2102、FT232，使用的时候电脑要按照电平转换芯片的驱动(虚拟出一个串口)
协议层
串口通讯的协议层中，规定了数据包的内容，它由启始位、主体数据、校验位以及停止位组成，通讯双方的数据包格式要约定一致（一样的起始位 数据 校验位 停止位）才能正常收发数据
串口通讯的一个数据包从起始信号开始，直到停止信号结束。数据包的起始信号由一个逻辑 0 的数据位表示，而数据包的停止信号可由 1 或 2 个逻辑 1 的数据位表示
1个停止位：停止位位数的默认值。
2个停止位：可用于常规USART模式、单线模式以及调制解调器模式。
在数据包的起始位之后紧接着的就是要传输的主体数据内容，也称为有效数据，有效数据的长度常被约定为 5、6、7 或 8 位长
偶校验：校验位使得一帧中的7或8个LSB数据以及校验位中1的个数为偶数。
例如：数据=00110101，有4个1，如果选择偶校验(在USART_CR1中的PS＝0)，校验位将是0，最后数据检验如果数据有偶数个1则数据传输没有出错(但不是绝对的，如果同时两个数据为发送错误(0变成1)则还是偶数个1)
奇校验：此校验位使得一帧中的7或8个LSB数据以及校验位中1的个数为奇数。
例如：数据=00110101，有4个1，如果选择奇校验(在USART_CR1中的PS＝1)，校验位将是1，最后数据检验如果数据有奇数个1则数据传输没有出错，但同样不是绝对的(同时两个1变成0)
实例说明
以origincar_controller为例，使用USB转串口的方式，将OriginCar主板与电脑PC连接，并在电脑端打开串口助手，即可接收到来自OriginCar主板的如下数据，关于数据解析待下节分享。
此处大家应该思考一个问题，数据是如何发出来的？
| C
void data_task(void *pvParameters)
{
u32 lastWakeTime = getSysTickCnt();
while(1) {
vTaskDelayUntil(&lastWakeTime, F2T(RATE_20_HZ));
data_transition();
USART1_SEND(); //串口1发送数据
USART3_SEND(); //串口3(ROS)发送数据
USART5_SEND(); //串口5发送数据
CAN_SEND(); //CAN发送数据
}
} |
首先如上所示，OriginCar主板基于 FreeRTOS进行周期性的数据发送，即data_task 函数，接下来以USART3_SEND为例看数据如何发送。
| C
void USART3_SEND(void)
{
unsigned char i = 0;
for(i=0; i<24; i++) {
usart3_send(Send_Data.buffer[i]);
}
} |
如上，此处只是将Send_Data的数据给了usart3_send函数，那么Send_Data数据是什么呢？其实就是协议层封装数据啦。再看usart3_send
| C
void usart3_send(u8 data)
{
USART3->DR = data;
while((USART3->SR&0x40)==0);
} |
到此处，大家直接思考的问题就变成了USART3是什么？USART3->DR、USART3->SR是什么？
此处其实使用到了STM32固件库编程方式，大家可以通过查看固件库手册来对寄存器进行操作。
状态寄存器：USARTX->SR
其用于描述USART的工作状态，为编程者提供一个串口的实时状态，一般而言，发送时需要判断上一帧有没有发送完毕；接收时需要判断一帧数据有没有接收完毕，二者需要一个标志位进行状态表示，这其中的标志就在此寄存器中。
数据寄存器：USARTX->DR
发送和接收虽然是两个动作，但是在单片机内部是一个数据寄存器，这两个操作的唯一区别方法就是，执行写操作就是发送数据寄存器(TDR)，执行读操作的时候就是接受数据寄存器（RDR）。这也就解释了为什么上面的代码中，读和写都是使用的DR寄存器。
如何实现智能小车的通信协议
通信协议设计
在Part2《如何开发智能小车的驱动器软件》中我们已经介绍了什么是通讯方式的协议层，它是指通信双方对数据传送控制的一种约定，协议中包括数据格式、同步方式、传送速度、传送步骤、检纠错方式以及控制字符定义等，通信双方必须共同遵守。更具体一些常见的协议如下图所示
接下来以OriginCar机器人为例，介绍其通信协议。
首先关于几个细节需要和大家拉平，上位机一般而言指的控制器，以OriginCar为例是RDK X3开发板；下位机一般指的是驱动板，此处指OriginCar主板。
第二点，上下位机的指令发什么由开发者决定，往往取决于希望获取什么样的数据，例如对于下位机而言需要的是一个速度指令，那么上位机只需要发布X、Y、Z轴的速度指令即可，下位机负责对数据帧进行解析。
第三点，数据帧包含了帧头、帧尾以及校验位，关于这三位方式很多，但是上下位机一定要做到同步，这样确保数据解析正确。
第四点，校验位一般如何操作，什么是异或校验、是否可以自定义校验方式？
| C++
unsigned char checkXor(const char *strData,int len)
{
char checksum = 0;
for (int i = 0;i < len;i++)
{
checksum = checksum ^ strData[i];
}
return (unsigned char)checksum;
} |
上位机下发指令
下位机上发指令
通信协议的下位机实现
以和上位机通信的串口3为例，关于USART3初始化等在上一个Part已经和大家介绍。
波特率设置
| C
// system.c
void systemInit(void)
{
...
uart3_init(115200);
...
}
//usartx.c
void uart3_init(u32 bound)
{
...
USART_InitStructure.USART_BaudRate = bound;
...
} |
数据接收
关于数据的接收，其实是通过串口中断进行获取，并根据上位机下发指令进行数据解析，即可得到上位机下发的X、Y、Z速度，并将该速度给到运动控制进行实现。
| C
int USART3_IRQHandler(void)
{
static u8 Count=0;
u8 Usart_Receive;
if(USART_GetITStatus(USART3, USART_IT_RXNE) != RESET)
{
Usart_Receive = USART_ReceiveData(USART3); //读取数据
if(Time_count < CONTROL_DELAY)
return 0;
Receive_Data.buffer[Count] = Usart_Receive;
if(Usart_Receive == FRAME_HEADER||Count>0)
Count++;
else
Count=0;
if (Count == 11) //验证数据包的长度
{
Count = 0; //为串口数据重新填入数组做准备
if(Receive_Data.buffer[10] == FRAME_TAIL) //验证数据包的帧尾
{
//数据异或位校验计算，模式0是发送数据校验
if(Receive_Data.buffer[9] ==Check_Sum(9,0)) {
float Vz;
PS2_ON_Flag=0;
Remote_ON_Flag=0;
APP_ON_Flag=0;
CAN_ON_Flag=0;
Usart1_ON_Flag=0;
Usart5_ON_Flag=0;
command_lost_count=0;
Move_X = XYZ_Target_Speed_transition(Receive_Data.buffer[3],Receive_Data.buffer[4]);
Move_Y = XYZ_Target_Speed_transition(Receive_Data.buffer[5],Receive_Data.buffer[6]);
Vz = XYZ_Target_Speed_transition(Receive_Data.buffer[7],Receive_Data.buffer[8]);
Move_Z = Vz_to_Akm_Angle(Move_X, Vz);
}
}
}
}
return 0;
}
float XYZ_Target_Speed_transition(u8 High,u8 Low)
{
//数据转换的中间变量
short transition;
//将高8位和低8位整合成一个16位的short型数据
transition=((High<<8)+Low);
return transition/1000+(transition%1000)*0.001; //单位转换, mm/s->m/s
} |
数据发送
发送数据的方式：将要发送的数据打包到一个数组中，数组的长度是 24 字节，使用串口逐位发送出去。因为串口一次只能发送一个 8 位（1 个字节）的数据，因此 2 个字节（short 型）的数据会拆分成高 8 位和低 8 位发送。
以下给出数据封装代码，其作用就是将下位机上发指令按照代码的形式进行封装。
| C
void data_transition(void)
{
Send_Data.Sensor_Str.Frame_Header = FRAME_HEADER; //帧头
Send_Data.Sensor_Str.Frame_Tail = FRAME_TAIL; //帧尾
Send_Data.Sensor_Str.X_speed = ((MOTOR_A.Encoder+MOTOR_B.Encoder)/2)*1000;
Send_Data.Sensor_Str.Y_speed = 0;
Send_Data.Sensor_Str.Z_speed = ((MOTOR_B.Encoder-MOTOR_A.Encoder)/Wheel_spacing)*1000;
//加速度计三轴加速度
Send_Data.Sensor_Str.Accelerometer.X_data= accel[1]; //加速度计Y轴转换到ROS坐标X轴
Send_Data.Sensor_Str.Accelerometer.Y_data=-accel[0]; //加速度计X轴转换到ROS坐标Y轴
Send_Data.Sensor_Str.Accelerometer.Z_data= accel[2]; //加速度计Z轴转换到ROS坐标Z轴
//角速度计三轴角速度
Send_Data.Sensor_Str.Gyroscope.X_data= gyro[1]; //角速度计Y轴转换到ROS坐标X轴
Send_Data.Sensor_Str.Gyroscope.Y_data=-gyro[0]; //角速度计X轴转换到ROS坐标Y轴
if(Flag_Stop==0)
//如果电机控制位使能状态，那么正常发送Z轴角速度
Send_Data.Sensor_Str.Gyroscope.Z_data=gyro[2];
else
//如果机器人是静止的（电机控制位失能），那么发送的Z轴角速度为0
Send_Data.Sensor_Str.Gyroscope.Z_data=0;
//电池电压(这里将浮点数放大一千倍传输，相应的在接收端在接收到数据后也会缩小一千倍)
Send_Data.Sensor_Str.Power_Voltage = Voltage*1000;
Send_Data.buffer[0]=Send_Data.Sensor_Str.Frame_Header; //帧头
Send_Data.buffer[1]=Flag_Stop; //小车软件失能标志位
//小车三轴速度,各轴都拆分为两个8位数据再发送
Send_Data.buffer[2]=Send_Data.Sensor_Str.X_speed >>8;
Send_Data.buffer[3]=Send_Data.Sensor_Str.X_speed ;
Send_Data.buffer[4]=Send_Data.Sensor_Str.Y_speed>>8;
Send_Data.buffer[5]=Send_Data.Sensor_Str.Y_speed;
Send_Data.buffer[6]=Send_Data.Sensor_Str.Z_speed >>8;
Send_Data.buffer[7]=Send_Data.Sensor_Str.Z_speed ;
//IMU加速度计三轴加速度,各轴都拆分为两个8位数据再发送
Send_Data.buffer[8]=Send_Data.Sensor_Str.Accelerometer.X_data>>8;
Send_Data.buffer[9]=Send_Data.Sensor_Str.Accelerometer.X_data;
Send_Data.buffer[10]=Send_Data.Sensor_Str.Accelerometer.Y_data>>8;
Send_Data.buffer[11]=Send_Data.Sensor_Str.Accelerometer.Y_data;
Send_Data.buffer[12]=Send_Data.Sensor_Str.Accelerometer.Z_data>>8;
Send_Data.buffer[13]=Send_Data.Sensor_Str.Accelerometer.Z_data;
//IMU角速度计三轴角速度,各轴都拆分为两个8位数据再发送
Send_Data.buffer[14]=Send_Data.Sensor_Str.Gyroscope.X_data>>8;
Send_Data.buffer[15]=Send_Data.Sensor_Str.Gyroscope.X_data;
Send_Data.buffer[16]=Send_Data.Sensor_Str.Gyroscope.Y_data>>8;
Send_Data.buffer[17]=Send_Data.Sensor_Str.Gyroscope.Y_data;
Send_Data.buffer[18]=Send_Data.Sensor_Str.Gyroscope.Z_data>>8;
Send_Data.buffer[19]=Send_Data.Sensor_Str.Gyroscope.Z_data;
//电池电压,拆分为两个8位数据发送
Send_Data.buffer[20]=Send_Data.Sensor_Str.Power_Voltage >>8;
Send_Data.buffer[21]=Send_Data.Sensor_Str.Power_Voltage;
//数据校验位计算，模式1是发送数据校验
Send_Data.buffer[22]=Check_Sum(22,1);
Send_Data.buffer[23]=Send_Data.Sensor_Str.Frame_Tail; //帧尾
} |
通信协议的上位机实现
数据接收
| C++
bool origincar_base::Get_Sensor_Data()
{
short transition_16 = 0, j = 0, Header_Pos = 0, Tail_Pos = 0;
uint8_t Receive_Data_Pr[RECEIVE_DATA_SIZE] = {0};
Stm32_Serial.read(Receive_Data_Pr,sizeof (Receive_Data_Pr));
for (j = 0; j < 24; j++) {
if (Receive_Data_Pr[j] == FRAME_HEADER)
Header_Pos=j;
else if (Receive_Data_Pr[j] == FRAME_TAIL)
Tail_Pos = j;
}
if (Tail_Pos == (Header_Pos + 23)) {
memcpy(Receive_Data.rx, Receive_Data_Pr, sizeof(Receive_Data_Pr));
} else if (Header_Pos == (1 + Tail_Pos)) {
for (j = 0;j < 24; j++)
Receive_Data.rx[j] = Receive_Data_Pr[(j+Header_Pos) % 24];
} else {
return false;
}
Receive_Data.Frame_Header = Receive_Data.rx[0];
Receive_Data.Frame_Tail = Receive_Data.rx[23];
if (Receive_Data.Frame_Header == FRAME_HEADER) {
if (Receive_Data.Frame_Tail == FRAME_TAIL) {
if (Receive_Data.rx[22] == Check_Sum(22,READ_DATA_CHECK)||(Header_Pos == (1 + Tail_Pos))) {
Receive_Data.Flag_Stop=Receive_Data.rx[1];
Robot_Vel.X = Odom_Trans(Receive_Data.rx[2],Receive_Data.rx[3]);
Robot_Vel.Y = Odom_Trans(Receive_Data.rx[4],Receive_Data.rx[5]);
Robot_Vel.Z = Odom_Trans(Receive_Data.rx[6],Receive_Data.rx[7]);
Mpu6050_Data.accele_x_data = IMU_Trans(Receive_Data.rx[8],Receive_Data.rx[9]);
Mpu6050_Data.accele_y_data = IMU_Trans(Receive_Data.rx[10],Receive_Data.rx[11]);
Mpu6050_Data.accele_z_data = IMU_Trans(Receive_Data.rx[12],Receive_Data.rx[13]);
Mpu6050_Data.gyros_x_data = IMU_Trans(Receive_Data.rx[14],Receive_Data.rx[15]);
Mpu6050_Data.gyros_y_data = IMU_Trans(Receive_Data.rx[16],Receive_Data.rx[17]);
Mpu6050_Data.gyros_z_data = IMU_Trans(Receive_Data.rx[18],Receive_Data.rx[19]);
Mpu6050.linear_acceleration.x = Mpu6050_Data.accele_x_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.y = Mpu6050_Data.accele_y_data / ACCEl_RATIO;
Mpu6050.linear_acceleration.z = Mpu6050_Data.accele_z_data / ACCEl_RATIO;
Mpu6050.angular_velocity.x = Mpu6050_Data.gyros_x_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.y = Mpu6050_Data.gyros_y_data * GYROSCOPE_RATIO;
Mpu6050.angular_velocity.z = Mpu6050_Data.gyros_z_data * GYROSCOPE_RATIO;
transition_16 = 0;
transition_16 |= Receive_Data.rx[20]<<8;
transition_16 |= Receive_Data.rx[21];
Power_voltage = transition_16/1000+(transition_16 % 1000)*0.001;
return true;
}
}
}
return false;
} |
数据发送
| C++
void origincar_base::Cmd_Vel_Callback(const geometry_msgs::msg::Twist::SharedPtr twist_aux)
{
short transition;
Send_Data.tx[0]=FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
transition=0;
transition = twist_aux->linear.x*1000;
Send_Data.tx[4] = transition;
Send_Data.tx[3] = transition>>8;
transition=0;
transition = twist_aux->linear.y*1000;
Send_Data.tx[6] = transition;
Send_Data.tx[5] = transition>>8;
transition=0;
transition = (twist_aux->angular.z)*1000;
Send_Data.tx[8] = transition;
Send_Data.tx[7] = transition>>8;
Send_Data.tx[9]=Check_Sum(9,SEND_DATA_CHECK);
Send_Data.tx[10]=FRAME_TAIL;
try {
if (akm_cmd_vel == "none") {
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
}
} catch (serial::IOException& e) {
RCLCPP_ERROR(this->get_logger(),("Unable to send data through serial port"));
}
} |
构建机器人ROS 底层驱动
以OriginCar为例，其实完成上下位机的解析与数据封装发送后，ROS相关的驱动就自然而然出来了。大家可以看到在origincar_base 中是小车上位机ROS底层驱动实现的代码，笔者当时写的时候关注的点由两个，第一个哪些数据是之后开发需要的，第二个点是如何让代码变得更好用。
哪些数据是之后开发需要的？
其实主要是传感器数据，例如电压的电压值，对于开发者而言，电压值有可能影响后续功能能否正常使用，如何让开发者不看小车就知道小车电量如何是一件很重要的事情。
| C++
void origincar_base::Publish_Voltage()
{
std_msgs::msg::Float32 voltage_msgs;
static float Count_Voltage_Pub = 0;
if (Count_Voltage_Pub++ > 10) {
Count_Voltage_Pub = 0;
voltage_msgs.data = Power_voltage;
voltage_publisher->publish(voltage_msgs);
}
} |
又好比里程计和IMU值，同样重要，甚至此处可能涉及了里程计和IMU校正的问题，所以自然也需要做一层封装
| C++
void origincar_base::Publish_ImuSensor()
{
sensor_msgs::msg::Imu Imu_Data_Pub;
Imu_Data_Pub.header.stamp = rclcpp::Node::now();
Imu_Data_Pub.header.frame_id = gyro_frame_id;
Imu_Data_Pub.orientation.x = Mpu6050.orientation.x;
Imu_Data_Pub.orientation.y = Mpu6050.orientation.y;
Imu_Data_Pub.orientation.z = Mpu6050.orientation.z;
Imu_Data_Pub.orientation.w = Mpu6050.orientation.w;
Imu_Data_Pub.orientation_covariance[0] = 1e6;
Imu_Data_Pub.orientation_covariance[4] = 1e6;
Imu_Data_Pub.orientation_covariance[8] = 1e-6;
Imu_Data_Pub.angular_velocity.x = Mpu6050.angular_velocity.x;
Imu_Data_Pub.angular_velocity.y = Mpu6050.angular_velocity.y;
Imu_Data_Pub.angular_velocity.z = Mpu6050.angular_velocity.z;
Imu_Data_Pub.angular_velocity_covariance[0] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[4] = 1e6;
Imu_Data_Pub.angular_velocity_covariance[8] = 1e-6;
Imu_Data_Pub.linear_acceleration.x = Mpu6050.linear_acceleration.x;
Imu_Data_Pub.linear_acceleration.y = Mpu6050.linear_acceleration.y;
Imu_Data_Pub.linear_acceleration.z = Mpu6050.linear_acceleration.z;
imu_publisher->publish(Imu_Data_Pub);
}
void origincar_base::Publish_Odom()
{
tf2::Quaternion q;
q.setRPY(0,0,Robot_Pos.Z);
geometry_msgs::msg::Quaternion odom_quat=tf2::toMsg(q);
origincar_msg::msg::Data robotpose;
origincar_msg::msg::Data robotvel;
nav_msgs::msg::Odometry odom;
odom.header.stamp = rclcpp::Node::now();
odom.header.frame_id = odom_frame_id;
odom.child_frame_id = robot_frame_id;
odom.pose.pose.position.x = Robot_Pos.X;
odom.pose.pose.position.y = Robot_Pos.Y;
odom.pose.pose.position.z = 0.0;
odom.pose.pose.orientation = odom_quat;
odom.twist.twist.linear.x = Robot_Vel.X;
odom.twist.twist.linear.y = Robot_Vel.Y;
odom.twist.twist.angular.z = Robot_Vel.Z;
robotpose.x = Robot_Pos.X;
robotpose.y = Robot_Pos.Y;
robotpose.z = Robot_Pos.Z;
robotvel.x = Robot_Vel.X;
robotvel.y = Robot_Vel.Y;
robotvel.z = Robot_Vel.Z;
odom_publisher->publish(odom);
robotpose_publisher->publish(robotpose);
robotvel_publisher->publish(robotvel);
} |
谈到这，再来说如何对里程计和IMU进行校正
大家可以看到这一段：
| C++
void origincar_base::Control()
{
...
Robot_Pos.X+=1.03*(Robot_Vel.X * cos(Robot_Pos.Z) - Robot_Vel.Y * sin(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Y+=1.125*(Robot_Vel.X * sin(Robot_Pos.Z) + Robot_Vel.Y * cos(Robot_Pos.Z)) * Sampling_Time;
Robot_Pos.Z+=Robot_Vel.Z * Sampling_Time;
...
} |
此处对小车具体执行的X、Y、Z进行了控制，大家可以如何解决呢？让小车真实走1m距离，然后观察odom位置是多少，如果存在差异，那么说明控制需要修正。比如此时实际运行的距离是1.023m，而里程计反馈的是1.139m，那么计算线速度的校准系数 = 实际运行距离 / 里程计反馈距离 比如：1.023/1.139 = 0.898；这个值就应该填入以上1.03的位置。
如何让代码变得更好用？
这里其实解决了一个很重要的问题，例如底盘挂掉了，或者你希望一关掉底盘程序车就停下来该如何做。
此处用了一些小ticks，即引入了一个新线程去做调度。
| C++
void sigintHandler(int sig)
{
sig = sig;
printf("OriginCar shutdown...\n");
serial::Serial Stm32_Serial;
Stm32_Serial.setPort("/dev/ttyACM0");
Stm32_Serial.setBaudrate(115200);
serial::Timeout _time = serial::Timeout::simpleTimeout(2000);
Stm32_Serial.setTimeout(_time);
Stm32_Serial.open();
SEND_DATA Send_Data;
if (Stm32_Serial.isOpen()) {
Send_Data.tx[0]=FRAME_HEADER;
Send_Data.tx[1] = 0;
Send_Data.tx[2] = 0;
Send_Data.tx[4] = 0;
Send_Data.tx[3] = 0;
Send_Data.tx[6] = 0;
Send_Data.tx[5] = 0;
Send_Data.tx[7] = 0;
Send_Data.tx[8] = 0;
int check_sum = 0;
for (int k = 0; k < 9; k++) {
check_sum = check_sum^Send_Data.tx[k];
}
Send_Data.tx[9]=check_sum;
Send_Data.tx[10]=FRAME_TAIL;
try {
Stm32_Serial.write(Send_Data.tx,sizeof (Send_Data.tx));
} catch (serial::IOException& e) {
}
}
// 关闭ROS2接口，清除资源
rclcpp::shutdown();
} |
如何开发智能小车的应用功能
基础知识
中间件
ROS
ROS系统是机器人领域最经典的中间件通信框架之一。目前流行的主要开源算法几乎都在ROS上有功能包的实现，如gmapping、teb local planner等等。ROS系统的一大优越性，即是其完备的配套工具。它提供了一个未完成的系统调试时需要的几乎全部的功能，并且完美支持分布式访问。它作为出现较早的机器人领域的主要框架，几乎被应用于各种机器人的研发。而早期的无人驾驶，甚至当前国内部分厂商的低速系统，仍在使用此系统。
由于这个系统最初是为实验性研究所建立的，更注重的是通用性和适应性。所以在性能方面就有所欠缺了。基于xmlrpc的service调用功能速度较差，同时由于缺少 Qos 机制，Topic 的稳定性与质量也难以保证。并且运行时要依赖 RosCore ，一旦 RosCore 出现问题就会造成软件节点崩溃。且又因为 ROS 安装与运行体积又较大，对很多低资源系统会造成负担。
所以，一般认为ROS 适用对稳定性要求较低的，对实时性要求较低的项目。
ROS2
正因为 ROS 具有一些问题，ROS2 便逐渐被提起和研发出来。ROS2与ROS不同，它底层主要是基于DDS进行数据传输。DDS是基于RTPS的去中心化的通信框架，这就去除了对 RosCore 的依赖，使得系统的稳定性及对资源的消耗得到了降低。并且，ROS2 提供了 Qos 机制，对通信的实时性、完整性、历史追溯等功能有了支持。大幅加强了框架功能，避免了在 ROS 上出现的高速系统难以适用等问题。
正因为 ROS2 的强大性，越来越多的开发者正在加入 ROS2 开发的生态中。
Apollo
Apollo是百度所开发的一款类ROS无人驾驶系统。在早期版本（v3.1前）中，Apollo其实是基于ROS开发的一个二级系统，底层通信还是 ROS，但是被包裹成了Apollo App 的形式。在ROS的弊端逐渐被认识到后，百度开始对核心进行改进，对其进行优化。其所做的主要工具与 ROS2 类似，即去中心化，并且通过如内存映射技术等内核技术进行速度优化。目前百度已将其应用在自家生产的各种系统上。
其相对于其他系统的一大优势是，专为无人架驶设计。这一点与 ROS 最初主要为机器人设计是不同的，所以Apollo系统从一开始就主攻高精地图技术，并以此为整个导航系统的高质基础保障。在配以深度网络的识别能力、prediction演算的预测能力、gps/lidar/radar等多传感器的综合定位能力、横纵控制器等的众多功能，实现了一个较精细全面和功能强大的导航能力。仅在无人驾驶这一块，Apollo具有天然优势。
而与之相对的，太过强大的能力自然也就带来了麻烦。由于高度依赖高精地图，使得Apollo系统难以简单使用。而各种方面演算对硬件平台的算力要求也极大。故此系统仅适用于较复杂场景下的大中型车使用。如玩具车、扫地车、快递车等较小型低速车并不是十分适合。
镜像版本
- 镜像版本，大家在使用电脑时一般习惯了看到图形化界面，但是一般而言在机器人开发中为了减少CPU桌面显示损耗让开发板有更好的表现会使用到无图形方式。这就引入了镜像的两个版本，serve版本和desktop版本。
server版推出，就是为了构建7*24小时运行的服务器，故而抛弃了影响性能的界面等，而专注于底层性能。简单来说，server版追求性能，desktop版追求易用。
终端软件
目前常用终端工具有Putty、MobaXterm等，用户可根据自身使用习惯来选择。不同工具的端口配置流程基本类似，下面以MobaXterm为例，介绍新建SSH连接过程：
- 打开MobaXterm工具，点击Session，然后选择SSH
- 输入开发板IP地址，例如192.168.1.10
- 选中specify username，输入root
- 点击OK后，输入用户名（root）、密码（sunrise）即可完成登录
又例如下图中的串口连接。
连接方式
- 连接方式，和开发板的连接方式一般可以分为三种，需要有对应的硬件支持。有线连接、串口连接、无线连接。
- 有线连接一般指通过网口、网线进行PC端和开发板连接，同时需要将PC端静态IP地址与开发板静态IP地址前三位字段调整成一致；
- 无线连接一般指通过WIFI进行连接，需要板载具备WIFI模块，也分为多个频段。目前主流的开发板一般都会搭载2.4GHZ及以上的频段以供开发者使用。
- 串口连接，即RX-TX、TX-RX、GND-GND，一般用于调试，可以理解成只有一个终端的调试工具。
前处理和后处理
- 前处理：因为模型推理的输入是Tensor（多维矩阵）数据，但是正常AI应用的输入都是图片，视频，文字等数据，所以前处理就是要将业务的输入数据（图像，视频，文字等）预先处理成模型推理可以接收的数据即Tensor（多维矩阵）。
- 后处理：将模型推理后的Tensor数据转换成业务可以识别的特征数据；
先简短的和大家谈到以上的一些概念，后续有其他需要补充的概念再和大家交流。
代码编辑软件
以Vscode为例，其支持本地开发代码应用，同时可以通过ssh-remote远程连接开发板。
问题：
| C++
Host key for 192.168.1.10 has changed and you have requested strict checking. > Host key verification failed. |
解决方式：
视觉巡线
机器人巡线是智能机器人比较常见的一个功能，实现的方式也有多种，一般会根据传感器的种类有一些差别。接下来以视觉巡线为例，以OriginCar机器人为载体给大家做介绍。
opencv巡线
OpenCV主要使用C/C++语言编写，执行效率较高，致力于真实世界的实时应用。OpenCV实现了图像处理和计算机视觉方面很多通用算法，这样我们在开发视觉应用的时候，就不需要重新去造轮子，而是基于这些基础库，专注自己应用的优化，同时大家的基础平台一致，在知识传播的时候也更加方便，只要你看得懂OpenCV的函数，就可以很快熟悉别人用OpenCV写的代码，大家交流起来非常方便。
和机器人操作系统一样，一款可以快速传播的开源软件，一般都会选择相对开放的许可证，OpenCV主要采用BSD许可证，我们基于OpenCV写的代码，可以对原生库做修改，不用开源，还可以商业化应用。
OpenCV目前支持的编程语言也非常多，无论你熟悉哪一种，都可以调用OpenCV快速开始视觉开发，比如C ++，Python，Java、MATLAB等语言，而且还支持Windows，Linux，Android和Mac OS等操作系统。
接下来以视觉巡线为例给大家做代码讲解
| Python
import rclpy, cv2, cv_bridge, numpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
class Follower(Node):
def __init__(self):
super().__init__('line_follower')
self.get_logger().info("Start line follower.")
self.bridge = cv_bridge.CvBridge()
self.image_sub = self.create_subscription(Image, '/image_raw', self.image_callback, 10)
self.cmd_vel_pub = self.create_publisher(Twist, 'cmd_vel', 10)
self.pub = self.create_publisher(Image, '/camera/process_image', 10)
self.twist = Twist()
def image_callback(self, msg):
image = self.bridge.imgmsg_to_cv2(msg, 'bgr8')
hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV)
lower_yellow = numpy.array([ 10, 70, 30])
upper_yellow = numpy.array([255, 255, 250])
mask = cv2.inRange(hsv, lower_yellow, upper_yellow)
h, w, d = image.shape
search_top = int(h/2)
search_bot = int(h/2 + 20)
mask[0:search_top, 0:w] = 0
mask[search_bot:h, 0:w] = 0
M = cv2.moments(mask)
if M['m00'] > 0:
cx = int(M['m10']/M['m00'])
cy = int(M['m01']/M['m00'])
cv2.circle(image, (cx, cy), 20, (0,0,255), -1)
# 基于检测的目标中心点，计算机器人的控制参数
err = cx - w/2
self.twist.linear.x = 0.1
self.twist.angular.z = -float(err) / 400
self.cmd_vel_pub.publish(self.twist)
self.pub.publish(self.bridge.cv2_to_imgmsg(image, 'bgr8'))
def main(args=None):
rclpy.init(args=args)
follower = Follower()
rclpy.spin(follower)
follower.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main()
|
深度学习巡线
使用Opencv视觉巡线，我们已经可以让小车跟随黑色的路径线运动，实现了最基础的视觉巡线任务，不过大家可能已经发现，基于OpenCV的图像识别受光线影响较大，更换场地后，就需要重新调整阈值，有没有可能让机器人自主适应环境的变化呢？也就是让机器人自己来学习。
相比传统图像处理，深度学习能够让机器视觉适应更多的变化，从而提高复杂环境下的精确程度。在正式开始之前，我们先简单介绍下深度学习的基本流程。
机器学习的核心目的是要帮助我们解决问题，可以分为六个主要步骤：
- 问题定义：我们要解决的问题是什么？比如这里的视觉巡线，那就要识别线在图像中的位置。
- 数据准备：针对要解决的问题，着手准备数据。比如要准备各种巡线场景的照片，给机器学习使用。
- 模型选择/开发：模型就是处理数据的一套流程，也就是我们常听说的CNN卷积神经网络、GAN生成对抗网络、RNN循环神经网络等等。
- 模型训练与调优：把数据放入模型中，训练得到最优化的参数，可以理解为是机器在学习的过程。
- 模型评估测试：就像小测验一样，我们拿一些数据给训练好的模型，看下效果如何。
- 部署：一切准备就绪之后，就可以把训练好的模型放到机器人上了，也就是正式把知识传授给某一个机器人，它就可以解决之前提出的问题啦。
使用方法
启动机器人
SSH连接OriginCar成功后，在终端中输入如下指令，启动机器人底盘和相机：
| C++
ros2 launch origincar_base origincar_bringup.launch.py #启动底盘
ros2 launch origincar_bringup usb_websocket_display.launch.py #启动相机 |
启动视觉巡线节点
| Bash
ros2 run line_follower_perception line_follower_perception |
结果展示
[媒体1.mp4]
部署方法
启动相机节点
首先启动OriginCar智能机器人相机节点
| Bash
#bash指令
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
ros2 launch origincar_bringup camera.launch.py |
| Python
#camera.launch.py
from launch import LaunchDescription
from launch_ros.actions import Node
def generate_launch_description():
return LaunchDescription([
Node(
package='hobot_usb_cam',
executable='hobot_usb_cam',
name='hobot_usb_cam',
parameters=[
{"camera_calibration_file_path": "/opt/tros/lib/hobot_usb_cam/config/usb_camera_calibration.yaml"},
{"frame_id": "default_usb_cam"},
{"framerate": 30},
{'image_width': 640},
{'image_height': 480},
{"io_method": "mmap"},
{"pixel_format": "mjpeg"},
{"video_device": "/dev/video8"},
{"zero_copy": False}
],
arguments=['--ros-args', '--log-level', 'error']
),
Node(
package='hobot_codec',
executable='hobot_codec_republish',
output='screen',
parameters=[
{"channel": 1},
{"in_mode": "ros"},
{"in_format": "jpeg"},
{"out_mode": "ros"},
{"out_format": "bgr8"},
{"sub_topic": "/image"},
{"pub_topic": "/image_raw"}
],
arguments=['--ros-args', '--log-level', 'error']
)
]) |
启动数据采集与标注程序
在PC端，通过如下指令，启动line_follower_model包中的数据采集与标注程序，
| Bash
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
cd ~/dev_ws/src/origincar_desktop/origincar_deeplearning/line_follower_model
ros2 run line_follower_model annotation |
| Bash
# annotation.py
import rclpy
from rclpy.node import Node
from cv_bridge import CvBridge
import cv2 as cv
import uuid
import os
import numpy as np
from sensor_msgs.msg import Image
class ImageSubscriber(Node):
def __init__(self):
super().__init__('ImageSubscriber')
# 创建sub节点，订阅image_raw话题
self.subscription = self.create_subscription(
Image,
'image_',
self.listener_callback,
1)
# 创建CvBridge实例
self.bridge = CvBridge()
self.x = -1
self.y = -1
self.uuid = -1
self.image = np.zeros((640, 224, 3))
self.initialize = False
# 检查用户存放标定数据的image_dataset文件夹是否存在，如果不存在则创建
if not os.path.exists('./image_dataset'):
os.makedirs('./image_dataset')
# 设置opecv展示窗属性
cv.namedWindow("capture image", cv.WINDOW_NORMAL)
cv.resizeWindow("capture image", 640, 224)
self.subscription
# 鼠标事件，当左键按下则在对应坐标处画圈，表示标记的位置
def mouse_callback(self, event, x, y, flags, userdata):
if event == cv.EVENT_LBUTTONDOWN:
imageWithCircle = userdata.copy()
self.x = x
self.y = y
cv.circle(imageWithCircle, (x,y), 5, (0,0,255), -1)
cv.imshow("capture image", imageWithCircle)
# sub回调函数
def listener_callback(self, msg):
keyValue = cv.waitKey(1)
# 检测到按下回车键，则获取一张新的图像
if keyValue == 13:
captureImage = self.bridge.imgmsg_to_cv2(msg)
cropImage = captureImage[128:352,:,:].copy()
if self.initialize == False:
self.image = cropImage.copy()
self.initialize = True
# 注册鼠标回调
cv.setMouseCallback("capture image", self.mouse_callback, cropImage)
cv.imshow("capture image", cropImage)
if self.x != -1 and self.y != -1:
self.uuid = 'xy_%03d_%03d_%s' % (self.x, self.y, uuid.uuid1())
# 保存上一次标定的结果
cv.imwrite('./image_dataset/' + self.uuid + '.jpg', self.image)
# 载入新的图像
self.image = cropImage.copy()
self.x = -1
self.y = -1
def main(args=None):
rclpy.init(args=args)
image_subscriber = ImageSubscriber()
rclpy.spin(image_subscriber)
image_subscriber.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main() |
数据采集与标注
启动成功后，按下键盘上回车键，程序会订阅最新图像话题，剪裁后通过一个可视化窗口显示出来，数据采集成功：
用鼠标左键点击画面垂直方向上路径线的中心处，即完成该帧图像数据的标注：
按下回车键，程序自动保存该图片至当前路径下的image_dataset文件夹中，并且保存标记结果。图片命名方式为：
xy_[x坐标]_[y坐标]_[uuid].jpg
其中uuid为图片唯一标志符，避免出现文件名称相同。
不断调整机器人的在场景中的位置，考虑各种可能出现的图像效果，循环完成以上数据采集和标注过程，采集足够数量的数据，建议至少100张，用于后续模型训练。当环境或者场地变化时也可采集对应的图片一起训练提高模型的适应性。
模型选择
卷积神经网络（Convolutional Neural Network, CNN）是一种目前广泛用于图像，自然语言处理等领域的深度神经网络模型。1998 年，Lecun 等人提出了一种基于梯度的反向传播算法用于文档的识别。在这个神经网络中，卷积层 (Convolutional Layer) 扮演着至关重要的角色。随着运算能力的不断增强，一些大型的 CNN 网络开始在图像领域中展现出巨大的优势，2012 年Krizhevsky等人提出了AlexNet网络结构，并在ImageNet图像分类竞赛中以超过之前11%的优势取得了冠军。随后不同的学者提出了一系列的网络结构并不断刷新ImageNet的成绩，其中比较经典的网络包括：VGG(Visual Geometry Group) ，GoogLeNet和ResNet。卷积神经网络由输入层、卷积层、池化层、全连接层以及输出层组成，其结构如下：
综合考虑模型成熟度、训练模型对CPU/GPU的硬件要求，这里选择ResNet网络作为backbone。残差神经网络（ResNet）是由微软研究院的何恺明、张祥雨、任少卿、孙剑等人提出，在2015 年的ILSVRC（ImageNet Large Scale Visual Recognition Challenge）中取得了冠军。ResNet巧妙地利用了shortcut连接，解决了深度网络中模型退化的问题，是当前应用最为广泛的CNN特征提取网络之一。ResNet18结构如下：
模型训练
| C++
ros2 run line_follower_model training |
| Bash
# Copyright (c) 2022，Horizon Robotics.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import torch
import torch.optim as optim
import torch.nn.functional as F
import torchvision.models as models
import torchvision.transforms as transforms
import glob
import PIL.Image
import os
import numpy as np
# 创建一个torch.utils.data.Dataset的实现。因为模型输入为224*224，图像分辨率为640*224所以X方向坐标需要缩放
def get_x(path, width):
"""Gets the x value from the image filename"""
return (float(int(path.split("_")[1])) * 224.0 / 640.0 - width/2) / (width/2)
def get_y(path, height):
"""Gets the y value from the image filename"""
return ((224 - float(int(path.split("_")[2]))) - height/2) / (height/2)
class XYDataset(torch.utils.data.Dataset):
def __init__(self, directory, random_hflips=False):
self.directory = directory
self.random_hflips = random_hflips
self.image_paths = glob.glob(os.path.join(self.directory, '*.jpg'))
self.color_jitter = transforms.ColorJitter(0.3, 0.3, 0.3, 0.3)
def __len__(self):
return len(self.image_paths)
def __getitem__(self, idx):
image_path = self.image_paths[idx]
image = PIL.Image.open(image_path)
x = float(get_x(os.path.basename(image_path), 224))
y = float(get_y(os.path.basename(image_path), 224))
if self.random_hflips:
if float(np.random.rand(1)) > 0.5:
image = transforms.functional.hflip(image)
x = -x
image = self.color_jitter(image)
image = transforms.functional.resize(image, (224, 224))
image = transforms.functional.to_tensor(image)
image = image.numpy().copy()
image = torch.from_numpy(image)
image = transforms.functional.normalize(image,
[0.485, 0.456, 0.406], [0.229, 0.224, 0.225])
return image, torch.tensor([x, y]).float()
def main(args=None):
# 需要根据自己的环境改为数据集存放位置
dataset = XYDataset('./image_dataset', random_hflips=False)
# 创建训练集和测试集
test_percent = 0.1
num_test = int(test_percent * len(dataset))
train_dataset, test_dataset = torch.utils.data.random_split(dataset, [len(dataset) - num_test, num_test])
train_loader = torch.utils.data.DataLoader(
train_dataset,
batch_size=24,
shuffle=True,
num_workers=0
)
test_loader = torch.utils.data.DataLoader(
test_dataset,
batch_size=24,
shuffle=True,
num_workers=0
)
# 创建ResNet18模型，这里选用已经预训练的模型，
# 更改fc输出为2，即x、y坐标值
model = models.resnet18(pretrained=True)
model.fc = torch.nn.Linear(512, 2)
device = torch.device('cpu')
model = model.to(device)
NUM_EPOCHS = 100
BEST_MODEL_PATH = './best_line_follower_model_xy.pth'
best_loss = 1e9
optimizer = optim.Adam(model.parameters())
for epoch in range(NUM_EPOCHS):
model.train()
train_loss = 0.0
for images, labels in iter(train_loader):
images = images.to(device)
labels = labels.to(device)
optimizer.zero_grad()
outputs = model(images)
loss = F.mse_loss(outputs, labels)
train_loss += float(loss)
loss.backward()
optimizer.step()
train_loss /= len(train_loader)
model.eval()
test_loss = 0.0
for images, labels in iter(test_loader):
images = images.to(device)
labels = labels.to(device)
outputs = model(images)
loss = F.mse_loss(outputs, labels)
test_loss += float(loss)
test_loss /= len(test_loader)
print('%f, %f' % (train_loss, test_loss))
if test_loss < best_loss:
print("save")
torch.save(model.state_dict(), BEST_MODEL_PATH)
best_loss = test_loss
if __name__ == '__main__':
main() |
模型转换
pytorch训练得到的浮点模型如果直接运行在RDK X3上效率会很低，为了提高运行效率，发挥BPU的5T算力，这里需要进行浮点模型转定点模型操作。
生成onnx模型
在PC端，通过如下命令，使用generate_onnx将之前训练好的模型，转换成onnx模型：
| Plaintext
$ ros2 run line_follower_model generate_onnx |
| Python
# Copyright (c) 2022，Horizon Robotics.
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
# http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
import torchvision
import torch
def main(args=None):
model = torchvision.models.resnet18(pretrained=False)
model.fc = torch.nn.Linear(512,2)
model.load_state_dict(torch.load('./best_line_follower_model_xy.pth'))
device = torch.device('cpu')
model = model.to(device)
model.eval()
x = torch.randn(1, 3, 224, 224, requires_grad=True)
torch_out = model(x)
torch.onnx.export(model,
x,
"./best_line_follower_model_xy.onnx",
export_params=True,
opset_version=11,
do_constant_folding=True,
input_names=['input'],
output_names=['output'])
if __name__ == '__main__':
main() |
AI工具链转换
[2024年05月09日 屏幕视频 16时27分29秒.mp4]
模型部署
通过前面模型转换已经得到可以在RDK X3BPU上运行的定点模型，如何将其部署在RDK X3上，实现图像获取、模型推理、运动控制整套功能呢？这里基于TogetheROS中的Hobot DNN实现。Hobot DNN是TogetheROS软件栈中的板端算法推理框架，在地平线RDK X3上利用BPU处理器实现AI推理功能，为机器人应用开发提供更简单易用的模型集成开发接口，包括模型管理、基于模型描述的输入处理及结果解析，以及模型输出内存分配管理等功能。
https://gitee.com/guyuehome/originbot/tree/master/originbot_deeplearning/line_follower_perception
目标检测
介绍
目标检测是计算机视觉领域的核心问题之一，其任务是给定一张图片或者是一个视频帧，让计算机定位出这个目标的位置并且知道目标物是什么，即输出目标的Bounding Box(边框)以及标签。
目标检测的定义为：目标检测时图片中对可变数量的目标进行查找和分类。
需要解决的问题包括：1、目标种类与数量；2、目标尺度问题：目标稠密、目标大小等3、外在环境干扰问题：遮挡、光照。
由于各类不同物体有不同的外观，姿态，以及不同程度的遮挡，加上成像是光照等因素的干扰，目标检测一直以来是一个很有挑战性的问题。目标检测的方法有传统方法，也有深度学习的方法。传统方法需要手动设计特征，使用滑动窗口和传统分类器，准确性和实时性比较差。深度学习方法通过深度网络学习特征，Proposal或者直接回归，使用深度网络，支持端到端，准确性高和实时性比较好。
传统的目标检测方法包括：Viola-Jones（采用积分图特征+AdaBoost方法对人脸进行检测等）；HOG+SVM（主要用于行人检测，通过对行人目标候选区域提取HOG特征，并结合SVM分类器来进行判定）；DPM（是基于HOG特征检测的一种变种，不同的地方在于，DPM中会加入一些额外的策略，来提升检测的精度，而DPM方法是目前非深度学习方法，检测效果最好的，性能最优。在后续的深度学习方法中会用到很多DPM算法中提到的策略，例如包围框策略等）等等。
深度学习方法主要分为两类：一阶段（One Stages）的目标检测算法和两阶段（Two Stages）的目标检测算法。一阶段的目标检测算法直接对输入图像应用算法并输出类别和相应的定位，如YOLO、SSD等。二阶段的目标检测算法先产生候选区域然后进行CNN分类，如R-CNN，Fast R-CNN，Faster R-CNN，Mask R-CNN等。两阶段的目标检测算法精度较高，但速度较慢；一阶段的目标检测算法速度较快，但精度相对较低。
接下来介绍如何在RDK X3上部署YOLOV5目标检测算法：
图像采集与标注
https://gitee.com/xiaobairisk/yolov5-for-rdkx3
| Python
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from cv_bridge import CvBridge
import glob
import os
class ImageAnnotationNode(Node):
def __init__(self):
super().__init__('image_annotation_node')
self.cv_bridge = CvBridge()
self.image_subscriber = self.create_subscription(
Image,
'image_out',
self.image_callback,
10
)
self.current_frame = None
self.annotation_data = []
self.enable_annotation = False
self.locked_frame = None
self.save_data = SaveDate()
# 标注是否开始
self.annotation_flag = 1
def image_callback(self, msg):
if self.annotation_flag == 1:
self.current_frame = self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="bgr8")
self.locked_frame = cv2.copyTo(self.current_frame, None)
self.h = self.current_frame.shape[0]
self.w = self.current_frame.shape[1]
else:
pass
cv2.imshow('Image', self.current_frame)
cv2.setMouseCallback('Image', self.mouse_callback)
def mouse_callback(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
# 正在标注图像，不再刷新
self.annotation_flag = 0
# 鼠标按下，记录起始点
self.start_point = (x, y)
elif event == cv2.EVENT_LBUTTONUP:
# 鼠标释放，记录结束点，并绘制矩形框
self.end_point = (x, y)
self.current_frame = cv2.rectangle(self.current_frame, self.start_point, self.end_point, (0, 255, 0), 2)
cv2.imshow('Image', self.current_frame)
# 获取标注的类别
label = self.get_current_label()
if label == 'erro':
print("erro class!")
else:
center_point_x = ((self.start_point[0] + self.end_point[0]) / 2) / self.w
center_point_y = ((self.start_point[1] + self.end_point[1]) / 2) / self.h
bbox_w = (abs(self.start_point[0] - self.end_point[0])) / self.w
bbox_h = (abs(self.start_point[1] - self.end_point[1])) / self.h
annotation = [label, center_point_x, center_point_y, bbox_w, bbox_h]
print(annotation)
self.annotation_data.append(annotation)
def get_current_label(self):
key = cv2.waitKey(0)
if key == ord('1'):
return '1'
elif key == ord('2'):
return '2'
elif key == ord('3'):
return '3'
elif key == ord('4'):
return '4'
else:
return 'erro'
def run(self):
while rclpy.ok():
# self.annotation_data = []
rclpy.spin_once(self)
key = cv2.waitKey(1)
if key == ord('q'):
break
elif key == 13:
self.save_annotation_data()
self.annotation_flag = 1
print(f"No.{self.save_data.get_index()} data has been saved!")
self.annotation_data = []
cv2.imshow('Image', self.current_frame)
def save_annotation_data(self):
self.save_data.save_data(self.locked_frame, self.annotation_data)
class SaveDate():
def __init__(self) -> None:
self.path_image = "/home/lql/yolo/dataset/images"
self.path_label = "/home/lql/yolo/dataset/labels"
self.index = len(glob.glob(os.path.join(self.path_image, '*.jpg')))
assert len(glob.glob(os.path.join(self.path_image, '*.jpg'))) == len(glob.glob(os.path.join(self.path_label, '*.txt')))
def save_data(self, image, label):
self.index = self.index + 1
image_name = self.path_image + "/" + str(self.index).zfill(4) + ".jpg"
txt_name = self.path_label + "/" + str(self.index).zfill(4) + ".txt"
cv2.imwrite(image_name, image)
open_txt = open(txt_name, "w", encoding="utf-8")
for item in label:
class_id = item[0]
x = item[1]
y = item[2]
w = item[3]
h = item[4]
line = f"{class_id} {x} {y} {w} {h}\n"
open_txt.write(line)
open_txt.close()
def get_index(self):
return self.index
def main(args=None):
rclpy.init(args=args)
node = ImageAnnotationNode()
node.run()
node.destroy_node()
rclpy.shutdown()
if __name__ == '__main__':
main() |
模型训练
https://developer.horizon.cc/forumDetail/198686198578007656
模型部署
[附录_yolo模型转换.mp4]
可能存在的问题：
| C++
此处使用的是colob环境，使用cpu时可以直接运行
使用gpu会遇到，如下改正即可
#报错：TypeError: can't convert cuda:0 device type tensor to numpy. Use Tensor.cpu() to copy the tensor to host memory first.
根据错误信息提示的路径：你电脑的路径\yolo_v5\lib\site-packages\torch\_tensor.py 的第643、645行，将self.numpy()修改为self.cpu().numpy()如下
if dtype is None:
# return self.numpy()
return self.cpu().numpy()
else:
# return self.numpy().astype(dtype, copy=False)
return self.cpu().numpy().astype(dtype, copy=False)
```
#报错：File "/content/drive/MyDrive/yolov5_2.0/yolov5-2.0/utils/utils.py", line 533, in build_targets
a, t = at[j], t.repeat(na, 1, 1)[j] # filter
RuntimeError: indices should be either on cpu or on the same device as the indexed tensor (cpu)
此外在util.py中需要打开注释，或者使用util_gpu.py可解决
#使用gpu时注释
#at = at.to('cuda')
#j = j.to('cuda')
使用指令：
环境测试：!python3 detect.py --source ./inference/images/ --weights yolov5s.pt --conf 0.4
训练指令：!python3 train.py --img 672 --batch 16 --epochs 1 --data data/data.yaml --weights yolov5s.pt |
附录资料
RDK X3 资源参考
地平线机器人社区
https://developer.horizon.cc/
- 地平线RDK套件：https://developer.horizon.cc/documents_rdk/
- TogetheROS.Bot：https://developer.horizon.cc/documents_tros/
- NodeHub：https://developer.horizon.cc/nodehub
古月居机器人社区
https://www.guyuehome.com
- 博客：https://www.guyuehome.com/index/search
- 项目：https://www.guyuehome.com/project
- 泡泡：https://www.guyuehome.com/bubble/index
- 古月学院：https://class.guyuehome.com/
- originbot :http://originbot.org/
带AI工具链PC端镜像
链接：https://pan.baidu.com/s/1ZhvWKfzaW7Vlu2LKeh93yA?pwd=gyh1
提取码：gyh1
镜像密码: ros
进入docker ，创建data文件，将run_docker.sh换成以下文件
[run_docker.sh]
地平线 hobot_dnn框架：https://github.com/HorizonRDK/hobot_dnn
OriginCar快速上手指南
OriginCar快速上手指南
3万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
