정리노트

Stewart Platform Impedance Control 본문

잡다한 정리/Project 관련

Stewart Platform Impedance Control

Krex_Kim 2022. 7. 10. 23:23

Stewart Platform으로 Impedance Control을 적용할 일이 있어서,

제어하면서 필요했던 내용을 간단하게라도 정리해보고자 포스팅하게되었습니다

 

 

 

 

 

◆ 목차

◎ Stewart Platform

◎ 전체 제어구조

◎ Dynamic Controller

   ○ Dynamic Controller

   ○ Impedance Force

   ○ Merge

◎ Other Nodes

   ○ Force to Torque Mapping

   ○ Forward Kinematics

 

 

 

 

 

 

 

 

◎ Stewart Platform

 

Stewart Platform은 위와같이 생긴 구조의 Manipulator를 말하며,

보통

상판 - Spherical Joint(3자유도) - Linear Actuator - Universial Joint(2자유도) - 하판

의 구조를 띄고있습니다.

 

Stewart Platform은 

주로 힘을 내야하는 방향(상판에 수직한방향)과 모터 방향이 비슷한 방향을 하고있기 때문에

단순한 Mechanism에도 6자유도에 가반하중이 매우 높으며 (High Roberstness),

견고한 Control이 가능하다는 장점이 있습니다.

 

 

 

 

◎ 전체 제어구조

 

가장먼저 진행한 작업은

전체적인 제어구조를 짜는 일이었고,

 

일반적인 로봇 Arm Manipulator에서는

Force Control를 어떤 형태로 적용하고 있는지 먼저 살펴보았습니다.

 

 

 

그 결과,

아래 논문에서 많은 힌트를 얻을 수 있었는데요,

 

https://www.researchgate.net/profile/Ruth-Canahuire/publication/347119124_Sensorless_Impedance_Control_for_the_UR5_Robot/links/61106834169a1a0103ea070d/Sensorless-Impedance-Control-for-the-UR5-Robot.pdf?origin=publication_detail 

 

 

모터 엔코더로 피드백된 Position 정보를 통해 

UR로봇을 Impedance Control하는 논문이며,

 

전체적인 제어구조는 아래와 같은 형태를 띄게됩니다.

 

 

위 구조를 간단히 정리해보면,

 

 

*** Cartesian Space = Task Space = End effector가 위치하는 유클리드 공간 x,y,z,rx,ry,rz

 

<순서>

 

1, Controller에서 "현재의 Cartesian Space에서의 Position"Feed Back 받아서

"Desired Position으로 가기 위해 각 Actuator에서 내야하는 힘" 을 계산

 

2, Forward Dynamics에서 Joint Space Force를 Cartessian Space에서의 Acceleration으로 바꿈

 

3, Cartesian Space에서의 Acceleration을 두번 적분하여 Cartesian Space에서의 Position으로 전환

 

4, Inverse Kinematics(a.k.a I.K Solver)를 이용해

"Catesian Space 에서의 Position"을 "Joint Space 에서의 Position"으로 변환

    -> Linear Actuator인 경우, 길이가 될 것이고, 회전모터일 경우 회전각

 Joint Position값을 모터드라이버에 input

 

5, 모터 드라이버는 Position 값을 모터에 인가하고, 

모터 엔코더로부터 현재 모터의 Actual Position을 Forward Kinematics(a.k.a F.K Solver) 노드에 전달

 

6, Forward Kinematics를 이용해 현재의 Actual Joint Position을 Actual Cartesian Position으로 전환,

Controller 노드에 Feedback

 

 

> 위 과정을 반복

 

제가 제어하는 매니풀레이터는 Open Chain의 로봇팔이아니라,

Closed Chain의 Stewart Platform 매니풀레이터라서 'Controler'의 구조가 달라지는 점만 빼면

전체적인 제어흐름은 위와 똑같이 구성됩니다.

 

 

 

* UR 로봇의 경우, 모터 드라이버가 받아들이는 값이 Position이기 때문에,

Forward Dynamics를 통해 Joint Space의 Force를 Cartesian Space에서의 Acceleration으로 바꿔주고,

이를 두번 적분하여 Position으로 바꿔준후 다시 I.K Solver를 이용해 Joint Space에서의 Position으로 바꿔주는 

작업이 필요했습니다.

 

하지만 제가 제어할때 사용해야하는 모터드라이버에서는 input값이 Torque값이어서,

굳이 Forward Dynamics, IK Solver를 이용한 험난한 변환과정이 필요하지 않았습니다.

(다만 이렇게 할 경우 모터 드라이버 제어가 훨씬더 어려워지더라구요..... 안하는 데는 이유가 있었습니다)

 

 

위 그림을 힌트삼아

제가 짜낸 제어구조 그림을 아래와같이 표현해 보았습니다

 

Joint Force에서 그대로 Joint Torque로의 변환만 해주면 되는 구조로 더 단순해졌음을 알 수 있습니다.

 

여기서 Dynamic Controller가 전체 제어구조에서 핵심이라고 볼 수 있는데,

 

Dynamic Controller는

Joint Space의 각 Linear Actuator가 내야하는 Force를 계산하기 위해서

 

원하고자 하는 Cartesian Space에서의 Position(P) , Velocity(P') , Acceleration(P'')과 (=줄여서 Desired P Set)

현재의 Cartesian Space에서의 Position(P), Velocity(P'), Acceleration (P'')  피드백값 (=줄여서 Actual P Set)

 

을 input으로 합니다.

 

그리고 Feed Back 값에서의 Velocity(P')와 Acceleration(P'')값은 F.K Solver에서 나온 Output을 Sampling Rate로 미분해줌으로서 구현했습니다.

 

 

 

 

 

 

◎ Dynamic Controller

 

 Dynamic Controller

 

Dynamic Controller는 아래 논문을 참조해서 만들었습니다

https://www.researchgate.net/publication/220103568_Kinematic_and_dynamic_analysis_of_Stewart_platform-based_machine_tool_structures

 

위 논문에서

Inverse Kinematics, Jacobian Mapping, Inverse Dynamics를 차용하였으며,

 

Cartesian Space {x,y,z, Rz1, Rx, Rz2} (Orientation쪽은 논문에서 ZXZ Euler Angle을 사용함)

에 대하여

Positiion, Velocity, Acceleration값을 input으로 넣어주었을때,

설정된 중력에 대하여 Gravity Compensation하는 데 필요한 Force를 계산해주는 Node를 구현하였습니다.

 

(* 일단 따라해서 넣은거라, 구체적인 계산과정은 추후 여유가 생긴다면 정리해보도록 하겠습니다)

 

 

 

○ impedance Force

위 Dynamics는

Actual (P,P',P'') Set에 대해서 Manipulator가 내야하는 힘을 표현할때 사용되는 것이라고 할 수 있습니다.

 

실제 Desired (P,P',P'') Set으로 Manipulator를 움직이게 하기위해서는

impedance Control을 적용해주어야 하는데,

 

가장 일반적인 mck Modeling을 적용하였습니다.

 

가상의 Spring 과 Damper가

Manipulator 끝자락에 연결되어있다고 생각하고 이때 필요한 Force를 mck Modeling을 통해 계산하게됩니다.

-> Cartesian Impedance Control

 

이를 그림으로 표현하면

 

 

 

○ Merge 

Controller 노드의 최종 Output은 Joint Space Force입니다.

 

그런데 Impedance Force는 Cartesian Space Force이므로,

Dynamic Modeling에서 구해줬던 Jacobian을 이용해 Cartesian Space -> Joint Space Mapping을 진행하여 합쳐줍니다.

 

 

 

위와같은 mapping관계를 이용해서 아래처럼 수식을 정리해주면,

각 Joint에서 내야하는 Force값이 완성되었습니다

 

◎ Other Nodes

 

○ Force to Torque Mapping

원래대로라면 모터의 Dynamic 모델링을 하나하나 풀어서 해당 Force를 내려면

얼마만큼의 Current를 주어야하는지 일일이 계산해야 정확히 할 수 있는 부분인데요,

 

다만 여기서는 이 과정이 너무 복잡하고 시간적 여유가 없어서

일단 Force 와 Torque는 1차 Linear Mapping관계라고 생각하고 Heuristic하게 그냥 기울기를 찾아서 넣어주었습니다.

 

 

○ Forward Kinematics

Stewart Platform은 Closed Chain 구조의 매니풀레이터라서,

Open Chain인 일반적인 Kinematics Solution과 달리

F.K 의 해를 구하는 것이 I.K의 해를 구하는 것보다 훨씬더 어려우며,

Singular 발생지점또한 F.K는 40군데나 존재합니다.

 

이 때문에 보통의 Open Chain Manipulator에서 구했던 방식과 다르게

Newton Raphson 방식의 Solution을 I.K가 아니라 F.K에서 사용합니다.

 

 

 

이와 관련해서는 아래에

좋은 Open Source Reference가 있어서 이를 참조하여 노드를 구성하였습니다.

 

https://jak-o-shadows.github.io/electronics/stewart-gough/stewart-gough.html

 

Stewart-Gough Platform - Forward & Inverse Kinematics -- Jak-o-Shadows

WORK IN PROGRESS - NOT ALL DIAGRAMS COMPLETED, ETC Kinematics The forward and inverse kinematics of a Stewart-Gough platform are derived, and code created (available to download). Derivation The forward and inverse kinematics of a stewart-gough platform ar

jak-o-shadows.github.io

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

 

Dynamics Controller를 만들때 검증용으로 짠 Stewart Platform 매트랩 코드

https://github.com/krex4715/stewart_platform_Dynamics

 

GitHub - krex4715/stewart_platform_Dynamics: Stewart Platform Inverse Dynamics MATLAB Code

Stewart Platform Inverse Dynamics MATLAB Code. Contribute to krex4715/stewart_platform_Dynamics development by creating an account on GitHub.

github.com

 

Comments