第一章 概论
位姿
= 位置
+ 姿态
(x, y, z) 可以确定空间中某一坐标系原点相对于基坐标系的
位置
,( α \alpha α, β \beta β, γ \gamma γ) 可以确定
该坐标系的朝向
。
- 一般来说,机器人具有6个自由度,可以在
灵活工作空间
内具有任意的位姿
;但是对于FPS游戏来说,人物角色具有5个自由度,WASD控制x
、y
自由度,space
控制z
自由度,鼠标左旋右旋控制Yaw
,鼠标前移后移控制Pitch
,但是一般不能旋转,也就是没有Roll
参数,这样会导致整个画面旋转,容易晕。
第二章 空间变换
末端执行器
位姿的变化既可以认为是坐标系的变化,也可以认为是点的变化,这里的变化指的是旋转
和平移
,旋转是线性变换
(线性变换指一个点变化到另一个点可以由左乘一个矩阵来得到,但是平移是相加),但是平移是非线性变换
,为了能够统一二者,引入了齐次坐标,在齐次坐标下的变换统称为仿射变换
。
旋转和平移如下图所示:
- Rotation(绕Z轴旋转
θ
\theta
θ)
R Z ( θ ) = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] R_{Z}(\theta ) = \begin{bmatrix} \cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0 & 0 & 1 \end{bmatrix} RZ(θ)= cosθsinθ0−sinθcosθ0001
[ x ′ y ′ z ′ ] = [ cos θ − sin θ 0 sin θ cos θ 0 0 0 1 ] [ x y z ] \begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \end{bmatrix} = \begin{bmatrix} \cos \theta & -\sin \theta & 0\\ \sin \theta & \cos \theta & 0\\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x \\ y \\ z \end{bmatrix} x′y′z′ = cosθsinθ0−sinθcosθ0001 xyz
- Translation(三个分量分别平移
△
x
、
△
y
、
△
z
\bigtriangleup x、\bigtriangleup y、\bigtriangleup z
△x、△y、△z)
T ( △ x , △ y , △ z ) = [ △ x △ y △ z ] T(\bigtriangleup x, \bigtriangleup y, \bigtriangleup z) = \begin{bmatrix} \bigtriangleup x \\ \bigtriangleup y \\ \bigtriangleup z \end{bmatrix} T(△x,△y,△z)= △x△y△z
[ x ′ y ′ z ′ ] = [ △ x △ y △ z ] + [ x y z ] \begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \end{bmatrix} = \begin{bmatrix} \bigtriangleup x \\ \bigtriangleup y \\ \bigtriangleup z \end{bmatrix} +\begin{bmatrix} x \\ y \\ z \end{bmatrix} x′y′z′ = △x△y△z + xyz
如何要进行连续变换,比如先旋转再平移,则不能写成一个矩阵乘积的形式,所以引入第四维齐次坐标。
- 齐次坐标下的旋转矩阵
R Z ( θ ) = [ cos θ − sin θ 0 0 sin θ cos θ 0 0 0 0 1 0 0 0 0 1 ] R_{Z}(\theta ) = \begin{bmatrix} \cos \theta & -\sin \theta & 0 & 0\\ \sin \theta & \cos \theta & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix} RZ(θ)= cosθsinθ00−sinθcosθ0000100001 - 齐次坐标下的平移矩阵
T ( △ x , △ y , △ z ) = [ 1 0 0 △ x 0 1 0 △ y 0 0 1 △ z 0 0 0 1 ] T(\bigtriangleup x, \bigtriangleup y, \bigtriangleup z) = \begin{bmatrix} 1 & 0 & 0 & \bigtriangleup x\\ 0 & 1 & 0 & \bigtriangleup y\\ 0 & 0 & 1 & \bigtriangleup z\\ 0 & 0 & 0 & 1 \end{bmatrix} T(△x,△y,△z)= 100001000010△x△y△z1
所以最终的变换矩阵可以写成如下形式
T
f
i
n
a
l
=
T
(
△
x
,
△
y
,
△
z
)
R
Z
(
θ
)
=
[
cos
θ
−
sin
θ
0
△
x
sin
θ
cos
θ
0
△
y
0
0
1
△
z
0
0
0
1
]
T_{final} = T(\bigtriangleup x, \bigtriangleup y, \bigtriangleup z)R_{Z}(\theta ) = \begin{bmatrix} \cos \theta & -\sin \theta & 0 & \bigtriangleup x\\ \sin \theta & \cos \theta & 0 & \bigtriangleup y\\ 0 & 0 & 1 & \bigtriangleup z\\ 0 & 0 & 0 & 1 \end{bmatrix}
Tfinal=T(△x,△y,△z)RZ(θ)=
cosθsinθ00−sinθcosθ000010△x△y△z1
2.1绕基向量旋转
接下来我们从两个角度来推导旋转矩阵(以绕Z轴为例)
- 从点变换的角度
x
=
cos
α
y
=
sin
α
z
=
c
o
n
s
t
a
n
t
x = \cos \alpha \\ y = \sin \alpha \\ z = constant
x=cosαy=sinαz=constant
旋转之后坐标变化
x
′
=
cos
(
α
+
θ
)
=
cos
α
cos
θ
−
sin
α
sin
θ
=
cos
θ
x
−
sin
θ
y
y
′
=
sin
(
α
+
θ
)
=
sin
α
cos
θ
+
cos
α
sin
θ
=
sin
θ
x
+
cos
θ
y
z
′
=
z
x' = \cos (\alpha + \theta) = \cos \alpha \cos \theta - \sin \alpha \sin \theta = \cos \theta x - \sin \theta y \\ y' = \sin (\alpha + \theta) = \sin \alpha \cos \theta + \cos \alpha \sin \theta = \sin \theta x + \cos \theta y\\ z' = z
x′=cos(α+θ)=cosαcosθ−sinαsinθ=cosθx−sinθyy′=sin(α+θ)=sinαcosθ+cosαsinθ=sinθx+cosθyz′=z
然后写成矩阵乘积的形式就是
P
′
=
R
Z
(
θ
)
P
=
[
cos
θ
−
sin
θ
0
sin
θ
cos
θ
0
0
0
1
]
[
x
y
z
]
P' = R_{Z}(\theta )P = \begin{bmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{bmatrix} \begin{bmatrix} x\\ y\\ z \end{bmatrix}
P′=RZ(θ)P=
cosθsinθ0−sinθcosθ0001
xyz
3. 从坐标系变换的角度
X
^
\hat{X}
X^方向的基向量从 (1, 0, 0) 变换到
(
cos
θ
,
sin
θ
,
0
)
(\cos \theta , \sin \theta , 0)
(cosθ,sinθ,0),
Y
^
\hat{Y}
Y^方向的基向量从 (0, 1, 0) 变换到
(
−
sin
θ
,
cos
θ
,
0
)
(-\sin \theta , \cos \theta , 0)
(−sinθ,cosθ,0),
Z
^
\hat{Z}
Z^方向的基向量不变,还是 (0, 0, 1)。然后把变化后的基向量填写到矩阵的每一列,可得出旋转矩阵
B
′
B
R
=
[
cos
θ
−
sin
θ
0
sin
θ
cos
θ
0
0
0
1
]
_{B'}^{B}R = \begin{bmatrix} \cos \theta & -\sin \theta & 0 \\ \sin \theta & \cos \theta & 0 \\ 0 & 0 & 1 \end{bmatrix}
B′BR=
cosθsinθ0−sinθcosθ0001
绕X、Y轴同理。
2.2绕任意轴旋转(Rodrigues’ rotation formula)
设
P
P
P点绕
n
^
\hat{n}
n^旋转
θ
\theta
θ到
P
′
P'
P′(
n
^
、
P
\hat{n}、P
n^、P已知)。
旋转前:
P
=
P
⊥
+
P
∥
P = P_{\perp } + P_{\parallel }
P=P⊥+P∥
P
∥
=
(
n
^
⋅
P
)
n
^
(
p
r
o
j
e
c
t
i
o
n
)
(1)
P_{\parallel} = (\hat{n} \cdot P)\hat{n} (projection)\tag{1}
P∥=(n^⋅P)n^(projection)(1)
P
⊥
=
P
−
P
∥
=
P
−
(
n
^
⋅
P
)
n
^
(2)
P_{\perp} = P - P_{\parallel} = P - (\hat{n} \cdot P)\hat{n} \tag{2}
P⊥=P−P∥=P−(n^⋅P)n^(2)
旋转后:
P
′
=
P
⊥
′
+
P
∥
′
P' = P'_{\perp } + P'_{\parallel }
P′=P⊥′+P∥′
又
∵
\because
∵
P
∥
′
=
P
∥
P'_{\parallel} = P_{\parallel}
P∥′=P∥
∴
\therefore
∴
P
⊥
′
=
P
′
−
P
∥
(3)
P'_{\perp } = P' - P_{\parallel } \tag{3}
P⊥′=P′−P∥(3)
看向
−
n
^
-\hat{n}
−n^可得到如下坐标系
n
^
×
P
⊥
=
n
^
×
(
P
−
P
∥
)
=
n
^
×
P
−
n
^
×
P
∥
=
n
^
×
P
\hat{n}\times P_{\perp} = \hat{n} \times (P - P_{\parallel}) = \hat{n} \times P - \hat{n} \times P_{\parallel} = \hat{n} \times P
n^×P⊥=n^×(P−P∥)=n^×P−n^×P∥=n^×P
且
∥
n
^
×
P
∥
=
∥
n
^
×
P
⊥
∥
=
∥
n
^
∥
⋅
∥
P
⊥
∥
⋅
sin
π
2
=
∥
P
⊥
∥
\left \| \hat{n} \times P \right \| = \left \| \hat{n} \times P_{\perp} \right \| = \left \| \hat{n} \right \| \cdot \left \| P_{\perp} \right \| \cdot \sin \frac{\pi }{2} = \left \| P_{\perp} \right \|
∥n^×P∥=∥n^×P⊥∥=∥n^∥⋅∥P⊥∥⋅sin2π=∥P⊥∥
又
∵
\because
∵
∥
P
⊥
′
∥
=
∥
P
⊥
∥
\left \| P'_{\perp} \right \| = \left \| P_{\perp} \right \|
∥P⊥′∥=∥P⊥∥
∴
\therefore
∴
P
⊥
′
=
∥
P
⊥
∥
cos
θ
P
⊥
∥
P
⊥
∥
+
∥
P
⊥
∥
sin
θ
n
^
×
P
∥
n
^
×
P
∥
=
cos
θ
P
⊥
+
sin
θ
(
n
^
×
P
)
(4)
P'_{\perp} = \left \| P_{\perp} \right \| \cos \theta \frac{P_{\perp}}{\left \| P_{\perp} \right \|} + \left \| P_{\perp} \right \| \sin \theta \frac{\hat{n} \times P}{\left \| \hat{n} \times P \right \|} = \cos \theta P_{\perp} + \sin \theta (\hat{n} \times P) \tag{4}
P⊥′=∥P⊥∥cosθ∥P⊥∥P⊥+∥P⊥∥sinθ∥n^×P∥n^×P=cosθP⊥+sinθ(n^×P)(4)
由(1)和(4)得
P
′
=
P
⊥
′
+
P
∥
′
=
P
⊥
′
+
P
∥
=
cos
θ
P
+
(
1
−
cos
θ
)
(
n
^
⋅
P
)
n
^
+
sin
θ
(
n
^
×
P
)
(5)
P' = P'_{\perp} + P'_{\parallel} = P'_{\perp} + P_{\parallel} = \cos \theta P + (1 - \cos \theta)(\hat{n} \cdot P)\hat{n} + \sin \theta (\hat{n} \times P) \tag{5}
P′=P⊥′+P∥′=P⊥′+P∥=cosθP+(1−cosθ)(n^⋅P)n^+sinθ(n^×P)(5)
第三章 正运动学
- 连杆长度:两轴间
公垂线
的长度,记作 a i a_{i} ai,沿 X i X_{i} Xi轴,从 Z i Z_{i} Zi移动到 Z i + 1 Z_{i+1} Zi+1的距离; - 连杆转角:两轴线形成的
夹角
,记作 α i \alpha _{i} αi,绕 X i X_{i} Xi轴,从 Z i Z_{i} Zi旋转到 Z i + 1 Z_{i+1} Zi+1的角度; - 连杆偏距:两相邻公垂线之间的长度,记作 d i d_{i} di,沿 Z i Z_{i} Zi轴,从 X i − 1 X_{i - 1} Xi−1移动到 X i X_{i} Xi的距离;
- 关节角:两相邻公垂线之间的夹角,记作 θ i \theta_{i} θi,绕 Z i Z_{i} Zi轴,从 X i − 1 X_{i-1} Xi−1旋转到 X i X_{i} Xi的角度。
正运动学主要解决已知上述参数,求解出
末端执行器
的位置的问题,一般问题中会固定其中的一些参数。
机器臂系统中常用的五个坐标系定义如下:
- 基座标系{B};
- 腕部坐标系{W};
- 工具坐标系{T};
- 工作台坐标系{S};
- 目标坐标系{G}。
最终的目的就是让工具坐标系
和目标坐标系
重合。
第四章 逆运动学
逆运动学所要求解的方程是非线性方程,求解方法主要分为解析解
和数值解
两大类,数值解
需要迭代,一般情况下比较慢;解析解
又分为代数法
和几何法
两种。
v-rep中simIK工具包
在v-rep中想要求解FK/IK
任务需要理解IK elements
和IK groups
这两个概念,一个IK task
由一个IK group
和一个或多个IK elements
组成。
IK element
代表了一个简单的运动链(kinematics chain
),一个运动链至少要有一个关节,一般还要有一个基座,一个末端操纵器和一个目标点。具体可以由以下组件组成:- 基座:代表运动链的开始
- 一些连杆
- 一些关节
- 一个末端操纵器(
end-effector
) - 一个目标点
IK group
包含一个或多个IK element
,在单次仿真中可以对多个运动链求解,且可以设置求解其、迭代次数等参数。
API
想要完成一个inverse kinematics
问题求解,需要以下几个函数
int environmentHandle=simIK.createEnvironment(int flags=0)
创建求解环境,相当于initint ikGroupHandle=simIK.createGroup(int environmentHandle,string ikGroupName='')
创建groupsimIK.setGroupCalculation(int environmentHandle,int ikGroupHandle,int method,float damping,float maxIterations)
设置求解参数(求解方法、迭代次数等)int ikElement,map simToIkObjectMap,map ikToSimObjectMap=simIK.addElementFromScene(int environmentHandle,int ikGroup,int baseHandle,int tipHandle,int targetHandle,int constraints)
从场景中加载模型,生成IK elementsimIK.setElementFlags(int environmentHandle,int ikGroupHandle,int elementHandle,int flags)
设置IK element属性int result,int flags,float[2] precision=simIK.handleGroup(int environmentHandle,int ikGroupHandle,map options={})
求解
1~5
一般在脚本的sysCall_init()
中设置,6
在sysCall_actuation
中设置。
生成IK element
所依赖的模型不一定要从场景中获取真实的具有网格形体的模型,也可以在simIK
单独创建虚拟的模型,具备相应的数据也可进行仿真求解。