返回目录

ROS2-FishBot运动学

2026-07-19 ROS2

根据组装说明组装Fishbot机器人，完成后如图：

FishBot机器人

包含的传感器有：

距离传感器——单线旋转式三角测距激光雷达、超声波
轮子速度传感器——编码器
惯性测量单元——IMU传感器
图像传感器——单目、双目、深度摄像头

包含的执行器有：

额定电压12V的370减速电机，额定转速为130转/分、额定电流0.5A，转矩600克力厘米
OLED

驱动电机

Fishbot使用H桥来控制电机的正反转和制动：

该电路由四个独立开关管（MOSFET）组成，每个MOSFET可根据其栅极施加的电压控制导通和截止。当接通Q1和Q4时，电机M正转；当接通Q2和Q3时，电机M反转；全部开关管都截止时，制动。

开发板上采用H桥电路芯片DRV8833，通过AIN1（IO23）的高低电平控制H桥中Q1和Q4的开关，通过AIN2（IO22）控制Q2和Q3的开关

H桥电路芯片

正反转

由H桥原理可知，当AIN1为高电平，AIN2为低电平时，电机正转；当AIN1为低电平，AIN2为高电平时，电机反转；当AIN1和AIN2均为高电平或均为低电平时，电机不动。

代码如下：

/**
 * @file main.cpp
 * @author fishros@foxmail.com
 * @brief 电机正反转控制
 * @version 0.1
 * @date 2022-12-19
 *
 * @copyright Copyright (c) 2022
 *
 */

#include <Arduino.h>

#define AIN1 23 // 电机驱动模块AIN1引脚
#define AIN2 22 // 电机驱动模块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(KEY) == 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;
  }
}

以上代码展示了四种H桥控制状态的结果，

每次按下BOOT按键时，电机状态会在后退、前进、不动、不动四种状态之间循环切换，并通过串口输出当前的控制状态。

速度控制

PWM（Pulse Width Modulation，脉宽调制）是一种利用数字信号（方波）模拟模拟信号（电压大小）的技术。占空比是指方波中高电平持续时间占整个周期的比例。例如，如果电压是 5V，占空比为 50%（一半时间开，一半时间关），那么平均输出电压就是 2.5V

用PWM控制电机速度时，由于线圈有电感，电流不会瞬间突变，高频通断会让电机平滑地改变转速，由此实现速度控制的效果。

/**
 * @file main.cpp
 * @author fishros@foxmail.com
 * @brief 电机速度控制
 * @version 0.1
 * @date 2022-12-19
 * 
 * @copyright Copyright (c) 2022
 * 
 */

#include <Arduino.h>

#define AIN1 23  // 电机驱动模块AIN1引脚
#define AIN2 22  // 电机驱动模块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(KEY) == 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));     // 延迟一段时间，时间长度由占空比决定
}

以上代码展示了使用PWM控制电机速度的原理。每次按下按键时，占空比会增加0.1，当占空比达到1.0时，会重新从0开始计数。由此实现了十一个速度档位的效果。

实际效果为：每次按下按键时，电机的转速会逐渐增加，直到达到最大速度，然后再次从零开始。

使用delay()函数来控制PWM并不是最优的方式，因为它会阻塞程序的执行，在实际应用中可使用开源库。

速度控制 - MCPWM

MCPWM（Motor Control Pulse Width Modulator，电机控制脉宽调制器）是ESP32提供的一个硬件PWM控制模块，在本实验中使用Esp32McpwmMotor库来驱动

添加依赖：

lib_deps = 
    https://github.com/fishros/Esp32McpwmMotor.git

编写代码：

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

#define AIN1 23  // 电机驱动模块AIN1引脚
#define AIN2 22  // 电机驱动模块AIN2引脚
#define BIN1 12  // 电机驱动模块BIN1引脚
#define BIN2 13  // 电机驱动模块BIN2引脚

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

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

    motor.attachMotor(0, AIN1, AIN2); // 电机0控制A轮
    motor.attachMotor(1, BIN1, BIN2); // 电机1控制B轮
}

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); // 延迟两秒
}

以上代码以2秒为周期，控制电机0和电机1的速度分别为正负70%（两个电机方向应相反），实现了前进和后退的效果。

转向控制 - 订阅Twist话题

通过让小车订阅Twist消息来控制电机，可以实现小车的前进、后退、左转、右转等动作的远程遥控。Twist包含线速度和角速度信息

添加依赖：

board_microros_transport = wifi  ; 指定使用的Micro-ROS传输方式为Wi-Fi
lib_deps =
    https://github.com/fishros/Esp32McpwmMotor.git  ; ESP32-MCPWM-Motor库，用于驱动电机
    https://gitee.com/ohhuo/micro_ros_platformio.git  ; Micro-ROS平台库，用于在ESP32上运行ROS 2

编写代码：

#include <Arduino.h>
#include <Esp32McpwmMotor.h>  // 电机控制
#include <micro_ros_platformio.h>  // micro-ROS 平台库
#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, 22, 23);
  motor.attachMotor(1, 12, 13);

  // micro-ROS 通信参数，根据实际情况修改
  IPAddress agent_ip;
  agent_ip.fromString("10.28.8.94");
  set_microros_wifi_transports("fishbot", "12345678", agent_ip, 8888);
  delay(2000);

  // 初始化 ROS2 执行器和支持结构
  allocator = rcl_get_default_allocator();
  rclc_support_init(&support, 0, NULL, &allocator);
  // 初始化 ROS2 节点 esp32_car
  rclc_node_init_default(&node, "esp32_car", "", &support);
  // 初始化订阅者，订阅 /cmd_vel 话题，消息类型为 geometry_msgs/msg/Twist
  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)); // 循环处理数据
}

以上代码实现了一个ROS2节点esp32_car，该节点订阅了/cmd_vel话题，接收Twist消息，并根据消息中的线速度和角速度控制两个电机的选择，实现小车的前进、后退、左转、右转等动作，但并未实现多档速度控制。

构建下载至开发板，启动docker agent

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

运行键盘控制节点，使用I/J/K/L控制小车：

ros2 run teleop_twist_keyboard teleop_twist_keyboard

Agent teleop_twist_keyboard

fishbot teleop_twist_keyboard

编码器测量速度

当使用PWM控制电机时，电机的转速会受到负载、摩擦等因素的影响，导致实际转速与设定转速不一致，例如小车无法精确地走直线。为此需要将开环控制改成闭环控制，根据实际情况实时调整电机的转速。

AB电磁编码器的圆形磁铁固定在电机的转子上，当电机转动时，带动磁铁转动，此时用于检测磁性的霍尔传感器就会检测到磁性的变化。

编码器直接连接到了单片机IO上（32、33和26、25），当电机转动时，IO上的电平高低就会产生变化，从而产生低-高-低的脉冲信号。通过计算单位时间内的脉冲数，就可以计算出电机的转速。

Fishbot电机轮子直径为65mm，当轮子转一圈时产生N个脉冲，则1个脉冲前进的距离D=(π*65)/N mm。

某一段时间ΔT(ms)内有P个脉冲，则轮子转速为V=(D*P)/ΔT m/s。

测量一个脉冲前进距离D

Esp32PcntEncoder库调用了ESP32的脉冲计算外设进行编码器脉冲的计算

添加依赖：

lib_deps =
    https://github.com/fishros/Esp32PcntEncoder.git

编写代码如下：

#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());
}

以上代码实现了对两个编码器的初始化和读取脉冲数。

构建并下载至开发板，打开串口监视器，手动将其中一个轮子转动10圈

编码器脉冲数

可看到其中一个轮子的脉冲数为tick1=19608，可计算得当前电机为1960.8脉冲/圈，则1个脉冲前进的距离为D=(π*65)/1960.8=0.10414296mm。

测量最大速度

已知D，在时间ΔT内测量脉冲数P，则轮子转速为V=(D*P)/ΔT m/s。

仍然使用Esp32PcntEncoder库，代码如下：

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

#define D 0.10414296

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] * D) / dt; // 计算第一个电机的速度
  speeds[1] = float(pt[1] * D) / 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("speed1=%f,speed2=%f\n", speeds[0], speeds[1]); // 打印两个电机的速度
}

以上代码测量了电机的最大速度，构建并下载至开发板，打开串口监视器，可看到电机全速运行时的速度约为0.47m/s

最大速度

PID控制

获取实际速度反馈之后，需要调整电机实现闭环控制，使用PID控制器来实现。

PID（Proportional-Integral-Derivative，比例-积分-微分）控制器是一种常用的反馈控制器。PID控制器通过测量目标系统的反馈信号和期望输出信号之间的误差，根据一定的数学模型计算出控制信号，使目标系统能够稳定地达到期望输出。公式如下：

PID公式

其中，Kp、Ki和Kd分别表示比例系数、积分系数和微分系数，Error表示目标系统的误差。PID参数调节可参看PID参数调节浅谈

添加依赖：

board_microros_transport = wifi
lib_deps = 
    https://gitee.com/ohhuo/micro_ros_platformio.git
    https://github.com/fishros/Esp32McpwmMotor.git
    https://github.com/fishros/Esp32PcntEncoder.git

lib下新建PidController文件夹，添加PidController.h和PidController.cpp文件

── lib
   ├── PidController
   │   ├── PidController.cpp
   │   └── PidController.h
   └── README

定义Pid控制器类如下，PidController.h：

#ifndef __PIDCONTROLLER_H__ 
#define __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

其中reset()，update_pid()，update_target()和out_limit()方法仅为简单赋值，update方法根据公式、参数和控制值计算返回输出值，并限制输出值在特定范围内。对于PidController类定义的方法实现如下：

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; // 控制输出最大值
}

编写主程序，main.cpp：

#include <Arduino.h>
#include <micro_ros_platformio.h>    // Micro-ROS
#include <WiFi.h>                    // WiFi

#include <rcl/rcl.h>             
#include <rclc/rclc.h>           
#include <rclc/executor.h>       
#include <geometry_msgs/msg/twist.h> // Twist 消息类型

#include <Esp32PcntEncoder.h>        // 脉冲计数编码器库
#include <Esp32McpwmMotor.h>         // PWM 电机控制库
#include <PidController.h>           // PID 控制器库


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 消息对象

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机
float out_motor_speed[2];          // 创建一个长度为 2 的浮点数数组，用于保存输出电机速度
float current_speeds[2];           // 创建一个长度为 2 的浮点数数组，用于保存当前电机速度
PidController pid_controller[2];   // 创建PidController的两个对象

// /cmd_vel话题的回调函数
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); // 停止0号电机
    motor.updateMotorSpeed(1, 0); // 停止1号电机
    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 Agent的通信。
void microros_task(void *param)
{
  // 设置 micro-ROS 代理的 IP 地址，根据实际情况修改
  IPAddress agent_ip;
  agent_ip.fromString("10.28.8.94");

  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("fishbot", "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.104143) / dt * 1000;
  current_speeds[1] = float(pt[1] * 0.104143) / 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, 22, 23);
  motor.attachMotor(1, 12, 13);
  // 在引脚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]);
  // 更新电机0和电机1的速度值
  motor.updateMotorSpeed(0, out_motor_speed[0]);
  motor.updateMotorSpeed(1, out_motor_speed[1]);
  // 延迟10毫秒
  delay(10);
}

以上代码创建了一个闭环控制系统。

在核心0上：运行microros_task任务，接收/cmd_vel话题的Twist消息，并设置目标速度。
在核心1上：运行主循环loop，通过编码器测量当前速度current_speeds，并使用PID控制器计算输出速度out_motor_speed，控制电机的转速

编译下载，运行agent与键盘控制器，控制小车前进:

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 run teleop_twist_keyboard teleop_twist_keyboard

可以看到使用闭环控制后，前进时小车行驶得更直了。

两轮差速

以上只讨论了单个电机的速度控制，实际描述速度通常描述的是小车的整体速度。

两轮差速模型指机器人底盘由两个驱动轮和若干支撑轮构成的底盘模型，根据两个驱动轮的转速，可以让小车达到特定的线速度和角速度。两轮差速运动学可以分为正运动学和逆运动学两种：

正运动学即由两个轮子的转速计算小车的线速度和角速度。
逆运动学即由小车的线速度和角速度计算两个轮子的转速。

正运动学

机器人的整体线速度为左右轮的平均值，即v = (v_l + v_r) / 2

由轮间距l = r_r - r_l = v_r/w - v_l/w，可得角速度w = (v_r - v_l) / l

逆运动学

由正运动学公式可得：

v_r = v + w * l / 2
v_l = v - w * l / 2

运动学应用

修改platformio.ini，添加依赖：

board_microros_transport = wifi
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

在lib下添加PidController库如上，另外添加Kinematics运动学库，其中新建Kinematics.h、Kinematics.cpp文件

── lib
   ├── Kinematics
   │   ├── Kinematics.cpp
   │   └── Kinematics.h
   ├── PidController
   │   ├── PidController.cpp
   │   └── PidController.h
   └── README

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;

    void set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter);
    void set_kinematic_param(float wheel_distance);
    void kinematic_inverse(float line_speed, float angle_speed, float &out_wheel1_speed, float &out_wheel2_speed);
    void kinematic_forward(float wheel1_speed, float wheel2_speed, float &line_speed, float &angle_speed);
    void update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2);
    float motor_speed(uint8_t id);
};

#endif // __KINEMATICS_H__

在Kinematics.h中定义了一个motor_param_t结构体，用于存储电机的参数，同时定义了一个Kinematics类，有一个大小为2的motor_param_数组，并有各种方法。

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)
{
    // out_wheel1_speed = 20;
    // out_wheel2_speed = 20;
    out_wheel1_speed = linear_speed - (angular_speed * wheel_distance_ / 2.0);   // 计算电机1轮子速度
    out_wheel2_speed = linear_speed + (angular_speed * wheel_distance_ / 2.0);   // 计算电机2轮子速度
}

// 根据轮子速度计算线速度和角速度
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_;   // 计算角速度
}

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

在Kinematics.cpp中实现了Kinematics类的各个方法

set_motor_param、set_kinematic_param用于设置参数，并使用公式进行参数计算
update_motor_ticks用于根据编码器读数计算轮子速度
motor_speed用于获取指定id的轮子速度
核心在于kinematic_inverse和kinematic_forward方法，kinematic_inverse可用于根据目标线速度和角速度计算目标轮子速度，kinematic_forward可用于根据当前轮子速度计算实际线速度和角速度。

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>              // 运动学相关实现

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 消息对象

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机

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

// 回调函数，更新两个PID控制器的目标速度
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("10.28.8.94");

  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("fishbot", "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 setup()
{
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 22, 23);
  motor.attachMotor(1, 12, 13);
  // 在引脚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 uint64_t last_update_info_time = millis();
  // 计算轮速
  kinematics.update_motor_ticks(micros(), encoders[0].getTicks(), encoders[1].getTicks());
  // 每隔1000毫秒打印一次整体速度
  if (millis() - last_update_info_time > 1000)
  {
    last_update_info_time = millis();
    float line_speed, angle_speed;
    kinematics.kinematic_forward(kinematics.motor_speed(0), kinematics.motor_speed(1), line_speed, angle_speed);
    Serial.printf("line_speed: %.2f mm/s, angle_speed: %.2f rad/s\n", line_speed, angle_speed);
  }
  // 根据当前轮速计算PID控制器的输出
  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]);
  // 延迟10毫秒
  delay(10);
}

以上代码对之前有关运动学的部分进行了封装，并在上一次代码的基础上实现了角速度的控制。通过kinematic_inverse方法将目标线速度和角速度转换为两个轮子的目标速度，并使用PID控制器进行闭环控制。通过kinematic_forward方法可以将当前轮速转换为整体线速度和角速度，并打印输出。

为进行测试，临时固定目标线速度为20mm/s，修改kinematic_inverse方法：

void Kinematics::kinematic_inverse(float linear_speed, float angular_speed, float &out_wheel1_speed, float &out_wheel2_speed)
{
    // 无论输入的线速度和角速度是多少，直接设置两个轮子的速度为20mm/s，即整体线速度为20mm/s，角速度为0
    out_wheel1_speed = 20;
    out_wheel2_speed = 20;
}

编译、下载、启动agent

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

启动键盘控制器，随意发送/cmd_vel话题信息激活小车kinematic_inverse方法:

ros2 run teleop_twist_keyboard teleop_twist_keyboard

串口输出在20mm/s左右徘徊，与预期相符：

整体速度

里程计

可使用简单的公式近似求出小车的里程计信息

一段时间内前进的距离 d = v * t，旋转的角度 Δθ = w * t，则可求出在x和y方向的位移量：

θ = θ + Δθ
x = x + d * cos(θ)
y = y + d * sin(θ)

修改Kinematics.h，添加里程计结构并增加Kinematics类的成员变量和方法：

// ..SNIP..
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];
    odom_t odom_;          // 里程计数据
    float wheel_distance_;
public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;
    // 增加里程计相关方法
    odom_t &odom();
    static void TransAngleInPI(float angle,float& out_angle);
    void update_bot_odom(uint32_t dt);
    // 运动学方法
    void set_motor_param(uint8_t id, uint16_t reducation_ratio, uint16_t pulse_ration, float wheel_diameter);
    void set_kinematic_param(float wheel_distance);
    void kinematic_inverse(float line_speed, float angle_speed, float &out_wheel1_speed, float &out_wheel2_speed);
    void kinematic_forward(float wheel1_speed, float wheel2_speed, float &line_speed, float &angle_speed);
    void update_motor_ticks(uint64_t current_time, int32_t motor_tick1, int32_t motor_tick2);
    float motor_speed(uint8_t id);
};
// ..SNIP..

修改Kinematics.cpp，实现里程计相关方法，并在update_motor_ticks方法中调用更新里程计：

// ..SNIP..

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;  
    int32_t dtick2 = motor_tick2 - motor_param_[1].last_encoder_tick;   

    motor_param_[0].motor_speed = dtick1 * (motor_param_[0].speed_factor / dt);   
    motor_param_[1].motor_speed = dtick2 * (motor_param_[1].speed_factor / dt);   

    motor_param_[0].last_encoder_tick = motor_tick1;   
    motor_param_[1].last_encoder_tick = motor_tick2;   
    motor_param_[0].last_update_time = current_time;   
    motor_param_[1].last_update_time = current_time;   
    // 更新机器人里程计
    this->update_bot_odom(dt);
}

// 输入经过的时间dt，更新机器人里程计数据odom_
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);

}

// 将角度限制为[-PI, PI]范围内
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_;
}

修改main.cpp，在loop中打印里程计信息：

void loop()
{
  static float out_motor_speed[2];
  static uint64_t previousMillis = 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]);

  uint64_t currentMillis = millis(); // 获取当前时间
  if (currentMillis - previousMillis >= 1000)
  {                                 // 每1000ms打印一次
    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);
}

编译下载，运行agent与键盘控制器，在控制器中先使用w/x调整速度至0.05（m/s）左右，再使用i控制小车前进，可看到串口输出的里程计信息每次增加0.05（m）左右，线速度约为50（cm/s），符合预期

里程计

发布里程计信息

ROS2已有消息接口nav_msgs/msg/Odometry，使用ros2 interface show nav_msgs/msg/Odometry查看消息结构：

# 头部信息，包含时间戳stamp和父坐标系名称frame_id（参考坐标系）
std_msgs/Header header
        builtin_interfaces/Time stamp
                int32 sec
                uint32 nanosec
        string frame_id

# 子坐标系名称child_frame_id（机器人坐标系）
string child_frame_id

# 位姿估计，包括位姿pose和协方差covariance，在固定的世界坐标系下
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

# 速度估计，包括速度twist和协方差covariance，在子坐标系下
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

修改Kinematics.h，增加四元数相关结构和方法：

// ...SNIP..
// 四元数结构
typedef struct
{
    float w;
    float x;
    float y;
    float z;
} quaternion_t;

typedef struct
{
    float x;             
    float y;             
    float yaw;           
    quaternion_t quaternion; // 里程计的四元数表示
    float linear_speed;  
    float angular_speed;   
} odom_t;

class Kinematics
{
private:
    motor_param_t motor_param_[2];
    odom_t odom_;      
    float wheel_distance_;
public:
    Kinematics(/* args */) = default;
    ~Kinematics() = default;

    odom_t &odom();
    // 增加欧拉角转四元数方法 Euler2Quaternion
    static void Euler2Quaternion(float roll, float pitch, float yaw, quaternion_t &q);
    static void TransAngleInPI(float angle,float& out_angle);
    void update_bot_odom(uint32_t dt);
// ...SNIP..
};

修改Kinematics.cpp，实现欧拉角转四元数方法：

// ...SNIP..
// 欧拉角转四元数
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;
}
// 获取里程计数据
odom_t &Kinematics::odom()
{

    Kinematics::Euler2Quaternion(0, 0, odom_.yaw, odom_.quaternion);
    return odom_;
}

修改main.cpp，增加里程计发布相关代码：

#include <Arduino.h>
#include <micro_ros_platformio.h>    // micro-ROS PlatformIO 库
#include <micro_ros_utilities/string_utilities.h> // string处理
#include <WiFi.h>                    // 无限通信
#include <rcl/rcl.h>                 // 包含 ROS 客户端库 (RCL)
#include <rclc/rclc.h>               // 包含用于 C 的 ROS 客户端库 (RCLC)
#include <rclc/executor.h>           // 包含 RCLC 执行程序库，用于执行订阅和发布
#include <geometry_msgs/msg/twist.h> // 消息类型
#include <nav_msgs/msg/odometry.h>   // 消息类型

#include <Esp32PcntEncoder.h>        // 包含用于计数电机编码器脉冲的 ESP32 PCNT 编码器库
#include <Esp32McpwmMotor.h>         // 包含使用 ESP32 的 MCPWM 硬件模块控制 DC 电机的 ESP32 MCPWM 电机库
#include <PidController.h>           // 包含 PID 控制器库，用于实现 PID 控制
#include <Kinematics.h>              // 运动学相关实现

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 订阅对象，订阅/cmd_vel 主题
rcl_publisher_t odom_publisher;       // 创建一个 RCL 发布对象，发布/odom 主题
geometry_msgs__msg__Twist sub_msg; // 创建一个 ROS2 geometry_msgs/Twist 消息对象
nav_msgs__msg__Odometry odom_msg;   // 创建一个 ROS2 nav_msgs/Odometry 消息对象

Esp32PcntEncoder encoders[2];      // 创建一个长度为 2 的 ESP32 PCNT 编码器数组
Esp32McpwmMotor motor;             // 创建一个 ESP32 MCPWM 电机对象，用于控制 DC 电机

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

// 回调函数，更新两个PID控制器的目标速度
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)
{
  // 初始化里程计消息的主坐标系和副坐标系 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");
  // 设置 micro-ROS 代理的 IP 地址。
  IPAddress agent_ip;
  agent_ip.fromString("10.28.8.94");
  // 使用 WiFi 网络和代理 IP 设置 micro-ROS 传输层。
  set_microros_wifi_transports("fishbot", "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);
  // 初始化subscription，订阅/cmd_vel
  rclc_subscription_init_default(
      &subscriber,
      &node,
      ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
      "/cmd_vel");
  // 初始化publisher，在loop中发布/odom
  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);
      delay(10);
    }
    delay(100);
    // 运行 micro-ROS 执行器。
    rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100));
  }
}


void setup()
{
  // 初始化串口通信，波特率为115200
  Serial.begin(115200);
  // 将两个电机分别连接到引脚22、23和12、13上
  motor.attachMotor(0, 22, 23);
  motor.attachMotor(1, 12, 13);
  // 在引脚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 previousMillis = 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]);

  uint64_t currentMillis = millis(); // 获取当前时间
  if (currentMillis - previousMillis >= 1000)
  {                                 // 每1000ms打印一次
    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(); // 时间戳
    odom_t odom = kinematics.odom();
    odom_msg.header.stamp.sec = static_cast<int32_t>(stamp / 1000); // 毫秒，int32_t
    odom_msg.header.stamp.nanosec = static_cast<uint32_t>((stamp % 1000) * 1e6); // 纳秒，uint32_t
    // 填充位姿信息至待发布的odom_msg中
    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中
    odom_msg.twist.twist.linear.x = odom.linear_speed;
    odom_msg.twist.twist.angular.z = odom.angular_speed;
    // 发布odom消息
    rcl_publish(&odom_publisher, &odom_msg, NULL);
  }

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

以上代码新增了一个odom_publisher，在microros_task中初始化，并在loop中每隔1秒发布一次里程计信息至/odom话题。

编译下载，运行agent和键盘控制器，可在上位机中看到里程计话题，同时agent日志能看到每秒传来的数据：

里程计Agent

在上位机中查看/odom话题信息，若看不到可按下RST按钮重启：

# 查看话题列表
ros2 topic list
# 打印odom话题信息
ros2 topic echo /odom

里程计话题

header:
  stamp:
    sec: 1784283251
    nanosec: 791000000
  frame_id: odom
child_frame_id: base_link
pose:
  pose:
    position:
      x: 0.40502551198005676
      y: 0.0003706401912495494
      z: 0.0
    orientation:
      x: 0.0
      y: 0.0
      z: 0.0007333334069699049
      w: 0.9999997019767761
  covariance:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
twist:
  twist:
    linear:
      x: 0.05000000074505806
      y: 0.0
      z: 0.0
    angular:
      x: 0.0
      y: 0.0
      z: 0.0
  covariance:
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
  - 0.0
---

完整项目

完整项目可在github fishbot_motion_control_microros下载，IMU模块，里程计模块，OLED模块等都在该项目中整合了。

git clone https://github.com/fishros/fishbot_motion_control_microros.git

根据实际情况修改配置include/fishbot_config.h：

/*=========================================默认值定义=====================================*/
#define CONFIG_DEFAULT_TRANSPORT_MODE_WIFI_SERVER_IP "192.168.2.105" // 默认UDP服务端IP
#define CONFIG_DEFAULT_TRANSPORT_MODE_WIFI_SERVER_PORT "8888"        // 默认UDP服务端端口号
#define CONFIG_DEFAULT_TRANSPORT_MODE "udp_client"                   // 默认传输模式-udp_client模式
#define CONFIG_DEFAULT_SERIAL_ID "0"                                 // 可选使用0或者2,使用2则需要使用GPIO16和17作为RXTX
#define CONFIG_DEFAULT_TRANSPORT_SERIAL_BAUD "921600"

//------------------------------------WIFI SSID-----------------------------------------
#define CONFIG_DEFAULT_WIFI_STA_SSID "m5"
#define CONFIG_DEFAULT_WIFI_STA_PSWK "88888888"

编译下载后，OLED屏显示连接状态、电池电压、速度、时间信息

完整项目

在8888端口上运行UDP Agent，可看到ROS2话题信息：

ros2 topic list

/cmd_vel
/imu
/odom
/parameter_events
/rosout

优化：从源码编译Micro-ROS Agent

# 安装依赖
sudo apt-get install -y build-essential
# 创建工作空间
mkdir -p microros_ws/src
cd microros_ws/src
# 克隆源码
git clone https://github.com/micro-ROS/micro-ROS-Agent.git -b $ROS_DISTRO
git clone https://github.com/micro-ROS/micro_ros_msgs.git -b $ROS_DISTRO
# 在microros_ws目录下编译
cd ..
colcon build

使用上，与一般的ROS2功能包相似，使用ros2 run启动

# source安装环境，也可写入~/.bashrc文件中永久生效
source install/setup.bash 
# 启动Agent（使用UDP）
ros2 run micro_ros_agent micro_ros_agent udp4 --port 8888 -v6

ROS2-FishBot运动学

驱动电机

正反转

速度控制

速度控制 - MCPWM

转向控制 - 订阅Twist话题

编码器测量速度

测量一个脉冲前进距离D

测量最大速度

PID控制

两轮差速

正运动学

逆运动学

运动学应用

里程计

发布里程计信息

完整项目

优化：从源码编译Micro-ROS Agent

留言