【机器人学导论】简明学习笔记2.1——空间描述和变换（1/2）

标签：cdot 矩阵导论 AP bmatrix theta 机器人学 hat 2.1

主要参考学习资料：

《机器人学导论（第4版）》John J.Craig著

台大机器人学之运动学——林沛群（本文插图来自该课程课件）

本章前置知识：矢量和矩阵的四则运算-单位矩阵-转置矩阵-逆矩阵-正交矩阵

码字不易，求点赞收藏(´•ω•̥`)

有问题欢迎评论区讨论~

空间描述和变换

描述

我们用 { A } \{A\} {A}来表示一个坐标系(Frame)

讨论问题之前，我们需要建立一个世界坐标系(World Frame)，之后定义的位置和姿态都是参照世界坐标系定义的笛卡尔坐标系

一个平面中的物体拥有沿 x x x轴、 y y y轴两个方向移动的自由度和绕垂直于平面的轴转动的自由度（Degree Of Freedom，可简记为DOF），即两个移动自由度和一个转动自由度，共三个自由度

同理，一个空间中的物体拥有沿 x x x轴、 y y y轴、 z z z轴三个方向移动的自由度和绕这三个轴转动的自由度，即三个移动自由度和三个转动自由度，共六个自由度

位置描述

位置描述涵盖刚体在三个方向上的移动自由度

一旦建立了坐标系，我们可以用一个 3 × 1 3\times 1 3×1的位置矢量给世界坐标系中的任何点定位

矢量用一个左上标来表明其定义在哪个坐标系，例如 A P ^AP AP的元素数值是沿坐标系 { A } \{A\} {A}三个主轴方向上的投影，矢量的各个元素用下标 x x x、 y y y和 z z z来标明：

A P = A [ p x p y p z ] ^AP=^A\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix} AP=A pxpypz

（其中矩阵左上标表示矩阵中坐标都是相对于该坐标系而言，由于格式输入限制位置偏下）

位置矢量应用举例

姿态描述

姿态描述涵盖刚体三个方向上的转动自由度

我们可以在刚体上固定一个坐标系 { B } \{B\} {B}，用 { B } \{B\} {B}相对于 { A } \{A\} {A}的描述来表示物体的姿态，而 { B } \{B\} {B}可以用该坐标系三个主轴方向的单位矢量来确定

我们用 X ^ B \hat{X}_B X^B、 Y ^ B \hat{Y}_B Y^B和 Z ^ B \hat{Z}_B Z^B来表示 { B } \{B\} {B}主轴方向的单位矢量，当 { A } \{A\} {A}作为参考坐标系时，它们被写为 A X ^ B ^A\hat{X}_B AX^B、 A Y ^ B ^A\hat{Y}_B AY^B和 A Z ^ B ^A\hat{Z}_B AZ^B

将这三个单位矢量按照 A X ^ B ^A\hat{X}_B AX^B、 A Y ^ B ^A\hat{Y}_B AY^B和 A Z ^ B ^A\hat{Z}_B AZ^B的顺序排列组成一个 3 × 3 3\times3 3×3的矩阵，称为旋转矩阵(Rotation Matrix)。表达 { B } \{B\} {B}相对于 { A } \{A\} {A}的旋转矩阵记为 B A R _B^AR BAR

B A R _B^AR BAR= ( A X ^ B A Y ^ B A Z ^ B ) \begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix} (AX^BAY^BAZ^B)

又 A X ^ B ^A\hat{X}_B AX^B、 A Y ^ B ^A\hat{Y}_B AY^B和 A Z ^ B ^A\hat{Z}_B AZ^B是以 { A } \{A\} {A}为参考，所以矢量中各个元素的数值为 X ^ B \hat{X}_B X^B、 Y ^ B \hat{Y}_B Y^B和 Z ^ B \hat{Z}_B Z^B分别在 { A } \{A\} {A}三个主轴方向上的投影长度，因而上式可以进一步写为

又矩阵中各个矢量均为单位矢量，点积之值也是 X ^ B \hat{X}_B X^B、 Y ^ B \hat{Y}_B Y^B和 Z ^ B \hat{Z}_B Z^B对于 { A } \{A\} {A}的方向余弦，该矩阵又称为方向余弦矩阵(Direction Cosine Matrix)

综上，我们用旋转矩阵描述了刚体的姿态

旋转矩阵

接下来重点介绍旋转矩阵的性质与作用

性质

将旋转矩阵中各个点积前后矢量互换，我们有如下推导（其中右上标T为转置符号）：

B A R = ( A X ^ B A Y ^ B A Z ^ B ) = [ X ^ B ⋅ X ^ A Y ^ B ⋅ X ^ A Z ^ B ⋅ X ^ A X ^ B ⋅ Y ^ A Y ^ B ⋅ Y ^ A Z ^ B ⋅ Y ^ A X ^ B ⋅ Z ^ A Y ^ B ⋅ Z ^ A Z ^ B ⋅ Z ^ A ] _B^AR=\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}=\begin{bmatrix}\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{Z}_A&\hat{Z}_B\cdot\hat{Z}_A\end{bmatrix} BAR=(AX^BAY^BAZ^B)= X^B⋅X^AX^B⋅Y^AX^B⋅Z^AY^B⋅X^AY^B⋅Y^AY^B⋅Z^AZ^B⋅X^AZ^B⋅Y^AZ^B⋅Z^A
= [ X ^ A ⋅ X ^ B X ^ A ⋅ Y ^ B X ^ A ⋅ Z ^ B Y ^ A ⋅ X ^ B Y ^ A ⋅ Y ^ B Y ^ A ⋅ Z ^ B Z ^ A ⋅ X ^ B Z ^ A ⋅ Y ^ B Z ^ A ⋅ Z ^ B ] = [ X ^ A T Y ^ A T Z ^ A T ] =\begin{bmatrix}\hat{X}_A\cdot\hat{X}_B&\hat{X}_A\cdot\hat{Y}_B&\hat{X}_A\cdot\hat{Z}_B\\\hat{Y}_A\cdot\hat{X}_B&\hat{Y}_A\cdot\hat{Y}_B&\hat{Y}_A\cdot\hat{Z}_B\\\hat{Z}_A\cdot\hat{X}_B&\hat{Z}_A\cdot\hat{Y}_B&\hat{Z}_A\cdot\hat{Z}_B\end{bmatrix}=\begin{bmatrix}\hat{X}_A{^T}\\\hat{Y}_A{^T}\\\hat{Z}_A{^T}\end{bmatrix} = X^A⋅X^BY^A⋅X^BZ^A⋅X^BX^A⋅Y^BY^A⋅Y^BZ^A⋅Y^BX^A⋅Z^BY^A⋅Z^BZ^A⋅Z^B = X^ATY^ATZ^AT = ( A X ^ B A Y ^ B A Z ^ B ) T = A B R T =\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}^T=^B_AR^T =(AX^BAY^BAZ^B)T=ABRT

由此 B A R = A B R T ^A_BR=^B_AR^T BAR=ABRT

又由：

B A R T B A R = [ X ^ A T Y ^ A T Z ^ A T ] ( A X ^ B A Y ^ B A Z ^ B ) = I 3 ^A_BR{^T}^A_BR=\begin{bmatrix}\hat{X}_A{^T}\\\hat{Y}_A{^T}\\\hat{Z}_A{^T}\end{bmatrix}\begin{pmatrix}^A\hat{X}_B&^A\hat{Y}_B&^A\hat{Z}_B\end{pmatrix}=I_3 BARTBAR= X^ATY^ATZ^AT (AX^BAY^BAZ^B)=I3

其中 I 3 I_3 I3是 3 × 3 3\times3 3×3的单位矩阵，因此：

B A R = A B R − 1 = A B R T ^A_BR=^B_AR^{-1}=^B_AR{^T} BAR=ABR−1=ABRT

即 A B R ^B_AR ABR的转置矩阵等于它的逆矩阵，又同时等于 B A R ^A_BR BAR。由线性代数可知，前者是正交矩阵的性质

旋转矩阵有九个数字，但只有三个矢量两两垂直、均为单位矢量总共六个条件约束，因此只有三个数字是独立变量，与空间中转动具有三个自由度相符

作用

除了前文所讲的描述一个刚体的姿态外，旋转矩阵还有两个作用

1.转换矢量的坐标

矢量 P P P相对于 { B } \{B\} {B}的坐标表示： B P = B p x X ^ B + B p y Y ^ B + B p z Z ^ B ^BP=^Bp_x\hat{X}_B+^Bp_y\hat{Y}_B+^Bp_z\hat{Z}_B BP=BpxX^B+BpyY^B+BpzZ^B

矢量 P P P相对于 { A } \{A\} {A}的坐标表示： A P = A p x X ^ A + A p y Y ^ A + A p z Z ^ A ^AP=^Ap_x\hat{X}_A+^Ap_y\hat{Y}_A+^Ap_z\hat{Z}_A AP=ApxX^A+ApyY^A+ApzZ^A

B P ^BP BP和 A P ^AP AP只是坐标表示不同，本质上是同一个矢量，由此矢量 P P P相对于 { A } \{A\} {A}的坐标也可以用 B P ^BP BP在 { A } \{A\} {A}三个主轴上的投影计算：

A p x = B P X ^ A = B p x X ^ B X ^ A + B p y Y ^ B X ^ A + B p z Z ^ B X ^ A ^Ap_x=^BP\hat{X}_A=^Bp_x\hat{X}_B\hat{X}_A+^Bp_y\hat{Y}_B\hat{X}_A+^Bp_z\hat{Z}_B\hat{X}_A Apx=BPX^A=BpxX^BX^A+BpyY^BX^A+BpzZ^BX^A

A p y = B P Y ^ A = B p x X ^ B Y ^ A + B p y Y ^ B Y ^ A + B p z Z ^ B Y ^ A ^Ap_y=^BP\hat{Y}_A=^Bp_x\hat{X}_B\hat{Y}_A+^Bp_y\hat{Y}_B\hat{Y}_A+^Bp_z\hat{Z}_B\hat{Y}_A Apy=BPY^A=BpxX^BY^A+BpyY^BY^A+BpzZ^BY^A

A p z = B P X ^ A = B p x X ^ B Z ^ A + B p y Y ^ B Z ^ A + B p z Z ^ B Z ^ A ^Ap_z=^BP\hat{X}_A=^Bp_x\hat{X}_B\hat{Z}_A+^Bp_y\hat{Y}_B\hat{Z}_A+^Bp_z\hat{Z}_B\hat{Z}_A Apz=BPX^A=BpxX^BZ^A+BpyY^BZ^A+BpzZ^BZ^A

用矩阵表示则为：

A P = A [ p x p y p z ] = [ X ^ B ⋅ X ^ A Y ^ B ⋅ X ^ A Z ^ B ⋅ X ^ A X ^ B ⋅ Y ^ A Y ^ B ⋅ Y ^ A Z ^ B ⋅ Y ^ A X ^ B ⋅ Z ^ A Y ^ B ⋅ Z ^ A Z ^ B ⋅ Z ^ A ] B [ p x p y p z ] = B A R B P ^AP=^A\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}=\begin{bmatrix}\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{Z}_A&\hat{Z}_B\cdot\hat{Z}_A\end{bmatrix}^B\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}=^A_BR^BP AP=A pxpypz = X^B⋅X^AX^B⋅Y^AX^B⋅Z^AY^B⋅X^AY^B⋅Y^AY^B⋅Z^AZ^B⋅X^AZ^B⋅Y^AZ^B⋅Z^A B pxpypz =BARBP

由此得到 { B } \{B\} {B}相对于 { A } \{A\} {A}的旋转矩阵可以将 P P P的表示由相对于 { B } \{B\} {B}转换为相对于 { A } \{A\} {A}

2.描述刚体转动的状态

分别研究坐标系绕三个主轴旋转的状态

以 { A } \{A\} {A}绕 Z ^ A \hat{Z}_A Z^A逆时针旋转 θ \theta θ（以后不标明旋转方向默认为逆时针旋转）得到 { B } \{B\} {B}为例，计算此时 { B } \{B\} {B}相对于 { A } \{A\} {A}的旋转矩阵得：

B A R = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] = [ c θ − s θ 0 s θ c θ 0 0 0 1 ] ^A_BR=\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix}=\begin{bmatrix}c\theta&-s\theta&0\\s\theta&c\theta&0\\0&0&1\end{bmatrix} BAR= cosθsinθ0−sinθcosθ0001 = cθsθ0−sθcθ0001

（由于我们将在计算中用到许多三角函数，因此三角函数用对应首字母简记）

由此，我们将绕 Z ^ A \hat{Z}_A Z^A旋转 θ \theta θ以这种方式计算出来的旋转矩阵记为 R Z ^ A ( θ ) R_{\hat{Z}_A}(\theta) RZ^A(θ)，则：

R Z ^ A ( θ ) = [ c θ − s θ 0 s θ c θ 0 0 0 1 ] R_{\hat{Z}_A}(\theta)=\begin{bmatrix}c\theta&-s\theta&0\\s\theta&c\theta&0\\0&0&1\end{bmatrix} RZ^A(θ)= cθsθ0−sθcθ0001

在这里插入图片描述

同理得到：

R X ^ A ( θ ) = [ 1 0 0 0 c θ − s θ 0 s θ c θ ] R_{\hat{X}_A}(\theta)=\begin{bmatrix}1&0&0\\0&c\theta&-s\theta\\0&s\theta&c\theta\end{bmatrix} RX^A(θ)= 1000cθsθ0−sθcθ

R Y ^ A ( θ ) = [ c θ 0 s θ 0 1 0 − s θ 0 c θ ] R_{\hat{Y}_A}(\theta)=\begin{bmatrix}c\theta&0&s\theta\\0&1&0\\-s\theta&0&c\theta\end{bmatrix} RY^A(θ)= cθ0−sθ010sθ0cθ

通过接下来的例子我们来理解这类旋转矩阵如何描述刚体转动的状态

在这里插入图片描述

A P = A ( p x p y p z ) T ^AP=^A(p_x\;p_y\;p_z)^T AP=A(pxpypz)T对 X ^ A \hat{X}_A X^A旋转 θ \theta θ得到 A P ′ ^AP' AP′， A P ′ = ? ^AP'=? AP′=?

不妨让 { A } \{A\} {A}跟着 A P ^AP AP同步旋转，得到 { A ′ } \{A'\} {A′}，那么 A P ′ ^AP' AP′相对于 { A ′ } \{A'\} {A′}的坐标跟 A P ^AP AP相对于 { A } \{A\} {A}的坐标相等，而后者已知

又通过题目条件和前文定义我们知道 A ′ A R = R X ^ A ( θ ) = [ 1 0 0 0 c θ − s θ 0 s θ c θ ] ^A_{A'}R=R_{\hat{X}_A}(\theta)=\begin{bmatrix}1&0&0\\0&c\theta&-s\theta\\0&s\theta&c\theta\end{bmatrix} A′AR=RX^A(θ)= 1000cθsθ0−sθcθ

利用旋转矩阵转换矢量坐标的作用，我们可以把 A P ′ ^AP' AP′相对于 { A ′ } \{A'\} {A′}的坐标转换为相对于原坐标系 { A } \{A\} {A}的坐标，即：

A P ′ = A ′ A R A P = R X ^ A ( θ ) A P ^AP'=^A_{A'}R^AP=R_{\hat{X}_A}(\theta)^AP AP′=A′ARAP=RX^A(θ)AP

所以旋转矩阵 R ( θ ) R(\theta) R(θ)可以计算矢量在同一坐标系下旋转后的坐标

位姿描述

位置和姿态的组合称为位姿

我们同样用 { A } \{A\} {A}来表示一个位姿

以 { A } \{A\} {A}为参考描述位姿 { B } \{B\} {B}，结合前文的位置描述和姿态描述，我们用 B A R ^A_BR BAR表示 { B } \{B\} {B}的姿态，用 A P B O R G ^AP_{BORG} APBORG表示 { B } \{B\} {B}的原点的位置矢量（下标ORG意为原点origin），于是位姿描述记为：

{ B } = { B A R , A P B O R G } \{B\}=\{^A_BR,{\,}^AP_{BORG}\} {B}={BAR,APBORG}

下一篇文章将讲述本章后半部分空间中的变换内容

标签：cdot,矩阵,导论,AP,bmatrix,theta,机器人学,hat,2.1
From： https://blog.csdn.net/mojin2005/article/details/143431800