正运动学分析详解

Robotics System Toolbox模块库主要包含“Manipulator Algorithms”“Mobile Robot Algorithms”“ROS”和“Utilities”四个模块，如图7.38所示。

图7.38　Robotics System Toolbox模块库

其中，Manipulator Algorithms子模块库以RigidBodyTree（刚体树）实例对象来构建机器人的模型，主要用于构建高级运动控制器并与机器人模型交互、在机器人模型上执行碰撞检查及正逆运动学和动力学计算。Mobile Robot Algorithms子模块库主要用于移动机器人的路径跟随和避障。ROS子模块库主要用作ROS模拟器，可进行ROS设置并获取ROS的相关信息。Utilities子模块库包含附加模块，主要用于坐标系转换。

Manipulator Algorithms子模块库中包含可用于机器人正运动学计算的Get Transform模块，如图7.39所示。该模块用于在给定系统模型下计算任意两个实体框架间的坐标变换矩阵。该模块接受N×1的关节转角行向量作为Config接口的输入，单位为m或rad，输出一个4×4的坐标转换矩阵。可选取自定义的刚体树（即假设机器人的各关节机械臂为刚体，对其分别进行连接形成的刚体树）实例对象作为模块模型参数的输入。

通过RigidBodyTree构造函数来构建刚体树实例对象，并通过该机器人各关节的几何和物理参数构造各关节的刚体实例对象，最后将各关节刚体实例对象添加到整个刚体树实例对象中构造出机器人的刚体树模型。新建脚本命名为sixJointRigidBodyTree.m，并添加以下代码：

图7.39　Get Transform模块

以第6章中的Staubli TX200六轴机器人为例，标准D-H参数可参考表6.4。在脚本中添加该模型的标准D-H参数如下：

基于标准D-H参数分别对各关节的刚体模型建模，并添加到刚体树实例对象treerbt中。例如关节1的刚体模型构造如下：

(www.xing528.com)

构造整个刚体树模型，结果如图7.40所示。

在Simulink中构建Staubli TX200机器人的正运动学计算模型，如图7.41所示。

Get Transform模块（正运动学计算模块）用于计算多关节机器人的正运动学，得到末端执行器相对于基座坐标系的齐次变换矩阵。设置模块的模型输入参数为前面定义的treerbt对象，Source参数为末端执行器tool，Target参数为基座base，模块T用于将计算结果输出到工作空间中。

图7.40　Staubli TX200机器人的三维刚体树模型

图7.41　用于正运动学计算的Simulink模型

输入input_qr＝［0，-1.570 8，1.570 8，0，0，0］，可得：

输入input_qn＝［0-pi／2-3∗pi／4 pi／4 pi／4 0］，可得：

单位为m。对比第6章6.4.1小节计算的结果，发现基本一致，这也证明了Get Transform模块正运动学计算模块的准确性。