기본적으로 터틀봇에서 위치인식을 진행하기 위한 노드는
src/navigation/amcl/src/amcl_node.cpp 에 대한 내용들로 작업된다.
그 후 해당 코드의 생성자가 실행되고 reconfigureCB( )함수가 실행되며 생성자 함수가 종료된다.
그 후에 진행되는 함수들을 나열하면
laserReceived( ) -> mapReceived( ) -> handleMapMessage( ) -> convertMap( ) -> updatePoseFromServer( )
-> applyInitialPose( ) -> laserReceived( ) -> getOdomPose( ) -> laserReceived( ) -> getOdomPose( ) -> ...
위와 같은 순서로 진행되며 laserReceived와 getOdomPose가 반복되며 실행되게 된다.
중간중간 savePoseToServer( ) 함수가 실행되며 초기 위치를 지정해주게 되면 initialPoseReceived -> handleInitialPoseMessage -> getYaw -> applyInitialPose -> laserReceived -> getOdomPose -> laserReceived -> ...
위의 과정을 진행하며 결국 다시 laserReceived 와 getOdomPose 를 반복하며 주기적으로 savePoseToServer를 실행하게 된다.
그럼 파티클이 수렴하는 과정에는 어떤 함수가 실행될까?
이 과정에서는 amcl_laser.cpp 소스의 UpdateSensor( ) 함수와 LikelihoodFieldModel 함수가 실행되게 됨을 확인하였다.
이제 조금 더 자세히 과정을 살펴보자.
amcl_node.cpp 파일에 아래와 같은 부분이 존재한다.
아래의 코드에서 ldata.range_count는 360의 값을 갖고있는 변수로 이는 Lidar센서가 360도 회전하며
1도 회전할때마다 거리를 취득하게 되는 것으로 예상할 수 있다.
즉 아래의 코드는 라이다센서가 1회 회전할때 취득되는 거리들을 ldata 변수에 저장하는 것으로 볼 수 있다.
ldata.range[n][0]은 거리값, ldata.range[n][1]은 해당 거리값이 얻어진 라이다의 회전 각도로 볼 수 있을 것이다.
그리고 최종적으로는 아래의 코드에 나타낸 UpdateSensor( ) 함수와 pf_update_resample( ) 함수를 통해
파티클의 정보를 업데이트하고 resampling하는 과정을 진행하게 된다.
pf_updata_reample( )을 주석처리 하게되면 파티클들이 로봇의 odom에 따라 이동은 하게 되지만
취득되는 Lidar 데이터와 지도를 매칭시키는 과정이 없으므로 운동량에 따라 파티클들이 이동만 하게되고
위치인식을 진행하지 않게 된다.
반면 UpdateSensor( ) 함수를 주석처리 하게 되면 아래 그림과 같이 파티클들이 어느정도 수렴하게 되기는 하지만 정확한 위치를 나타내지 못하고 심한 오차를 갖게 되는 것을 확인할 수 있다.
하지만 resampling단계에서 파티클들의 오차분포를 포함하여 re-distribution하게 되고 이를 통해 resampling을 진행하므로 어느정도 로봇의 운동량을 추정할 수 있게 됨을 확인할 수 있다.(아래 그림에서 파티클들의 방향이 로봇과 어느정도 일치한다는 것을 통해 이를 확인할 수 있다.)
UpdateSensor( ) 함수에서는 어떠한 과정이 진행되기에 이러한 문제가 발생하는 것일까?
UpdateSensor( ) 함수 먼저 살펴보면 이는 아래와 같이 amcl/sensors/amcl_laser.cpp에 위치하고 있고
amcl_laser.cpp에서 실행되는 pf_update_sensor는 pf.c에 해당 코드가 존재하게 된다.
UpdateSensor( )함수의 파라미터로는 파티클필터의 정보와 Lidar의 취득 데이터가 들어가게 되는데
최종적으로 해당 함수는 취득된 data를 토대로 파티클필터의 정보를 업데이트 하기 위함으로 보인다.
위의 코드는 pf.c에 존재하는 pf_update_sensor( ) 함수의 내용이다.
line281에 total 변수를 통해 sample의 weight가 결정되는 것으로 보이는데 total변수는 (*sensor_fn)의 값으로 이는(sensor_data, set)을 통해 값이 지정되는 것으로 보인다. (sensor_fn)은 함수의 파라미터로 pf_sensor_model_fn_t 자료형을 띄고 있는데 이는 아래와 같이 pf.h에서 정의된 것을 확인할 수 있다.
(pf_sensor_model_fn_t 형은 double형의 재정의로 볼 수 있다.)
그럼 double값을 갖는 pf_sensor_model_fn_t 는 sensor_data와 set을 통해 어떻게 값이 결정되는가?
이 값은 amcl_laser.cpp에서 처음 update_sensor 함수가 실행될 때 정해지게 되는데 이 부분을 다시 확인해보면 LikelihoodFieldModel의 값이 인자로 전달되는 것을 확인할 수 있다.
그리고 LikelihoodFieldModel의 값은 동일한 source file에서 정의된 것을 확인할 수 있다.
즉, 위의 과정을 통해 double값을 return하게 되고 이 값이 가중치를 나타내는 값으로 sensor_update 함수에 전달되는 것이다.
그럼 이 LikelihoodFieldModel( ) 함수를 이해하면 파티클의 가중치값이 어떻게 결정되는지 알 수 있을 것이다.
우선 이 과정에서는 반복문을 통해 각 샘플(하나의 파티클)에 대해 파티클의 위치를 받아오고
라이다에서 취득되는 360개의 거리 데이터를 통해 각각의 라이다 끝점의 위치를 파악한다.
라이다 끝점의 위치는 레이저가 반사된 지점으로 이는 로봇의 기준으로 위치를 나타내게 된다.
따라서 로봇의 위치를 기준으로 표시된 라이다의 끝 점을 지도를 중심으로 변환해주는 과정을 진행하고 이 부분은 아래 그림에 나타낸 부분이다.
지도를 기준으로 표현되는 라이다의 끝점 mi, mj 정보를 바탕으로 z값이 계산되고 이를 통해 최종적인 weight값이 계산되는데 여기서 z값이 의미하는 바는 아직 이해가 부족하다.
UpdateSensor()에서 particle filter를 재정의하고 sample들에 대한 가중치를 재정의 한 후
resample()에서 파티클들을 가중치에 따라 재정의하는 것인가?