移动机器人控制系统搭建

一、 移动机器人底盘结构介绍

对于一个移动机器人底盘来说，所要提供的核心功能有两个-感知和执行能力，我们分别进行介绍。

1. 感知-传感器

所谓感知即通过给类传感器获取环境信息的能力。在移动机器人中，我们常用的传感器有

• 距离传感器——雷达、超声波
• 轮子速度传感器——编码器
• 惯性测量单元——IMU传感器
• 图像传感器——单目、双目、深度摄像头

除了上面几类之外，你也可以根据需求挂载其他你需要的传感器，比如温湿度测量传感器等。

对于我们的移动机器人来说，搭载的传感器有雷达、相机、编码器和IMU四种。

1.1 雷达

我们采用的激光雷达是单线旋转式三角测距激光雷达LD14。

LD14主要由激光测距核心，无线传电单元，无线通讯单元，角度测量单元、电机驱动单元和机械外壳组成。 LD14测距核心采用三角测量法技术，可进行每秒2300次的测距。每次测距时，LD14从一个固定的角度发射出红外激光，激光遇到目标物体后被反射到接收单元。通过激光、目标物体、接收单元形成的三角关系，从而解算出距离。 获取到距离数据后，LD14 会融合角度测量单元测量到的角度值组成点云数据，然后通过 无线通讯将点云数据发送到外部接口。同时电机驱动单元会驱动电机，通过PID算法闭环控制到指定的转速。此款雷达的转接板可以用来控制激光雷达的启停，不用时可通过串口通信来关闭雷达，可延长雷达的使用寿命。

**雷达测距原理介绍**

在雷达的头部分别有一个激光发射头和线性的CCD接收头。

右边黑色的是发射头，左边是CCD接收头，发射头发射出的光属于波长大约在800nm左右的红外光，肉眼是不可见的。

LD14激光雷达的测距原理是三角测距法

从上图可以看出，当我们已知H（机械安装值）和d1（CCS测量值）的情况下，我们可以得到D的值，即激光雷达到被测物体的距离。

激光头通过不断旋转，这样就可以测量出360度的深度信息。

1.2 霍尔编码器

在移动机器人中我们需要实时的获取到机器人各个轮子的转速，通过转速根据机器人的运动学模型将轮子的速度转换成机器人的速度，通过对速度进行积分（速度*距离）得到机器人行走的距离。

我们如何对轮子速度的测量所使用的传感器就是编码器，我们采用的是AB电磁编码器。

编码器是一种将角位移或者角速度转换成一连串电数字脉冲的旋转式传感器，我们可以通过编码器测量到底位移或者速度信息。其中，霍尔编码器通过检测磁性目标的运动来生成位置和速度信息，具体步骤如下：

• 磁场变化检测：
  当磁性目标（如磁环或磁铁）随轴旋转时，霍尔传感器检测到磁场的变化。

• 霍尔效应生成信号：
  霍尔传感器将磁场变化转换为电压信号。这些电压信号的波形取决于磁性目标的极性和旋转速度。

• 信号处理：
  信号处理电路将霍尔传感器生成的模拟信号处理为数字脉冲信号。这些脉冲信号的频率与旋转速度成正比，脉冲的数量与旋转角度成正比。

• 输出信号：
  处理后的数字脉冲信号通过输出接口传输到单片机。单片机根据这些信号计算转速、位置和方向。

电磁编码器是由1和2两个霍尔传感器和圆形磁铁3共同组成的，该磁铁的磁性是间隔分布的，磁铁固定在电机的转子上，当电机转动时，带动磁铁转动，此时用于检测磁性的霍尔传感器就会检测到磁性的变化，从而就可以测量出电机在某段时间内转了多少圈即电机的转速。

**霍尔编码器原理简介:**

• 霍尔编码器圆盘上分布有磁极，当圆盘随电机主轴转动时，会输出两路相位差90°的方波，用这两路方波可测出电机的转速和转向。
• 霍尔编码器一般是13线的，就是转一圈每项会输出13个脉冲，这个精度基本能够满足大部分使用场景的要求。
• E1A和E1B是13线霍尔编码器的两个输出信号，用于输出轴旋转方向和位置信息，E1A是正交方波信号，当轴逆针旋转时，E1A会先于E1B产生信号变化，用于判断旋转方向。E1B是正交方波信号，当轴顺时针旋转时，E1B会先
• E1A产生信号变化，用于判断旋转方向。
• 两个信号数量相同，相位差90度，可以通过对信号的计数和相位差计算来确定轴的位置。

1.3 IMU传感器

MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件，内带3轴陀螺仪和3轴加速度传感器（3+3=6，因此我们常说是6轴传感器，如果再加上个3轴磁力计，就一共是9轴传感器），并且含有一个第二IIC接口（XCL和XDA），可用于连接外部磁力传感器，利用自带数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主IIC接口，可以向应用端输出完整的9轴姿态融合演算数据。有了DMP，我们可以使用InvenSense公司提供的运动处理资料库，非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度 。

MPU6050具有如下特性：

① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧

拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）

② 具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec

的 3 轴角速度感测器(陀螺仪)

③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器

④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移

⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算

数据、感测器同步化、姿势感应等的负荷

⑥ 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求

⑦ 自带一个数字温度传感器

⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS

⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降

中断、high-G 中断、零动作感应、触击感应、摇动感应功能

⑩ VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC 可低至 1.8V± 5%

⑪ 陀螺仪工作电流：5mA，陀螺仪待机电流：5uA；加速器工作电流：500uA，加速器省

电模式电流：40uA@10Hz

⑫ 自带 1024 字节 FIFO，有助于降低系统功耗

⑬ 高达 400Khz 的 IIC 通信接口

⑭ 超小封装尺寸：4x4x0.9mm（QFN）

2. 执行器

所谓执行器就是负责动的部件，在移动机器人上，最重要的一个执行器就是电机了

我们采用的是一个额定电压7.4V的310减速电机，额定转速为400±13%RPM、额定电流220M
A，转矩0.4Kgf.cm

减速电机指的是带减速器的电机。

电机一般由定子和转子组成的，一般转速都比较快，但输出的力矩比较小，所以我们会给电机配备减速器，让转速降低，提高力矩。

3. 其他配件

除了执行器和传感器外，还有一些必要配件

• 控制器ESP32
• 电池，提供电力支持
• 稳压模块，提供电压转换
• 万向轮等必要支撑结构

二、电机驱动原理介绍

MG310 直流减速电机原理

1. 直流电机原理

直流电机（DC Motor）的基本原理是基于电磁感应和电磁力。其工作原理可以归结为利用电流通过线圈在磁场中产生力，从而驱动电机转动。以下是直流电机的详细原理和工作机制：

• 定子（Stator）：
  永磁体或电磁体，产生静态磁场。

• 转子（Rotor）：
  也称为电枢（Armature），通常是一个带有线圈的铁芯，位于定子磁场中，并且能够自由转动。

• 换向器（Commutator）：
  半圆形导体片，连接在转子轴上，与电刷接触，用于切换电流方向，确保转矩方向恒定。

• 电刷（Brushes）：
  通常由碳材料制成，固定在电机外壳上，与换向器接触，将外部电源的电流传递到转子线圈。

当直流电流通过电刷和换向器进入转子线圈时，依据右手定则，电流在磁场中会产生洛伦兹力。电流方向和磁场方向决定了力的方向，具体如下：

1）电流通过线圈： 线圈的一部分电流从换向器和电刷流入，流经线圈，然后通过另一组换向器和电刷流出。
2）力的产生： 根据弗莱明左手定则，在磁场中的电流承受洛伦兹力，力的方向与电流方向和磁场方向成垂直。在线圈的两边产生相反方向的力，形成一个转矩，使转子旋转。
3）换向器的作用： 随着转子旋转，换向器不断地切换电流方向，以保持转子线圈中电流方向相同，从而维持持续的转矩方向。

2. 减速器

减速器是机械传动系统中的一种重要部件，其主要作用是降低转速并增加转矩。

• 降低转速：
  减速器的主要功能是将输入轴的高速旋转转变为输出轴的低速旋转。这是通过齿轮传动来实现的，输入轴连接到高速齿轮，通过多个齿轮级的传动，输出轴的转速被降低。
• 增加转矩：
  当转速降低时，输出轴的转矩（扭矩）会相应增加。这是由于功率守恒定律，即在理想情况下，输入功率等于输出功率。减速器通过增加输出转矩，使得负载能被有效驱动。

从上图可知，直流电机其实只有两个线（最边上两条），排线中间的四根线是编码器，只是用于测速，和直流电机本身没有联系，在开环控制时不需要使用，在闭环控制时使用。

三、电机控制之正反转实验

1. 新建工程

新建example09_motor_direction_control

2. 编写代码

根据第前面硬件控制章节学到的内容可知，控制IO电平只需要使用pinMode和digitalWrite相关函数即可。

#include <Arduino.h>

#define AIN1 12 // 电机驱动模块AIN1引脚
#define AIN2 13 // 电机驱动模块AIN2引脚
#define KEY 0   // 按键引脚

int motorStatus = 0; // 电机状态变量，0-3循环变化

void setup()
{
    
    
  Serial.begin(115200);    // 初始化串口通信
  pinMode(KEY, INPUT);     // 设置按键引脚为输入模式
  pinMode(AIN1, OUTPUT);   // 设置AIN1引脚为输出模式
  pinMode(AIN2, OUTPUT);   // 设置AIN2引脚为输出模式
}

void loop()
{
    
    
  if (digitalRead(KEY) == LOW) // 检测按键是否按下
  {
    
    
    delay(50);                 // 延迟50ms，以防止误触
    if (digitalRead(KEY) == LOW)
    {
    
    
      while (digitalRead(0) == LOW) // 等待按键松开，避免连续按下
        ;
      motorStatus++;                 // 切换电机状态
      motorStatus = motorStatus % 4; // 保持该变量值在0-4之间
    }
  }

  // 根据电机状态切换IO电平
  switch (motorStatus)
  {
    
    
    case 0:
      Serial.println("AIN1: HIGH, AIN2: LOW"); // 调试信息：AIN1为高电平，AIN2为低电平
      digitalWrite(AIN1, HIGH);
      digitalWrite(AIN2, LOW);
      break;
    case 1:
      Serial.println("AIN1: LOW, AIN2: HIGH"); // 调试信息：AIN1为低电平，AIN2为高电平
      digitalWrite(AIN1, LOW);
      digitalWrite(AIN2, HIGH);
      break;
    case 2:
      Serial.println("AIN1: HIGH, AIN2: HIGH"); // 调试信息：AIN1和AIN2均为高电平
      digitalWrite(AIN1, HIGH);
      digitalWrite(AIN2, HIGH);
      break;
    case 3:
      Serial.println("AIN1: LOW, AIN2: LOW"); // 调试信息：AIN1和AIN2均为低电平
      digitalWrite(AIN1, LOW);
      digitalWrite(AIN2, LOW);
      break;
    default:
      break;
  }
}

代码解释

在setup函数中，通过pinMode函数将KEY、AIN1和AIN2引脚设置为对应的输入输出模式。

在loop函数中，首先通过digitalRead函数检测按键是否按下，如果检测到按键按下，则会将电机状态变量motorStatus加1，然后通过switch语句根据电机状态改变AIN1和AIN2引脚的电平状态。同时，还会通过Serial.println函数将AIN1和AIN2引脚的电平状态输出到串口，方便调试。

3. 测试

将代码下载到ESP32主控板上，点击按键，查看轮子转动效果。
你可以发现，当AIN1和AIN2同时为高电平或者同时为低时，电机并不会转动.

四、电机控制之速度控制实验

我们可以通过改变PWM的占空比可以实现对输出电压的大小调节。占空比越大，输出电压越高；占空比越小，输出电压越低。

1. 新建工程

新建example10_motor_speed_control

2. 编写代码

程序的基本思路是，通过检测按键输入来改变占空比的大小，从而控制电机的转速。按下按键后，每次增加0.1的占空比，当占空比达到1.0时，重新从0开始计数。在loop函数中，通过控制AIN1引脚的高低电平来实现PWM信号的输出，从而控制电机的速度。

#include <Arduino.h>

#define AIN1 12  // 电机驱动模块AIN1引脚
#define AIN2 13  // 电机驱动模块AIN2引脚
#define KEY 0    // 按键引脚
#define CYCLE 10 // 定义PWM信号的周期长度，单位为ms

float duty = 0.0; // 定义占空比变量，并初始化为0.0

void setup()
{
    
    
  Serial.begin(115200);   // 初始化串口通信
  pinMode(KEY, INPUT);    // 设置按键引脚为输入模式
  pinMode(AIN1, OUTPUT);  // 设置AIN1引脚为输出模式
  pinMode(AIN2, OUTPUT);  // 设置AIN2引脚为输出模式
  digitalWrite(AIN2, LOW);// 设置AIN2引脚为低电平，控制电机转向
}

void loop()
{
    
    
  // 检测按键是否按下
  if (digitalRead(KEY) == LOW) 
  {
    
    
    delay(50); // 延迟50ms，以防止误触
    // 确认按键已经按下
    if (digitalRead(KEY) == LOW)
    {
    
    
      // 等待按键松开，避免连续按下
      while (digitalRead(0) == LOW) 
        ;
      // 每次增加0.1的占空比，当占空比达到1.0时，重新从0开始计数
      duty = duty + 0.1;
      if (duty > 1.0)
        duty = 0;
    }
  }

  // 输出PWM信号控制电机转速
  digitalWrite(AIN1, HIGH);     // 将AIN1引脚设置为高电平
  delay(CYCLE * duty);           // 延迟一段时间，时间长度由占空比决定
  digitalWrite(AIN1, LOW);      // 将AIN1引脚设置为低电平
  delay(CYCLE * (1 - duty));     // 延迟一段时间，时间长度由占空比决定
}

3. 测试

将代码下载到主控板上，点击BOOT按键，观察电机转速。

本节我们通过简单的一个实验学习了如何通过PWM调节电机的PWM，但有一点需要注意，程序中使用了delay函数来控制PWM信号的占空比，这种方法在简单的应用场景下是可行的，但是在需要更高精度的控制场景下可能会产生问题。为了实现更高精度的PWM控制，我们可以采用ESP32的电机PWM控制单元，下一节我们就尝试使用这一开源库实现更精细化的控制。

五、电机控制之使用开源库驱动多路电机

本节我们采用开源库调用ESP32的外设MCPWM进行精细化的电机PWM控制。

1. MCPWM简介

MCPWM中文名是电机控制脉宽调制器 （Motor Control Pulse Width Modulator ），是一款多功能 PWM 发生器，包含各种子模块，使其成为电机控制、数字电源等电力电子应用的关键元件。MCPWM 外设可用于以下场景：

• 数字电机控制，例如有刷/无刷直流电机、RC 伺服电机
• 基于开关模式的数字电源转换
• 功率DAC，其中占空比相当于DAC模拟值
• 计算外部脉冲宽度，并将其转换为其他模拟值，如速度、距离
• 为磁场定向控制 （FOC） 生成空间矢量 PWM （SVPWM） 信号

这里只需要了解MCPWM可以用来做什么就足够了。

2. 新建工程并添加依赖

新建example11_mcpwm_control

在platformio.ini添加依赖

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
lib_deps = 
	https://github.com/fishros/Esp32McpwmMotor.git

这里使用的驱动库是小鱼开源的Esp32McpwmMotor，支持12路PWM输出，可以同时控制6个直流电机，所以对于只有两个驱动轮的FishBot来说绰绰有余。

3. 编写代码

#include <Arduino.h>
#include <Esp32McpwmMotor.h>

Esp32McpwmMotor motor; // 创建一个名为motor的对象，用于控制电机

void setup()
{
    
    
    Serial.begin(115200); // 初始化串口通信，波特率为115200

    motor.attachMotor(0, 12, 13); // 将电机0连接到引脚23和引脚22
    motor.attachMotor(1, 14, 27); // 将电机1连接到引脚12和引脚13
}

void loop()
{
    
    
    motor.updateMotorSpeed(0, -70); // 设置电机0的速度(占空比)为负70%
    motor.updateMotorSpeed(1, 70); // 设置电机1的速度(占空比)为正70%
    delay(2000); // 延迟两秒

    motor.updateMotorSpeed(0, 70); // 设置电机0的速度(占空比)为正70%
    motor.updateMotorSpeed(1, -70); // 设置电机1的速度(占空比)为负70%
    delay(2000); // 延迟两秒
}

上面这段代码是用于控制两个电机进行正反转的程序。其中使用了Esp32McpwmMotor库来控制电机，该库提供了一些常用的控制函数，比如attachMotor()用于连接电机，updateMotorSpeed()用于更新电机速度。

在setup()函数中，首先通过Serial.begin()函数初始化串口通信，然后通过motor.attachMotor()函数将两个电机连接到指定的引脚。

在loop()函数中，通过motor.updateMotorSpeed()函数分别控制电机0和电机1的速度。每次调用该函数时，第一个参数是电机的编号，第二个参数是电机的速度，正数表示正转，负数表示反转。然后通过delay()函数延迟两秒，实现电机正反转的循环控制。

4. 测试

下载到开发板，测试下能不能动起来。

六、做个遥控车-订阅ROS2 Twist

本节我们结合上一节电机控制以及前面章节的MicroROS话题订阅部分知识点，来实现一个可以用键盘遥控的小车。

1. 新建工程

新建工程example12_ros2_car

修改配置

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
board_microros_transport = wifi  ; 指定使用的Micro-ROS传输方式为Wi-Fi
lib_deps =  ; 列出所有依赖库的URL，这些库将被下载和安装
	https://github.com/fishros/Esp32McpwmMotor.git  ; ESP32-MCPWM-Motor库，用于驱动电机
    https://gitee.com/ohhuo/micro_ros_platformio.git  ; Micro-ROS平台库，用于在ESP32上运行ROS 2

2. 编写代码

#include <Arduino.h>
#include <Esp32McpwmMotor.h>
#include <Arduino.h>
#include <micro_ros_platformio.h>
#include <WiFi.h>
#include <rcl/rcl.h>
#include <rclc/rclc.h>
#include <rclc/executor.h>
#include <geometry_msgs/msg/twist.h>

// 定义 ROS2 执行器和支持结构
rclc_executor_t executor;
rclc_support_t support;
// 定义 ROS2 内存分配器
rcl_allocator_t allocator;
// 定义 ROS2 节点和订阅者
rcl_node_t node;
rcl_subscription_t subscriber;
// 定义接收到的消息结构体
geometry_msgs__msg__Twist sub_msg;

// 定义控制两个电机的对象
Esp32McpwmMotor motor;

// 回调函数，当接收到新的 Twist 消息时会被调用
void twist_callback(const void *msg_in)
{
    
    
  // 将接收到的消息指针转化为 geometry_msgs__msg__Twist 类型
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  // 从 Twist 消息中获取线速度和角速度
  float linear_x = twist_msg->linear.x;
  float angular_z = twist_msg->angular.z;
  // 打印接收到的速度信息
  Serial.printf("recv spped(%f,%f)\n", linear_x, angular_z);
  // 如果速度为零，则停止两个电机
  if (linear_x == 0 && angular_z == 0)
  {
    
    
    motor.updateMotorSpeed(0, 0);
    motor.updateMotorSpeed(1, 0);
    return;
  }

  // 根据线速度和角速度控制两个电机的转速
  if (linear_x > 0)
  {
    
    
    motor.updateMotorSpeed(0, 70);
    motor.updateMotorSpeed(1, 70);
  }

  if (linear_x < 0)
  {
    
    
    motor.updateMotorSpeed(0, -70);
    motor.updateMotorSpeed(1, -70);
  }

  if (angular_z > 0)
  {
    
    
    motor.updateMotorSpeed(0, -70);
    motor.updateMotorSpeed(1, 70);
  }

  if (angular_z < 0)
  {
    
    
    motor.updateMotorSpeed(0, 70);
    motor.updateMotorSpeed(1, -70);
  }
}

void setup()
{
    
    
  // 初始化串口
  Serial.begin(115200);

  // 初始化两个电机的引脚
  motor.attachMotor(0, 12, 13);
  motor.attachMotor(1, 14, 27);

  // 设置 micro-ROS 通信参数，连接到指定的 ROS2 代理
  IPAddress agent_ip;
  agent_ip.fromString("192.168.43.235");
  set_microros_wifi_transports("LLLLLL", "12345678", agent_ip, 8888);
  delay(2000);

  // 初始化 ROS2 执行器和支持结构
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  // 初始化 ROS2 节点
  rclc_node_init_default(&node, "esp32_car", "", &support);
  // 初始化订阅者
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  // 设置订阅的回调函数
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);
}

void loop()
{
    
    
  rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)); // 循环处理数据
}

代码使用 Esp32McpwmMotor 库初始化电机，设置 micro-ROS 通信参数以连接到 ROS2 代理，并初始化一个 ROS2 节点和一个订阅者，以订阅/cmd_vel主题上的 Twist 消息。

当接收到新的 Twist 消息时，调用twist_callback()函数提取线性和角速度，并相应地控制电机。如果两个速度都为零，则电机停止。否则，根据方向设置电机速度。在正向方向上，速度设置为 70，在反向方向上为 -70。

loop() 函数重复调用rclc_executor_spin_some()来处理来自 ROS2 网络的传入数据。

需要注意的是，你要根据自己的网络情况修改下面的代码以实现无线通信，如果不知道怎么设置，请回看前面章节。

  agent_ip.fromString("192.168.43.235");
  set_microros_wifi_transports("LLLLLL", "12345678", agent_ip, 8888);

3.下载测试

将代码下载到小车，运行agent，点击RST等待接入。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

接着我们使用ROS 2的键盘控制节点来进行控制测试

ros2 run teleop_twist_keyboard teleop_twist_keyboard

接着按下入JKL，几个按键，看一下小车是否动了起来。

本节我们通过将小车接入MicroROS完成了一个遥控小车的开发。下一节我们开始使用编码器来测量轮子的转速。

七、脉冲测量与校准实验

上节做完小车，遥控时小车前进时你应该会发现，小车很难走一条直线，但明明我们给到两个电机的PWM占空比都是相同的，原因在于每一个电机的硬件参数并不能完全的保证一致，所以当我们采用开环控制时，即使我们给到每个电机相同的电压，也不能让两个电机保持相同的转速。

要解决这个问题我们就要把开环控制改成闭环控制，我们要实现的是速度闭环，所以第一步我们要实现的是对电机速度的测量。

1. 轮速测量原理

第一节中介绍过，我们采用的是AB磁编码器，编码器直接连接到了我们的单片机IO上，当电机转动时，IO上的电平高低就会产生变化，我们称这种电平从低到高再到低的过程称作一个脉冲。

因为有减速机的存在，当减速器的输出轴（轮胎）转动了一圈，我们会检测到多个脉冲。所以要想通过编码器得出轮子的速度，我们需要知道检测到一个脉冲时，轮子行走多远距离。

我们小车上的电机轮子直径为48mm，当轮子转一圈时产生N个脉冲，那么一个脉冲轮子前进的距离D可以这样计算，单位是mm。

$$D=48\ast PI/N$$

下面我们将通过实际的测试确定D的值，已知D的情况下，我们测得，某一段时间$\Delta T$(ms)内测得脉冲数为 $P_T$ ，则此时电机的转速为$V_T$ (m/s)

$$V_T=((P_T\ast D）/1000）/(\Delta T/1000)=(P_T\ast D）/\Delta T$$

2. 编码器连接

下面的接线是最终的连接方式

编码器1
编码器A——GPIO 32
编码器B——GPIO 33
正负极分别接5V电源

编码器2
编码器A——GPIO 26
编码器B——GPIO 25
正负极分别接5V电源

3. 新建工程并导入开源库

新建example13_encoder

修改配置

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
lib_deps =  ; 列出所有依赖库的URL，这些库将被下载和安装
	https://github.com/fishros/Esp32PcntEncoder.git  ; ESP32 编码器驱动库

这里我们使用的是Esp32PcntEncoder开源库，这个库调用了ESP32的脉冲计算外设进行编码器脉冲的计算，使用非常简单。

4. 代码实现

编写代码

#include <Arduino.h>
#include <Esp32PcntEncoder.h>

Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器

void setup()
{
    
    
  // 1.初始化串口
  Serial.begin(115200); // 初始化串口通信，设置通信速率为115200

  // 2.设置编码器
  encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接
}

void loop()
{
    
    
  delay(10); // 等待10毫秒

  // 读取并打印两个编码器的计数器数值
  Serial.printf("tick1=%d,tick2=%d\n", encoders[0].getTicks(), encoders[1].getTicks());
}

上面这段代码使用了ESP32PcntEncoder库来读取两个旋转编码器的计数器数值。其中，函数setup()用于初始化串口和编码器；函数loop()用于读取并打印两个编码器的计数器数值。以下是代码的详细解释：

1. 首先包含了两个头文件Arduino.h和Esp32PcntEncoder.h，用于编写Arduino程序和使用ESP32PcntEncoder库。
2. 在全局变量中创建了一个长度为2的Esp32PcntEncoder数组，用于存储两个编码器。
3. 函数setup()用于初始化串口和编码器。在本代码中，首先通过Serial.begin()函数初始化串口，设置通信速率为115200。然后通过encoders[0].init()和encoders[1].init()函数分别初始化了两个编码器。其中，函数init()需要传入三个参数，分别是编码器的ID、引脚A的GPIO编号和引脚B的GPIO编号。在本代码中，第一个编码器的ID为0，引脚A连接的GPIO为32，引脚B连接的GPIO为33；第二个编码器的ID为1，引脚A连接的GPIO为26，引脚B连接的GPIO为25。
4. 函数loop()用于读取并打印两个编码器的计数器数值。在本代码中，首先通过delay()函数等待10毫秒。然后通过encoders[0].getTicks()和encoders[1].getTicks()函数分别读取了两个编码器的计数器数值。最后通过Serial.printf()函数将这两个数值打印。

5. 下载测试

将代码下载进入开发板，打开串口监视器，查看输出。
此时输出tick1=0，tick2=0

6. 脉冲/圈计算

为了计算一个脉冲轮子前进的距离，我们可以通过手动将轮子旋转10圈，然后利用前面的公式进行计算。

将轮子转动10圈后得到脉冲数为11283，也就是说当前电机1128.3个脉冲/圈

根据公式可以算出，一个脉冲轮子前进的距离为

$$D=48\ast PI/(11283/10)=0.1336492$$

接着我们可以利用公式计算速度。

八、速度转换-机器人最大速度测量

接着上一节的连线方式

1. 新建工程并导入开源库

新建example14_max_speed_measurement

修改配置

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
lib_deps =  ; 列出所有依赖库的URL，这些库将被下载和安装
	https://github.com/fishros/Esp32PcntEncoder.git  ; ESP32 编码器驱动库

2. 编写代码

#include <Arduino.h>
#include <Esp32PcntEncoder.h>

Esp32PcntEncoder encoders[2]; // 创建一个数组用于存储两个编码器
int64_t last_ticks[2]; // 记录上一次读取的计数器数值
int32_t pt[2]; // 记录两次读取之间的计数器差值
int64_t last_update_time; // 记录上一次更新时间
float speeds[2]; // 记录两个电机的速度

void setup()
{
    
    
  // 1.初始化串口
  Serial.begin(115200); // 初始化串口通信，设置通信速率为115200

  // 2.设置编码器
  encoders[0].init(0, 32, 33); // 初始化第一个编码器，使用GPIO 32和33连接
  encoders[1].init(1, 26, 25); // 初始化第二个编码器，使用GPIO 26和25连接

  // 3.让电机1以最大速度转起来
  pinMode(23, OUTPUT);
  digitalWrite(23, HIGH);
}

void loop()
{
    
    
  delay(10); // 等待10毫秒

  // 4.计算两个电机的速度
  uint64_t dt = millis() - last_update_time; // 计算两次读取之间的时间差
  pt[0] = encoders[0].getTicks() - last_ticks[0]; // 计算第一个编码器两次读取之间的计数器差值
  pt[1] = encoders[1].getTicks() - last_ticks[1]; // 计算第二个编码器两次读取之间的计数器差值

  speeds[0] = float(pt[0] * 0.1336492) / dt; // 计算第一个电机的速度
  speeds[1] = float(pt[1] * 0.1336492) / dt; // 计算第二个电机的速度

  // 5.更新记录
  last_update_time = millis(); // 更新上一次更新时间
  last_ticks[0] = encoders[0].getTicks(); // 更新第一个编码器的计数器数值
  last_ticks[1] = encoders[1].getTicks(); // 更新第二个编码器的计数器数值

  // 6.打印信息
  Serial.printf("tick1=%d,tick2=%d\n", encoders[0].getTicks(), encoders[1].getTicks()); // 打印两个编码器的计数器数值
  Serial.printf("spped1=%f,spped2=%f\n", speeds[0], speeds[1]); // 打印两个电机的速度
}

在loop()函数中，首先等待10毫秒，然后读取两个编码器的计数器数值，并且计算出它们的旋转速度。

其中，last_ticks数组用于存储上一次读取的计数器数值，pt数组存储两次读取之间的计数器增量，last_update_time变量存储上一次读取的时间，speeds数组存储两个编码器的旋转速度。

最后，通过串口打印出两个编码器的计数器数值和旋转速度。此外，还让GPIO 23输出高电平，使电机1以最大速度转动。

3. 下载测试

下载代码，观察串口打印

最大速度为-1.482708m/s

九、控制速度-PID控制器实现

上一节我们通过编码器完成了对机器人单个轮子的速度测量，完成了电机速度闭环控制的重要一步-反馈。

有了反馈，接着我们需要设计一个控制器来帮助我们实现这个需求，这个控制器的输入是当前的速度和目标速度，输出是应该给到电机的PWM占空比。

1. PID控制器介绍

PID控制器是一种广泛应用于工业控制、自动化控制等领域的控制算法，其名称来源于“比例-积分-微分”三个控制器参数，即Proportional（比例）、Integral（积分）、Derivative（微分）。

PID控制器的基本原理是通过测量目标系统的反馈信号和期望输出信号之间的误差，根据一定的数学模型计算出控制信号，使目标系统能够稳定地达到期望输出。具体来说，PID控制器的计算公式为：
$$Output=K_p\cdot Error+K_i\cdot \int Errordt+K_d\cdot d(Error)/dt$$

其中，Kp、Ki和Kd分别表示比例系数、积分系数和微分系数，Error表示目标系统的误差，Integral(Error)表示误差的积分，Derivative(Error)表示误差的微分。

在PID控制器中，比例系数、积分系数和微分系数的选取是关键，需要根据具体的控制需求进行调整。比例系数主要影响系统的响应速度和稳定性，积分系数主要影响系统的稳态误差，而微分系数主要影响系统的抗干扰性能。

​

2. 新建工程搭建框架

2.1 新建工程

example15_pid_controller

修改platformio.ini配置，添加开源库和microros配置

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
board_microros_transport = wifi
board_microros_distro = humble        #这里需要改为自己的ubuntu版本
lib_deps = 
	https://gitee.com/ohhuo/micro_ros_platformio.git
	https://github.com/fishros/Esp32McpwmMotor.git
	https://github.com/fishros/Esp32PcntEncoder.git

2.2 添加PidController

在lib下新建PidController文件夹，并在PidController下新建PidController.h和PidController.cpp

最终目录结构

.
├── include
│   └── README
├── lib
│   ├── PidController
│   │   ├── PidController.cpp
│   │   └── PidController.h
│   └── README
├── platformio.ini
├── src
│   └── main.cpp
└── test
    └── README

5 directories, 7 files

2.3 复制并修改代码

将之前遥控车的代码复制粘贴到当前的main函数中，同时

• 添加PidController.h的头文件
• 删除原有的控制逻辑
• 添加电机速度测量函数
• 修改为双核通信
• 添加了一些注释

最终代码如下

#include <Arduino.h>
#include <micro_ros_platformio.h>    // 包含用于 ESP32 的 micro-ROS PlatformIO 库
#include <WiFi.h>                    // 包含 ESP32 的 WiFi 库
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 包含 ROS2 geometry_msgs/Twist 消息类型
#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
rclc_executor_t executor;          // 创建一个 RCLC 执行程序对象，用于处理订阅和发布
rclc_support_t support;            // 创建一个 RCLC 支持对象，用于管理 ROS2 上下文和节点
rcl_allocator_t allocator;         // 创建一个 RCL 分配器对象，用于分配内存
rcl_node_t node;                   // 创建一个 RCL 节点对象，用于此基于 ESP32 的机器人小车
rcl_subscription_t subscriber;     // 创建一个 RCL 订阅对象，用于订阅 ROS2 消息
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机
float out_motor_speed[2];          // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
float current_speeds[2];           // 创建一个长度为 2 的浮点数数组，用于保存当前电机速度

void twist_callback(const void *msg_in)
{
    
    
    const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
    float linear_x = twist_msg->linear.x;   // 获取 Twist 消息的线性 x 分量
    float angular_z = twist_msg->angular.z; // 获取 Twist 消息的角度 z 分量
    if (linear_x == 0 && angular_z == 0)    // 如果 Twist 消息没有速度命令
    {
    
    
        motor.updateMotorSpeed(0, 0); // 停止第一个电机
        motor.updateMotorSpeed(1, 0); // 停止第二个电机
        return;                       // 退出函数
    }
}

// 这个函数是一个后台任务，负责设置和处理与 micro-ROS 代理的通信。
void microros_task(void *param)
{
    
    
  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("192.168.43.235");
  
  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("LLLLLL", "12345678", agent_ip, 8888);
  
  // 等待 2 秒，以便网络连接得到建立。
  delay(2000);
  
  // 设置 micro-ROS 支持结构、节点和订阅。
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "esp32_car", "", &support);
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  
  // 设置 micro-ROS 执行器，并将订阅添加到其中。
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);
  
  // 循环运行 micro-ROS 执行器以处理传入的消息。
  while (true)
  {
    
    
    delay(100);
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  }
}

// 这个函数根据编码器读数更新两个轮子速度。
void update_speed()
{
    
    
  // 初始化静态变量以存储上一次更新时间和编码器读数。
  static uint64_t last_update_time = millis();
  static int64_t last_ticks[2];
  
  // 获取自上次更新以来的经过时间。
  uint64_t dt = millis() - last_update_time;
  if (dt == 0)
    return;
  
  // 获取当前的编码器读数并计算当前的速度。
  int32_t pt[2];
  pt[0] = encoders[0].getTicks() - last_ticks[0];
  pt[1] = encoders[1].getTicks() - last_ticks[1];
  current_speeds[0] = float(pt[0] * 0.1336492) / dt * 1000;
  current_speeds[1] = float(pt[1] * 0.1336492) / dt * 1000;
  
  // 更新上一次更新时间和编码器读数。
  last_update_time = millis();
  last_ticks[0] = encoders[0].getTicks();
  last_ticks[1] = encoders[1].getTicks();
}

void setup()
{
    
    
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 12, 13);
  motor.attachMotor(1, 14, 27);
  // 在引脚32、33和26、25上初始化两个编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  // 在核心0上创建一个名为"microros_task"的任务，栈大小为10240
  xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 0);
}

void loop()
{
    
    
  // 更新电机速度
  update_speed();
  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);
  // 延迟10毫秒
  delay(10);
}

3. PID控制器代码实现

接着我们来编写PidController.h和PidController.cpp

3.1 PidController.h

#ifndef __PIDCONTROLLER_H__ // 如果没有定义__PIDCONTROLLER_H__
#define __PIDCONTROLLER_H__ // 定义__PIDCONTROLLER_H__

class PidController
{
    
     // 定义一个PID控制器类
public:
    PidController() = default;                   // 默认构造函数
    PidController(float kp, float ki, float kd); // 构造函数，传入kp、ki、kd

public:
    float target_;      // 目标值
    float out_mix_;     // 输出下限
    float out_max_;     // 输出上限
    float kp_;          // 比例系数
    float ki_;          // 积分系数
    float kd_;          // 微分系数
    float last_output_; // 上一次输出值
    // pid
    float error_sum_;           // 误差累积和
    float derror_;              // 误差变化率
    float error_pre_;           // 上上次误差
    float error_last_;          // 上一次误差
    float intergral_up_ = 2500; // 积分上限

public:
    float update(float control);                   // 更新输出值
    void reset();                                  // 重置PID控制器
    void update_pid(float kp, float ki, float kd); // 更新PID系数
    void update_target(float target);              // 更新目标值
    void out_limit(float out_mix, float out_max);  // 输出限制
};

#endif // __PIDCONTROLLER_H__ // 结束条件

定义PidController，提供五个函数。

• update(control): 传入当前控制量control并返回PID控制器的输出值
• reset(): 将PID控制器的状态重置为初始状态
• update_pid(kp, ki, kd): 更新PID控制系数
• update_target(target): 更新目标值
• out_limit(out_mix, out_max): 输出限制

3.2 PidController.cpp

#include "PidController.h"
#include "Arduino.h"

PidController::PidController(float kp, float ki, float kd)
{
    
    
    reset(); // 初始化控制器
    update_pid(kp, ki, kd); // 更新PID参数
}

float PidController::update(float control)
{
    
    
    // 计算误差及其变化率
    float error = target_ - control; // 计算误差
    derror_ = error_last_ - error; // 计算误差变化率
    error_last_ = error;

    // 计算积分项并进行积分限制
    error_sum_ += error;
    if (error_sum_ > intergral_up_)
        error_sum_ = intergral_up_;
    if (error_sum_ < -1 * intergral_up_)
        error_sum_ = -1 * intergral_up_;

    // 计算控制输出值
    float output = kp_ * error + ki_ * error_sum_ + kd_ * derror_;

    // 控制输出限幅
    if (output > out_max_)
        output = out_max_;
    if (output < out_mix_)
        output = out_mix_;

    // 保存上一次的控制输出值
    last_output_ = output;

    return output;
}

void PidController::update_target(float target)
{
    
    
    target_ = target; // 更新控制目标值
}

void PidController::update_pid(float kp, float ki, float kd)
{
    
    
    reset(); // 重置控制器状态
    kp_ = kp; // 更新比例项系数
    ki_ = ki; // 更新积分项系数
    kd_ = kd; // 更新微分项系数
}

void PidController::reset()
{
    
    
    // 重置控制器状态
    last_output_ = 0.0f; // 上一次的控制输出值
    target_ = 0.0f; // 控制目标值
    out_mix_ = 0.0f; // 控制输出最小值
    out_max_ = 0.0f; // 控制输出最大值
    kp_ = 0.0f; // 比例项系数
    ki_ = 0.0f; // 积分项系数
    kd_ = 0.0f; // 微分项系数
    error_sum_ = 0.0f; // 误差累计值
    derror_ = 0.0f; // 误差变化率
    error_last_ = 0.0f; // 上一次的误差值
}

void PidController::out_limit(float out_mix, float out_max)
{
    
    
    out_mix_ = out_mix; // 控制输出最小值
    out_max_ = out_max; // 控制输出最大值
}

上面这段代码是用于实现一个PID控制器的C++代码。PID控制器是一种常用的控制器，它的输入是控制系统的误差信号，输出是控制器的控制量。PID控制器由比例项、积分项和微分项三个部分组成，这三个部分的系数可以通过调节来实现控制器的性能优化。

以下是代码中各部分的注释：

• PidController::PidController(float kp, float ki, float kd)：PID控制器的构造函数，用于初始化控制器状态并更新PID参数。
• void PidController::update_target(float target)：用于更新控制器的目标值。
• void PidController::update_pid(float kp, float ki, float kd)：用于更新控制器的PID参数。
• void PidController::out_limit(float out_mix, float out_max)：用于限制控制器的控制输出范围。
• float PidController::update(float control)：控制器的核心函数，用于根据当前控制量计算出下一时刻的控制量。具体实现包括以下步骤：
  • 计算误差及其变化率；
  • 计算积分项并进行积分限制；
  • 计算控制输出值，并进行输出限幅；
  • 保存上一次的控制输出值。
• void PidController::reset()：用于重置控制器的状态。包括重置PID参数、目标值、控制输出范围等状态变量。

在代码实现中，float代表浮点数类型，在C++中用于表示实数。kp_、ki_、kd_分别代表PID控制器中的比例项系数、积分项系数、微分项系数。target_代表控制器的目标值，out_mix_和out_max_用于限制控制器的控制输出范围。error_sum_代表误差累计值，derror_代表误差变化率，error_last_代表上一次的误差值。last_output_保存上一次的控制输出值。

4. 修改主程序

#include <Arduino.h>
#include <micro_ros_platformio.h>    // 包含用于 ESP32 的 micro-ROS PlatformIO 库
#include <WiFi.h>                    // 包含 ESP32 的 WiFi 库
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 包含 ROS2 geometry_msgs/Twist 消息类型
#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
rclc_executor_t executor;          // 创建一个 RCLC 执行程序对象，用于处理订阅和发布
rclc_support_t support;            // 创建一个 RCLC 支持对象，用于管理 ROS2 上下文和节点
rcl_allocator_t allocator;         // 创建一个 RCL 分配器对象，用于分配内存
rcl_node_t node;                   // 创建一个 RCL 节点对象，用于此基于 ESP32 的机器人小车
rcl_subscription_t subscriber;     // 创建一个 RCL 订阅对象，用于订阅 ROS2 消息
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机
float out_motor_speed[2];          // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
float current_speeds[2];           // 创建一个长度为 2 的浮点数数组，用于保存当前电机速度
PidController pid_controller[2];   // 创建PidController的两个对象

void twist_callback(const void *msg_in)
{
    
    
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  float linear_x = twist_msg->linear.x;   // 获取 Twist 消息的线性 x 分量
  float angular_z = twist_msg->angular.z; // 获取 Twist 消息的角度 z 分量
  if (linear_x == 0 && angular_z == 0)    // 如果 Twist 消息没有速度命令
  {
    
    
    pid_controller[0].update_target(0); // 更新控制器的目标值
    pid_controller[1].update_target(0);
    motor.updateMotorSpeed(0, 0); // 停止第一个电机
    motor.updateMotorSpeed(1, 0); // 停止第二个电机
    return;                       // 退出函数
  }

  // 根据线速度和角速度控制两个电机的转速
  if (linear_x != 0)
  {
    
    
    pid_controller[0].update_target(linear_x * 1000);  // 使用mm/s作为target
    pid_controller[1].update_target(linear_x * 1000);
  }
}


// 这个函数是一个后台任务，负责设置和处理与 micro-ROS 代理的通信。
void microros_task(void *param)
{
    
    
  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("192.168.43.235");
  Serial.println("Connecting to WiFi...");

  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("LLLLLL", "12345678", agent_ip, 8888);

  // 等待 2 秒，以便网络连接得到建立。
  delay(2000);

  if (WiFi.status() != WL_CONNECTED)
  {
    
    
    Serial.println("WiFi connection failed");
    return;
  }
  else
  {
    
    
    Serial.println("WiFi connected");
  }

  // 设置 micro-ROS 支持结构、节点和订阅。
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "esp32_car", "", &support);
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");

  // 设置 micro-ROS 执行器，并将订阅添加到其中。
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);

  // 循环运行 micro-ROS 执行器以处理传入的消息。
  while (true)
  {
    
    
    delay(100);
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  }
}

// 这个函数根据编码器读数更新两个轮子速度。
void update_speed()
{
    
    
  // 初始化静态变量以存储上一次更新时间和编码器读数。
  static uint64_t last_update_time = millis();
  static int64_t last_ticks[2];

  // 获取自上次更新以来的经过时间。
  uint64_t dt = millis() - last_update_time;
  if (dt == 0)
    return;

  // 获取当前的编码器读数并计算当前的速度。
  int32_t pt[2];
  pt[0] = encoders[0].getTicks() - last_ticks[0];
  pt[1] = encoders[1].getTicks() - last_ticks[1];
  current_speeds[0] = float(pt[0] * 0.1336492) / dt * 1000;
  current_speeds[1] = float(pt[1] * 0.1336492) / dt * 1000;

  // 更新上一次更新时间和编码器读数。
  last_update_time = millis();
  last_ticks[0] = encoders[0].getTicks();
  last_ticks[1] = encoders[1].getTicks();
}


void setup()
{
    
    
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 12, 13);
  motor.attachMotor(1, 14, 27);
  // 在引脚32、33和26、25上初始化两个编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  // 初始化PID控制器的kp、ki和kd
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  // 初始化PID控制器的最大输入输出，MPCNT大小范围在正负100之间
  pid_controller[0].out_limit(-100, 100);
  pid_controller[1].out_limit(-100, 100);

  // 在核心0上创建一个名为"microros_task"的任务，栈大小为10240
  xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 0);
}

void loop()
{
    
    
  // 更新电机速度
  update_speed();
  // 计算最新的电机输出值
  out_motor_speed[0] = pid_controller[0].update(-current_speeds[0]);
  out_motor_speed[1] = -pid_controller[1].update(current_speeds[1]);   // 这里的接线方式导致需要加一个负号

  Serial.print("current_speeds[0]: "); 
  Serial.println(current_speeds[0]); 
  Serial.print("current_speeds[1]: "); 
  Serial.println(current_speeds[1]);


  Serial.print("out_motor_speed[0]: "); 
  Serial.println(out_motor_speed[0]); 
  Serial.print("out_motor_speed[1]: "); 
  Serial.println(out_motor_speed[1]);

  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);
  // 延迟10毫秒
  delay(10);
}

添加PidController控制器到main函数中，关于Pid控制器的kp、ki和kd的设置，这里小鱼直接使用了比较合适的0.625和0.125，对于KD并没有设置，接下来我们下载代码进去并修改下PID进行测试。

需要注意：你要修改网络参数为你的当前环境的网络参数。

5. 下载测试

下载代码，运行agent，点击RST按键。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

看到连接建立表示通信成功，接着用ros2 topic list

ros2 topic list

看到/cmd_vel表示正常，接着我们使用teleop_twist_keyboard进行键盘控制

ros2 run teleop_twist_keyboard teleop_twist_keyboard

把速度修改为0.10左右，接着把小车放到地上，点击键盘上的i，记时10s之后点击k或者控制让机器人停下来，接着看看机器人行走距离是不是1m。

6. 总结

本节我们完成了PID控制器对两个电机速度的控制，但是仅限于前进和后退，如果想实现角速度的控制，我们还要结合两轮差速运动学模型才行。

十、两轮差速机器人运动学介绍

1. 两轮差速运动学模型

两轮差速模型指机器人底盘由两个驱动轮和若干支撑轮构成的底盘模型，像turtlebot等都是两轮差速模型。

两轮差速模型通过两个驱动轮可以通过不同转速和转向，使得机器人的达到某个特定的角速度和线速度。

2. 正逆解

了解了两轮差速模型，那正逆解又是怎么回事？

正运动学：已知两个轮子的速度，求整车的角速度（弧度/秒）和线速度（米/秒）

逆运动学：已知目标角速度和线速度，求两个轮子的转速

3. 轮式里程计

当我们知道了两个轮子之间的相对位置，同时知道了每一时刻机器人的角速度和线速度，那我们如何获取机器人的当前角度和位置呢？

3.1 角度

影响机器人当前角度的因素只有一个，就是角速度。

某一时刻机器人转动的角度 = 这一时刻机器人的角速度*这一时刻时长

假如我们认定初始时刻机器人的角度为0,通过对机器人转动角度角度进行累加，即可获得机器人的当前角度。

上述过程其实就是对角速度进行积分得到角度。

3.2 位置

通过对角速度积分，我们得到了角度。

机器人某一时刻自身方向上的前进速度可以分解为里程计坐标系中x轴和y轴方向上的速度。

从图中可以看出：

$$v_x=v\ast cos(\theta )$$$$v_y==v\ast sin(\theta )$$

得到了x和y方向上的速度，乘上该速度对应的某一时刻经过的时间，即可得到这一时刻在x轴和y轴方向上的位移，对位移进行累加即可得到里程计中的x和y。

十一、实时速度计算-运动学正解

本节我们线进一步的了解两轮差速正运动学的推导过程，并利用两轮差速运动学正解，来完成对小车的实时速度计算。

1. 正运动学解推导

两轮差速机器人是一种常见的移动机器人类型，由两个轮子和一个中心点组成。我们可以通过控制每个轮子的转速来实现移动，并且可以在一个平面上进行自由移动。

前面章节我们通过PID+编码器完成了底盘两个轮子单独速度的测量，但是在实际使用当中，我们把机器人当作一个整体来看，而对于这样一个整体在空间中的速度，我们一般采用X轴线速度 v 和Z轴角速度 ω 来描述。

需要注意的是：在ROS中，机器人的前方通常指的是机器人本体坐标系的正方向。本体坐标系是相对于机器人自身的一个坐标系，通常定义在机器人的中心位置，以机器人的前进方向为X轴，左侧为Y轴，垂直于机器人平面的方向为Z轴。
而全局坐标系中的正方向X轴指向右方，Y轴指向前方，Z轴垂直于地面。

-	X	Y	Z
机器人本体坐标系	前方	左侧	垂直于机器人平面
全局坐标系	右方	前方	垂直于地面

所以问题就变成了假设机器人在一小段时间 $t$ 内，它的左右轮子线速度$v_l$和$v_r$，保持不变 ，两轮之间的安装间距$l$，求机器人的线速度 ，求机器人的线速度，求机器人的线速度$v$ ，角速度$\omega$。

我们看上图来推导

因为机器人的线速度方向和轮子转动方向始终保持一致，所以机器人的线速度为做右轮线速度的平均值，即：

$$v=(v_l+v_r)/2$$

我们知道$v=\omega \ast r$ ，根据上图所以有
$$l=r_r-r_l=v_r/{\omega }_r-v_l/{\omega }_l$$

同一个机器人角速度相同，所以有
​$${\omega }_l={\omega }_r$$

可以求出
$$\omega =(v_r-v_l)/l$$

2. 正运动学代码实现

2.1 新建工程

从本节开始我们持续的在一个工程上进行开发
在PlatformIO上新建example16_motion_control_microros工程。

修改platformio.ini配置

[env:esp32doit-devkit-v1]
platform = espressif32
board = esp32doit-devkit-v1
framework = arduino
board_microros_transport = wifi
board_microros_distro = humble        #这里需要改为自己的ubuntu版本
board_build.f_cpu = 240000000L
board_build.f_flash = 80000000L
monitor_speed = 115200
lib_deps = 
	https://gitee.com/ohhuo/micro_ros_platformio.git
	https://github.com/fishros/Esp32McpwmMotor.git
	https://github.com/fishros/Esp32PcntEncoder.git

接着将前面章节中pid_controller样例程序的lib下的内容和main.cpp内容复制过来，最终就目录结构如下：

.
├── include
│   └── README
├── lib
│   ├── PidController
│   │   ├── PidController.cpp
│   │   └── PidController.h
│   └── README
├── LICENSE
├── platformio.ini
├── src
│   └── main.cpp
└── test
    ├── my_main.cpp
    ├── README

2.2 添加Kinematic库

在lib下添加Kinematics文件夹，并添加Kinematics.h和Kinematics.cpp文件。

编写Kinematics.h

#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__
#include <Arduino.h>

typedef struct
{
    
    
    uint8_t id;                // 电机编号
    uint16_t reducation_ratio; // 减速器减速比，轮子转一圈，电机需要转的圈数
    uint16_t pulse_ration;     // 脉冲比，电机转一圈所产生的脉冲数
    float wheel_diameter;      // 轮子的外直径，单位mm

    float per_pulse_distance;  // 无需配置，单个脉冲轮子前进的距离，单位mm，设置时自动计算
                               // 单个脉冲距离=轮子转一圈所行进的距离/轮子转一圈所产生的脉冲数
                               // per_pulse_distance= (wheel_diameter*3.1415926)/(pulse_ration*reducation_ratio)
    uint32_t speed_factor;     // 无需配置，计算速度时使用的速度因子，设置时自动计算，speed_factor计算方式如下
                               // 设 dt（单位us,1s=1000ms=10^6us）时间内的脉冲数为dtick
                               // 速度speed = per_pulse_distance*dtick/(dt/1000/1000)=(per_pulse_distance*1000*1000)*dtic/dt
                               // 记 speed_factor = (per_pulse_distance*1000*1000)
    int16_t motor_speed;       // 无需配置，当前电机速度mm/s，计算时使用
    int64_t last_encoder_tick; // 无需配置，上次电机的编码器读数
    uint64_t last_update_time; // 无需配置，上次更新数据的时间，单位us
} motor_param_t;


class Kinematics
{
    
    
private:
    motor_param_t motor_param_[2];
    float wheel_distance_; // 轮子间距
public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;

    /**
     * @brief 设置电机相关参数
     * 
     * @param id 
     * @param reducation_ratio 
     * @param pulse_ration 
     * @param wheel_diameter 
     */
    void set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter);
    /**
     * @brief 设置运动学相关参数
     * 
     * @param wheel_distance 
     */
    void set_kinematic_param(float wheel_distance);

    /**
     * @brief 运动学逆解，输入机器人当前线速度和角速度，输出左右轮子应该达到的目标速度
     * 
     * @param line_speed 
     * @param angle_speed 
     * @param out_wheel1_speed 
     * @param out_wheel2_speed 
     */
    void kinematic_inverse(float line_speed, float angle_speed, float &out_wheel1_speed, float &out_wheel2_speed);


    /**
     * @brief 运动学正解，输入左右轮子速度，输出机器人当前线速度和角速度
     * 
     * @param wheel1_speed 
     * @param wheel2_speed 
     * @param line_speed 
     * @param angle_speed 
     */
    void kinematic_forward(float wheel1_speed, float wheel2_speed, float &line_speed, float &angle_speed);

    /**
     * @brief 更新轮子的tick数据
     * 
     * @param current_time 
     * @param motor_tick1 
     * @param motor_tick2 
     */
    void update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2);

    /**
     * @brief 获取轮子当前速度
     * 
     * @param id 
     * @return float 
     */
    float motor_speed(uint8_t id);
};

#endif // __KINEMATICS_H__

这里主要定义了一个电机参数结构体，并定义了一个类，该类包含以下6个函数
​

函数名称	描述
set_motor_param()	设置电机相关参数
set_kinematic_param()	设置运动学相关参数
kinematic_inverse()	运动学逆解，输入机器人当前线速度和角速度，输出左右轮子应该达到的目标速度
kinematic_forward()	运动学正解，输入左右轮子速度，输出机器人当前线速度和角速度
update_motor_ticks()	更新轮子的tick数据
motor_speed()	获取轮子当前速度

2.3 Kinematics.cpp代码实现

#include "Kinematics.h"

void Kinematics::set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter)
{
    
    
    motor_param_[id].id = id;   // 设置电机ID
    motor_param_[id].reducation_ratio = reducation_ratio;   // 设置减速比
    motor_param_[id].pulse_ration = pulse_ration;   // 设置脉冲比
    motor_param_[id].wheel_diameter = wheel_diameter;   // 设置车轮直径
    motor_param_[id].per_pulse_distance = (wheel_diameter * PI) / (reducation_ratio * pulse_ration);   // 每个脉冲对应行驶距离
    motor_param_[id].speed_factor = (1000 * 1000) * (wheel_diameter * PI) / (reducation_ratio * pulse_ration);   // 计算速度因子
    Serial.printf("init motor param %d: %f=%f*PI/(%d*%d) speed_factor=%d\n", id, motor_param_[id].per_pulse_distance, wheel_diameter, reducation_ratio, pulse_ration, motor_param_[id].speed_factor);   // 打印调试信息
}

void Kinematics::set_kinematic_param(float wheel_distance)
{
    
    
    wheel_distance_ = wheel_distance;   // 设置轮间距离
}

void Kinematics::update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2)
{
    
    

    uint32_t dt = current_time - motor_param_[0].last_update_time;   // 计算时间差
    int32_t dtick1 = motor_tick1 - motor_param_[0].last_encoder_tick;   // 计算电机1脉冲差
    int32_t dtick2 = motor_tick2 - motor_param_[1].last_encoder_tick;   // 计算电机2脉冲差
    // 轮子速度计算
    motor_param_[0].motor_speed = dtick1 * (motor_param_[0].speed_factor / dt);   // 计算电机1轮子速度
    motor_param_[1].motor_speed = dtick2 * (motor_param_[1].speed_factor / dt);   // 计算电机2轮子速度

    motor_param_[0].last_encoder_tick = motor_tick1;   // 更新电机1上一次的脉冲计数
    motor_param_[1].last_encoder_tick = motor_tick2;   // 更新电机2上一次的脉冲计数
    motor_param_[0].last_update_time = current_time;   // 更新电机1上一次更新时间
    motor_param_[1].last_update_time = current_time;   // 更新电机2上一次更新时间
}

void Kinematics::kinematic_inverse(float linear_speed, float angular_speed, float &out_wheel1_speed, float &out_wheel2_speed)
{
    
    
    // 直接返回指定速度20mm/s
    out_wheel1_speed = 20;
    out_wheel2_speed = 20;
}

void Kinematics::kinematic_forward(float wheel1_speed, float wheel2_speed, float &linear_speed, float &angular_speed)
{
    
    
    linear_speed = (wheel1_speed + wheel2_speed) / 2.0;   // 计算线速度
    angular_speed = (wheel2_speed - wheel1_speed) / wheel_distance_;   // 计算角速度
}

float Kinematics::motor_speed(uint8_t id)
{
    
    
    return motor_param_[id].motor_speed; // 返回指定id的轮子速度
} 

​

2.4 修改main.cpp

#include <Arduino.h>
#include <micro_ros_platformio.h>    // 包含用于 ESP32 的 micro-ROS PlatformIO 库
#include <WiFi.h>                    // 包含 ESP32 的 WiFi 库
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 包含 ROS2 geometry_msgs/Twist 消息类型
#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制
#include <Kinematics.h>              // 运动学相关实现

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
rclc_executor_t executor;          // 创建一个 RCLC 执行程序对象，用于处理订阅和发布
rclc_support_t support;            // 创建一个 RCLC 支持对象，用于管理 ROS2 上下文和节点
rcl_allocator_t allocator;         // 创建一个 RCL 分配器对象，用于分配内存
rcl_node_t node;                   // 创建一个 RCL 节点对象，用于此基于 ESP32 的机器人小车
rcl_subscription_t subscriber;     // 创建一个 RCL 订阅对象，用于订阅 ROS2 消息
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机

float out_motor_speed[2];        // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
PidController pid_controller[2]; // 创建PidController的两个对象
Kinematics kinematics;           // 运动学相关对象

void twist_callback(const void *msg_in)
{
    
    
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  static float target_motor_speed1, target_motor_speed2;
  float linear_x = twist_msg->linear.x;   // 获取 Twist 消息的线性 x 分量
  float angular_z = twist_msg->angular.z; // 获取 Twist 消息的角度 z 分量
  kinematics.kinematic_inverse(linear_x * 1000, angular_z, target_motor_speed1, target_motor_speed2);
  pid_controller[0].update_target(target_motor_speed1);
  pid_controller[1].update_target(target_motor_speed2);
}


// 这个函数是一个后台任务，负责设置和处理与 micro-ROS 代理的通信。
void microros_task(void *param)
{
    
    
  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("192.168.43.235");
  Serial.println("Connecting to WiFi...");

  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("LLLLLL", "Laj19980918.", agent_ip, 8888);

  // 等待 2 秒，以便网络连接得到建立。
  delay(2000);

  if (WiFi.status() != WL_CONNECTED)
  {
    
    
    Serial.println("WiFi connection failed");
    return;
  }
  else
  {
    
    
    Serial.println("WiFi connected");
  }

  // 设置 micro-ROS 支持结构、节点和订阅。
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "esp32_car", "", &support);
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");

  // 设置 micro-ROS 执行器，并将订阅添加到其中。
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);

  // 循环运行 micro-ROS 执行器以处理传入的消息。
  while (true)
  {
    
    
    delay(100);
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  }
}


void setup()
{
    
    
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 12, 13);
  motor.attachMotor(1, 14, 27);
  // 在引脚32、33和26、25上初始化两个编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  // 初始化PID控制器的kp、ki和kd
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  // 初始化PID控制器的最大输入输出，MPCNT大小范围在正负100之间
  pid_controller[0].out_limit(-100, 100);
  pid_controller[1].out_limit(-100, 100);

    // 设置运动学参数
  kinematics.set_motor_param(0, 45, 44, 65);
  kinematics.set_motor_param(1, 45, 44, 65);
  kinematics.set_kinematic_param(150);

  // 在核心0上创建一个名为"microros_task"的任务，栈大小为10240
  xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 0);
}

void loop()
{
    
    
  static float out_motor_speed[2];
  static uint64_t last_update_info_time = millis();
  kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks());
  out_motor_speed[0] = pid_controller[0].update(-kinematics.motor_speed(0));
  out_motor_speed[1] = -pid_controller[1].update(kinematics.motor_speed(1));

  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);
  // 延迟10毫秒
  delay(10);
}

​这里主要调用Kinematic完成相关函数的调用。

主要有下面几行

// 初始化运动学相关对象
Kinematics kinematics;           
// 设置运动学参数
kinematics.set_motor_param(0, 45, 44, 65);
kinematics.set_motor_param(1, 45, 44, 65);
kinematics.set_kinematic_param(150);
// 更新电机速度
kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks());
// 运动学逆解
kinematics.kinematic_inverse(linear_x * 1000, angular_z, target_motor_speed1, target_motor_speed2);

3. 上传测试

下载代码，运行agent，点击RST按键。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

看到连接建立表示通信成功，接着用ros2 topic list

ros2 topic list

看到/cmd_vel表示正常，接着我们使用teleop_twist_keyboard进行键盘控制

ros2 run teleop_twist_keyboard teleop_twist_keyboard

​随便发送一个指令，打开串口，观察打印
速度一直在20左右徘徊，和我们设置的速度相同。

void Kinematics::kinematic_inverse(float linear_speed, float angular_speed, float &out_wheel1_speed, float &out_wheel2_speed)
{
    
    
    // 直接返回指定速度20mm/s
    out_wheel1_speed = 20;
    out_wheel2_speed = 20;
}

​

十二、目标速度控制-运动学逆解

​上一节我们推导并在代码中实现了运动学正解，本节我们来学习下运动学逆解，实现给定线速度和角速度，计算出轮子达到怎样的转速才能达到这个速度。

1. 逆解推导

我们直接用正解结果进行求逆解即可。
$$v=(v_l+v_r)/2$$$$\omega =(v_r-v_l)/l$$
所以有
$$v_l=v-wl/2$$$$v_r=v+wl/2$$

2. 编写代码

继续在上一节中的代码Kinematics.cpp中完善即可。

void Kinematics::kinematic_inverse(float linear_speed, float angular_speed, float &out_wheel1_speed, float &out_wheel2_speed)
{
    
    
    out_wheel1_speed =
        linear_speed - (angular_speed * wheel_distance_) / 2.0;
    out_wheel2_speed =
        linear_speed + (angular_speed * wheel_distance_) / 2.0;
}

3. 下载测试

下载代码，运行agent，点击RST按键。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

看到连接建立表示通信成功，接着用ros2 topic list

ros2 topic list

看到/cmd_vel表示正常，接着我们使用teleop_twist_keyboard进行键盘控制

ros2 run teleop_twist_keyboard teleop_twist_keyboard

先调整下速度，降低到0.05左右（50cm/s），然后使用i\j\j\k,测试。

十三、里程计计算-速度积分

前面两节中我们完成机器人底盘正逆解的计算，我们通过机器人的运动学逆解完成了机器人实时的角速度和线速度的测量，那我们能不能利用对线速度和角速度的积分，计算机器人当前的位置呢？答案肯定是可以的，那么本节我们就来编写代码实现机器人的里程计。

1. 里程计计算原理

在某一个时间段$t$中，机器人的线速度为$v_t$ ，角速度为$w_t$，机器人在初始时刻的位置为 $x_t$ , $y_t$
朝向为 ${\theta }_t$，求经过$t$时刻是机器人新的位置和朝向，这一过程中假设机器人仅在平面上运动。

在这一段时间内机器人前进的距离为$d$
$d=v_t\ast t$

转过的角度为$\theta$
$$\theta ={\omega }_t\ast t$$

则机器人新的角度为${\theta }_{t+1}$

$${\theta }_{t+1}={\theta }_t+\theta$$

我们将机器人前进的距离根据其朝向分解为在x和y轴上的位移量，则可得出

$$x_{t+1}=x_t+d\ast cos({\theta }_{t+1})$$$$y_{t+1}=y_t+d\ast sos({\theta }_{t+1})$$

2. 编写代码

先修改Kinematics.h头文件，增加角度范围限制，里程计更新和里程计结构体定义，完成后代码如下：

#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__
#include <Arduino.h>

typedef struct
{
    
    
    uint8_t id;                // 电机编号
    uint16_t reducation_ratio; // 减速器减速比，轮子转一圈，电机需要转的圈数
    uint16_t pulse_ration;     // 脉冲比，电机转一圈所产生的脉冲数
    float wheel_diameter;      // 轮子的外直径，单位mm

    float per_pulse_distance;  // 无需配置，单个脉冲轮子前进的距离，单位mm，设置时自动计算
                               // 单个脉冲距离=轮子转一圈所行进的距离/轮子转一圈所产生的脉冲数
                               // per_pulse_distance= (wheel_diameter*3.1415926)/(pulse_ration*reducation_ratio)
    uint32_t speed_factor;     // 无需配置，计算速度时使用的速度因子，设置时自动计算，speed_factor计算方式如下
                               // 设 dt（单位us,1s=1000ms=10^6us）时间内的脉冲数为dtick
                               // 速度speed = per_pulse_distance*dtick/(dt/1000/1000)=(per_pulse_distance*1000*1000)*dtic/dt
                               // 记 speed_factor = (per_pulse_distance*1000*1000)
    int16_t motor_speed;       // 无需配置，当前电机速度mm/s，计算时使用
    int64_t last_encoder_tick; // 无需配置，上次电机的编码器读数
    uint64_t last_update_time; // 无需配置，上次更新数据的时间，单位us
} motor_param_t;


/**
 * @brief 里程计相关信息，根据轮子速度信息和运动模型推算而来
 *
 */
typedef struct
{
    
    
    float x;                 // 坐标x
    float y;                 // 坐标y
    float yaw;               // yaw
    float linear_speed;      // 线速度
    float angular_speed;     // 角速度
} odom_t;



class Kinematics
{
    
    
private:
    motor_param_t motor_param_[2];
    float wheel_distance_; // 轮子间距
    odom_t odom_;          // 里程计数据
public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;

    static void TransAngleInPI(float angle,float& out_angle);

    /**
     * @brief 设置电机相关参数
     * 
     * @param id 
     * @param reducation_ratio 
     * @param pulse_ration 
     * @param wheel_diameter 
     */
    void set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter);
    /**
     * @brief 设置运动学相关参数
     * 
     * @param wheel_distance 
     */
    void set_kinematic_param(float wheel_distance);

    /**
     * @brief 运动学逆解，输入机器人当前线速度和角速度，输出左右轮子应该达到的目标速度
     * 
     * @param line_speed 
     * @param angle_speed 
     * @param out_wheel1_speed 
     * @param out_wheel2_speed 
     */
    void kinematic_inverse(float line_speed, float angle_speed, float &out_wheel1_speed, float &out_wheel2_speed);


    /**
     * @brief 运动学正解，输入左右轮子速度，输出机器人当前线速度和角速度
     * 
     * @param wheel1_speed 
     * @param wheel2_speed 
     * @param line_speed 
     * @param angle_speed 
     */
    void kinematic_forward(float wheel1_speed, float wheel2_speed, float &line_speed, float &angle_speed);

    /**
     * @brief 更新轮子的tick数据
     * 
     * @param current_time 
     * @param motor_tick1 
     * @param motor_tick2 
     */
    void update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2);

    /**
     * @brief 获取轮子当前速度
     * 
     * @param id 
     * @return float 
     */
    float motor_speed(uint8_t id);

    /**
     * @brief 更新机器人里程计信息
     * 
     * @param dt 间隔时间dt
     */
    void update_bot_odom(uint32_t dt);
    
    /**
     * @brief 获取里程计函数
     * 
     * @return odom_t& 
     */
    odom_t &odom();

};

#endif // __KINEMATICS_H__

接着在Kinematics.cpp中实现刚刚定义的函数，主要添加函数代码如下：

void Kinematics::update_bot_odom(uint32_t dt)
{
    
    
    static float linear_speed, angular_speed;
    float dt_s = (float)(dt / 1000) / 1000;

    this->kinematic_forward(motor_param_[0].motor_speed, motor_param_[1].motor_speed, linear_speed, angular_speed);

    odom_.angular_speed = angular_speed;
    odom_.linear_speed = linear_speed / 1000; // /1000（mm/s 转 m/s）

    odom_.yaw += odom_.angular_speed * dt_s;

    Kinematics::TransAngleInPI(odom_.yaw, odom_.yaw);
    

    /*更新x和y轴上移动的距离*/
    float delta_distance = odom_.linear_speed * dt_s; // 单位m
    odom_.x += delta_distance * std::cos(odom_.yaw);
    odom_.y += delta_distance * std::sin(odom_.yaw);

}

void Kinematics::TransAngleInPI(float angle, float &out_angle)
{
    
    
    if (angle > PI)
    {
    
    
        out_angle -= 2 * PI;
    }
    else if (angle < -PI)
    {
    
    
        out_angle += 2 * PI;
    }
}

odom_t &Kinematics::odom()
{
    
    
    return odom_;
}

同时修改update_motor_ticks函数，在其中添加update_bot_odom

void Kinematics::update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2)
{
    
    

    uint32_t dt = current_time - motor_param_[0].last_update_time;   // 计算时间差
    int32_t dtick1 = motor_tick1 - motor_param_[0].last_encoder_tick;   // 计算电机1脉冲差
    int32_t dtick2 = motor_tick2 - motor_param_[1].last_encoder_tick;   // 计算电机2脉冲差
    // 轮子速度计算
    motor_param_[0].motor_speed = dtick1 * (motor_param_[0].speed_factor / dt);   // 计算电机1轮子速度
    motor_param_[1].motor_speed = dtick2 * (motor_param_[1].speed_factor / dt);   // 计算电机2轮子速度

    motor_param_[0].last_encoder_tick = motor_tick1;   // 更新电机1上一次的脉冲计数
    motor_param_[1].last_encoder_tick = motor_tick2;   // 更新电机2上一次的脉冲计数
    motor_param_[0].last_update_time = current_time;   // 更新电机1上一次更新时间
    motor_param_[1].last_update_time = current_time;   // 更新电机2上一次更新时间

    // 更新机器人里程计
    this->update_bot_odom(dt);
}

修改main.cpp中加入打印里程计数据

unsigned long previousMillis = 0; // 上一次打印的时间

unsigned long interval = 50;      // 间隔时间，单位为毫秒

void loop()
{
    
    
  static float out_motor_speed[2];
  static uint64_t last_update_info_time = millis();
  kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks());
  out_motor_speed[0] = pid_controller[0].update(kinematics.motor_speed(0));
  out_motor_speed[1] = - pid_controller[1].update(kinematics.motor_speed(1));
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);

  unsigned long currentMillis = millis(); // 获取当前时间
  if (currentMillis - previousMillis >= interval)
  {
    
                                     // 判断是否到达间隔时间
    previousMillis = currentMillis; // 记录上一次打印的时间
    float linear_speed, angle_speed;
    kinematics.kinematic_forward(kinematics.motor_speed(0), kinematics.motor_speed(1), linear_speed, angle_speed);
    Serial.printf("[%ld] linear:%f angle:%f\n", currentMillis, linear_speed, angle_speed);                       // 打印当前时间
    Serial.printf("[%ld] x:%f y:%f yaml:%f\n", currentMillis,kinematics.odom().x, kinematics.odom().y, kinematics.odom().yaw); // 打印当前时间
  }

  // 延迟10毫秒
  delay(10);
}

3. 下载测试

下载代码，运行agent，点击RST按键。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

看到连接建立表示通信成功，接着用ros2 topic list

ros2 topic list

看到/cmd_vel表示正常，接着我们使用teleop_twist_keyboard进行键盘控制

ros2 run teleop_twist_keyboard teleop_twist_keyboard

先调整下速度，降低到0.05左右（50cm/s），然后使用i\j\j\k,测试。

可以先让机器人空转，点击i，让机器人前进用串口查看数据变化。

可以看到每次大约增加0.5左右，数据正常。

十四、采用MicroROS发布里程计

获得了里程计数据后，下一步就是将里程计通过MicroROS话题发布到ROS 2 系统中。

1. 了解接口

在 ROS 2 已有的消息接口中：

nav_msgs/msg/Odometry

用于表示里程计数据，该接口内容如下：

 ros2 interface show nav_msgs/msg/Odometry 
 ---
# This represents an estimate of a position and velocity in free space.
# The pose in this message should be specified in the coordinate frame given by header.frame_id
# The twist in this message should be specified in the coordinate frame given by the child_frame_id

# Includes the frame id of the pose parent.
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# Frame id the pose points to. The twist is in this coordinate frame.
string child_frame_id

# Estimated pose that is typically relative to a fixed world frame.
geometry_msgs/PoseWithCovariance pose
        Pose pose
                Point position
                        float64 x
                        float64 y
                        float64 z
                Quaternion orientation
                        float64 x 0
                        float64 y 0
                        float64 z 0
                        float64 w 1
        float64[36] covariance

# Estimated linear and angular velocity relative to child_frame_id.
geometry_msgs/TwistWithCovariance twist
        Twist twist
                Vector3  linear
                        float64 x
                        float64 y
                        float64 z
                Vector3  angular
                        float64 x
                        float64 y
                        float64 z
        float64[36] covariance

注意看，除了表示位置的 pose 和表示速度的 twist ，还有 child_frame_id 这一参数，它表示里程计子坐标系名称，根据ROS 导航堆栈定义，一般用 base_link 或者 base_footprint 。

接着我们来编写代码。

2. 编写代码

如何发布话题在前面的章节中我们已经学习过了，现在我们来编写代码，因为是直接在原来的代码基础上修改的，

2.1Kinematics.h

首先是/lib/Kinematics/Kinematics.h，增加四元数定义和欧拉角转四元数函数，这是因为ROS中姿态的表示使用的是四元数。

#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__
#include <Arduino.h>

typedef struct
{
    
    
    uint8_t id;                // 电机编号
    uint16_t reducation_ratio; // 减速器减速比，轮子转一圈，电机需要转的圈数
    uint16_t pulse_ration;     // 脉冲比，电机转一圈所产生的脉冲数
    float wheel_diameter;      // 轮子的外直径，单位mm

    float per_pulse_distance;  // 无需配置，单个脉冲轮子前进的距离，单位mm，设置时自动计算
                               // 单个脉冲距离=轮子转一圈所行进的距离/轮子转一圈所产生的脉冲数
                               // per_pulse_distance= (wheel_diameter*3.1415926)/(pulse_ration*reducation_ratio)
    uint32_t speed_factor;     // 无需配置，计算速度时使用的速度因子，设置时自动计算，speed_factor计算方式如下
                               // 设 dt（单位us,1s=1000ms=10^6us）时间内的脉冲数为dtick
                               // 速度speed = per_pulse_distance*dtick/(dt/1000/1000)=(per_pulse_distance*1000*1000)*dtic/dt
                               // 记 speed_factor = (per_pulse_distance*1000*1000)
    int16_t motor_speed;       // 无需配置，当前电机速度mm/s，计算时使用
    int64_t last_encoder_tick; // 无需配置，上次电机的编码器读数
    uint64_t last_update_time; // 无需配置，上次更新数据的时间，单位us
} motor_param_t;


/**
 * @brief 里程计相关信息，根据轮子速度信息和运动模型推算而来
 *
 */

typedef struct
{
    
    
    float w;
    float x;
    float y;
    float z;
} quaternion_t;


typedef struct
{
    
    
     float x;                 // 坐标x
     float y;                 // 坐标y
     float yaw;               // yaw
     quaternion_t quaternion; // 姿态四元数
     float linear_speed;      // 线速度
     float angular_speed;     // 角速度
} odom_t;


class Kinematics
{
    
    
private:
    motor_param_t motor_param_[2];
    float wheel_distance_; // 轮子间距
    odom_t odom_;          // 里程计数据
public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;
    static void Euler2Quaternion(float roll, float pitch, float yaw, quaternion_t &q);

    static void TransAngleInPI(float angle,float& out_angle);

    /**
     * @brief 设置电机相关参数
     * 
     * @param id 
     * @param reducation_ratio 
     * @param pulse_ration 
     * @param wheel_diameter 
     */
    void set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter);
    /**
     * @brief 设置运动学相关参数
     * 
     * @param wheel_distance 
     */
    void set_kinematic_param(float wheel_distance);

    /**
     * @brief 运动学逆解，输入机器人当前线速度和角速度，输出左右轮子应该达到的目标速度
     * 
     * @param line_speed 
     * @param angle_speed 
     * @param out_wheel1_speed 
     * @param out_wheel2_speed 
     */
    void kinematic_inverse(float line_speed, float angle_speed, float &out_wheel1_speed, float &out_wheel2_speed);


    /**
     * @brief 运动学正解，输入左右轮子速度，输出机器人当前线速度和角速度
     * 
     * @param wheel1_speed 
     * @param wheel2_speed 
     * @param line_speed 
     * @param angle_speed 
     */
    void kinematic_forward(float wheel1_speed, float wheel2_speed, float &line_speed, float &angle_speed);

    /**
     * @brief 更新轮子的tick数据
     * 
     * @param current_time 
     * @param motor_tick1 
     * @param motor_tick2 
     */
    void update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2);

    /**
     * @brief 获取轮子当前速度
     * 
     * @param id 
     * @return float 
     */
    float motor_speed(uint8_t id);

    /**
     * @brief 更新机器人里程计信息
     * 
     * @param dt 间隔时间dt
     */
    void update_bot_odom(uint32_t dt);
    
    /**
     * @brief 获取里程计函数
     * 
     * @return odom_t& 
     */
    odom_t &odom();

};

#endif // __KINEMATICS_H__

2.2 Kinematics.cpp

接着是：lib/Kinematics/Kinematics.cpp，增加 Euler2Quaternion 函数实现，在 odom 函数中增加对 Euler2Quaternion 函数的调用。

#include "Kinematics.h"

void Kinematics::set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter)
{
    
    
    motor_param_[id].id = id;   // 设置电机ID
    motor_param_[id].reducation_ratio = reducation_ratio;   // 设置减速比
    motor_param_[id].pulse_ration = pulse_ration;   // 设置脉冲比
    motor_param_[id].wheel_diameter = wheel_diameter;   // 设置车轮直径
    motor_param_[id].per_pulse_distance = (wheel_diameter * PI) / (reducation_ratio * pulse_ration);   // 每个脉冲对应行驶距离
    motor_param_[id].speed_factor = (1000 * 1000) * (wheel_diameter * PI) / (reducation_ratio * pulse_ration);   // 计算速度因子
    Serial.printf("init motor param %d: %f=%f*PI/(%d*%d) speed_factor=%d\n", id, motor_param_[id].per_pulse_distance, wheel_diameter, reducation_ratio, pulse_ration, motor_param_[id].speed_factor);   // 打印调试信息
}

void Kinematics::set_kinematic_param(float wheel_distance)
{
    
    
    wheel_distance_ = wheel_distance;   // 设置轮间距离
}

void Kinematics::update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2)
{
    
    

    uint32_t dt = current_time - motor_param_[0].last_update_time;   // 计算时间差
    int32_t dtick1 = motor_tick1 - motor_param_[0].last_encoder_tick;   // 计算电机1脉冲差
    int32_t dtick2 = motor_tick2 - motor_param_[1].last_encoder_tick;   // 计算电机2脉冲差
    // 轮子速度计算
    motor_param_[0].motor_speed = dtick1 * (motor_param_[0].speed_factor / dt);   // 计算电机1轮子速度
    motor_param_[1].motor_speed = dtick2 * (motor_param_[1].speed_factor / dt);   // 计算电机2轮子速度

    motor_param_[0].last_encoder_tick = motor_tick1;   // 更新电机1上一次的脉冲计数
    motor_param_[1].last_encoder_tick = motor_tick2;   // 更新电机2上一次的脉冲计数
    motor_param_[0].last_update_time = current_time;   // 更新电机1上一次更新时间
    motor_param_[1].last_update_time = current_time;   // 更新电机2上一次更新时间

    // 更新机器人里程计
    this->update_bot_odom(dt);
}



void Kinematics::kinematic_inverse(float linear_speed, float angular_speed, float &out_wheel1_speed, float &out_wheel2_speed)
{
    
    

    out_wheel1_speed =
        linear_speed - (angular_speed * wheel_distance_) / 2.0;
    out_wheel2_speed =
        linear_speed + (angular_speed * wheel_distance_) / 2.0;

}

void Kinematics::kinematic_forward(float wheel1_speed, float wheel2_speed, float &linear_speed, float &angular_speed)
{
    
    
    linear_speed = (wheel1_speed + wheel2_speed) / 2.0;   // 计算线速度
    angular_speed = (wheel2_speed - wheel1_speed) / wheel_distance_;   // 计算角速度
}

float Kinematics::motor_speed(uint8_t id)
{
    
    
    return motor_param_[id].motor_speed; // 返回指定id的轮子速度
} 

void Kinematics::update_bot_odom(uint32_t dt)
{
    
    
    static float linear_speed, angular_speed;
    float dt_s = (float)(dt / 1000) / 1000;

    this->kinematic_forward(motor_param_[0].motor_speed, motor_param_[1].motor_speed, linear_speed, angular_speed);

    odom_.angular_speed = angular_speed;
    odom_.linear_speed = linear_speed / 1000; // /1000（mm/s 转 m/s）

    odom_.yaw += odom_.angular_speed * dt_s;

    Kinematics::TransAngleInPI(odom_.yaw, odom_.yaw);
    

    /*更新x和y轴上移动的距离*/
    float delta_distance = odom_.linear_speed * dt_s; // 单位m
    odom_.x += delta_distance * std::cos(odom_.yaw);
    odom_.y += delta_distance * std::sin(odom_.yaw);

}

void Kinematics::TransAngleInPI(float angle, float &out_angle)
{
    
    
    if (angle > PI)
    {
    
    
        out_angle -= 2 * PI;
    }
    else if (angle < -PI)
    {
    
    
        out_angle += 2 * PI;
    }
}

odom_t &Kinematics::odom()
{
    
    
    // 调用 Euler2Quaternion 函数，将机器人的欧拉角 yaw 转换为四元数 quaternion。
    Kinematics::Euler2Quaternion(0, 0, odom_.yaw, odom_.quaternion);
    return odom_;

}


// 用于将欧拉角转换为四元数。
void Kinematics::Euler2Quaternion(float roll, float pitch, float yaw, quaternion_t &q)
{
    
    
    // 传入机器人的欧拉角 roll、pitch 和 yaw。
    // 计算欧拉角的 sin 和 cos 值，分别保存在 cr、sr、cy、sy、cp、sp 六个变量中    
    // https://blog.csdn.net/xiaoma_bk/article/details/79082629
    double cr = cos(roll * 0.5);
    double sr = sin(roll * 0.5);
    double cy = cos(yaw * 0.5);
    double sy = sin(yaw * 0.5);
    double cp = cos(pitch * 0.5);
    double sp = sin(pitch * 0.5);
    // 计算出四元数的四个分量 q.w、q.x、q.y、q.z
    q.w = cy * cp * cr + sy * sp * sr;
    q.x = cy * cp * sr - sy * sp * cr;
    q.y = sy * cp * sr + cy * sp * cr;
    q.z = sy * cp * cr - cy * sp * sr;
}

2.3 main.cpp

接着修改了 /src/main.cpp ，主要添加了一个发布者，接着对时间进行同步，方便发布里程计话题时使用当前的时间。

然后对数据的各项进行设置，最后添加了里程计数据的发布，间隔 50ms 进行发布。

#include <Arduino.h>
#include <micro_ros_platformio.h>    // 包含用于 ESP32 的 micro-ROS PlatformIO 库
#include <WiFi.h>                    // 包含 ESP32 的 WiFi 库
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 包含 ROS2 geometry_msgs/Twist 消息类型
#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制
#include <Kinematics.h>              // 运动学相关实现
#include <nav_msgs/msg/odometry.h>
#include <micro_ros_utilities/string_utilities.h>
rcl_publisher_t odom_publisher;   // 用于发布机器人的里程计信息（Odom）
nav_msgs__msg__Odometry odom_msg; // 机器人的里程计信息


Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
rclc_executor_t executor;          // 创建一个 RCLC 执行程序对象，用于处理订阅和发布
rclc_support_t support;            // 创建一个 RCLC 支持对象，用于管理 ROS2 上下文和节点
rcl_allocator_t allocator;         // 创建一个 RCL 分配器对象，用于分配内存
rcl_node_t node;                   // 创建一个 RCL 节点对象，用于此基于 ESP32 的机器人小车
rcl_subscription_t subscriber;     // 创建一个 RCL 订阅对象，用于订阅 ROS2 消息
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机

float out_motor_speed[2];        // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
PidController pid_controller[2]; // 创建PidController的两个对象
Kinematics kinematics;           // 运动学相关对象


void twist_callback(const void *msg_in)
{
    
    
  const geometry_msgs__msg__Twist *twist_msg = (const geometry_msgs__msg__Twist *)msg_in;
  static float target_motor_speed1, target_motor_speed2;
  float linear_x = twist_msg->linear.x;   // 获取 Twist 消息的线性 x 分量
  float angular_z = twist_msg->angular.z; // 获取 Twist 消息的角度 z 分量
  kinematics.kinematic_inverse(linear_x * 1000, angular_z, target_motor_speed1, target_motor_speed2);
  pid_controller[0].update_target(target_motor_speed1);
  pid_controller[1].update_target(target_motor_speed2);
}

// 这个函数是一个后台任务，负责设置和处理与 micro-ROS 代理的通信。
void microros_task(void *param)
{
    
    

  // 使用 micro_ros_string_utilities_set 函数设置到 odom_msg.header.frame_id 中
  odom_msg.header.frame_id = micro_ros_string_utilities_set(odom_msg.header.frame_id, "odom");
  odom_msg.child_frame_id = micro_ros_string_utilities_set(odom_msg.child_frame_id, "base_link");
   // 等待 2 秒，以便网络连接得到建立。
   delay(2000);


  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("192.168.43.235");
  Serial.println("Connecting to WiFi...");

  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("LLLLLL", "Laj19980918.", agent_ip, 8888);

  // 等待 2 秒，以便网络连接得到建立。
  delay(2000);

  if (WiFi.status() != WL_CONNECTED)
  {
    
    
    Serial.println("WiFi connection failed");
    return;
  }
  else
  {
    
    
    Serial.println("WiFi connected");
  }

  // 设置 micro-ROS 支持结构、节点和订阅。
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  rclc_node_init_default(&node, "esp32_car", "", &support);
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");

  rclc_publisher_init_best_effort(
      &odom_publisher,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(nav_msgs, msg, Odometry),
      "odom");


  // 设置 micro-ROS 执行器，并将订阅添加到其中。
  rclc_executor_init(&executor, &support.context, 1, &allocator);
  rclc_executor_add_subscription(&executor, &subscriber, &sub_msg, &twist_callback, ON_NEW_DATA);

      // 循环运行 micro-ROS 执行器以处理传入的消息。
      while (true)
      {
    
    
       if (!rmw_uros_epoch_synchronized())
       {
    
    
         rmw_uros_sync_session(1000);
         // 如果时间同步成功，则将当前时间设置为MicroROS代理的时间，并输出调试信息。
         delay(10);
       }
        delay(100);
        rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
      }

}

 unsigned long previousMillis = 0; // 上一次打印的时间

 unsigned long interval = 50;      // 间隔时间，单位为毫秒


void setup()
{
    
    
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 12, 13);
  motor.attachMotor(1, 14, 27);
  // 在引脚32、33和26、25上初始化两个编码器
  encoders[0].init(0, 32, 33);
  encoders[1].init(1, 26, 25);
  // 初始化PID控制器的kp、ki和kd
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  // 初始化PID控制器的最大输入输出，MPCNT大小范围在正负100之间
  pid_controller[0].out_limit(-100, 100);
  pid_controller[1].out_limit(-100, 100);

  // 设置运动学参数
  kinematics.set_motor_param(0, 45, 44, 65);
  kinematics.set_motor_param(1, 45, 44, 65);
  kinematics.set_kinematic_param(150);

  // 在核心0上创建一个名为"microros_task"的任务，栈大小为10240
  xTaskCreatePinnedToCore(microros_task, "microros_task", 10240, NULL, 1, NULL, 0);
}

void loop()
{
    
    
  static float out_motor_speed[2];
  static uint64_t last_update_info_time = millis();
  kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks());
  out_motor_speed[0] = pid_controller[0].update(-kinematics.motor_speed(0));
  out_motor_speed[1] = -pid_controller[1].update(kinematics.motor_speed(1));

  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);

  unsigned long currentMillis = millis(); // 获取当前时间

  if (currentMillis - previousMillis >= interval)
  {
    
     // 判断是否到达间隔时间
    previousMillis = currentMillis; // 记录上一次打印的时间
    float linear_speed, angle_speed;
    kinematics.kinematic_forward(kinematics.motor_speed(0), kinematics.motor_speed(1), linear_speed, angle_speed);
    Serial.printf("[%ld] linear:%f angle:%f\n", currentMillis, linear_speed, angle_speed);                       // 打印当前时间
    Serial.printf("[%ld] x:%f y:%f yaml:%f\n", currentMillis, kinematics.odom().x, kinematics.odom().y, kinematics.odom().yaw); // 打印当前时间


    int64_t stamp = rmw_uros_epoch_millis();
    // 获取机器人的位置和速度信息，并将其存储在一个ROS消息（odom_msg）中
    odom_t odom = kinematics.odom();
    odom_msg.header.stamp.sec = stamp * 1e-3;
    odom_msg.header.stamp.nanosec = stamp - odom_msg.header.stamp.sec * 1000;
    odom_msg.pose.pose.position.x = odom.x;
    odom_msg.pose.pose.position.y = odom.y;
    odom_msg.pose.pose.orientation.w = odom.quaternion.w;
    odom_msg.pose.pose.orientation.x = odom.quaternion.x;
    odom_msg.pose.pose.orientation.y = odom.quaternion.y;
    odom_msg.pose.pose.orientation.z = odom.quaternion.z;

    odom_msg.twist.twist.angular.z = odom.angular_speed;
    odom_msg.twist.twist.linear.x = odom.linear_speed;

    rcl_publish(&odom_publisher, &odom_msg, NULL);


  }

  // 延迟10毫秒
  delay(10);
}

这三个文件修改好，接着就可以下载代码进行测试了。

3. 下载测试

下载代码，运行agent，点击RST按键。

sudo docker run -it --rm -v /dev:/dev -v /dev/shm:/dev/shm --privileged --net=host microros/micro-ros-agent:$ROS_DISTRO udp4 --port 8888 -v6

看到连接建立表示通信成功，接着用ros2 topic list

ros2 topic list
---
/cmd_vel
/odom
/parameter_events
/rosout

接着我们可以查看里程计数据或其发布频率。

ros2 topic echo /odom --once # 查看里程计数据或其发布频率
ros2 topic hz /odom  # 查看数据频率
ros2 topic bw /odom  # 查看数据带宽

有了控制话题和里程计话题，底盘部分就完成的差不多了，但是对于一个合格的底盘来说，其实还有很多待完善的地方，下一节我们就来说说可以怎么完善，以及完善的后的代码。

十五、源码编译Agent

1. 下载microros-agent

首先，我们需要下载MicroROS的Agent源码，并准备相应的依赖。以下是下载和准备的步骤：

1. 安装必要的依赖项：

   sudo apt-get install -y build-essential
   

2. 创建工作空间并进入源码目录：

   mkdir -p microros_ws/src
   cd microros_ws/src
   

3. 下载MicroROS Agent和相关消息包的源码：

   git clone https://github.com/micro-ROS/micro-ROS-Agent.git -b humble
   git clone https://github.com/micro-ROS/micro_ros_msgs.git -b humble
   

2. 编译运行

在成功下载源码并准备好依赖后，我们可以进行编译并运行MicroROS Agent。以下是编译和运行的步骤：

1. 返回工作空间目录并执行编译：

   cd microros_ws
   colcon build
   

2. 运行MicroROS Agent，运行命令如下（假设串口为/dev/ttyUSB0，波特率为921600）：

   ros2 run micro_ros_agent micro_ros_agent serial -b 921600 --dev /dev/ttyUSB0 -v
   

   或者使用UDP方法（二选一）：

   ros2 run micro_ros_agent micro_ros_agent serial udp4 --port 8888 -v6
   ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6
   

可能会存在串口权限问题

单次生效，立即生效

sudo chmod 666 /dev/ttyUSB0

给当前用户添加永久权限，重启生效

sudo usermod -aG dialout `whoami`

十六、可能出现的问题

在humble版本，之前我们都是用PC控制esp32的，但是后续如果继续使用PC控制esp32，并用树莓派发布话题数据时，会出现下面的问题

详细情况就是：先运行esp32控制后，再运行树莓派发布话题，此时esp32控制终端会kill并且显示下面的问题，这其实是前面的ROS版本都存在的问题。

[INFO] [1621451548.255623249] [trajectory_action_server]: Server Ready
terminate called after throwing an instance of 'eprosima::fastcdr::exception::NotEnoughMemoryException'
what():  Not enough memory in the buffer stream

**解决：**

• 方法一：当换成rolling版本后就不会有通信问题（不推荐）

• 方法二：当我们把esp32控制和话题发布终端放在一个设备上，就可以避免这个问题，因此我们可以在树莓派上来进行操作，此时不要忘记修改main.cpp中修改地址

  main.cpp
  在哪个设备上控制esp32就需要将下面的IP地址改为对应的设备。

    // 设置 micro-ROS 代理的 IP 地址。
    IPAddress agent_ip;
    agent_ip.fromString("192.168.43.235");
    Serial.println("Connecting to WiFi...");
  

十七、项目总结与扩展

到此，你已经完整的做好了小车的底盘，其连线方式如下

最终的代码就是第十四节写的，接下来就开始使用树莓派来写上位机的程序。