软件系统依赖于ROS机器人操作系统，通过介绍move_base规划的机器人空间速度和move_group规划的机械臂空间姿态，改数据通过串口实现与硬件系统之间的控制功能。

4.1 准备工作

4.1.1 ROS机器人操作系统安装与配置

本文使用的是Ubuntu 20.04版本的系统，对应的ROS版本为Noetic。首先执行相关更新，保证软件源、软件为最新，无依赖项冲突，具体操作如下：

（1）更新软件源列表，操作命令为sudo apt update；

（2）更新软件源仓库，操作命令为sudo apt upgrade； 

（3）更新系统插件，操作命令为sudo apt full-upgrade ； 

（4）移除无效软件，操作命令为sudo apt autoremove；

（5）移除无效配置，操作命令为sudo apt autoclean。

由于国内网络环境原因，在此使用Tsinghua数据服务器进行安装配置，具体操作命令如下：

sudo sh -c '. /etc/lsb-release &&  \

echo "deb http://mirrors.tuna.tsinghua.edu.cn/ros/ubuntu/  \

`lsb_release -cs` main" > /etc/apt/sources.list.d/ros-latest.list'

同时根据Tsinghua开源表单找到相应的密钥并写入到当前的系统当中，具体操作命令如下：

sudo apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80'  \

--recv-key C1CF6E31E6BADE8868B172B4F42ED6FBAB17C654

在环境当中，需要使用到Navigation导航功能包、Gmapping建图功能包、Hector建图功能包、RtabMap建图功能包、RVIZ三维数据可视化平台、Gazebo物理仿真平台、IMU-Tools工具箱、OctoMap八叉树地图功能包，以及Google Cartographer，可以通过apt install+“对应的名称”即可安装。

Cartographer需要依赖Ceres-solver优化求解、Protobuf协议、Aseil-cpp库文件等内容，需要进行cmake编译。

4.1.2 Gazebo物理仿真平台和RVIZ三维数据可视化平台

仿真/模拟(Simulation)泛指出于试验或培训的目的，将原来的控制系统、事物或过程，构建一种模式以表现其核心特征(Key Characteristics)的动作/能力，并进行系列化和式子化，从而对核心特征进行模拟。

Gazebo是一种功用强劲的三维物理建模与仿真平台，具有强力的物理引擎、高效的图像制作、简易的程序与绘图用户界面等，更关键的是其开放与免费的特点。Gazebo中的机器人建构工艺与RVIZ所用的建构工艺相似，但也是要求在建模中添加与机器人和环境相关的基本物理属性，包括品质、磨擦性、弹性系数等。另外，机器人的传感器数据资讯还能够透过插件的形态直接进入模型仿真情境中，以可视化的方法进行展示。

机器人控制系统中存在大量数据分析，但这种数据分析在计算过程中一般会保持原始数字的形态，如对图像统计分析的0~255的RGB数据。但是，由于这种数据分析形式的真数据分析往往不利于开发者去理解数据分析所表达的内容，所以常常需要把数据分析可视化的形式展示，例如机器人建模的数据分析可视化、图像数据挖掘的可视化、地图数据挖掘的可视化，等等。ROS通过基于机器人系统的数据分析可视化技术，为开发者创造出了一个呈现不同数据挖掘形式的三维可视化平台RVIZ。

RVIZ是一个三维可视化开发工具，很好的整合了多个通过ROS软件所构建的人工智能平台。在RVIZ中，通过利用XML对机器人、周围物体等的所有实体物件，进行尺寸、质量、部位、材料、关节等属性的描述，并在网页中显示出来。而且，RVIZ还能够利用图像化的方法，随时提供有关机器人传感器的数据、机器人的运行状况、环境的情况等，界面如图4.1所示。

 

图4.1 RVIZ可视化界面

4.2 模型理论

这里通过对差分轮运动学模型、六轴机器臂运动学模型等加以运算，并通过运动学的正逆解来实现算法设计。

4.2.1 差分轮运动学模型

移动机器人在两个相邻时刻的位姿，其中是两相邻时刻移动机器人绕圆弧运动的角度，是两相邻时刻移动机器航向角（或称朝向角，head）的变化量。l是左右轮之间的间距，d是右轮比左轮多走的距离。r是移动机器人圆弧运动的半径，如图4.2所示。

 

图4.2 差分轮运动学模型推导

移动机器人的前进转速为V等于左右车轮转速的平均：

 

将两个时刻的机器人位置重叠在一起，就能够明显的发现移动机器人航向角变量为θ3。由图4.2中的几何关系可得出式（4.2）：

让机器人做圆周运动，从起点出发绕圆心一圈回到起点处，在这过程中机器人累计的航向角为360度，同时它也确实绕轨迹圆心运动了360度，说明机器人航向角变化多少度，就绕圆心旋转了多少度。而这三个角度较容易计算出来，由于相邻时刻时间很短，角度变化量很小。由图4.2中的几何关系可得出近似公式，如式（4.3）所示：

由以上可以得出机器人围绕着圆心位置运动的角旋转速度，它同时又是指机器人航向夹角变换的旋转速度，由图4.2中的几何关系可得出式（4.4）：

4.2.2 六轴机械臂运动学模型

对于机械臂的控制，可以直接设置机械臂各个关节角度值实现控制。但是在大多数情形下，必须设定好机械手臂末端的位姿来进行操控。利用机械手臂各个关节的的角度值可算得出机械手臂末端的姿势，此为机械手臂运动学正解。相反，若设定由机械手臂的末端姿势求出机械手臂各个关节角度值，此为机械手臂运动学逆解。在视觉机械臂系统当中需要将摄像头识别到的目标位姿传递个机械臂末端执行，从而实现视觉抓取的功能。

本文使用DH表示法来分析六轴机械臂运动学模型。DH参数控制系统是一种用四个参数表示二个主要关节或连杆机构间位移方向关联的机器臂模型和坐标系选择系统。因为控制原点定位与X轴线的相对方向，而人为降低了二个自由度，所以你只需要四个参数就可以定义为一种有六自由度的极坐标系。

本文选取的DH四个参数的具体物理意义如下所示：

（1）link length（连杆总直径）：二个主要关节的轴（回转关节的回转轴，平移关节的平移轴）间的公共法线方向间距。

（2）link twist（连杆扭转）：指一个关节的主轴相对于另一关节的主轴，绕它的公共法线所转动的夹角。

（3）link offset（连杆偏移）：上一个关节与下一个关节的公用法线方向，以及其与上一关节的公用法线方向与这个滑膜关节轴线的长度。

（4）joint angle（关节转动）：上一个关节与下一个关节的共同法线向，以及其与上一关节的共同法线向绕这个关节轴的转动。

要用DH计算法为机器人进行模型，首先事件就是给各个关节确定一个本地的基础座标系，所以对各个关节来说只需要确定一下Z轴和X轴。

给定Z轴，如果关节是公转地，Z轴就是按右手规则自转的方位。绕Z轴的公转角为关节的自变量；确定关节为滑关节，则Z轴是沿垂直运行的方向。沿Z轴的连杆机构长度，d为关节变量。

对于X轴，当二个滑膜关节并不相互平行或交叉时，则Z轴都是等高，且都有一个相距较短的公垂线，它们都正交于任何二个等高。在公垂线方位上，确定了本地基准座标系的X轴。若an线表示为Zn一区间的公垂线，则以Xn的水平方位为沿an线，如下图表示。

 

图4.3 六轴机械臂模型简化

机械臂末端姿态矩阵T可由式（4.5）计算得到：

机臂运动学逆解是指使用机臂末端姿态计算机臂的关节值，这里先对机械臂模型进行简化，如图4.4所示。

 

图4.4 机械臂逆运动学描述

通过把机器臂的模型简单化，去掉底座云台，和执行器部分获得了机器臂的主体。由图4.4所示，机械臂的端点p的方向位置，并最终由三部分所构成。

其中，图4.4的均为所需要解的操舵装置的角度，是手指爪和水平面的相对角度。从图4.4可知，爪子的俯视角

，由此可得到机械臂末端位置（x，y）和其关节角度的关系如下所示：

其中x，y由程序所给定，l1、l2、l3而均为机械臂的机械结构固有特性。通过三元一次方程带入求解获得角度值即可。