토픽 결과물: Mobile 로봇이 벽을 따라갈 수 있어요.
이번 토픽에서는 wall following을 흉내내는 정도로 구현합니다. wall following의 핵심은 로봇의 측면 거리를 벽과 유지하면서 주행하는 것입니다. 이 때, 간단히 측면 거리를 일정 거리로 유지하도록 PD 제어를 하면 됩니다. P, D gain은 적절히 선택합니다. 항상 gain 선택하는 것이 쉽진 않네요. 여기서는 장애물 상태에 따라 wall following 방향을 정하는 부분, PD 제어로 wall following 하는 부분, 장애물 상태에 따라 일반 주행에서 wall following 주행 상태 변이에 대해서 다룹니다.
설명 동영상
Wall Following 방향 정하기
장애물 상태에 따라 wall following 할 방향을 정합니다. 예를 들어 로봇 오른쪽에 장애물이 있다면 로봇 오른쪽 면으로 wall following하고, 왼쪽에 장애물이 있다면 왼쪽 면으로 wall following 합니다. wall following 방향 정하는 것은 주행전략에 따라 변경될 수 있습니다. 예를 들어 모든 영역이 막혀있다면 사용자가 적절히 선택해 주어야 합니다. 여기서는 단순히 장애물 상태에 따라 방향을 정하는 코드를 구현하였습니다.
/*
* Determine wall following direction based on the obstacle state.
* It is called when obstacle is detected.
*/
int MRS::determineWFDirection()
{
// set left as default direction
int wf_dir= WF_LEFT;
// right direction cases
switch( obs_state_ )
{
case OBS_R:
case OBS_FR:
case OBS_FR_R:
case OBS_F_R:
wf_dir= WF_RIGHT;
break;
case OBS_F:
// choose the side having the shorter distance
if( obs_dist_[FRIGHT] < obs_dist_[FLEFT] )
wf_dir= WF_RIGHT;
break;
case OBS_ALL:
// choose the direction to the goal
break;
case OBS_L_R:
break;
default: // wf_dir= WF_LEFT cases: OBS_L, OBS_L_FL, OBS_L_F, OBS_FL, OBS_CLEAR
break;
}
return wf_dir;
}
Wall Following 제어하기
아래 구현된 wall following은 단순히 벽과의 거리가 desired distance가 되도록 PD 제어하는 부분입니다. 즉, 센서로부터 획득한 벽면과의 거리값과 desired distance의 차이, 즉 오차를 계산하여 desired distance를 추종하도록 P, D gain을 곱합니다. 여기서는 선속도는 일정한 값을 사용하고, 각속도만 오차를 줄이도록 변경시켰습니다.
/*
* Do wall following.
* @param wf_dir wall following direction
* @param desired_dist desired wall following distance
*/
void MRS::doWallFollowing( const int _wf_dir, const double _desired_dist )
{
// current distance to the wall
double dist= 0.0;
// initialize the error distance
double dist_error= _desired_dist;
// static double old_dist_error= 0.0;
// left wall following
if( _wf_dir == WF_LEFT )
{
dist= obs_dist_[LEFT];
// compute the distance error
if( dist <= _desired_dist )
{
dist_error= (_desired_dist - dist );
}
else
{
dist_error= -(dist - _desired_dist);
dist_error*= 0.5;
}
}
// right wall following
else
{
dist= obs_dist_[RIGHT];
if( dist <= _desired_dist )
{
dist_error= -(_desired_dist - dist );
}
else
{
dist_error= (dist - _desired_dist );
dist_error*= 0.5;
}
}
// @todo reset old error distance if a new wall following starts
// by counting the dist having over threshold?
// gain tuning
double p_gain= params_.wf_slow_pgain * dist_error;
double d_gain= params_.wf_slow_dgain * (dist_error - old_dist_error_ );
double angular_vel= p_gain + d_gain;
angular_vel *= -1;
double linear_vel= params_.obs_approach_linvel*2;
#if __DEBUG__
ROS_WARN( "[WF] dist, desired dist, dist_error, old_dist_error: %.4f, %.4f, %.4f, %.4f", dist, _desired_dist, dist_error, old_dist_error_ );
ROS_WARN( "[WF] p, d gain: %.5f, %.5f", p_gain, d_gain );
ROS_WARN( "[WF] linear, angular vel: %.4f, %.4f", linear_vel, angular_vel );
#endif
// publish velocity
publishVelocity( linear_vel, angular_vel );
// update the old error distance
old_dist_error_= dist_error;
}
ScanCB() 함수 수정
이미 작성된 scanCB 함수 끝부분에 아래의 내용을 추가합니다. 아래 구현된 부분은 로봇이 일반주행하다가 장애물을 만났을 때, wall following으로 주행을 변경하는 부분에 대한 것입니다. 대략적인 주행 변이는 아래와 같습니다.
- 일반 주행(drive)중 전방에 장애물 만나면 감속하며 접근(approach) 한다.
- 일반 주행(drive)중 측면에 장애물 감지되면 wall following 한다.
- 접근(approach)중에 로봇 전방에 장애물 감지되면 회피(avoid) 한다.
- wall following 중에 장애물 감지 안되면 일반 주행(drive)로 변경한다.
/*
* scan callback
*/
void MRS::scanCB( const sensor_msgs::LaserScanConstPtr& _scan )
{
...
// determine wall following direction or choose one
int wf_dir= determineWFDirection();
// jump to a branch based on the driving state
switch( driving_state_ )
{
case DRIVE:
// obstacle detected?
if( detected )
{
// check if either side of the robot is close to a wall
if( obs_dist_[LEFT] <= params_.desired_wf_dist || obs_dist_[RIGHT] <= params_.desired_wf_dist )
{
// transit to wall following driving
driving_state_= DRIVE_WF;
#if __DEBUG__
ROS_WARN( "[DRIVE] --> [DRIVE_WF]" );
#endif
break;
}
// front obstacle?
if( obs_state_ > OBS_FRONT_SECT )
{
// transit to approaching driving
driving_state_= DRIVE_APPROACH;
// approach slowly
publishVelocity( params_.obs_approach_linvel, 0.0 );
#if __DEBUG__
ROS_WARN( "[DRIVE] --> [DRIVE_APPROACH]" );
#endif
}
// side obstacle
else
{
// transit to wall following driving
driving_state_= DRIVE_WF;
#if __DEBUG__
ROS_WARN( "[DRIVE] --> [DRIVE_WF]" );
#endif
}
}
else
{
// drive normally
publishVelocity( params_.drive_linvel, 0.0 );
}
break;
case DRIVE_APPROACH:
#if __DEBUG__
ROS_INFO( "[approach] param thresh: %.2f, obs_dist: %.2f", params_.obs_dist_threshs[OBS_SECTION_SIZE], obs_dist_[FLEFT] );
#endif
// check if either side of the robot is close to a wall
if( obs_dist_[LEFT] <= params_.desired_wf_dist || obs_dist_[RIGHT] <= params_.desired_wf_dist )
{
// transit to avoiding driving
driving_state_= DRIVE_AVOID;
// avoid obstacle by rotating the robot
publishVelocity( 0., wf_dir*params_.obs_avoid_angvel );
#if __DEBUG__
ROS_WARN( "[DRIVE_APPROACH] --> [DRIVE_AVOID]" );
#endif
break;
}
// approaching done? checking the front sections
if( obs_dist_[FLEFT] <= params_.obs_dist_threshs[OBS_SECTION_SIZE] ||
obs_dist_[FRIGHT] <= params_.obs_dist_threshs[OBS_SECTION_SIZE] )
{
// transit to avoiding driving
driving_state_= DRIVE_AVOID;
// let the robot rotate
publishVelocity( 0., wf_dir*params_.obs_avoid_angvel );
#if __DEBUG__
ROS_WARN( "[DRIVE_APPROACH] --> [DRIVE_AVOID]" );
#endif
}
else
{
// approach slowly
publishVelocity( params_.obs_approach_linvel, 0.0 );
}
break;
// rotate until no front obstacle is detected
case DRIVE_AVOID:
// keep rotating while front obstacle exists
if( obs_dist_[FLEFT] <= params_.obs_dist_threshs[FLEFT] ||
obs_dist_[FRIGHT] <= params_.obs_dist_threshs[FRIGHT] )
{
// rotate the robot just in case ( robot is already rotating)
publishVelocity( 0., wf_dir*params_.obs_avoid_angvel );
break;
}
// now no front obstacle
// transit to wf
driving_state_= DRIVE_WF;
// clear the error distance
old_dist_error_= 0.0;
#if __DEBUG__
ROS_WARN( "[DRIVE_AVOID] --> [DRIVE_WF]" );
#endif
break;
case DRIVE_WF:
// no obstacle?
if( !detected )
{
// transit to normal driving state
driving_state_= DRIVE;
#if __DEBUG__
ROS_WARN( "[DRIVE_WF] --> [DRIVE]" );
#endif
// exit the wall following state
break;
}
else
{
// check if either side of the robot is close to a wall.
if( obs_dist_[LEFT] <= params_.desired_wf_dist-0.2 || obs_dist_[RIGHT] <= params_.desired_wf_dist-0.2 )
{
doWallFollowing( wf_dir, params_.desired_wf_dist );
break;
}
// front obstacle?
if( obs_dist_[FLEFT] <= params_.obs_dist_threshs[FLEFT] ||
obs_dist_[FRIGHT] <= params_.obs_dist_threshs[FRIGHT] )
{
// transit to approaching driving
driving_state_= DRIVE_APPROACH;
// approach slowly
publishVelocity( params_.obs_approach_linvel, 0.0 );
#if __DEBUG__
ROS_WARN( "[DRIVE_WF] --> [DRIVE_APPROACH]" );
#endif
break;
}
// keep wall following
doWallFollowing( wf_dir, params_.desired_wf_dist );
}
break;
default:
ROS_WARN( "Driving state is undefined" );
break;
} // end of switch( driving_state_ )
} // end of scanCB()
노드 실행
노드를 실행하기 전에 catkin_make하여 소스를 빌드합니다. 아래와 같이 Gazebo 시뮬레이션 노드를 실행하면 로봇이 스스로 앞으로 이동하며 벽을 만나면 wall following을 하게 됩니다. 본 토픽에서는 정밀하게 wall following구현 하지 않아서, 로봇이 벽면 모서리에서 wall following을 유지하지 않고, 앞으로 나아가게 됩니다. wall following 모션이 제대로 동작 안한다면 mrs.launch.xml 파일에서 P, D gain 파라미터값 변경과 desired distance 변경 등을 해봅니다. 또는 보다 정교하게 wall following 전략을 코딩해 보는 것도 좋습니다. wall following이 어느 정도 되면 zigzag 모션 등을 구현해 볼 수 있습니다.
$ roslaunch mobile_robot_sim turtlebot_hokuyo.launch
