ekf_localization (Wheel + IMU odometry)

- 개발 환경 : Raspberry Pi 4B (Ubuntu 18.04 / ROS melodic)

- 사용품 : md400T(모터드라이버), MDT150(인휠모터), ydlidar X4(라이다),

                IntelliThings iAHRS RB-SDA-v1(IMU센서)

- 참고 : https://cafe.naver.com/openrt/26846

 

 

-----------------------------------------------------------------------------------------------------------------------------------------------------------------

 

* 보통 모바일 로봇 Naviagation에서 엔코더나 홀센서를 이용한 wheel odometry를 기본적으로 사용하는데, 바닥과의 슬립으로 인하여 로봇의 자세가 맞지 않는 문제가 빈번하게 발생합니다. 따라서 ekf_localization을 이용하여 IMU센서를 Fusion하므로써 보정할 수 있습니다.

 

1. IMU 센서 장착 및 환경 구축

- 로봇의 구동축 중심 위치에 IMU를 장착

로봇 구동축 중심에 IMU 장착

- IMU 환경 구축

  https://do-9un6.tistory.com/20

 

 

2. odometry 코드 수정

- 모바일 로봇의 Wheel odometry를 계산하는 부분에서 odom -> base_link(or base_footprint) 좌표변환에 사용되는 소스를 제거 또는 조건문 작성하기

 

- 저의 경우 md사의 package에 코드를 추가하여 odometry를 publish했으므로 cmd_main.cpp 파일에서 아래의 코드를 추가하여 수정했습니다.

//ekf_localization
bool m_bEKF_option = false;


//odom_pub
void pub()
    {

	if(!m_bEKF_option) //add...ekf_localization option _ wbjin
	{
		odom_trans.header.stamp = current_time;
		
		odom_trans.header.frame_id = "odom";
		odom_trans.child_frame_id = "base_footprint";
		
		odom_trans.transform.translation.x = coordinates[0];
		odom_trans.transform.translation.y = coordinates[1];
		odom_trans.transform.translation.z = 0.0;
		odom_trans.transform.rotation = odom_quat;
		odom_broadcaster.sendTransform(odom_trans);
	}
    
    
//Node main function
int main(int argc, char** argv)
{
    //Read Conveyor Option Param Read//
    nh.getParam("ekf_option", m_bEKF_option);
    printf("##ekf_option: %d \n", m_bEKF_option);

 

3. ekf_localization.yaml 파일 만들기

- navigation/param 폴더에 ekf_localization.yaml 파일을 아래와 같이 만들어줍니다.

ekf_localization.yaml

- IMU에서 x, y축 노이즈 현상이 빈번하게 발생합니다. 따라서 odom0_config에 x, y Pose를 적용하고, imu0_config에는 IMU의 Yaw 각속도만 적용해줍니다.

 

4. launch 파일 수정

- 모바일 로봇의 기본구동을 하는 launch file에 ekf_localization.yaml 파일을 로드해서 ekf_localization_node를 호출하도록 구성해야합니다. (필자의 경우 morobot_bringup/launch/morobot_G2.launch 파일에서 아래 부분을 추가했습니다.)

morobot_G2.launch

 

- move_base를 호출하는 launch file에 odom topic을 기존에 사용하던 odom에서 odometry/filtered로 바꿔주어야 합니다.

  (필자의 경우 morobot_navigation/launch/move_base.launch 파일을 아래와 같이 수정했습니다.)

move_base.launch

 

- 위의 사항들을 다하고 나면, Navigaiton 실행 시에 로봇을 발로 밀거나 슬립이 생겨도 위치를 잘 찾는 것을 확인할 수 있습니다.