Advanced
Tilt Angle Estimation of Plane with a Pair of Accelerometers and a Gyroscope
Tilt Angle Estimation of Plane with a Pair of Accelerometers and a Gyroscope
Transactions of the Korean Society for Noise and Vibration Engineering. 2013. Nov, 23(11): 966-972
Copyright © 2013, The Korean Society for Noise and Vibration Engineering
  • Received : September 03, 2013
  • Accepted : November 14, 2013
  • Published : November 20, 2013
Download
PDF
e-PUB
PubReader
PPT
Export by style
Article
Author
Metrics
Cited by
TagCloud
About the Authors
민 식 강
Corresponding Author ; Member, Department of Mechanical Engineering, Gachon University E-mail :mskang@gachon.ac.krTel : +82-31-750-5524, Fax : +82-31-750-5273

Abstract
Measurement or estimation of tilt angle is necessary for balancing robot such as Segway which is considered as a next generation transportation vehicle. However, it requires high-cost accurate sensors to hold balancing during stationary and moving situations. In this paper, a tilt angle estimation of a plane rotating in a vertical plane using low-cost sensors. Estimation using a set of 2-axis orthogonal accelerometers along with an inaccurate rate gyro has been considered. Feasibility and performance of the proposed technique has been verified through some experimental results.
Keywords
1. 서 론
최근 차세대 이동수단으로 Segway가 많은 주목을 받고 있다. Segway는 밸런싱 로봇(balancing robot) 의 한 종류로 진행 방향에 수직인 두 바퀴로 구성되어 있으며, 각 바퀴는 독립적인 전기 모터로 구동된다. Segway는 사용자의 질량을 포함한 전체 몸체 부분의 중력방향 기준 기울어진 각도와 각속도를 측정하여 몸체가 앞뒤로 넘어지지 않도록 균형을 유지하면서, 기울어진 방향으로 이동하도록 바퀴를 구동하는 개념을 갖는다 (1~3) . 따라서 몸체의 중력방향 기준 기울어진 각도와 각속도 측정이 필수적이다. 이를 위해 관성센서를 이용할 수 있지만 고가인 단점이 있으므로 상용 센서를 이용하는 방법들이 연구되어 왔다. 관성좌표계에서 경사각 측정을 위해 사용되는 대표적인 센서인 틸트센서는 기본적으로 진자운동을 이용하여 경사각을 측정하므로 저주파 대역에서의 반응성은 좋으나 반응속도가 느리고 고고주파 대역에서의 특성이 나쁜 단점을 가지며, 또한 경사각과 직선가속도에 의한 영향이 합해진 신호를 얻게 되므로 직선운동이 존재하는 경우 경사각 측정이 어려워진다. 특히 틸트센서는 구조적으로 뎀핑을 갖는 진자운동을 이용하여 경사각을 측정하므로 정밀한 동역학적 모델이 필요하다 (4) . 다른 예로는 가속도를 이용하는 방법으로, 중력방향과의 상대적인 각도에 따른 가속도를 측정하여 계산에 의해 경사각을 결정한다. 가속도계는 응답특성이 우수하여 상대적으로 변화가 빠른 운동을 측정할 수 있지만, 직선가속도가 존재하는 경우 경사각과 직선가속도에 의한 영향을 구분할 수 없는 단점을 갖는다. 자이로는 각속도 측정에 우수한 동적 특성을 갖지만 피할 수 없는 편류(drift)가 존재하여 적분에 의해 경사각을 측정하는 경우 시간이 경과할수록 경사각 측정오차가 커지는 문제점을 갖는다 (5~7) .
이러한 문제를 해결하기 위해 틸트센서와 자이로 센서, 또는 가속도 센서와 자이로 센서의 융합이 제시되고 있다 (8) . 융합된 센서의 신호 처리에는 상보필터(complementary filter)나 확장된 칼만필터를 사용하여 측정 신호의 잡음과 자이로 편류를 동시에 제거하는 방법이 사용되고 있다 (9~11) . 하지만 직선가속도가 존재하고, 이 정보가 주어지지 않을 경우 직선가속도에 의한 큰 경사각 추정오차가 존재하므로, 정밀한 추정을 위해서는 직선방향의 가속도 정보가 필요하다. 그러나 실제 평면의 직선가속도 측정을 위해서는 추가적인 가속도계가 필요하다.
이 연구에서는 Segway와 같이 이동방향의 가속도를 갖고 평면운동하는 로봇에서 두 개의 가속도계와 상용 자이로 신호를 융합하여 쉽고 정밀하게 경사각을 추정하는 방법을 제시하였으며, 그 적용성과 성능을 실험적으로 검증하였다.
2. 경사각 측정해석
- 2.1 가속도계를 이용한 경사각 측정
Fig. 1 은 평면운동하는 모바일 역진자 형태의 로봇으로, 아래 부분은 좌우로 구르는 바퀴이며, 몸체는 바퀴의 회전 중심부 o를 기준으로 회전이 가능하다. 바퀴가 좌우로 구르며 동시에 몸체가 회전하는 경우 몸체에 고정된 점 P 위치에서의 직선가속도를 해석한다. Fig. 1 에서 X-Z는 절대좌표계로 각각 수평축과 수직상방축이며, x-y는 몸체에 고정된 회전좌표계이다. 몸체의 절대좌표계 기준 회전각은 θ 이다.
PPT Slide
Lager Image
Schematic of mobile inverted pendulum
점 P의 위치벡터
PPT Slide
Lager Image
은 식 (1)과 같이 바퀴 회전 중심점의 절대 위치벡터
PPT Slide
Lager Image
과 x-y좌표계 기준 점 P의 위치벡터
PPT Slide
Lager Image
의 합으로 나타낼 수 있다. 즉,
PPT Slide
Lager Image
PPT Slide
Lager Image
여기서
PPT Slide
Lager Image
PPT Slide
Lager Image
는 각각 X-Z좌표 계와 x-y좌표계의 단위벡터이며, 두 단위벡터 사이의 관계는 다음과 같다.
PPT Slide
Lager Image
점 P의 가속도벡터는 식 (1)의 미분을 통해 구할 수 있으며, 다음과 같다.
PPT Slide
Lager Image
여기서
이다. 점 P는 몸체에 고정된 점이며, z-축 상에 있는 것으로 가정하면 즉,
PPT Slide
Lager Image
, 점 P의 X-축과 Z-축 방향 가속도 벡터 [ a X a Z ]는 식 (5)로 표현된다.
PPT Slide
Lager Image
식 (5)의 우변에서 각 항은 순서대로 바퀴 회전축의 직선가속도, 몸체의 각가속도에 의한 직선가속도와 몸체의 회전속도에 의한 구심가속도에 해당한다. 따라서 점 P가 바퀴회전축에 일치할 경우 즉, z=0이면 점 P의 직선가속도에 몸체 운동의 영향은 존재하지 않는다. 따라서 몸체의 직선가속도를 측정하여 몸체의 경사각을 추정할 경우 센서를 바퀴회전축상에 설치하는 것이 유리함을 알 수 있다.
직선운동이 없이 수직 평면에서 회전운동하는 몸체의 경사각은 한 개의 가속도계를 몸체의 회전축에 장착하여 측정할 수 있다. 그러나 이 경우 가속도계의 측정잡음에 의해 추정각의 바이어스 오차가 발생하며, 직교하는 가속도계를 사용하는 경우 이 바이어스 오차를 제거할 수 있다. 또한 직선가속도가 존재하는 경우 단일 가속도계로는 경사각을 측정할 수 없으므로, Fig. 2 와 같이 바퀴 회전축상에 가속도계 2개를 직교하도록 설치하여 측정한다.
PPT Slide
Lager Image
Accelerometers for tilt angle measurement
Fig. 2 에서 A와 B는 가속도계이며, 각 가속도계에서 측정되는 가속도를 각각 A와 B라 하면, 식 (6)의 측정값을 얻게 된다.
PPT Slide
Lager Image
여기서 g 는 중력가속도이다.
식 (6)에서 경사각은 다음의 관계를 갖는다.
PPT Slide
Lager Image
식(7)에서 수직가속도 a Z 는 알 수 없으므로 경사각을 결정할 수 없다. 그러나 Fig. 1 과 같은 모바일 로봇이 운동하는 바닥은 평평하여 수직방향 가속도가 중력가속도에 비해 작아 무시한다면 식 (7)는 다음으로 수정된다.
PPT Slide
Lager Image
하지만, 수평방향 가속도의 부호를 알 수가 없으므로 식 (7)와 같이 두 개의 추정치를 갖게 되며, 이 추정치는 모두 식 (6)를 만족한다.
모바일 역진자 로봇은 넘어지지 않도록 안정화 되어야 하며, 이를 위해 자이로 센서를 사용한다. 이 때 자이로는 고유의 편류(drift)가 존재하므로 이를 보정하여 사용해야 된다.
이 연구에서는 식 (8)에서 계산된 두 추정치에서 자이로 측정값을 이용하여 참값을 추정하고, 이를 이용하여 자이로 편류를 보상한 후 적분을 통해 경사각을 결정하는 방법을 제안코자 한다.
- 2.2 자이로와 가속도계를 이용한 경사각 측정
만일 자이로 신호에 편류가 존재하지 않으면 식 (8)에서 얻은 두 값의 미분을 구하고, 이 미분과 자이로 측정치와의 비교를 통해 참값을 쉽게 정할 수 있다. 그러나 자이로의 편류가 있으므로 이를 보정한 실제 각속도를 구하여 이를 이용해야 한다. 이를 위해 다음과 같이 자이로 편류와 경사각 추정을 동시에 수행할 수 있다.
PPT Slide
Lager Image
PPT Slide
Lager Image
식 (9)는 자이로 편류 보상이며, 블록선도로 나타 내면 Fig. 3 과 같다. Fig. 3 에서 점선 내부가 자이로 편류보상에 해당한다. b ( k )는 편류, ω ( k )는 실제각속도, ω m ( k )는 편류가 포함된 자이로 신호,
PPT Slide
Lager Image
는 편류 추정치,
PPT Slide
Lager Image
는 편류 보상된 각속도 추정치, θ i ( k )는
PPT Slide
Lager Image
)의 적분으로 구한 경사각,
PPT Slide
Lager Image
( k )는 식 (10)에서 구한 경사각 추정치,
PPT Slide
Lager Image
는 비례-적분기를 갖는 편류보상기, T 는 샘플링 간격이며, k 는 해당 신호를 시간 t = kT 에서 샘플링한 값을 의미한다.
PPT Slide
Lager Image
Gyro drift compensation
식 (10)은 식 (8)에서 구한 두 경사각 중 각속도 추정치를 이용하여 경사각을 결정하는 과정으로, 전스텝에서 추정한 경사각
PPT Slide
Lager Image
에 각속도 추정치
PPT Slide
Lager Image
를 이용하여 예측한 각도에 가장 가까운 경사각으로 결정하게 된다.
식 (9)와 Fig. 3 에서
PPT Slide
Lager Image
이므로, 이를 정리하면
PPT Slide
Lager Image
여기서
PPT Slide
Lager Image
,
PPT Slide
Lager Image
이며,
PPT Slide
Lager Image
는 편류를 보정한 자이로 신호의 적분 θ i ( k )와 실제 경사각의 차이이다.
식 (12)에서 편류보상기의 동특성은 제어 이득 K p K i 로 조정할 수 있다. 또한 식(10)에서 경사각의 최대오차는
PPT Slide
Lager Image
이므로, 식(9)의 편류추정 오차
PPT Slide
Lager Image
)는 항상 일정 범위 이내에 존재하게 된다. 또한 일정하거나 일정한 기울기로 단순 증가하는 편류는 정상상태에서 편류추정오차에 영향이 없으며, 일정크기의 경사각 추정오차 역시 정상 상태에서 편류추정오차에 영향이 없다.
또한 편류 보상된 자이로 신호의 적분과 실제 경사각 오차는 일정 크기의 편류에 영향을 받지 않으며,
PPT Slide
Lager Image
이므로 일정 범위 내에 존재하게 된다.
만일 직선가속도가 없을 경우에는 식 (10)에서 ψ ( k ) = 0이며, 따라서 일정크기의 편류가 존재하는 경우 편류보상오차와 경사각 추정오차는 정상상태에서 모두 제거된다.
3. 실험 및 결과
앞에서 제안한 편류보상 및 경사각 추정 방법의 타당성을 검증하기 위해 Fig. 4 와 같이 수평 평판에 엔코더가 내장된 모터를 장착하고, 모터축에 수직한 평면을 만든 다음 이 평면에 2-축 가속도계와 자이로를 설치하였다. 수평 평판은 수평방향 이동 기구인 리니어스테이지에 부착하였으며, 수평평판의 가속도는 가속도계를 이용하여 측정하였다.
PPT Slide
Lager Image
Experimental apparatus
실험에서는 수평평판을 리니어스테이지의 가이드를 따라 모터축의 수직방향으로 크기 약 ±0.3 g 범위 내에서 임의로 이동시키면서, 모터를 이용하여 센서장착부를 주파수 약 0.5 Hz, 크기 ±65˚ 범위 내의 정현파 형태로 회전시키면서 가속도계, 엔코더, 자이로 신호를 0.01초 간격으로 획득하였다.
Fig. 5 는 모터축 엔코더로 측정한 센서장착부의 절대 경사각으로 110초 동안 측정한 신호의 처음과 끝 20초 동안 신호이다. 모터는 위치제어를 하지 않고 회전시켰기 때문에 저주파의 굴곡이 존재한다. Fig. 6 은 수평평판에 장치된 가속도계로 측정한 평판의 직선 가속도로 평판을 임의로 움직였지만 약 0.9 Hz 정도의 주기를 갖고 있다. Fig. 7 8 은 각각 수직가속도계 A와 수평가속도계 B의 출력신호이다. 가속도계 A와 B의 장착 위치는 Fig. 2 와 일치한다. 따라서 수직가속도계의 경우 중력방향을 중심으로 좌우로 회전하게 되며, 결과적으로 수직가속도계에서 측정된 가속도 신호는 수평가속도계 신호에 비해 주파수가 2배가 된다. Fig. 9 는 자이로 신호로, 약 -0.4 deg./ s 의 편류가 존재한다. 따라서 자이로 신호를 적분하여 얻은 경사각은 시간이 지남에 따라 직선적으로 발산하게 된다.
PPT Slide
Lager Image
Tilt angle of sensor plate
PPT Slide
Lager Image
Linear acceleration
PPT Slide
Lager Image
Acceleration measured by acc. A
PPT Slide
Lager Image
Acceleration measured by acc. B
PPT Slide
Lager Image
Gyro rate
Fig. 10 은 앞에서 제안한 알고리즘을 적용하여 편류보상된 자이로 신호를 적분하여 구한 경사각과 엔코더로 측정한 실제 경사각을 보인다. 알고리즘 적용 초기의 천이구간에서 큰 오차가 존재하지만, 정상상태에 도달하면서 적분으로 구한 경사각이 엔코더 신호에 근접됨을 확인할 수 있다. Fig. 11 Fig. 10 에서 두 신호의 차이를 그린 것으로, 약 60초 이후에는 정상상태에 도달되어 그 오차가 ±0.5 deg. 이내로 수렴함을 확인할 수 있다.
PPT Slide
Lager Image
Encoder angle and estimated tilt angle
PPT Slide
Lager Image
Tilt angle estimation error
이상의 결과에서 이 연구에서 제안한 상용의 2-축 가속도계와 편류를 갖는 자이로를 이용하여 수평방향의 직선 가속도가 존재하는 경우 평면의 경사각을 추정하는 방법의 타당성을 실험적으로 검증하였다. 단, 추정각도는 천이 구간에서는 정밀도가 보장되지 못하므로, 정상상태 도달 이후 제어에 이용하는 것이 바람직하다.
4. 결 론
밸런싱 로봇의 제어를 위해서는 관성좌표계 기준 몸체의 경사각이 필요하며, 이를 위해 고가의 정밀한 가속도계와 자이로 센서가 이용되고 있다. 이 연구에서는 저가의 2-축 가속도계와 저가의 단일 자이로를 이용하여 평면의 경사각을 추정하는 방법을 제안하였다. 이 방법은 간단한 삼각함수 계산과 PI-제어 루프 계산을 통해 자이로의 편류 추정과 동시에 경사각을 추정하는 방법으로 구조적으로 단순한 장점을 갖는다.
실제 실험결과 분석을 통해 제안한 방법의 타당성과 성능을 검증하였다.
Recommended by Editor SungSoo Na
Acknowledgements
이 논문은 2013년도 정부(교육과학기술부)의 재원으로 한국연구재단의 기초연구사업의 지원을 받아 수행된 연구임(No. 2013R1A1A2008841).
BIO
Min Sig Kang received a B.S. degree in Department of Mechanical Engineering from Seoul National University in 1980. He then went on to receive M.S. and Ph.D. degrees from KAIST in 1983 and 1987, respectively. After receiving Ph.D., he worked for Agency for Defence Development during 1987~1998. Dr. Kang is currently a professor of the Department of Mechanical Engineering at Gachon University in Sungnam, Korea. His research interests include dynamic systems measurement and control, industrial robotics, and manufacturing systems.
References
http://www.degway.com/
Grasser F. , D'Arrigo A. , Colmbi S. , Rufer A. C. 2002 JOE: a Mobile, Inverted Pendulum IEEE Trans. on Industrial Electronics 49 (1) 107 - 114    DOI : 10.1109/41.982254
Lee H. J. , Jung S. 2011 Balancing Control of a Two Wheeled Mobile Robot System J. of Electrical Engineering 48 (6) 1 - 7
Leavitt J. , Sideris A. , Bobrow J. E. 2006 High Bandwidth Tilt Measurement Using Low-cost Sensors IEEE/ASME Trans. on Mechatronics 11 (3) 320 - 327    DOI : 10.1109/TMECH.2006.875571
Lee A. L. , Kim J. H. 2008 Development of 3-Dimensional Pose Estimation Algorithm using Inertial Sensors for Humanoid Robot Journal of Institute of Control, Robotics and Systems 14 (2) 1 - 8    DOI : 10.5302/J.ICROS.2008.14.2.133
Imaumra R. , Takei T. , Yuta S. 2008 Sensor Drift Compensation and Control of a Wheeled Inverted Pendulum Mobile Robot Advanced Motion Control, IEEE International Workshop on Motion Control 137 - 142
Hadri A. E. , Benallegue A. 2009 Sliding Mode Observer to Estimate Both the Attitude and the Gyro-bias by Using Low-cost Sensors The IEEE/RSJ International Conference on Intelligent Robots and Systems 2867 - 2872
Ha H. U. , Lee J. M. 2010 A Control of Mobile Inverted Pendulum using Single Accelerometer J. of Institute of Control, Robotics, and Systems 16 (5) 440 - 445    DOI : 10.5302/J.ICROS.2010.16.5.440
Lei W. , Shanmao G. , Shengwen Y. , Jinghuan F. 2010 Research on Novel High Frequency Signal Extraction Method Based on Extended Kalman Filter Theory Proc. of the 8th World Congress on Intelligent Control and Automation 2419 - 2423
Lee J. H. , Kim K. J. , Jang H. K. 2008 Estimation of Sensitivity Axis Offset of an Accelerometer for Accurate Measurement of the 6 DOF Human Head Motion Transactions of the Korean Society for Noise and Vibration Engineering 18 (9) 905 - 912    DOI : 10.5050/KSNVN.2008.18.9.905
Grewal M. S. , Andews A. P. 2008 Kalman Filtering: Theory and Practice Using Matlab 3rd Ed. John Wiely & Sons Inc. New York