线性代数群论应用:正逆运动学 变换矩阵

news/2024/11/1 17:13:02/

在机器人学中,为了描述机器人的运动,将机器人建模为正运动学和逆运动学

  • 正运动学:从机器人的关节空间描述计算笛卡尔空间描述的机器人末端执行器的位置和姿态,该问题通常是一个几何问题,给定一组关节角度,计算末端坐标系相对于基坐标系的位置和姿态。
  • 逆运动学:从笛卡尔空间描述下的机器人末端执行器位置和姿态反算出机器人关节空间应该达到的关节角度组合,是实现机器人控制的一个基本问题。通常因为正运动学方程是非线性的,因此逆运动学问题较为困难,很难得到封闭解,甚至无解。而正运动学的像空间就形成了逆运动学的有解空间,称为机器人的工作空间。

运动学变换不止在机械臂中有使用,在图形学中也有使用,图形学中通过旋转矩阵,平移矩阵,投影矩阵,将3D的mesh投影到2D的显示器区域中。

这些变换背后的数学原理都是线性代数和群论。

变换的表示方式

变换的表示分为位置和姿态两部分,各三个自由度,
位置变换没有异议,都是用平移向量 [ x y z ] \begin{bmatrix} x \\ y \\ z \end{bmatrix} xyz 表示。
旋转分为:旋转矩阵,欧拉角,四元数三种表达方式

变换矩阵

旋转矩阵

旋转矩阵的思想是:想要表达姿态,可以用坐标系的三个轴的单位矢量来表示即 [ e 1 e 2 e 3 ] [ x y z ] = [ x e 1 + y e 2 + z e 3 ] \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}\begin{bmatrix} x \\ y \\ z \end{bmatrix} = \begin{bmatrix} xe_1 + ye_2 + ze_3 \end{bmatrix} [e1e2e3] xyz =[xe1+ye2+ze3],表示旋转就是对这个向量A做变换得到一个新的向量B, R A = B RA = B RA=B,求旋转向量可以从坐标系入手,原坐标系中的XYZ轴,经过旋转得到新的XYZ轴,基向量 [ e 1 e 2 e 3 ] \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix} [e1e2e3]乘以旋转矩阵R变成了 [ e 1 ′ e 2 ′ e 3 ′ ] \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix} [e1e2e3]
R [ e 1 e 2 e 3 ] = [ e 1 ′ e 2 ′ e 3 ′ ] R\begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix}= \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix} R[e1e2e3]=[e1e2e3]
因为基向量之间是正交的且自身又是单位矩向量,所以:
[ e 1 T e 2 T e 3 T ] [ e 1 e 2 e 3 ] = I \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix} = I e1Te2Te3T [e1e2e3]=I
所以可以两边同时乘以 [ e 1 T e 2 T e 3 T ] \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} e1Te2Te3T ,得到R:
R = [ e 1 ′ e 2 ′ e 3 ′ ] [ e 1 T e 2 T e 3 T ] R= \begin{bmatrix} e_1^{\prime} & e_2^{\prime} & e_3^{\prime} \end{bmatrix}\begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} R=[e1e2e3] e1Te2Te3T
R就是两个坐标系的内积,如果只看旋转,即从A坐标系旋转到B坐标系,坐标系原点位置不变。则这种计算方式可以理解为新坐标系B的各轴在原坐标系A的投影,又称为方向余弦矩阵
B A R = [ A x B A y B A z B ] = [ cos ⁡ ( ∠ X B X A ) cos ⁡ ( ∠ Y B X A ) cos ⁡ ( ∠ Z B X A ) cos ⁡ ( ∠ X B Y A ) cos ⁡ ( ∠ Y B Y A ) cos ⁡ ( ∠ Z B Y A ) cos ⁡ ( ∠ X B Z A ) cos ⁡ ( ∠ Y B Z A ) cos ⁡ ( ∠ Z B Z A ) ] { }_{B}^{A} R=\left[\begin{array}{llll} { }^{A} x_{B} & { }^{A} y_{B} & { }^{A} z_{B} \end{array}\right]=\left[\begin{array}{lll} \cos \left(\angle X_{B} X_{A}\right) & \cos \left(\angle Y_{B} X_{A}\right) & \cos \left(\angle Z_{B} X_{A}\right) \\ \cos \left(\angle X_{B} Y_{A}\right) & \cos \left(\angle Y_{B} Y_{A}\right) & \cos \left(\angle Z_{B} Y_{A}\right) \\ \cos \left(\angle X_{B} Z_{A}\right) & \cos \left(\angle Y_{B} Z_{A}\right) & \cos \left(\angle Z_{B} Z_{A}\right) \end{array}\right] BAR=[AxBAyBAzB]= cos(XBXA)cos(XBYA)cos(XBZA)cos(YBXA)cos(YBYA)cos(YBZA)cos(ZBXA)cos(ZBYA)cos(ZBZA)
当只有一个轴旋转时,例如绕Z轴旋转 α \alpha α度时,则:
R = [ cos ⁡ α − sin ⁡ α 0 sin ⁡ α − cos ⁡ α 0 0 0 1 ] R = \begin{bmatrix} \cos \alpha & -\sin\alpha & 0\\ \sin \alpha & -\cos \alpha & 0\\ 0 & 0 & 1 \end{bmatrix} R= cosαsinα0sinαcosα0001
平移变换时,平移矩阵T为:
T ( t x , t y ) = [ 1 0 0 t x 0 1 0 t y 0 0 1 t z ] A + Y = B T(t_x,t_y) = \begin{bmatrix} 1 & 0 & 0 & t_x \\ 0 & 1 & 0 &t_y \\ 0 & 0 & 1 & t_z \end{bmatrix} \\ A+Y=B T(tx,ty)= 100010001txtytz A+Y=B正好空出来个3*3的位置,为了统一起来,可以扩展一个dimension,让坐标系变为 [ x , y , z , ω ] [x,y,z,\omega] [x,y,z,ω],其中 ω \omega ω表示放缩,一般都设为1,表示不放缩。这样就可以得到一个4*4的变换矩阵了,前三列表示旋转,最后一列表示平移,称为齐次坐标系变换
[ x ′ y ′ z ′ 1 ] = [ a b c t x d e f t y g h i t z 0 0 0 1 ] [ x y z 1 ] \begin{bmatrix} x'\\ y' \\ z' \\ 1 \end{bmatrix} = \begin{bmatrix} a & b & c & t_x\\ d & e & f & t_y\\ g & h & i & t_z \\ 0 & 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \\ 1 \end{bmatrix} xyz1 = adg0beh0cfi0txtytz1 xyz1

在这里插入图片描述

投影矩阵View变换

机器人学里面不会用到,但是在计算机图形学和计算机视觉中会用到,3D的模型要投影到2D中,又被称为视图变换,投影的方式有两种,正交变换和透视变换:
在这里插入图片描述

其中透视变换就是相机的工作原理:
在这里插入图片描述

这张图是视觉SLAM十四讲里面的图,我感觉那个光心平面应该是镜头而不是“相机”,后面的成像平面就是CMOS感光元件,实际相机里面的CMOS也确实是倒置的,而且左右也是反的。

这里面有几个坐标系,可能CV跟CG的符号不太一样,但是具体原理都是一样的:

  • 像素坐标系:其实就是CMOS坐标系,为了区别其他坐标系,把这个坐标系称为 u − v u-v uv坐标系:左上角是原点,往右是 u u u正方向,往下是 v v v正方向
  • 相机坐标系:以相机为原点的局部坐标系
  • 世界坐标系:全局坐标系,原点,方向,由用户自行参考决定

相机成像的过程如下:

CG里面是分为了放缩和正交投影两步,因为CG里面需要考虑整个视场内的所有物体,是自己计算出来的。而相机成像只关注三个面:物平面,像平面,像素平面

将物面坐标归一化到成像平面 { x ′ = f z y y ′ = f z x \left\{\begin{matrix} x'=\frac{f}{z}y \\ \\ y'=\frac{f}{z}x \end{matrix}\right. x=zfyy=zfx,再将成像平面坐标系(图像物理坐标系)变换到像素坐标系 { u = α x ′ + c x v = β y ′ + c y \left\{\begin{array}{l} u=\alpha x^{\prime}+c_{x} \\ v=\beta y^{\prime}+c_{y} \end{array}\right. {u=αx+cxv=βy+cy放缩比例计算方法为CMOS像素点个数/CMOS实际物理尺寸,单位为 p i x e l / m pixel/m pixel/m,平移量为CMOS像素点的一半,单位为 p i x e l pixel pixel,将两个联立,得到如下:
z ( u v 1 ) = ( f x 0 c x 0 f y c y 0 0 1 ) ( x y z ) ≜ K P . z\begin{pmatrix}u\\\\v\\\\1\end{pmatrix}=\begin{pmatrix}f_x&0&c_x\\[0.3em]0&f_y&c_y\\[0.3em]0&0&1\end{pmatrix}\begin{pmatrix}x\\[0.3em]y\\[0.3em]z\end{pmatrix}\triangleq\boldsymbol K\boldsymbol P. z uv1 = fx000fy0cxcy1 xyz KP.
这个变换矩阵定义为相机的内参矩阵K,与相机本身结构相关,出厂就确定了,一般是厂家给,也可以自己标定得到。常见标定方法:张正友标定。
进行一般化,将相机坐标和物体坐标经过旋转 R R R和平移 t t t,转化为世界坐标系:
z P u v = z [ u v 1 ] = K ( R P w + t ) = K T P w . z\boldsymbol{P}_{uv}=z\left[\begin{array}{c}u\\\\v\\\\1\end{array}\right]=\boldsymbol{K}\left(\boldsymbol{R}\boldsymbol{P}_w+\boldsymbol{t}\right)=\boldsymbol{K}\boldsymbol{T}\boldsymbol{P}_w. zPuv=z uv1 =K(RPw+t)=KTPw.
其中的变换矩阵T是相机的外参,随着相机的位置变动而变动,将相机固定在机器人上,通过估计相机的外参就可以估计出机器人的运动

相机由于镜头做工和装配工艺,并不完全是线性关系,还存在畸变,分为径向畸变和纵向畸变
桶形畸变是由于图像放大率随着离光轴的距离增加而减小,而枕形畸变却恰好相反。
在这两种畸变中,穿过图像中心和光轴有交点的直线还能保持形状不变。

欧拉角

姿态可以用欧拉角 [ r o l l p i t c h y a w ] \begin{bmatrix} roll \\ pitch \\ yaw \end{bmatrix} rollpitchyaw 来表示。

注意,这里的位置变换和欧拉角都是相对于局部坐标系 x y z xyz xyz自身三轴的转动,不是相对于全局坐标系 X Y Z XYZ XYZ的变换。
相对于世界坐标系的旋转被称为“静态欧拉角”,但是一般是很难得知世界坐标系信息的,更常用的还是绕自身坐标轴旋转

在这里插入图片描述

该图是左手坐标系,X叉乘Y需要用左手定则才能得到Z,右手坐标系中是使用右手定则。

优点:直观,易理解,没有限制,任何三个 [ 0 , 360 ] [0,360] [0360]的数都是欧拉角
缺点:

  • 欧拉角是讲旋分解为绕xyz轴的旋转,这种旋转在部分角度会有多解,所以欧拉角会出现别名问题。当两个轴重合时会出现万向节死锁问题
    • 三维空间中,旋转顺序的不同结果是不一样的,先x再y再z的顺序和先z再y再x的顺序结果是不一样的,所以欧拉角是按照顺序来转动的,可以分为rpy(ZYX)顺序以及XYZ等欧拉角顺序。
    • 无论哪种都会出现万向节死锁问题,以ZYX为例,如果第二个轴的旋转角为90°倍数,那第二次旋转过后的X轴的绝对方向跟第一次旋转的时候的Z轴的绝对方向是重合的,这样就丢失了一个自由度,第三次旋转即yaw的旋转,跟第一次旋转即roll的旋转是一样的效果。即:动态欧拉角的旋转: [ 20 ° , 90 ° , 30 ° ] [20°,90°,30°] [20°,90°,30°]等同于静态欧拉角中的 [ 50 ° , 90 ° , 0 ° ] [50°,90°,0°] [50°,90°,],且无论怎么改动,绕世界坐标系Z轴的旋转始终为0。
    • 静态欧拉角没有万向节死锁问题
  • 在欧拉角表示下,想要表示一个旋转,是没办法进行线性插值的,无论是机械臂旋转,还是图形学中物体旋转,都需要进行插值来将旋转这个动作细化为过程加以控制。
    • 90度和275度实际位置只差5度,但是插补就会绕个大圈

四元数

四元数可以解决上述问题,四元数的思想为:不分解为三轴的旋转,而是表达出旋转轴和旋转角度.假如一个四元数 ( w , x , y , z ) (w,x,y,z) (w,x,y,z),旋转轴为 ( x , y , z ) (x,y,z) (x,y,z),右手法则旋转角度为 w w w.四元数的后三分别表示三个虚轴 i j k ijk ijk,所以四元数又可以写成一个标量和一个向量的组合: [ s , v ] T [s,v]^T [s,v]T,后三个虚轴关系满足: i 2 = j 2 = k 2 = i j k = − 1 i^2=j^2=k^2=ijk=-1 i2=j2=k2=ijk=1
四元数运算也和普通的向量不大一样:

  • 加减法:与普通向量一致。
  • 乘法: p q = [ p w q w − p x q x − p y q y − p z q z p w q x + p x q w + p y q z − p z q y p w q y − p x q z + p y q w + p z q x p w q z + p x q y − p y q x + p z q w ] p q=\begin{bmatrix} p_wq_w-p_xq_x-p_yq_y-p_zq_z \\ p_wq_x+p_xq_w+p_yq_z-p_zq_y \\ p_wq_y-p_xq_z+p_yq_w+p_zq_x \\ p_wq_z+p_xq_y-p_yq_x+p_zq_w \end{bmatrix} pq= pwqwpxqxpyqypzqzpwqx+pxqw+pyqzpzqypwqypxqz+pyqw+pzqxpwqz+pxqypyqx+pzqw
    类似于矩阵乘法,不满足交换律,但满足结合律和分配律:
    p q = [ p w q w − p v T q v p w q v + q w p v + p v × q v ] p q=\begin{bmatrix} p_wq_w-\mathbf p_v^T\mathbf q_v \\ p_w\mathbf q_v+q_w\mathbf p_v+\mathbf p_v\times\mathbf q_v \end{bmatrix} pq=[pwqwpvTqvpwqv+qwpv+pv×qv]
  • 共轭:实部不变,对虚部取逆
  • 取模 ∥ q ∥ = q ⊗ q ∗ = q ∗ ⊗ q = q w 2 + q x 2 + q y 2 + q z 2 ∥ p ⊗ q ∥ = ∥ q ⊗ p ∥ = ∥ p ∥ ∥ q ∥ \|\mathbf q\|=\sqrt{\mathbf q\otimes\mathbf q^*}=\sqrt{\mathbf q^*\otimes\mathbf q}=\sqrt{q_w^2+q_x^2+q_y^2+q_z^2} \\ \|\mathbf p\otimes \mathbf q\|=\|\mathbf q\otimes \mathbf p\|=\|\mathbf p\|\|\mathbf q\| q=qq =qq =qw2+qx2+qy2+qz2 pq=qp=p∥∥q
  • 求逆: q − 1 = q ∗ ∥ q ∥ 2 q ⊗ q − 1 = q − 1 ⊗ q = q 1 = [ 1 0 ] \mathbf q^{-1}=\frac{\mathbf q^*}{\|\mathbf q\|^2} \\\mathbf q\otimes\mathbf q^{-1}=\mathbf q^{-1}\otimes\mathbf q=\mathbf q_1=\begin{bmatrix} 1 \\ \mathbf 0 \end{bmatrix} q1=q2qqq1=q1q=q1=[10]

用四元数表示旋转时,先将三维点 [ x , y , z ] [x,y,z] [x,y,z]写成虚四元数形式 [ 0 , x , y , z ] [0,x,y,z] [0,x,y,z],经过四元数 q q q的变换后,结果为:
p ′ = q p q − 1 p^{\prime} = qpq^{-1} p=qpq1

Summary

变换本身确定时,表达方式可以互相转换:
四元数->旋转矩阵
R = [ q 0 2 + q 1 2 − q 2 2 − q 3 2 2 ( q 1 q 2 − q 0 q 3 ) 2 ( q 0 q 2 + q 1 q 3 ) 2 ( q 0 q 3 + q 1 q 2 ) q 0 2 − q 1 2 + q 2 2 − q 3 2 2 ( q 2 q 3 − q 0 q 1 ) 2 ( q 1 q 3 − q 0 q 2 ) 2 ( q 0 q 1 + q 2 q 3 ) q 0 2 − q 1 2 − q 2 2 + q 3 2 ] R=\left[\begin{array}{ccc} q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2} & 2\left(q_{1} q_{2}-q_{0} q_{3}\right) & 2\left(q_{0} q_{2}+q_{1} q_{3}\right) \\ 2\left(q_{0} q_{3}+q_{1} q_{2}\right) & q_{0}^{2}-q_{1}^{2}+q_{2}^{2}-q_{3}^{2} & 2\left(q_{2} q_{3}-q_{0} q_{1}\right) \\ 2\left(q_{1} q_{3}-q_{0} q_{2}\right) & 2\left(q_{0} q_{1}+q_{2} q_{3}\right) & q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{array}\right] R= q02+q12q22q322(q0q3+q1q2)2(q1q3q0q2)2(q1q2q0q3)q02q12+q22q322(q0q1+q2q3)2(q0q2+q1q3)2(q2q3q0q1)q02q12q22+q32
旋转矩阵->四元数的转换关系为:
q 0 = 1 2 1 + r 11 + r 22 + r 33 , [ q 1 q 2 q 3 ] = 1 4 q 0 [ r 32 − r 23 r 13 − r 31 r 21 − 2 12 ] \begin{aligned} q_{0} & =\frac{1}{2} \sqrt{1+r_{11}+r_{22}+r_{33}}, \\ {\left[\begin{array}{c} q_{1} \\ q_{2} \\ q_{3} \end{array}\right] } & =\frac{1}{4 q_{0}}\left[\begin{array}{l} r_{32}-r_{23} \\ r_{13}-r_{31} \\ r_{21}-2_{12} \end{array}\right] \end{aligned} q0 q1q2q3 =211+r11+r22+r33 ,=4q01 r32r23r13r31r21212
此外旋转的参考点不同结果也不同:基于固定坐标系的旋转变换左乘旋转矩阵,基于运动坐标系的旋转变换右乘旋转矩阵

推导参考:旋转矩阵左右乘不同的推导
左乘右乘的区别仅局限于多个旋转时,这里的固定矩阵也不一定是世界坐标系,而是旋转开始时的初始坐标系。运动坐标系指旋转后的坐标系

李群与李代数

群的概念

集合+运算符号。要求满足:

  • 封闭性
  • 结合律
  • 单位元

感觉有点像线性代数里面的向量空间的mini版,运算符号的要求变弱了

常见群:

  • 一般线性群,可逆矩阵,对矩阵乘法成群。
  • 特殊正交群SO(n):n维旋转矩阵群,可以用来当作旋转矩阵
  • 特殊欧式群SE(n):n维欧式变换群。

作用

例如研究旋转矩阵,旋转矩阵乘以旋转矩阵还是旋转矩阵,但是加上就不一定了,因此如果用线性代数中向量空间的方法去分析,会有很多都不能用,所以提出了群,研究群的性质。

Ref

GAMES101
GAMES202
视觉SLAM十四讲
Sola, Joan, Jeremie Deray, and Dinesh Atchuthan. “A micro Lie theory for state estimation in robotics.” arXiv preprint arXiv:1812.01537 (2018).


http://www.ppmy.cn/news/1543634.html

相关文章

Vue将所展示文本内容的换行与空格显示出来

使用<pre>标签 <pre>{{ content }}</pre>设置white-space样式&#xff08;推荐&#xff09; <div class"content">{{ content }} </div> .content{white-space: pre-wrap; }菜鸟教程&#xff1a;white-space: pre-wrap;的用途在于它…

QT-使用QSS美化UI界面

一、QSS简介&#xff1a; Qt Style Sheet&#xff1a;Qt样式表&#xff0c;用来自定义控件外观的一种机制&#xff0c;可以把他类比成CSS&#xff08;CSS主要功能与最终目的都是能使界面的表现与界面的元素分离&#xff09;。QSS机制使应用程序也能像web界面那样随意地改变外观…

瑞芯微 RKNN SDK 快速上手指南--Banana Pi开源社区

此文档面向零基础用户详细介绍如何快速在计算机上使用 RKNN-Toolkit2 完成模型转换&#xff0c;并通过 RKNPU2 部署到 Rockchip 开发板上。本文所用示例已集成到 RKNN Model Zoo 中。 支持的平台&#xff1a;RK3562、RK3566系列、RK3568系列、RK3576系列、RK3588系列 开发板&a…

H2 Database IDEA 源码 DEBUG 环境搭建

H2 Database IDEA 源码 DEBUG 环境搭建 基于最新的 version-2.3.230 拉取分支。 git remote add h2 https://github.com/h2database/h2database.git git fetch h2 git checkout -b version-2.3.230 version-2.3.230使用 # 启动 java -jar h2*.jar# H2 shell 方式使用 java …

如何使用java雪花算法在分布式环境中生成唯一ID?

引言 在现代分布式系统中,生成唯一标识符(ID)是一个常见的需求。传统的自增ID在分布式环境中会导致冲突,因此需要一种能够在分布式系统中生成全局唯一ID的算法。 雪花算法(Snowflake)就是为了解决这个问题而提出的一种高效的ID生成算法。本文将详细介绍雪花算法的原理、…

qt QAction详解

1、概述 QAction是Qt框架中的一个抽象类&#xff0c;用于表示用户界面中的一个动作&#xff08;action&#xff09;。这些动作可以绑定到菜单项、工具栏按钮或快捷键上&#xff0c;提供了一种灵活的方式来处理用户交互。QAction不仅包含了动作的名称、图标、提示信息等属性&am…

云智慧完成华为原生鸿蒙系统的适配, 透视宝 APM 为用户体验保驾护航

2024 年 10 月 22 日&#xff0c;首个国产移动操作系统 —— 华为原生鸿蒙操作系统 HarmonyOS NEXT 正式面世&#xff0c;成为继 iOS 和 Android 后的全球第三大移动操作系统。HarmonyOS NEXT&#xff0c;从系统内核、数据库根基&#xff0c;到编程语言创新、AI&#xff08;人工…

MAC | 应用全屏快捷键 |浏览器隐藏导航栏

问题1&#xff1a;有时候我们想要全屏应用&#xff0c;用鼠标又太麻烦了 解决&#xff1a; command ctrl F&#xff1a;全屏和取消全屏都是同一个键&#xff1b; 问题2: 浏览器上面的标签导航栏占据了屏幕的大部分空间&#xff0c;如下图所示&#xff0c;我们可以用快捷键将…