两轮差速小车运动模型与PID控制实现详解-嵌云网-嵌入式AI开发资源站

两轮差速小车运动模型与PID控制实现详解

郑自春

1. 两轮差速小车运动模型解析

两轮差速小车是移动机器人领域最基础且广泛应用的底盘结构之一。其核心特点是通过左右轮速差实现转向，结构简单但运动控制复杂。下面我将从实际工程角度详细拆解其运动学原理。

1.1 机械结构与运动原理

典型的两轮差速小车由以下部件构成：

两个独立驱动的主动轮（通常为直流电机+编码器）
一个或多个万向从动轮（仅起支撑作用）
刚性连接的车体框架

关键参数定义：

轮半径 r（左右轮相同）
轮间距 L（两驱动轮中心距）
左轮转速 ωₗ（rad/s）
右轮转速 ωᵣ（rad/s）

运动学方程推导过程：

单轮线速度计算：
- 左轮线速度 vₗ = r × ωₗ
- 右轮线速度 vᵣ = r × ωᵣ
整车运动合成：
- 质心线速度 v = (vᵣ + vₗ)/2 = r(ωₗ + ωᵣ)/2
- 角速度 ω = (vᵣ - vₗ)/L = r(ωᵣ - ωₗ)/L

注意：实际应用中需考虑轮径制造误差，建议通过实验标定有效半径

1.2 状态空间表示与微分方程

采用全局坐标系和车身坐标系的联合描述：

全局位姿状态量：X = [x, y, θ]ᵀ
- (x,y)：质心在全局坐标系下的坐标
- θ：车身纵轴与全局X轴夹角（航向角）

运动微分方程：

code复制ẋ = v·cosθ
ẏ = v·sinθ
θ̇ = ω

数值积分实现建议：

matlab复制% 欧拉法离散化
dt = 0.01; % 控制周期10ms
x(k+1) = x(k) + v(k)*cos(theta(k))*dt;
y(k+1) = y(k) + v(k)*sin(theta(k))*dt;
theta(k+1) = theta(k) + omega(k)*dt;

1.3 运动约束与特性分析

两轮差速模型存在以下运动约束：

非完整约束：无法瞬时侧向移动
- 满足方程：ẋ·sinθ - ẏ·cosθ = 0
最大运动限制：
- v_max = min(r·ωₗ_max, r·ωᵣ_max)
- ω_max = r·(ωᵣ_max - ωₗ_min)/L

典型参数示例：

参数	典型值	单位
轮半径 r	0.05	m
轮距 L	0.20	m
电机转速	0~300	rpm
最大线速度	0.78	m/s
最大角速度	3.93	rad/s

2. PID控制算法实现

2.1 控制架构设计

采用双闭环PID控制结构：

外环：轨迹跟踪控制器
- 输入：期望位姿与实际位姿偏差
- 输出：目标线速度和角速度
内环：电机转速控制器
- 输入：目标轮速与实际编码器反馈
- 输出：电机PWM占空比

控制框图：

code复制[轨迹生成] → [位姿控制器] → [速度分配] → [电机控制器] → [小车]
               ↑                        ↑
           [位姿反馈]              [编码器反馈]

2.2 位姿控制算法

定义误差项：

位置误差：e_p = √[(x_ref - x)² + (y_ref - y)²]
航向误差：e_θ = θ_ref - θ

PID控制律：

code复制v_cmd = Kp_p·e_p + Ki_p·∫e_p dt + Kd_p·de_p/dt
ω_cmd = Kp_θ·e_θ + Ki_θ·∫e_θ dt + Kd_θ·de_θ/dt

参数整定经验：

先调P项直到出现小幅振荡
加入D项抑制振荡
最后加I项消除静差
典型初始值：
- Kp_p = 0.5, Ki_p = 0.01, Kd_p = 0.1
- Kp_θ = 1.2, Ki_θ = 0.02, Kd_θ = 0.15

2.3 速度分配与电机控制

将指令速度转换为轮速：

code复制ωₗ_cmd = (v_cmd - L·ω_cmd/2)/r
ωᵣ_cmd = (v_cmd + L·ω_cmd/2)/r

电机PID实现要点：

matlab复制% 增量式PID实现
error = target_speed - actual_speed;
d_error = error - last_error;
i_error = i_error + error;

output = Kp*error + Ki*i_error + Kd*d_error;
last_error = error;

3. 轨迹规划与跟踪

3.1 参考轨迹生成方法

常用轨迹生成算法对比：

方法	优点	缺点	适用场景
直线插值	计算简单	转折点不连续	简单路径
多项式曲线	平滑连续	计算量较大	精确控制
贝塞尔曲线	可调曲率	参数难优化	复杂路径
样条插值	高阶连续	实时性差	离线规划

推荐实现（三次多项式）：

matlab复制% 生成从(0,0)到(1,1)的轨迹
t = 0:0.1:1;
x = 3*t.^2 - 2*t.^3; 
y = 3*t.^2 - 2*t.^3;

3.2 跟踪误差分析

定义跟踪误差指标：

横向误差：e_lat = -sinθ·(x_ref-x) + cosθ·(y_ref-y)
纵向误差：e_long = cosθ·(x_ref-x) + sinθ·(y_ref-y)
航向误差：e_θ = θ_ref - θ

误差收敛判定条件：

code复制max(|e_lat|) < 0.02m 
max(|e_long|) < 0.05m
max(|e_θ|) < 0.1rad

3.3 前馈补偿改进

在标准PID基础上增加前馈项：

code复制v_ff = v_ref
ω_ff = ω_ref + K_curv·v_ref·κ

其中κ为轨迹曲率，计算方式：

code复制κ = (ẋ_ref·ÿ_ref - ẏ_ref·ẍ_ref) / (ẋ_ref² + ẏ_ref²)^(3/2)

4. MATLAB仿真实现

4.1 仿真环境搭建

推荐使用Simulink建模架构：

轨迹生成模块 - MATLAB Function
控制器模块 - PID Controller
小车模型 - S-Function
可视化模块 - VR Sink

关键仿真参数设置：

matlab复制sim_time = 10;       % 仿真时长
sample_time = 0.01;  % 采样周期
init_pose = [0 0 0]; % 初始位姿

4.2 核心代码解析

运动学模型实现：

matlab复制function [pose] = updateModel(pose, v, omega, dt)
    % 欧拉积分更新位姿
    pose(3) = pose(3) + omega*dt; % 更新角度
    pose(1) = pose(1) + v*cos(pose(3))*dt;
    pose(2) = pose(2) + v*sin(pose(3))*dt;
end

PID控制器实现：

matlab复制classdef PIDController
    properties
        Kp, Ki, Kd
        last_error, integral
    end
    
    methods
        function obj = PIDController(Kp, Ki, Kd)
            obj.Kp = Kp;
            obj.Ki = Ki;
            obj.Kd = Kd;
        end
        
        function [output] = compute(obj, error, dt)
            obj.integral = obj.integral + error*dt;
            derivative = (error - obj.last_error)/dt;
            output = obj.Kp*error + obj.Ki*obj.integral + obj.Kd*derivative;
            obj.last_error = error;
        end
    end
end

4.3 结果可视化技巧

轨迹对比图：

matlab复制plot(ref_x, ref_y, 'r--', actual_x, actual_y, 'b-');
legend('参考轨迹','实际轨迹');
axis equal;

误差分析图：

matlab复制subplot(3,1,1);
plot(time, lat_error);
title('横向误差');
subplot(3,1,2);
plot(time, long_error); 
title('纵向误差');
subplot(3,1,3);
plot(time, theta_error);
title('航向误差');

5. 工程实践问题与解决方案

5.1 电机特性补偿

常见问题：

死区现象：PWM小于阈值时电机不转
非线性响应：转速-PWM不是严格线性

解决方案：

死区补偿：

matlab复制if PWM > 0
    PWM = PWM + deadzone;
elseif PWM < 0
    PWM = PWM - deadzone;
end

非线性校正：

matlab复制% 实验测得PWM-转速曲线
pwm_table = [0 10 20 30 40 50 60 70 80 90 100];
speed_table = [0 50 110 180 260 350 450 560 680 810 950];
cmd_pwm = interp1(speed_table, pwm_table, target_speed);

5.2 传感器噪声处理

编码器数据处理流程：

原始计数 → 2. 转速计算 → 3. 低通滤波 → 4. 异常值剔除

MATLAB实现示例：

matlab复制% 移动平均滤波
window_size = 5;
filtered_speed = movmean(raw_speed, window_size);

% 中值滤波抗脉冲噪声
filtered_speed = medfilt1(raw_speed, 3);

5.3 实时性优化

提高控制频率的关键措施：

代码优化：
- 使用固定步长求解器
- 预分配数组内存
- 避免��环中的动态内存分配
硬件选择：
- 选用带硬件编码器接口的MCU
- 使用FPGA实现高速脉冲计数
任务调度：

c复制// 典型RTOS任务划分
void ControlTask(void *arg) {
    while(1) {
        read_sensors();
        update_pose();
        compute_control();
        output_pwm();
        vTaskDelay(10); // 10ms周期
    }
}

在实际项目中，我发现电机参数的标定精度对控制效果影响极大。曾经有个项目因为轮径测量误差5%，导致跟踪误差达到15cm。后来我们开发了自动标定程序：让小车直线行驶固定距离，通过编码器计数反算实际轮径，将控制精度提升到了2cm以内。