Craig의 Robotics 4장 예제. PUMA 560의 역기구학 풀이 (Inverse Kinematics)

여러가지 기초스러운 것들을 즐거운 마음으로 학습할때(^^) 자꾸 하나에 집중하지 못하고 이것저것하는 나쁜 버릇을 제가 가지고 있는데요.ㅠㅠ. 아무튼 여러가지를 요즘 공부하는데 그 중 하나가 Craig의 Introduction to Robotics 3rd Edition을 가벼운 마음으로 읽고 있는데요. 지난번에 PUMA 560의 정방향 기구학(direct kinematics)을 Python으로 보이면서 학습했습니다.^^[바로가기] 이번에는 역기구학으로 4장을 공부한거죠^^. 각 조인트의 각도를 가지고 맨 끝단 - 작업 툴의 위치와 방위를 알 수 있는 것이 정방향 기구학(direct kinematics)이라면 반대로 끝단의 방위와 위치를 정해놓고 각 조인트의 각도를 알려는 것이 역방향 기구학(inverse kinematics)랍니다. 사실 저는 실제 실무에서는 그냥 역행렬을 바로 구하고.. 역행렬이 존재하지 않는 상황에 대한 구속 조건만 따지면 되는 환경이었기 때문에 이걸 손으로 푸는 것에는 관심이 없었습니다. 그런데 이번 리뷰를 하면서 살짝 손으로 푸는 것도 해보고 싶었지만.... 다 하지는 않고^^ python sympy의 symbolic 연산의 힘도 빌렸습니다^^

출처 : Craig의 Introduction to Robotics 3rd Edition

위 그림이 PUMA 560이지요. 저기서 

출처 : Craig의 Introduction to Robotics 3rd Edition

각 조인트를 정의하고 시작했었습니다.

그렇게 해서 지난번[바로가기]에 정방향 기구학에 대한 허접한(^^) 시뮬레이션도 해 보았는데요. 이번에는 거꾸로(inverse) 가야죠~^^

그 때 사용했던 link parameter였습니다.

그걸 이용해서 각 축의 정방향 변환행렬들을 정의했습니다.

그리고 당연히 끝단의 위치와 방위에 대해 정의해서 등가로 잡았네요. 이제 각 조인트단의 각도를 따라가봐야죠....

이후 나오는 모든 수식 중 캡쳐를 뜬듯한 어색한 수식 그림은 모두 출처가 Craig의 Introduction to Robotics 3rd Edition입니다.

먼저 최초 식의 양번에 Trans_0to1의 역행렬을 곱할겁니다. 그것도 손으로 구하기 귀찮아서 그냥 Python Sympy의 심볼릭 연산을 사용했습니다.ㅠㅠ.

위 식에 대입해서 정리하는거죠.

당연위 위와 같이 될겁니다. 우변은 Trans_1to6가 되겠죠.

이게 교재아 이야기 하듯이 양변의 (2,4)번 요소를 확인합니다. Python은 0부터 인덱스가 시작해서 [1,3]이라고 표기가 되었습니다.

그러면 위 수식 하나를 얻을 수 있습니다.

일반적인 삼각함수의 풀이에서 위와 같이 표현을 하고...

나면...

이렇게 완성되죠...

그러면 phi-theta1의 sin, cos 식을 얻었네요. 그러면 이를 tan로 두고 arctan를 사용하면 각도를 표기할 수 있게 됩니다.

위키백과의 atan2의 정의

그런데 통상 atan2를 많이 사용하니 이를 이용해서 표현하면...

가 되네요....이제 phi는 위에서 정의해 두었으니...

theta1을 구했습니다.

이번에는 아까 theta1 구하기때 시작단계에서 도출했던 식의 (1,4)번째 요소를 확인하면

이번에는 위 식을 얻을 수 있습니다.

또, (3,4)번째 요소를 확인하면 

를 얻을 수 있습니다. 그런데 3rd Edition의 오타가 아닐까 하는데요. 저는 좌변에 Pz여야 한다고 나오더라구요. 뭐 아무튼...

오타라고 (저는 믿고 있는) Pz를 반영해서 두 식을 변변 제곱해서 더하고 난 후, K를 위와 같이 잡으면...

이 됩니다.

그러면... 살짝꿍 Atan2를 이용해서 theta3를 구할 수 있습니다.

Trans_3to6를 구해서 계산한 결과에서 (1,4), (2,4)번째 요소를 확인하려고 했죠.. 그런데 문자연산의 문제가 원하는데로 이쁘게 정리된 형태를 얻기가 쉽지 않다는 거죠^^....

위 식의 우변은 방금 위에서 Python으로 확인했지만, 좌변은 간단히 손으로 구하는 것이 좋습니다.

그래서 양 변의 (1,4), (2,4)요소를 정리해서 위 두 식을 얻을 수 있습니다.

저 식으로 정리를 했습니다. 목적은 tan로 theta23을 표현할려고 하는 겁니다.

아까 수식의 양변을 s23와 c23에 관해 정리해서 tan로 표현할 수 있습니다.

이렇게 말이죠^^

그리고 위와 같이 Atan2로 표현하면 위와 같이 theta2+theta3를 구하게 되었네요... 그럼 아까 theta3는 구해두었으니...

이 몹시 기초스러운 수식으로 theta2를 구할 수 있습니다.^^

이제 위 그림처럼 양변에 Trans_0to3의 역행렬을 곱해서 (1,3), (3,3) 요소를 확인하면

위 두 식을 얻을 수 있습니다.

그리고 sin(theta5)가 0가 아니라는 조건을 달 수 있다면 ...

theta4를 구할 수 있게 됩니다.

이제 양변에 Trans_0to4의 역행렬을 곱하고....

sin(theta5)와 cos(theta5)를 구하면 tan로 표현이 가능해서 theta5를 구할 수 있게 됩니다. 위 문자연산의 결과를 보면 (1,3)과 (3,3) 요소를 보면 되는 거죠...

그죠?^^

그러면 위 Trans_0to4의 역행렬에서 해당 위치를 찾아서

이렇게 식을 완성할 수 있습니다.

역시 같은 방법으로 theta5를 구할 수 있다는 군요^^

마지막까지 역행렬을 곱하고 theta6 찾으러 가야죠...

저기 보이네요...^^

이렇게 되면 tan를 사용할 거라는 의도가 나오는거죠...

theta6를 구했습니다. ㅎㅎㅎ^^ 사실... 처음 부분에서도 말씀드렸지만... 저는 이걸 손으로 푸는 환경이 아닙니다만... 그래도 한 번쯤은 요정도를 이렇게 힘들게(^^) 손으로 푸는 것도 재미있지 않을까하고 시작해 보았습니다.^^. 흠.. 역시 그냥 공부는 중독성이 있어요.. ㅋㅋㅋㅋ