摘要
双足仿人机器人由于其最接近人类的外形和强大的环境适应能力，成为最有希望在人类发展环境中服务于人的机器人。仿人机器人的技术虽然己经有了长足的发展，但是离我们所期望达到的效果和实现实际应用还有很大的距离。本研究针对仿人型机器人的运动规划作为切入点，以机器人操作系统 ROS（Robot Operating System）作为算法设计框架，以仿人型机器人NAO为载体，完成机器人在非重构环境下从起始点到目标点的路径规划，以及对NAO机器人手臂的运动规划，实现简单的运动。具体研究如下：
（1）对服务机器人领域研究的背景、意义和发展现状做了详细的分析。对仿人型机器人、机器人运动规划方法的国内外研究现状进行研究。
（2）借助 ROS 中的三维可视化工具 Rviz 以及运动规划工具 MoveIt!搭建 NAO机器人运动平台。ROS 系统的基本组成、ROS 的运行机制及原理以及 NAO机器人关节参数。在 ROS 系统下，使用 NAO 机器人模型文件，借助可视化工具Rviz 实现 NAO 机器人模型的三维可视化显示。编译运动组节点用于机器人的运动控制，完成 ROS 系统下 NAO 机器人运动平台的搭建。
（3）研究路径规划技术。针对路径规划的问题，研究了包括人工势场法、图搜索法以及随机采样方法等路径规划算法。在路径完备性和时间复杂度之间做出了权衡，最终采用随机采用方法中的快速随机拓展树算法（RRT），以损失路径完备性为代价，提高算法工作效率，实现快速规划从起始点到达目标点的路径。
（4）研究机器人运动规划技术。针对人型机器人 NAO 的手臂，在笛卡尔空间以及关节空间进行空间的轨迹规划。利用 ROS 中的 MoveIt!工具，对机器人的手臂关节进行运动规划。
（5）基于上述研究，在二维栅格地图中，实现 NAO 机器人在地图中从起始点移动到目标点，并在目标点完成手臂从初始位置移动到指定位置，完成手臂的空间运动规划问题。
关键词：仿人机器人；NAO；运动规划；路径规划
第1章 绪论
1.1课题研究背景及意义
随着“中国制造2025”发展理念的提出，智能工厂、工业互联等技术的兴起带动着工业发展越来越自动化和智能化[1]。在此背景下，机器人技术在许多重要的制造、家庭、服务等领域得到广泛应用[2-5]。机器人技术在精度和智能化程度方面有着较大的发展，其在一般工业场景中的应用也逐渐增加。在机器人面向于应用方向开发以来，经过几十年的发展，我们又可以将机器人的发展分为三个阶段[6-8]：第一阶段为简单个体机器人应用阶段，这一阶段主要是在工业革命的背景下，催生了一批机器人应用领域的领头企业，世界上第一台电动工业机器人就由ABB集团发明研制。随着工业的进步，单台机器人无法完成复杂的加工程序，工业机器人迎来了第二个发展阶段--协作机器人，最常见的是协作机械臂，它们构成复杂的生产线，推进社会进步。这一阶段爆发时间为上世纪八十年代和九十年代，德国机械人生产厂家库卡(KUKA ) ,日本厂家发那科(FANU C)就是在这时高速发展。随着生活水平的提高，机器人发展的第三阶段--服务机器人领域在上世纪末走入人们的视线并大力发展。在形式各样的机器人当中，仿人形机器人受到越来越多研究者的重视。
双足仿人机器人由于其最接近人类的外形和强大的环境适应能力，成为最有希望在人类发展环境中服务于人的机器人[9]。从仿人机器人的技术虽然己经有了长足的发展，但是离我们所期望达到的效果和实现实际应用还有很大的距离。究其原因，与其独特的结构构造和双足运动特性具有密切联系。仿人型机器人是依照人的形态和行为而设计制造的一种机器人，相对于其他机器人它具有人的很多优点，主要体现在以下几个方面:
（1）具有广阔的工作空间。与固定的机器人不同，仿人机器人可以在多个位置提供服务，因此具有更大的操作空间[10];
（2）具备避障能力。得益于仿人型机器人的自由度的高冗余性，仿人型机器人作为移动机器人可以自由移动，同时机械手臂也可以灵活规划轨迹，使得其能够灵活避障[11];
（3）机构仿人。仿人型机器人依据人的形态设计，外形活泼可爱，适合给人类提供服务，同时机械手臂具备人类手的功能，可以代替人类提供服务[12];
研究仿人形机器人的主要意义在于:
（1）仿人机器人在众多类型的机器人当中是与人在形体上最为相似的机器人。人类的许多行为，比如跑步、跳跃等都具有复杂的动力学和运动学特征，通过研究仿人机器人，能够把这些复杂的特征运用到机器人上，使机器人能够更好地为人类服务。仿人机器人研究课题本身也是一个融合了多学科的课题，通过对仿人机器人的研究，能够促进多学科的共同发展。
（2）仿人形机器人与其他类型的机器人相比，双足设计使其在行走等情况下受地面环境影响较小，能够在不平整的地面行走，还能够翻越障碍，大大提升了机器人的活动空间和工作范围，便于机器人在各种环境下工作。
（3）在当今社会逐渐进入人口老龄化的情况下，发展机器人使其能够代替人类进行工作具有重要的意义。仿人机器人的类人性恰好满足了这种情况下机器人照顾老年人、残疾人的要求。另外，仿人机器人还能到达深海、外太空等人类无法生存的环境进行工作，也能代替人类进行核泄露事故处理等危险性高的工作。
1.2 国内外研究现状
1.2.1 仿人型机器人研究现状
双足人形机器人发展具有里程短、进展快的特点，从上世纪60年代兴起
以来就受到学术界诸多学者和科研机构的关注，但是由于其造价昂贵，对地形适应能力不够，尚未有一款成型的双足人形机器人成熟的应用在市场领域。国外的双足人形机器人得益于其工业基础雄厚的优势，具有发展快，体系成熟的特点，因此其形态和应用潜在价值均远超国内，其中以双足人形机器人的第一梯队:美国和日本表现最为突出。美国是最早开展双足人形机器人研究的国家[13]。上世纪九十年代初，MIT LegLab实验室发布了Planar Biped双足机器人，可以实现跑跳、上下台阶等复杂动作[14,15]。上世纪九十年代，MIT LegLab实验室研究出可以实现跑跳的双足人形机器人3D Biped，同样在当时属于世界上最灵活的双足人形机器人[16,17]。此后，波士顿动力公司发布了一款采用液压驱动的四足机器人--BigDog X12，如图1.l (a)，由于其出色的平衡能力和运动性能，该产品一经发布后受到国内外研究学者广泛关注。随后波士顿动力发布了PETMAN与Atlas这2款机器人[18]，如图1.l (a)和图1.l (c)，它可以在受到外部环境干扰下使其保持平衡。美国在机器人研究中具有体系化的特点，其迭代次数多，研究早，发展成体系化。仅仅Atlas机器人从研究之初至今就迭代了不少于6个小版本，而ATRIA S同样经过了多轮的产品迭代。
(a) BigDog (b) Atlas
(c) PETMAN
图1.1 BigDog，Atlas和PETMAN机器人
日本东京大学利用HRP-2[19]开展了理论和实验研究。在他们的研究中，该机器人能自主试探物体重量，并根据物体重量改变策略移动物体。例如机器人会先用手推重物，如图1.2 (b)所示;当推不动时会改变策略，以全身的力量去推动，如图1.2 (a)所示。
(a) (b)
图1.2 HRP-2 推动重物
国内企业近年来也相继研发过人形机器人，包括能力风暴研发的面向于教育的双足人形机器人珠穆朗玛系列，如图1.3(a) 所示，优必选研发的面向于教育和娱乐的Alpha系列机器人，如图1.3 (b)所示。2017年优必选发布了大型双足腿式机器人--Walker，可以双足行走，踢球和上下楼梯，其运动模式酷似日本本田的Asimo机器人，如图1.3 (c)。
(a) 珠穆朗玛机器人 (b) Alpha机器人 (c) Walker机器人
图1.3 能力风暴珠穆朗玛和优必选Alpha，Walker机器人
国内双足人形机器人运动性能差，环境适应能力差，小型机器人甚至没有一款机器人可以实现流畅的双足步态行走，其运动方式多使用静步态，而目前国内所有产品均不具备在外界推力下调整自身姿态保持平衡的能力。
1.2.2 机器人运动规划方法研究现状
运动规划作为机器人研究的一个重点以及难点，通常运动规划有点到点(Point to Point, PTP)路径规划和连续路径(Continuous Path, CP)轨迹规划两类问题。
在二维平面上，可以把NAO机器人简化为移动机器人，以一个点来代替，需要在初始点和目标点之间规划出一条路径，此时就是一个典型的路径规划问题。
在三维空间中，NAO机器人手臂的运动规划，可以在两类空间中对机器人进行运动规划，一类是机器人工作空间(笛卡尔空间)运动规划，另一类是机器人关节空间运动规划。通常，上述两个空间轨迹规划可以通过正逆雅可比矩阵相互转换，但这种转换可能会带来非线性误差。其中，笛卡尔空间轨迹规划法在笛卡尔空间直接描述NAO机器人手臂的轨迹，更直观，易想象，它需要沿着轨迹通过逆运动学求解每个位姿对应的关节变量，导致计算量增加，同时存在多解与奇异性。而关节空间轨迹规划法则首先需要从目标轨迹中抽取特征点序列，然后通过运动学反演获取关节变量，最终用关节变量及其一阶二阶导数插补来间接描述
NAO机器人手臂的运动轨迹，这种方法通常可以避免奇异性，更易于求解能耗较少的运动，避免了运动学求逆问题[20, 21]。
移动机器人局部路径规划，就是将机器人置于未知环境或者己知局部信息的环境下，只通过机器人自身携带的传感器对周围环境进行探测识别和路径搜寻。同时，传感器将采集的障碍物环境信息返回给中央控制器上位机，上位机通过局部经规划算法算出机器人的运动方向。在这一过程，如果说传感器相当于“人体的四肢”，那么局部路径规划算法就相当于“大脑”，其在移动机器人局部路径规划当中发挥着至关重要的作用。
目前，用于局部路径规划的算法主要有模拟退火法、人工势场法、图搜索法和随机采样法。
(1) 模拟退火法
模拟退火算法是一种启发式搜索算法，与贪婪策略算法原理类似，比较适合用来解决关于资源分配、组合优化等复杂问题。该算法来源于金属热处理的退火原理，它通过设置合适的初始热处理参数，对金属高温热处理的退火过程进行模拟，控制金属表面温度持续性的降低以及温度的概率性突变，并在此过程中利用随机搜索的方法在解空间中搜寻全局最优解，即当面对局部最优解时，能够以一定的随机概率避开而趋向全局最优解。假设a点为模拟退火算法初始解，如图1.4所示，当算法搜索到b时陷入局部最优解，这时算法并未终结，而是以一定的随机概率继续往右搜索到达c点。而后经过多次的非局部最优的搜索最终能够到达全局最优点d，这一过程即实现了算法对局部最优的逃离。
图1.4 模拟退火算法
(2) 人工势场法
人工势场法是由Khatib于1986年提出的一种基于虚拟力的算法，原理如图1.5所示。
图1.5 人工势场算法
人工势场法基本原理就是假设在机器人的工作空间内，障碍物产生斥力场，而目标点产生引力场[22]。其中，引力场与斥力场都是由机器人与障碍物以及目标点之间的相对位置等因素产生的。但当引力场产生的引力与斥力场产生的斥力的合力为O时，机器人将停止在原地不动，到达局部最小点[23]。一般采用两种方法来规避或解决局部最小点，一种是通过有效的搜索算法检测到局部最小点，并规避局部最小点，这种方法的缺点是会增加计算量;另一种是设计不会产生局部最小点的势能函数。
(3) 图搜索法
图搜索算法包括:Dijkstra算法、A*算法、D*算法等。通常适合可离散化的环境空间中的路径规划。
Dijkstra算法[24]是一种典型的最短路径搜索算法，由荷兰计算机科学家Edsger Wybe Dij kstra于1959年提出的，用于计算一个节点到其他所有节点的最短路径。该算法的主要特点是从起始点开始，一层一层向外层拓展，直到找到目标点为止。该算法是一种最优路径搜索方法，解决的是有向图中最短路径问题，但其主要的缺点是耗时较长，复杂度较局。
D*是动态A*算法(D-Star, Dynamic A Star，属于一种动态规划算法，在未知环境中比Dijkstra算法以及A*算法适应性更强。该算法由卡耐基梅隆大学机器人中心的Stentz在1994和1995年两篇文章提出[25,26]，主要用于机器人探路，并且成功应用于火星探测器上。机器人自带传感器检测实际环境信息，D*算法比较实际环境信息与原有环境信息的差距，实时更新环境信息，并重新规划一条路径。
A*算法[27, 28]是一种直接搜索方法，属于一种启发式路径搜索算法，其基本上等同Dijkstra算法，只是在拓展节点时加入了评价函数。A*在静态路网中非常有效(very efficient for static worlds，但不适于在动态路网。
(4) 随机采样规划方法
在具有完备性的算法中，它们总能找一条最优路径，实现机器人从起始点到目标点运动。然而机器人运动规划问题会随着机器人自由度增多而出现“维数灾难”问题。因而在多自由度机器人在复杂环境中实时规划的问题中，不适合采用确定性算法，而是采用基于规划空间的随机采用方法，损失完备性为代价来提高工作效率，例如多自由度机械臂的空间轨迹规划通常采用随机采样的规划方法。这一类算法包括阿里阿德涅线团算法(The Ariadne's Clew Algorithm, ACA)[29]、随机路标算法(Probabilistic Roadmap Method, PRM )[30]和快速随机拓展树算法(Rapidly-exploring Random Tree, RRT ) [31]等。在这里主要介绍PRM和RRT这两类规划算法。
随机路标算法(PRM)是一种快速有效的路径规划算法。PRM是一种基于图搜索的方法，它将连续空间转换成离散空间，再利用A*等搜索算法在路线图上寻找路径，以提高搜索效率。快速随机扩展树算法(RRT)是一种增量式前向搜索算法，规划是以状态空间中的一个初始点作为根节点，随机增加叶节点的方式，生成一棵随机扩展树。当随机树的叶节点中包含了目标点或目标区域中的点，便可以在随机树中找到一条以叶节点组成的从初始点到目标点的可达路径。该算法拥有极强的增长式生长特性，对动态环境有着很强的适应性，因此被广泛应用于动态环境以及存在运动动力学约束的系统环境中[32-34]。
本文所做的工作分为两部分，在二维平面上，从初始点与目标点之间规划出机器人移动的路径，在三维空间中，在初始状态与目标状态之间规划出手臂移动的轨迹。在运动规划过程中，需要快速地得到可行的路径或轨迹，而不要求该路径或轨迹最优，综上所述的运动规划方法，本文使用快速随机拓展树算法，并进行优化，最终完成两部分的工作。
1.3 本文主要研究内容
本文的所研究的内容为仿人型机器人运动规划，是以仿人型机器人NAO为研究对象，以ROS为总体软件框架。结合NAO机器人的实际情况，以ROS为基础，实现NAO机器人的运动规划，主要做了以下几项研究：
（1）对服务机器人领域研究的背景、意义和发展现状做了详细的分析。对仿人型机器人、机器人操作系统 ROS、运动规划与控制的背景和国内外研究现状进行研究。
（2）借助 ROS 中的三维可视化工具 Rviz 以及运动规划工具 MoveIt!搭建 NAO机器人运动平台。ROS 系统的基本组成、ROS 的运行机制及原理以及 NAO机器人关节参数。在 ROS 系统下，使用 NAO 机器人模型文件，借助可视化工具Rviz 实现 NAO 机器人模型的三维可视化显示。编译运动组节点用于机器人的运动控制，完成 ROS 系统下 NAO 机器人运动平台的搭建。
（3）研究路径规划技术。针对路径规划的问题，研究了包括人工势场法、图搜索法以及随机采样方法等路径规划算法。在路径完备性和时间复杂度之间做出了权衡，最终采用随机采用方法中的快速随机拓展树算法（RRT），以损失路径完备性为代价，提高算法工作效率，实现快速规划从起始点到达目标点的路径。
（4）研究机器人运动规划技术。针对人型机器人 NAO 的手臂，在笛卡尔空间以及关节空间进行空间的轨迹规划。利用 ROS 中的 MoveIt!工具，对机器人的手臂关节进行运动规划。
（5）基于上述研究，在二维栅格地图中，实现 NAO 机器人在地图中从起始点移动到目标点，并在目标点完成手臂从初始位置移动到指定位置，完成手臂的空间运动规划问题。
第2章 基于ROS的机器人运动平台搭建
2.1 ROS软件架构
随着机器人产业的快速发展，迫切需要一个灵活性高、可扩展性好的软件框架来编写机器人软件程序，ROS机器人系统因其自身特点能够很好的满足开发人员的需求。作为机器人程序开发的开源元操作系统，它封装了多种通信协议、SDK和开源工具，提供了包括底层设备控制、进程间通讯、常用函数实现、图形化工具、包管理以及编译和运行工具等服务，极大降低了机器人开发和部署的要求。
ROS目前囊括了很多机器人开源项目，例如OROCOS，该开源软件主要关注底层控制器的设计，包含机械臂逆运动学数值解析的KDL库等;还有基于运动规划的OMPL库、OpenRave库;以及开源点云处理的PCL库;以及关注于移动机器人导航算法的全局规划器模块Navigation库，关注于机器人运动规划的模块Moveit等。同时它还提供了一系列开发工具，例如rqt_plot绘画主题的数值曲线，rqt_graph绘制节点连接状态，TF (Transform)展现各坐标系的位姿，以及RViz这样强大的可视化工具等。
ROS的系统架构包含三个层级，分别是文件层级，计算图级，社区层级。其中计算图级是ROS一种点对点的信息数据处理的网络样式。当后台程序运行时，各执行的进程都是通过点对点呈现出来管理器(node master)、主题(topic)通过发布或订阅的节点(node)、节点服务(service)等来呈现。ROS的基本的计算图级概念包括:节点、节点管理器、参数服务器、消息、服务和主题[35]。这些概念以各种形式来提供数据，如图2.1所示。
图2.1 ROS计算图级
(1)节点(Nodes)：节点是一个可执行文件用来，用来执行和其他节点之间计算的进程，每个功能包(package)都可以产生可执行文件，一旦节点启动就连接到了ROS的通讯网络中。例如在有摄像头的机械臂完成运动仿真时，需要控制器节点来控制旋转关节转动，需要传感器节点读取数据，需要路径规划节点实现路径规划，还有ros提供的状态节点可以观察机器人的运动位姿，Rviz显示节点展现可视化机器人图形等。开发者可以根据不同的需求选择不同的节点来创建。各个节点独立订阅和发布消息，构成了完整的仿真系统。
(2)节点管理器(Master)：节点管理器就是用来管理每个节点的，节点都需要在节点管理器下来进行节点名字的注册，节点管理器方便各个节点相互查找到对方，如果没有节点管理器，所有消息将不能通过节点发送和接收。只要ROS通讯网络不断，还有节点接收或发布，那么节点管理器将一直运行。
(3)参数服务器(Parameter server)：通过rosparam命令来存储和操作节点运行过程中的配置参数，因为在机器人系统中参数传递是很重要的功能，无论是传感器的设置，还是控制参数的调整，需要留有便捷的参数调试接口，可以通过参数服务器来实现。
(4)消息(Messages)：是ROS的一种数据结构，类型包括整型、浮点型、布尔型等，用来订阅或发布到一个话题，是节点之间的通信内容。
(5)主题(Topic)：节点可以发布消息到主题，也可以订阅主题以接收消息「25]。每个节点只有在订阅或者发布的消息主题一致的情况下才可以通信，每一个节点可以同时发布和订阅多个主题。例如图1-5所示，节点a和节点b在节点管理器连通下，节点a发布主题2，接收主题1;节点b发布主题3，接收主题2,可以看出两个节点在主题2下进行通讯。
(6)服务(Service):服务是节点间通信的另外一种方式，服务允许节点之间发送请求(request和接收应答(respond)
2.2 NAO机器人介绍
NAO机器人是一款由Aldebaran Robotics公司开发的机器人，是仿人机器人科研、竞赛领域内使用最广泛的机器人之一[36]。该机器人也是一款可完全编程的开放式自主机器人，操作比较方便，内置的英特尔处理器使得机器人的内部运算速度很快，动作非常流畅，反应也比较快。另外，NAO机器人内部装有Linux的嵌入式处理器，可以使用包括Python, Java, Matlab和C++等多种主流的语言进行编程控制。而且NAO机器人还自带一款图形化编程软件，可以用来编写机器人动作、控制机器人摄像头等。
NAO机器人开放式编程的特点，使其广泛应用于机器人视觉跟踪、机器人感知与认知、手势建模、机器人定位与导航和人工智能等领域。在国外，还有一些高校和机构把NAO机器人运用到治疗小儿自闭症和导盲犬机器人开发等医疗保健领域。自2007年，NAO机器人代替索尼狗爱宝，成为了机器人世界杯标准平台组的统一硬件平台。NAO机器人全身结构示意图如图2.2所示。
图2.2 NAO机器人全身结构示意图
NAO机器人是一款拥有32个自由度的双足仿人形机器人，其动力由机器人内部的直流电机提供。NAO机器人的硬件配置如下表所示。
表2.1 NAO机器人硬件配置表
| 尺寸（H*D*W） | 重量 | 制作材料 | 电池 | CPU | RAM/FLASH MEMORY |
| 573*275*311mm | 5.2KG | ASB-PC | 锂电池 | ATOM Z 530 | 1G/2G |
在机器人内部安装了视觉、触觉等多种形式的传感设备，用来感知周围的信息。NAO机器人使用的传感器分为以下几种:超声波传感器、压力传感器、惯性传感器。
本文将对NAO的手臂进行运动规划研究，所有在本小节着重探讨NAO的手臂。这里以NAO左手臂为例，它的关节活动范围如表2.2所示。
表2.2 NAO机器人左手关节活动范围
| 关节名称 | 运动方向 | 范围（度） | 范围（弧度） |
| LShoulderPitch | 左手肩关节前后运动(Y) | -119.5 ~ 119.5 | -2.0857 ~ 2.0857 |
| LShoulderRoll | 左手肩关节左右运动(Z) | -18 ~ 76 | -0.3142 ~ 1.3265 |
| LElbowYam | 左手肘关节扭动（X） | -119.5 ~ 119.5 | -2.0857 ~ 2.0857 |
| LElbowRoll | 左手肘关节(Z) | -88.5 ~ -2 | -1.5446 ~ -0.0349 |
| LWristYam | 左手腕关节(X) | -104.5 ~ 104.5 | -1.8238 ~ 1.8238 |
| LHand | 左手 | 开或关 | 开或关 |
NAOqi是在NAO内部使用的主要软件，用来控制机器人。NAOqi Framework是为NAO编程的主要框架。该框架允许不同的模块(motion, audio, video)之间进行信息共享，完成信息的传递。通过使用NAOqi, NAO机器人可以实现编程、模拟、管理一体化，使用C++SDK, Python SDK以及脚本或者图像化界面进行编程，还可以从Choregraphe软件中调用相应的函数进行机器人的三维模拟。
2.3 机器人模型的分析与建立
在ROS系统中用URDF文件描述机器人的3D模型，URDF 是一种用于表示机器人模型的 XML 格式。机器人的组成有连杆和关节以及各种传感器，因此URDF文件中包含了大量的连杆和关节的信息。在该文件中，连杆使用<link>表示，包括连杆质量、外观、碰撞等基本属性，这部分内容存储在<link>和</link>标签之间。关节使用勺<joint>表示，包括两个连杆的关节参数，存储在<joint>和</joint>标签之间。相邻关节连接如图2.3所示，父连杆与子连杆采用轴向关节相连。
图2.3 相邻关节连接示意图
机器人的描述文件是XML Macros 宏（Xacro）格式，Xacro格式是优化后的URDF格式。URDF格式是由很多<link>和<joint>组成，<link>主要描述机器人手臂连杆部分的外观和物理属性，外观属性主要包括连杆的颜色（color）、尺寸（size）和形状（shape）等，物理属性主要包括碰撞矩阵（inertial matrix）、碰撞参数（collision properties）和惯性（inertial）等。<joint>描述了子link和父link之间的坐标变换，包括无限旋转、固定关节、旋转、移动等，本课题使用的URDF模型为NAO官方提供的机器人模型。在Rviz中可通过TF库实时获取坐标系和各关节转换关系，并发布到MoveIt!中。
RViz是ROS中的3D可视化工具，它只是作为一个显示工具，用来显示话题与消息的数据，而不提供其他的功能。在RViz中显示的NAO机器人模型如图2.4所示。
图2.4 RViz中NAO机器人三维模型
2.4 机器人运动规划组配置
2.4.1 MoveIt! 介绍
MoveIt![37]是ROS提供的一套具有运动控制和软件设计的开源软件库，它包含了环境感知、运动规划、正逆运动学及导航等功能的集合，可通过图像界面 (GUI) 进行机械臂运动和控制的配置。MoveIt!的系统结构如图2.5所示。
图2.5 MoveIt!的系统结构图
从结构图中可以看出，move_group是整个系统的控制中心，用来融合外围各独立模块，并提供ROS通讯类型的动作和服务。MoveIt!对外提供了三种可供用户使用的接口，分别为C++、Python编程接口和GUI图像界面接口，实现系统、各类传感器和软硬件之间的交互。Move group集成了topic和service的数据传递方法，可以实现对外提供关节状态信息、坐标变换信息和节点信息等。OMPL (Open Motion Planning Library)是集成了大量最优秀的运动规划算法所组成的运动规划库。MoveIt将OMPL融合进自身系统架构中，使用OMPL模块里的运动规划器给机器人提供运动轨迹规划。图2.6为MoveIt规划场景显示器框架图，规划场景监视器可以监听并发布机器人关节状态信息，保存机器人中的世界信息，并进行机器人碰撞检测，并且可以监听ROS系统中传感器信息。
图 2.6 MoveIt! 规划场景框图
2.4.2 运动规划组节点配置
MoveIt! Setup Assistant(设置助手)是一个图形用户界面，用于配置任何机器人以与 MoveIt 一起使用。它的主要功能是为机器人生成语义机器人描述格式 (SRDF) 文件。此外，它还生成其他必要的配置文件以与 MoveIt 管道一起使用，如图2.7为螺母拧紧机器人使用Setup Assistant配置页面。
图2.7 Setup Assistant图形用户界面
MoveIt! 设置助手可以配置虚拟关节、自碰撞矩阵、机器人规划组以及末端执行器状态等功能，配置流程如图2.8 所示。
图 2.8 MoveIt!参数功能包配置流程图
点击Create New MoveIt!Configuration Package按钮，创建一个新的配置。使用NAO机器人的URDF文件，并将其导入配置助手。依次完成NAO机器人碰撞免检矩阵(ACM)、虚拟节点、规划组群、机器人的姿态、末端执行器和被动关节等的配置。
图2.9为配置的运动规划组的部分内容，通过Setup Assistant(设置助手)，生成了一些配置文件(config文件夹)和一些Launch文件(launch文件夹)。
图2.9 设置Planning Groups信息
通过利用MoveIt! Setup Assistant完成了NAO机器人手臂、腿部和头部三部分的运动规划配置。得到了NAO_moveit_config功能包，后续即可通过MoveGroup编程接口，实现对NAO机器人手臂的规划和运动控制。
2.5 本章小结
本章借助 ROS 中的三维可视化工具 Rviz 以及运动规划工具 MoveIt!搭建 NAO机器人运动平台。分析了ROS 系统的基本组成、ROS 的运行机制和原理以及NAO机器人关节参数。在 ROS 系统下，使用 NAO 机器人模型文件，借助可视化工具Rviz 实现 NAO 机器人模型的三维可视化显示。编译运动组节点用于机器人的运动控制，完成 ROS 系统下 NAO 机器人运动平台的搭建。
第3章 基于非重构环境下的路径规划研究与实现
本章通过分析快速随机拓展树(RRT)算法的原理，并利用RRT算法，在Rviz现有的栅格地图上实现路径规划，并模拟NAO机器人沿着规划好的路径运动。首先通过分析机器人在二维平面上的约束类型，确定快速随机拓展树算法的参数。采用C++语言编程实现了RRT算法，通过分析相同环境下和不同环境下RRT算法的效果。
3.1 RRT算法原理
RRT (Rapid Exploring Random Tree)快速随机搜索树是一种基于随机采样的搜索路径的方法，它是一种类似树木根部生长扩展形式的路径搜索图[38]。RRT算法通过在状态空间中随机选取节点的方式，进行碰撞检测，将随机树的节点不断扩展至可通行区域，最终生成一条可行的路径，但是由于扩展过程中存在盲目性，所以算法收敛速度慢，规划的路径质量差。基于RRT的路径规划算法流程如下：
步骤1：将二维栅格地图视为状态空间，机器人在地图上的起始点被定义为随机树的根节点，将目标点定义为结束节点。
步骤2：在状态空间内随机生成一个节点，计算随机树中所有节点距离、的长度d，将d最小的节点定义为。
步骤3：使用线段连接和，定义扩展步长step，从出发沿的朝向扩展一个step长度，在该位置定义一个节点，判断及其与的连接线上是否与障碍物发生碰撞，若发生碰撞，则放弃该节点，然后跳转至步骤2，若无碰撞，则将节点加入到随机树中。
步骤4：重复执行步骤2和步骤3，若和之间的距离小于一个step，且没有与障碍物发生碰撞，则随机树构建成功，若超出最大迭代次数则路径规划失败。
RRT算法的扩展过程如图3.1。
图3.1 基本RRT算法扩展过程
RRT的基本原理是在现有的树的基础上，通过采样函数随机采样一节点，设定循环次数，在己有的树的基础上去生成新的节点，对于原有树和采样点可以通过模块去拓展。首先调用函数寻找到距离最近的，然后再调用生成步长函数去生成新的节点，最后再调用去做碰撞检测，如果发生了碰撞返回Trapped，如果未碰撞，则返回Advanced代表节点生成成功；函数判断是否到达目标区域，如果到达返回Reached，直到N次搜索全部完成。RRT算法路径搜索伪代码如下：
| 1 2 3 if then 4 return Advanced; 5 if then return Reached; 6 else return Trapped; |
| RRT Planner |
| 1 2 for do 3 4 if =Reached then break; 5 return |
3.2 RRT算法研究与测试
本文使用的开发环境是一台搭载Ubuntu16.04，ROS版本是Kinetic的笔记本电脑。电脑配置为16GRAM和Intel i7十二核处理器，显卡型号为英伟达RTX2060，文中的所有算法都运行在该笔记本电脑上。该算法的实现分为两个部分，第一步，构建路径规划所处的环境，本节构建的环境为一个边长为100m的正方形，在这个区域里面，分别构建一个20m 30m和10m 30m的长方形的障碍。第二步，进行路径规划，指定起始点和终止点，使用RRT算法，从起始点到终止点之间搜索一条路径。结果如图3.2所示。
图3.2 RRT算法求出路径图
其中，创建环境的主要流程如图3.3所示。首先，初始化env node节点，创建ROS句柄，并通知ROS Master节点，将要在path planner rrt话题上发布visualization msgs::Marker类型的消息，这样ROS Master节点控制器就会告诉所有订阅path-planner rrt话题的节点，有消息要发布。在ROS Master活跃的情况下，1秒发送一次边界和障碍信息。
图3.3 环境创建流程图
上述100m 100m正方形环境边界和20m 30m障碍都是visualization msgs::Marker类型的消息(该消息的详细情况如表3.1所示)，其中visualization msgs::Marker消息的详细情况如下表。接下来初始化环境，到此环境创建完成。最后env node节点每秒向ROS Master发布一次环境边界和障碍的消息。
表3.1 visualization_msgs::Marker消息详细情况
| 编号 | 内容 | 编号 | 内容 |
| 1 | uint8 ARROW=0 | 22 | geometry_msgs/Vector3 scale |
| 2 | uint8 CUBE=1 | float64 x | |
| 3 | uint8 SPHERE=2 | float64 y | |
| 4 | uint8 CYLINDER=3 | float64 z | |
| 5 | uint8 LINE STRIP=4 | ||
| 6 | uint8 LINE LIST=5 | 23 | std_msgs/ColorRGBA color |
| 7 | uint8 CUBE LIST=6 | float32 r | |
| 8 | uint8 SPHERE LIST=7 | float32 g | |
| 9 | uint8 POINTS=8 | float32 b | |
| 10 | uint8 TEXT VIEW FACING=9 | float32 a | |
| 11 | uint8 MESH RESOURCE=10 | ||
| 12 | uint8 TRIANGLE LIST=11 | ||
| 13 | uint8 ADD=0 | 24 | duration lifetime |
| 14 | uint8 MODIFY=0 | ||
| 15 | uint8 DELETE=2 | 25 | bool frame locked |
| std_msgs/Header header | |||
| 16 | uint32 seq | 26 | geometry_msgs/Point[]points |
| time stamp | float64 x | ||
| string frame_ id | float64 y | ||
| 17 | string ns | float64 z | |
| 18 | int32 id | ||
| 19 | int32 type | 27 | std_msgs/ColorRGBA[] colors |
| 20 | int32 action | float32 r | |
| geometry_msgs/Pose pose | float32 g | ||
| geometry_msgs/Point position | float32 b | ||
| float64 x | |||
| float64 y | 28 | string text | |
| float64 z | |||
| 21 | geometry_msgs/Quaternion orientation | 29 | string meshes resource |
| float64 x | |||
| float64 y | 30 | bool mesh use embedded materials | |
| float64 z | |||
| float64 w |
路径规划整个程序的流程如图3.4所示，它的的步骤为:首先，初始化rrt node节点，创建ROS句柄。定义发布节点，发布数据到话题path_lanner_rrt上，消息格式同样为visualization msgs::Marker。指定初始点(2, 2)和终止点(95, 95)的位置，初始化RRT树，接下来进入循环，每0.01 s执行一次RRT算法，该算法的具体流程介绍见上一小节。
图3.4路径规划流程图
由于RRT算法是一种随机采样算法，它只是按照一定的规则进行拓展，所以不改变起始点和目标点的情况下，在相同环境中，规划出来的路径也会有很大的差异。如图3.5与3.6所示，两张图起始点和目标点都一致，起始点坐标均为(2, 2)，目标点坐标均为(95 , 95)环境、障碍也一样，最终规划出来的路径不同，其中路径在图中以黄色较粗的线条表示，红色方块为模拟的NAO机器人模型沿路径运动。
图3.5 相同环境下RRT算法规划出的路径1
图3.6 相同环境下RRT算法规划出的路径2
在此基础上，我们改变环境中障碍的大小、数量和位置，测试在不同环境下的路径的区别。两种环境下起始点与目标点坐标都分别为(0, 0)和(100 , 100)。如图3.7所示为测试环境一，在两个矩形和两个L形障碍物的情况下进行路径规划，得到规划出的路径。如图3.8所示为测试环境二，在五个矩形障碍物的情况下进行路径规划，得到规划出的路径。
图3.7 测试环境一下RRT算法规划出的路径
图3.8 测试环境二下RRT算法规划出的路径
3.3 本章小结
本章主要利用快速随机拓展树(RRT)算法，在现有的栅格地图上实现路径规划，并模拟NAO机器人沿着规划好的路径运动。首先通过分析机器人在二维平面上的约束类型，确定快速随机拓展树算法的参数。采用C++语言编程实现了RRT算法，通过分析相同环境下RRT算法生成路径不同，验证了RRT算法的随机性。然后在Rviz可视化界面中模拟不同障碍物数量和种类的环境下，从起始点到目标点的路径情况在二维栅格地图中利用快速随机拓展树算法实现路径规划。
第4章 NAO手臂的运动规划研究与实现
本章主要利用RO S中的MoveIt!工具以及MoveGroup接口，编程实现NAO机器人手臂不同场景下的运动规划。首先基于手臂运动的基础理论，研究了笛卡尔空间以及关节空间的手臂轨迹规划，并分别在关节空间和笛卡尔空间下对NAO机器人手臂进行规划。使用MoveIt!对NAO机器人的左手臂指定目标状态，采用OMPL运动规划库实现左手臂从初始状态到目标状态的运动。通过MoveGroup接口，编程实现NAO机器人手臂的运动规划。
4.1 MoveIt！运动规划
根据第二章的介绍，ROS提供MoveIt!工具，为实现多关节机械臂或者机器人执行复杂的运动提供了便利。领域构建集成机器人产品提供了一个易于使用的平台。本节利用第二章配置的运动组节点文件，实现NAO左手臂的运动规划。
使用命令roslaunch nao_moveit_config demo.launch运行demo.launch文件，得到如图4.1所示界面，界面中显示的是该命令生成的虚拟NAO机器人。MotionPlanning菜单中默认状态下是对左手臂(leftarm and hand)进行运动规划，从界面中可以看到左手腕关节处会出现蓝色的球体，可供我们拖动手臂到指定位置，拖拽红绿蓝三色的箭头可以使左手臂进行单轴移动，而拖拽红绿蓝三色的圆圈，则是可以使手臂进行转动的操作。
图4.1 NAO机器人MoveIt!可视化控制界面
本文默认是使用OMPL运动规划库中的RRTConnect算法。NAO机器人手臂的运动规划主要是对关节数据、角度进行修改，达到不同的位置，完成运动规划的过程。在RO S中表现出来就是将修改后的数据发送到话题“/joint_states”上，达到机器人手臂运动规划的目的，而发送给该话题的数据格式为“sensor_msgs/JointState "，使用"rosmsg show sensor_msgs/JointState”命令可以查看该数据格式具体包含的内容。运行上述命令可以得到该数据格式包含“std_msgs/Headerheader、string[] name、float64[] position、float64[] velocity、float64[] effort"，而“std_msgs/Header header”包括“uint32 seq, time stamp, string frame_id”。
4.2 关节空间下的轨迹规划
机器人关节空间下的轨迹规划用关节角度值描述，用运动函数表示各个关节。因其各个关节运动函数相互独立[39]，计算简单且不存在奇异性矩阵问题。仅需规划好中间节点变量函数保证整个轨迹规划过程中是连续且各关节到达始末点时间相同即可。使用多项式插值法来“内插”预设路径，产生一系列路径点，适合点到点运动控制场景下使用。如无需避障情况下机器人搬运操作等。对关节空间下插值算法进行基础性研究。
多项式拼接是指在相邻两节点间使用多项式进行插值，前一段轨迹的终止速度、加速度是后一段轨迹的起始速度、加速度，从而将各段轨迹依次平滑地连接起来。常用的多项式插值一般有三次多项式插值和五次多项式插值，多项式次数越高，关节轨迹的精度越高，关节运行过程也更加平稳，但同时计算量也随之增大。分别对三次多项式和五次多项式插值原理进行描述。
(1) 三次多项式插值
目标函数三次多项式共有四个未知数，需四个条件才能求出这四个系数。可通过目标点的角度和角速度给定四个条件从而求出四个系数。将时间视为一个独立变量，得到三次多项式插值下对应函数为:
为得出上式中四个不同系数解，需要四个约束条件。起始点和终止点的节角度, 和角速度., 。
设定起始点为0，解得各系数是:
(2) 五次多项式插值
目标函数五次多项式共有六个未知数，需六个条件才能求出这六个系数。可通过目标点角度和角速度及角加速度给定这六个条件从而求出六个系数。将时间视为一个独立变量，到五次多项式插值下对应函数为:
为得到出上式中六个不同系数解，六个约束条件约束条件分别是起始点和终止点关节角，，角速度，，角加速度，：
设定起始点为0，令关节运动参数=0，=0，=0，解得各系数是:
本节以NAO机器人的左手臂为例进行介绍，控制NAO左手臂位置的参数一共6个，它们名称分别为“LShoulderPitch, LShoulderRoll, LElbowYaw, LElbowRoll, LWristYaw, LHand"。使用MoveIt!对左臂在关节空间下进行轨迹规划，利用五次多项式插值算法，使其从A点运动到B点。在Rviz中对算法进行实验，得到规划出的直线路径和机器人运动轨迹，如图4.2所示。
图4.2 关节空间下机器人手臂运动轨迹
由第二章NAO机器人介绍部分可知，控制NAO机器人各关节位置的参数一共有42个，都以float64的格式保存到position数组中。本节以NAO机器人的左手臂为例进行介绍，控制NAO左手臂位置的参数一共6个，它们名称分别为“LShoulderPitch, LShoulderRoll,LElbowYaw, LElbowRoll, LWristYaw, LHand"，分别对应position数组的14-19位(数组从0开始计数)。使用ROS中的rqt_plot工具查看各个关节位置的数据，可以得到如图4.3所示的曲线，其中纵坐标为弧度，横坐标为时间。由图4.3可知，整个规划过程中，初始状态时LShoulderPitch, LShoulderRoll, LElbowYaw, LElbowRoll, LHand关节的位置值为0弧度，LWristYaw关节的位置值为-0.75弧度，目标状态的LHand关节的位置值保持不变，LWri stYaw关节的位置值为-1.3弧度，其他几个关节的位置值分别有不同程度的增加。由4.3可以看出通过上述算法，在关节空间下对手臂进行轨迹规划生成了一条连续、平滑的机器人运动路径。
图4.3 机器人手臂关节空间下运动过程各关节位置变化
节点图表示ROS系统运行过程中，所有主题节点之间的关系。节点图由rqt-graph工具生成，在ROS Master节点控制器运行的前提下，启动一个新的终端，输入rosrun rqt-graph rqt-graph，即可启动rqt-graph工具。本节中，虚拟的NAO机器人运动规划时在ROS中运行的节点图如图4.4所示，图中Topic(话题)采用矩形线框表示，Node(节点)采用椭圆线框表示(包括发布和订阅的节点)，箭头表示节点与主题之间的关系。
图4.4 NAO机器人节点图
4.3 笛卡尔空间下的轨迹规划
上文中机器人手臂运动均是在关节空间下，通过控制手臂的五个轴来控制手臂运动，每个轴的运动都是通过插补来完成运动的，五个轴对于其他轴是如何运动的都互不关心，因此，机器人末端套筒在两个已知空间点之间走出的轨迹是任意的弧线。考虑到手臂五轴空间结构的复杂性，虽然关节空间下机器人工作空间内的所有位置和姿态都可以到达，但是到达指定末端位姿时姿态和轨迹都是不可预测的。要想确定机器人末端运行轨迹，需要对手臂进行直线轨迹规划。
笛卡尔空间下手臂末端位姿表示方法与关节空间有较大不同，其末端的位姿用方向矢量表示，其坐标点使用矩阵来表示。同时笛卡尔空间下手臂运动轨迹由许多中间点（路点）来描述，这些路点形成了手臂运动的轨迹。要想保证手臂运动过程节点间加速度的连续性和节点之间连线的平滑，需要同时规划节点之间的速度和加速度，防止在执行轨迹时机器人运动产生迟滞，导致其磨损加剧。
与各种不规则曲线相比较，直线轨迹规划是笛卡尔空间下相对简单的规划方法，并且大多数复杂的曲面曲线的规划可以通过划分成许多直线来完成。机器人手臂运动过程的直线插补需要首先获得手臂的起始位姿与终点位姿，然后在起始和终点之间通过插补获得多个路点来进行直线轨迹的规划，路点的个数决定规划的时间和路径的精度。本文中手臂运动的直线轨迹规划实现原理如下：
（1）获得始末位置直线轨迹的长度
设机器人初始位置为，终点位置为，直线轨迹规划的长度就是与的间距，其大小由如下公式可以计算得到：
（2）确定直线轨迹中的速度变化阶段
在螺母插接运动过程的直线轨迹规划理论上是先加速到固定速度，然后匀速运动，最后再减速到停止过程，具体可以分为两种不同的情况：
第一种情况是当减速运动的距离和加速运动的距离小于直线轨迹的长度时，这样能够完成整个插接运动过程的速度要求，因此机器人走过整个路径所需时间为加速过程、匀速过程和减速过程所需时间总和。
第二种情况是当减速运动的距离和加速运动的距离大于直线轨迹的长度时，需要将直线轨迹分为加速过程和减速过程两个部分，并且两个阶段各占一半，其所用时间均为，因此所需时间总和为。
（3）确定机器人末端直线轨迹中插补点的数量
式中，为插补点的个数，为对直线进行插补时的时间周期，由于可能为小数，因此对进行取整得到的就是最终插补点个数。
得到插补点个数之后，按照如图4.5所示的直线轨迹插补原理进行插补，图中为插补增量，，，。
图4.5 机器人直线轨迹插补工作原理图
（4）对直线轨迹向量化
（5）对直线轨迹微分化
在某一时间机器人末端套筒的位置可以表示成如下向量形式：
因此，利用公式（5-4）（5-6）可以得到笛卡尔空间下不同时刻机器人末端套筒的坐标为：
（6）把直线轨迹中的所有路径点转换到关节空间下
通过上述直线轨迹规划算法得到笛卡尔空间下轨迹点，将这些轨迹点的坐标利用逆变换转换到关节空间下。本文利用Moveit!提供的API接口对生成的直线轨迹进行执行，实现机器人的直线插补过程。
在仿真环境下对直线轨迹规划算法进行测试。首先以初始位姿作为起点，初始位姿y方向加上100mm作为终点位姿，计算末端套筒移动距离。接下来根据移动距离和所需的末端速度定义轨迹时间，在初始点和目标点之间插值，设置插值步长为0.01，得到中间的插值轨迹点个数和坐标。完成路径规划后通过ROS addTimeParametrization模块进行处理，该模块能够为规划出的空间轨迹添加速度与加速度约束，其优化算法采用的是TOPP (Time-Optimal Path Parameterization Algorithm)[40]，通过输入规划路径中的所有轨迹坐标点，就可以利用时间最优的前提，得到每个轨迹坐标点的速度、加速度和时间信息。最后在关节空间下执行运动。在Rviz中对笛卡尔直线插值算法进行实验，得到规划出的直线路径和机器人运动轨迹，如图4.6所示。
图4.6 笛卡尔空间下机器人运动轨迹
如图4.7所示为手臂在笛卡尔空间下直线运动过程各关节位置变化曲线，由4.7可以看出通过上述算法，在笛卡尔空间下通过直线轨迹插补生成了一条连续、平滑的机器人运动路径。
图4.7 笛卡尔空间下手臂直线运动过程各关节位置变化
4.4 本章小结
本章研究机器人运动规划技术。针对人型机器人 NAO 的手臂，主要对关节空间下三次多项式插值和五次多项式插值以及笛卡尔空间下的直线轨迹规划方法和原理进行研究。利用 ROS 中的 MoveIt!工具，对机器人的手臂关节分别在关节空间和笛卡尔空间进行运动规划。
第5章 NAO机器人运动与手臂规划
上一节在Rviz可视化界面的菜单中实现虚拟NAO机器人手臂的运动规划。作为一种服务机器人，更多的时候需要自主完成某项工作，例如移动手臂到茶几上取一个茶杯。基于上述研究，在二维栅格地图中，实现 NAO 机器人在地图中从起始点移动到目标点，并在目标点完成手臂从初始位置移动到指定位置，完成手臂的空间运动规划问题。
5.1 机器人运动实验
本节实现NAO机器人在地图中从起始点移动到目标点，并在目标点完成手臂从初始位置移动到指定位置。首先使用NAO python SDK使机器人从A(0,0,0)点运动到B(5, 2, 1)点，在Rviz中机器人起始点如图5.1所示，红色直线为机器人运动轨迹，机器人运动过程如图5.2所示，机器人运动到目标点如图5.3所示。
图5.1 NAO机器人起始点
图5.2 NAO机器人运动过程
图5.3 NAO机器人运动到目标点
5.2 机器人手臂规划实验
实现 NAO 机器人在地图中从起始点移动到目标点之后，接下来在目标点完成手臂从初始位置移动到指定位置，在关节空间下完成手臂的空间运动规划问题。机器人手臂在目标位置的轨迹规划如图5.4所示。
图5.4 机器人手臂轨迹规划
5.3 本章小节
本章基于上述研究，在二维栅格地图中，实现 NAO 机器人在地图中从起始点移动到目标点，并在目标点完成手臂在关节空间下从初始位置移动到指定位置，完成手臂的空间运动规划问题。
扫一扫
1643
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
