[Pegasus] 이론적 전달함수 모델링의 한계

Ros2 - Matlab 통신 인터페이스

각 환경에서 서버와 클라이언트가 잘 작동하는 것을 확인 했으니, 두 환경 사이의 통신을 확인할 차례이다.

먼저 Matlab에서 수신할 14개의 데이터를 각각 데이터 그룹별로 분리하는 다이어그램을 만들었다.

14개의 데이터는
- pos (x, y, z) : 지도 기준 현재 위치
- vel (vx, vy, vz) : 지도 기준 이동 속도
- pqr (wx, wy, wz) : 롤/피치/요 각도의 초당 각속도.
  - 우리는 지상 로봇이기 때문에 wz 값을 주요하게 본다.
- euler (roll, pitch, yaw) : 롤/피치/요 각도
  - ros2는 자세를 쿼터니언으로 4개의 숫자로 주는데, 직관적으로 롤/피치/요가 좋기 때문에 변환해서 사용한다.
- body_vel : 전진 속도, 좌우 속도를 로봇 기준으로 나타낸 속도
  - 지상 좌표계 기준이 아닌, 로봇이 자기가 보기에 앞으로 가면 전진, 뒤로 가면 후진이다.
Ros2 파이썬에서 리틀에디안으로 값을 보내고 받고 있기 때문에 리틀 에디안을 사용한다.

Matlab에서 송신한 속도 1, 각속도 3 데이터가 Ros2 터미널에 수신되었고

Ros2에서 송신한 값들도 Matlab에서 수신되는 것을 볼 수 있다.

          self.robot_pos = [1.0, 2.0, 3.0]
          self.robot_vel = [4.0, 5.0, 6.0]
          self.robot_body_vel = [0.0, 0.0]
          self.robot_pqr = [0.0, 0.0, 0.0]
          self.euler = [0.0, 0.0, 0.0]
          self.robot_cmd = [0.0, 0.0]
          self.linear_cmd = 0.0
          self.angular_cmd = 0.0

통신 테스트 - 문제 발생

제어를 적용하지 않고 단순히 음의 피드백만 적용된 시스템이다.
step input을 0.5로 주고 실행했지만 로봇이 전진이 아닌 후진을 하기 시작했다.

응답 그래프도 음수로 뒤집혀 있었다.

통신 테스트 - 문제 해결

문제는 모델링 좌표계였다. 바퀴의 회전 방향은 양의 z 축을 기준으로 한 오른손 법칙의 회전 방향이였다.
하지만 바퀴는 z축이 몸체 링크의 z축과 같은 초기 상태였고, x축을 기준으로 양의 roll 회전을 하고나니 전진 명령을 걸면 -x 축으로 회전하는 것이였다.
```
  <link name="front_left_wheel">
  		  <pose>0.0 0.25 0.03 -1.57079 0 0</pose>
```
롤 회전 방향을 음수로 바꿔주니 정상적으로 전진할 수 있게 되었다.

PID 제어 적용

이제 플랜트 앞에 PID 제어기를 추가해서 응답이 어떻게 바뀌는 지 확인할 차례이다.

블록 다이어그램은 위와 같은 피드백 구조로 구성했다.

앞서 설계한 병진 운동의 PID 제어 파라미터를 적용했다.
시스템에 이산적인 데이터가 들어오기 때문에 일반적인 미분/적분 블록을 사용하면 값이 정확하지 않다.
따라서 이산 미분/적분 블록을 사용하여 인풋 데이터에 맞게 PID 계산을 해주었다.

0.5의 스텝 인풋에 의해 1차 시스템과 유사하게 오버슈트 없이 0.5 주변으로 수렴하는 결과를 얻을 수 있다.
전체적인 경향성은 예상대로 나왔지만, 자글 자글한 이유가 노이즈인지 시스템 다이어그램 설계 문제인지 궁금했다.
따라서 가우시안 노이즈 값을 0으로 설정해봤다.
큰 차이가 없었다.
가우시안 노이즈도 값이 작은 값인 0.01 표준 편차로 지정되어 있어서 크게 영향이 없었던 것이다.
다음으로 I 제어기 파라미터를 15에서 5로 줄여봤다.
라이징 타임만 늘어났지 미세한 진동은 잡히지 않았다.
진동의 원인을 찾아보는 도중 2가지 가능성을 알아냈다.
1. 물리 엔진의 접촉/마찰 때문에 실제 속도가 미세하게 출렁이는 상황.
  1. 바퀴와 지면 접촉은 수치해석이라 상수 마찰계수로 고정되지 않는 경우 발생
2. 샘플링 주기와 matlab/ros2의 타이머 주기가 맞지 않아서 잔떨림 발생

값이 사라지는 현상

응답 출력 도중 그래프가 끊기면서 로그의 값들도 nan으로 바뀌며 로봇이 원점으로 순간이동 하는 현상이 있다.

어느 순간 cur 값이 매우 크게 커지면서 아래 로그를 내보낸다.

[ERROR]: Error receiving UDP data: The ‘data’ field must be a float in [-3.402823466e+38, 3.402823466e+38]

값이 지정 범위보다 커져서 생긴 문제이며 gazebo의 물리값이 수치적으로 불안정하다는 것을 의미한다.

$K_p$ 값이 큰 경우 오차를 더 크게 반영하므로 더 쉽게 불안정해지는 경향이 있었다.

[INFO]: Received from ('127.0.0.1', 41356): b'\xfc\xa9`\xbf5\xdb\xfc?~d\xed$\x9d\xf0\xaf?'
[cmd] linear :  1.8119537873993377  angular :  0.06435719878402678
[cur] linear :  1.0991584502632518e+50  angular :  -5.976224117146322e+51
[INFO]: Received from ('127.0.0.1', 41356): b'X\xe5+A\xc3\xfd\xfc?\xff \n\xa0\xb6y\xb0?'
[cmd] linear :  -1.0991584502632518e+51  angular :  1.1952448234292644e+52
[cur] linear :  nan  angular :  nan
[ERROR]: Error receiving UDP data: The 'data' field must be a float in [-3.402823466e+38, 3.402823466e+38]
[cmd] linear :  -1.1156458270172006e+51  angular :  1.2311021681321425e+52
[cur] linear :  nan  angular :  nan
[ERROR]: Error receiving UDP data: The 'data' field must be a float in [-3.402823466e+38, 3.402823466e+38]
[cmd] linear :  -1.1321332037711494e+51  angular :  1.2669595128350202e+52
[cur] linear :  nan  angular :  nan
[ERROR]: Error receiving UDP data: The 'data' field must be a float in [-3.402823466e+38, 3.402823466e+38]
[cmd] linear :  nan  angular :  nan
[cur] linear :  nan  angular :  nan
[INFO]: Received from ('127.0.0.1', 41356): b'\x00\x00\x00\x00\x00\x00\xf8\xff\x00\x00\x00\x00\x00\x00\xf8\xff'
[cmd] linear :  nan  angular :  nan
[cur] linear :  nan  angular :  nan
[INFO]: Received from ('127.0.0.1', 41356): b'\x00\x00\x00\x00\x00\x00\xf8\xff\x00\x00\x00\x00\x00\x00\xf8\xff'
[cmd] linear :  nan  angular :  nan
[cur] linear :  nan  angular :  nan
[INFO]: Received from ('127.0.0.1', 41356): b'\x00\x00\x00\x00\x00\x00\xf8\xff\x00\x00\x00\x00\x00\x00\xf8\xff'

검색해보니 물리 엔진이 발산함에 따라 발생하는 흔한 현상이라는 정보를 얻을 수 있었다.

이상적인 제어와 시뮬레이션 환경 제어 비교

이상적인 환경에서 설계한 전달함수로 pid 제어하는 다이어그램이다.

시뮬레이션 환경에서 받아오는 값으로 pid 제어하는 다이어그램이다.

이 두 값을 모두 Matlab 워크스페이스에 내보내서 그래프를 그려서 비교해보려고 한다.

$K_p = 6, K_i = 15$ 인 경우 동일한 0.5의 step input을 넣었을 때 속도 응답이다.
I 제어기의 영향으로 0.5로 $e_{ss}$ = 0 으로 수렴하는 것을 볼 수 있다.
P 제어기의 $K_p$ 값이 너무 커서 그런지 공진처럼 미세 노이즈가 남아 있는 것을 볼 수 있다.
왜 이런 차이가 발생할까?
- 모터 플러그인에 이미 제어기가 있어서 오차를 반영하여 전압을 만들고 [-1,1]로 클램프 한다.
- 모터 플러그인의 모델링은 내가 시뮬링크로 모델링한 전달함수와 다르다.
- 저항/인덕턴스/감쇠/관성에 의해 전류에서 토크로 즉시 바뀌지 못하고 에너지가 소모된다.
- 바퀴와 지면 사이의 마찰이 있기 때문에 에너지가 소모되어 초기에 오버슈트가 잘 나지 않는다.

Gazebo 환경에서의 응답을 기준으로 노이즈를 줄이고 수렴속도를 높이기 위해 $K_p$ 와 $K_i$ 값을 수정해보았다.

병진 속도는 $K_p = 1, K_i = 35$ 일때 오버슈트 없이 가장 안정적인 응답이 나왔다.

회전 속도는 $K_p = 0.6, K_i = 20$ 일때 가장 안정적인 응답이 나왔다.
수학적인 계산으로 1차 파라미터를 찾고 그 파라미터를 기준으로 실제 환경이나 시뮬레이션에 적용했을때, 이론적으로 찾은 파라미터가 최적이 아니였다.
지글러-니콜스 방법 처럼 경험적으로 파라미터를 쉽게 찾는 등의 방법론이 있지만 결국 pid 제어에서는
trial-error 방식으로 더 적합한 파라미터를 찾아 반복하는 과정이 제어기 설계의 핵심이라는 것을 배울 수 있었다.