티스토리 뷰
Forward Kinematics
이번 장은 3장에서 학습한 screw axes와 PoE를 이용하여 매니퓰레이터의 FK를 구하는 방법에 대해서 학습해보려고 한다. FK(Forward Kinematics, 순기구학, 정기구학)는 end-effector의 position과 orientation을 각 조인트의 θ를 통해 구하는 것이다. 기존 Craig의 Robotics는 D-H파라미터를 통해 FK를 구하는 게 일반적이었지만, 요즘 논문은 거진 다 modern robotics의 PoE로 설명되어 있다.
θ1,θ2...θn→(x,y,ϕ)

기본적인 trigonometry개념으로도 2차원의 open chain인 3R planar는 아래의 식과 같이 쉽게 풀 수 있다, 그럼에도 불구하고 이렇게 간단해 보이는 2차원 3R planar의 IK는 굉장히 까다로워진다. 하지만 모던로보틱스에서 제시하는 PoE의 개념을 적용하게 되면, 쉽게 FK와 IK를 모두 구할 수 있게 된다.
x=L1cosθ1+L2cos(θ1+θ2)+L3cos(θ1+θ2+θ3),
y=L1sinθ1+L2sin(θ1+θ2)+L3sin(θ1+θ2+θ3),
ϕ=θ1+θ2+θ3
PoE를 이용하여 Forward Kinematics를 구하는 순서는 다음과 같다.
- 모든 Joint 각도를 0으로 고정한 뒤 body frame의 position과 orientation을 구한다. M∈SE(3)
- 하나의 Joint를 제외 한 나머지 Joint의 각도를 0으로 유지(고정)한 뒤, 지정한 Joint를 1 rad/s으로 회전 시켰을 때 origin에서 발생하는 선속도를 구한다. ω
- Screw Axis의 v 를 구하는데 두가지 방법이 있다.
- 회전하고자 하는 조인트를 제외한 나머지 조인트의 각도를 0으로 고정한 뒤, ω를 1 rad/s으로 회전시켰을 때, origin이 회전하는 선속도를 직관적으로 구할 수 있다.
- 두 번째로, algebraically 하게 origin부터 회전하고자 하는 joint까지의 거리 벡터 q를 구한 뒤, v=ω×(−q)를 통해 구 할 수도 있다.
- 구해진 Screw axis S는 se(3)의 matrix로 표현이 가능하며, 여기서 [w]는 skew-symmetric matrix이다.
[S]=[[w]v03x10]∈se(3) - 구해진 matrix를 활용하여 screw motion의 matrix exponential로 표현하면 아래의 식으로 표현이 가능하다.
e[S]θ=[e[ω]θG(θ)v03x11]e[ω]θ=I+sinθ[w]+(1−cosθ)[ω]2G(θ)=θI+(1−cosθ)[ω]+(θ−sinθ)[ω]2 - 결론적으로 각 조인트의 Screw axis (ω,v)를 통해 아래와 같은 식을 만들어 Transform matrix를 구할 수 있다.
T04=e[S1]θ1e[S2]θ2e[S3]θ3M
Product of Exponentials Formula
PoE는 두 가지 방법으로 FK를 구현할 수 있다. (Screw Axes in the Base frame, Screw Axes in End effector frame).
그전에 먼저, PoE 공식을 이용하기 위해서 다음과 같은 작업을 해줘야 한다.
- Assign a stationary frame {s} at the fixed frame and body frame {b} at the end-effector.
- Describe M, when the robot is at its zero position.
- Use frame {i} for links i at joints i
First Formulation : Screw Axes in the Base Frame
FK in Space(fixed) Frame에 필요한 elements와 공식
- End-effector configuration M∈SE(3) when the robot at its home(zero) position. (Space Frame에서 구한 M)
- Screw axes S1,S2,⋅⋅⋅,Sn expressed in the fixed frame. (해당 조인트를 돌렸을때 space frame의 origin을 기준으로 나오는 선속도)
- Joint variables : θ1,⋅⋅⋅,θn.
- FK in Space : T=e[S1]θ1⋅⋅⋅e[Sn]θnM
Second Formulation : Screw Axes in the End-Effector Frame
FK in Body Frame에 필요한 elements와 공식
- End-effector configuration M∈SE(3) when the robot at its home(zero) position. (Space Frame에서 구한 M)
- Screw axes B1,B2,⋅⋅⋅,Bn expressed in the body frame. (해당 조인트를 돌렸을때 body frame의 origin을 기준으로 나오는 선속도)
- Joint variables : θ1,⋅⋅⋅,θn.
- FK in Space : T=Me[B1]θ1⋅⋅⋅e[Bn]θn
The Universal Robot Description Format (URDF)

URDF는 ROS에서 사용되는 포맷으로 XML문법을 따르게 되어 있다. 로봇의 Kinematics, inertial properties, link geometry가 표현되어 있고 기본적인 구성요소(노드)로 Joint와 Link가 있다. 위 그림을 보면 금방 파악 가능하다.
- Joint : 두 링크를 연결하는 부분으로 로봇의 Kinematics를 표현하며, parent link와 child link를 넣어줘야 한다.
- Link : 링크는 로봇의 mass, origin, inertia 등 mass properties를 넣어서 구성해야 한다.
마지막으로 간단한 코멘트를 넣어보자면 :
1. Craig책에서 설명한 방법이 익숙하고 편 할 수 있지만, POE를 통해 FK/IK를 할 때 Screw Axis만 구하면 되기 때문에, 훨씬 쉽고 접근성이 좋으니 꼭 숙지해놓으면 좋을 것 같다.
2. Craig의 Robotics책도 마찬가지지만, FK/IK는 직접 손으로 많이 풀어볼수록 이해에 도움이 되고 Kinematics에 친밀해지는 거 같다. 코드로 구현할 때에도 손으로 많이 풀어본 경험이 도움이 많이 되었던 기억이 있다.
- Total
- Today
- Yesterday
- odom
- Jacobian
- Forward Kinematics
- LaTeX
- Kinematics
- PID CONTROL
- Mobile Robot
- odometry
- NumericalComputation
- inverse kinematics
- trapezoidal
- Configuration Space
- Robotics
- optimal control
- paper review
- git
- trajectory planning
- ORB
- ICP
- wrench
- visual slam
- lqr control
- Localization
- control
- Slam
- repeatability
- manipulator
- Modern Robotics
- Screw
- Twists
일 | 월 | 화 | 수 | 목 | 금 | 토 |
---|---|---|---|---|---|---|
1 | 2 | 3 | 4 | 5 | ||
6 | 7 | 8 | 9 | 10 | 11 | 12 |
13 | 14 | 15 | 16 | 17 | 18 | 19 |
20 | 21 | 22 | 23 | 24 | 25 | 26 |
27 | 28 | 29 | 30 |