如何使用PID控制 双轮差速运动模型的方形小车双轮的速度，使其逼近墙边，并沿着墙边行走。

Question

如图所示，可以根据传感器数据建模，从而实时地计算出小车靠墙一侧的前拐角距离墙边的距离d和小车车身与墙体的倾角θ。

求如何用PID算法调整小车的双轮速度， 使小车逼近墙边，在d达到期望距离时调整车头使小车与墙边平行并做直线运动。

Answer 1

%%%%两轮差速履带车数学模型建立%%%%%%
    %%参数设置
 L=4;%两个轮子间的距离
 T=0.1;%采样时间
 x=2;%初始化位置x坐标
 y=1;%初始化位置y坐标
 theta=pi/2;%初始化位置theta坐标
 
 x_goal=10;%终点位置坐标
 y_goal=10;%终点位置坐标
while((x-x_goal)^2+(y-y_goal)^2 >0.01&&(x-x_goal<=0)) 
%%%%%%%%%这一段设置跟踪器，跟踪一段直线%%%%%%
%%%计算当前与目标的朝向误差
theta_goal=atan((y_goal-y)/(x_goal-x));
theta_error=theta-theta_goal;
u=-k*(theta_error);
 
 
 %%%%控制输入，左电机和右侧电机。
 vr=4+u;%控制输入需要你去更改//4代表着你想让车走多快，我这里未考虑到。
 vl=4;%控制输入需要你去更改
 
 
 
 %%%%process model
 
 %%%运动模型
 v=(vl+vr)/2;%中心速度
 w=(vr-vl)/L;
 x=x+v*cos(theta)*T;
 y=y+v*sin(theta)*T;
 theta=theta+w*T;
 x_push=[x_push;x];
 y_push=[y_push;y];
 theta_push=[theta_push;theta];
end