移动机械臂整体控制研究

陈庄浩， 孙亦劼， 蒋文萍， 江永林

(上海应用技术大学 电气与电子工程学院， 上海 201418)

近年来，机械臂在国内外已有了较大的发展，应用的领域也很广泛，但机械臂的控制系统、轨迹规划还处在一个快速发展时期。随着人工智能、深度学习、神经网络、大数据分析等科学技术的快速兴起，机械臂相关技术迅猛发展。移动机械臂是一个集环境感知、动态决策与规划、行为控制与执行等多功能于一体的综合系统[1]。它集中了传感器技术、信息处理、电子工程、计算机工程、自动化控制工程以及人工智能等多学科的研究成果，是机电一体化、计算机科学完美结合的典型代表[2]，是目前科学技术发展最活跃的领域之一。随着移动机械臂性能不断地完善，其应用范围大幅度的扩展，不仅在工业、农业、医疗、服务等行业中得到广泛应用，而且在城市安全、国防和空间探测领域等高危场合得到很好的推广应用。随着科学技术的飞速发展，人类的研究和活动领域已经由陆地扩展到海洋和太空。将机械臂搭载到移动平台上进行空间探测和开发，是21世纪世界各国探索开发空间资源的主要手段之一。

本文的移动机械臂将一个六自由度的机械臂安装于TurtlrBot2机器人之上。通过TurtleBot2机器人的轨迹规划，配合六自由度机械臂的手动抓取和自动抓取来实现一系列的功能。

1 移动平台的硬件系统及其模型

1.1 TurtleBot2移动机器人

TurtleBot2移动机器人是一款带有开源软件的低成本个人移动机器人。TurtleBot2机器人本体由YUJIN Kobuki底座，2 200 mA·h的电池和65 W快充和硬件安装套件组成[3]，如图1所示。TurtleBot2移动机器人通过与Kinect传感器与笔记本电脑的组装，可以运行标准的同步定位和映射(simultaneous localization and mapping，SLAM)算法来构建1个地图，并且可以从1个笔记本电脑或Android的智能手机进行远程控制并进行轨迹规划[4]。

图1 TurtleBot2 硬件组成Fig.1 Hardware composition of TurtleBot2

1.2 移动机器人模型

TurtleBot2是一种差动式2轮移动机器人，在它的底座上有2个结构相同的驱动轮，驱动轮转动为机器人提供驱使其进行运动的作用力，在驱动轮前方有1个随动轮，随动轮起到支撑的作用，避免机器人翻倒，并不起驱动作用[5]。移动机器人模型如图2所示。

图2 移动机器人模型Fig.2 Mobile robot model

移动机器人的状态由2个驱动轮中点M所在的坐标系的位置和导航角θ来决定。令p=(xyθ)T，q=(vω)T，其中(xy)分别为移动机器人的线速度和角速度。

移动机器人的运动学方程为

(1)

移动机器人的位姿从p=(xyθ)T移动到位姿qr=(xryrθr)T，移动机器人在新的坐标系Xe-Ye中的坐标为

pe=(xeyeθe)T

(2)

式中，θe=θr-θ。

设新坐标系Xe-Ye与X-Y之间的夹角为θ。根据坐标转换公式，可得描述移动机器人位姿的误差方程为：

(3)

位姿的误差微分方程为：

(4)

1.3 Kinect传感器

一台Kinect上装载了3个摄像头，分别为中间的彩色摄影头，最左边的是红外线发射器和最右边的红外线CMOS摄像头[7]，左右两边的镜头组成了3D深度传感器，提供全身3D运动捕捉、人脸识别[6]。Kinect传感器如图3所示。

图3 Kinect传感器Fig.3 Kinect sensor

2 机械臂的硬件设计及其运动参数

2.1 机械臂的硬件设计

六自由度机械手臂控制系统的硬件主要由舵机控制电路、TBS2701舵机、无线蓝牙模块、STM32单片机系统、超声波测距模块和设计电路组成。通过超声波来确定物体与机械臂之间的距离，当满足一定条件的情况下，微控制器就发出信号使六路数字舵机[8]工作抓取目标物。再通过无线蓝牙模块与电脑进行通信，这样就可以通过人机交互界面来观察并调整机械臂的运动状态，以完成对目标物的抓取。硬件控制系统框图如图4所示。

图4 机械臂硬件系统方框图Fig.4 Block diagram of the robotic arm hardware system

2.2 机械臂的运动参数

通过对机械臂的6个旋转关节分别依据右手法以及D-H 原则来建立各个关节的坐标系。将各个连杆的关系通过D-H参数法需要用到4个最基本的参数来进行表达，即αi、ai、di和θi[9]。本文研究的六自由度机械臂连杆参数见表1。

表1 机械臂连杆参数Tab.1 Link parameters of manipulator

3 移动机械臂的整体控制

3.1 移动机械臂的避障算法

3.1.1 Dijkstra算法

Dijkstra算法的基本思想：设置一个集合S存放已经找到最短路径的顶点，S的初始状态只包含源点v，对vi∈V-S，假设从源点v到vi的有向边为最短路径。以后每求得一条最短路径v,…,vk就将vk加入集合S中，并将路径v,…,vk,vi与原来的假设相比较，取路径长度较小者为最短路径。重复上述过程，直到集合V中全部顶点加入到集合S中。

3.1.2 DWA算法

在速度空间(v,ω)中采样多组速度，并模拟这些速度在一定时间内的运动轨迹，再通过一个评价函数对这些轨迹打分，最优的速度被选择出来发送给移动底座。

首先，建立移动机器人运动学模型

(5)

然后进行速度采样。在速度(v,ω)的二维空间中，存在无穷多组速度。但根据机器人本身的限制和环境的限制可以将采样速度控制在一定范围内。

移动机器人受自身最大、最小速度的限制：

vm={v∈[vmin,vmax],ω∈[ωmin,ωmax]}

(6)

移动机器人受电机性能影响：

vd={(v,ω)|v∈[vc-vbΔt,vc+vaΔt]∧

ω∈[ωc-ωbΔt,ωc+ωaΔt]}

(7)

最后是评价函数：

G(v,ω)=σ(α·heading(v,ω)+β·dist(v,ω)+

γ·velocity(v,ω))

(8)

3.2 移动机械臂的地图构建

移动机器人通过ROS具有在未知环境中工作和运动的功能，来检测扫描四周环境，从而创建出不同环境下的地图[10]。图5(a)为上海应用技术大学第一学科楼的某层楼道和移动机器人的实照。通过对楼道的扫描得到图5(b)所示的扫描图像。其中凹陷处为楼道两侧办公室的门，楼道中间的小点为由电脑主机设置的障碍物用于避障操作。

图5 (a)楼道与移动机械臂的实景图；(b)由Kinect传感器扫描得到的图像Fig.5 (a) Real map of the corridor and mobile manipulator, (b) Image scanned by Kinect sensor

3.3 移动机械臂的避障

在使用Python进行避障程序编写前，要使用到Pygame库。移动机械臂避障流程图，如图6所示。

图6 移动机械臂避障流程图Fig.6 Flow chart of mobile manipulator obstacle avoidance

Pygame作为一个游戏模块具备了绘制图形、显示图片、动画效果、外设交互、播放声音和碰撞检测等功能。仿真中使用小球代替小车。通过get_rect()得到小球的位置，返回一个surface对象，进入循环，进行碰撞检测[11]。当小球与障碍物之间的距离小于障碍物半径时，会发生碰撞。所以此时需要提前修改轨迹。将障碍物视作一个为10 cm的圆，当小球与障碍物之间距离小于80 cm时，速度为[3,2.5cos(t) cm/s]。程序通过改变surface对象的位置实现小球的运动。在小球避开障碍物后小球将回到原来的运动轨迹上。Python上的仿真示例，如图7所示。

图7 小球在Python上的避障仿真Fig.7 Obstacle avoidance simulation of small ball on Python

在完成前提工作，创建地图后，把机器人放置在地图的某一点上。在rviz中选择2D Pose Estimate和选择机器人的初始位姿，调用避障程序。机器人会向前运动3 m，并且避开路线上的障碍物。具体实际避障情况如图8所示。

图8 Turtlebot2移动机器人的避障(a)移动机械臂位于起始位置；(b)移动机械臂避开障碍物；(c)移动机械臂运动到障碍物3 m后处Fig.8 Obstacle avoidance for TurtleBot2 mobile robot

3.4 移动机械臂的轨迹规划和自动抓取

3.4.1 移动机械臂的轨迹规划

轨迹规划实际上是对机械臂在空间内的运动进行一次规划[12]。本文使用3次多项式插值方法。 3次多项式插值法中每一小段路径通过之前的3次多项式求解可以得出下一段路径中的初始位置并用一组关节角进行表示[13]。需要确定的是插入的点的期望位置，速度以及所用的时间。先选取若干中间点处的期望位置、速度和时间来进行3次多项式的验证，如表2所示。

表2 3次多项式插值法参数Tab.2 Cubic polynomial interpolation parameters

以下由MATLAB来解决3次多项式的解，如图9所示。

图9 MATLAB的三次多项式解的仿真Fig.9 The simulation of cubic polynomial of MATLAB

图10 机械臂自动抓取主程序流程图Fig.10 Main program flow chart of manipulator automatic capture

3.4.2 移动机械臂的自动抓取

六自由度机械臂系统功能比较复杂，在对机械臂进行分析时，将复杂的系统分为了对多个模块进行封装的方式来进行编程[14]。这里仅对主程序进行讲解，程序流程图如图10所示。程序首先完成初始化过程，然后一直循环主程序的内容，看是否有来自串口数据的指令，如果需要重新编辑动作，则运动调试程序，获取动作组指令，存储数据，随后检测超声波是否有检测到目标位置，如果存在需要抓取的物体，分析处理动作组指令，并控制舵机执行指令。

在遥控器的控制下，实现了手动控制和自动控制2个模式下的抓取物块实现。手动模式下，可以实现对机械臂各个关节的控制，完成抓取并通过上位机观察各个关节的转角。自动模式下，通过超声波的距离检测判断是否进行抓取，执行调试时写入的动作组，完成了自动模式下的自动抓取。图11所示为机械臂通过超声波测距模块自动抓取小物块过程。

图11 机械臂自动抓取过程Fig.11 Process of manipulator automatic capture

4 结 语

本文将六自由度机械臂安装在差动式两轮移动机器人(Turtlebot2)之上，通过对移动机器人的运动学模型建立并进行了仿真，以及对Dijkstra算法和DWA算法的分析，设计了一种由Kinect传感器、TurtleBot2移动机器人和笔记本电脑组成的移动工作站的运动控制，包括基本的路径规划、地图构建和避障运动的实现。搭载上六自由度机械臂通过超声波测距模块实现的对物体的位置判断，能够自动并准确地定位物体位置并进行抓取、放置到指定区域的操作。由于本文实现的是移动机器人与机械臂的协同运动控制，因此即便是移动机械臂与物块间存在一定的距离，也可以通过以上功能实现对物体远距离的定位并移动到物块附近，抓取并放置到人工指定的区域。