Two wheel balancing robot - LQR 제어

안녕하세요. 오늘은 오랜만에 진행 사항을 공유드리고자 글을 쓰네요.

혹시 지난 연구 기록들에 대해 궁금하신 분들은 아래에 있는 study note를 참조해 주세요 :)

 

Study note

1. 2020/08/09 - [OnGoingProj] - Ascento - 귀여운 legged wheel robot

2. 2020/08/27 - [OnGoingProj] - Mobile Robot(Differential Drive Robot) Modeling and Control 진행 중

 

이번은 복잡한 Ascento를 two wheel balancing robot으로 단순화하여, 밸런스(balance)와 속도를 LQR control(제어) 시뮬레이션을 해보았습니다. two wheel balacing robot은 inverted pendulum에 바퀴가 달린 모델이기 때문에 시스템 자체가 불안정합니다. 그래서 반드시 balance를 제어해주어야 합니다. 대부분의 two wheel balancing robot이 직진하면서 balance를 잡는 것과는 달리 이번 시뮬레이션에서는 linear velocity(선 속도), angular velocity(각 속도), balance를 동시에 제어 가능합니다.

 

1. Ascento Modeling

 

먼저, Ascento를 모델링하기 전에 전체 구조에 대해 한번 살펴 보겠습니다. Ascento는 두 개의 바퀴(two wheel)과 Four bar linkage(4절 링크)의 다리를 갖고 있습니다. 바퀴(wheel)가 있어서 바퀴 주행을 할 수 있고, 엉덩이(Hip)쪽에 있는 다리(leg)로 인해 장애물이 있을 때, 점프를 할 수 있습니다.

 

Four bar linkage구조는 점프를 할때, 무게 중심이 그대로 위, 아래로(지면과 수직 하도록) 움직이게 해줍니다. 그래서 Stabilizing(=balancing) control과 jumping control을 각각 독립적으로 할 수 있게 만들어 줍니다.

jumping 동작과 stabilizing이 독립적이기 때문에, jump동작은 feedforward 값으로 주도록 하고 stabilizing에 초점을 맞춰보도록 하겠습니다.

 

  

 

Ascento 및 four bar linkage legged 구조

 

 

처음부터 four bar linkage구조인 다리를 고려하기 어렵기때문에 좀 더 단순화 되었지만 강력한 모델인 two wheel balancing robot으로 모델링을 해보겠습니다. 추후에 위 모델을 바탕으로 실제 제어가 잘 된다면, four bar linkage 구조도 추가하여 적용해 보겠습니다. ^^

 

 

Simplified Ascento model

 

 

Ascento의 문헌에서는 lean angle인 ${\beta}$역시 정의되어 있지만, 이번 시뮬레이션에서는 생략하겠습니다.

 

※ Assumption

 

1) 바퀴는 지면과 항상 접촉하고 있으며, no-slip 상태(=미끄러지지 않고, 굴러가는 상태)
   -> 구속 조건(constraints)

2) 다리 관절(four bar linkage)의 dynamics는 무시

3) 모터의 dynamics는 무시

4) lean angle (${\beta}$) 일단 무시

 

2. Generalized Coordinate & Robot Dynamics,  State &  State Space Model 정의

 

1. Generalized Coordinate, ${q}$  & Robot Dynamics

 

 Robot dynamics를 정의하기 위해서는 먼저 generalized coordinate, ${q}$를 정의해야 합니다. ${q}$ = {${x, y, \gamma, \phi_1, \phi_2, \theta}$}과 같이 정의하였습니다. 이렇게 정의된 generalized coordinate을 바탕으로 Lagrangian Method를 통해 two wheel balancing robot의 dynamics를 구할 수 있습니다. 

 

 ${x, y}$ : robot's position

 ${\gamma}$ : robot's orientation

 ${\phi_1, \phi_2}$ : wheel's rotation angle

 ${\theta}$ : pitch angle(=tilt angle)

 

 

 

바퀴는 no-slip이라는 구속 조건(constraints)이 들어 갔기때문에 constraint matrix인 ∧, lagrangian multiplier vector인 ${\lambda}$ 가 robot dynamics에 추가 되었습니다.

 

2. State for LQR  & State-space

 

 LQR제어를 하기 위해서는 위의 robot dynamics가 아닌 state - space모델이 필요합니다. 로봇에게 직접 target velocity를 제어하면서 balance를 잡기 위해 state는 다음과 같이 정의했습니다. ${x}$ = {${v, w, \dot{\theta}, \theta}$}.

robot's kinematics와 constraint의 null space조건을 이용하면 최종적으로 아래와 같은 state - space 모델을 구할 수 있습니다. 식을 간단하게 풀기 위해 평형점인 ${\theta = 0, \dot{\theta} = 0, \dot{\gamma} = 0}$ 에서 선형화를 시켰습니다.

 

 ${v}$ : linear velocity

 ${w}$ : angular velocity

 ${\dot{\theta}}$ : derivative of pitch angle( = tilt angle)

 ${\theta}$ : pitch angle( = tilt angle)

 

 

 

3. 선형 LQR 제어기 설계

 

 자, 이제 위에서 구한 state - space model을 바탕으로 cost function J를 최소화하는 optimal gain K를 구하면 됩니다.

 

 

 

4. Result

 

시뮬레이션은 multi body dynamics를 제공하는 python의 pydy package를 사용해서 animation화 했습니다. 비선형식을 operation point에서 선형화를 시켰기 때문에, 다양한 기울기에서도 제 성능이 나오는 지 확인하기 위해 일부러 로봇의 tilt angle 초기값을 - pi/10으로 주었습니다.  target값은 linear velocity, ${v}$ = 2 m/s, angular velocity = 0.5 rad/s 입니다.

 

 

Ascento simplifed model - two wheel balancing robot stabilizing simulation(LQR)

 

 

오.. 애니메이션 결과를 보면 로봇이 상당히 밸런스를 잘 잡으면서 움직이는 군요. 자세한 값을 보기 위해 그래프로 그려보았습니다. target linear velocity와 angualr velocity도 잘 따라 갔네요. 그리고 로봇의 balance를 나타내는 theat_dot과 theta를 보면 알수 있듯이 balance 또한 잘 잡았습니다. :)

 

 

 

 

 

지금까지 Ascento를 two wheel balance robot으로 simplifed해서 LQR 제어를 해보았습니다. 다음으로는 실제 가상환경을 모사한 물리엔진 bullet에서 위 제어기를 갖고 제어를 해보고자 합니다. 다양한 disturbance도 넣어보고자 합니다. 그리고 점프 동작은 feedforward로 고려해서 넣어보려고 합니다. 잘 된다면, leg dynamics도 고려하여 MPC까지 적용해보려고 합니다. 혹시 이 내용과 관련해서 비슷한 프로젝트를 진행을 하다가 궁금한 점이 있다면 아래 댓글에 남겨주세요~

 

그리고 최근에 Kinematics(기구학) 포스팅이 인기가 많은데요.. Kinematics(기구학) 시리즈 2편과, PID control(제어) A to Z 2편도 곧 업로드 됩니다. ^^ 조금만 기다려주세요. 

 

 

감사합니다.