在机器人控制领域，两轮差速驱动模型是最基础且应用最广泛的运动学模型之一。从经典的PID控制到更具最优性的LQR控制，再到结合CoppeliaSim的虚拟仿真联调，每一步都需要深入理解系统本质。下面我们按步骤拆解整个实现流程，重点涵盖模型推导、控制算法代码以及仿真验证环节。
一、运动学建模（基于驱动轴中心模型）
两轮差速驱动车的运动学模型可表示为：
x_dot = v*cosθ
y_dot = v*sinθ
θ_dot = ω
其中v代表线速度，ω代表角速度，θ为航向角。该模型假设驱动轮与地面之间无滑动，且车辆运动发生在二维平面内。
二、PID控制实现
% 初始化参数
dt = 0.1; % 时间步长
T = 20; % 总仿真时间
L = 0.5;% 轮距
v = 1.0;% 线速度
% 初始状态 [x, y, θ]
state = [0; 0; pi/8];
target = [10; 10];
% PID参数
Kp = 1.0;
Ki = 0.01;
Kd = 0.1;
% 误差计算
dx = target(1) - state(1);
dy = target(2) - state(2);
target_angle = atan2(dy, dx);
error = angle_diff(target_angle, state(3));
% PID控制循环
for t = 0:dt:T
% 计算控制量
integral_error = integral_error + error*dt;
derivative_error = (error - prev_error)/dt;
omega = Kp*error + Ki*integral_error + Kd*derivative_error;
% 更新状态
state(3) = state(3) + omega*dt;
state(1) = state(1) + v*cos(state(3))*dt;
state(2) = state(2) + v*sin(state(3))*dt;
% 更新误差
prev_error = error;
end
% 轨迹可视化
plot(state(1), state(2),'ro','MarkerSize',10);
hold on;
plot([0 state(1)],[0 state(2)],'b-');
axis equal;
grid on;
PID参数的初始推荐值为Kp=1.0、Ki=0.01、Kd=0.1，实际调试时可根据系统响应进一步微调。特别要注意，角度差的计算必须使用angle_diff函数处理角度环绕问题，否则容易产生跳变导致控制震荡。
三、LQR控制实现
% 系统参数
A = [0 1 0; 0 0 1; 0 0 0];
B = [0; cos(state(3))*dt; sin(state(3))*dt/L];
Q = diag([1000, 1000, 10]);% 状态权重
R = 0.1;% 控制权重
% LQR增益计算
[K,S,e] = lqr(A,B,Q,R);
% 控制循环
for t = 0:dt:T
error = [target(1)-state(1); target(2)-state(2); angle_diff(target_angle,state(3))];
u = -K*error;
% 更新状态
state = A*state + B*u;
end
LQR相比PID具有更好的最优性，但需要事先对系统模型进行线性化，并合理调整权重矩阵Q和R。建议将Q中位置误差的权重设得较大（如1000），航向角误差权重相对较小（如10），这样控制器会优先保障位置收敛。
四、CoppeliaSim联合仿真
场景搭建：使用PioneerP3DX模型，修改控制脚本实现轨迹跟踪。MATLAB通信代码如下：
% 建立通信连接
client = RemoteAPIClient();
sim = client.require('sim');
% 获取模型句柄
car_handle = sim.getObject('/PioneerP3DX');
left_motor = sim.getObject('/leftMotor');
right_motor = sim.getObject('/rightMotor');
% 控制循环
while sim.getSimulationTime() < max_time
% 获取当前状态
position = cop2mat(sim.getObjectPosition(car_handle, -1));
% 计算控制指令
[vl, vr] = calculate_wheel_speeds(position, target);
% 发送控制指令
sim.setJointTargetVelocity(left_motor, vl/wheel_radius);
sim.setJointTargetVelocity(right_motor, vr/wheel_radius);
end
联合仿真的关键在于保持MATLAB与CoppeliaSim的时钟同步，并将仿真步长设置为一致（本例中dt=0.1s）。实际应用中还应注意远程API的调用频率不宜过高，否则容易导致通信阻塞。
五、关键要点
- 模型选择：根据应用场景选择驱动轴中心模型或偏移模型。中心模型适用于对称结构，偏移模型适用于前驱或后驱车体。
- 参数整定：PID参数建议初始值为Kp=1.0, Ki=0.01, Kd=0.1；LQR权重矩阵需根据系统动态调整，一般先固定R，逐步增大Q直至响应满足需求。
- 可视化分析：绘制轨迹对比图，显示航向角变化曲线和速度变化曲线，这些是判断控制器性能最直观的方式。
六、扩展应用
- 避障功能：添加超声波传感器模型，结合势场法或动态窗口法进行局部避障。
- 路径规划：结合A*或RRT算法实现全局路径规划，上述控制算法可作为底层轨迹跟踪器。
- 多车协同：利用CoppeliaSim的多机仿真接口，实现多辆两轮差速车的编队控制。
