이 포스트는 theconstructsim.com의 Kalman Filters 를 참고하였습니다.
이번 포스트에서 다룰 내용은
- 파티클 필터 속성의 이해
- main filter step 배우기
- AMCL의 설정 옵션 이해
- global positioning problem을 해결하기 위한 AMCL패키지 실행
이번 course에서는 turtlebot simulation package를 사용합니다. 아래 링크를 참조하세요
emanual.robotis.com/docs/en/platform/turtlebot3/simulation/
시뮬레이션 모델은 Waffle 모델을 사용합니다.
waffle 모델을 사용하기 위해서는 환경 변수를 추가하거나, bash shell에 해당 명령을 입력하면 됩니다.
$ export TURTLEBOT3_MODEL=waffle
또는, ~/.bashrc 를 수정하여 맨 밑에 export TURTLEBOT3_MODEL=waffle 를 추가합니다.
사용된 ROS버전은 noetic입니다.
ros noetic을 사용할 경우, 외부의 예시를 다운받을 경우 line1의 주석은 항상 python3로 수정해주세요.
#!/usr/bin/env python >> #!/usr/bin/env python3
이번 포스트 관련 이론은 다음 포스트를 참조하세요
4.3. The Particle Filter (tistory.com)
이번에는 파티클 필터(particle filter)에 대해 알아보겠습니다. 파티클 필터가 mobile robotics localization problem에 적용되면, 이 방법은 Monte Carlo Localization(MCL) algorithm이라고도 합니다. ROS Navigation Stack은 Adaptive MCL(AMCL)을 사용하는데, 이는 기본적인 파티클 필터를 확장한 개념입니다.
파티클 필터 또한 베이즈 필터 중 하나입니다. 파티클 필터는 비선형성을 공식화하고 대처하는 방식이 독특한데, 이런 특성 때문에 global localization problem을 해결하기 위해 자주 사용됩니다.
파티클 필터의 특징:
- non_Gaussian distributions를 효과적으로 나타낼 수 있습니다
- linear states-based 가정에 제한되지 않습니다
- state space를 불연속적인(discrete) 방법으로 모델링 합니다.
- multi-modal beliefs를 나타낼 수 있습니다.
이름에서 알 수 있듯이, 파티클 필터는 초기에 유한한 개수의 파티클을 state space에서 무작위로 샘플링 합니다. robot localization의 맥락에서는 파티클은 로봇 환경의 지도 전체에 걸쳐 분포됩니다.
지금까지 봤던 다른 베이즈 필터처럼, 파티클 필터도 반복적인 prediction-correction loop를 실행합니다. 또한, non-Gaussian distribution인 process와 measurement noise에도 불구하고, 상태를 잘 추정합니다.
다른 칼만 필터들과 다르게, 파티클 필터는 하나 이상의 솔루션을 모델링 할 수도 있는데, 이 특징은 multimodal state estimation이라고 합니다. 이 알고리즘의 두번째 특징은 resampling step이라는 단계가 있다는 것입니다.
1. Generic Particle Filter Algorithm
파티클 필터의 구조는 다음과 같습니다.
파티클 필터는 정의된 파티클의 숫자를 초기화하면서 시작합니다. 그 후, predict function, correct function, resample steps를 반복합니다. 이 알고리즘은 true state value 근처에 수렴할 때까지 반복됩니다.
2. Particle Filter Initialization
반복 프로세스를 시작하기 전에, 알고리즘은 state space를 통해 파티클을 initialize 및 distribute해야합니다. 그런데 정확히 파티클이 무엇일까요?
간단히 말해서, 파티클은 시뮬레이션 된 잠재적인, 로봇의 pose입니다. 각각의 파티클은 로봇이 있을 수 있는 위치에 대한 belief를 갖습니다. 따라서, 각각의 파티클은 위치 정보 (x, y)및 방향 값을 갖습니다.
연습 1. 다음 명령으로 시뮬레이션 node를 실행합니다.
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch
turtlebot의 gazebo 환경을 보면 로봇이 오른쪽 아래에 있는데, RViz에서는 정확히 지도의 중심에 있는 것을 볼 수 있습니다. 이는 파티클 필터가 초기에 특정 위치를 중심으로 초기화 하는데, 기본값이 맵의 중앙, (x, y) = (0, 0)이기 때문입니다.
만약 로봇의 true position 근처에 파티클이 없으면, 이 알고리즘은 로봇의 위치를 찾을 수 없게됩니다.
이 문제를 다루기 위한 다음과 같이 몇 가지 방법이 있습니다.
- 파티클의 평균이 로봇의 실제 위치에 더 가깝도록 초기화 합니다
- 파티클을 맵의 더 넓은 영역으로 퍼뜨립니다. 즉, 파티클의 초기 분산을 크게 설정합니다.
- 더 많은 파티클을 생성합니다.
먼저, 첫 번째 방법을 사용해보겠습니다. RViz 상단의 "2D Pose Estimate"를 클릭하고, 로봇의 실제 위치와 가까운 곳에 드래그 앤 드롭을 해주세요. 그렇게 정확하게 할 필요는 없고, 그냥 실제 위치와 비슷한 위치이기만 하면 됩니다.
다음으로, 로봇의 pose에 대한 불확실성이 큰 경우입니다. 예를 들어, 로봇이 어떤 방 안에 있다는 것은 아는데, 그 방의 어디에 있는지 모르는 경우입니다. 이런 경우에는 맵의 좀 더 큰 범위에서 파티클이 생성되도록 합니다.
다음 명령을 입력하여 로봇의 좌표와 분산을 설정해줍니다.(다음 명령을 실행하기 전에, ~/catkin_ws/kalman_filters_files/particle_filter/scripts/init_particles.py 파일에서 맨 윗줄의 주석을 #!/usr/bin/env python3 로 수정하세요
$ rosrun particle_filter init_particles.py -x -3.5 -y 5 -cov_xx 4.0 -cov_yy 4.0 -cov_aa 9.8696
세 번째 방법으로, 파티클의 수를 조절해줄 수 있습니다. 이 값은 launch파일을 수정할 수도 있지만, dynamic_reconfigure을 사용하여 실시간으로 조절할 수도 있습니다.(아래 사진처럼 파티클만 보이게 하려면 RViz 왼쪽의 플러그인에서 global map, local map, Lase scan의 체크박스를 해제하세요)
$ rosrun dynamic_reconfigure dynparam set /amcl max_particles 100
$ rosrun dynamic_reconfigure dynparam set /amcl max_particles 2000
A note on the computational effort
각각의 파티클에 대해 생각해보면, 알고리즘은 로봇이 실제로 있을 수 있는 곳들을 계산합니다. 파티클의 숫자가 많아지면 더 정확해지겠지만, 그만큼 localization process에서 필요한 계산량이 더 많아집니다. 또한, 로봇의 위치를 찾기 위한 충분한 양의 파티클이 한 번 더해지면, 그 이상 더 많은 파티클에 의한 효과는 0으로 줄어들게 됩니다. 따라서, 우리가 필요한 것은 로봇의 위치도 정확하게 찾으면서 컴퓨팅 파워도 적게 필요한 적절한 양의 파티클을 사용하는 것입니다.
만약 로봇이 어디에 있는지 전혀 모르겠다면, 다음 명령으로 지도의 모든 영역에 걸쳐 uniformly random distributed particles를 초기화할 수 있습니다.
$ rosservice call /global_localization "{}"
그러나 이 과정은 더 많은 파티클을 필요로 하고, 따라서 더 높은 computational cost가 필요합니다.따라서 이 방법은 정말 로봇이 어디에 있는지 전혀 모르겠을 경우에만 사용하는것이 좋습니다.
연습 1. 끝
3. Particle Filter Predict Step
이론상, 파티클 필터의 predict_step도 지금까지 봤던 필터들과 비슷합니다. 파티클 필터의 predict step에서 특정한 경우에는 로봇이 움직일 때, 로봇의 가능한 모든 pose를 나타내기 위해 파티클을 사용하기 때문에, 모든 파티클을 같은 움직임으로 이동시킵니다. 예를 들어, 로봇이 전방으로 움직인다면, 파티클이 어떤 방향을 가리키든 상관없이 모든 파티클을 전방으로 이동시킵니다.
noise의 처리를 고려하기 위해, prediction step은 각각의 파티클의 움직임에 multivariate Gaussian을 더합니다. 이 noise는 그 순간의 맵 전체에 있는 파티클이 더 퍼지도록 만듭니다. 이 퍼짐(dispersion)은 nosie가 있음에도 로봇의 true pose가 파티클에 의해 커버된다는 것을 보여주기 때문에 매우 중요합니다.
다음 그림은 predict step에서 noisy odometry motion model로 인해 파티클이 퍼지는 모습을 보여줍니다.
연습 2. 연습 1.과 같이 node들을 실행하고, 2D Pose Estimate로 초기 위치를 설정한 후, 다음 node를 실행하여 turtlebot을 조작하고, 어떻게 파티클이 업데이트되는지 확인해봅니다.
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
- 연습 2. 끝
4. Particle Filter Correct Step
필터가 correct_step을 실행할 때 마다, 센서의 측정도 같이 이루어집니다. 우리는 다음번의 반복 과정을 위해 correct_step이 이 측정값들을 사용한다는 것을 배웠습니다.
이번에 알아볼 내용은 다음과 같습니다.
- 측정값의 처리
- 지도상의 파티클에서 시뮬레이션 된 측정값과 실제 측정값의 비교
- 측정값 및 지도에서의 오차가 가장 작은 파티클의 우선순위 지정
먼저, 로봇은 근처의 장애물로부터의 거리를 알 수 있는 레이저 센서를 통해 주변 환경을 관찰합니ㅏㄷ.
필터는 각 파티클의 포즈에서 내부 맵에 대해 raycasting하여 레이저 스캔을 각 파티클에 대해 시뮬레이션합니다.
실제 레이저 스캔에 의해 획득된 거리와 각각의 파티클에 의해 계산된 기대 거리 측정값(expected distance measurement)의 일치 정도는 로봇의 ground truth pose주변 파티클의 likelihood를 결정합니다. 이 프로세스 동안, 각각의 파티클에 가중치가 할당됩니다.
아래 그림은 실제 레이저 스캔 값과 파티클에서 시뮬레이션 된 레이저 스캔의 값의 매칭이 더 잘될수록 더 높은 가중치를 할당하는 것을 보여줍니다.
파티클 중에서 가장 높은 가중치를 가진 파티클이 로봇의 위치를 추정할때 사용되며, 낮은 가중치를 가진 파티클은 무시됩니다.
로봇의 환경에서 unique structural features가 얼마나 중요한지 알아두세요 : 이런 feature는 많을수록 좋습니다. 만약 로봇의 환경이 애매하다고 생각해보겠습니다. 예를 들어, 모서리도 없고, 창문도 없고, 출입문도 없는 그냥 평평한 벽 두개로 이루어진 곳에서 로봇이 위치를 찾는다고 생각해보세요. 그러면 센서로부터 로봇의 상태를 결정하기 위한 유용한 정보가 없게됩니다. 이렇게 feature가 없는 긴 복도같은 환경에서는 복도에 중심에서 복도 방향으로 평행하게 바라본다면, 모든 파티클이 같은 가중치를 갖게 될 것이고, 따라서 파티클 필터는 로봇의 위치를 제대로 찾을 수 없게됩니다.
- 연습 3.
이번에는 업데이트 마다, 즉 correct_step마다 자신의 위치를 어떻게 찾아가는지를 확인해보겠습니다.
다음 명령으로 node를 실행하고, 파티클을 초기화합니다.
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch
$ rosrun dynamic_reconfigure dynparam set /amcl max_particles 10000
$ rosservice call /global_localization "{}"
위와 같이 node를 실행하면, 아래와 같이 particle이 초기화된 모습을 볼 수 있습니다.
그 후, 다음 명령을 입력하며 파티클이 어떻게 업데이트 되는지 확인해보세요
$ rosservice call /request_nomotion_update "{}"
파티클 필터는 지도의 모든 파티클에 대해 현재 데이터, 즉 빨간색으로 표시된 레이저 스캔 데이터를 시뮬레이션 해보고, 일치하지 않는 파티클은 무시하다가, 최종적으로 지도와 가장 잘 일치하는 파티클을 찾습니다.
참고로, 위 환경은 비슷한 feature가 어느정도 있기 때문에 현재 이 위치가 아니라 비슷한 다른 위치에서 수렴할 수도 있습니다. 이럴때는 global localization을 다시 실행해보고, 위 과정을 반복하세요.
- 연습 3. 끝
5. Particle Resampling
파티클의 가중치들은 correct_step 이후 바뀌게 됩니다. resampling은 가중치가 작은 파티클을 확률이 높은 다른 구역에 있는 파티클로 대체하는 작업이라고 할 수 있습니다. resampling step이 없으면, 측정에 의한 제대로 된 정보를 사용하지 못하고 그냥 파티클이 지도 전체에 퍼져있게 될것입니다. 그러므로, resampling은 파티클이 로봇의 true state로 수렴하도록 도와주는 중요한 단계입니다.
resampling동안, 특정한 수의 파티클이 무작위로 선택됩니다. 가중치가 높은 파티클은 선택 될 확률이 높고, 다음의 반복에서도 남아있게 됩니다. 낮은 가중치를 가진 샘플들은 삭제되며, 살아남은 파티클의 주변에 새로운 파티클이 생성됩니다. 결국, 이는 몇 개의 그룹으로 파티클을 만들게 됩니다. 하나의 파티클 집합 안에 있는 모든 파티클은 하나의 추정되는 pose를 만듭니다. 실제로, 몇 개의 파티클이 형성되면, 파티클 필터는 로봇의 pose로써 가능한 가설들을 효과적으로 평가합니다. 다시 말해, 파티클 필터는 로봇의 가능한 위치의 여러 가설들을 multimodal distribution이라고 하는 통계로 나타낼 수 있습니다. 마침내, 높은 가중치를 가진 파티클의 합이 가장 높은 파티클 집합은 로봇의 실제 위치를 커버하게 됩니다. 그 파티클 집합의 평균과 분산은 로봇의 estimate pose로써 사용됩니다.
AMCL은 지금까지 설명한 파티클 필터의 확장 버전입니다. AMCL은 resampling step을 활용하여 파티클의 수를 동적으로 조정합니다. 로봇 pose의 불확실성이 높으면(가중치가 낮은 파티클이 많으면), 파티클의 수가 증가하고, 로봇이 현재 위치에 대한 확실성이 높으면, 파티클의 수가 감소합니다. AMCL은 파티클의 수를 조정하기 위해 KLD-sampling이라고 하는 방법을 사용합니다.
- 연습 4.
연습 3.과 같이 파티클을 초기화 한 후, 로봇이 제자리에서 회전 운동만 하도록 명령을 내려봅니다.
$ roslaunch turtlebot3_gazebo turtlebot3_world.launch
$ roslaunch turtlebot3_navigation turtlebot3_navigation.launch
$ rosrun dynamic_reconfigure dynparam set /amcl max_particles 10000
$ rosservice call /global_localization "{}"
$ roslaunch turtlebot3_teleop turtlebot3_teleop_key.launch
제자리에서 반시계 방향으로 0.2rad/sec로 회전시킨 결과는 다음과 같습니다.
- 연습 5. 끝
'Study > [ROS] KalmanFilter' 카테고리의 다른 글
| Kalman Filters(3. Extended Kalman Filter and Unscented Kalman Filter) (0) | 2021.08.30 |
|---|---|
| Kalman Filters (2. Kalman Filter) (0) | 2021.01.20 |
| Kalman Filters (1. Bayesian Filter) (0) | 2021.01.19 |