一、运动学建模（基于驱动轴中心模型）
两轮车的运动学模型可表示为：
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;
三、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:Terror = [target(1)-state(1); target(2)-state(2); angle_diff(target_angle,state(3))];u = -K*error;% 更新状态state = A*state + B*u;
end
四、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/Simlink仿真程序 www.youwenfan.com/contentcnn/95759.html
五、关键要点
- 模型选择：根据应用场景选择驱动轴中心模型或偏移模型
- 参数整定： PID参数建议初始值：Kp=1.0, Ki=0.01, Kd=0.1 LQR权重矩阵需根据系统动态调整
- 可视化分析： 绘制轨迹对比图 显示航向角变化曲线 速度变化曲线
六、扩展应用
- 避障功能：添加超声波传感器模型
- 路径规划：结合A*或RRT算法
- 多车协同：实现编队控制