convex MPC(모델 예측 제어) for Quadruped Robot (4족 로봇) (feat. 스쿼트 모션)

안녕하세요. 오랜만에 재미있는 내용으로 인사드립니다. 글 작성이 좀 뜸했지요..? ㅎㅎ

업무도 있고, 개인 일상도 바쁘다 보니 블로그 글을 많이 업로드를 하지 못했네요.

그럼에도 그 사이에 구독자 분들이 두배 이상 늘었네요. ^^ 감사합니다 :)

 

연구노트를 보니 20년도부터 Legged (or Legged Wheel) Robot에 관심을 갖고, 공부를 시작했네요. 여담이지만 저는 박사 과정을 밟으면, Legged Robot을 연구하고 싶었습니다. 마음 한켠으로는 연구실에서 해보지 못한 아쉬움이 있었는데, 지금이라도 해볼 수 있어서 행복합니다.

Quadruped Robot을 시작으로 Bipedal Robot으로 까지 확장할 수 있도록 박차를 가해야겠네요. :)

 

오늘 공유할 내용은 convex MPC를 기반으로 Quadruped robot(4족 로봇)의 모션 제어입니다.

 

구현에 참고했던 논문은 김상배(Sangbae Kim) 교수님 연구실의 'Jarad Di Carlo'의 'Dynamic Locomotion in the MIT cheetah 3 Through Convex Model Predictive Control(2018)' 입니다.

 

이전에 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 진행 중
3. 2020/09/24 - [OnGoingProj] - Two wheel balancing robot - LQR 제어
4. 2020/09/25 - [OnGoingProj] - Two wheel balancing robot - 하드웨어 구매 및 앞으로의 프로젝트 계획
5. 2020.09.27 - [OnGoingProj] - [4족 로봇] Petoi Bittle - 클라우드 펀딩 후원 및 구매

6. 2022.02.11 - [OnGoingProj] - 4족로봇의 Centroidal Model 및 MPC를 이용한 Path Planning(경로 계획) Study 계획

 

1. About Plant ?

 

Fig1. 귀요미 Quadruped Robot (4족 로봇)

 

오늘 시뮬레이션할 로봇입니다. 각각의 다리가 2 자유도씩 (Hip 1 자유도, Knee 1 자유도)를 가진 simple한 4족 로봇입니다. 제어 알고리즘 적용 및 테스트를 위한 로봇입니다. ^^ 귀엽죠?

 

2. simulation goal?

이번 시뮬레이션의 목적은 1) Single Rigid Body (SRB) Dynamics 적용과

                                                2) Convex MPC의 적용에 있습니다.

 

이번 시뮬레이션에서는 Gait에 대한 고려를 하지 않았습니다. (swing motion 고려하지 않음)

즉, 항상 바닥과 contact이 유지가 된다고 가정을 했습니다.  그리고 접촉에 대한 고려는 다양한 Gait 패턴에 대한 내용을 고려할 때, 추가하겠습니다.

그리고, 시뮬레이션이기 때문에 모든 상태를 알 수 있다고 가정하고, state estimator 역시 고려하지 않았습니다. 

 

로봇 모션은 스쿼트 모션을 해보려고 합니다. 즉,  CoM의 z방향으로 로봇이  앉았다가 일어나는 모션입니다. ㅎㅎㅎ

 

3. Control System Block Diagram

 

Fig2. Control Block Diagram

 

그림 2는 전반적인 Control Bock Diagram입니다. Reference Trajectory를 받으면, convex MPC(Model Predictive Control)에서는 Ground Reaction Force(GRF)를 input을 생성합니다. 이를 joint space의 torque로 변환하여 4족 로봇을 토크 제어를 해주었습니다.

 

그리고  물리엔진을 사용한 것이 아니라 contact에 대한 고려는 하기와 같은 contact dynamics를 사용했습니다.

Fig3. Contact Dynamics

 

4.  SRB (Single Rigid Body) Dynamics

시스템의 모델은 다음과 같은 가정을 바탕으로 구하였고,  선형화를 진행 하였습니다.

 

* Assumption

  1) Body Mass에 비해 Leg Mass 무시 가능  → Single Rigid Body로 가정

  2) Small roll, pitch angle  → euler derivative와 angular velocity 간 근사

  3) Small angular velocity  → euler equation에서 precession 운동 무시

 

1) SRBD (Single Rigid Body Dynamics)

 

하기는 Single Rigid Body에 대한 식입니다. 

 

Fig4. SRBD (Single Rigid Body)

여기서 p는 world 기준 CoM의 postion, f_i 는 i번째 다리의 grf(지면반력), g는 중력입니다.

그리고 Iw는 world 기준 각운동량(angular momentum), r_i는 CoM기준으로 contact지점의 position입니다.

마지막으로 [w]x는 world 기준 angular velocity의 skew matrix, R은 body frame에서 world frame으로 변환하는 rotation matrix입니다.

 

2) Angular Velocity Dynamics Approximation

 

convex MPC를 적용하기 위해서는 선형 모델을 구해야합니다. 위의 SRBD를 가정 2와 3을 바탕으로 선형화를 진행해보겠습니다. Euler angle을  zyx convention으로 표현하면 다음과 같습니다.

Angular velocity와 Euler angle derivative와의 관계는 아래와 같으며, small roll, pitch를 가정하면 다음과 같이 근사됩니다.

small angular velocity라고 가정하여, euler equation에서 뒤에 있는 precession 운동 무시하였습니다.

state는 다음과 같이 구성하였습니다. 

x = [euler, CoM position in world frame, angular velocity in world frame, linear velocity in world frame]

이를 기반으로 linearized SRBD를 구성하면 다음과 같습니다.

마지막으로 state에 gravity term을 추가하여 구성합니다.

5. Convex Model Predictive Control(MPC, 모델 예측 제어)

MPC를 구성하면, 다음과 같습니다. 하기 식에서 k는 MPC의 time horizon의 갯수입니다.

위의 식을 최적화 하면, state trajectory와 input에 대한 size로 인해 time complexity가 높아집니다. 이를 다음과 같은 형태로 condesned 하게 변환하면, input에 대한 식으로 변환합니다. 사이즈가 줄었지요?

6.  Simulation Results

자, 지금까지 식을 바탕으로 시뮬레이션을 구성했습니다.

reference trajectory로 스쿼트 모션을 주기 위하여, CoM의 선속도를 -0.1m/s의 크기를 갖는 0.5Hz의 sine파로 가하였습니다. 

 

결과를 보실까요?

 

2초간 시뮬레이션을 한 결과인데,  viusalization시 python 실행 속도가 느려서 그런지 영상이 느리게 나오네요.

하기 그래프를 보면,  desired CoM의 z축 방향 위치와 CoM이 정확히 일치하는 결과가 나왔네요.

state weight matirx Q에서 CoM의 z성분을 많이 높여놨더니 이런 결과가 나왔네요.ㅎㅎㅎ?

 

 

감사합니다. :)