PID制御

2013-05-27

AI for Robotics - Lesson5: PID control (pdf) 動画

Smoothing algorithm

  • 直角に動くのはよろしくない。
  • 経路探索でみつけた頂点に対して
  • 「パスの元の位置との差」「隣の点との差」をウェイトを付けて小さくする

PID制御

  • P: Proportional(比例)。目的の値との差(CrossTrack Error: CTE)に比例した変化量を加える
  • D: Derivative(微分)。CTEの微分(減る速度)を減らすようにする。Pだけだとオーバーシュートする(行き過ぎてしまう)のを抑える。微分の代わりに前回との差分を使う。
  • I: Integral(積分)。CTEの積分(合計値)。バイアスがあった場合(例えばハンドルのアライメントがずれてるとか)目的の値に達しない。その場合、I制御を使うと解決する。

パラメータの調整

  • PID制御の各パラメータをどうやって調整するか
  • 適当なパラメータから始めて、少しずつ弄ってエラーが小さいパラメータを探す
    • 何ステップか動かして、CTEの二乗誤差の大きさを測る
    • パラメータの1つを調整して、エラーが小さければ採用して、ステップ幅を増やす(x1.1)。エラーが小さくなければステップ幅を狭めていく(x0.9)。
def twiddle(tol = 0.2):
n_params = 3

params = [0.0 for row in range(n_params)]
dparams = [1.0 for row in range(n_params)]
best_err = run(params)

while sum(dparams) > tol:
for i in range(len(params)):
op = params[i]
params[i] += dparams[i]
err = run(params)
if err < best_err:
best_err = err
dparams[i] *= 1.1
else:
params[i] -= 2.0 * dparams[i]
err = run(params)
if err < best_err:
best_err = err
dparams[i] *= 1.1
else:
params[i] = op
dparams[i] *= 0.9

return params
問題
陸上のトラックのようなコースに沿って車を走らせる問題。cte()メソッドで目的のトラックとの距離を返すだけで、目的のトラック沿って走ってくれるなんて、不思議。
# --------------
# User Instructions
#
# Define a function cte in the robot class that will
# compute the crosstrack error for a robot on a
# racetrack with a shape as described in the video.
#
# You will need to base your error calculation on
# the robot's location on the track. Remember that
# the robot will be traveling to the right on the
# upper straight segment and to the left on the lower
# straight segment.
#
# --------------
# Grading Notes
#
# We will be testing your cte function directly by
# calling it with different robot locations and making
# sure that it returns the correct crosstrack error.

from math import *
import random


# ------------------------------------------------
#
# this is the robot class
#

class robot:
# --------
# init:
# creates robot and initializes location/orientation to 0, 0, 0
#
def __init__(self, length = 20.0):
self.x = 0.0
self.y = 0.0
self.orientation = 0.0
self.length = length
self.steering_noise = 0.0
self.distance_noise = 0.0
self.steering_drift = 0.0

# --------
# set:
#sets a robot coordinate
#
def set(self, new_x, new_y, new_orientation):
self.x = float(new_x)
self.y = float(new_y)
self.orientation = float(new_orientation) % (2.0 * pi)

# --------
# set_noise:
#sets the noise parameters
#
def set_noise(self, new_s_noise, new_d_noise):
# makes it possible to change the noise parameters
# this is often useful in particle filters
self.steering_noise = float(new_s_noise)
self.distance_noise = float(new_d_noise)

# --------
# set_steering_drift:
#sets the systematical steering drift parameter
#
def set_steering_drift(self, drift):
self.steering_drift = drift

# --------
# move:
# steering = front wheel steering angle, limited by max_steering_angle
# distance = total distance driven, most be non-negative
def move(self, steering, distance,
tolerance = 0.001, max_steering_angle = pi / 4.0):

if steering > max_steering_angle:
steering = max_steering_angle
if steering < -max_steering_angle:
steering = -max_steering_angle
if distance < 0.0:
distance = 0.0


# make a new copy
res = robot()
res.length = self.length
res.steering_noise = self.steering_noise
res.distance_noise = self.distance_noise
res.steering_drift = self.steering_drift

# apply noise
steering2 = random.gauss(steering, self.steering_noise)
distance2 = random.gauss(distance, self.distance_noise)

# apply steering drift
steering2 += self.steering_drift

# Execute motion
turn = tan(steering2) * distance2 / res.length

if abs(turn) < tolerance:
# approximate by straight line motion
res.x = self.x + (distance2 * cos(self.orientation))
res.y = self.y + (distance2 * sin(self.orientation))
res.orientation = (self.orientation + turn) % (2.0 * pi)
else:
# approximate bicycle model for motion
radius = distance2 / turn
cx = self.x - (sin(self.orientation) * radius)
cy = self.y + (cos(self.orientation) * radius)
res.orientation = (self.orientation + turn) % (2.0 * pi)
res.x = cx + (sin(res.orientation) * radius)
res.y = cy - (cos(res.orientation) * radius)

return res

def __repr__(self):
return '[x=%.5f y=%.5f orient=%.5f]' % (self.x, self.y, self.orientation)

def cte(self, radius):
cte = 0.0
if self.x < radius:
dx = self.x - radius
dy = self.y - radius
cte = sqrt(dx * dx + dy * dy) - radius
elif self.x > 3 * radius:
dx = self.x - 3 * radius
dy = self.y - radius
cte = sqrt(dx * dx + dy * dy) - radius
elif self.y > radius:
cte = self.y - 2 * radius
else:
cte = -self.y

return cte


# ------------------------------------------------------------------------
#
# run - does a single control run.
def run(params, radius, printflag = False):
myrobot = robot()
myrobot.set(0.0, radius, pi / 2.0)
speed = 1.0 # motion distance is equal to speed (we assume time = 1)
err = 0.0
int_crosstrack_error = 0.0
N = 200

crosstrack_error = myrobot.cte(radius) # You need to define the cte function!

for i in range(N*2):
diff_crosstrack_error = - crosstrack_error
crosstrack_error = myrobot.cte(radius)
diff_crosstrack_error += crosstrack_error
int_crosstrack_error += crosstrack_error
steer = - params[0] * crosstrack_error \
- params[1] * diff_crosstrack_error \
- params[2] * int_crosstrack_error
myrobot = myrobot.move(steer, speed)
if i >= N:
err += crosstrack_error ** 2
if printflag:
print myrobot
return err / float(N)

radius = 25.0
params = [10.0, 15.0, 0]
err = run(params, radius, True)
print '\nFinal paramaeters: ', params, '\n ->', err