else:
# 在此方向上不利于减少误差均值，把刚加的值去掉，还要往反方向减一倍
k_pid[0] -= 2 * dp
robot.reset()
x_trajectory, y_trajectory, err = run(robot, k_pid)
if err < best_err:
# 在此方向有利于减少误差均值
best_err = err
dp *= 1.1
else:
k_pid[0] += dp # 把k值恢复原始值
dp *= 0.9 # 缩小增益系数变换量
it += 1
print("Iteration: {}, best_err: {} dp: {}".format(it, best_err, dp))
return k_pid, best_err
if __name__ == '__main__':
params, err = twiddle()
print("Final twiddle error: {} params: {}".format(err, params))
robot = make_robot()
x_trajectory, y_trajectory, best_err = run(robot, params)
show(x_trajectory, y_trajectory, label="Twiddle PID")
x_trajectory = []
y_trajectory = []
p_arr = []
for i in range(n):
# ---------------------------- start
cte = 0.0 - robot.y # 误差值
p = k_p * cte # p
steer = p
p_arr.append(p)
# ---------------------------- end
# 以steer为偏转角，speed 为速度，执行一次运动
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
print(robot)
return x_trajectory, y_trajectory, p_arr
if __name__ == '__main__':
# 创建
robot = Robot()
# 初始位置 x=0, y=-1, orient=0
robot.set(0, -1, 0)
# 运行并收集所有的x，y
x_trajectory, y_trajectory, p_arr = run(robot, k_p=0.2)
# 可视化运行结果
show(x_trajectory, y_trajectory, p_array=p_arr, label='P')
# 创建
robot = Robot()
# 初始位置 x=0, y=-1, orient=0
robot.set(0, -1, 0)
# 设置10度的轮子系统性偏差
robot.set_steering_drift(10. / 180. * np.pi)
# 运行并收集所有的x，y
x_trajectory, y_trajectory, p_arr, d_arr, i_arr = run(robot,
k_p=0.2,
k_d=3.0,
k_i=0.005)
# 可视化运行结果
show(x_trajectory,
y_trajectory,
p_array=p_arr,
d_array=d_arr,
i_array=i_arr,
label='PID')
k_p, k_d, k_i = [
10.716018504541431, 18.68325573584582, 0.020275559590445292
]
robot.reset()
# 运行并收集所有的x，y
x_trajectory, y_trajectory, p_arr, d_arr, i_arr = run(robot,
k_p=k_p,
k_d=k_d,
k_i=k_i)
# 可视化运行结果
show(x_trajectory,
y_trajectory,
:param n: 循环次数
:param speed: 小车速度
"""
x_trajectory = []
y_trajectory = []
for i in range(n):
# ---------------------------- start
steer = 0.0
# ---------------------------- end
# 以steer为偏转角，speed为速度，执行一次运动
robot.move(steer, speed)
x_trajectory.append(robot.x)
y_trajectory.append(robot.y)
print(robot)
return x_trajectory, y_trajectory
if __name__ == '__main__':
# 创建
robot = Robot()
# 初始位置 x=0, y=-1, orient=0
robot.set(0, -1, 0)
# 运行并收集所有的x，y
x_trajectory, y_trajectory = run(robot)
# 可视化运行结果
show(x_trajectory, y_trajectory, label='Car')