전동 스쿠터를 위한 DGPS 기반의 위치 추정 및 반 자율 주행 시스템 개발 Development of a DGPS-Based Localization and
Semi-Autonomous Path Following System for Electric Scooters
송 의 규, 김 병 국*
(Ui Kyu Song
1and Byung Kook Kim
1)
1
Korea Advanced Institute of Science and Technology
Abstract: More and more elderly and disabled people are using electric scooters instead of electric wheelchairs because of higher mobility. However, people with high levels of impairment or the elderly still have difficulties in driving the electric scooters safely.
Semi-autonomous electric scooter system is one of the solutions for the safety: Either manual driving or autonomous driving can be used selectively. In this paper, we implement a semi-autonomous electric scooter system with functions of localization and path following. In order to recognize the pose of electric scooter in outdoor environments, we design an outdoor localization system based on the extended Kalman filter using DGPS (Differential Global Positioning System) and wheel encoders. We added an accelerometer to make the localization system adaptable to road condition. Also we propose a path following algorithm using two arcs with current pose of the electric scooter and a given path in the map. Simulation results are described to show that the proposed algorithms provide the ability to drive an electric scooter semi-autonomously. Finally, we conduct outdoor experiments to reveal the practicality of the proposed system.
Keywords: electric scooter, outdoor localization, path following
I. 서론
로봇 기술의 발전은 노약자나 장애인의 삶의 질을 높이기 위한 방향으로도 적용되어 많은 연구가 진행되어 왔다. 특히, 노약자나 장애인이 이동 시에 주로 사용하는 전동 휠체어의 편의성 및 기능성 향상을 위한 시도가 많이 이루어져 왔다 [1]. 요즘에는 장거리를 이동하는 데 제약이 있는 기존의 전 동 휠체어를 대신하여 상대적으로 기동성이 좋은 전동 스쿠 터의 수요가 늘어나고 있는 실정이다. 그러나 최대 시속 12 km 정도로 속도가 비교적 빠른 전동 스쿠터는 신체적인 제 약이 있는 노약자나 장애인이 사용하기에 안전성의 확보 문 제가 있으며, 이를 위해서는 사용자에 의한 주행과 자율 주 행 사이에 선택이 가능한 반 자율 주행 방식의 전동 스쿠터 시스템을 개발하는 것이 하나의 해결책이다. 전동 스쿠터는 차륜 주행 방식의 전동 휠체어와는 달리 조향과 주행이 분리 된 차량형 이동 로봇의 기구학 모델을 따르며, 주행하게 되 는 범위가 주로 실외이고, 차량의 비교적 빠른 속도 등 많은 부분에서 전동 휠체어와 차이점이 있기 때문에 이를 고려한 연구가 필요하다.
전동 스쿠터를 자동화 하는 데 있어서 가장 우선적으로 연 구되어야 할 부분은 전동 스쿠터가 자신의 위치를 알도록 하
는 것이다. 자신의 위치를 모르는 불확실한 상태에서는 아무 것도 할 수 없으며, 자신의 위치를 알고 있어야 주어진 목표 점까지 가기 위한 경로를 계획하고 경로를 추종하는 등의 작 업이 가능하게 된다.
전동 스쿠터의 위치를 인식한 후에는, 사용자에 의한 주행 을 감시하여 안전 문제가 있을 경우, 자율적으로 주어진 경 로를 부드럽게 따라갈 수 있도록 하는 반 자율 주행을 위한 전동 스쿠터를 조작하는 차량 제어 알고리즘이 연구되어야 한다.
종합적으로 반 자율 주행이 가능한 전동 스쿠터 시스템의 개발을 위해서는, 경로 계획, 위치 및 방향 인식, 차량 제어, 장애물 회피 등의 많은 기술들이 통합되어야 하지만, 이들 중 본 논문에서는 위치 및 방향 인식과 경로 계획 및 추종을 위한 알고리즘에 초점을 두기로 한다.
로봇의 위치 추정 분야의 연구는 실내∙ 외 환경에서 이루 어지고 있다. 그 동안 가정 환경에서 사용할 수 있는 로봇에 대한 연구가 많이 진행되어 왔으며, 인간과 어울려서 로봇이 이동하기 위해서는 로봇의 자가 위치 인식이 필수적이다. 이 에 따라 위치 추정 분야의 연구는 실내 쪽에서 비교적 많이 이루어지고 있는 편이다.
실내에서의 위치 추정은 초음파 센서나 레이저 센서 등 많 은 센서 시스템을 유용하게 사용할 수 있으며, 주변 환경의 일정한 패턴을 인식하여 상대적인 위치 추정이 가능하다는 장점이 있다. 이와는 반대로 실외의 경우에는 사용할 수 있 는 센서의 범위가 제한적이며, 주변 환경이 불규칙하기 때문 에 실내와는 다른 어려운 점이 있다. 실외 환경에서는 다행 히도 GPS (Global Positioning System) 또는 DGPS (differential
Copyright© ICROS 2011
* 책임저자(Corresponding Author)
논문접수: 2010. 11. 16., 수정: 2011. 3. 14., 채택확정: 2011. 4. 26.
송의규: 한국과학기술연구원 미래융합기술연구본부 영상미디어센터 ([email protected])
김병국: KAIST 정보과학기술대학 전기및전자공학과 ([email protected])
※ 본 연구는 (고려대학교 로봇자율주행기술 전문인력양성센터를 통
한) 지식경제부/한국산업기술진흥원 융복합형로봇전문인력양성사
업의 지원으로 수행되었음.
GPS) 라는 지구 표면의 절대 좌표를 제공하는 센서가 개발되 어 있으며, 이를 활용한 위치 추정 연구가 많이 진행되어 왔 다. 로봇이나 차량의 위치 추정 연구에서 주로 DGPS와 다른 센서 데이터를 확장 칼만 필터(EKF: Extended Kalman Filter) [2] 를 사용해 융합함으로써 그 정확도를 높이는 방법이 널리 사용되고 있다.
R. Thrapp [3] 는 확장 칼만 필터를 사용하여 DGPS와 오도 메트리(odometry) 정보를 융합하여 실외 교내 주행 로봇의 위 치 추정에 사용하였으며, DGPS 센서에서 좌표 계산에 사용 한 위성의 갯수를 DGPS 정확도의 척도로 두고 측정 잡음의 공분산 행렬을 가변적으로 바꾸어 줌으로써 위치 정확도를 개선 하였다. 그러나 실제로 이 방식은 사용된 위성의 수가 6, 7 개 정도로 비교적 적은 경우에 정확도를 높이는 방식이며, 보통 하늘이 잘 보이는 공간에서는 10개 내외의 충분한 수의 위성이 잘 관찰되기 때문에 정확도 개선 효과가 미미하게 된 다. K. Ohno [4]의 연구에서도 확장 칼만 필터를 사용하여 DGPS 와 오도메트리 정보를 융합하였으며, 부정확하다고 생 각되는 DGPS 정보는 배제하는 방식으로 정확도 개선을 제 안하였다. 본 논문에서는 전동 스쿠터의 특성상 다양한 노면 에서 주행될 수 있다는 사실을 바탕으로 하여 가속도 센서로 노면의 상태를 측정하여 이를 위치 추정 시에 반영하므로써 실외에서 위치 추정 정확도를 향상시킬 수 있는 방안을 제시 한다.
전동 스쿠터의 반 자율 주행을 위해서는 추정된 위치 및 방향을 기반으로 하여 주어진 경로를 주행하는 경로 계획 및 추종 알고리즘이 필요하다. 차량형 로봇은 조향의 회전각에 의해 차체의 조향이 정해지기 때문에 조향 제어만으로 차량 의 제어가 가능하다[5]. 즉, 경로를 추종하기 위해서는 조향 을 자율적으로 제어해야 한다. 조향 제어 알고리즘은 pure pursuit 알고리즘[6]을 시작으로 많은 연구가 진행되어 왔다.
이 pure pursuit 알고리즘은 차량의 현재 위치(방향 제외)만을 기반으로 하여 경로 상에 있는 일정한 지점까지 가는 데 필 요한 조향각을 기하학적으로 풀게 된다. 이후에 pure pursuit 알고리즘을 변형하여 차량의 현재 위치뿐 아니라 방향까지 고려한 vector pursuit 알고리즘[7]도 제안되었다. 이러한 조향 제어 알고리즘 등은 최근 무인화 차량 대회 등에 많이 쓰이 고 있다. 그러나 이러한 알고리즘 등은 lookahead 거리가 적 절하게 선택되지 않게 되면 큰 추종 에러를 가지거나 (lookahead 거리가 긴 경우), 경로로 수렴하지 못하고 진동 (instablility) 하게 되는 경우가 발생할 수 있다(lookahead 거리 가 짧은 경우)[8].
본 논문에서는 상용의 전동 스쿠터에 반 자율 주행이 가능 하도록 조향 및 주행 기기부, DGPS 센서 등을 포함한 임베 디드 시스템을 추가하고, 전동 스쿠터의 보다 정확한 위치 추정 방식과 효율적인 경로의 계획 및 추종에 대하여 연구하 였다.
본 논문은 총 V 장으로 구성 되어 있다. 제 II 장에서는 자 율 주행이 가능하도록 개발된 전동 스쿠터를 소개하고, 제 III 장에서는 실외 환경에서 DGPS와 오도메트리 정보를 확 장 칼만 필터를 통해 위치 및 방향을 인식하고, 정확도 개선 을 위해 가속도 센서를 사용하는 방법을 제안한다. 또한 2개
의 원호를 이용해 직선과 원호로 모델링한 경로를 계획하고 추종하는 알고리즘을 제안한다. 제 IV 장에서는 시뮬레이션 을 통해서 제안한 시스템과 알고리즘의 유효성을 검증하고, 실제 제작한 전동 스쿠터에 적용하여 실험을 수행 하였다.
마지막으로 제 V 장에서는 연구에 대한 결론과 앞으로의 과 제에 대해서 기술하였다.
II. 설계 및 구현
이 장에서는 본 연구를 위해 설계 제작된 전동 스쿠터 시 스템의 하드웨어 부분을 기구부와 전자부로 나누어 소개하 고 소프트웨어 구조에 대한 내용을 기술한다.
1. 기구부
전동 스쿠터는 사용자 제어 모드와 자동 제어 모드로 선택 적으로 주행이 가능하도록 구현하였다.
주행 모터는 기존에 설치 되어 있는 모터 드라이버에 입력 되는 전압 레벨로 속력 및 전/후진이 바뀌기 때문에 제어용 보드에서 DAC를 통해 속도를 조절할 수 있도록 하였다.
MUX 를 이용해서 DAC의 출력과 속도 레버에 의한 전압 출 력을 선택적으로 하도록 함으로써 사용자 제어 모드, 자동 제어 모드 둘 다 가능하게 하였다. 주행 속도 측정을 위해서 모터 축에 그림 1(a)와 같이 엔코더(이하 주행 모터 엔코더) 를 설치하였다.
(a) An installed encoder for the driving motor.
(b) An encoder and a motor for the steering. (installed) 그림 1. 전동 스쿠터의 기구부.
Fig. 1. The mechanical part of the electric scooter.
조향 자동화를 위해 기계적으로 바퀴와 연결되어 있던 핸 들 조작부를 바퀴와 분리하고 바퀴 회전 축에 모터를 연결하 였다. 핸들 부분에는 그림 1(b)와 같이 엔코더(이하 조향 엔 코더)를 설치하여 그 회전각을 측정하고 이에 따라 모터를 회전하게 함으로써 사용자에 의한 조향 제어를 가능하게 하 였다. 이 때, 조향 모터에 부착된 엔코더(조향 모터 엔코더) 를 PID 각도 제어를 통해 사용자가 핸들 조작부를 회전한 각도 만큼 모터를 회전하여 바퀴를 회전하게 하도록 하였다.
자동 제어 모드에서는 제어용 보드를 통해서 직접 모터에 PWM 신호를 주어 조향을 제어한다.
2. 전자부
본 연구를 위해 그림 2와 같이 구축한 하드웨어에는 전동 스쿠터의 제어와 위치 추정 및 경로 추종 알고리즘 처리를 위한 임베디드 프로세서 보드(그림 3)가 있고, 실외에서 위치 좌표 측정을 위한 DGPS 모듈, 도로면 상태 감지를 위한 가 속도 센서가 있으며, 각 센서 데이터 처리를 위해 설계한 인 터페이스 보드(그림 5)가 포함되었다. 각 엔코더 신호를 카운 트 하기 위해 ALTERA사의 CPLD (EPM7128S)를 사용하였고, TTL 레벨의 DGPS 출력을 RS-232 출력으로 변환하기 위해 MAX232 칩을 사용하였다. 인터페이스 보드와 임베디드 보 드와의 통신에 있어서, DGPS 신호는 BTUART를 사용하였고, 가속도 센서 신호는 동기적 시리얼 포트를 이용하였으며, 그 리고 CPLD나 그 밖의 I/O를 위해서 general purpose I/O (GPIO) 를 사용하였다[9].
전동 스쿠터의 제어와 위치 추정 및 경로 추종 알고리즘 처리를 위해서 Intel PXA255 프로세서를 탑재한FA Linux사에 서 만든 그림 3과 같은 EZ-X5 보드를 사용하였다. PXA255는 인텔의 XScale architecture와 400MHz ARM 10 core를 사용하여 빠른 속도와 저전력으로 동작하기 때문에 한정된 자원의 배 터리로 구동 되는 전동 스쿠터의 제어용 보드로 적합하다.
실외 환경에서 절대 좌표 정보를 측정하기 위해서 사용한 DGPS 는 그림 4와 같이 MTK Chipset을 탑재한 FGPMMOPA6
모듈이다. 이 DGPS 모듈은 USB 또는 시리얼 포트를 통해 통신이 가능하며, 최대 업데이트 주파수는 5 Hz이다. DGPS 모듈이 위성 신호를 잘 받을 수 있도록 의자 등받이 상단에 설치하였다.
전동 스쿠터가 주행하고 있는 노면의 상태에 따라서 그 진 동 정도가 다르게 된다. 이러한 진동의 정도는 가속도 센서 로 측정이 가능하기 때문에 가속도 센서를 이용하면 노면의 종류를 파악할 수는 없더라도 적어도 노면이 얼마나 울퉁불 퉁 한지 정도는 감지할 수 있게 된다. 가속도 측정을 위해서 가속도 센서 모듈을 그림 5와 같이 인터페이스 보드에 설치 하였다. 인터페이스 보드는 지면과 평행하게 설치 되어 있으 며 노면에서 수직 방향의 진동을 측정하기 위해서 가속도 센 서로부터 z축 방향의 가속도 데이터를 수집하였다.
그림 3. 임베디드 프로세서 보드(EZ-X5).
Fig. 3. Embedded processor board (EZ-X5).
그림 4. 전동스쿠터 의자 상단에 설치한 DGPS 모듈.
Fig. 4. DGPS module mounted above the seat.
그림 5. 인터페이스 보드와 가속도 센서 측정 방향.
Fig. 5. Interface board and the axes of accelerometer.
그림 2. 전체 하드웨어 블록 다이어그램.
Fig. 2. Overall H/W block diagram.
3. 소프트웨어
전동 스쿠터의 정밀한 위치 추정을 위해서는 실시간성이 보장된 작업 수행이 되어야 한다. 임베디드 리눅스는 실시간 성을 보장하지 않기 때문에 본 연구에서는 임베디드 리눅스 에 RTAI (Real-Time Application Interface) [10]를 설치하여 사용 하였다. 센서 데이터 수집을 위해서 RTAI가 제공하는 실시간 장치 드라이버 중 시리얼 통신을 위한 SPDRV (Serial Port Driver) 를 사용하였다. 정확한 위치 추정을 위해서는 지정한 주기에 측정된 값이 위치 계산을 위해 지연 없이 전달되어야 한다. 전동 스쿠터의 속력이나 조향 각도 등은 10 ms 마다 값을 측정하고 있다. 그러나 이 값이 다른 원인으로 인하여 지연되게 되면 정확한 위치 추정이 되지 않는 문제가 생기게 된다. 일반 리눅스 시리얼 통신 드라이버와 같은 경우에는 블로킹 함수 또는 장치 드라이버에서 사용되는 다른 프로세 스들이나 다른 인터럽트 등에 의해서 지연이 발생할 수 있으 며, 이는 부정확한 위치 추정 결과를 초래 하게 된다. RTAI를 사용함으로써 정확히 10ms마다 측정 및 제어가 가능하게 되 었다. 또한 RT-FIFO를 사용하여 알고리즘이 수행되는 사용자 프로그램(application program)과 하드웨어 제어 및 통신을 위 한 모듈 프로그램(module program) 사이에 지연 없는 데이터 교환을 할 수 있도록 하였다.
전동 스쿠터의 제어와 위치 추정 및 경로 추종 알고리즘을 수행하기 위해서 그림 6과 같은 소프트웨어 구조를 구현하 였다. 모듈 프로그램에서 100 Hz OS Timer를 이용해 주기적으 로 EPLD의 엔코더 카운터 및 가속도 센서 데이터를 읽어오 고, 이를 RT-FIFO를 통해서 사용자 프로그램으로 넘겨준다.
사용자 프로그램에서는 pthread (POSIX thread)를 이용하여 RT-FIFO 에 값이 들어오는 대로 작업이 수행되기 때문에 100 Hz 로 동기화 된다. 이 쓰레드에서는 전동 스쿠터의 속력과 조향 각도를 이용해 전동 스쿠터의 위치 및 방향 예측(EKF prediction) 이 이루어진다. 매 초마다 시리얼 드라이버를 통해 전달되는 GPS 데이터도 RT-FIFO를 통해서 사용자 프로그램 쪽에 넘겨주게 되며 이를 받는 쓰레드는 1 Hz로 동기화 되어 GPS 데이터를 받는 대로 전동 스쿠터의 위치 및 방향을 구 하게 된다(EKF update). 또한 추정된 위치, 방향 정보를 토대 로 2개의 원호를 그려서 경로를 추종하는 경로 계획 알고리 즘이 수행된다.
그림 6. 전체 소프트웨어 블록 다이어그램.
Fig. 6. Overall S/W block diagram.
III. 위치 추정 및 경로 계획 추종
전동 스쿠터의 반 자율 주행을 위해서는 우선적으로 전동 스쿠터 자신의 위치를 파악하는 위치 추정 시스템이 필요하 다. 로봇 분야의 위치 추정 연구에 있어서 실외 환경에서는 실내와 달리 GPS (Global Positioning System)을 사용하여 현재 위치의 절대 좌표를 얻을 수 있다는 장점이 있다. GPS의 정 확도는 예전에 비해 많이 좋아졌지만 아직 수 미터 정도의 정확도만을 제공하기 때문에 GPS만으로는 도로 상에서 반 자율 주행을 하기에 충분하지 않다. 따라서 추가적인 센서 시스템을 필요로 하며 정확도 향상을 위한 별도의 위치 추정 알고리즘이 필요하다. 전동 스쿠터의 경우에는 일반 아스팔 트 도로, 인도, 자전거 도로 등 매우 다양한 도로를 시시각각 이동하며 운행될 수 있다. 이로 인해 도로 상황에 따라서 주 행/조향 엔코더 데이터 (오도메트리 데이터)의 특성이 달라지 게 된다. 예를 들면, 인도나 흙에서 주행할 때가 일반 아스팔 트 도로에서 주행할 때보다 부정확한 오도메트리 데이터가 나오게 된다. 이러한 도로의 특성을 파악하기 위해서 가속도 센서를 이용할 수 있다. 본 장에서는 실외 환경에서의 위치 추정을 위해서 확장 칼만 필터를 이용하여 DGPS 데이터와 오도메트리 데이터를 융합함으로써 전동 스쿠터의 보다 정 확한 위치 및 방향을 추정하고, 가속도 센서를 이용하여 도 로의 특성을 파악한 뒤 확장 칼만 필터의 노이즈 모델을 가 변적으로 바꿔줌으로써 도로 상황에 적응력 있는 위치 추정 을 할 수 있음을 보인다.
1. 전동 스쿠터의 기구학
전동 스쿠터는 차량형 이동 로봇의 기구학이 적용되며, 방 향 제어가 조향의 회전각에 의해 정해지기 때문에 차량의 주 행 속력에 독립적으로 차량의 제어가 가능하다는 장점이 있 다. 이러한 특성 때문에 이 연구에서의 ‘반 자율 주행’은 전 동 스쿠터의 자동 제어에 있어서 조향을 자동화 할 뿐, 주행 속력 조절은 사용자에 의한 것으로 간주한다.
전동 스쿠터의 자세(위치 및 방향) X 는 식 (1)과 같고, 전 동 스쿠터의 속력 ,
υ 조향각 ,s φ 앞 바퀴 축과 뒷 바퀴 축 s간 거리인 휠 베이스의 길이 L (그림 7 참조)을 포함한 기구 학은 식 (2)와 같다.
[
s s s]
TX
=x y θ (1)
cos cos cos sin
sin
s s s
s s s
s s
x y
L
υ φ θ
υ φ θ
θ υ φ
=
(2)
그림 7. 전동 스쿠터의 기구학 모델.
Fig. 7. Kinematic model of the electric scooter.
2. 위치 추정 시스템 2.1 확장 칼만 필터의 설계
설계한 확장 칼만 필터는 그림 8과 같은 구조를 갖는다.
이전까지 추정된 위치 및 방향을 기반으로 하여 조향 및 주 행 엔코더로 측정한 전동 스쿠터의 속력 및 조향 각도를 이 용해 제어 주기(10 ms)마다 차량형 기구학 모델을 통해서 전 동 스쿠터의 현재 위치 및 방향을 예측하게 된다. 1초의 주기 로 입력되는 DGPS 데이터를 이용하여 그 동안 예측한 위치 및 방향 정보를 업데이트함으로써 현재 전동 스쿠터가 있을 수 있는 가장 근사한 위치 및 방향 ˆ
X 을 계산하게 된다.확장 칼만 필터로 추정하고자 하는 것은 전동 스쿠터의 위 치 및 방향으로 이를 상태 벡터(state vector)로 정의하였다.
( ) [ ( ) ( ) ( )]
s s s TX k
=x k y k
θk (3)
센서 등을 이용해서 측정할 수 있는 데이터들은 확장 칼만 필터의 측정 벡터(measurement vector)로 정의할 수 있는데, 본 연구에서는 DGPS에서 출력되는 위도, 경도, 진행 방향 데이 터를 x-y 좌표계로 변환한 값을 측정 벡터로 정하였다.
( ) ( ( ), ) ( )
( ( ), ) [ ( ) ( ) ( )]
g g g TY k h X k k w k
where h X k k x k y k
θk
= +
= (4)
제어 입력은 전동 스쿠터의 속력
υ 와 조향각 s φ 로 하였다. s( ) [ ( ) ( )]
s s Tu k
=υ k φ k (5)
2.1.1 예측 단계(Prediction step)
확장 칼만 필터의 예측단계에서는 주어진 현재 상태 추정 (state estimate) ˆ X k k 로부터, 전동 스쿠터의 기구학적 모델 ( | ) 과 제어 입력을 이용하여 매 제어주기마다 식 6을 이용하여 상태를 예측(state prediction)한다.
ˆ ( 1| )
X k
+k = f X k k u k k ( ( | ), ( ), ) ˆ
( )cos ( )cos ( ) ( ( ), ( ), ) ( ) ( )cos ( )sin ( )
( )sin ( )
s s s
s s s s
s s
k k k
f X k u k k X k T k k k
k k L
υ φ θ
υ φ θ
υ φ
= +
(6)
여기서 f()는 상태 천이함수를 나타내며, 현재상태와 엔코더 입력 (속도
υs와 방향각
φ )로 나타내진다. 또한 상태의 정s확도를 나타내는 예측된 상태의 공분산 행렬(predicted covariance matrix) P k (
+1| ) k 도 식 (7)을 통해서 계산된다.
( 1| )
X( ) ( | ) ( )
X T u( ) ( ) ( )
u TP k + k = F k P k k F k + F k V k F k
1 0 ( )cos ( )sin ( ) ( ) 0 1 ( )cos ( )cos ( )
0 0 1
s s s s
X s s s s
where
T k k k
F k T k k k
υ φ θ
υ φ θ
−
=
(7)
cos ( )cos ( ) ( )sin ( )cos ( ) ( ) cos ( )sin ( ) ( )sin ( )sin ( )
sin ( ) ( )cos ( )
s s s s s s s
u s s s s s s s
s s s s s
T k k T k k k
F k T k k T k k k
T k L T k k L
φ θ υ φ θ
φ θ υ φ θ
φ υ φ
−
= −
이에 대한 계산에 있어서 제어 입력에 대한 오차 성분인 공정 잡음 벡터(process noise vector)가 필요하며, 이는 영분산 에 공분산 행렬이 V k 인 백색 가우시안 잡음(white Gaussian ( ) noise) 으로 가정한다(식 (8)).
2 2
( ) 0 0
s s
V k
υφ
σ σ
=
(8)
확장 칼만 필터의 정확도 향상을 위해서는 잡음 모델을 잘 세워주는 것이 중요하며, 본 연구에서는 가속도 센서를 이용 해서 잡음 모델을 가변 하는 방법을 제안하였다.
2.1.2 갱신 단계(Update step)
제어 주기마다 예측한 상태와 DGPS 데이터를 이용하여 최적의 상태 예측 ˆ ( X k + 1| k + 를 갱신한다. 이 때 DGPS 1) 로부터 측정하는 데이터는 x방향 위치, y방향 위치, 방향각이 며, 현재 및 과거 위치를 기반으로 방향각을 출력하는 기능 이 있다. 좌표계를 동일하게 하기 위해서 위, 경도 좌표계의 DGPS 데이터를 평면 좌표계인 UTM 좌표계로 변경하는 과 정이 필요하다. 우리나라의 경우에는 UTM Zone 51을 기준으 로 한다. 이렇게 예측한 값과 측정한 값을 이용해 최적의 값 을 도출해 내는 과정을 갱신 단계(update step)라 하며, 예측된 상태의 공분산 행렬 P k ( + 1| ) k 와 DGPS의 오차 모델인 측 정 잡음의 공분산 행렬 W k 를 이용하여 최종 상태의 공분 ( ) 산 행렬 P k ( + 1| k + 을 최소로 만드는 칼만 이득(Kalman 1) gain)
Rk를 계산하여 가장 근사한 전동 스쿠터의 위치 및 방 향을 계산하게 된다.
1
( 1| ) ( 1)
( 1) ( 1| ) ( 1) ( 1)
k T
T
R P k k H k
H k P k k H k W k
−= + +
+ + + + +
(9)
2 2
2
0 0
( ) 0 0
0 0
g g
g x
W k
yθ
σ σ
σ
=
(10)
ˆ ( 1| 1)
X k + k + = ˆ ( X k
+1| ) k + R Y k +
k[ ( 1) ( ( ˆ 1| ), 1)]
h X k k k
− + + (11)
ˆ ( 1| )
( 1| 1) ( 1| ) ( 1) ( 1| ) 1 0 0
( 1) 0 1 0
0 0 1
k
X X k k
P k k P k k R H k P k k where H k h
X
= ++ + = + − + +
∂
+ = ∂ =
(12) 그림 8. 위치 및 방향 추정 확장 칼만 필터 구조.
Fig. 8. EKF structure for pose estimation.
2.2 가속도 센서를 이용한 정확도 향상
가속도 센서를 이용해 측정한 z축 방향의 가속도를 이용해 서 도로 면의 상태를 파악할 수 있다. 그림 9에는 전동 스쿠 터에 설치한 가속도 센서에서 z축 방향의 가속도 값을 일정 수의 샘플에 대한 표준 편차를 표시한 것인데 보는 것처럼 도로의 상황에 따라서 그 특성이 다름을 알 수 있다. 일반 아스팔트 도로(asphalt road)나 자전거 도로 포장 재료로 쓰이 는 투수콘 재질의 도로(permeable asphalt road)에 비해 지면이 울퉁불퉁한 인도(sidewalk)에서 주행할 때 가속도 값의 표준 편차가 더 크게 나타난다.
가속도 센서로부터 측정된 가속도 값으로부터 다음과 같 이 공정 잡음 공분산 행렬을 가변적으로 바꾸어 줄 수 있다.
기본적으로 작은 진동은 존재하기 때문에 일정한 임계 가속 도를 정하고 기본 가속도(z 축의 경우에는 중력에 의해 +1 g 가 기본적으로 측정됨)와 측정된 가속도의 차이의 절대값이 임계 가속도보다 작을 경우에는 공정 잡음 공분산 행렬을 기 본 값으로 하고, 임계 가속도 이상일 경우에는, 초과치에 비 례하도록 이 행렬의 값을 가변해 준다. 이렇게 할 경우에 노 면의 상태에 따라 다른 엔코더 값의 정확도 차이를 위치 추 정 시에 반영할 수 있게 되어 정확도가 향상된 추정 결과를 얻을 수 있다. 실제 전동 스쿠터에서 식 (13)을 통해서 가변 을 해 주었으며, 비례 계수 κ 는 실험을 통해서 정해주어야 한다.
2 2 2
2
0 , 1 ( ) 0
0 , 1
0
s s
s s
V k
υ φ
υ φ
α
κα σ α
σ
σ α
σ
=
≥
= <
측정 가속도 - 기본 가속도 임계 가속도
(13)
3. 경로 계획 추종 알고리즘
전동 스쿠터가 주어진 경로를 따라가기 위해서는 전동 스 쿠터의 현재 위치를 기반으로 한 경로 계획 알고리즘이 필요 하다. 차량형 로봇은 주행 속력과는 독립적으로 조향 제어만
으로 차량의 제어가 가능하다는 특징이 있기 때문에 경로 계 획의 문제는 결국 조향 계획의 문제로 귀결된다.
일반적으로 도로는 선분과 원호의 조합으로 모델링이 가 능하다[11]. 각 구간의 시작점과 끝점, 원호 구간일 경우에는 추가적으로 원호를 이루는 원의 중심 정보를 이용하면, 현재 위치를 기반으로 경로를 추종할 수 있는 기하학적 풀이가 가 능하다. 전동 스쿠터는 경로에서 벗어난 위치에서는 2개의 원호를 그리면서 경로로 복귀하며, 경로 상에서 일정 거리 이상 벗어나기 전까지는 경로를 따라 직선 주행 혹은 곡선 주행을 하게 된다. 이 알고리즘은 알고리즘과 관련된 변수에 대한 설명을 표 1에 표시하였다.
직선 구간에서는 그림 10과 같이 모델링 된 경로의 시작점 과 끝점을 이용하여 경로를 직선의 방정식으로 나타내며, 정 해진 반경의 2개의 원을 그려서 두 번째 원이 경로와 접하도 록 하는 하나의 솔루션이 도출된다. 결과적으로 경로로 진입 하기 위해 회전해야 할 각도 2개를 구할 수 있다.
원호로 이루어진 구간에서도 마찬가지로 그림 11과 같이 2 개의 원을 그려 경로 상에 접하도록 하는 하나의 솔루션을 구할 수 있다.
실제로 경로 계획은 일정 주기로 수행된다. 다시 말하면 계획된 경로는 일정 주기로 수정되어 바뀐다는 것이다. 그러 나 전동 스쿠터가 이전에 계획 된 원호에 의해서 주행하다가
그림 9. 다양한 도로에서 측정된 가속도.
Fig. 9. The measured acceleration for various roads.
표 1. 변수 설명.
Table 1. Variables.
변수 설명
O
1첫 번째 원호의 중심 O
2두 번째 원호의 중심
O
3원호 경로의 중심
M 두 번째 원호의 시작점
P
ii 번째 경로의 시작점 R
d전동 스쿠터의 회전 반경
T 전동 스쿠터가 경로에 들어서는 좌표
그림 10. 직선 구간에서 2개의 원호를 이용한 경로 추종 해법.
Fig. 10. Solution for path following using 2 arcs at straight line path.
두 번째 원호 상에서 진입한 경우에 있어서는 2개의 원호를 그리는 경로 계획을 수행하지 않는다. 그 대신 경로에 접하 도록 하는 원의 반지름을 구하는 알고리즘이 수행된다.
그림 12에서 1), 2)의 위치에서는 2개의 원호를 그리는 경 로 계획이 이루어지지만 3)의 위치인 두 번째 원호로 진입한 시점에서는 2개의 원호를 구하는 것이 아닌, 경로 상에 접하 도록 하는 한 원의 반지름, 즉, 회전 반경을 구해야 한다. 회 전 반경은 식 (14)로부터 구할 수 있으며, 부호는 전동 스쿠 터의 경로에 대한 상대 위치에 의해 결정된다. 또한 회전 반
경은 아무리 작아도 전동 스쿠터의 최소 회전 반경보다 작을 수는 없다.
2 2
sin cos
s s
s s
Ax By C
r A B A θ B θ
+ +
= ± + + − (14)
원호 구간에서도 그림 13과 같이 경로에 접하도록 하는 원 의 반지름을 구할 수 있다.
{ }
2 2 2
3 3 3
3 2 3 3
( ) ( )
2 ( )sin ( )cos
s s c
s s s c
