DOI QR코드

DOI QR Code

Real-Time Acquisition Method of Posture Information of Arm with MEMS Sensor and Extended Kalman Filter

MEMS센서와 확장칼만필터를 적용한 팔의 자세정보 실시간 획득방법

  • Received : 2020.03.05
  • Accepted : 2020.06.08
  • Published : 2020.06.28

Abstract

In the future, robots and drones for the convenience of our lives in everyday life will increase. As a method for controlling this, a remote control or a human voice method is most commonly used. However, the remote control needs to be operated by a person and can not ignore ambient noise in the case of voice. In this paper, we propose an economical attitude information acquisition method to accurately acquire the posture information of the arm in real time under the assumption that the surround drones or robots can be controlled wirelessly with the posture information of the arm. For this purpose, the extended Kalman filter was used to eliminate the noise of the arm position information. in order to detect the arm movement, a low cost MEMS type sensor was applied to secure the economical efficiency of the apparatus. To increase the wear ability of the arm, We developed a compact and lightweight attitude information acquisition system by integrating all functions into one chip as much as possible. As a result, the real-time performance of 1 ms was secured and the extended Kalman filter was applied to acquire the accurate attitude information of the arm with noise removed and display the attitude information of the arm in real time. This provides a basis for generating commands using real-time attitude information of the arm.

미래에는 일상생활에서 우리 삶을 편의를 위한 로봇이나 드론들이 증가할 것이다. 그리고 이것을 제어하기 위한 방법으로 현재 리모컨이나 사람의 음성에 의한 방법이 가장 보편적으로 사용되고 있다. 하지만 리모컨은 사람이 찾아서 일일이 조작해야하며 음성의 경우에는 주변 노이즈를 무시할 수 없다. 그래서 본 논문에서는 팔의 자세정보만으로 무선으로 간편하게 주변 드론이나 로봇들을 제어 할 수 있다는 전제하에 실시간으로 정확하게 팔의 자세정보를 획득하기 위한 경제적인 자세정보 획득방법에 대하여 연구하였다. 이를 위해서 확장 칼만필터를 이용하여 팔의 자세정보에 대한 노이즈를 제거하였으며 팔의 움직임을 감지하기 위하여 저가의 MEMS 타입의 센서를 적용하여 장치의 경제성을 확보하였으며 팔의 착용성을 증대시키기 위하여 FPGA를 활용하여 최대한 칩 하나에 모든 기능을 집적화시켜 소형 경량의 자세정보 획득장치를 개발하였다. 그 결과 1 ms의 실시간성을 확보하였고 확장칼만필터를 적용하여 노이즈가 제거된 정확한 팔의 자세정보를 획득하고 실시간으로 팔의 자세정보를 전시하였다. 이를 통해서 팔의 실시간 자세정보를 이용하여 명령을 생성할 수 있는 기초가 마련되었다.

Keywords

I. 서론

1. 연구배경

미래에는 우리 삶의 편의를 위한 드론과 로봇들이 증가할 것이며 그에 따라 개인이 제어해야하는 전자장비들이 점점 많아질 것이라고 예상이 된다. 초소형 센서들과 임베디드 기술을 이용한 드론, 집안 일을 덜어주는 청소용 로봇, 전자장비 비중이 높아지고 있는 자동차 등이 대표적인 예라고 할 수 있겠다. 그리고 이것들을 제어하기 위한 방법은 전통적으로 직접 전자장비들을 조작하거나 리모컨을 이용하거나 사람의 음성으로 제어하는 방법들이 보편적으로 사용되고 있다. 하지만 리모컨의 경우 사람이 일일이 찾아서 조작해야 하는 번거러움이 있으며 사람의 음성의 경우에는 주변의 노이즈를 무시할 수 없다.

그래서 본 논문에서는 사람의 팔의 자세정보를 이용하여 무선으로 명령을 내릴 수 있는 방안을 고려하였고 이를 위한 기초연구로 사람의 정확한 팔의 자세정보 획득을 위한 확장칼만필터 적용, 팔의 착용성을 위한 소형 경량화, 자세정보 획득장치의 저가화에 초점을 두고 연구를 진행하였다.

칼만필터는 잡음이 포함되어 있는 선형 역학계의 상태를 추적하는 재귀필터로 루돌프 칼만이 개발하였다. 칼만필터는 컴퓨터 비젼, 로봇공학, 레이더 등의 여러분야에 사용되며, 많은 경우에 매우 효율적인 이 알고리즘은 시간에 따라 진행한 측정을 기반으로 한다. 에러에 대한 상태변수를 추정하고 추정한 것에 대하여 보정하는 것을 반복적으로 수행한다[1].

해당 순간에만 측정한 결과만 사용한 것보다 좀 더 정확한 결과를 기대할 수 있다.

본 논문에서는 비선형 시스템을 해결하기 위한 칼만필터의 또 다른 버전인 확장 칼만필터를 적용하여 자세정보 정밀도를 높였다.

소형, 경량화를 위해서 FPGA(Field ProgrammableGate Array)기반의 SoC(System on Chip)설계와 나노 기술을 이용해 제작되는 매우 작은 기계인 MEMS(Micro Electro Mechanical System)를 적용하였다. 여기서 FPGA는 설계 가능 논리 소자와 프로그래밍 가능 내부선이 포함된 반도체 소자이며 SoC는 집적회로에 집적된 컴퓨터나 전자 시스템 부품을 말한다.

저가화를 위해서 IMU(Inertial MeasurementUnit)처럼 높은 센서 데이터의 정밀도를 가지는 적용하는 대신 저가의 MEMS타입의 센서를 적용하여 자세정보 획득장치를 구현하였다. MEMS타입의 센서의 낮은 정밀도를 극복하기 위해서 센서융합과 필터를 적용하였다.

2. 논문구성

본 논문의 구성은 다음과 같이 총 3장으로 구성이 되어 있다. 본론 1 장에서는 팔의 자세정보 획득장치 개발을 위한 자이로스코프, 가속도계, FPGA로 구성된 초소형 임베디드 시스템 설계 구성을 소개한다. 본론 2장에서는 팔의 자세정보 획득을 위하여 확장칼만필터를 적용하여 팔의 자세정보를 실시간으로 획득하는 내용을 소개한다. 본론 3 장에서는 팔의 자세정보 획득장치로부터 수신한 데이터를 GUI를 통해서 전시하고 실시간으로 명령을 내리는 내용을 기술한다. 끝으로 결론에서는 본 논문을 통해서 적용한 내용을 정리하고 향후 개선사항과 진행계획에 대하여 언급하며 끝을 맺는다.

II. 본론

1. 팔의 자세정보 획득을 위한 임베디드 시스템

1.1 MEMS타입의 센서

본 논문에서 적용할 MEMS타입의 3축 가속도계는Analog device사의 제품으로서 파트넘버는 ADXL375이다.

3축에 대하여 ±200 G까지 측정이 가능하며 Bandwidth 1 kHz이며 SPI(Serial PeripheralInterface Bus)통신으로 3축에 대한 가속도계 값을 제공한다[2].

[그림 1]은 중력에 대하여 가속도계의 위치에 따라서 출력되는 가속도계 출력을 200초 동안의 출력한 것이다. 그림에서 보는 바와 같이 Z축에 대하여 중력가속도가 작용하고 있으며 나머지 X, Y축은 Z축에 대하여 직교하므로 가속도 값이 0이 나오는 것을 확인 할 수 있다.

CCTHCV_2020_v20n6_99_f0001.png 이미지

그림 1. Accelerometer output by gravity force

본 논문에서 적용할 MEMS타입의 2축 자이로스코프(각속도계)는 Analog device사의 제품으로서 파트넘버는 ADXRS290이다. 2축에 대하여 ±100 deg/sec까지 측정이 가능하며 최대 Bandwidth 4.25 kHz이며 SPI통신으로 2축에 대한 각속도 값을 제공한다. 또한 자이로스코프 안에 컷 오프 주파수를 설정할 수 있는 로우패스 필터와 하이패스 필터가 내장되어 있어서 시스템의 특성에 맞게 조정가능하다[3].

자이로스코프 패키지 내부에 온도센서가 있어서 자이로스코프의 온도를 측정할 수 있다. 일반적으로 자이로스코프의 출력에 대한 에러는 자이로스코프의 내부적인 요인과 외부적인 요인으로 나눌 수 있다. 내부적인 요인으로는 제작 시 발생하는 공정 오차에 따른 비대칭 형상에 의해 원하는 동작 이외의 진동을 일으키는 에러가 있을 수 있다.

외부적인 요인으로는 2가지가 있는데 첫 번째는 온도에 의한 요인이다. 온도는 공진주파수와 Q-Factor 그리고 출력 값에 영향을 미치며 센서의 패키지 역시 주변 온도에 의해서 영향을 받는다. 두 번째는 가속도에 의한 영향이다. 선형 가속에 의한 출력 값의 왜곡으로서 일반적으로 크게 발생하지 않는다[4].

두 번째는 가속도에 의한 영향이다. 선형 가속에 의한 출력 값의 왜곡으로서 일반적으로 크게 발생하지 않는다[5].

본 논문에서 사용된 자이로스코프의 경우 약 0.3deg/sec 정도 발생한다고 데이터시트에 명시되어 있으므로 무시할 수 있는 값이다. 하지만 온도에 의한 영향은 무시할 수 없으므로 자이로스코프에서 출력되는 온도 값을 읽어서 소프트웨어에서 오프셋 값을 조절하였다.

1.2 임베디드 하드웨어구성

사람의 팔의 자세정보 획득을 위한 임베디드 시스템의 하드웨어 구성은 팔의 장착성과 움직임을 고려할 때 초소형, 경량이어야 한다. 이를 위해서 저전력을 요구하는 프로세서와 주변장치들로 구성을 해야 한다. 이를 위해서 FPGA를 설계에 적용하였다. FPGA의 경우 발명된 지 30년이 조금 안되지만 다양한 방면에서 영향력이 커지고 있으며 점점 더 속도 또한 빨라지고 있다. 그 이유는 여러 가지가 있겠지만 타고난 합성능력과 상대적으로 저렴한 개발비용이 큰 이유이다[6]. 프로세서는 FPGA에서 합성해주는 Microblaze를 활용하였다.

본 논문에서 사용할 XILINX사의 FPGA인 ARTIX 7 모델의 경우 16나노 공정으로 제작되었으며 가로, 세로 15 mm, 15 mm 크기를 가지며 이 공간 안에 로직 뿐만 아니라 Soft Core 형태의 프로세서인 Microblaze를 제공한다.

Microblaze는 부동 소수점 연산이 가능하며 동작 주파수, 로컬메모리, 캐쉬 등을 설계자가 선택할 수 있다. 또한 리소스가 허용된다면 Microblaze를 몇 개 더 만들어 하나의 FPGA안에서 멀티코어 형태로 사용할 수 있다. 이 FPGA의 전원 공급하는 스위칭 레귤레이터 역시 크기가 소형이며 3 mm, 5 mm 크기를 갖는다.

FPGA 설계는 XILINX사에서 제공하는 VIVADO라는 설계 툴을 이용하여 설계하는데 최적의 로직 시뮬레이션 환경을 제공하며 타겟 보드에 프로그램을 장입한 후 실시간으로 내부의 모든 신호들을 모니터링할 수 있는 개발 환경을 제공한다.

[그림 2]는 팔의 자세정보 획득을 위한 임베디드 시스템을 구성하고 있는 회로카드와 하우징을 나타내고 있다. FPGA 회로카드에는 FPGA, 3축 가속도계, 2축Gyro 센서가 장착되어 있으며 전원공급 회로카드에는 센서와 FPGA에 전원을 공급하기 위한 스위칭 레귤레이터가 실장되어 있다. 모기판은 위 두장의 회로카드를 연결하며 2축 Gyro센서가 장착되어 있다.

CCTHCV_2020_v20n6_99_f0002.png 이미지

그림 2. Embedded system acquiring arm movementInformation

[그림 3]은 위 회로카드들이 결합된 모습을 나타내고 있으며 X, Y, Z 3축 방향에 대하여 가속도계가 센싱할 수 있으며 Pitch, Yaw, Roll 방향에 대하여 각속도를 센싱할 수 있게 된다.

CCTHCV_2020_v20n6_99_f0003.png 이미지

그림 3. Combined circuit card

[그림 4]은 위 회로카드가 하우징과 결합되고 다시 팔에 장착된 자세정보 획득장치를 보여준다.

CCTHCV_2020_v20n6_99_f0004.png 이미지

그림 4. 자세정보 획득장치

[그림 5]의 임베디드 시스템인 자세정보 획득장치는 장착된 각 센서의 레지스터 셋팅하고 SDLC통신을 이용하여 실시간으로 센서 정보를 운용 컴퓨터에게 전달한다. 그리고 운용 컴퓨터는 확장칼만필터 연산을 수행하고 자세정보 분류 결과와 함께 명령을 생성한다.

CCTHCV_2020_v20n6_99_f0005.png 이미지

그림 5. 하우징에 결합된 자세정보 획득장치

1.3 FPGA설계

본 논문에서 FPGA는 다음과 같은 기능을 수행한다.

첫째, Softcore형태의 Microblaze라는 프로세서를 FPGA안에서 동작시켜서 3축 가속도계와 2개의 2축 자이로의 레지스터들을 원하는 값으로 셋팅한다.

둘째, SPI통신으로 센서들의 데이터를 실시간으로100 Hz이상의 bandwidth를 가지고 수신한다.

셋째, 수신한 데이터를 실시간으로SDLC(Synchrous Data Link Controller)통신으로 메인컴퓨터 GUI(Graphic User Interface)로 10Mbps이상으로 송신한다. SDLC는 1970년대 중반에 IBM사에서 개발했으며 데이터 링크 프로토콜의 하나이며 SNA에 채용된 동기식 데이터 전송 제어 프로토콜로 국제 표준 규격이며, HDLC의 원형이다.

위 3가지 역할을 하기 위해는 다음 그림과 같이 FPGA를 설계하였다.

[그림 6]에서 보는 바와 같이 Microblaze는 SoC 설계에서 각종 IP들끼리 연결에 최적화된 AMBA(Advanced Micro controller BusArchitecture)를 이용하여 센서 인터페이스 로직과 통신 로직과 연결된다. AMBA는 영국의 ARM 사가 개발한 시스템 온 칩의 버스 표준이다.

CCTHCV_2020_v20n6_99_f0006.png 이미지

그림 6. FPGA 내부 설계 구성

각각의 로직들의 구성은 다음과 같다.

1. 자이로스코프 컨트롤러 로직모듈

2. 가속도계 컨트롤러 로직모듈

3. 타이머 카운터 로직모듈

4. SDLC통신모듈

1.4 Microblaze 설계

FPGA 내부의 Microblaze라는 소프트 코어는FPGA의 리소스 중에 BRAM(Block RAM)을 이용하여 만들어지며 32비트 RISC(Reduced Instruction SetComputer)구조의 프로세서이다.

Microblaze 프로세서는 70가지의 옵션에 따라서 프로세서의 성능을 선택할 수 있다. 예를 들어 Instruction Cache, Data Cache, Floating PointUnit, Memory Management 등을 선택할 수 있다. 개발자의 선택에 따라서 작은 state machine부터 Linux기반의 고성능의 컴퓨터까지 선택할 수 있는 것이다. 물론 고성능의 컴퓨터로 Microblaze를 설정할수록 FPGA내부의 BRAM은 소모되고 로직에서 사용할 메모리가 줄어들게 된다[7]. ARM 코어나 TI의 DSP 정도의 연산처리 속도는 내지는 못하지만 1.44DMIPS/MHz 정도의 연산량을 갖는다. DMIPS는 1984년 Reinhold P. Weicker에 의해서 개발된 컴퓨터 벤치마크 프로그램으로서 처음에는 Interger 프로그래밍에만 사용되었다가 지금은 일반적인 프로세서의 성능을 나타내는 데 사용된다.

이렇게 구성된 Microblaze를 이용하여 이클립스 기반의 SDK(Software Developement Kit)로 C언어를 이용하여 소프트웨어를 설계하였다. 이클립스는 다양한 플랫폼에서 쓸 수 있으며 자바를 비롯한 다양한 언어를 지원하는 프로그래밍 통합 개발 환경으로 시작하였으나, 현재는 범용 응용 소프트웨어 플랫폼으로 진화하였다. [그림 7]은 이클립스 기반의 SDK의 개발환경을 나타낸다.

CCTHCV_2020_v20n6_99_f0007.png 이미지

그림 7. 이클립스 기반의 SDK 개발환경

[그림 7]에서 보는 바와 같이 SDK 개발 환경으로 모든 전역변수, 지역변수를 실시간으로 확인 할 수 있으며 브레이크 포인트를 찍어가면서 디버깅을 할 수 있다. 또한 스택과 힙의 크기를 조절할 수 있다.

2. 확장 칼만필터를 적용하여 실시간 센서 데이터 획득

2.1 팔의 자세정보 표현

이동체의 위치, 속도 및 자세를 표현하기 위해서 관성항법시스템에 사용되는 좌표계(Frame orCoordiante system)는 몇 가지 종류로 나뉘게 되며, 적절한 좌표계의 선정은 관성항법식의 간략화 및 계산량의 감소, 목표로 하는 항법시스템의 요구조건에 따라서 설계자가 임의로 선정하게 된다[8].

본 논문에서는 팔의 자세정보를 오일러 각으로 표현하여 나타내기로 한다. 오일러 각은 강체의 자세를 3차원 공간좌표계의 회전으로 표현하는 각도 표현법이다.

팔의 자세정보를 오일러 각으로 표현함으로서 특정 오일러 각에 대하여 특정 명령을 주변 전자장비에 내릴 수 있다. 팔의 좌표축은 항공기의 좌표축을 따르도록 한다.

항공기의 동체 축은 x,y,z로 표현하는데 x축은 항공기의 무게중심을 기준으로 길이 방향의 동체 축을 의미하고, y축은 x축과 수직이며 오른쪽 날개로 정의되고 z축은 x, y축과 직교를 이룬다[9].

마찬가지로 사람의 전방을 x축, 오른쪽을 y축, 아래를 z축으로 가정한다. 그리고 이에 따른 팔에 장착된 가속도 센서의 좌표축도 다음 [그림 8]과 같이 항공기 좌표축을 따른다. 그리고 팔에 장착된 자이로의 각속도 p는 roll 방향의 각속도를 표현하고 q는 pitch 방향의 각속도를 표현하고 r은 yaw 방향의 각속도를 표현한다.

CCTHCV_2020_v20n6_99_f0008.png 이미지

그림 8. 팔의 좌표 축

팔의 좌표 축에 따른 각속도 변화는 항공기 축에 대한 각속도를 따르도록 할 때 𝜙(파이)는X축 방향에 대한 회전을 나타내며 𝛳(세타)는 Y축 방향에 대한 회전을 나타내며, 𝜓(프사이)는 Z축 방향에 대한 회전을 나타낸다.

여기서 팔에 장착되어 있는 자이로 값, 즉 각속도 값을 적분하여 오일러 각을 구할 수 는 없다. 자이로는 오일러각의 변화율이 아니라 팔의 각속도 p, q, r을 측정한 것이기 때문이다. 그래서 다음과 같이 널리 알려져 있는 오일러 각과 동체 각속도의 관계식을 이용하여 오일러 각의 변화율을 구한 뒤 적분을 해서 팔의 오일러 각을 구한다. 이 값이 팔의 자세정보이다

\(\left[\begin{array}{c} \dot{\phi} \\ \dot{\theta} \\ \dot{\psi} \end{array}\right]=\left[\begin{array}{ccc} 1 & \sin \phi \tan \theta & \cos \phi \tan \theta \\ 0 & \cos \phi & -\sin \phi \\ 0 & \sin \phi \sec \theta & \cos \phi \sec \theta \end{array}\right] \cdot\left[\begin{array}{l} p \\ q \\ r \end{array}\right]\)       (1)

하지만 수식 (1)은 오일러 각이 행렬 안에 있으므로 다음과 같이 수식 (2)처럼 선형 시스템이 아니다.

\(X_{t}=A_{t} X_{t-1}+B_{t} u_{t}+\epsilon_{t}\)       (2)

그러므로 선형시스템에만 사용되는 칼만필터를 적용할 수 없다. 하지만 오일러 각을 쿼터니언으로 표현하면 다음과 같이 선형 시스템을 만들 수 있다.

쿼터니언은 3개의 벡터 요소와 하나의 스칼라 요소로 구성된다. q = {η,ε} = {η,εx, εy, εz} 여기서 η은 쿼터니언의 스칼라 성분이고, ε = (εx, εy, εz)는 쿼터니언 벡터 성분이다.

쿼터니언으로 상태 변수는 다음 수식 (3)과 같게 된다.q1은 스칼라 성분이고 나머지는 벡터성분이 된다.

\(X=\left[\begin{array}{c} q_{1} \\ q_{2} \\ q_{3} \\ q_{4} \end{array}\right]\)       (3)

쿼터니언과 각속도와의 관계식을 통하여 수식 (4)와 같은 선형 시스템을 얻을 수 있다.

\(\left[\begin{array}{l} q_{1} \\ q_{2} \\ q_{3} \\ q_{4} \end{array}\right]=I+\Delta t / 2\left[\begin{array}{cccc} 0 & -p & -q & r \\ p & 0 & r & -q \\ q & -r & 0 & p \\ r & q & -p & 0 \end{array}\right]\left[\begin{array}{l} q_{1} \\ q_{2} \\ q_{3} \\ q_{4} \end{array}\right]\)       (4)

하지만 이렇게 칼만 필터를 적용할 때 4 by 4 행렬의 곱셈과 역행렬을 실시간으로 계산해야 하므로 연산량이 많아지게 된다. 그러므로 파이와 세타 성분의 자세정보만을 고려하고 실시간으로 획득함으로서 팔의 자세정보를 얻기 위해서 비선형 시스템에 적용할 수 있는 확장 칼만필터를 적용하도록 한다.

CCTHCV_2020_v20n6_99_f0009.png 이미지

그림 9. 쿼터니언 표현

2.2 자이로스코프와 가속도계 융합

움직이는 속도가 충분히 느리거나 속도의 크기와 방향이 빠르게 변하지 않는 경우에는 아래 수식 (5), (6)을 이용하여 수평자세를 구할 수 있다.

\(\phi=\sin ^{-1}\left(\frac{-f_{y}}{g \cos \theta}\right)\)       (5)

\(\theta=\sin ^{-1}\left(\frac{f_{x}}{g}\right)\)       (6)

위 수식 (5), (6)에서 g는 중력 가속도이고 ⨍x y는 가속도계로 측정한 X축, Y축의 가속도계 성분이다. 이 식만 이용하여 팔의 자세정보를 바로 구할 수 있으면 좋겠지만 가속도계는 오차가 너무 큰 단점이 있다. 또한 빠른 속도로 회전하거나 속도가 변하면 오차는 더욱 커진다. 자이로를 이용한 자세정보는 아래 식처럼 측정 각속도계 값(p, q, r)을 이용하여 \(\dot{\phi}\), \(\dot{\theta}\) , \(\dot{\psi}\)를 구한 뒤 적분하여 𝜙(파이), 𝛳(세타), 𝜓(프사이)를 구할 수 있다. 하지만 자이로의 측정값에는 오차가 섞여 있기 때문에\(\dot{\phi}\), \(\dot{\theta}\), \(\dot{\psi}\)​를 적분하면 오차가 계속 누적된다.

그러므로 본 논문에서는 자이로 센서와 가속도계 센서를 [그림 10]처럼 융합하여 확장 칼만필터를 적용하여 자세 정보를 실시간으로 획득하고자 한다.

CCTHCV_2020_v20n6_99_f0010.png 이미지

그림 10. 자이로와 가속도계 센서의 융합

칼만필터를 적용한 센서융합은 센서들의 잡음을 확률변수로 취급하여 상태측정오차의 기댓값이 최소가 되도록 상태변수를 측정하고 시간에 따른 잡음변화를 적절하게 예측함으로서 더욱 정밀한 위치측정을 가능하게 한다[10].

2.3 확장칼만필터 적용

필터를 이용하는 것은 공학이나 임베디드 시스템의 여러 분야에서 매우 유용하다. 공학적인 관점에서 칼만필터는 신호를 예측하는 툴이라 할 수 있으며 수학적 관점에서 선형 또는 비선형시스템에서 선형 시스템으로 모델링하여 근사화한 후 상태를 예측한다고 할 수 있다. 과거 많은 연구에 의한 결과로서 칼만필터가 좋은 결과를 보여주었을 뿐 아니라 이론적으로도 많은 여러 필터들 중에서 예측오차가 가장 작은 필터에 속한다.

다음 수식 (7-1, 7-2) 일반적인 선형시스템의 상태변화모델과 측정 모델이다.

\(X_{t}=A_{t} X_{t-1}+B_{t} u_{t}+\epsilon_{t}\)       (7-1)

\(Z_{t}=C_{t} X_{t}+\delta_{t}\)       (7-2)

다음 수식 (8)은 위 수식(7)에 대한 칼만필터 알고리듬이다.

\(\text { Kalman Filter }\left(\mu_{t-1}, \Sigma_{t-1}, u_{t}, z_{t}\right)\)

\(\overline{\mu_{t}}=A_{t} \mu_{t}+B_{t} u_{t}\)       (8-1)

\(\Sigma_{t}=A_{t} \Sigma_{t-1} A_{t}^{T}+R_{t}\)       (8-2)

\(K_{t}=\overline{\Sigma_{t}} C_{t}^{T}\left(C_{t} \overline{\Sigma_{t}} C_{t}^{T}+Q_{t}\right)^{-1}\)       (8-3)

\(\mu_{t}=\overline{\mu_{t}}+K_{t}\left(z_{t}-C_{t} \overline{\mu_{t}}\right)\)       (8-4)

\(\Sigma_{t}=\left(I-K_{t} C_{t}\right) \bar{\Sigma}_{t}\)       (8-5)

\(\text { Return } \mu_{t} \Sigma_{t}\)

(8-1), (8-2)번 식은 예측하는 부분이고 (8-3)번 식은 Kalman Gain을 나타낸다. (8-4), (8-5)번 식은 측정한 값을 업데이트 하는 부분이다. 각 파라메터에 대하여 간략하게 설명하면 다음과 같다.

𝜇t는 평균 값, ∑t는 분산 값, 𝜇t는 제어 값, Zt는 측정 값, Kt는 칼만 게인을 나타낸다. 𝜀t와 𝛿t와 평균이 0이고 분산이 각각 Rt, Qt인 독립 가우시안 백색잡음이다. 칼만필터는 t-1 순간까지의 관측값에 근거하여 X(t|t-1)을 추정하며 알고리듬은 반복형 식으로 표현된다[11][12].

Rt는 시스템에 대한 노이즈의 공분산 값이고 Qt는센서 측정 값에 대한 노이즈의 공분산 값이다.

칼만필터를 이용하여 신호를 예측하기 위해서는 선형 시스템이어야 한다. 그러나 비선형 시스템의 경우에는 칼만필터를 적용할 수 없다. 대신 비선형 학습방법을 적용해야 한다. 비선형 시스템을 특징점을 중심으로 선형화하고 선형 최적 설정(Linear OptimalEstimation)을 적용하는데 확장칼만필터 알고리듬이 가장 적합하다[13].

그러므로 본 논문에서는 비선형시스템을 적용해야함으로 확장 칼만필터를 적용한다.

확장칼만필터는 비선형 시스템 모델을 테일러 시리즈 전개를 이용하여 1차항으로 선형화하여 고차 항을 생략하는 선형화 과정을 거치는 방법이다[14].

확장 칼만필터는 다음 수식(9-1,9-2)과 같은 상태변화모델과 측정모델을 갖는다.

\(X_{t}=g\left(u_{t} X_{t-1}\right)+\epsilon_{t}\)       (9-1)

\(Z_{t}=h\left(X_{t}\right)+\delta_{t}\)       (9-2)

여기서 Xt는 마르코프 프로세스(Markov Process)를 따르는 상태 벡터이다. 마르코프 프로세스는 상태벡터가 결정될 확률이 과거의 다른 상태 벡터들과 무관하고 그 직전의 결과인 Xt-1에 따라 결정된다는 가정이다.

다음 수식(10)은 위 상태방정식 (9)에 대한 확장 칼만필터 알고리듬이다[15].

\(\text { Extended Kalman Filter }\left(\mu_{t-1}, \Sigma_{t-1}, u_{t}, z_{t}\right)\)

\(\overline{\mu_{t}}=g\left(u_{t}, \mu_{t-1}\right)\)       (10-1)

\(\overline{\Sigma_{t}}=G_{t} \Sigma_{t-1} G_{t}^{T}+R_{t}\)       (10-2)

\(K_{t}=\overline{\Sigma_{t}} H_{t}^{T}\left(H_{t} \overline{\Sigma_{t}} H_{t}^{T}+Q_{t}\right)^{-1}\)       (10-3)

\(\mu_{t}=\overline{\mu_{t}}+K_{t}\left(z_{t}-h\left(\overline{\mu_{t}}\right)\right)\)       (10-4)

\(\Sigma_{t}=\left(I-K_{t} H_{t}\right) \overline{\Sigma_{t}}\)       (10-5)

\(\text { Return } \mu_{t} \Sigma_{t}\)

마찬가지로 (10-1), (10-2)번 식은 예측하는 부분이고 (10-3)번 식은 Kalman Gain을 나타낸다. (10-4), (10-5)번 식은 측정한 값을 업데이트 하는 부분이다.

우리가 사는 세상은 비선형적인 시스템이 거의 대부분이므로 실제로 확장 칼만필터가 칼만필터보다 더 많이 사용되고 있다. 위 필터가 비선형 시스템에 적용할 수 있는 이유는 바로 테일러 시리즈와 자코비안을 확장칼만필터에 적용하였기 때문이다. 테일러 시리즈는 비선형 시스템을 선형화시켜주며 자코비안은 복잡하게 얽혀 있는 식을 미분을 이용하여 선형 근사화시켜 간단한 근사 선형식을 만들어 준다[16].

확장 칼만필터는 비선형 시스템에 적용할 수 있으므로 쿼터니언 좌표계를 사용할 필요 없이 오일러 각을 이용한다.

그러므로 팔의 자세 정보 획득을 위한 상태변수 x는다음 수식 (11)과 같이 X축 방향에 대한 회전 각 𝜙(파이), Y축 방향에 대한 회전 각 𝛳(세타), Z축 방향에 대한 회전 각 𝜓(프사이)으로 구성이 된다.

\(x=\left[\begin{array}{l} \phi \\ \theta \\ \psi \end{array}\right]\)       (11)

위에서 소개한 첫 번째 식 (10-1)에 각속도와 오일러각의 관계식을 적용할 수 있다. 하지만 비선형식이다.

확장칼만 필터의 예측하는 첫 번째 식 (10-1)은 제어하는 부분은 없으므로 ut는 삭제하고 다음 수식 (12)와같이 나타낼 수 있다.

\(\overline{\mu_{t}}-g\left(\mu_{t-1}\right)\)       (12)

위 (12)식을 팔의 자세를 나타내는 오일러 각으로 풀수 있다. 여기서 우리는 팔의 𝜙(파이)와 𝛳(세타)각도만을 고려하기 때문에 𝜓(프사이)는 사용하지 않는다.

𝜙(파이)와 𝛳(세타) 각도는\(\dot{\phi}\)(파이)와 \(\dot{\theta}\)(세타)를 적분함으로서 구할 수 있다. 즉 위 식(12)을 적분하면 다음 수식(13)과 같이 팔의 위치 정보를 나타내는 상태모델(state model)이 된다.

<팔의 자세정보 상태 모델>

\(\dot{\phi}=p+q \times \sin (\phi) \times \tan (\theta)+r \times \cos (\phi) \times(\theta)\)       (13-1)

\(\dot{\theta}=q \times \cos (\phi)-r \times \sin (\phi)\)       (13-2)

\(\dot{\psi}=q \times \sin (\phi) \times \sec (\theta)+r \times \cos (\phi) \times \sec (\theta)\)       (13-3)

아래 (14-1, 14-2, 14-3)식은 위식 (13)을 적분한 식이다.

\(\phi_{t}=\phi_{t-1}+\dot{\phi} \times d t\)       (14-1)

\(\theta_{t}=\theta_{t-1}+\dot{\theta} \times d t\)       (14-2)

\(\psi_{t}=\psi_{t-1}+\dot{\psi} \times d t\)       (14-3)

위에 (14-1, 14-2, 14-3)식이 \(\overline{\mu_{t}}\) = g(𝜇 - 1)가 되는 것이다.

dt는 미소시간을 나타낸다. 여기서는 0.001초로 프로그래밍 하였다. 또한 측정 모델은 다음과 같다.

<팔의 자세정보 측정 모델>

\(Z_{t}=\left[\begin{array}{lll} 1 & 0 & 0 \\ 0 & 1 & 0 \end{array}\right]\left[\begin{array}{l} \phi \\ \theta \\ \psi \end{array}\right]\)       (15)

위 (15)식은 선형이므로 자코비안을 사용할 필요 없이 바로 다음 수식 (16)과 같이 측정모델로 나타낼 수 있으며

\(Z_{t}=H \cdot\left[\begin{array}{l} \phi \\ \theta \\ \psi \end{array}\right]\)       (16)

H는 아래 수식 (17)과 같이 표현 할 수 있다.

\(H-\left[\begin{array}{lll} 1 & 0 & 0 \\ 0 & 1 & 0 \end{array}\right]\)        (17)

미분을 통하여 시스템을 선형 근사화 시키기 위해서 자코비안을 이용하는데 자코비안은 다음 수식 (18)과같이 구할 수 있다.

\(G_{t}=\left[\begin{array}{cc} \frac{\sigma f_{1}}{\sigma \phi} & \frac{\sigma f_{1}}{\sigma \theta} \\ \frac{\sigma f_{2}}{\sigma \phi} & \frac{\sigma f_{2}}{\sigma \theta} \end{array}\right]\)       (18)

이렇게 구한 자코비안을 이용하여 확장 칼만필터의 예측단계 아래 두 번째 수식인 (10-2)식을 사용하여 예측 공분산값 \(\overline{\Sigma_{t}}\)을 구한다.

칼만 이득인 Kt는 (10-3)식을 사용하여 측정 모델, 예측 공분산, 센서 측정 노이즈의 공분산값을 이용하여구한다.

센서 측정 값을 업데이트 하는 수식 (10-4), (10-5)번식은 3축 가속도계에 획득한 데이터 𝜙(파이)와 𝛳(세타)를 입력한다. Rt는 시험을 통해서 0.005를 선정했으며 Qt는 자이로스코프 데이터 시트에 명시된 노이즈 값을 참조하여 0.007569값을 적용하였다. Rt에 따라서 시스템이 달라지는데 Rt값이 클수록 예측값의 반영 비율이 높아지고 변화가 완만해진다. 또한 측정값의 영향을 덜 받게 되고 칼만 이득은 작아지게 된다.

[그림 11]은 위 내용을 실제로 구현한 내용을 간단히 플로우 차트로 그려본 것이다.

CCTHCV_2020_v20n6_99_f0011.png 이미지

그림 11. 확장칼만필터를 적용한 프로그램 Flow Chart

[그림 11]을 살펴보면 확장칼만필터 알고리듬은 START에서 시작하여 최종적으로 𝜇t, ∑t을 구한뒤 다시 이 값을 𝜇t – 1, ∑t - 1으로 입력하여 반복적으로 수행하는 것을 볼 수 있다. 또한 수행하는 과정에서 최신화 된 센서 값 자이로와 가속도계 값을 업데이트하는 것을 확인할 수 있으며 자코비안 값도 계속해서 업데이트를 수행한다.

3. 실험 및 결과

3.1 실시간 데이터 전시

SDLC통신을 통하여 실시간으로 2축 자이로 센서 2개, 가속도계 센서 그리고 팔의 자세정보인 파이, 세타값을 전시하기 위한 실시간 GUI(Graphic UserInterface) 설계하였다. 실시간 시스템은 정해진 제한된 응답시간을 만족하거나 실패를 포함한 위험한 결과를 발생하지 않는 시스템을 말한다[17].

이를 위해서 시스템의 응답시간을 1 ms로 정하였고 이를 만족하는 시스템을 설계하였다.

GUI는 Visual Studio 2013을 이용하여 설계하였다. C#언어를 이용하여 Visual Studio에서 제공하는 윈폼 개발환경을 사용하였다. C#은 C++ 연산력과 Visual basic의 편의성을 결합하기 위한 목적으로 Microsoft사에서 개발한 객체지향 프로그래밍 언어이다. C++ 기반이며, 다중상속과 포인터 사용, 런 타임시에 가비지 메모리 관리 등에서 자바랑 비슷한 특성을 가지고 있다[18].

API 및 각종 라이브러리를 담고 있는 프레임워크는 닷넷 프레임워크 4.0을 사용하였다.

다음 [그림 12]는 GUI를 통해 3축 가속도계 센서신호를 1 ms 주기로 수신하고 2개의 2축 자이로 신호에 대하여 역시 1 ms 단위로 실시간으로 수신하고 있는 결과이다. 실시간 처리를 위해서는 수 개의 비동기적인 쓰레드들이 병렬적으로 수행해야 한다[19]. 윈폼 개발환경은 여러개의 다중 쓰레드들을 운영할 수 있는 환경을 제공한다.

CCTHCV_2020_v20n6_99_f0012.png 이미지

그림 12. 자이로, 가속도계 데이터 실시간 수신 결과

SDLC통신을 수신하기 위해서 쓰레드를 생성하여 별도로 구동하였으며 시스템 타이머를 이용하여 센서 데이터들을 1 ms 단위로 업데이트하여 실시간으로 센서값들을 전시하였다.

Visual Studio 2015부터는 WPF(WindowPresentaion Foundation)을 이용하여 GUI 응용프로그램을 개발한다. XAML(eXtensible ApplicationMarkup Language)과 C#을 동시에 사용하여 개발하며 시각적인 측면에서 뛰어나다.

다음 [그림 13]은 손으로 가볍게 흔들었을 때 2축 자이로스코프 데이터를 실시간으로 200초 동안 전시하고 있는 것이다.

CCTHCV_2020_v20n6_99_f0013.png 이미지

그림 13. 2축 자이로스코프 값 실시간 전시화면

팔에는 2축 자이로스코프가 2개 장착되어 있으므로 총 4개의 축에 대한 각속도 변화를 실시간으로 볼 필요가 있다. 그래서 1 ms 단위로 각속도 변화를 전시하게 설계하였다. 아래 그림에서 위 2개 차트는 1번 자이로스코프의 X축, Y축에 대한 각속도 방향을 실시간으로 200초 동안 전시하고 있으며 아래 [그림 14]의 아래 2개 차트는 2번 자이로스코프의 Y축, Z축에 대한 각속도 방향을 실시간으로 200초 동안 전시하고 있다. Y축은 중복으로 겹치므로 1개만 선택하여 확장 칼만필터에 적용하였다.

CCTHCV_2020_v20n6_99_f0014.png 이미지

그림 14. 2개의 2축 자이로스코프의 가속도 성분 실시간 전시화면

3.2 결과

자세정보 획득장치에서 보내는 센서 데이터를 GUI컴퓨터는 1 Mbps 속도로 수신 받는다. 그리고 수신 받은 신호를 실시간으로 확장 칼만필터 처리하고 𝜙(파이), 𝛳(세타)방향에 대한 자세정보를 구하고 이 정보를 다시 확장 칼만필터에 입력한다. 그리고 그 결과를 GUI에 전시한다. 이 과정이 1 ms 이내에 이루어진다.

측정값에 확률적인 오차가 포함되고 특정시점에서의 상태가 이전 시점의 상태와 선형적인 관계가 있는 경우 칼만필터를 효과적으로 적용할 수 있다. 본 논문에서 사용된 자이로 센서의 경우 비선형성이 ±0.5%이며 노이즈는 0.004 °/s/√Hz 이며 가속도 센서의 경우 비선형성이 ±0.25%이며 노이즈는 5mg/√Hz이다. 다음 그림에서 보는 바와 같이 센서의 비선형성과 노이즈가 개선되었으며 아래 [그림 15-17]에 표현되었다.

[그림 15]은 팔의 움직임에 따른 𝜙(파이), 𝛳(세타)방향에 대한 자세정보를 실시간으로 200초 동안 전시하고 있는 화면이다.

CCTHCV_2020_v20n6_99_f0015.png 이미지

그림 15. 실시간으로 팔의 자세정보를 전시

위에 [그림 15] 중 𝜙(파이)에 해당하는 부분만 확대하면 [그림 16]과 같다. 아래 [그림 16]에서 점선은 확장 칼만필터를 통과하기 전의 값이고 실선은 확장 칼만필터를 통과한 후의 값이다. 보는 바와 같이 확장 칼만필터 통과로 노이즈와 비선형성이 개선된 것을 확인할 수 있다.

CCTHCV_2020_v20n6_99_f0016.png 이미지

그림 16. 실시간으로 팔의 𝜙(파이)방향 자세정보 전시

또한 위에 [그림 15] 중 𝛳(세타)에 해당하는 부분만 확대하면 다음 [그림 17]과 같다. 마찬가지로 아래 [그림 17]에서 점선은 확장 칼만필터를 통과하기 전의 값이고 실선은 확장 칼만필터를 통과한 후의 값이다. 보는 바와 같이 확장 칼만필터 통과로 노이즈와 비선형성이 개선된 것을 확인할 수 있다.

CCTHCV_2020_v20n6_99_f0017.png 이미지

그림 17. 실시간으로 팔의 𝛳(세타)방향 자세정보 전시

다음 [그림 18]은 칼만 이득을 나타낸다.

CCTHCV_2020_v20n6_99_f0018.png 이미지

그림 18. 팔이 정지했을 때의 칼만 이득

팔이 움직이지 않을 때의 칼만 이득 값은 0.32, 0, 0,0.1 이다. 측정모델을 다음과 같이 설정하였기 때문에 예상대로 측정 모델의 1행 1열과 2행 2열 값만 계속 변하는 결과가 나왔다.

\(H=\left[\begin{array}{ll} 1 & 0 \\ 0 & 1 \end{array}\right]\)       (19)

이번에는 칼만 이득의 모든 행과 열에 대하여 변하는 것을 확인하였다. 1행 2열과 2행 1열 값이 0이 아니라 조금씩 변한 이유는 아래 식에서 보는 바와 같이 Qt(센서의 노이즈의 공분산) 값이 변하였기 때문이다[20].

\(K_{t}=\overline{\Sigma_{t}} H_{t}^{T}\left(H_{t} \overline{\Sigma_{t}} H_{t}^{T}+Q_{t}\right)^{-1}\)       (20)

CCTHCV_2020_v20n6_99_f0019.png 이미지

그림 19. 팔이 움직였을 때의 칼만 이득

III. 결론

본 논문에서는 팔의 자세정보만으로 무선으로 간편하게 주변 드론이나 로봇들을 제어 할 수 있다는 전제하에 실시간으로 정확하게 팔의 자세정보를 획득하기 위한 경제적인 자세정보 획득방법에 대하여 연구하였다. 이를 위해서 전통적으로 로봇 팔에 적용되는 모터, 리졸버, 포텐쇼미터를 고려해 보았지만 로봇 팔처럼 사람 팔 내부에 시스템을 장착해야 한다는 한계에 부딪혔다.

추가적으로 요즘 한창 연구가 진행 중인 딥러닝 기술을 적용하여 팔의 자세나 동작을 학습하여 분류를 통해서 명령을 얻는 방법[21]도 고려해 보았지만 거대한 연산 파워를 가지는 GPU를 구동시킴으로서 발생하는 휴대성과 실시간성이 떨어진다는 한계가 존재하였다.

또한 요즘 게임에 많이 적용 중인 키넥트 센서를 이용하여 팔의 자세정보를 얻는 방법을 고려하였지만 구비해야 하는 하드웨어가 늘어나서 착용성에 문제가 있었으며 정확도와 실시간성에도 한계가 존재하였다. 그래서 본 논문에서는 MEMS 타입의 센서와 확장칼만틸터 그리고 초소형 실시간 임베디드 시스템을 적용하여 착용성, 경제성, 실시간성, 정확성을 한층 끌어올렸다.

팔에 착용할 수 있도록 FPGA와 MEMS타입의 센서를 활용한 100g 이내의 초소형 임베디드 시스템을 적용하여 착용성을 증대시켰으며 고가의 IMU대신 저가의 1만원 이하의 MEMS 센서를 적용하여 경제적인 자세정보 획득장치를 개발하였다. [그림 15-17]에서 보는 바와 같이 확장칼만필터를 적용하여 자이로 센서와 가속도센서가 갖고 있는 비선형성과 노이즈로 인해서 발생하는 𝜙(파이), 𝛳(세타)방향 팔의자세정보 오차를 최대 ±10 Deg 오차를 줄였으으며 또한 FPGA를 활용하여 1ms 이내의 실시간성을 갖는 자세정보 획득장치를 개발하였다.

아쉬운 점이 있다면 연산량이 많은 확장칼만필터 연산 수행을 임베디드 자세정보 시스템에서 직접 연산을 못한 것이 아쉽다. FPGA안에 Microblaze의 연산능력이 확장칼만필터의 많은 곱하기 연산을 처리하기에는 부족한 점이 있었다.

향후에는 ARM코어와 FPGA 합성된 ZYNQ라는 초소형 디바이스를 이용하여 본 논문의 Microblaze에서 연산량 부족으로 수행하지 못했던 확장칼만필터 알고리듬의 모든 상태 변수를 듀얼 ARM코어에서 수행하여 팔의 파이, 세타 각도 뿐만 아니라 프사이값도 실시간으로 획득할 예정이다.

References

  1. S. H. Choi and C. G. Park, "A Fusion Structure of TRN and IBN with Adaptive Logic Based on Altitude Gradient," Institute of Control, Robotics and Systems, pp.1458-1462, 2019.
  2. ANALOG DEVICE, ADXL375 Datasheet, 2014.
  3. ANALOG DEVICE, ADXRS290 Datasheet, 2014.
  4. C. H. Kim and J. M. Chung, "Design of a Low Noise 6-Axis Inertial Sensor IC for Mobile Devices," The KICS, Vol.40, No.02, pp.397-407, 2015. https://doi.org/10.7840/kics.2015.40.2.397
  5. M. Ali, "Compensation of Temperature and Acceleration effects on MEMS Gyroscope," IEEE IBCAST, Vol.16, pp.274-279, 2016.
  6. V. Sklyarov and Iouliia Skliarova, Synthesis and Optimization of FPGA-Based Systems, heidelberg: Springer, 2013.
  7. XILINX, Microblaze Processor Refernece Guide, 2010.
  8. Y. H. Kim, A Study on Characteristics of UAV-Grade AHRS that Uses MEMS Sensors, Master's Thesis, Inha University, p.68, 2005.
  9. David G. Hull, Fundamentals of Airplane Flight Mechanics, Verlag Berlin Heidelberg: Springer, 2007.
  10. J. M. Kim, Y. T. Kim, and S. S. Kim, "Indoor Localization for Mobile Robot using Extended Kalman Filter," Institute of Intelligent Systems, Vol.18, No.05, pp.706-711, 2008. https://doi.org/10.5391/JKIIS.2008.18.5.706
  11. G. Chen, Approximate Kalman Filtering, World Scientific Publishing Co., 1993.
  12. B. D. O. Anderson and J. B. Moore, Optimal Filtering, Englewood Cliffs, NJ: Prentice-Hall, 1979.
  13. E. T. Kim, "A Study on On-line modeling of Fuzzy System via Extended Kalman Filter," The EEK,, Vol.40, No.05, pp.10-18, 2003.
  14. W. S. Lee and J. H. Lim, "Unscented Kalman Filter Based Outdoor Localization of a Mobile Robot," The KSPE, Vol.36, No.02, pp.183-190, 2019. https://doi.org/10.7736/KSPE.2019.36.2.183
  15. Thrun Sebastian, Wolfram Burgard and Dieter Fox, Probabilistic Robotics, Cambridge Messachusetts: MIT-Press, 2005.
  16. J. H. Park and G. W. Kim, "Nonlinear Observer and Simplified Equivalent Circuit Model-based EKF-SOC Estimator," The ECCE, pp.1467-1472, 2019.
  17. Phillip A. Laplante and Seppo J. Ovaska, Real-Time Systems Design and Analysis, New Jersey: Wiley, 2012.
  18. Faraz Rasheed, Programmer's Heaven C# School, Fuengirola: Synchron Data, 2006.
  19. M. Keck, L. Galup, and C. Stauffer, "Real-time tracking of low-resolution vehicles for wide-area persistent surveillance," IEEE WACV, pp.441-448, 2013.
  20. W. S. Choi, Real Time System Applied Extended Kalman Filter for Acquiring Position Information of Combat Hat, Master's Thesis, Yonsei University, p.68, 2017.
  21. J. S. Chair, Hand Gestures For Drone Control Using Deep Learning, Master's Thesis, Oklahoma University, Norman, USA, 2018.