概率密度变换公式 雅可比矩阵_【机器人学】微分变换与雅可比矩阵

假定有三个坐标系W、A、B,其中W为基坐标系。

变换矩阵的左乘和右乘

当坐标系A变换成坐标系B时,可左乘一个矩阵,也可右乘一个矩阵

对于左乘的情况,变换矩阵为

表示的是一个基于基坐标系的平移和绕轴旋转的变换,式中的变量均以基坐标系为参考坐标系。

对于右乘的情况,变换矩阵为

表示的是一个基于A坐标系(联体坐标系)的平移和绕轴旋转的变换,变换矩阵刚好为B坐标系在A坐标系下的表示,式中的变量均以A坐标系为参考坐标系。

微分变换

当变换前后的两个坐标系非常接近时,变换矩阵简化为

math?formula=%5Cbegin%7Baligned%7D%20%7B%5EW%7DT%7B_U%7D%20%26%3D%20Trans(d%7B_x%7D%2Cd%7B_y%7D%2Cd%7B_z%7D)Rot(f%2Cd%7B_%5Ctheta%7D)%20%5C%5C%20%26%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-f%7B_z%7Df%7B_%5Ctheta%7D%20%26%20f%7B_y%7Df%7B_%5Ctheta%7D%20%26%20d%7B_x%7D%20%5C%5C%20f%7B_z%7Df%7B_%5Ctheta%7D%20%26%200%20%26%20-f%7B_x%7Df%7B_%5Ctheta%7D%20%26%20d%7B_y%7D%20%5C%5C%20-f%7B_y%7Df%7B_%5Ctheta%7D%20%26%20f%7B_x%7Df%7B_%5Ctheta%7D%20%26%200%20%26%20d%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5C%5C%20%26%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-%5Cdelta%7B_z%7D%20%26%20%5Cdelta%7B_y%7D%20%26%20d%7B_x%7D%20%5C%5C%20%5Cdelta%7B_z%7D%20%26%200%20%26%20-%5Cdelta%7B_x%7D%20%26%20d%7B_y%7D%20%5C%5C%20-%5Cdelta%7B_y%7D%20%26%20%5Cdelta%7B_x%7D%20%26%200%20%26%20d%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5Cend%7Baligned%7D

math?formula=%5Cbegin%7Baligned%7D%20%7B%5EA%7DT%7B_B%7D%20%26%3D%20Trans(%7B%5ET%7Dd%7B_x%7D%2C%7B%5ET%7Dd%7B_y%7D%2C%7B%5ET%7Dd%7B_z%7D)Rot(%7B%5ET%7Df%2C%7B%5ET%7Dd%7B_%5Ctheta%7D)%20%5C%5C%20%26%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-%7B%5ET%7Df%7B_z%7D%7B%5ET%7Df%7B_%5Ctheta%7D%20%26%20%7B%5ET%7Df%7B_y%7D%7B%5ET%7Df%7B_%5Ctheta%7D%20%26%20%7B%5ET%7Dd%7B_x%7D%20%5C%5C%20%7B%5ET%7Df%7B_z%7D%7B%5ET%7Df%7B_%5Ctheta%7D%20%26%200%20%26%20-%7B%5ET%7Df%7B_x%7D%7B%5ET%7Df%7B_%5Ctheta%7D%20%26%20%7B%5ET%7Dd%7B_y%7D%20%5C%5C%20-%7B%5ET%7Df%7B_y%7D%7B%5ET%7Df%7B_%5Ctheta%7D%20%26%20%7B%5ET%7Df%7B_x%7D%7B%5ET%7Df%7B_%5Ctheta%7D%20%26%200%20%26%20%7B%5ET%7Dd%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5C%5C%20%26%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-%7B%5ET%7D%5Cdelta%7B_z%7D%20%26%20%7B%5ET%7D%5Cdelta%7B_y%7D%20%26%20%7B%5ET%7Dd%7B_x%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_z%7D%20%26%200%20%26%20-%7B%5ET%7D%5Cdelta%7B_x%7D%20%26%20%7B%5ET%7Dd%7B_y%7D%20%5C%5C%20-%7B%5ET%7D%5Cdelta%7B_y%7D%20%26%20%7B%5ET%7D%5Cdelta%7B_x%7D%20%26%200%20%26%20%7B%5ET%7Dd%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5Cend%7Baligned%7D

两种坐标系下微分变换平移和旋转的关系

由于左乘和右乘两种变换是等价的,即

其中

得到基坐标系下和联体坐标系下微分变换之间的关系

math?formula=%5Cbegin%7Baligned%7D%20%7B%5EA%7DT%7B_B%7D%20%26%3D%20%7B%5EW%7DT%7B_A%7D%7B%5E%7B-1%7D%7D%20%7B%5EW%7DT%7B_U%7D%20%7B%5EW%7DT%7B_A%7D%20%5C%5C%20%26%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-%5Cdelta.a%20%26%20%5Cdelta.o%20%26%20%5Cdelta.(p%20%5Ctimes%20n)%2Bd.n%20%5C%5C%20%5Cdelta.a%20%26%200%20%26%20-%5Cdelta.n%20%26%20%5Cdelta.(p%20%5Ctimes%20o)%2Bd.o%20%5C%5C%20-%5Cdelta.o%20%26%20%5Cdelta.n%20%26%200%20%26%20%5Cdelta.(p%20%5Ctimes%20a)%2Bd.a%20%5Cend%7Bbmatrix%7D%20%5C%5C%20%26%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-%7B%5ET%7D%5Cdelta%7B_z%7D%20%26%20%7B%5ET%7D%5Cdelta%7B_y%7D%20%26%20%7B%5ET%7Dd%7B_x%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_z%7D%20%26%200%20%26%20-%7B%5ET%7D%5Cdelta%7B_x%7D%20%26%20%7B%5ET%7Dd%7B_y%7D%20%5C%5C%20-%7B%5ET%7D%5Cdelta%7B_y%7D%20%26%20%7B%5ET%7D%5Cdelta%7B_x%7D%20%26%200%20%26%20%7B%5ET%7Dd%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5Cend%7Baligned%7D

由此可得到基坐标系下和联体坐标系下平移旋转微分量之间的关系

math?formula=%5Cbegin%7Bbmatrix%7D%20%7B%5ET%7Dd%7B_x%7D%20%5C%5C%20%7B%5ET%7Dd%7B_y%7D%20%5C%5C%20%7B%5ET%7Dd%7B_z%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_x%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_y%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%20n%7B_x%7D%20%26%20n%7B_y%7D%20%26%20n%7B_z%7D%20%26%20(p%20%5Ctimes%20n)%7B_x%7D%20%26%20(p%20%5Ctimes%20n)%7B_y%7D%20%26%20(p%20%5Ctimes%20n)%7B_z%7D%20%5C%5C%20o%7B_x%7D%20%26%20o%7B_y%7D%20%26%20o%7B_z%7D%20%26%20(p%20%5Ctimes%20o)%7B_x%7D%20%26%20(p%20%5Ctimes%20o)%7B_y%7D%20%26%20(p%20%5Ctimes%20o)%7B_z%7D%20%5C%5C%20a%7B_x%7D%20%26%20a%7B_y%7D%20%26%20a%7B_z%7D%20%26%20(p%20%5Ctimes%20a)%7B_x%7D%20%26%20(p%20%5Ctimes%20a)%7B_y%7D%20%26%20(p%20%5Ctimes%20a)%7B_z%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%20n%7B_x%7D%20%26%20n%7B_y%7D%20%26%20n%7B_z%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%20o%7B_x%7D%20%26%20o%7B_y%7D%20%26%20o%7B_z%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%20a%7B_x%7D%20%26%20a%7B_y%7D%20%26%20a%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7D%20d%7B_x%7D%20%5C%5C%20d%7B_y%7D%20%5C%5C%20d%7B_z%7D%20%5C%5C%20%5Cdelta%7B_x%7D%20%5C%5C%20%5Cdelta%7B_y%7D%20%5C%5C%20%5Cdelta%7B_z%7D%20%5Cend%7Bbmatrix%7D

math?formula=%5Cbegin%7Bbmatrix%7D%20d%7B_x%7D%20%5C%5C%20d%7B_y%7D%20%5C%5C%20d%7B_z%7D%20%5C%5C%20%5Cdelta%7B_x%7D%20%5C%5C%20%5Cdelta%7B_y%7D%20%5C%5C%20%5Cdelta%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%20n%7B_x%7D%20%26%20o%7B_x%7D%20%26%20a%7B_x%7D%20%26%20(p%20%5Ctimes%20n)%7B_x%7D%20%26%20(p%20%5Ctimes%20o)%7B_x%7D%20%26%20(p%20%5Ctimes%20a)%7B_x%7D%20%5C%5C%20n%7B_y%7D%20%26%20o%7B_y%7D%20%26%20a%7B_y%7D%20%26%20(p%20%5Ctimes%20n)%7B_y%7D%20%26%20(p%20%5Ctimes%20o)%7B_y%7D%20%26%20(p%20%5Ctimes%20a)%7B_y%7D%20%5C%5C%20n%7B_z%7D%20%26%20o%7B_z%7D%20%26%20a%7B_z%7D%20%26%20(p%20%5Ctimes%20n)%7B_z%7D%20%26%20(p%20%5Ctimes%20o)%7B_z%7D%20%26%20(p%20%5Ctimes%20a)%7B_z%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%20n%7B_x%7D%20%26%20o%7B_x%7D%20%26%20a%7B_x%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%20n%7B_y%7D%20%26%20o%7B_y%7D%20%26%20a%7B_y%7D%20%5C%5C%200%20%26%200%20%26%200%20%26%20n%7B_z%7D%20%26%20o%7B_z%7D%20%26%20a%7B_z%7D%20%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7D%20%7B%5ET%7Dd%7B_x%7D%20%5C%5C%20%7B%5ET%7Dd%7B_y%7D%20%5C%5C%20%7B%5ET%7Dd%7B_z%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_x%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_y%7D%20%5C%5C%20%7B%5ET%7D%5Cdelta%7B_z%7D%20%5Cend%7Bbmatrix%7D

math?formula=R%20%3D%20%5Cbegin%7Bbmatrix%7D%20n%7B_x%7D%20%26%20o%7B_x%7D%20%26%20a%7B_x%7D%20%5C%5C%20n%7B_y%7D%20%26%20o%7B_y%7D%20%26%20a%7B_y%7D%20%5C%5C%20n%7B_z%7D%20%26%20o%7B_z%7D%20%26%20a%7B_z%7D%20%5Cend%7Bbmatrix%7D%2C%20S(p)%20%3D%20%5Cbegin%7Bbmatrix%7D%200%20%26%20-p%7B_z%7D%20%26%20p%7B_y%7D%20%5C%5C%20p%7B_z%7D%20%26%200%20%26%20-p%7B_x%7D%20%5C%5C%20-p%7B_y%7D%20%26%20p%7B_x%7D%20%26%200%20%5Cend%7Bbmatrix%7D

微分变换的无序性

绕各个轴旋转的变换矩阵分别为

math?formula=Rot(x%2C%20%5Cdelta%7B_x%7D)%20%3D%20%5Cbegin%7Bbmatrix%7D%201%20%26%200%20%26%200%20%26%200%20%5C%5C%200%20%26%201%20%26%20-%5Cdelta%7B_x%7D%20%26%200%20%5C%5C%200%20%26%20%5Cdelta%7B_x%7D%20%26%201%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7D%20%5C%5C%20Rot(y%2C%20%5Cdelta%7B_y%7D)%20%3D%20%5Cbegin%7Bbmatrix%7D%201%20%26%200%20%26%20%5Cdelta%7B_y%7D%20%26%200%20%5C%5C%200%20%26%201%20%26%200%20%26%200%20%5C%5C%20-%5Cdelta%7B_y%7D%20%26%200%20%26%201%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7D%20%5C%5C%20Rot(z%2C%20%5Cdelta%7B_z%7D)%20%3D%20%5Cbegin%7Bbmatrix%7D%201%20%26%20-%5Cdelta%7B_z%7D%20%26%200%20%26%200%20%5C%5C%20%5Cdelta%7B_z%7D%20%26%201%20%26%200%20%26%200%20%5C%5C%200%20%26%200%20%26%201%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7D

XYZ和ZYX旋转结果分别为

math?formula=Rot(x%2C%20%5Cdelta%7B_x%7D)%20Rot(y%2C%20%5Cdelta%7B_y%7D)%20Rot(z%2C%20%5Cdelta%7B_z%7D)%20%3D%20%5Cbegin%7Bbmatrix%7D%201%20%26%20-%5Cdelta%7B_z%7D%20%26%20%5Cdelta%7B_y%7D%20%26%200%20%5C%5C%20%5Cdelta%7B_x%7D%5Cdelta%7B_y%7D%2B%5Cdelta%7B_z%7D%20%26%201-%5Cdelta%7B_x%7D%5Cdelta%7B_y%7D%5Cdelta%7B_z%7D%20%26%20-%5Cdelta%7B_x%7D%20%26%200%20%5C%5C%20-%5Cdelta%7B_y%7D%2B%5Cdelta%7B_x%7D%5Cdelta%7B_z%7D%20%26%20%5Cdelta%7B_y%7D%5Cdelta%7B_z%7D%2B%5Cdelta%7B_x%7D%20%26%201%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7D

math?formula=Rot(z%2C%20%5Cdelta%7B_z%7D)%20Rot(y%2C%20%5Cdelta%7B_y%7D)%20Rot(x%2C%20%5Cdelta%7B_x%7D)%20%3D%20%5Cbegin%7Bbmatrix%7D%201%20%26%20-%5Cdelta%7B_z%7D%2B%5Cdelta%7B_x%7Ddelta%7B_y%7D%20%26%20%5Cdelta%7B_x%7D%5Cdelta%7B_z%7D%2B%5Cdelta%7B_y%7D%20%26%200%20%5C%5C%20%5Cdelta%7B_x%7D%20%26%201%2B%5Cdelta%7B_x%7D%5Cdelta%7B_y%7D%5Cdelta%7B_z%7D%20%26%20-%5Cdelta%7B_x%7D%2B%5Cdelta%7B_y%7D%5Cdelta%7B_z%7D%20%26%200%20%5C%5C%20-%5Cdelta%7B_y%7D%20%26%20%5Cdelta%7B_x%7D%20%26%201%20%26%200%20%5C%5C%200%20%26%200%20%26%200%20%26%201%20%5Cend%7Bbmatrix%7D

在忽略高阶无穷小的前提下,两式结果相同。另外,用同样的方法容易验证微小平移和微小旋转之间与变换顺序无关。在忽略高阶无穷小的前提下(即多个变分相乘的项),微分变换与次序无关,即微分变换具有无序性。

雅可比矩阵

雅可比矩阵为笛卡尔空间与关节空间的速度之间的关系

math?formula=%5Cbegin%7Bbmatrix%7D%20v%20%5C%5C%20%5Comega%20%5Cend%7Bbmatrix%7D%20%3D%20%5Cbegin%7Bbmatrix%7D%20J_%7B11%7D%20%26%20J_%7B12%7D%20%26%20...%20%26%20J_%7B1n%7D%20%5C%5C%20J_%7B21%7D%20%26%20J_%7B22%7D%20%26%20...%20%26%20J_%7B2n%7D%20%5C%5C%20J_%7B31%7D%20%26%20J_%7B32%7D%20%26%20...%20%26%20J_%7B3n%7D%20%5C%5C%20J_%7B41%7D%20%26%20J_%7B42%7D%20%26%20...%20%26%20J_%7B4n%7D%20%5C%5C%20J_%7B51%7D%20%26%20J_%7B52%7D%20%26%20...%20%26%20J_%7B5n%7D%20%5C%5C%20J_%7B61%7D%20%26%20J_%7B62%7D%20%26%20...%20%26%20J_%7B6n%7D%20%5C%5C%20%5Cend%7Bbmatrix%7D%20%5Cbegin%7Bbmatrix%7D%20q%5E%7B%5Cprime%7D_%7B1%7D%20%5C%5C%20q%5E%7B%5Cprime%7D_%7B2%7D%20%5C%5C%20...%20%5C%5C%20q%5E%7B%5Cprime%7D_%7Bn-1%7D%20%5C%5C%20q%5E%7B%5Cprime%7D_%7Bn%7D%20%5C%5C%20%5Cend%7Bbmatrix%7D

可得到微分运动量之间的关系

转动关节旋转时,以当前位置的连杆坐标系为参考坐标系,则微分运动的连杆坐标系绕其z轴旋转,该旋转运动在该坐标系下的微分平移和微分旋转矢量为

由此基于当前位置的连杆坐标系关于该关节的雅可比向量为

乘上基坐标系下和联体坐标系下平移旋转微分量之间关系的矩阵,转换到机器人基坐标系为

你可能感兴趣的:(概率密度变换公式,雅可比矩阵)