on
[Robotics] Inverse Kinematics (역기구학)
[Robotics] Inverse Kinematics (역기구학)
목적
역기구학을 해석하는 방법 2가지 Analytic, Numerical 중
Jacobian을 이용한 역기구학을 해석하여 Target Position을 set하는 제어 알고리즘 공부~
순서
I. 로봇좌표계
II. 정기구학 VS 역기구학
Forward / Inverse
III. Jacobian Matrix (자코비안 행렬)
IV. 제어 알고리즘으로 구현
I. 로봇 좌표계(Kinematics)
항상 교과서에서는 2축, 3축 링크로만 봤지만 실제 로봇 좌표는 다음과 같습니다.(UR5e)
좌측의 6개 좌표계를 요리조리 해석하면 DH Parameter도 뽑을 수 있고
이 DH를 기반으로 Homogeneous Matrix를 만들어서
Tool좌표(TCP)를 구합니다.
--> 0축부터 6축 끝단까지의 전체 Rotation & Transpose Matrix
SDH Table / HomoGen Matrix
따라서 모든 기구학을 풀기 위해서 가장 기본이 되는 세트가 됩니다.
II. 정기구학 VS 역기구학
Forward Kinematics(FK) / Inverse Kinematics(IK)
우리가 로봇을 구동할 때 로봇의 각 축에는 모터-엔코더-모터 드라이브가 있어
원하는 각도로 움직일 수 있습니다.
하지만 각 Joint의 각도는 모터로 어떻게 제어해서 알 수 있다고 해도
6축 맨 끝단(이하 TCP)의 위치를 어떻게 알까요??
각도가 아닌 Cartesian (X,Y,Z)좌표일 텐데요.
이 때 FK 를 사용합니다.
즉, 각 Joint를 이용해 TCP의 Cartesian 좌표를 아는 것이죠 (Base 기준)
하지만 이번 글에서는 역 기구학이니 FK는 개념만 알고,
IK 는 반대로
TCP의 Cartesian 좌표를 통해 각 Joint의 값을 아는 것입니다.
예를 들어,
각 축의 각도가 (30,30,30,30,30,30)deg 일 때,
TCP의 위치 X Y Z(mm) RX Ry Rz(deg)가 (100,150,200,10,20,30) 임을 찾는 것은 FK,
그 반대의 경우는 IK입니다.
IK는 FK에 비해 굉장히 복잡합니다.
그 이유는 FK같은 경우에, 각 Joint의 값이 정해지면 주어진 TCP 위치는 유일하지만
IK의 경우 TCP의 위치가 정해져도 각 Joint의 조합이 다를 수 있고,
모든 Joint의 조합으로도 원하는 위치에 도달할 수 없는 상황이 발생하기에(Singular Pose)
해를 찾기가 굉장히 힙듭니다.
이렇게 어려운 IK를 풀기 위해 2가지 방법이 존재합니다.
1. Analytic : 기하학적으로 모든 수식을 풀어 Joint의 각도가 고정된다.
2. Numerical : 순차적으로 계산되어 해를 수치적으로 찾아낸다
이 중에서 Numerical 방법으로 구현하고자 Jacobian 행렬을 사용합니다.
III. 자코비안 행렬 (Jacobian Matrix)
자코비안 행렬은 다음과 같이 표현됩니다.
q : Angular p : Cartesian
다음 식에서와 같이 Cartesian 좌표계 (직각좌표)의 속도를 로봇 Joint의 속도로 변환시켜줍니다.
그렇다면 우리가 원하는 Joint의 값은 다음과 같이 됩니다.
여기서 자코비안을 통해 Numeric하게 계산할 수 있는 이유가 나오게 되는데,
바로 속도라는 점 입니다.
우리가 원하는 q를 얻으려면 q_dot에 시간을 곱해야하고, 이는 결국 적분값이므로
매 Iteration이 지날 때 마다 누적해주며 원하는 Joint Angle을 찾아내는 방법이 됩니다.
물론 이 과정에서 타겟 위치는 고정인 상태로 로봇의 현재 위치값은 계속 업데이트 됩니다.
IV. 제어 알고리즘으로 구현
1. Target 위치 S (고정)를 받아온다.
2. Target S- Actual S로 Error값을 구한다. (위치제어라면)
2.1 data Update Cycle에 따라 속도 Error를 사용한다면 속도제어 ....등등
3. PID제어를 통해 에러값을 추종한다. (P, I, D 원하는 제어 조합)
4. 다음 Cycle의 Joint Position Q로 Update 한다. (FK 풀어서 이동)
5. (2~4) 과정을 반복한다.
여기서 어려운점은 사실 Jacobian을 구하는 알고리즘 정도....??
#include #include #include #include using namespace Eigen; double dt; double pidOutput; Vector6D CartTarget; Vector6D ActualCart; Vector6D CartError; Vector6D CartVelError; Vector6D QDot; Vector6D JointTarget; int main() { dt = 0.002; double P,I,D; // 적당한 값으로 튜닝해야함 JointTarget = getCutJoint(); while(1) { usleep(dt); //2ms update CartError = CartTarget - CartActual; // 현재 위치와의 에러값 CartVelError = CartError / dt; // I 제어는 적분이므로 매 Cycle마다 Error를 누적해준다. // D 제어는 미분이므로 가속도 필요 없어서 PI제어만 해준다. VelErrorSum += CartVelError * dt; pidOutput = P*CartVelError + I*VelErrorSum // P*err + I * Integral(err*dt) QDot = InverseJacob() * pidOutput; JointTarget += QDot*dt ForwardKinematics(JointTarget) } return 0; }
다음 코드는 굉장히 간소화 시켰는데,
추가적으로 필요한 것은,
1. 속도 데이터에 대한 Filter를 통해 급격한 움직임이 없도록 해야함.
2. InverseJacobian을 풀어야 함( Singular를 회피하고 싶다면 Sudo Inverse로...)
3. 마지막에 QDot*dt의 update된 각도로 초기 위치부터 누적시켜 FK 풀어줌.
궤적을 이용한 속도 프로파일 생성은 그 다음에 생각하자...
from http://aronglife.tistory.com/77 by ccl(A) rewrite - 2021-03-19 02:00:38