提炼总结—ROS2机器人开发（第9章）（中）

写在最前面的话

为什么做该博客？该博客的特点是什么？
随着DeepSeek、ChatGPT等AI技术的崛起，促使机器人技术发展到了新的高度，诞生了宇树科技、特斯拉为代表的人形机器人，四足机器人等等，越来越多的科技巨头涌入机器人赛道，行业对于相关人才的需求也随之达到了顶峰。本博客的内容是替你阅读所有关于机器人的经典书籍，采用书籍瘦身计划，帮你提炼出核心内容，采用最通俗易懂的语言来解释原理，将书读薄。大大缩短学习时间，助你快速成为机器人时代的佼佼者。

一、书籍介绍

《ROS2机器人开发 从入门到实践》是一本从零基础到实战落地的机器人开发指南，通过"基础理论+工具详解+项目实战"的三阶学习路径，带你掌握机器人操作系统ROS2核心架构。书中不仅包含通信机制、URDF建模等必备知识，更有SLAM导航、机械臂控制等12个工业级案例，配合Git代码库和仿真环境配置指南，帮助开发者快速实现从算法仿真到真机部署的完整闭环。无论是学生、工程师还是科研人员，都能通过本书构建系统的ROS2开发能力，抢占机器人技术前沿阵地。

二、第9章 搭建一个实体移动机器人（提炼总结）

9.3节 机器人控制系统的实现（提炼总结）

• FishBot的底盘上有两个连接电动机的驱动轮和一个万向轮，其转弯原理和两轮平衡车类似，所以该地盘模型又称为两轮差速模型

9.3.1节 使用开源库驱动多路电动机（提炼总结）

• FishBot开发板采用DRV8833芯片来实现电动机的驱动

1、新建工程fishbot_motion_contro，接着修改platformio.ini，添加依赖库Esp32McpwmMotor

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

该库可以同时控制6个直流电动机
2、打开src/main.cpp，编写如下内容：

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

Esp32McpwmMotor motor;
void setup() {
  motor.attach(0,22,23);
  motor.attach(1,12,13);
}

void loop() {
  motor.updateMotorSpeed(0, 70);
  motor.updateMotorSpeed(1, 70);
  delay(2000);

  motor.updateMotorSpeed(0, -70);
  motor.updateMotorSpeed(1, -70);
  delay(2000);
}

attachMotor()用于连接电动机，第一个参数是电动机编号，后面两个是电动机的引脚。updateMotorSpeed()用于更新电动机速度，第一个参数是电动机编号，第二个是电动机速度百分比
将代码编译下载到开发板，可以看到电动机已经转起来了

9.3.2节 电动机速度测量与转换（提炼总结）

• FishBot采用的电动机都安装了两个霍尔传感器

1、修改platformio.ini，添加依赖库Esp32PcntEncoder，是脉冲计算开源库

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

2、打开src/main.cpp，编写如下内容：

#include <Arduino.h>
#include <Esp32PcntEncoder.h>
Esp32PcntEncoder encoder[2];

void setup() {
  Serial.begin(115200);
  encoder[0].init(0, 32, 33);
  encoder[1].init(1, 26, 25);
}

void loop() {
  delay(10);
  Serial.printf("tick1=%d, tick2=%d\n", encoder[0].getTick(), encoder[1].getTick());
}

下载代码到开发板，打开串行监视器，FishBot采用的电动机直径为65mm，乘以圆周率后就可以得到转动一圈行走的距离，出粮储电动机转动一周的脉冲数后就可以计算出单个脉冲对应的距离
3、有了参数再次编写代码，将脉冲数转换为速度

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

Esp32McpwmMotor motor;
Esp32PcntEncoder encoder[2];

int64_t last_ticks[2];
int32_t delta_ticks[2];
int64_t last_update_time;
float current_speed[2];

void setup() {
  Serial.begin(115200);
  encoder[0].init(0, 32, 33);
  encoder[1].init(1, 26, 25);

  motor.attachMotor(0, 23, 24);
  motor.attachMotor(1, 12, 13);
  motor.updateMotorSpeed(0, 70);
  motor.updateMotorSpeed(1, 70);
}

void loop() {
  delay(10);
  unit64_t dt=millis() - last_update_time;

  delta_ticks[0] = encoder[0].getTicks() - last_ticks[0];
  delta_ticks[1] = encoder[1].getTicks() - last_ticks[1];

  current_speed[0] = float(delta_ticks[0]*0.1051566) / dt;
  current_speed[1] = float(delta_ticks[1]*0.1051566) / dt;

  last_update_time = millis();
  last_ticks[0] = encoder[0].getTicks();
  last_ticks[1] = encoder[1].getTicks();

  Serial.printf("speed1=%f, speed2=%f\n", current_speed[0], current_speed[1]);
}

9.3.3节 使用PID控制轮子转速（提炼总结）

• 即使设置相同的速度，两个电动机的实际转速并不一致，如果要保持相同的转速，则要根据电动机来动态调节速度值，可以使用PID控制器来实现这一功能

1、在lib目录下新建PidController，在目录下新建PidController.h和PidController.cpp，PidController.h程序如下：

#ifndef __PID_CONTROLLER_H__
#define __PID_CONTROLLER_H__ 

class PidController { 
    public:
        PidController() = default;
        PidController(float kp, float ki, float kd);

    private:
        float target_;
        float out_min_;
        float out_max_;
        float kp_;
        float ki_;
        float kd_;

        float error_sum_;
        float derror_;
        float error_last_;
        float error_pre_;
        float intergral_up_ = 2500;

    public:
        float update(float current);
        void update_target(float target);
        void update_pid(float kp, float ki, float kd);
        void reset();
        void out_limit(float out_min, float out_max);

};

#endif

2、编写PidController.cpp程序

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

PidController::PidController(double kp, double ki, double kd, double dt)
{
    reset(); 
    update_pid(kp, ki, kd, dt);
}

float PidController::update(float current)
{ 
    float error = target_ - current;
    derror_ = error_last_ - error;
    error_last_ = error;

    error_sum_ += error;
    if (error_sum_ > intergral_up_)
    {
        error_sum_ = intergral_up_;
    }
    else 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_;
    }
    else if (output < out_min_)
    {
        output = out_min_;
    }
    return output;
}

void PidController::update_pid(double kp, double ki, double kd)
{
    reset(); 
    kp_ = kp;
    ki_ = ki;
    kd_ = kd;
}

void PidController::reset()
{
    target_ = 0.0f;
    out_min_ = 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::output_limit(float out_min, float out_max)
{
    out_min_ = out_min;
    out_max_ = out_max;
}

3、修改main.cpp，将PID控制器引入到代码中

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

Esp32McpwmMotor motor;
Esp32PcntEncoder encoder[2];
PidController pid_controller[2];

int64_t last_ticks[2];
int32_t delta_ticks[2];
int64_t last_update_time;
float current_speed[2];

void motorSpeedControl() {
  uint64_t dt=millis() - last_update_time;
  delta_ticks[0] = encoder[0].getTicks() - last_ticks[0];
  delta_ticks[1] = encoder[1].getTicks() - last_ticks[1];
  current_speed[0] = float(delta_ticks[0]*0.1051566) / dt;
  current_speed[1] = float(delta_ticks[1]*0.1051566) / dt;
  last_update_time = millis();
  last_ticks[0] = encoder[0].getTicks();
  last_ticks[1] = encoder[1].getTicks();
  motor.updateMotorSpeed(0, pid_controller[0].update(current_speed[0]));
  motor.updateMotorSpeed(1, pid_controller[1].update(current_speed[1]));
  Serial.printf("speed1=%f, speed2=%f\n", current_speed[0], current_speed[1]);
}
void setup() {
  Serial.begin(115200);
  encoder[0].init(0, 32, 33);
  encoder[1].init(1, 26, 25);

  motor.attachMotor(0, 23, 24);
  motor.attachMotor(1, 12, 13);
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  pid_controller[0].out_limit(-100,100)
  pid_controller[1].out_limit(-100,100)
  pid_controller[0].update_target(100);
  pid_controller[1].update_target(100);
}

void loop() {
  delay(10);
  motorSpeedControl();
}

4、下载代码，打开串口监视器，可以看到速度和设定的非常接近

9.3.4节 运动学正逆解的实现（提炼总结）

• 目前可以控制机器人的两个轮子按照设定的速度转动，但是轮子的速度要和机器人的速度进行转换还需要经过运动学正逆解才可以

1、在lib下新建目录Kinematics，在目录下新建Kinematics.h文件，编写如下代码：

#ifndef __KINEMATICS_H__
#define __KINEMATICS_H__

#include "Arduino.h"

typedef struct {
  float per_pulse_distance;
  int16_t motor_speed;
  int64_t last_encoder_tick;
} motor_param_t;

class Kinematics {
  public:
    Kinematics() = default;
    ~Kinematics() = default;
    void set_motor_param(uint8_t id,float per_pulse_distance);
    void set_wheel_distance(float wheel_distance);
    void kinematics_inverse(float linear_speed,float angular_speed,float &out_left_speed,float &out_right_speed);
    void kinematics_forward(float left_speed,float right_speed,float &out_linear_speed,float &out_angular_speed);
    void update_motor_speed(uint64_t current_time,int32_t left_tick,int32_t right_tick);
    int16_t get_motor_speed(uint8_t id);

  private:
    motor_param_t motor_param_[2];
    uint64_t last_update_time;
    float wheel_distance_;
};

#endif

2、在目录Kinematics，在目录下新建Kinematics.cpp文件，编写如下代码：

#include "Kinematics.h"
void Kinematics::set_motor_param(uint8_t id, float per_pulse_distance)
{
    motor_param_[id] = per_pulse_distance = per_pulse_distance;
}

void Kinematics::set_wheel_distance(float wheel_distance)
{
    wheel_distance_ = wheel_distance;
}

int16_t Kinematics::get_motor_speed(uint8_t id)
{
    return motor_param_[id].motor_speed;
}

void Kinematics::update_motor_speed(uint64_t current_time, int32_t left_tick,
int32_t right_tick) {
    //计算出自上次更新以来经过的时间dt
    uint32_t dt = current_time - last_update_time;
    last_update_time = current_time;
    //计算电动机1和电动机2的编码器读数变化量dtickl和dtick2
    int32_t dtick1 = left_tick - motor_param_[0].last_encoder_tick;
    int32_t dtick2 = right_tick - motor_param_[1].last_encoder_tick;
    motor_param_[0].last_encoder_tick = left_tick;
    motor_param_[1].last_encoder_tick = right_tick;
    //轮子速度计算
    motor_param_[0] . motor_speed =float(dtick1 * motor_param_[0].per_pulse_distance)/ dt * 1000;
    motor_param_[1] .motor_speed = float(dtick2 * motor_param_[1].per_pulse_distance) / dt* 1000;
}

void Kinematics::kinematics_forward(float left_speed, float right_speed,float &out_linear_speed,float &out_angle_speed){
    //两轮转速之和除以2
    out_linear_speed = (right_speed + left_speed) / 2.0;
    //两轮转速之差除以轮距
    out_angle_speed = (right_speed - left_speed) / wheel_distance_;
}

void Kinematics::kinematics_inverse(float linear_speed, float angle_speed,float &out_left_speed,float &out_right_speed){
    out_left_speed = linear_speed - (angle_speed * wheel_distance_) / 2.0;
    out_right_speed = linear_speed + (angle_speed * wheel_distance_) / 2.0;
}

3、修改main.cpp代码

#include <Arduino.h>
#include <Esp32PcntEncoder.h>
#include <Esp32McpwmMotor.h>
#include <PidController.h>
#include <Kinematics.h>

Esp32McpwmMotor motor;
Esp32PcntEncoder encoder[2];
PidController pid_controller[2];
Kinematics kinematics;

float target_linear_speed = 50.0;
float target_angular_speed = 0.1f;
float out_left_speed;
float out_right_speed;
void setup() {
  Serial.begin(115200);
  encoder[0].init(0, 32, 33);
  encoder[1].init(1, 26, 25);

  motor.attachMotor(0, 23, 24);
  motor.attachMotor(1, 12, 13);
  pid_controller[0].update_pid(0.625, 0.125, 0.0);
  pid_controller[1].update_pid(0.625, 0.125, 0.0);
  pid_controller[0].out_limit(-100,100)
  pid_controller[1].out_limit(-100,100)
  Kinematics.set_wheel_distance(175);
  kinematics.set_motor_param(0,0.1051566);
  Kinematics.set_motor_param(1,0.1051566);
  kinematics.kinematics_inverse(target_linear_speed,target_angular_speed,out_left_speed,out_right_speed);
  pid_controller[0].update_target(out_left_speed);
  pid_controller[1].update_target(out_right_speed);
}

void loop() {
  delay(10);
  kinematics.update_motor_speed(millis(),encoder[0].get_Ticks(),encoder[1].get_ticks());
  motor.updateMotorSpeed(0 ,pid_controller[0].update_pid(kinematics.get_motor_speed(0)),pid_controller[1].update_pid(kinematics.get_motor_speed(1)));
}

4、重新编译和下载到开发板，可以看到机器人将以0.5m为半径转圈

9.3.5节 机器人里程计计算（提炼总结）

• 通过运动学正解，可以获得机器人底盘实时的速度信息，通过对线速度和角速度的积分，就可以获得机器人的实时位置信息

1、修改Kinematics.h代码

typedef struct {
  float x;
  float y;
  float angle;
  float linear_speed;
  float angle_speed;
} odom_t;

    void update_odom(unit16_t dt);
    odom_t &get_odom();
    static void TransAngleInPI(float angle,float &out_angle);

    odom_t odom_;

2、修改Kinematics.cpp代码

odom_t &Kinematics::get_odom() { return odom_;}
//用于将角度转换到 -π到π的范围内
void Kinematics::TransAngleInPI (float angle, float&out_angle){
    //如果 angle大于π,则将 out_angle减去2m
    if (angle > PI) {
    out_angle -= 2 * PI;
    }
    //如果 angle小于一元,则将 out_angle加上2π
    else if (angle < -PI) {
    out_angle += 2 * PI;}
}

void Kinematics::update_odom(uint16_t dt) {
    //ms转为s
    float dt_s = (float)dt / 1000;
    //运动学正解,计算机器人的线速度和角速度
    this->kinematic_forward(motor_param_[0].motor_speed, motor_param_[1].motor_speed, odom_.linear_speed, odom_.angle_speed);
    //转换线速度单位(mm/s转m/s)
    odom_.linear_speed = odom_.linear_speed / 1000;//
    //计算当前角度
    odom_.angle += odom_.angle_speed * dt_s;
    //将角度值odom.yaw转换到-π到π的范围内
    Kinematics::TransAngleInPI (odom_.angle, odom_.angle),
    //计算机器人移动距离和在两轴上的分量并进行累积
    float delta_distance = odom_linear_speed * dts;
    odom_.x += delta_distance * std::cos (odom_.ang1e)
    odom_.y += delta_distance * std::sin(odom_.ang1e)
}

3、修改main.cpp代码，在最后一行添加如下代码：

update_odom(dt);

重新编译工程并下载到开发板，可以 看到里程计信息可以正常获取了

下面需要将控制系统接入到ROS2中