Robot Kinematics Analysis Theory

I. Experiment Objective

Understand the kinematics model of the differential-drive robot chassis, and master the forward/inverse kinematics solving methods from the left and right wheel speeds to the overall linear velocity and angular velocity of the robot. By reading the source code, understand how to implement robot motion control functions on the ESP32-S3, and how to control the car's motion state through interfaces such as Motion_Ctrl() and Motion_Ctrl_State().

II. Experiment Principle

1. Keywords

2. Technical Architecture

Building on PID motor control (Section 16.11), the program adds the car_motion kinematics solving layer, which encapsulates the forward/inverse kinematics calculation of the differential chassis and provides high-level motion control interfaces such as Motion_Ctrl, Motion_Get_Speed, and Motion_Ctrl_State.

main/main.c
    └── Motion_Ctrl(Vx, Vy, Wz)  /  Motion_Ctrl_State(state, speed)
         └── components/car_motion/       Kinematics solving layer (inverse → wheel speeds / forward → chassis velocity)
              ├── components/motor/        Reuse PID motor control (Section 11)
              ├── components/encoder/      Reuse the encoder driver
              └── components/pwm_motor/    Reuse the PWM motor driver
       

Main control flow: call Motion_Ctrl(1.0, 0, 0) to set the linear velocity → inverse kinematics calculates the target speeds of the left and right wheels → call Motor_Set_Speed to drive the motors → PID closed-loop adjustment → encoder feedback → forward kinematics calculates the actual chassis velocity.

III. Experiment Steps

1. Hardware Preparation

2. Prerequisites

1. Set up the ESP32-IDF development environment (see Chapter 15).

2. Complete Section 16.11, PID Motor Speed Control, and understand the basic flow of motor closed-loop control

3. Connect the MicroROS-V2 control board to the computer via USB

4. The motor power supply is normal

3. Compile and Flash

​x

cd ~/esp/base_samples/car_motion
source ~/esp/esp-idf/export.sh
idf.py build flash monitor

To exit the monitor: press Ctrl + ].

4. Stopping Method

In the terminal, press Ctrl+] to exit the serial monitor.

IV. Experiment Result

After flashing and running, you can see the following output in the serial monitor terminal:

xxxxxxxxxx
hello yahboom
I (1012) MAIN: Nice to meet you!
I (1022) MOTOR: Start Motor_Task with core:1
I (1022) MAIN: Read Motion:0.28, 0.00, 0.02
I (1122) MAIN: Read Motion:0.52, 0.00, -0.01
I (1222) MAIN: Read Motion:0.73, 0.00, 0.01
I (1322) MAIN: Read Motion:0.89, 0.00, -0.01
I (1422) MAIN: Read Motion:0.96, 0.00, 0.00
I (1522) MAIN: Read Motion:0.99, 0.00, 0.00

• Vx gradually accelerates from 0 to near the set 1.0 m/s

• Vy is always 0 (the differential chassis has no Y-direction velocity)

• Wz is near 0 (straight-line motion, theoretically no rotation)

• About 20 seconds later, the motor coasts to a stop

V. Program Analysis

Source code paths:

• Firmware source: ~/esp/base_samples/car_motion/main/main.c

• Kinematics solving: ~/esp/base_samples/car_motion/components/car_motion/car_motion.h, car_motion.c

• Motor PID: ~/esp/base_samples/car_motion/components/motor/motor.h, motor.c

1. Directory Structure

xxxxxxxxxx
car_motion/
├── CMakeLists.txt
├── main/
│   ├── CMakeLists.txt
│   └── main.c
└── components/
    ├── car_motion/           # Kinematics solving layer
    │   ├── car_motion.c
    │   └── car_motion.h
    ├── motor/                # Reuses the motor control from Section 11
    ├── encoder/              # Reuses the encoder from Section 11
    └── pwm_motor/            # Reuses the PWM motor driver from Section 11

The current main program only demonstrates motion control.

2. Main Program Logic

main/main.c implements a straight-line motion test:

1. Delay 1 second to wait for the system to stabilize

2. Call Motor_Init() to initialize the motor system

3. Call Motion_Ctrl(1.0, 0, 0.0) to set the linear velocity to 1.0 m/s and the angular velocity to 0 rad/s, i.e., moving straight forward

4. Loop to read and print the car's current linear velocity and angular velocity

5. After running 200 cycles (about 20 seconds), call Motion_Stop(STOP_COAST) to stop

xxxxxxxxxx
Motor_Init();
Motion_Ctrl(1.0, 0, 0.0);   // Linear velocity 1.0 m/s, angular velocity 0 rad/s

while (1) {
    Motion_Get_Speed(&micro_car);
    ESP_LOGI(TAG, "Read Motion:%.2f, %.2f, %.2f",
             micro_car.Vx, micro_car.Vy, micro_car.Wz);
    vTaskDelay(pdMS_TO_TICKS(100));
    count++;
    if(count >= 200) {
        Motion_Stop(STOP_COAST);
    }
}

3. Key Components

car_motion layer (kinematics model)

Files: components/car_motion/car_motion.h, components/car_motion/car_motion.c

Physical parameters:

Parameter definition file: ~/esp/base_samples/car_motion/components/car_motion/car_motion.h

Motion state enum:

xxxxxxxxxx
typedef enum {
    MOTION_STOP,       // Stop (coast)
    MOTION_RUN,        // Straight forward
    MOTION_BACK,       // Straight backward
    MOTION_LEFT,       // Turn left while moving forward
    MOTION_RIGHT,      // Turn right while moving forward
    MOTION_SPIN_LEFT,  // Spin left in place
    MOTION_SPIN_RIGHT, // Spin right in place
    MOTION_BRAKE,      // Brake
} motion_state_t;

Data structure:

xxxxxxxxxx
typedef struct {
    float Vx;  // Linear velocity in the X direction (m/s), positive means forward
    float Vy;  // Linear velocity in the Y direction (m/s), always 0 for a differential chassis
    float Wz;  // Angular velocity around the Z axis (rad/s), positive means counterclockwise
} car_motion_t;

Detailed Kinematics Model

The car is a two-wheel differential-drive chassis. The left and right wheels are driven independently, and steering is achieved by the speed difference between the left and right wheels.

Inverse kinematics (target velocity -> wheel speeds):

Given the target linear velocity Vx and angular velocity Wz, calculate the target speeds of the left and right wheels:

xxxxxxxxxx
V_left  = Vx - Wz * (ROBOT_WIDTH / 2)
V_right = Vx + Wz * (ROBOT_WIDTH / 2)

where ROBOT_WIDTH / 2 = 0.0857m is the half wheel track.

Code implementation (the Motion_Ctrl function):

xxxxxxxxxx
speed_L_setup = line_v - angular_v * ROBOT_HALF_WIDTH;
speed_R_setup = line_v + angular_v * ROBOT_HALF_WIDTH;
Motor_Set_Speed(speed_L_setup, speed_R_setup);

• When Vz = 0, the left and right wheel speeds are equal, and the car moves straight forward/backward

• When Vz > 0, the left wheel speed < the right wheel speed, and the car turns left

• When Vx = 0 and Vz ≠ 0, the left and right wheel speeds are equal in magnitude and opposite in direction, and the car rotates in place

The speed values are automatically limited to ±1.0 m/s.

Forward kinematics (wheel speeds -> actual velocity):

Given the actual linear velocities speed_m1 (left wheel) and speed_m2 (right wheel), calculate the actual linear and angular velocities of the chassis:

xxxxxxxxxx
Vx = (V_left + V_right) / 2
Wz = -(V_left - V_right) / ROBOT_WIDTH

Note the negative sign in the angular velocity formula. This is because the positive direction of the angular velocity is defined as counterclockwise, while the encoder direction sign configuration makes an increase in the right wheel speed correspond to clockwise rotation.

Code implementation (the Motion_Get_Speed function):

xxxxxxxxxx
car->Vx = (speed_m1 + speed_m2) / 2.0f;
car->Vy = 0;
car->Wz = -(speed_m1 - speed_m2) / (ROBOT_WIDTH);

Motion state control:

The Motion_Ctrl_State function encapsulates common motion control modes. You only need to pass the state enum and the speed magnitude:

Core functions:

• Motion_Ctrl(float V_x, float V_y, float V_z) — Controls the car's motion. The V_y parameter is invalid in the current differential chassis model and is kept only for interface extension

• Motion_Get_Speed(car_motion_t *car) — Gets the current linear velocity and angular velocity of the car

• Motion_Ctrl_State(uint8_t state, float speed) — Controls motion through predefined states for quick invocation

• Motion_Stop(uint8_t brake) — Stops the car's motion

GPIO pins:

• S1: GPIO8

• S2: GPIO16

PWM parameters:

• Resolution: 1 MHz (1us/tick)

• Period: 20000 ticks (20ms, i.e., 50Hz)

• Pulse width range: 500 ~ 2500us (corresponding to angle -90° ~ +90°)

• S1 default angle: 0°, range: -90° ~ 90°

• S2 default angle: -45° (camera facing straight ahead), range: -90° ~ 20°

VI. Common Problems