Home [데브코스] 7주차 - ROS steering angle control using PID
Post
Cancel
Preview Image

[데브코스] 7주차 - ROS steering angle control using PID

Posted Mar 30, 2022 Updated Apr 6, 2022 21 min read


필터 기반 조향각 제어

조향각이 계속 변하면 차량이 계속 좌우로 흔들리게 된다. 그래서 조향각에 대해서도 필터링을 하면 부드러운 핸들링이 될 수 있다.

필터는 평균 필터, 이동 평균 필터, 가중 이동 평균 필터, 저주파 통과 필터(지수 가중 이동 평균 필터) 등이 있다.

핸들링을 조작하는데 과거 값은 필요가 없고, 최신 데이터가 중요하므로 가중 이동 평균 필터는 적절하지 않다.

그렇다면 차선인식 데이터를 기반으로 하거나 조향각 데이터를 기반으로 필터를 적용하는 것이 좋을 것 같다. 그러나 차선의 위치값에도 노이즈가 존재할 수 있으니 이에 대해서도 필터를 적용해야 한다.

차선 위치정보를 저장하는 변수를 생성해서 데이터를 계속 모은다. 왼쪽, 오른쪽 각각의 데이터를 이용하여 계산한 조향각 angle값에 대해 필터를 적용한다.

가중 이동 평균 필터를 적용하는데 최신 데이터에 더 많은 가중치를 부여하는 방식으로 적용한다.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25

class MovingAverage:
	# 1.
	def __init__(self, n): 
		self.samples = n
		self.data = []
		# 2.
		self.weights = list(range(1, n+1))
	
	def add_sample(self, new_sample):
		if len(self.data) < self.samples:
			# 3.
			self.data.append(new_sample)
	
		else: # 4.
			self.data = self.data[1:] + [new_sample]
		print("samples: %s" % self.data)

	def gem_mm(self): # 5.
		return float(sum(self.data)) / len(self.data)

	def get_wmm(self): # 6.
		s = 0
		for i, x in enumerate(self.data):
			s += x * self.weights[i]
		return float(s) / sum(self.weights[:len(self.data)])

1.n으로 초기화, n은 데이터 개수를 의미한다.

2.가중치 값 만들기

3.새로운 샘플을 맨 뒤에 추가

4.리스트가 꽉차면 오래된 데이터는 버리기

5.일반 평균 필터

6.가중 이동 평균 필터


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28

def start():
    global pub, image, cap, video_mode, width, height

    mm1 = MovingAverage(50)

    rospy.init_node('auto_drive')
    pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size = 1)
    image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, img_callback)
    rospy.sleep(2)

    while True:
				# 1.
        while not image.size == (640*480*3):
            continue
				# 2. 
        lpos, rpos = process_image(image)
        
				# 3.
				center = (lpos, rpos) / 2
        angle = (center - width/2)

        mm1.add_sample(angle)
        
				# 4.
				wmm_angle = mm1.get_wmm()
				
				# 5.
        driver(wmm_angle, 30)

1.카메라 영상 이미지를 한장씩 처리하기 위함

2.왼쪽 차선과 오른쪽 차선의 위치 찾기

3.차선의 중심과 화면 중앙과의 차이값을 조향각으로 계산

4.50개 샘플에서 가중이동 평균값 구하기

5.구해진 조향각을 모터제어노드로 보내기


그렇다면 조향각 데이터가 아닌 차선 위치 데이터에 적용을 해보면 어떨까? 또는 차량의 속도가 느릴 때와 빠를 때 각각 어떤 필터가 효과적인지도 확인해볼 필요가 있다.



PID 기반 조향각 제어

control 기법에는 크게 2가지가 있다.

  1. open loop control
    • controller이 입력을 받아 control signal을 보내고 프로세스에서 출력한다.
    • 결과를 확인할 수 없다.
  2. closed loop control
    • 센서 등을 통해 데이터를 수집하고 수집한 데이터를 기반으로 반복적인 피드백으로 제어한다.
    • 대표적인 기법 : PID control

피드백 제어란 process를 거쳐서 나온 output이 input에 영향을 미치는 loop를 말한다. 예를 들어 시속 60km/h로 달리려는 자동차가 있다고 할 때, 단순히 에너지 공급/차단에 대한 on/off 방식은 출력값의 변화가 너무 크고, 시간이 지나도 목표값과의 오차가 줄어들지 않는다. 그래서 PID제어기를 사용한다.

  • Proportional : 비례
  • Integral : 적분
  • Differential : 미분

제어 대상의 목표값(desired value)과 출력값(output)과의 차이로 제어하는 방식이다.


  • 오버슈트 : 최종 정상상태 값(1)을 넘어서는 상승 오차
  • 피크 시간 : 가장 큰 overshoot(최대오버슈트)가 발생했을 때 시간
  • 상승 시간 : output의 0.1부터 0.9까지 걸리는 시간
  • 정착 시간 : 최종 정상상태에 도달하는 시간

\(MV(t) = Kp * e(t) + (Ki \int e(t)dt - Ki \int e(0)dτ) + Kd * {de \over dt}\) 각각 비례, 적분, 미분에 대한 것이다.

P 제어

피드백 제어 신호가 오차에 비례하게 만드는 비례 제어기를 말한다.

\[Kp * e(t) = Pgain x error(오차)\]

편차에 따라 반비례한 값으로 조작한다.

  • Kp(p상수)가 클 때
    • 오차가 작아도 출력값이 크게 변한다.
      • 장점 : 빠른 응답속도를 가진다.
      • 단점 : 오버슈트가 발생
  • Kp가 작을 때
    • 오차가 작을수록 출력값이 작게 변한다.
      • 장점 : 오버슈트가 적게 발생
      • 단점 : 느린 응답속도

P제어기만 사용할 경우 정상상태 오차가 발생한다. 정상 상태 오차(steady state error)란 반응이 일정수준에 정작한 이후에도 존재하는 오차를 말한다. 즉, 시간을 무제한으로 늘려도 목표값에 완전히 도달하지는 못한다.

이 그래프는 예전에 직접 실험했던 자료를 가져왔다. 맨 처음 그래프는 Kp = 0 인 상태이다. 증가하다가 목표값을 지나 다시 내려온다. 이 때, Kp값을 증가시키면 진동이 일어나긴 하나 목표값에 더 빠르게 도달한다. Kp값을 더 높이게 되면 진동이 더 심하게 일어나긴 하나 빠르게 목표값에 도달한다. 또한, 10일때는 목표값인 1에 도달하지 않고 다소 크게 오차가 있다. 100일 경우 목표값과의 오차가 줄어든 것을 볼 수 있다.


I 제어

적분을 이용하여 비례 제어에서 남아있는 오차를 제거하는 제어 방법을 말한다. 출력값이 목표값에 빠르게 도달하고 수렴하게 한다.

\[Ki \int e(t)dt - Ki \int e(0)dτ = Igain x 누적 error\]

이 때는 순간 오차가 아닌 누적오차를 이용하여 조작한다.

  • Ki(i상수)가 클 때
    • 누적 오차가 빠르게 증가
      • 장점 : 빠른 응답 속도를 가짐
      • 단점 : 오버 슈트가 크게 발생
  • Ki가 작을 때
    • 누적 오차가 느리게 증가
      • 장점 : 오버슈트가 작게 발생
      • 단점 : 느린 응답 속도를 가짐

I 제어기를 사용할 경우 정상상태 오차를 줄일 수 있다. 그러나 오차가 없는 상태에도 I 제어기에 남아있는 누적오차때문에 제어값이 계속 발생한다.

응답 속도는 비슷하나 Kp값을 고정한 채로 Ki값을 증가시키니 목표값과의 오차가 거의 없어졌지만, 계속 흔들리는 상태이 존재한다.


D 제어

미분을 이용하여 진동을 줄이고 안정성을 향상하는 제어 방법이다. 급격한 출력값의 변동이 발생했을 때 급격하게 변하지 않도록 조정한다. 오차가 상수일 경우 D제어기의 출력은 0이 되어 정상상태 오차를 줄일 수 없다. 특정 신호가 급현하는 경우 미분 제어기의 출력이 급격하게 커져 시스템을 파괴하는 경우도 있어 주의해야 한다.

\[Kd * {de \over dt} = Dgain x error 변화량\]
  • Kd가 클 때
    • 장점 : 오버슈트가 작게 발생
    • 단점 : 신호가 급변하는 경우 시스템이 파괴될 수 있다
  • Kd가 작을 때
    • 장점 : 신호가 급변해도 적절한 피드백이 가능
    • 단점 : 오버슈트가 다소 크게 발생

D 제어기를 사용할 경우 시스템의 안정도를 증가시킬 수 있다. 미분이 불가능한 오차인 경우 적절한 제어가 되지 않을 수 있다.

Kd를 적용하지 않을 때는 오버슈트가 발생하는데, Kd를 적용하니 오버슈트가 거의 발생하지 않는 것을 볼 수 있다.


따라서 이상적인 제어 결과는 다음 조건을 만족해야 한다.

  • 빠른 응답속도
  • 오버슈트 발생 x
  • 정상상태 오차 발생 x

그러나 gain값들 사이의 trade-off가 존재한다. 즉, 1개가 좋아지면 다른 하나가 나빠지거나 한다. 따라서 기준을 잡고 이에 만족하도록 최적화를 해야 한다. 주로 5% 이하의 오버슈트, 0.2초 이내의 정착 시간으로 설정한다.


PID 제어를 통한 조향각 제어

참고 영상


핸들 조작에 PID 제어를 적용해보고자 한다. 이 때는 CTE(Cross Track Error) 값을 0 으로 만드는 것을 목표로 한다. 즉, 목표 궤적이 있고, 그와 실제 주행중인 거리에 대한 오차를 CTE라고 한다.


P 제어

P제어의 경우 steering angle = P * ep 식이 적용되고, 이 때 ep가 CTE이다. P제어를 핸들링에 적용할 경우 목표 지점과의 오차가 크면 핸들을 많이 꺾고, 작으면 적게 꺾는다.

P gain을 작으면 목표지점까지 오래 걸리고, 높으면 금방 도달하는 것을 볼 수 있다.


PD 제어

P gain 값에 D 제어를 추가하여 steering angle = P * ep + D * ed를 적용해보았다. 이 때, ed는 CTE rate, 즉 error 변화량을 나타낸다.

P 제어는 오차를 빨리 줄이기 위한 초록색 화살표에 해당한다. 오버슈트를 줄이기 위한 D gain는 주황색 화살표에 해당한다. 그래서 실제 주행 방향은 이 둘은 더한 방향인 진한 주황색 화살표가 된다.

D gain이 작으면 오버슈트가 발생하고, 높으면 너무 심하게 제어된다.


PID 제어

이번에는 I 제어까지 추가해서 steering angle = P * ep + D * ed + I * ei 를 적용했다.

이는 돌이나 특정 물체로 인해 궤적을 벗어낫을 때 다시 되돌아오는 상황을 살펴보았다. I gain 이 작으면 벗어나고 다시 돌아오기 어려워진다. 그러나 I gain이 높으면 너무 심하게 핸들을 꺽어 심하게 흔들린다.


이 때 closed loop control 방식을 사용한다.



PID 실제 적용

카메라를 통해 핸들을 조정할 때 PID를 적용한다면, CTE = 화면 중앙과 좌우 차선의 중점 사이의 간격 이 될 것이다. 그래서 이에 대해 PID 제어를 적용한다.

P term : \(Pgain x error(오차)\)

I term : \(Igain x 누적 error\)

D term : \(Dgain x error 변화량\)

MV(t) = P term + I term + D term


이를 파이썬에 적용하기 위해서 클래스를 구현한다.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

class PID():
  def __init__(self,kp,ki,kd):
    self.kp = kp
    self.ki = ki
    self.kd = kd
    self.p_error = 0.0
    self.i_error = 0.0
    self.d_error = 0.0

  def pid_control(self, cte):
    # 1.
    self.d_error = cte-self.p_error
    # 2.
    self.p_error = cte
    # 3.
    self.i_error += cte

    return self.kp*self.p_error + self.ki*self.i_error + self.kd*self.d_error

1.d값은 현재 cte값과 이전 p값과의 차이값(변화량)을 적용

2.p값은 cte 값 그대로 적용

3.i값은 cte값을 계속 더해서 누적한 값을 적용


과정을 조금 더 자세하게 설명하자면 다음과 같다.

CTE = 목표값 - 현재값

  • 목표값 : 영상처리를 통해 찾아낸 좌우 차선의 중점 위치
  • 현재값 : 화면 중앙점 위치 (가로 640)
  • => CTE = (rpos + lpos)/2 - 320


  • PID 조향각 설정 코드
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

class PID():
  def __init__(self,kp,ki,kd):
    self.kp = kp
    self.ki = ki
    self.kd = kd
    self.p_error = 0.0
    self.i_error = 0.0
    self.d_error = 0.0

  def pid_control(self, cte):
    self.d_error = cte-self.p_error
    self.p_error = cte
    self.i_error += cte

    return self.kp*self.p_error + self.ki*self.i_error + self.kd*self.d_error

def process_image(image):
    global Width
    global Offset, Gap

    # gray
    gray = cv2.cvtColor(frame,cv2.COLOR_BGR2GRAY)

    # blur
    kernel_size = 5
    blur_gray = cv2.GaussianBlur(gray,(kernel_size, kernel_size), 0)

    # canny edge
    low_threshold = 60
    high_threshold = 70
    edge_img = cv2.Canny(np.uint8(blur_gray), low_threshold, high_threshold)

    # HoughLinesP
    roi = edge_img[Offset : Offset+Gap, 0 : Width]
    all_lines = cv2.HoughLinesP(roi,1,math.pi/180,30,30,10)

    # divide left, right lines
    if all_lines is None:
        return 0, 640
    left_lines, right_lines = divide_left_right(all_lines)

    # get center of lines
    frame, lpos = get_line_pos(frame, left_lines, left=True)
    frame, rpos = get_line_pos(frame, right_lines, right=True)

    # draw lines
    frame = draw_lines(frame, left_lines)
    frame = draw_lines(frame, right_lines)
    frame = cv2.line(frame, (230, 235), (410, 235), (255,255,255), 2)
                                 
    # draw rectangle
    frame = draw_rectangle(frame, lpos, rpos, offset=Offset)
    #roi2 = cv2.cvtColor(roi, cv2.COLOR_GRAY2BGR)
    #roi2 = draw_rectangle(roi2, lpos, rpos)

    # show image
    cv2.imshow('calibration', frame)

    return lpos, rpos

def start():
  global pub, image, cap, width, height

  rospy.init_node('auto_drive')
  pub = rospy.Publisher('xycar_motor', xycar_motor, queue_size=1)

  image_sub = rospy.Subscriber("/usb_cam/image_raw", Image, img_callback)
  rospy.sleep(2)

  while True:
    while not image.size == (640*480*3): continue

    lpos, rpos = process_image(image)

    center = (lpos + rpos)/2
    error = (center - width/2)

    pid = PID(0.5,0.0005,0.05)

    angle = pid.pid_control(error)
    drive(angle,30) # drive(angle, speed)


실험 과정

  1. gain값을 임의로 설정한다.
  2. P gain은 1이하, I gain은 매우 작게 0.001이하, D gain은 0.1 이하로 설정
    • 대체로 P gain = 0.5, I gain = 0.0005, D gain = P gain의 1/10
  3. 값을 넣고 차를 구동시켜서 현상을 관찰하며 적절하게 gain값을 조정
    • P gain = 0.5, I gain = 0.0005, D gain = 0.05
      • 차가 진동하면서 이동한다면 오버슈트가 발생해서 목표값 이상의 값을 만들기 때문에 발생하는 현상이므로 P gain을 감소시켜 오버슈트를 감소시킴
    • P gain = 0.45, I gain = 0.0005, D gain = 0.05
      • 차의 진동이 완화되었지만, 여전히 진동이 발생한다면 D gain값이 작아서 오버슈트를 줄이지 못하기 때문일 수 있으므로 D gain을 증가시킨다.
    • P gain = 0.5, I gain = 0.0005, D gain = 0.15
      • 차의 진동은 잡았지만 중앙선에서 벗어난 주행을 한다면 정상상태 오차이므로 I gain을 증가시킨다.
    • P gain = 0.5, I gain = 0.0007, D gain = 0.15
      • 차가 안정적인 주행을 한다면 성공


이 때 차량의 속도를 높이면 PID 제어를 다시 해야 한다. 속도를 높인다면 정착시간까지 반응이 일어나지 않기 때문에 I 제어기가 필요하지 않다. 따라서 PD 제어기만 사용하여 P = 0.55, D = 0.4로 지정해본다.


Classlog, devcourse
devcourse ros
This post is licensed under CC BY 4.0 by the author.
Recently Updated
Contents