Introduction to robotics I. spatial transformation (1) posture, transformation

Preface

Due to the close relationship between visual servo and manipulator , Therefore, we should start from the foundation , Record the robot kinematics .

This paper records the position and posture of rigid bodies . actually , Spatial transformation in SLAM It has been mentioned once in the column , However, the introduction to robotics gives a more detailed description of rigid body motion .

Coordinate system

There are usually two coordinate systems , One is the world coordinate system for reference （ Cartesian coordinate system ）, One is the rigid body coordinate system with the center of mass of the rigid body as the origin .

The robot coordinate system follows the right-hand rule , As shown in the figure below

Postures

Posture is position and posture , Corresponding displacement and rotation . The position and posture of the robot must be referred to the world coordinate system .

Posture represents the relationship between reference systems .

Location

The position of the rigid body is represented by a vector .

There is a point in the figure above $P$, In a coordinate system $\{A\}$ For reference , You can put the vector $OP$ To express ${}^{A}P$ The location of , Its left superscript indicates the reference system .${}^{A}P$ The coordinates of are expressed as ：
$${}^{A}P=[p_x,p_y,p_z{]}^T$$

Posture

The attitude of the rigid body is represented by the unit vector of the three principal axes of the rigid body coordinate system in the reference system .

Above picture ,${\hat{X}}_B,{\hat{Y}}_B,{\hat{Z}}_B$ Represents the unit vector of the three principal axes of the rigid body in the upper right corner , They are in the frame of reference $\{A\}$ The expression of in is ${}^A{\hat{X}}_B{,}^A{\hat{Y}}_B{,}^A{\hat{Z}}_B$. These three unit vectors form the rotation matrix ：
$${}_B^{A}R={(}^A{\hat{X}}_B{}^A{\hat{Y}}_B{}^A{\hat{Z}}_B)$$

So how to calculate the coordinates of the principal axis unit vector of the rigid body in the reference system ？

With ${}^A{\hat{X}}_B$ For example , Its x The value of the coordinate is actually ${}^A{\hat{X}}_B$ In the frame of reference X The projection on the axis ,y, z The coordinates are in the reference system Y, Z The projection on the axis ：
$${}_B^{A}R={(}^A{\hat{X}}_B{}^A{\hat{Y}}_B{}^A{\hat{Z}}_B)=\left[\begin{array}{ccc}{\hat{X}}_B\cdot {\hat{X}}_A& {\hat{Y}}_B\cdot {\hat{X}}_A& {\hat{Z}}_B\cdot {\hat{X}}_A\\ {\hat{X}}_B\cdot {\hat{Y}}_A& {\hat{Y}}_B\cdot {\hat{Y}}_A& {\hat{Z}}_B\cdot {\hat{Y}}_A\\ {\hat{X}}_B\cdot {\hat{Z}}_A& {\hat{Y}}_B\cdot {\hat{Y}}_A& {\hat{Z}}_B\cdot {\hat{X}}_A\end{array}\right]$$

The rotation matrix is an orthogonal matrix , The proof is simple ：
$${}_B^A{R}^T{}_B^{A}R=\left[\begin{array}{c}{}^A{\hat{X}}_B^T\\ {}^A{\hat{Y}}_B^T\\ {}^A{\hat{Z}}_B^T\end{array}\right]{(}^A{\hat{X}}_B{}^A{\hat{Y}}_B{}^A{\hat{Z}}_B)=I_3$$
The inner product of the unit vector in the same direction is 1, The inner product of mutually perpendicular unit vectors is 0, therefore ${}^A{\hat{X}}_B^T{}^A{\hat{X}}_B=1{,}^A{\hat{X}}_B^T{}^A{\hat{Y}}_B=0$, So we can get the identity matrix above .

Pose transformation

Pose transformation refers to the change of the position and pose of the rigid body coordinate system , Translation and rotation , Get the relative pose between the rigid system and the reference system

Still in order to 2-2 For example , The pose transformation of rigid body coordinate system relative to reference system can be expressed as ：
$${\{}_B^{A}R,{}^A{P}_{BORG}\}$$
That is to say SLAM In the fourteenth lecture $R,t$

mapping

Mapping represents the coordinate transformation of the same point between different coordinate systems .

Mapping includes translation and rotation .

translation

Translation is related to the position of the origin of the rigid body coordinate system .

Above picture , It is known that ${}^{B}P$ Indication point P In the coordinate system $\{B\}$ Position in ,${}^A{P}_{BORG}$ Expressed in the reference system $\{A\}$ Next $\{B\}$ The position of the origin . be ${}^{A}P$ Can be expressed as ：
$${}^{A}P={}^{B}P+{}^A{P}_{BORG}$$

Only when the attitude of the two coordinate systems is the same , To perform the above translation .

rotate

Rotation is related to the attitude of the rigid body coordinate system .

Above picture , It is known that ${}^{B}P$ Indication point P In the coordinate system $\{B\}$ Position in .${}^{A}P$ Expressed in the reference system $\{A\}$ The next position , Vector ${}^{A}P$ In the direction of the principal axis of the reference system ${\hat{X}}_A,{\hat{Y}}_A,{\hat{Z}}_A$ The projection of （ Unit vector inner product ）. Besides , We know , The inner product of a vector needs to be expressed in the same coordinate system to be meaningful . be ${}^{A}P$ Can be expressed as ：
$${}^{A}P=\left[\begin{array}{ccc}{}^B{\hat{X}}_A\cdot {}^{B}P& {}^B{\hat{Y}}_A\cdot {}^{B}P& {}^B{\hat{Z}}_A\cdot {}^{B}P\end{array}\right]={}_B^{A}R{}^{B}P$$

Transformation

Transformation = Spin first , Back translation .

Above picture , Need to use ${}^{B}P$ Express ${}^{A}P$. First of all, will ${}^{B}P$ Map to an intermediate coordinate system $\{B^{\prime }\}$, The attitude of this coordinate system is consistent with $\{A\}$ identical , Origin and $\{B\}$ identical . And then use ${}^{B^{\prime }}P$ Express ${}^{A}P$：
$${}^{A}P={}^{B^{\prime }}P+{}^A{P}_{B^{\prime }ORG}={}_B^{B^{\prime }}R{}^{B}P+{}^A{P}_{B^{\prime }ORG}={}_B^{A}R{}^{B}P+{}^A{P}_{BORG}$$
It corresponds to SLAM In the fourteenth lecture $P^{\prime }=RP+t$

Introduce homogeneous matrix , There is the concept of transformation matrix ：
$$\begin{gathered} \left[\begin{array}{c}{}^{A}P\\ 1\end{array}\right]=\left[\begin{array}{cc}{}_B^{A}R& {}^A{P}_{BORG}\\ 0& 1\end{array}\right]\left[\begin{array}{c}{}^{B}P\\ 1\end{array}\right] \\ { \\ }^{A}P={}_B^{A}T{}^{B}P \end{gathered}$$

Composite transformation

If known ${}_B^{A}T,{}_C^{B}T$, You can use the ${}^{C}P$ Express ${}^{A}P$：
$${}^{A}P{=}_B^{A}T{}_C^{B}T{}^{C}P$$

inverse transformation

Inverse of rotation matrix ：
$${}_B^{A}R={}_A^B{R}^T$$

Inverse of transformation matrix ：
$$\left[\begin{array}{cc}{}_B^A{R}^T& {-}_B^A{R}^T{}^A{P}_{BORG}\\ 0& 1\end{array}\right]$$
There are two ways to find the inverse of a transformation matrix , One is to find the inverse of the matrix directly ; The other is to find the inverse through the transformation property . Introduce the latter .

If known ${}^{A}P$ seek ${}^{B}P$, Yes ：
$${}^{B}P={}_A^{B}R{}^{A}P+{}^B{P}_{AORG}$$
If ${}^{A}P$ yes $\{B\}$ The origin of , namely ：
$$\begin{gathered} 0{=}^B{P}_{BORG}{=}_A^{B}R{}^A{P}_{BORG}+{}^B{P}_{AORG} \\ {}^B{P}_{AORG}={-}_A^{B}R{}^A{P}_{BORG}={-}_B^A{R}^T{}^A{P}_{BORG} \\ {}^{B}P={}_B^A{R}^T{}^{A}P-{}_B^A{R}^T{}^A{P}_{BORG} \end{gathered}$$

Postscript

This article is actually SLAM The clearer expression method of posture transformation in Lecture 14 , The reference system is placed on the left superscript .