[PX4 개발자] Obstacle Avoidance 기능 구현

 

 

PX4에서 충돌회피(Obstacle Avoidance 또는 Collision Avoidance)를 SITL환경에서 구현하는 방법에 대해 알아보자.

 

PX4 개발자 사이트에서 제공하는 정보를 바탕으로 작성하였다.

 

github.com/PX4/PX4-Avoidance#run-on-hardware

PX4/PX4-Avoidance

충돌회피는 미션모드 또는 Offboard 모드 두가지 모드에서만 실행이 가능하다.

 

- Companion computer가 필요하다.

- Local planner, Global planner 등의 offboard 용 app을 통해 구현할 수 있다.

- 초당 5~10Hz정도 속도로 처리된다.

 

 

방법은 세가지가 있으며 모두 독립적인 기능을 갖추고 있다.

1.  Local planner

-  VFH(Vector Field Histgram)+ 기반의 planner로 구현, 연산량이 상대적으로 적음

2. Global Planner

- 전통적인 Octomap occupancy grid의 그래픽 기반의 planner로 구현, 정확도 높으나 연산량이 많음

- Global positioning, Heading 정보를 이용하여 Depth 계산

3. Safe Landing

- z축 좌표 포인트 클라우드의 평균값 및 편차를 이용하여 바닥면의 그리드를 형성하고 경계면을 구분

 

 

 

SITL환경을 구현하기 위해서는 먼저  Ubuntu 18.04에 ROS Melodic과 Gazebo 및 PX4 펌웨어를 컴파일할 수 있는 필수 툴체인들이 설치되어 있어야 한다.

 

필요한 과정들은 아래 포스팅을 참고하면 된다.

 

 

swiftcam.tistory.com/267

[PX4 개발자] Ubuntu 18.04에 ROS(Melodic)/Gazebo 설치하기

 

swiftcam.tistory.com/269

[PX4 개발자] Ubuntu 18.04에서 PX4 Firmware 컴파일

PX4 에 필요한 dependency를 gstreamer를 설치한다.

$ ./Tools/setup/ubuntu.sh --no-sim-tools --no-nuttx $ sudo apt install gstreamer1.0-plugins-bad gstreamer1.0-plugins-base gstreamer1.0-plugins-good gstreamer1.0-plugins-ugly libgstreamer-plugins-base1.0-dev

 

geographiclib 를 설치가 필요한데 위 설치과정을 따라했으면 이미 설치되어 있으므로 이 과정은 skip 할 수 있다. 하지만  geographiclib 없다면 아래와 같은 과정으로 설치하면 된다.

$ wget https://raw.githubusercontent.com/mavlink/mavros/master/mavros/scripts/install_geographiclib_datasets.sh $ chmod +x install_geographiclib_datasets.sh $ sudo ./install_geographiclib_datasets.sh

 

 

ROS에서 구동할 avoidance 모듈을 설치한다.

 

$ sudo apt install libpcl1 ros-melodic-octomap-*

$ cd ~/catkin_ws/src

$ git clone https://github.com/PX4/avoidance.git

 

 

cakin 툴로 빌드한다.

$ catkin build -w ~/catkin_ws

 

만약 release 모드로 빌드하고자 하면 아래와 같이 할 수 있다.

 

$ catkin build -w ~/catkin_ws --cmake-args -DCMAKE_BUILD_TYPE=Release

 

 

 

위와 같이 정상적으로 컴파일이 되었는지 확인한다.

 

catkin의 환경변수를 .bashrc에 등록해준다.

이유는 .bashrc는 terminal이 실행될 때마다 현재 사용자의 범위에서 환경설정 파일들을 실행해주는 파일이다. 여기 등록을 해주면 경로 및 환경 변수들을 일일이 입력해주지 않아도 터미널이 실행될 때 등록이 된다.

 

아래 명령어는 설치시 한번만 등록하면 된다.

 

$ echo "source ~/catkin_ws/devel/setup.bash" >> ~/.bashrc $ source ~/.bashrc

이렇게 등록해두면 다음부터는 위 명령어를 실행하지 않아도 터미널을 실행하면 바로 등록이 된다.

 

 

 

Gazebo가 정상적으로 동작하는지 확인하고 창을 닫는다. 그리고 아래과 같이 Gazebo 환경변수들을 등록하여 준다.

 

$ export QT_X11_NO_MITSHM=1 $ cd PX4-Autopilot $ make px4_sitl_default gazebo $ . ~/PX4-Autopilot/Tools/setup_gazebo.bash ~/PX4-Autopilot ~/PX4-Autopilot/build/px4_sitl_default

 

 

vim 에디터에서 /devel/setup.bash에 아래와 같이 추가한다.

$ . ~/PX4-Autopilot/Tools/setup_gazebo.bash ~/PX4-Autopilot ~/PX4-Autopilot/build/px4_sitl_default $ export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/PX4-Autopilot $ export GAZEBO_MODEL_PATH=${GAZEBO_MODEL_PATH}:~/catkin_ws/src/avoidance/avoidance/sim/models:~/catkin_ws/src/avoidance/avoidance/sim/worlds $ source ~/catkin_ws/devel/setup.bash

 

 

 

그리고 추가로 필요한 카메라 RSO 모듈들을 설치한다.

$ sudo apt install ros-melodic-stereo-image-proc ros-melodic-image-view

 

 

 

 

 

 

 

 

이제 PX4 Avoidance를 SITL 환경에서 시험할 준비를 마쳤다. 앞 과정은 설치과정이며 이후부터는 실행과정이다.

 

 local_planner 실행파일을 구동시킨다. 시뮬레이션에서는 스테레오 카메라도 모의하기 위해서 local_planner_stereo.launch 도 함께 실행시킨다.

 

 

클라우드 포인트의 depth 정보는 Block Matching Algorithm을 수행하고 그 결과로 얻을 수 있다.

roslaunch local_planner local_planner_stereo.launch

 

 

 

 

화면에서 보이는 붉은색 점들은 스테레오 카메라를 이용하여 두 카메라가 가지는 diparity를 검출한 결과이다.

 

스테레오 카메라의 영상과disparity는 아래 명령으로 볼 수 있다.

 

$ rosrun image_view stereo_view stereo:=/stereo image:=image_rect_color

 

 

 

 

qgroundcontrol 을 실행한다.

 

기능을 활성화하기 위해서 PX4 stack에서는 COM_OBS_AVOID 파라메터를 활성화해주어야 한다.

 

아래 설정에서 Safety로 가면 Object Detection 항목에서 활성화 가능하며 충돌 거리도 설정할 수 있다.

 

 

 

 

설정이 변경되면 아래와 같이 관련 파라메터들이 활성화되어 있고 설정된 충돌거리도 반영되어 있음을 확인할 수 있다.

 

 

 

그리고 SITL에서 Return시 높이의 디폴트 값이 30m인데 시뮬레이션에서는 공간이 크지 않으므로 5m이내로 제안하였다.

 

 

 

미션맵에서 waypoint를 아래와 같이 설정하였다.

 

전방에 나무가 있는 곳을 직진경로로 비행하고 다시 착륙지점으로 돌아오는 것이다.

 

Local Planner의 처리속도가 아직빠르지 않다고 생각하므로 속도는 3m/s로 설정하였고, 전방 오브젝트의 높이도 높지 않으므로 4m이내로 제한하였다.

 

 

 

qgroundcontrol에서 보이는 맵과 SITL이 수행되는 환경은 다르지만 Rviz에서 보이는 것 같이 전방에는 약 3m이상의 나무가 있으므로 이 위치를 지나가는 경우 local planner의 알고리즘을 확인한다.

 

waypoint 설정이 끝나면 오른쪽 상단의 Upload를 해주어야 미션에서 설정한 값이 PX4로 전달된다.

 

그리고 미션을 실행한다.

 

 

 

 

 

스테레오 비젼카메라와 SITL을 이용한 Obstacle avoidance의 시뮬레이션을 수행해보았다.

 

그 결과   Object와 vehicle의 거리는 최대한 좁혀져야 한다. 거리가 넓으면 나무사이를 지나가기 어렵고 둘러가게 된다. 또한 장애물이 없는 오픈된 공간에서도 waypoint를 정확하게 찾아내지 못하는 맹점이 있다.

waypoint를 찍으려고 계속 선회하면서 다음 waypoint로 진입을 하지 못하는 현상이 발생한다. 이 문제를 해결하기 위해서는 waypoint 진입시 근접반경을 넓혀서 원활하게 다음point로 진입할 수 있도록 하던지 아니면 알고리즘의 튜닝이 필요할 것 같다.

 

 

다음은 Depth 카메라를 이용하여 Object avoidance를 수행해보았다.

 

수행명령은 아래와 같다.

 

$ roslaunch local_planner local_planner_depth-camera.launch

 

 

Depth 카메라를 사용한 것 외에는 스테레오 카메라를 이용한 방법과 환경은 모두 동일하게 구성하였다.

 

 

 

 

Depth 카메라를 이용한 결과 처리속도 면에서나 waypoint 인식면에서나 거의 스테레오 비젼 카메라를 사용한 결과와 거의 동일한지만 Depth 카메라는 Vehicle과 Object 사이의 거리를 최소화했음에도 불구하고 나무사이를 통과하지는 못했다.

 

물론 파라메터를 수정하여 성능을 향상시킬 수도 있지만 현재 상태에서는 스테레오 카메라가 좀 더 최적화된 경로로 비행을 할 수 있다고 판단한다.

 

 

한가지 더 전방, 좌, 우 센서를 탑재하고 동일한 미션을 수행해보았다.

 

$ roslaunch local_planner local_planner_sitl_3cam.launch

 

 

 

3개의 Range Sensor를 사용한 결과는 처리속도는 가장 빠르다고 판단된다. 센서는 카메라보다 제한된 정보를 가지고 단순하게 신호처리가 가능하고, 그리고 나무사이를 통화하는 감도는 스테레오 카메라보다는 못하지만 양호한 편이다.