刚体运动的表示及其Mathematica实现

刚体运动理论基础

刚体在空间中运动时，其运动只能是三种情况：
1、纯平移
2、绕一个轴的纯旋转
3、平移与旋转的结合
一、纯平移
在三维空间中，一般平移用一个向量表示，如平移向量为D=(dx,dy,dz)，一般习惯写成列向量的形式：

空间中任意一点P(x,y,z)经过上述向量D平移后的新坐标为

二、绕一个轴的纯旋转
关于右手笛卡尔坐标系的 x-, y- 和 z-轴的旋转也习惯分别叫做 roll, pitch 和 yaw 旋转

三、平移与旋转的结合
当刚体是既有平移又有旋转的复合运动，为了方便计算和表示，一般会把上述两种变换矩阵结合起来，使其能同时表示平移和旋转。将一个3 x 3旋转矩阵和3 x 1位移向量组合成一个4 x 4齐次变换矩阵是以后会经常用到的操作。例如将上述绕 X 轴旋转和平移向量合成一个齐次变换矩阵为

Mathematica实现

方法（一）-现有函数包
Mathematica系统没有直接实现这个功能的函数，但有大牛写的第三方函数包可以实现，如：Screws.m，该程序包基于旋量理论，其中有个函数RPToHomogeneous可以实现此功能。该程序包在此处可以下载
使用方法1：先安装该程序包，然后直接调用。
使用方法2：先设置该程序包所在目录，然后载入，即可调用。
个人推荐使用方法2，演示如下

SetDirectory["Z:\Wushuichuan\05-计算仿真\基于Mathematica机器人仿真\Packages"];
<< Screws.m
R = RotationMatrix[\[CurlyTheta], {1, 0, 0}];(*产生一个绕X轴旋转\[CurlyTheta]角的旋转矩阵*)
P = {dx, dy, dz};
RPToH = RPToHomogeneous[R, P] // MatrixForm

值得注意的是，在使用RPToHomogeneous时，要注意第二个参数的形式，否则会出错。应该使用坐标点（向量）的形式，即{dx,dy,dz}。
方法（二）-自定义函数
要在Mathematica中实现矩阵合并的功能并不难，毕竟是宇宙第一的科学计算软件，这怎么能难倒MMA呢？仅一行就搞定

RPToH[u_, v_] := ArrayFlatten[{{u, v}, {0, 1}}];(* u是3x3旋转矩阵，v是3x1平移矩阵 *)
R = RotationMatrix[\[CurlyTheta], {1, 0, 0}];
(*产生一个绕X轴旋转\[CurlyTheta]角的旋转矩阵*)
P = {{dx}, {dy}, {dz}};
RPToH[R, P] // MatrixForm

注意上述代码中的注释，v是矩阵，不能是坐标点或向量的形式，否则会出错。

注意上述两个合并旋转矩阵和平移矩阵的方法中，平移向量的定义是不一样的，在方法（一）中，P是一个一维列表；在方法（二）中，P是一个二维列表，但其实它们写成矩阵形式是一样的，不同的是它们的数据结构不同，这里与函数ArrayFlatten有关。

P1 = {dx, dy, dz} // MatrixForm
P2 = {{dx}, {dy}, {dz}} // MatrixForm