108 로봇학회논문지제 6 권제 호 (011. 6) 등반능력향상을위한이륜역진자로봇의최적 ARS 제어 Optimal ARS Control of an Inverted Pendulum Robot for Climbing Ability Improvement 권영국 1, 이장명 Young-Kuk Kwon 1, Jang-Myung Lee Abtract Thi paper propoe an optimal ARS control of a two-wheel mobile inverted pendulum robot. Conventional reearche are highly concentrated on the robut control of a mobile inverted pendulum on the flat ground, i.e., motly focu on the compenation of gyrocope ignal. Thi newly propoed algorithm deal with a climbing control of a lanted urface baed on the dynamic modeling uing the conventional tructure. During the climbing control of the robot, unexpected diturbance force are eentially caued by the irregular contact force which come from the irregular contact angle between the wheel and the terrain. The diturbance have effect on the optimal poture of the mobile robot to compenate the lanted angle. Therefore the dynamic equation through phyical interpretation are derived for the election of optimum climbing poture through ARS. Alo uing the ultraonic enor the lope information i obtained to compenate for the force of gravity. The control input are dynamically adjuted to climb up the lanted urface effectively. The propoed algorithm i demontrated through the real experiment. Keyword: Two - Wheel Inverted Pendulum Robot, Optimal Poture Control, Climbing Capability, SEGWAY, ARS (Attitude Reference Sytem) 1. 서론 1) 대기오염및화석연료고갈, 유가급등의심각성이대두되는가운데무공해이동수단인세그웨이 [1-3] 가개발되었다. 세그웨이는최신기술들이집약된스쿠터형식의운송수단으로서 001년 Dean Kamen에의해개발된이후로차세대이동수단으로큰반향을일으켰다. 이러한세그웨이는역진자로봇의일환으로동적안정화기술이라는무게균형의원리가적용되었다 [4-6]. 동적안정화기술은사람이균형을잡는원리와비슷하다. 사람이균형을잃고앞쪽으로몸을쓰러뜨리면자연스레발을내딛어균형을유지하라는것과같은이치다. 때문에각부 Received : Sep. 8. 010; Reviewed : Mar. 07. 011; Accepted : Mar. 11. 011 * 본연구는지식경제부및정보통신산업진흥원의대학 IT연구센터지원사업의연구결과로수행되었음 " (NIPA-010-C1090-101-0010). 교신저자 : 부산대학교전자전기공학과교수 1 부산대학교전자전기공학과석사과정 분의전정기관과뇌, 근육등사람이균형을유지하는신체기관들과유사한기능을가진다. 예를들면자이로센서와가속도센서는사람의전정기관에, 제어보드는뇌, 동작모터와기어박스는다리근육에각각해당한다. 센서가중력변화를감지해마이크로프로세서에정보를보내면 1초에수백번에달하는조정작업을통해균형과이동을조절한다. 모바일로봇의무게중심이앞이나뒤로기울일경우중심을잡기위해바퀴를앞뒤로회전시키는원리가응용된다. 자세결정및진행방향계산은자이로센서로부터출력되는각속도를적분함으로써이루어진다. 하지만자이로센서의출력값은바이어스오차를포함하고있기때문에이를그대로이용할경우큰자세오차가발생하게된다 [7]. 따라서이를보상하기위해가속도계를이용하게되고진행방향에대한오차를보상하기위해서지자기센서가이용된다. 기존의연구는주로이러한자이로센서에서발생하는바이어스오차와외란등을보상하
등반능력향상을위한이륜역진자로봇의최적 ARS 제어 109 여안정된동작수행에관한연구 [8], 또는이상적인환경에서의자세제어에만국한되어있으며이에비해기울기가있는환경에서의주행을위한자세제어에관한연구는아직까지많이이루어지지않았다. 본논문에서는바퀴구동형이륜역진자로봇메커니즘을고려하여이륜역진자로봇의평지주행뿐만아니라지면이기울어진환경에서주행실험을수행한연구결과를제시함으로써, 기존의선행연구와차별성을부각시키고자한다. 이러한이륜역진자로봇이경사진지면을등반할시 ARS (Attitude Reference Sytem) 을통한최적의자세제어 [9] 를목표로한다. 여기서최적의자세제어라함은등반시고려되여야할사항인모바일로봇의안정성을말한다.. 로봇메커니즘모델링.1 동역학방정식의유도시스템을적절하게이해하고, 제어하기위하여동역학적모델을구성할수있다. 그림 1은본논문에서제안하는이륜역진자로봇을도식화한그림이다. 여기서각각의파라미터들은아래의그림과같이 W 는몸체의넓이, H 높이, D 로봇의몸체의폭, R w 좌측, 우측바퀴의반경그리고 는좌측, 우측바퀴의회전각도를나타낸다. 동역학방정식을유도하기위하여, 그림 1의이륜역진자로봇을아래의그림 와같은 Free Body Diagram형태로바꾸어볼수있다. 본논문에서제안된시스템은바퀴축과그에대한수직축을기반으로하는모델로서 1차원평면에서움직임은몸체의기울기와선운동에의해결정된다. 여기서, Z 1 와 Z 는각각바퀴와몸체의중심으로부 그림. 로봇의 Free Body Diagram 터높이를말하며, S 1 과 S 는바퀴와몸체의이동거리를, l 은바퀴와몸체의중심간의거리를나타내었다. 또한중심각에서기울어진각도를 라하고, 바퀴가회전한각도를 φ라표기하였다. 또한, 동역학해석에필요한시스템파라미터들을표 1 에나타내었다. 이동역학모델의라그랑지 (Lagrange) 운동방정식은아래의식 (1,) 과같이나타낸다 [10]. 이륜역진자로봇의동역학은바퀴의회전에의한몸체각 φ와바퀴의회전각 에대하여다음과같이정의할수있다. d T T U D + + = Q dt & φ φ φ & φ d T T U D + + = Q dt φ (1) () 그림 1. 이륜역진자로봇의모델링 φ g η I M I B I W μ μ g M B M W 표 1. 이륜역진자로봇의파라미터 Rotation angle of wheel w.r.t. the vertical line A tilted angle of body w.r.t. the vertical line Gravity acceleration Reduction ratio of gear Rotational inertia of the motor axi Rotation inertia of the body Rotation inertia of the wheel Friction coefficient of bearing at the wheel Friction coefficient of wheel at the ground Ma of the body Ma of the wheel
110 로봇학회논문지제 6 권제 호 (011. 6) 여기서, T는 Kinetic energy, U는 Potential energy, D 는 Diipation energy function, Q φ 는 External force to φ 그리고 Q 는 External force to 를나타낸다. 또한방정식을유도를위해, T, U, D, Q φ, Q 를구하면다음과같다. E F ( MBl + IB + IMη )( MBRwl IMη ) =, ( MBl + MBl + IB) ( MBr MwRw + MBRwIw) μw μw M Bgl 0 = 0, G = μ g M Bgl 0, and ητ1 H = 0. 1 1 T = Mw( S& + Z& ) + MB( S& 1 + Z& 1 ) (3) 1 1 1 + I & w + IB( & & φ) + IMη & φ U = M gr + M gl co( φ) (4) w w B 1 ( μwφ μ g ) D = + & & (5) 그리고, 로봇의좌표계와가해지는힘의방향을그림 3에나타내었다. 여기서, R 1, 는좌측, 우측바퀴를말하며, 두바퀴사이의중심점단위벡터 n 1 n n 3 을각각정의하였다. 그리고 K 1, 는바퀴에가해지는힘, M 1, 는모터에의해발생된토크, G는몸체에가해지는중력, L은바퀴사의의거리의반지름그리고 R w 은바퀴의반지름을나타낸다. 또한, 표 에제어에필요한시스템파라미터를아래와같이나타내었다. Q φ = ητ u (6) 1 1 Q = 0 (7) 위식의선형화를위해, 위식 (3) 에서 (7) 을식 (1),() 에대입하면아래와같은식을얻을수있다. && φ + ( MBRwl IMη )&& + μφ& μ& M glφ = ητ u ( MBl + IB + IMη ) w w B 1 1 (8) ( MBRwl+ MBl + IB) + + + + μ& M glφ= 0 [( MB Mw) Rw MBRwl Iw] g B && φ && (9) 그림 3. 로봇의좌표계와가해지는힘의방향 표. Sytem parameter 선형화된모델의상태방정식은식 (8),(9) 으로부터식 (10) 과같이임의의변수 E,F,G, 그리고 H에대해서다음과같이간단히표현할수있다. && 1 E φ & F φ G φ H μ + + = μ (10) && & 여기서, M B M w L R w 11.01 kg 0.61 kg 0.1 m 0.09 m I B 0.4 kgm I W 6.1 x 10-4 kgm g 9.8 cm / τ T [0.035NM / A] I M [3.x10-6 kgm ] η η=44.6
등반능력향상을위한이륜역진자로봇의최적 ARS 제어 111 3. LQR 제어기설계기구부설계시동역학식으로부터구해진각각의파라미터값으로, MATLAB/Simulink를통해선형제어기를구현해봄으로써제어기의타당성을확인하였다. 선형화된식 (10) 에서 에대해서정리하면, 또한, 위식들을상태방정식으로다음과같이정리할수있다. x () t = Ax() t + Bu() t (15) yt () = Cxt () (16) && φ & 1 1 1 1 E F φ E G φ E H μ = + + μ (11) && & 임의의변수 O, P, Q, R, S, T, U, V, W 에대해다음과같이정리할수있다. && φ 1 OP & φ 1 S0 φ 1 U u1 = W QR + + W T0 W V u (1) && & && O + Pφ + Qφ + Ru = (13) W 0 1 0 0 O Q P 0 0 W W W R x =, = + u φ φ 0 0 0 1 φ 0 φ φ S T U φ V 0 W W W y = [ 1 0 1 0] φ φ (17) (18) && S Tφ + Uφ Vu φ = (14) W 여기서 O = M R lμ + M l μ + μ I B w w B w w B + M l μ + μ I + μ I η B g g B g M P = M R lμ + M l μ + μ I B w w B w w B Q = M gli η B M R = M R l+ M l + I B w B B S = M R lμ μ I η B w g g M + ( M + M ) R μ + M R lμ + μ I B w w w B w w w w T = ( M + M ) R μ + M R lμ + μ I B w w B w w w w U = M gli η + M gr l B M B w + M M gr l + M gli B w w B w V = ( M + M ) R + M R l + I B w w B w w W = M + M R l + ( M + M ) R I B w w B w w B + M l I + I I B W B W ( M ) B + Mw Rw + MBRwl + IMη + MBl + IB + I w 그림 4. LQR 제어기의설계 그림 5. 기울기변화에대한시뮬레이션결과
11 로봇학회논문지제 6 권제 호 (011. 6) 그리고이륜역진자로봇의제어를위해 MIMO (Multiple Input Multiple Output) 시스템에적합한선형제어기인 LQR제어기 [11] 를 MATLAB을통하여설계하였으며, 그림 4에나타내었다. 그림 5는선형화된 Model과 LQR controller 설계에따른 Tet 결과로서, 초기에몸체를 10 를기울였다놓았을때 φ, 를 Simulink로나타낸결과로 0.5초내에반응하여자세를유지함을보였다 4. 경사면등반이륜역진자로봇이경사지형을등반시바퀴의접점에서역학적힘이발생하게된다. 이힘은바퀴의접합점에서의변화또는접합면에의해발생하게된다. 결국이러한힘의변화는이륜역진자로봇의동작에영향을미치게된다. 또한경사진면으로올라갈때의운동은평면의진자운동에서지면이기울어진각만큼가해지는중력방향의힘을더해주어야한다. 4.1 등반시필요조건동역학적시스템을이끌어내기전에몇가지가정이성립되어야한다 [1]. (1) 로봇은지면이고른경사면에서의 x축과 y축에대한움직임만을고려되어야한다. () 시스템을간략화하기위해서, 바퀴와지면사이의마찰에의한손실은없다고가정한다. (3) 바퀴는지면과단하나의마찰부위를가지고있다. (4) 바퀴가달린몸체와지표면은단단한물체라고가정한다. 지금까지이륜역진자로봇은일반적으로평평한평지에서실험이주를이루고있지만, 본논문에서는경사가존재하는표면에서이륜역진자로봇의자세제어를위해 다음의그림 6과같은두가지상황을고려하였다. 첫번째그림은경사면의길이방향으로안정된자세의관점에서경사각허용한도를그리고두번째그림은경사면의수평방향에대한로봇의자세제어를나타내고있다. 또한, 각각경사면의각도변화에따른모터에얼마만큼의추가적인힘이필요한지도고려하였다. 기하학적관계를바탕으로최대허용경사각도ζ 는아래의식 (19) 와같이결정된다. ζ tan 1 L l + R w (19) 예를들어, 이륜역진자로봇의파라미터값중 l = 0.365, R w =0.09, L =0.1라면, 식 (19) 에따라지면에서로봇이넘어지지않게하기위해서는최대경사각도를 4 가초과하면안됨을알수있다. 또한, 평평한평지와경사가있는표면을비교하기위해서는각각의상황에따라 자유도모델이고려되어야한다. 평지의 자유도모델의운동방정식은단순화 3자유도모델에의해결정되어지며다음의식 (0),(1) 과같다. (3 m + m )&& x mlcoφφ&& + mlinφφ&& c α3 + β3 = R W ml φ&& x+ ml + I && φ mgl φ 3 co ( ) in = α β 3 3 (0) (1) 코리올리힘 (The Corioli force) 과회전동작과관련된것들은제외후경사면에서의 자유도모델은다음의식 (),(3) 과같이힘이선형적으로증가함을보여준다. (3 mc+ m )&& x ml co( φ ξ) && φ + mlin( φ ξ) && φ+ ( m+ m) ginξ c α3 + β3 = R W () ml && x+ ml + I 3 co( φ ξ) ( ) mglin( φ ξ) = α β 3 3 && φ (3) 그림 6. 경사면위의이륜역진자로봇
등반능력향상을위한이륜역진자로봇의최적 ARS 제어 113 다음위의두운동방정식을비교시위치 x를제어하기위해서로봇이 (m c+m )ginξ만큼의더많은힘이증가되어야함을보여주고있으며사인곡선의관계는 φ의함수로부터 φ ξ 로변경되어짐을알수있다. 이륜역진자로봇이경사진표면에서자세를유지하기위해요구되는힘에대해서도시뮬레이션을해보았으며, 그에따른결과는그림 7에나타내었다. 자세제어를위해모터에요구되는힘은각도가 5 이내는변화를보이지않았으나경사면의각도가 5 를넘어서는요구되는힘이선형적으로증가함을알수있다. 또한, 이륜역진자로봇이평지에서경사지형으로등반시로봇의바퀴와경사지면의접합면에서발생할수있는외란에대해서도실험이진행되었다. 아래의결과는로봇이경사면을인식하고주행하였을때와그렇지않은경우에대한결과로서, 로봇이경사를인식하고경사면을주행했을경우이륜역진자로봇의최대각도는 ±15 정도양호하였지만, 그렇지않은경우에서는 ±30 이상으로로봇이넘어지는것을확인할수있었다. 5. Attitude Reference Sytem 이륜역진자로봇이경사진면으로올라가는경우지면이기울어진각만큼가해지는중력방향의힘을더해주어야하며, 로봇이수평방향으로받는힘이평형일때, 운동방정식은다음과같다. F = M && + M ginα + f coα N (4) w w C 따라서, 지면의기울어진각 α를고려하여역진자로봇의균형을유지시키기위한운동방정식은식 (5) 과같다. F = ( M + M )&& + M l&& φcoφ w B B M lφ inα + M ginα + f coα& B w C (5) 위식은비선형운동방정식이므로동작점부근에서선형화가가능하다. 따라서 φ를아주작은값으로가정하면, φ = 0 (6) inφ = φ (7) coφ = 1 (8) 식 (6),(7),(8) 를식 (5) 대입하면, 식 (9) 을얻을수있고, 그림 7. 각도변화에따른모터출력 F M gin α = ( M + M )&& + M l&& φ + f & (9) w w B B C 이를통해선형화이전의운동방정식과비교하면지면의기울기변화에의해가해지는힘. 즉, M wginα만큼보상이되어야한다는것을알수있다. 따라서, 어떠한기울기 α에서 M wginα만큼보상될때그경사면에서정지할수있는조건을만족할수있으며, 진행하고자하는방향으로 M wginα+β를더해줌으로써 β 의기울기로주행이가능하다. 또한 β의속도값은 ARS각도변화에따라변경이가능함을알수있다. 6. 실험및검토 그림 8. 지면과바퀴의접합면에대한로봇의영향 본논문에서제안하는이륜역진자로봇시스템의구성
114 로봇학회논문지제 6 권제 호 (011. 6) 을그림 9에서와같이나타내었다. 모바일로봇은두개의바퀴와모터드라이버그리고 MCU (Micro Controller Unit, DSP 8335), 자이로및가속도, 초음파센서로구성된다. 이륜역진자로봇의주행을위한실험은, 평지에서외란을가했을경우와기울기가있는환경에서각각 5도씩변경하면서로봇의최대허용각도인 4도이내인 0도까지 5m거리를주행하는실험으로나눠서진행하였다. 또한, 수평계를사용하여지면이경사져있다는것을그림 10 에서와같이나타내었다. 기존 4륜역진자로봇의경우로봇몸체에기울기센서를부착하여현재의기울기정보를추정할수지만이륜역진자로봇의경우기울어진경사면에서도바퀴를중심으로몸체가수직으로서기때문에현재의기울기정보를알수없는단점이있어, 이륜역진자로봇이기울어진경사면을등반하는데에있어큰어려움이있어왔다. 이러한이륜역진자로봇의경우현재의기울기정보를 알수없는단점을보완하고자이륜역진자로봇의앞부분에초음파센서를부착하여초음파수신시현재의지면과로봇간의거리로각도정보를추정할수있도록하였다. 이를통하여경사면에서도이륜역진자로봇의주행이가능하도록하였다. 아래의표 3은초음파센서를통한거리에대한지면의기울기를변환한값이다 [13]. 평지에서외란을가해주었을때결과가그림 1에나타나있으며, 그림 10과같은환경에서의주행실험결과가그림 13에서그림 16에나타내었다. 실험은각경사면에 Angle (Degree) 표 3. 초음파센서와경사면과의거리를각도로변환 Ditance (Centimeter) 0 00cm 이상 5 10 cm 10 0 cm Time (u) 15 30 cm 0 40 cm 그림 11. 초음파센서를이용한거리측정원리 그림 9. 이륜역진자로봇시스템의구성도 그림 10. 경사지면의등반주행을위한실험환경 그림 1. 평지에서의외란에대한영향
등반능력향상을위한이륜역진자로봇의최적 ARS 제어 115 그림 16. 0 도의기울기에서의주행 그림 13. 5 도의기울기에서의주행 있게하였으며, 등반이완료된시점에다시현재의기울기에수렴되게하여이륜역진자로봇을경사면에정지시킬수있다. 아래에보여지는결과는경사면에서로봇이 5m 을 10초간주행하면서나타나는결과값으로위에서순서대로현재경사면의각도, 주행거리그리고 ARS를통한로봇의각도값을나타내었다. 추가적으로경사면의기울기변화에대한로봇의이동속도를그림 17에나타내었다. 그림 14. 10 도의기울기에서의주행 그림 17. 경사각변화에따른로봇의주행속도 표 4. 경사각변화에따른속도변화 Slope Angle [Degree] Target Angle [Degree] Meaured Average Velocity[m/] 5 0.79 [m/] 그림 15. 15 도의기울기에서의주행 서초음파센서로부터측정된기울기에서로봇에임의로 5 의가중치를더해주어이륜역진자로봇이주행될수 10 15 0 5 0.811 [m/] 0.831 [m/] 0.86 [m/]
116 로봇학회논문지제 6 권제 호 (011. 6) 그림 18. 실제이륜역진자로봇의사진 7. 결론본논문은기울기가있는환경에서기울기센서및가속도센서즉 ARS (Attitude Reference Sytem) 를이용한이륜역진자로봇의등반을목표로하였다. 등반시, 최적자세제어를위한이륜역진자로봇의주행알고리즘을연구하였고이륜역진자시스템의특성을고려하여물리적해석을통해동역학방정식을유도하고경사면에서이륜역진자의주행을위해초음파센서를이용하여지면의기울기정보를획득하였다. 이획득된기울기정보를통해역진자의동작점을가변함과동시에기울어짐에따라받는중력의힘을보상하기위하여제어입력에가중치를줌으로써지면의기울어짐에도경사면의각도에따라일정한속도를유지하면서효과적으로제어가가능함을위의결과값들을통하여확인하였다. 이륜역진자로봇은지면의평탄도에민감하기때문에지면이고르지못한환경에서자세제어를하기위해선지면의평탄도측정이실시간으로요구된다. 이를보상하기위한알고리즘과강인한제어방법 [14-16] 에대한연구를향후수행할계획이다. 참고문헌 [1] H. Azizan, M. Jafarinaab, Fuzzy control baed on LMI approach and fuzzy interpretation of the rider input for two wheeled balancing human tranporter, IEEE Proceeding of the 010 8th International Conference on Control and Automation (ICCA), pp. 19 197. [] K. M. Goher, M. O. Tokhi, A new configuration of two wheeled vehicle: Toward a more workpace and motion flexibility, IEEE Proceeding of the 4th International Sytem Conference, 010, pp. 54 58. [3] W. Zhong, H. Rock, Energy and Paivity Baed Control of the Double Inverted Pendulum on a Cart, IEEE Proceeding of the 001 International Conference on Control Application, pp. 896 901. [4] F. Graer, A. D Arrigo, S. Colombi and A. C. Ruffer, JOE: A Mobile, Inverted Pendulum, IEEE Tranaction on Indutrial Electronic, Vol. 49, No. 1, pp. 107 114, 00. [5] D. Voth, Segway to the Future autonomou mobile robot, IEEE Intelligent Sytem, Vol. 0, No.3, pp. 5 8, 005. [6] K. Pathak, J. Franch and S. K. Agrawal, Velocity and Poition Control of a Wheeled Inverted Pendulum by Partial Feedback Linearization, IEEE Tranaction on Robotic, Vol. 1, No. 3, pp. 505 513, 005. [7] S. S. Kim, S. Jung, Control experiment of a wheeldriven mobile inverted pendulum uing neural network, IEEE Tranaction on Control Sytem Technology, Vol. 16, No., pp. 97 303, 008. [8] H. J. Lee, S. Jung, Gyro enor drift compenation by Kalman filter to control a mobile inverted pendulum robot ytem, IEEE ICIT, pp. 106 1031, 009. [9] O. C. Heo, K.H. Park, Etimating Accelerated Body Attitude Uing an Inertial Senor, ICCAS SICE, pp. 5474 5478, 009. [10] Y. S. Ha, Trajectory tracking control for navigation of the invere pendulum type elf contained mobile robot, Robotic and Autonomou Sytem, Vol. 17, No. 1, pp. 65 80, 1996. [11] X. Xin, W. Zhou, The The Simulation of Double Inverted Pendulum Control Baed on Particle Swarm Optimization LQR Algorithm, ICSESS, pp. 53 56, 010. [1] Z. Kauar, K. Stol and N. Patel, Performance Enhancement of a Statically Untable Two Wheeled Mobile Robot Travering on an Uneven Surface, IEEE Conference on Robotic Automation and Mechatronic, 010, pp. 156 16. [13] H. N. Kim, H. J. Ha, A tudy on the obtacle avoidance uing ultraonic enor for a mobile
등반능력향상을위한이륜역진자로봇의최적 ARS 제어 117 robot, KIIS Proceeding of Spring Conference, Vol. 0, No. 1, pp. 56 534, 010. [14] Z, Li, Y. Yang, Robut adaptive motion/force control for wheeled inverted pendulum, IEEE Tranaction on Robotic and Automation, Vol. 46, No. 5, pp. 1346 1353, 010. [15] K. Pathak, J. Franch and S. K. Agrawal, Velocity and Poition Control of a Wheeled Inverted Pendulum by Partial Feedback Linearization, IEEE Tranaction on Robotic, Vol. 1, No. 3, pp. 505 513, 005. [16] X. Ruan, J. Chen, H Robut Control of Self Balancing Two Wheeled Robot, WCICA, pp. 654 657, 010. 이장명 1980 서울대학교전자공학과 ( 공학사 ) 198 서울대학교전자공학과대학원 ( 공학석사 ) 1990 미국남가주대학교 (USC) ( 공학박사 ) 199~ 현재부산대학교전자전기통신공학부교수관심분야 : 지능로봇시스템제어, 마이크로프로세서응용, 특수환경 Localization 및 Navigation 권영국 006 한국교육개발원전자공학과 ( 공학사 ) 009~ 현재부산대학교전자전기공학과석사과정재학중관심분야 : Mobile inverted pendulum, Humanoid robot, HRI, Motor control.