Advanced Robotics
1 Basic Concepts
1.1 Configuration (构型)
Definition: A specification of the positions of all points of the robot. 机器人上所有点的位置的表示方法。
For example:
- Using angles to represent the configuration of a revolving door.
- Use coordinates to represent the configuration of points on a 2D plane.
1.2 Links
Definition: A robot mechanism is constructed by connecting rigid bodies, called links.
1.3 Joints
Definition: Makes the relative motion between adjacent links possible.
1.4 Configuration space (C-space)
Definition: The n-dimensional space containing all possible configurations of the robot.
Note: The configuration of a robot is represented by a point in its C-space.
1.5 Degrees of Freedom (dof)
Definition: The minimum number of real-valued coordinates needed to represent the configuration.
- Spatial rigid body has six degrees of freedom in 3D space.
- Planar rigid body has three degrees of freedom in 2D plane.
- DoF of rigid robots = (sum of freedoms of all bodies) − (number of independent constraints).
Below shows typical robot joints and their DoF:
1.6 Manipulator Mechanisms
Serial Manipulator (串联机械手): several bodies are serially connected as a chain (open kinematic chain).
For example: the normal robotic arm.
Parallel Manipulator (并联机械手): consist of several serial chains which usually support a single platform or end-effector (closed kinematic chain).
1.7 Fully Actuated vs Under-Actuated
Fully Actuated: all joints (DoFs) are active.
Under-Actuated: at least one joint (DoF) is passive.
PS: If a manipulator has more DOF than the required task, it is called redundant (冗余的).
1.8 Workspace
Definition: The set of points that can be reached by the end-effector.
Note:
- Two mechanisms with different C-spaces may have the same workspace.
- Two mechanisms with the same C-space may also have different workspaces.
2 Kinematics
2.1 Pose of a Rigid Body
First, to describe the pose of a rigid body, we need a coordinate system. Normally, there are two coordinate systems, Left-handed system and Right-handed system. An important difference is that when describing the angle of rotation, the direction of the two is different.
2.1.1 The Representation of Position
$O'$ is a point on the rigid body, which is expressed by the following relation (with respect to the coordinate frame $O-xyz$): $$ \vec{O'} = O'_x \vec{x} + O'_y \vec{y} + O'_z \vec{z} $$
where $O'_x, O'_y, O'_z$ denote the components of the vector $\vec{O'} \in \mathbb{R}^3$. $\vec{x}, \vec{y}, \vec{z}$ are vector.
Also, the position of $\vec{O'}$ can be compactly written as a 3 by 1 vector. $$ \vec{O'} = \left[ \begin{matrix} O'_x \\ O'_y \\ O'_z \end{matrix} \right] $$
2.1.2 The Representation of Orientation
In order to describe the rigid body orientation, it is convenient to consider an orthonormal frame attached to the body, and express its unit vectors with respect to the reference frame.
Let then $O'-x'y'z'$ be such a frame with origin in $O'$, and $\vec{x'}, \vec{y'}, \vec{z'}$ be the unit vectors of the frame axes. So we have $$ \begin{aligned} \vec{x'} &= x'_x\vec{x} + x'_y\vec{y} + x'_z\vec{z} \\ \vec{y'} &= y'_x\vec{x} + y'_y\vec{y} + y'_z\vec{z} \\ \vec{z'} &= z'_x\vec{x} + z'_y\vec{y} + z'_z\vec{z} \end{aligned} $$
2.2 Rotation Matrix
By adopting a compact notation, the three unit vectors in $O'-x'y'z'$ can be combined in the 3 by 3 matrix: $$ \begin{aligned} R &= \left[ \begin{matrix} \vec{x'} & \vec{y'} & \vec{z'} \end{matrix} \right] \\ &= \left[ \begin{matrix} x'_x & y'_x & z'_x \\ x'_y & y'_u & z'_u \\ x'_z & y'_z & z'_z \end{matrix} \right] \\ &= \left[ \begin{matrix} \vec{x'}^\mathsf{T}\vec{x} & \vec{y'}^\mathsf{T}\vec{x} & \vec{z'}^\mathsf{T}\vec{x} \\ \vec{x'}^\mathsf{T}\vec{y} & \vec{y'}^\mathsf{T}\vec{y} & \vec{z'}^\mathsf{T}\vec{y} \\ \vec{x'}^\mathsf{T}\vec{z} & \vec{y'}^\mathsf{T}\vec{z} & \vec{z'}^\mathsf{T}\vec{z} \end{matrix} \right] \end{aligned} $$
where $\vec{x'}^\mathsf{T}\vec{x}$ can be considered as the projected component (投影分量) of $\vec{x'}$ on $\vec{x}$， other elements are simile.
Note: $R$ has property: $R^\mathsf{T} = R^{-1}$, so we have $p = Rp' \rightarrow p' = R^\mathsf{T}p$
Hence, when the reference frame is rotated by an angle $\alpha$ about axis $x$,$y$ and $z$, the rotation matrix of frame $O–x'y'z'$ with respect to reference frame $O–xyz$ are respectively: $$ R_x(\alpha) = \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos\alpha & -\sin\alpha \\ 0 & \sin\alpha & \cos\alpha \end{matrix} \right] $$ $$ R_y(\alpha) = \left[ \begin{matrix} \cos\alpha & 0 & \sin\alpha \\ 0 & 1 & 0 \\ -\sin\alpha & 0 & \cos\alpha \end{matrix} \right] $$ $$ R_z(\alpha) = \left[ \begin{matrix} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{matrix} \right] $$
In addition, it is easy to verify that $$ \begin{aligned} R_x(-\alpha) = R_x^\mathsf{T}(\alpha) \\ R_y(-\alpha) = R_y^\mathsf{T}(\alpha) \\ R_z(-\alpha) = R_z^\mathsf{T}(\alpha) \end{aligned} $$
2.3 Rotation of a Vector
Assuming we rotate a vector under the same frame.
Solution:
First, we set the angle between $\vec{p'}$ and $x$-axis as $\beta$, the magnitude of the vector as $L$ (we know that the magnitudes of $\vec{p}$ and $\vec{p'}$ are equal). Then, we have $$ \begin{aligned} p'_x &= L*\cos\beta \\ p'_y &= L*\sin\beta \\ p_x &= L*\cos(\beta + \alpha) \\ p_y &= L*\sin(\beta + \alpha) \end{aligned} $$
According to Trigonometric Identities, we have $$ \begin{aligned} p_x &= L*\cos\beta \cos\alpha - \sin\beta \sin\alpha \\ p_y &= L*\sin\beta \cos\alpha + \cos\beta \sin\alpha \end{aligned} $$
thus, $$ \begin{aligned} p_x &= p_x'\cos\alpha - p_y' \sin\alpha \\ p_x &= p_x'\sin\alpha + p_y' \cos\alpha \\ p_z &= p'_z \end{aligned} $$
It is easy to recognize that $\vec{p}$ can be expressed as
$$ \vec{p} = R_z(\alpha)\vec{p'} $$Summary
A rotation matrix $R$ attains three equivalent geometrical meanings:
- It describes the mutual orientation between two coordinate frames; its column vectors are the direction cosines of the axes of the rotated frame with respect to the original frame.
- It represents the coordinate transformation between the coordinates of a point expressed in two different frames (with common origin).
- It is the operator that allows the rotation of a vector in the same coordinate frame.
2.4 Composition of Rotation Matrices
-
Right multiplication in rotation Matrices - rotation relative to its own (current) frame.
-
Left multiplication in rotation Matrices - rotation relative to a fixed (reference) frame.
3 Forward Kinematics
3.1 Euler Angles
Euler Angles: minimal representation of orientation $$ \phi = \left[ \begin{matrix} \varphi & \vartheta & \psi \end{matrix} \right]^\mathsf{T} $$
3.1.1 Euler Angles: ZYZ Angles (Intrinsic / Dynamic Euler angles)
The rotation described by ZYZ angles is obtained as composition of the following elementary rotation:
- Rotate the reference frame by the angle $\varphi$ about axis $z$
- Rotate the current frame by the angle $\vartheta$ about axis $y'$
- Rotate the current frame by the angle $\psi$ about axis $z''$
$$ R_y(\alpha) = \left[ \begin{matrix} \cos\alpha & 0 & \sin\alpha \\ 0 & 1 & 0 \\ -\sin\alpha & 0 & \cos\alpha \end{matrix} \right] $$ $$ R_z(\alpha) = \left[ \begin{matrix} \cos\alpha & -\sin\alpha & 0 \\ \sin\alpha & \cos\alpha & 0 \\ 0 & 0 & 1 \end{matrix} \right] $$When current rotation is based on previous rotation (current frame), we use symbol ($'$) to represent it.
So that, we have $$ \begin{aligned} R(\phi) &= R_{z}(\varphi)R_{y'}(\vartheta)R_{z''}(\psi) \\ &= \left[ \begin{matrix} \cos\varphi & -\sin\varphi & 0 \\ \sin\varphi & \cos\varphi & 0 \\ 0 & 0 & 1 \end{matrix} \right] \cdot \left[ \begin{matrix} \cos\vartheta & 0 & \sin\vartheta \\ 0 & 1 & 0 \\ -\sin\vartheta & 0 & \cos\vartheta \end{matrix} \right] \cdot \left[ \begin{matrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right] \\ &= \left[ \begin{matrix} \cos\varphi \cos\vartheta & -\sin\varphi & \cos\varphi \sin\vartheta \\ \sin\varphi \cos\vartheta & \cos\varphi & \sin\varphi \sin\vartheta \\ -\sin\vartheta & 0 & \cos\vartheta \end{matrix} \right] \cdot \left[ \begin{matrix} \cos\psi & -\sin\psi & 0 \\ \sin\psi & \cos\psi & 0 \\ 0 & 0 & 1 \end{matrix} \right] \\ &= \left[ \begin{matrix} \cos\varphi \cos\vartheta \cos\psi - \sin\varphi \sin\psi & - \cos\varphi \cos\vartheta \sin\psi - \sin\varphi \cos\psi & \cos\varphi \sin\vartheta \\ \sin\varphi \cos\vartheta \cos\psi + \cos\varphi \sin\psi & - \sin\varphi \cos\vartheta \sin\psi + \cos\varphi \cos\psi & \sin\varphi \sin\vartheta \\ -\sin\vartheta \cos\psi& \sin\vartheta \sin\psi & \cos\vartheta \end{matrix} \right] \end{aligned} $$
It is useful to solve the inverse problem, that is to determine the set of Euler angles corresponding to a given rotation matrix
$$ R(\phi) = \left[ \begin{matrix} r_{11} & r_{12} & r_{13} \\ r_{21} & r_{22} & r_{23} \\ r_{31} & r_{32} & r_{33} \end{matrix} \right] $$By considering the elements $r_{13}$ and $r_{23}$, under the assumption that $r_{13} \neq 0$ and $r_{23} \neq 0$, it follows that $$ \varphi = Atan2(r_{23}, r_{13}) = Atan2 \left(\frac{\sin\varphi \sin\vartheta}{\cos\varphi \sin\vartheta}\right) $$
The function $Atan2(y, x)$ computes the arctangent of the ratio $(y/x)$ but utilizes the sign of each argument to determine which quadrant the resulting angle belongs to; this allows the correct determination of an angle in a range of $2\pi$. $$ Atan2(y,x) = \begin{cases} \arctan(y/x), &x>0 \\ \arctan(y/x)+\pi, &y\geq0,x<0 \\ \arctan(y/x)-\pi, &y<0,x<0 \\ \pi/2, &y>0,x=0 \\ -\pi/2, &y<0,x=0 \\ undefined, &y=0,x=0 \end{cases} $$
Similily, we can get $$ \begin{aligned} \vartheta &= Atan2(\sqrt{r^2_{13} + r^2_{23}}, r_{33}) \\ \psi &= Atan2(r_{32}, -r_{31}) \end{aligned} $$
where, the choice of the positive sign for the term $\sqrt{r^2_{13} + r^2_{23}}$ limits the range of feasible values of $\vartheta$ to $(0, \pi)$.
It is possible to derive another solution which produces the same effects.
3.1.2 Euler Angles: ZYX/RPY Angles (Extrinsic / Static Euler Angles)
Please note the rotation order.
The rotation described by RPY angles is obtained as composition of the following elementary rotation:
- Rotate the reference frame by the angle $\psi$ about axis $x$ (yaw)
- Rotate the reference frame by the angle $\vartheta$ about axis $y$ (pitch)
- Rotate the reference frame by the angle $\varphi$ about axis $z$ (roll)
So that, we have $$ \begin{aligned} R(\phi) &= R_{z}(\varphi)R_{y}(\vartheta)R_{x}(\psi) \\ &= \left[ \begin{matrix} \cos\varphi & -\sin\varphi & 0 \\ \sin\varphi & \cos\varphi & 0 \\ 0 & 0 & 1 \end{matrix} \right] \cdot \left[ \begin{matrix} \cos\vartheta & 0 & \sin\vartheta \\ 0 & 1 & 0 \\ -\sin\vartheta & 0 & \cos\vartheta \end{matrix} \right] \cdot \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos\psi & -\sin\psi \\ 0 & \sin\psi & \cos\psi \end{matrix} \right] \\ &= \left[ \begin{matrix} \cos\varphi \cos\vartheta & -\sin\varphi & \cos\varphi \sin\vartheta \\ \sin\varphi \cos\vartheta & \cos\varphi & \sin\varphi \sin\vartheta \\ -\sin\vartheta & 0 & \cos\vartheta \end{matrix} \right] \cdot \left[ \begin{matrix} 1 & 0 & 0 \\ 0 & \cos\psi & -\sin\psi \\ 0 & \sin\psi & \cos\psi \end{matrix} \right] \\ &= \left[ \begin{matrix} \cos\varphi \cos\vartheta & \cos\varphi \sin\vartheta \sin\psi - \sin\varphi \cos\psi & \cos\varphi \sin\vartheta \cos\psi + \sin\varphi \sin\psi \\ \sin\varphi \cos\vartheta & \sin\varphi \sin\vartheta \sin\psi + \cos\varphi \cos\psi & \sin\varphi \sin\vartheta \cos\psi - \cos\varphi \sin\psi \\ -\sin\vartheta & \cos\vartheta \sin\psi & \cos\vartheta \cos\psi \end{matrix} \right] \end{aligned} $$
The same, we can get $[\varphi, \vartheta, \psi]$ by below solution. For $\vartheta$ in the range $(−\pi/2, \pi/2)$ is $$ \begin{aligned} \varphi &= Atan2(r_{21}, r_{11}) \\ \vartheta &= Atan2(- r_{31}, \sqrt{r^2_{32} + r^2_{33}}) \\ \psi &= Atan2(r_{32}, r_{33}) \end{aligned} $$
For $\vartheta$ in the range $(\pi/2, 3\pi/2)$ is $$ \begin{aligned} \varphi &= Atan2(-r_{21}, -r_{11}) \\ \vartheta &= Atan2(- r_{31}, -\sqrt{r^2_{32} + r^2_{33}}) \\ \psi &= Atan2(-r_{32}, -r_{33}) \end{aligned} $$
3.1.3 Singularity (奇点) or Gimbal Lock（万向锁/环架锁定）
 |
 |
|---|
Gimbal Lock problem only happend on Intrinsic (Dynamic) Euler angles.
- Intrinsic / Dynamic Euler angles: The rotation around the current frame, using right multiplication in rotation Matrices. (e.g. ZYZ Angles)
- Extrinsic / Static Euler Angles: The rotation around the reference frame, using left multiplication in rotation Matrices. (e.g. ZYX Angles)
Gimbal lock will lead the solution cannot be uniquely determined from certain rotation.
3.2 Unit Quaternions
Quaternions can overcome the drawbacks of angle/axis representation (no singularities), which is defined as: $\mathcal{Q} = \{ \eta,\vec{\epsilon} \}$, where $$ \eta = \cos \frac{\vartheta}{2} $$ $$ \vec{\epsilon} = \sin \frac{\vartheta}{2} \vec{r} $$
$\eta$ is called the scalar part of the quaternion, while $\vec{\epsilon} = [\epsilon_x \ \epsilon_y \ \epsilon_z]^\mathsf{T}$ is called the vector part of the quaternion. They are constrained by the condition $$ \eta^2 + \epsilon_x^2 + \epsilon_y^2 + \epsilon_z^2 = 1 $$
hence, the name unit quaternion.
3.3 Denavit–Hartenberg convention (D-H parameters)
A good video on YouTube for learning Denavit-Hartenberg Reference Frame Layout (D-H parameters)
The D-H parameters of the above robot are
| Link | $a_i$ | $\alpha_i$ | $d_i$ | $\vartheta_i$ |
|---|---|---|---|---|
| 1 | 0 | $\pi/2$ | 0 | $\vartheta_1$ |
| 2 | $a_2$ | 0 | 0 | $\vartheta_2$ |
| 3 | 0 | $\pi/2$ | 0 | $\vartheta_3$ |
| 4 | 0 | $-\pi/2$ | $d_4$ | $\vartheta_4$ |
| 5 | 0 | $\pi/2$ | 0 | $\vartheta_5$ |
| 6 | 0 | 0 | $d_6$ | $\vartheta_6$ |
3.4 Homogeneous Transformations
In order to achieve a compact representation of the relationship between the coordinates of the same point in two different frames, the homogeneous representation of a generic vector $p$ can be introduced as the vector $\widetilde{p}$ formed by adding a fourth unit component, i.e., $$ \widetilde{p} = \left[ \begin{matrix} p \\ 1 \end{matrix} \right] $$
By adopting this representation for the vectors $p^0$ and $p^1$, the coordinate transformation can be written in terms of the (4 × 4) matrix
$$ A^0_1 = \left[ \begin{matrix} R^0_1 & o^0_1 \\ \vec{0}^\mathsf{T} & 1 \end{matrix} \right] $$This matrix is expressed in a block-partitioned form as $$ A^1_0 = \left[ \begin{matrix} {R^0_1}^\mathsf{T} & -{R^0_1}^\mathsf{T}o^0_1 \\ \vec{0}^\mathsf{T} & 1 \end{matrix} \right] = \left[ \begin{matrix} R^1_0 & -R^1_0o^0_1 \\ \vec{0}^\mathsf{T} & 1 \end{matrix} \right] $$
In general, $$ A^{-1} \neq A^\mathsf{T} $$
3.5 Direct Kinematics
With respect to a reference frame $O_b–x_by_bz_b$, the direct kinematics function is expressed by the homogeneous transformation matrix
$$ T^b_e(q) = \left[ \begin{matrix} n^b_e(q) & s^b_e(q) & a^b_e(q) & p^b_e(q) \\ 0 & 0 & 0 & 1 \end{matrix} \right] $$