UR5 역기구학 코드 작성(step2 기구학 코드 작성)

정기구학 코드를 모두 작성하셨다면 

이제 역기구학 코드를 작성해봅시다. 

 

가장 먼저 역기구학 코드의 입력에 알아봅시다. 

역기구학의 코드는 4x4 행렬의 형태가 될 수도 있고 6x1 백터 꼴이 될 수 있습니다. 

 

4x4 행렬이라면 아래 사진과 같은 정보를 가지고 있을 거에요 

행렬안에 행렬과 벡터가 있는 notation입니다. 

R행렬은 3차원 회전행렬이고 P백터는 cartesian 좌표계 3차원 위치 백터입니다. 

 6x1 백터 꼴이라면 cartesian 좌표계 3차원 위치 백터와 RPY혹은 euler angle을 포함한 백터일 것입니다. 

 

이제부터 입력으로 부터 역기구학을 하는 과정을 알아봅시다.

입력은 4x4 행렬이라고 가정합니다.(백터 꼴 입력이라면 행렬로 변환하는 과정을 거쳐야 합니다.)

 

입력을 받은 이후에 로봇 각 관절 모터의 회전각을 순서는 1, 5, 6, 3, 2, 4번 관절 순으로 진행한다. 

앞서서 나오는 수식 및 코드는  DH 파라미터가 달라지는 순간 구하는 수식은 물론 순서도 달라질 수 있음에 유의한다. 

 

첫 번째 모터각을 구하는 수식은 다음과 같다. 

T0_6이 역기구학 4x4 행렬 입력이다. (제 코드는 Q0_6으로 되어 있습니다.)

P0_5는 5번째 관절 카테시안 좌표계 위치 벡터인데 아래의 수식을 통해 구할 수 있다. 

 

 

P0_5를 구했다면 아래와 같은수식을 통해 theta1을 구해주면 된다. 

 

 

다음은 theta5를 구할 차례이다. 

theta5구하는 수식은 아래와 같다. 

T0_6을 알고 theta1을 알아냈으니 T1_6을 알아낼 수 있다.(제 코드는 Q를 T로 보시면 됩니다.)

필요한 변수를 알아낸 뒤 아래와 같이수식 작성해주면 theta5도 구할 수 있다. 

다음은 theta6를 구할 차례이다.

T6_1은 T1_6의 역행렬을 취해서 구하면 되고 

T6_1의 z_y성분과 z_x 성분을 이용하여 theta6를 구할 수 있다. (T행렬의 성분 헷갈리면 첫 번째 사진 참고)

 

다음은 theta3를 구할 차례이다.

theta1, 5, 6을 알고 T0_6을 아니까 (theta로 어떻게 T matrix 구하는 지 모른다면 DH 파라미터로 정기구학 구하는법을 다시 공부)

T1_4을 구할 수 있다. 

T1_4를 구한다음 P1_3은 아래 수식을 통해 구할 수 있다.  

P1_3정보와 DH 파라미터 정보를 통해서 Theta3을 구할수있다.   

P1_3의 크기는 P1_3의 2차 norm을 제곱한 것이랑 동일하다. (2차 norm은 백터 각 성분을 제곱하여 더한 뒤 제곱근을 씌운 것 )

 

theta2는 앞서 구했던 정보만 이용하면 바로 구할수 있다.   

theta4는 T3_4의 x_y, x_x 성분을 사용하여 구할 수 있다.

 

이러면 역기구학 코드는 완성!

 

자료를 찾아서 수식 그대로 코드를 작성한 거라 간단합니다. 

사실 로봇 디자인에 따라서 코드가 바뀌기 때문에 DH 파라미터가 바뀌면 코드도 바뀝니다. 

geometrial method의 원리가 자세히 알고 싶다면 원래 문서를 자세히 읽어보시는 걸 추천드립니다.   

https://tianyusong.com/wp-content/uploads/2017/12/ur5_inverse_kinematics.pdf