一种空间机械臂全局无碰轨迹规划系统的制作方法

本发明涉及一种空间机械臂全局无碰轨迹规划系统，包括人机交互模块、三维视景仿真及碰撞检测模块和无碰轨迹规划模块，属于机器人仿真技术领域。

背景技术：

目前，研制一条真实空间机械臂的成本代价很高，三维视景仿真主要是利用虚拟现实技术，在数据的驱动下，对空间机械臂运动进行三维可视化演示，在不需要真实硬件的条件下，可以深入地研究空间机械臂的安全运动问题。同时，我们知道，针对机械臂的轨迹规划需要在三维空间内进行，正确地描述这种以曲面为边界的空间是解决机器人无碰轨迹规划的关键所在。

此外，对于空间机械臂而言，在地面上首先通过仿真系统对指令序列进行验证，进而再将指令发送给真实的空间机械臂执行，是一种减小风险、提高安全性的通常做法。

技术实现要素：

本发明的技术解决问题是：克服现有技术的不足，提供一种空间机械臂全局无碰轨迹规划系统，通过建立三维视景仿真系统，基于几何关系的碰撞检测，实时标记出空间机械臂与周围环境的最短距离，进而利用关节空间随机采样方法，计算搜索出一组与周围环境无碰撞、且满足机械臂运动能力的轨迹数据，避免了复杂的动力学建模、求解过程。

本发明的技术解决方案是：

提供一种空间机械臂全局无碰轨迹规划系统，包括碰撞检测模块、关节空间随机采样模块、路径搜索器、逆运动学模块、路径优化器以及轨迹生成模块；

碰撞检测模块接收输入的空间机械臂dh参数、初始及目标构型以及环境信息，接收关节空间随机采样模块生成的空间机械臂构型，计算构型自身是否发生碰撞，接收路径搜索器输出的局部路径，判断局部路径中空间机械臂与基座各部件是否发生碰撞；

关节空间随机采样模块接收空间机械臂dh参数，生成所有可能的构型，并通过碰撞检测模块选出自本身无碰撞的构型发送给路径搜索器；

逆运动学模块接收空间机械臂dh参数以及末端执行器的初始与期望位姿，将其变换为构型发送给路径搜索器；

路径搜索器连接构型形成多条局部路径，并通过碰撞检测模块选出无碰撞的局部路径发送给路径优化器；

路径优化器，基于无碰撞的局部路径生成多个可行的从初始构型到期望构型的全局路径，按照关节空间最短路径的约束进行优化后，发送给轨迹生成模块；

轨迹生成模块使用空间机械臂的最大运动速度与帧率对优化后的路径作插值，生成一条含时轨迹。

优选的，碰撞检测模块，根据dh参数计算空间机械臂的操作空间，对操作空间之外的基座部件不进行碰撞检测。

优选的，还包括模型凸处理模块，对空间机械臂各个关节以及基座各部件进行凸处理，构建包围盒，使各个关节及基座各部件成为凸面体。

优选的，碰撞检测模块采用gjk算法进行碰撞检测，在三维空间中检测两凸面体是否重叠、相交或分离，如果存在重叠、相交表明会发生碰撞。

优选的，路径搜索器使用快速搜索随机树算法，沿空间机械臂期望构型方向搜索构型空间，通过连接采样得到的无碰构型形成多条局部路径。

优选的，路径优化器优化的方法为，取最短路径上任意两点，如果连接两点的直线间的运动不会发生碰撞，则使用直线代替两点间原来路径，直至遍历路径中的所有点，获得优化后的路径。

优选的，轨迹生成模块获取优化后的路径，令q¹,q²,...q^j,...,qⁿ为路径上的多个关节向量描述，wm为各个关节运动的最大角速度，tp为每帧的时间间隔，i为空间机械臂关节数，在第i个关节的相邻第j个路径点与第j-1个路径点之间线性插入n个点使路径满足机械臂运动能力的要求，其中从而获得机械臂含时轨迹。

优选的，还包括数据驱动模块，对空间机械臂及基座各部件进行建模，按照空间机械臂的含时轨迹，将空间机械臂从初始构型至目标构型的运动过程形成动画并演示。

优选的，还包括三维视景仿真模块，根据真实的宇宙环境，构建仿真环境，作为动画背景。

优选的，仿真环境包括一个有宇宙星空贴图的背景图；建立一个太阳系节点，其他渲染的元素必须渲染在这个太阳系节点中；进行地球和月球的绘制和渲染；将空间机械臂及基座各部件在solidworks建模软件中进行模型分解，形成stp格式可视化三维模型，转换为igs格式，在3dmax软件中添加熏染信息，转换成ive格式，通过osg三维渲染引擎实现三维视景仿真显示。

优选的，三维视景仿真模块还包括采集模块，采集空间机械臂模型中各关节之间的距离并显示，采集空间机械臂模型中各关节与基座各部件模型之间的最短距离并显示，采集各个关节角角度并显示。

优选的，还包括人机交互模块，接收操作指令，通过数据驱动模块获得对应的构型，并通过三维视景仿真模块显示空间机械臂构型，采集空间机械臂模型中各关节之间的距离并显示，采集空间机械臂模型中各关节与基座各部件模型之间的最短距离并显示，采集各个关节角角度并显示。

本发明与现有技术相比具有如下优点：

(1)本发明对空间机械臂各关节及基座上的各部件进行凸处理；同时计算机械臂的操作空间，对操作空间之外的部件不进行碰撞检测，以此优化碰撞检测算法，提高计算效率。

(2)本发明设计了基于关节空间随机采样的规划算法，可通过简单地的迭代计算搜索出一组与周围环境无碰撞、且满足机械臂运动能力的轨迹数据，无需考虑复杂的建模过程。

(3)本发明对于已有轨迹数据，可以将该组轨迹的运行状态通过三维视景仿真直观地动态展示，再结合碰撞检测验证轨迹运行过程中的安全性和正确性，可以减少错误指令发送给真实空间机械臂的风险，辅助提高操作的安全性。

附图说明

图1为本发明的人机交互模块信息流图；

图2为本发明的碰撞检测优化处理流程图；

图3为本发明的无碰轨迹规划算法流程图。

具体实施方式

本发明的空间机械臂全局无碰轨迹规划系统，首先需要具备状态显示和指令驱动的功能。其次，需要根据真实的宇宙环境和空间机械臂(含基座)模型，搭建出近似的仿真环境；并可对空间机械臂运动进行碰撞检测，计算与环境的最短距离。最后，需要根据输入的空间机械臂相关信息以及周围的环境信息，计算搜索出一组与周围环境无碰撞、且满足机械臂运动能力的轨迹数据并输出和保存。

一、人机交互模块设计

本发明涉及的三维视景仿真及碰撞检测的空间机械臂全局无碰轨迹规划系统通过操作人员指令接收模块接收操作指令，用户可以通过向三维视景仿真系统发送轨迹规划指令与参数，完成轨迹规划过程，并保存轨迹数据；也可以向三维视景仿真系统发送已有的轨迹数据，用于在三维视景仿真中复现。

此外，系统通过空间机械臂状态显示模块在三维视景仿真主界面上呈现空间机械臂状态，包括空间机械臂在指令驱动下的实时构型和相关信息，如空间机械臂与周围环境(包括关节之间)的最短距离、末端关节与周围环境和其他关节的最短距离、各个关节角角度、星时，以及通讯信息等。

二、三维视景仿真环境搭建

系统首先根据真实的宇宙环境和空间机械臂模型，搭建出近似的仿真环境。具体地，宇宙环境主要包含了一个有宇宙星空贴图的背景图，来模拟宇宙星空环境。宇宙星空贴图的背景图被设置为绝对投影，无论仿真视野如何变化，背景图的方位和大小都不会改变，这样就模拟出了一个相对静止的宇宙背景。

同时建立一个太阳系节点，其他渲染的元素必须渲染在这个太阳系节点中，这样就保证了其他元素被包含在太阳系中。太阳的绘制使用了基于标志板的技术，标志板技术是一项使三维场景中的二维表面，一直朝向仿真视野的技术。太阳的绘制就是一张拥有太阳贴图的标志板，它始终朝向于仿真视野，实现了类似太阳的效果。

月球的绘制使用常规的绘制方法。使用球形的几何体，并在球体上贴有月球表面的纹理贴图。

地球的绘制主要包含地球几何体的绘制以及地球光晕的绘制。地球几何体的绘制：为了更加真实的表现地球，地球几何体的绘制并不像月亮的绘制那样简单，地球的几何体的绘制采用更加复杂，更加逼真的方式进行绘制，采用了更大的地球贴图，如果选用更清晰的地球表面贴图，会使得贴图文件巨大，内存消耗和渲染时间会迅速增大，严重影响了帧率，综合考量，本系统中采用的地球贴图的大小为14mb。

云层纹理贴图是直接与地球贴图进行混合的，将最终的云层渲染颜色与地球贴图按照一定的算法进行混合，形成了被云层包围的地球特效，这样的方法减少了对渲染资源的要求，有效的提高了帧率。

三维视景仿真环境的另一项主要任务是对执行现场的环境状态进行三维重构再现，即空间机械臂(含基座)模型在三维视景仿真环境中的显示。本系统首先将stp格式的空间机械臂(含基座)的可视化模型进行处理转换成ive格式，进而考虑到直接利用opengl引擎仿真场景的复杂操作会存在很大的工作量，最终选择通过osg三维渲染引擎实现虚拟场景的显示。具体地，为了能在虚拟场景中进行读取显示模型，先将卫星及灵巧操控系统可视化模型在solidworks建模软件中进行模型分解、并重新定义模型的坐标系，将模型由stp格式转换为igs格式；然后在3dmax软件中读取，添加颜色、光照等渲染信息，可以让模型在具有准确外形信息的同时可以拥有更真实的表面信息，能够为虚拟仿真系统提供更好的沉浸感和逼真度，然后导出为ive格式；通过osg开源渲染引擎可以读取ive文件，通过定义的dh坐标系间的关系可以完成空间机械臂(含基座)的拼接渲染显示。

三、碰撞检测及最短距离计算

基于三维视景仿真环境，系统采用优化的gjk算法进行碰撞检测，计算最短距离。由于gjk算法只对凸体有效，非凸体间的碰撞检测需要首先转换为凸体，然后利用gjk算法进行碰撞检测；此外，为减小计算时间，同时对碰撞检测区域进行了有效分析和限定。结合图2，具体步骤如下：

1.模型凸处理：gjk算法只对凸体有效，由于空间机械臂(含基座)不是凸体，所以首先需要对空间机械臂(含基座)模型进行凸处理，对空间机械臂各关节及基座各部件进行构建包围盒，使其成为凸体。

2.碰撞检测优化：计算空间机械臂的操作空间，对操作空间之外的部件不进行碰撞检测。

3.gjk碰撞检测计算：采用gjk算法进行碰撞检测与最短距离计算，核心思想就是在三维空间中检测两凸面体是否重叠、相交或分离，可通过明可夫斯基和进行求解。最短距离包含空间机械臂与周围环境(包括关节之间)的最短距离，以及末端关节与周围环境和其他关节的最短距离。

4.实时显示预警处理：根据碰撞检测结果，实时显示最短距离点，并用线段连接两点进行实时显示，当空间机械臂与自身及周边环境最短距离小于安全距离时，越过安全范围的关节部分会以红色高亮不断闪烁以提醒操作人员。

四、无碰轨迹规划算法

无碰轨迹规划模块根据用户输入的空间机械臂相关信息(空间机械臂初始构型/初始末端位姿、期望构型/期望末端位姿、机械臂的dh参数、最大运动速度以及机械臂轨迹运动最大允许时间等)以及周围的环境信息，基于关节空间随机采样的规划算法计算搜索出一组与周围环境无碰撞、且满足机械臂运动能力的轨迹数据并输出和保存。对于存在的多条可行轨迹，模块将默认输出一条在关节空间中最短的路径。成功规划的结果以pvt指令的形式给出(周期32ms)。当满足无碰撞要求的路径不存在时，给出路径规划失败的提示。

基于关节空间随机采样的规划算法由关节空间随机采样、碰撞检测、路径搜索、逆运动学计算、路径优化、轨迹生成等相关过程构成。具体如下：

1.关节空间随机采样：输入空间机械臂的dh参数(关节自由度，连杆长度、连杆扭转、连杆偏移、关节转角)，输出从关节空间中均匀随机采样得到空间机械臂的构型。

2.碰撞检测模块：输入构型或路径，输出结果是否无碰。如果输入的是一个构型，检测由采样模块采样得到的构型是否一个无碰的构型，否则删除该构型。如果输入的是一条路径，则检测该路径是否无碰，碰撞的话也同样删除该路径。该模块使用bullet物理引擎检测碰撞，对于输入的空间机械臂几何信息及环境信息，bullet使用gjk算法将几何多面体剖分为多个三角形，保存三角形的一系列顶点作为数据模型，检测碰撞。

3.路径搜索器：给定采样得到的多个无碰构型，度量无碰构型间的距离，使用快速搜索随机树算法，沿期望构型方向搜索构型空间，连接采样得到的无碰构型形成多条局部路径。并将路径输出给碰撞检测模块校验查询。检测到碰撞，则删除该路径，否则留下。

其中，快速搜索随机树算法思想为从路径的初始构型出发不断地搜索构型空间，使用碰撞检测算法检测构型和构型间的运动是否发生碰撞。直到由初始构型为根节点生成的搜索树相接于期望构型。此时认为成功搜索到一条无碰路径。

4.逆运动学模块：当给定输入为末端执行器的初始与期望位姿时，需要使用逆运动学将其变换到关节空间采样，求解路径。注意到自由度冗余时，可能有多个关节构型对应一个末端位姿，需要调用碰撞检测模块排除其中发生碰撞的构型，选出初始与期望构型。

5.路径优化器：对于多自由度含冗余的空间机械臂，可能存在多个可行的从初始构型到期望构型的全局路径，优化器从路径搜索器模块获得一条无碰的路径，分段优化各段路径的长度，最后输出至系统。

具体优化时，由于路径规划以随机生成的路径点作为局部规划的目标，路径规划得到的路径常有较多非必要的往复折返运动，路程较长且不够平滑，不适宜直接用于轨迹生成。取路径上任意两点，显然在关节空间中连接两点的直线路径最短，如果连接两点的直线间的运动不会发生碰撞，即可使用直线代替原来路径较长的曲折轨迹。

6.轨迹生成模块：路径规划以及优化器生成的是不含时间的路径。轨迹生成模块使用空间机械臂的最大运动速度与帧率对路径作插值，生成一条含时并满足机械臂运动能力的轨迹，输出至系统主界面。

具体地，当已知工作空间中的一条无碰路径，由路径上的多个关节向量描述为q¹,q²,...q^j,...,qⁿ，其中即在点j处的7个关节角位置。wm为各个关节运动的最大角速度，tp为每帧的时间间隔，给定以上条件，需要在第i个关节的路径点与线性插入n个点使满足机械臂运动能力的要求。其中

即可给出机械臂的一条无碰轨迹。

本发明利用虚拟现实技术，在数据的驱动下，建立了空间机械臂运动的三维视景仿真系统，在不需要真实硬件的条件下，研究了空间机械臂的安全运动问题。基于几何关系的碰撞检测，实时标记出空间机械臂与周围环境的最短距离，进而利用关节空间随机采样方法，计算搜索出一组与周围环境无碰撞、且满足机械臂运动能力的轨迹数据，避免了复杂的动力学建模、求解过程。此外，系统验证控制指令序列正确之后，才将该指令发送给远端的实物系统，从而大大提高了空间操作的安全性和执行效率。通过该系统，可以更深入地研究空间机械臂在结构化环境下的轨迹规划、碰撞检测等问题。

本发明说明书中未作详细描述的内容属于本领域专业技术人员公知的现有技术。