matlab机器人工具箱学习笔记——瞬态运动学
瞬态运动学与雅克比矩阵
瞬态运动学
假设末端执行器的位姿是 x x x，关节角是 θ \theta θ，则线速度是 x ′ x' x′，关节角速度为 θ ′ \theta' θ′。
·对于正向运动学，需要解决的问题是： θ → x \theta\to x θ→x。
·对于逆运动学，需要解决的问题是： x → θ x \toθ x→θ。
·而对于瞬态运动学： θ + δ θ → x + δ x θ+δθ \to x+δx θ+δθ→x+δx，需要解决的问题则是： δ θ → δ x δθ \to δx δθ→δx,即从关节角速度到线速度： θ ′ → x ′ θ'\to x' θ′→x′。
雅克比矩阵
机械臂位姿 x x x与关节变量 q q q的函数关系是：
x = f ( q ) x=f(q) x=f(q)
机械臂的雅克比矩阵定义为机械臂末端的笛卡尔速度与关节速度的线性变换：
x ′ = J ( q ) q ′ x'=J(q)q' x′=J(q)q′
其中 J ( q ) J(q) J(q)为机械臂的雅克比矩阵，为 m × n m×n m×n的偏导数矩阵。
我们亦可得：
q ′ = J ( q ) − 1 x ′ q'=J(q)^{-1}x' q′=J(q)−1x′
将末端执行器的线速度 ( v x , v y , v z ) (v_x,v_y,v_z) (vx,vy,vz)和角速度 ( w x , w y , w z ) (w_x,w_y,w_z) (wx,wy,wz)这六个变量作为雅克比矩阵等式的左边：
[ v x v y v z w x w y w z ] = J 0 ( 6 × 1 ) q ( n × 1 ) ′ \begin{bmatrix} v_x\\ v_y\\ v_z\\ w_x\\ w_y\\ w_z\\ \end{bmatrix}=J_{0(6×1)}q'_{(n×1)} ⎣⎢⎢⎢⎢⎢⎢⎡vxvyvzwxwywz⎦⎥⎥⎥⎥⎥⎥⎤=J0(6×1)q(n×1)′
n n n代表机器人的自由度， J 0 J_0 J0可以用于对速度的表述。因为所有对速度的表述都与线速度和角速度相关，任何与速度有关的表述都可以用这个雅克比矩阵建立联系。
将雅克比矩阵去掉下标（即 J J J）作为基本雅克比矩阵，其分为两部分：
J = [ J v J w ] J= \begin{bmatrix} J_v\\ J_w\\ \end{bmatrix} J=[JvJw]
有
J v = [ ∂ x p ∂ q 1 ∂ x p ∂ q 2 . . . ∂ x p ∂ q n ] J_v=\begin{bmatrix} \frac{\partial x_p}{\partial q_1} & \frac{\partial x_p}{\partial q_2}& ...& \frac{\partial x_p}{\partial q_n} \end{bmatrix} Jv=[∂q1∂xp∂q2∂xp...∂qn∂xp]
和
w = [ ϵ 1 ‾ z 1 ϵ 2 ‾ z 2 . . . ϵ n ‾ z n ] w= \begin{bmatrix} \overline{\epsilon_1}z_1& \overline{\epsilon_2}z_2& ...& \overline{\epsilon_n}z_n \end{bmatrix} w=[ϵ1z1ϵ2z2...ϵnzn]
解法
我们可以使用SerialLink.jacob0()求出对应某个位姿下世界坐标系中的雅克比矩阵
可用函数SerialLink.jacobn()求出对应某个位姿下工具坐标系中的雅克比矩阵
>> mdl_KR5
>> q=[0 pi/2 pi pi/2 pi/4 pi/2]
>>J0=KR5.jacob0(q)
运行结果
J0 =
-0.0813 -0.4800 -0.1200 0.0000 0.0813 0
-0.5213 -0.0000 0.0000 -0.0000 0.0813 0
0.0000 -0.7013 0.7013 -0.0813 0 0
-0.0000 0 0.0000 -1.0000 0.0000 -0.7071
0 -1.0000 1.0000 0.0000 0.0000 0.7071
1.0000 0.0000 -0.0000 -0.0000 -1.0000 -0.0000
输入：
>> Jn=KR5.jacobn(q)
运行结果：
Jn =
0.0000 -0.7013 0.7013 -0.0813 -0.0000 0
-0.4261 -0.3394 -0.0849 -0.0000 0.1150 0
-0.3111 0.3394 0.0849 -0.0000 0 0
1.0000 0.0000 -0.0000 0.0000 -1.0000 0
0.0000 -0.7071 0.7071 -0.7071 -0.0000 0
-0.0000 -0.7071 0.7071 0.7071 0.0000 1.0000
雅克比矩阵参考坐标系的变换
A J ( q ) = [ B A R 0 0 B A R ] B J ( q ) {^A}J(q)= \begin{bmatrix} {^A_B}R&0\\ 0&{^A_B}R \end{bmatrix} {^B}J(q) AJ(q)=[BAR00BAR]BJ(q)
R可以使用R=t2r(T)获得
速度的笛卡尔变换
[ B v B B w B ] = [ A B R − A B R × A P B 0 A B R ] [ A v A A w B ] \begin{bmatrix} {^B}v_B\\ {^B}w_B \end{bmatrix}= \begin{bmatrix} {^B_A}R&-{^B_A}R×{^A}P_B\\ 0&{^B_A}R \end{bmatrix} \begin{bmatrix} {^A}v_A\\ {^A}w_B \end{bmatrix} [BvBBwB]=[ABR0−ABR×APBABR][AvAAwB]
注意：可用函数tr2jac()求出不同变换的雅克比矩阵。
例如，{ B B B}是通过{ A A A}平移(2,4,0)，在旋转45°得到的，求速度变换矩阵 T v T_v Tv。当{ A A A}中的 X X X方向的线速度为 2 m / s 2m/s 2m/s,求{ B B B}中速度。
>>T=transl(2,4,0)*troty(pi/4)
>>Tv=tr2jac(T)
运行结果：
Tv =
0.7071 0 -0.7071 -2.8284 1.4142 -2.8284
0 1.0000 0 0 0 2.0000
0.7071 0 0.7071 2.8284 -1.4142 -2.8284
0 0 0 0.7071 0 -0.7071
0 0 0 0 1.0000 0
0 0 0 0.7071 0 0.7071
输入命令：
vB=Tv*[2 0 0 0 0 0]';
vB =
1.4142 %%%在{B}中的X方向的速度
0
1.4142 %%%在{B}中的Z方向的速度
0
0
0
我也只是个复读机，顺便学习一下富文本的编写
这应该算准备知识8
参考资料：
杨辰光, 李智军, 许扬，机器人仿真与编程技术[M].北京:清华大学出版社,2018