如何使用PID控制 双轮差速运动模型的方形小车双轮的速度，使其逼近墙边，并沿着墙边行走。
如图所示，可以根据传感器数据建模，从而实时地计算出小车靠墙一侧的前拐角距离墙边的距离d和小车车身与墙体的倾角θ。
求如何用PID算法调整小车的双轮速度， 使小车逼近墙边，在d达到期望距离时调整车头使小车与墙边平行并做直线运动。
我大概分析了一下控制逻辑，你可以参考一下，相关车辆参数你可以替换为你的实际参数：
参数设定：车轮半径r=3cm，车轮间距wd=10cm，车身长length=16cm，侧边超声波传感器前后各一个，假设一个在车头一个在车尾，间距=车身=length=16cm，小车线速度恒定v = 0.2m/s，距离墙d=10cm，假设超声波传感器以10Hz的频率采集距离。
在以上条件下，设定小车出发点，方向与墙平行，距离30cm，根据以上参数，设计小车控制算法。
1、根据超声波测距结果f_dist和b_dist计算车辆与墙的夹角θ与距离dist
由上图关系可知：
dist = f_distcos(θ)
tan(θ) = (b_dist-f_dist)/length
--> θ=arctan(b_dist-f_dist)/length)
--> dist = f_distcos(arctan(b_dist-f_dist)/length))
到这里，我们通过超声波测距对小车的位姿进行了计算，得到了dist和θ
2、计算线速度v、角速度omega，与车轮转速的关系
车轮通过电机控制，电机通过PWM控制，上位机可以控制的是电机PWM控制的占空比，再进一步，就是上位机可以直接控制车轮的转速，假设左右车轮转速分别为omega_l和omega_r，那么对应两个轮子的线速度分别是v_l和v_r，二者关系为：
v_l = 2pir*(omega_l/2/pi) = omega_lr
同理：v_r = omega_rr
小车通过两轮差速驱动，车轮速度一致时走直线，不一致时走弧线，走弧线时，两个轮子所走弧线以及车实际的轨迹是3个同心圆，而单位时间内走过的轨迹如下图：
上图中，v_l和v_r是单位时间内两个轮走过的轨迹，v是车辆实际速度，R是内车轮轨迹半径，wd为两个轮子的距离，omega为单位时间内车辆转过额角度，实际就是车辆角速度，这些参数有以下关系：
v_r/R = v_l/(R+wd)=v/(R+wd/2)=omega
--> v_r = Romega
v_l = Romega+wdomega
v = omegaR + wd*omega/2
--> omega = (v_l-v_r)/wd = (v-v_r)*2/wd
v在上文我们设定为固定值0.2m/s，那么：
--> v_r = v - omega *wd/2
v_l = v + omega *wd/2
到此为止，我们得到了设定车辆角速度v，线速度omega时，科技计算出车轮线速度，根据线速度和车轮角速度关系，可以计算出上位机应该给车轮下达怎样的指令。
3、厘清各参数关系之后，我们来实现通过pid实现闭环控制小车，也就是通过第一步计算出的θ和dist，目标距离d，计算小车控制参数v（固定值0.2m/s）和omega：
首先看一下控制逻辑，下图控制逻辑是靠传感器数据驱动的，也可以设置为定时驱动，传感器数据采集以后放到固定位置，程序设置定时循环驱动计算和控制：
在上图中，dist和θ是即时计算出来的，
d_err = dist-d
phi_err = θ
另设：
Δt：车辆控制命令发送间隔
d_I_err：横向偏差积分项，每一轮+=d_err Δt 需要设定范围，避免积分项太大
phi_I_err：航向角度偏差积分项，每一轮+=phi_err Δt 需要设定范围，避免积分项太大
d_K：横向偏差比例系数
d_I：横向偏差积分系数
phi_K：航向角度偏差比例系数
phi_I：航向角度偏差积分系数
那么，在每一次控制循环中需要进行以下计算：
d_err = dist-d
phi_err = θ
d_I_err += d_err Δt
判断积分项是否在范围内，超出范围则设为范围边界值
phi_I_err += phi_err Δt
判断积分项是否在范围内，超出范围则设为范围边界值
omega = d_Kd_err+d_Id_I_err+phi_Kphi_err +phi_Iphi_I_err
其中4个比例系数都需要通过设其它比例系数为0的情况下，计算omega和系数相关参数的关系来判定系数的符号，然后就根据车辆运行状态来调节各项参数。
%%%%两轮差速履带车数学模型建立%%%%%%
%%参数设置
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
PID算法控制小车：https://blog.csdn.net/weixin_49303682/article/details/118686567
假设只有一个传感器，模拟量类型，能反应与墙的距离。那么，以传感器返回的距离为D。用d-D=err，err就是当前的错误值。然后设置一个变量ING，用于存储每一个err的值。然后得到一个输出值out=Kp * err+Ki * ING。这个out为0的时候，两个轮子的速度一致，也就是小车按照直线走；当out为负值，左侧轮子比右侧走的快，也就是向右侧的墙靠近，out负值越大，差速越大；同理，当out为正值，小车往左走远离墙面。
写成伪代码的化就是
while(1){
read D;
err=d-D;
INg=INg+err;
out=Kp * err+Ki * INg;
/*
根据out控制差速的程序段
*/
}
这个模型相对来说并不复杂，θ的条件不用太严格，只需要满足在0-90°范围内即可，那么只需要对d的检测能够满足要求即可，用离散PID模型可以建立，假设小车的速度是V，采样周期是t，那么在采样周期内小车靠近墙的距离为Vtsinθ，误差err=d-Vtsinθ，通过数组建立不同时刻的误差值err[k]，最近一次记为err[0]，供积分和微分运算用，如果积分取最近的10个周期，那么sum=err[0]+err[1]+...+err[9]，Pid表达式就可以列出：pid=PID=KPerr[0]+KIsum/10+KD*（err[0]-err[1]）,我们调整的值为速度，那么V=V+pid即可，可以参考博客：https://blog.csdn.net/qq_19979629/article/details/123451333
假设你使用的是超声波测距，那么超声波检测是有角度限制的，你需要测量出超声波检测的具体角度，同时还需要测量小车平行于墙面时传感器检测到的距离，如果侧边也有测距传感器就好办了，同事测量侧边传感器离墙距离和前方传感器测得离墙距离，当两个传感器测得距离墙体的值为设定值时即平行于墙体，两个传感器的值可以根据需要调整，使其尽可能的靠近墙体。如果只有前方有传感器，那就需要知道小车的航向角了，可以借助于陀螺仪。
PID算法控制小车转向
https://blog.csdn.net/weixin_49303682/article/details/118686567
看看这个是否对你有帮助https://www.bilibili.com/video/av295559315
牛啊，早上好，起了一趟早的我刷到了这个。看了题主前面的提问，DWA这个算法我在数模里接触过，它需要找几个点，就类似于目的地，然后再规划路径，但您好像是要沿着边缘，或者说墙，然后行驶，那DWA可能就不适用了，感觉应该可以用寻迹的方式（我电赛的经验分析而来）也就是PID来调，所以你现在需要的是什么？代码？！还是寻迹的部分
望采纳谢谢
如何使用PID控制 双轮差速运动模型的方形小车双轮的速度，使其逼近墙边，并沿着墙边行走
可以参考一下
Class definition
class PSO():
"""
PSO class
"""
def __init__(self,iters=100,pcount=50,pdim=2,mode='min'):
"""
PSO initialization
------------------
"""
self.w = None # Inertia factor
self.c1 = None # Learning factor
self.c2 = None # Learning factor
self.iters = iters # Number of iterations
self.pcount = pcount # Number of particles
self.pdim = pdim # Particle dimension
self.gbpos = np.array([0.0]*pdim) # Group optimal position
self.mode = mode # The mode of PSO
self.cur_pos = np.zeros((pcount, pdim)) # Current position of the particle
self.cur_spd = np.zeros((pcount, pdim)) # Current speed of the particle
self.bpos = np.zeros((pcount, pdim)) # The optimal position of the particle
self.trace = [] # Record the function value of the optimal solution
def init_particles(self):
"""
init_particles function
-----------------------
"""
# Generating particle swarm
for i in range(self.pcount):
for j in range(self.pdim):
self.cur_pos[i,j] = rd.uniform(MIN_POS[j], MAX_POS[j])
self.cur_spd[i,j] = rd.uniform(MIN_SPD[j], MAX_SPD[j])
self.bpos[i,j] = self.cur_pos[i,j]
# Initial group optimal position
for i in range(self.pcount):
if self.mode == 'min':
if self.fitness(self.cur_pos[i]) < self.fitness(self.gbpos):
gbpos = self.cur_pos[i]
elif self.mode == 'max':
if self.fitness(self.cur_pos[i]) > self.fitness(self.gbpos):
gbpos = self.cur_pos[i]
def fitness(self, x):
"""
fitness function
----------------
Parameter:
x :
"""
# Objective function
fitval = 5*np.cos(x[0]*x[1])+x[0]*x[1]+x[1]**3 # min
# Retyrn value
return fitval
def adaptive(self, t, p, c1, c2, w):
"""
"""
#w = 0.95 #0.9-1.2
if t == 0:
c1 = 0
c2 = 0
w = 0.95
else:
if self.mode == 'min':
# c1
if self.fitness(self.cur_pos[p]) > self.fitness(self.bpos[p]):
c1 = C1_MIN + (t/self.iters)*C1_MAX + np.random.uniform(0,0.1)
elif self.fitness(self.cur_pos[p]) <= self.fitness(self.bpos[p]):
c1 = c1
# c2
if self.fitness(self.bpos[p]) > self.fitness(self.gbpos):
c2 = C2_MIN + (t/self.iters)*C2_MAX + np.random.uniform(0,0.1)
elif self.fitness(self.bpos[p]) <= self.fitness(self.gbpos):
c2 = c2
# w
#c1 = C1_MAX - (C1_MAX-C1_MIN)*(t/self.iters)
#c2 = C2_MIN + (C2_MAX-C2_MIN)*(t/self.iters)
w = W_MAX - (W_MAX-W_MIN)*(t/self.iters)
elif self.mode == 'max':
pass
return c1, c2, w
def update(self, t):
"""
update function
---------------
Note that :
1. Update particle position
2. Update particle speed
3. Update particle optimal position
4. Update group optimal position
"""
# Part1 : Traverse the particle swarm
for i in range(self.pcount):
# Dynamic parameters
self.c1, self.c2, self.w = self.adaptive(t,i,self.c1,self.c2,self.w)
# Calculate the speed after particle iteration
# Update particle speed
self.cur_spd[i] = self.w*self.cur_spd[i] \
+self.c1*rd.uniform(0,1)*(self.bpos[i]-self.cur_pos[i])\
+self.c2*rd.uniform(0,1)*(self.gbpos - self.cur_pos[i])
for n in range(self.pdim):
if self.cur_spd[i,n] > MAX_SPD[n]:
self.cur_spd[i,n] = MAX_SPD[n]
elif self.cur_spd[i,n] < MIN_SPD[n]:
self.cur_spd[i,n] = MIN_SPD[n]
# Calculate the position after particle iteration
# Update particle position
self.cur_pos[i] = self.cur_pos[i] + self.cur_spd[i]
for n in range(self.pdim):
if self.cur_pos[i,n] > MAX_POS[n]:
self.cur_pos[i,n] = MAX_POS[n]
elif self.cur_pos[i,n] < MIN_POS[n]:
self.cur_pos[i,n] = MIN_POS[n]
# Part2 : Update particle optimal position
for k in range(self.pcount):
if self.mode == 'min':
if self.fitness(self.cur_pos[k]) < self.fitness(self.bpos[k]):
self.bpos[k] = self.cur_pos[k]
elif self.mode == 'max':
if self.fitness(self.cur_pos[k]) > self.fitness(self.bpos[k]):
self.bpos[k] = self.cur_pos[k]
# Part3 : Update group optimal position
for k in range(self.pcount):
if self.mode == 'min':
if self.fitness(self.bpos[k]) < self.fitness(self.gbpos):
self.gbpos = self.bpos[k]
elif self.mode == 'max':
if self.fitness(self.bpos[k]) > self.fitness(self.gbpos):
self.gbpos = self.bpos[k]
def run(self):
"""
run function
-------------
"""
# Initialize the particle swarm
self.init_particles()
# Iteration
for t in range(self.iters):
# Update all particle information
self.update(t)
#
self.trace.append(self.fitness(self.gbpos))
人类耳朵能听到的声波频率为20HZ～20KHz。当声波的振动频率大于20KHz或小于20Hz时，我们便听不见了。因此，我们把频率高于20000赫兹的声波称为“超声波”。因其方向性好，穿透能力强，易于获得较集中的声能，在水中传播距离远，可用于测距、测速、清洗、焊接、碎石、杀菌消毒等。在医学、军事、工业、农业上有很多的应用。如超声波清洗机，超声波加湿器，医学检查B超，彩超，超声波探伤仪等。
声音是由振动产生的，能够产生超声波的装置就是超声波传感器，习惯上称为超声换能器，或者超声探头。超声波探头主要由压电晶片组成，既可以发射超声波，也可以接收超声波。构成晶片的材料可以有许多种。晶片的大小，如直径和厚度也各不相同，因此每个探头的性能是不同的，使用前必须预先了解它的性能。
常用的是压电式超声波发生器，是利用压电晶体的谐振来工作的。超声波传感器探头内部有两个压电晶片和一个共振板。当它的两极外加脉冲信号，其频率等于压电晶片的固有振荡频率时，压电晶片将会发生共振，并带动共振板振动，便产生超声波。反之，如果两电极间未外加电压，当共振板接收到超声波时，将压迫压电晶片作振动，将机械能转换为电信号，这时它就成为超声波接收器了。 超声波传感器就是利用压电效应的原理将电能和超声波相互转化，即在发射超声波的时候，将电能转换成超声波发射出去；而在接收时，则将超声振动转换成电信号。