[Quadruped Robot] Forward Gait #1 - Foot Trajectory 설계

1. 서론

  지난 포스트까지 pyBullet을 통해 로봇을 시뮬레이션하기 위한 준비 단계였습니다. 지금부터는 실제로 로봇이 앞으로 전진하기 위한 방법에 대해서 알아보려고 합니다. 하지만, 단순히 전진 보행(Forward Gait)이 단순한 행동처럼 보이지만 구현하기 위해서는 많은 준비 단계가 필요합니다. 그래서 전진 보행을 하기 위해 발의 움직임을 구현하는 방법을 여러개의 포스트로 나누어 설명드리려고 합니다.

 

  먼저 이번 포스트에서는 전진 보행을 구현하기 위해서 어떻게 발을 움직여야 하는지에 대해서 알아보려고 합니다. 다행스럽게도 Yertle 프로젝트에서 pyBullet 내에서 Gait을 구현하였으므로 해당 코드를 리뷰하면서 어떻게 Gait을 구현하였는지 확인하면서 진행해보도록 하겠습니다.

 

2. Yertle 프로젝트의 Gait 구현 방법

  특히 Yertle 로봇에서는 Trot Gait이라고 하는 방법을 사용하여 보행을 구현하고 있습니다. Trot Gait은 말의 보행할때 처럼 대각선(Diagonal)으로 마주보는 다리들을 쌍으로 묶어 서로 다른 위상에서 움직이는 것을 말합니다. 예를 들어, 말의 경우 4개의 다리가 있으며 각 다리를 Left Front, Right Front, Left Back, Right Back으로 명칭하면 대각선으로 마주보는 다리의 쌍인 Left Front와 Right Back, Right front와 Left Back를 서로 묶어 보행하는 것을 말합니다. 

Yertle에서 Trot Gait시 발의 묶음

3. 발의 초기 각도

Yertle 로봇의 초기 발 관절 각도

  보행(Gait)를 하기 전 먼저 초기 발의 위치를 설정해주어야 합니다. 저는 2가지를 고려하여 발의 위치를 설정하였습니다.

• 어깨(중심 점)으로부터 발 끝 위치까지 거리
• 보행에 유리한 다리의 각 관절 각도

  해당 2가지를 어떻게 고려하였는지 설명드리겠습니다.

3.1. 어깨(중심 점)으로부터 발 끝 위치까지 거리

 

  θ1의 각도에 의해 "어깨부터 발끝까지 거리"를 설정할 수 있으며 "어깨부터 발끝까지 거리"에 따라 "발의 전후진 범위"가 결정됩니다. 결국 다시 말해 θ1의 적절한 값을 결정하여야 "발의 전후진 범위" 를 결정하게 되는 것입니다.

 

3.1.1. 어깨(중심 점)으로부터 발 끝 위치까지 거리가 너무 먼 경우

  만약 어깨(중심 점)으로부터 발 끝 위치까지 거리가 너무 먼 경우 전후진 하기 위한 범위가 짧아집니다. 그말은 전진을 빠르게 하기 위해서 발을 빠르게 구르게 되면 안정성이 떨어져 보행의 최대 속도가 떨어 질 수 있습니다.

 

3.1.2. 어깨(중심 점)으로부터 발 끝 위치까지 거리가 너무 짧은 경우

 

  그렇다고 거리가 너무 짧아 진다면 전후진을 하기 위해 각도를 크게 바꿔야한다는 단점이 있습니다. 그러므로 모터의 회전속도가 충분하지 않는다면 전진 속도가 느려지며, 저렴한 서보 모터의 경우 회전 가능 각도가 나오지 않아 전진 보행을 할 수 없을 수 있습니다. 

 

3.2. 보행에 유리한 다리의 각 관절 각도 설정

  저는 발의 X좌표를 0으로 하여 어깨의 바로 아래에 오도록 설정하였습니다. 그 이유로 보행(Gait)의 궤적을 설정할때 조금더 편하게 하기 위함입니다. 전진 범위와 후진 범위를 대칭 즉 같게 하여 보행의 궤적을 설정할 때 변수를 줄일 수 있습니다. 

 

4. 보행을 하기 위한 발의 움직임 계산 (Gait Trajectory Planning) 

4.1. 발의 좌표계 설정 

Yertle 로봇의 발 좌표계

  

    보행을 하기 앞서 발을 어떻게 움직일지를 수학적으로 계산하여야합니다. 그전에 발의 좌표를 표현하기 위한 좌표를 먼저 설정하여야 좌표 값(x, y)에 따라 발의 방향이 결정되기 때문입니다. 

 

  저희는 어깨를 중심를 기준으로 발의 위치를 좌표로 나타내였습니다. 그리고 X 좌표는 발의 전후진 방향을 나타내고 Y 좌표는 발의 높이를 나타내도록 하였습니다.

• X 좌표 : 발의 전후진 방향
  X 증가 : 발이 뒤로 후진
  X 감소 : 발이 앞으로 전진
  
  
• Y 좌표 : 발의 높이
  Y 증가 : 발을 밑으로 내림
  Y 감소 : 발을 위로 들어 올림

그리고 이를 바탕으로 발의 궤적을 수학적으로 나타내도록 하겠습니다. 다행스럽게 Yertle에서는 이미 수학적으로 발의 궤적을 계산을 해놓았으므로 해당 코드를 리뷰하면서 어떻게 발의 궤적을 계산하였는지 확인하였습니다.

 

4.2. Yertle 기존 프로젝트에서 보행 궤적 코드 리뷰

 위 좌표계를 바탕으로 발의 궤적을 수학적으로 나타내도록 하겠습니다. 다행스럽게 Yertle에서는 이미 수학적으로 발의 궤적을 계산을 해놓았으므로 해당 코드를 리뷰하면서 어떻게 발의 궤적을 계산하였는지 확인하였습니다.

 

https://github.com/MainForm/yertle/blob/main/software/yertle_ui/yertle_ui.py#L1006

        def gaitCalc(self,time):

            self.lf_gaitOffset[1] =  clamp(- self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)
            self.rf_gaitOffset[1] =  clamp(  self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)
            self.lb_gaitOffset[1] =  clamp(  self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)
            self.rb_gaitOffset[1] =  clamp(- self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)

            self.lf_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((math.pi/2)+math.radians(time))
            self.rf_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((-math.pi/2)+math.radians(time))                  
            self.lb_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((-math.pi/2)+math.radians(time))              
            self.rb_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((math.pi/2)+math.radians(time))

            self.lf_gaitOffset[2] =  self.parent.gui.gaitWindow.Slider4.get()*math.sin((-math.pi/2)+math.radians(time))
            self.rf_gaitOffset[2] =  self.parent.gui.gaitWindow.Slider4.get()*math.sin((math.pi/2)+math.radians(time)) 
            self.lb_gaitOffset[2] =  self.parent.gui.gaitWindow.Slider4.get()*math.sin((-math.pi/2)+math.radians(time)) 
            self.rb_gaitOffset[2] =  self.parent.gui.gaitWindow.Slider4.get()*math.sin((math.pi/2)+math.radians(time))

 

여기서 각 인덱스가 의미하는 것은 x,y,z 좌표입니다.

• X 좌표(0번 인덱스) : 발의 전후진 방향
• Y 좌표(1번 인덱스) : 발의 높이
• Z 좌표(2번 인덱스)  : 발의 좌우 방향

여기서 우리는 전진만 살펴볼 예정이므로 Z좌표는 사용하지 않겠습니다.

 

그리고 위 코드에서 볼수 있듣이 trot gait을 구현하기 위해 LF(Left Front)와 RB(Right Back), RF(Right Front)와 LB(Left Back)이 서로 쌍을 이루어 180도의 위상차를 두어 궤도를 설정한 것을 확인할 수 있습니다.

 

4.2.1. 발의 전후진(X좌표) 궤적 계산

self.lf_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((math.pi/2)+math.radians(time))
self.rf_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((-math.pi/2)+math.radians(time))                  
self.lb_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((-math.pi/2)+math.radians(time))              
self.rb_gaitOffset[0] =  self.parent.gui.gaitWindow.Slider3.get()*math.sin((math.pi/2)+math.radians(time))

 

  위 코드에서 알 수 있듯이 LF와 RB, RF와 LB가 묶여 같은 궤도로 움직입니다. 그리고 매우 단순한 sin 함수하나로 구성어 있습니다. 위 코드를 단순한 계산식으로 바꾸어 보도록 하겠습니다.

 

여기서 Slider3가 어떤것인지 알아봐야합니다. 

 

https://github.com/MainForm/yertle/blob/main/software/yertle_ui/yertle_ui.py#L638

self.Slider3 = tk.Scale(self.frm_gait,from_=-10,to=10,orient='horizontal',resolution=1)
#bMoveSlider3.bind("<ButtonRelease-1>",animateBodyTranslation)
self.Slider3.grid(row=2, column=1, sticky="nsew")

self.label3 = ttk.Label(self.frm_gait,text=' X Stride')
self.label3.grid(row=2, column=0, sticky="nsew")

 

  위 코드에서 Silder3에 대한 설명이 있는 Label3의 text를 보면 "X Stride"로 되어 있는 것을 확인 할 수 있습니다.  즉, X 축에 대한 보폭으로 Gait시 X축으로 얼마큼 움직일지 정하는 값입니다.

 

https://github.com/MainForm/yertle/blob/main/software/yertle_ui/yertle_ui.py#L1346

    def threadFunction(self):
        while True:
            if keyboard.is_pressed('up'): 
                self.parent.gui.gaitWindow.Slider1.set(30)
                self.parent.gui.gaitWindow.Slider2.set(4)
                self.parent.gui.gaitWindow.Slider3.set(7)
                self.parent.gui.gaitWindow.bMoveSlider4.set(3)
            elif keyboard.is_pressed('down'):
                self.parent.gui.gaitWindow.Slider1.set(30)
                self.parent.gui.gaitWindow.Slider2.set(4)
                self.parent.gui.gaitWindow.Slider3.set(-5)
                self.parent.gui.gaitWindow.bMoveSlider4.set(-3)

 

   좀더 확인해보면, 위 코드에서 'up'를 눌렀을때 그러니까 키보드 윗방향 화살표를 누른 경우 Slider3의 값을 7로 설정하는 것을 확인할 수 있습니다. 그리고 그 밑에 'down' 버튼을 눌렀을때는 slider3의 값을 -5로 하는 것을 확인 할 수 있습니다. 그러므로 'down'은 후진으로 짐작할 수 있습니다.

 

그렇다면 수식을 아래처럼 바꿀수 있습니다. 

이제 위처럼 변환된 수식을 얻을 수 있습니다. 이제 해당 수식에서 x_stride를 해석한다면 다음과 같습니다.

• x_stride
  해당 값은 sin함수의 진폭을 결정합니다. 
  즉, 발의 전후진의 거리를 결정합니다.

 

이제 위 함수를 그래프로 그리면 다음과 같습니다. 

여기서 중요한 것은 그래프의 기울기입니다. 그래프의 기울기에 따라서 발의 방향이 결정되기 때문입니다.

 

 

위 그래프를 확인해보면 다음과 같이 해석할 수 있습니다.

 

LF/RB는 0 ~ 180에서 발은 전진하고, 180 ~ 360에서 발은 후진하면서 반복하는 것입니다.

RF/LB는 0 ~ 180에서 발은 후진하고, 180 ~ 360에서 발은 전진하면서 반복하는 것입니다.

 

하지만 위 그래프에서 알 수 있듯이 sin 보다는 cos으로 변환하는 것이 수식이 더 간단해 질 것 입니다.

 

 

4.2.2. 발의 높이(Y좌표) 궤적 계산

self.lf_gaitOffset[1] =  clamp(- self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)
self.rf_gaitOffset[1] =  clamp(  self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)
self.lb_gaitOffset[1] =  clamp(  self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)
self.rb_gaitOffset[1] =  clamp(- self.parent.gui.gaitWindow.Slider2.get()*math.sin(math.radians(time)),-self.parent.gui.gaitWindow.Slider2.get(),0)

 

먼저 Silder2에 대한 값이 어떤것인지 알아보도록 하겠습니다.

 

https://github.com/MainForm/yertle/blob/main/software/yertle_ui/yertle_ui.py#L630

	self.Slider2 = tk.Scale(self.frm_gait,from_=0,to=5,orient='horizontal',resolution=1)
	#bMoveSlider2.bind("<ButtonRelease-1>",animateBodyTranslation)
	self.Slider2.grid(row=1, column=1, sticky="nsew")

	self.label2 = ttk.Label(self.frm_gait,text='Foot Elevation')
	self.label2.grid(row=1, column=0, sticky="nsew")

 

여기서보면 Silder2의 설명이 있는 Label2의 Text를 확인하면 "Foot Elevation"으로 발의 높이를 나타내는 값입니다.

 

https://github.com/MainForm/yertle/blob/main/software/yertle_ui/yertle_ui.py#L1345

    def threadFunction(self):
        while True:
            if keyboard.is_pressed('up'): 
                self.parent.gui.gaitWindow.Slider1.set(30)
                self.parent.gui.gaitWindow.Slider2.set(4)
                self.parent.gui.gaitWindow.Slider3.set(7)
                self.parent.gui.gaitWindow.bMoveSlider4.set(3)

 

 Yertle에서 전진을 위한 발의 높이(Foot Elevation)는 4로 설정되어 있습니다.

 

그러므로 발의 높이를 함수로 표현하면 다음과 같습니다.

위 함수에서 clamp라는 함수가 등장합니다. clamp는 함수의 결과값의 범위를 제한하는 함수입니다.

 

https://github.com/MainForm/yertle/blob/main/software/yertle_ui/yertle_ui.py#L1346

def clamp(n, minn, maxn):
    return max(min(maxn, n), minn)

 

  n이 minn 보다 작으면 minn의 값으로, n이 maxn 보다 크다면 maxn의 값으로 하여 값의 범위를 제한할 수 있습니다.

 

 이제 함수로 나타내면 다음과 같습니다.

 

해당 함수의 최대 값은 0, 최소값은 -Foot Elevation로 Y의 범위를 [-Foot Elevation, 0]로 제한합니다.

여기서 중요한 점은 하필 범위를 양수가 아닌 음수로 제한한 이유는 발을 들어올리는 방향으로 하기 위함입니다.

 

 

여기 위 그래프를 보면 알 수 있듯이 LF/RB, RF/LB가 서로 180도의 위상차를 가지고 있으며 마찬가지로 기울기에 따라 발의 방향이 결정됩니다.

 

 

 

위 그래프에서 알 수 있듯이 발의 높이가 순차적으로 변합니다.

• 0 ~ 90 : LF/RB 발을 들어올림
• 90 ~ 180 : LF/RB 발을 내림
• 180 ~ 270 : RF/LB발을 들어올림
• 270 ~ 360 : RF/LB발을 내림

이걸 위상에 따른 함수로 다시 표현하면 다음과 같습니다.

 

 

4.2.3. 전진 보행(Forward Gait) 의 궤적

 

  이제 위 XY축을 하나로 합치면 다음과 같은 궤적이 그려지게 됩니다.  발을 반달 모양으로 하여 회전하여 전진하도록 하는 궤적을 그리고 있습니다.

 

 

훨씬 보기 쉽도록 에니메이션을 그리면 다음과 같습니다.

  결국 사람의 걸음 걸이 처럼 한쪽으로 바닥을 지탱(Stance)하고, 나머지 한쪽은 발을 들어 회전(Swing)하여 전진하도록 하였습니다.

 

5. 결론

  Trot Gait으로 Yertle이 어떻게 전진하는지를 확인해보았습니다. 하지만 여기서 중요한 점이 있습니다. 위 그래프를 통해서 다음으로 움직여야할 다리의 좌표를 알 수 있지만, 발의 위치는 X,Y로 결정되는 데카르트 좌표계를 통해 위치가 결정되지 않습니다. 각 관절의 각도와 거리로 결정되는 극좌표계로 변환하는 과정이 필요합니다. 그러므로 다음 포스트에서는 위 발의 궤적을 극좌표계로 변환하는 역기구학(Inverse Kinematics)을 하기 위한 방법에 대해서 알아보겠습니다.