机器人的避障控制系统及方法与流程
本发明涉及计算机技术领域，尤其涉及一种机器人的避障控制系统及方法。
背景技术：
目前，自主移动机器人能够广泛应用于许多场景，比如担任展览馆的导览工作，带领参观者从一个展区介绍到另一个展区；餐厅的服务工作，主动欢迎客人，并带领客人到空位上点餐；公共场所的引导、巡逻工作，沿着程序设定的路线移动，有人需要帮助停下回答提问等等。在这些场景下，如何避免机器人在运动中碰撞环境中的障碍物，是一个重要的技术问题。目前，自主移动机器人是依靠自身的传感器来定位及避障，业界通常的避障方案是：在机器人上安装接近传感器(例如，超声波、红外、激光等传感器)，如果机器人检测到距障碍物一定距离(比如10cm)，则进行避障。
现有的避障方案有以下缺点：第一、只能在传感器所在高度平面检测到障碍物，对于四腿桌子等情况，假设传感器所在高度为30厘米，而桌面高度为60厘米，那么传感器无法检测到障碍物，最终会导致机器人撞上桌面；第二、只能在安装有传感器的方向检测到障碍物，假如机器人后面没有传感器，则后退会导致撞上障碍物。
因此，如何在传感器无法全面侦测的情况下，有效控制机器人避障已经成为一个亟待解决的技术问题。
技术实现要素：
本发明的主要目的在于提供一种机器人的避障控制系统及方法，旨在有效控制机器人避障。
为实现上述目的，本发明提供的一种机器人的避障控制系统，所述避障控制系统包括：
确定模块，用于实时或者定时获取机器人当前的定位数据，并根据当前定位数据及预先确定的移动区域内各个障碍物的位置数据，确定当前定位位置至目标位置路径中是否有离当前定位位置的距离小于预设距离的障碍物；
计算模块，用于若有离当前定位位置的距离小于预设距离的障碍物，则根据获取的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物的最短距离；
控制模块，用于根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向，根据计算出的运动方向控制机器人的运动姿态，以避开障碍物。
优选地，所述计算模块还用于：
对预先确定的机器人的3d模型及该障碍物的3d模型进行预处理，针对机器人的每一个关节，利用预先确定的算法找出包围各个关节的最小凸多面体，以将机器人的非凸模型转换为凸体模型；对该障碍物3d模型扇形均分，并对均分后的单个扇形进行凸分解；对获取的定位数据、预处理后的机器人3d模型数据及预处理后的该障碍物3d模型数据，利用预先确定的距离计算规则计算出机器人与该障碍物的最短距离。
优选地，所述计算模块还用于：
建立待扇形均分的障碍物3d模型的球形包围盒，找到球形包围盒的球心；设定一经过所述球心的初始扇形均分平面，将所述初始扇形均分平面按预设的旋转角绕所述球心进行多次旋转，以将球形包围盒均分为n个扇形部分；该球形包围盒的n个扇形部分作为障碍物3d模型的n个模型部分。
优选地，所述预先确定的距离计算规则包括：
根据机器人当前的定位数据及预先确定的筛选算法，对障碍物3d模型扇形均分后获得的各个模型部分进行筛选，筛选出待进行距离计算的模型部分；
对获取的定位数据、筛选出的模型部分，利用预先确定的距离计算算法计算出机器人与筛选出的模型部分的最短距离，该最短距离为机器人与障碍物的最短距离；
所述预先确定的筛选算法包括：
将障碍物3d模型的n个模型部分分别作为障碍物的n个节点，建立键值为相对于初始扇形均分平面的偏角的哈希表，以进行模型节点管理；
对各个模型部分进行标号，根据标号，建立标号为i的模型部分的偏角映射关系，定义所述偏角映射关系的哈希函数为：
hash(i)＝i*(360°/n)
其中，hash(i)代表标号为i的扇形模型部分与障碍物坐标系的x轴正轴的偏角；
建立机器人的运动学方程，根据建立的运动学方程计算出机器人各个关节的位姿，该运动学方程为：
ti＝a0a1a2…ai-1ai
其中，ak(k＝1,2,...,i)为机器人关节坐标系之间的齐次变换矩阵，a0为机器人当前位置矩阵，ti为第i个关节相对于机器人坐标系的位姿；
通过ti计算出机器人运动过程中各个关节在机器人坐标系下的坐标qi(x,y,z)，并计算出机器人坐标系变换到障碍物坐标系的变换矩阵tr，则机器人关节在障碍物坐标系下的坐标qi(xt,yt,zt)为：
qi(xt,yt,zt)＝trqi(x,y,z)
通过如下公式得到关节在障碍物坐标系下的偏角α：
根据偏角α及哈希函数hash(i)计算得到对应标号的模型部分，并基于对应标号的模型部分筛选出待进行距离计算的模型部分。
优选地，所述控制模块还用于：
将机器人及该障碍物3d模型投影到同一坐标系平面中；
根据预先确定的投影分析规则及障碍物3d模型投影到所述坐标系平面的投影区域外轮廓各个点的坐标，计算出该障碍物3d模型的投影相对于机器人的当前定位位置及目标位置有效遮挡区域的面积；
根据计算的最短距离及有效遮挡区域的面积确定出第一预设类型避障参数，根据目标位置与机器人当前定位位置的距离确定出第二预设类型避障参数，根据所述第一预设类型避障参数及所述第二预设类型避障参数确定出机器人当前应运动的方向。
优选地，所述预先确定的投影分析规则为：
设定坐标系平面的p1位置点为机器人的当前定位位置，p2位置点为目标位置，投影区域p3为障碍物3d模型在所述坐标系平面中的投影，并在所述坐标系平面中连接p1p2，得到直线j；
若直线j与投影区域p3没有交点或者交点只有一个，则确定不存在有效遮挡区域；
若直线j与投影区域p3的交点个数大于1，则直线j将投影分割为两部分；在投影区域p3中任意找一点ps，过ps作直线j的垂线，设定垂线与直线j的交点为pj,得到向量
优选地，所述第一预设类型避障参数为虚拟斥力，所述第二预设类型避障参数为虚拟引力，所述控制模块还用于：
根据计算的最短距离和有效遮挡区域的面积，利用第一计算规则计算出作用在机器人上的虚拟斥力；
根据当前定位位置与目标点位置的距离，利用第二计算规则计算出作用在机器人上的虚拟引力；
计算出该虚拟引力和虚拟斥力的合力方向作为机器人当前应运动的方向。
优选地，所述第一计算规则为：
设定机器人与障碍物的最短距离的向量为
其中，kr、br为预设的虚拟斥力系数，s0为预设的有效遮挡区域面积阈值，d0为预设的距离阈值；虚拟斥力方向与最短距离方向相同。
优选地，所述第二计算规则为：
设定目标位置对机器人的虚拟引力为
此外，为实现上述目的，本发明还提供一种机器人避障方法，所述方法包括以下步骤：
a1、实时或者定时获取机器人当前的定位数据，并根据当前定位数据及预先确定的移动区域内各个障碍物的位置数据，确定当前定位位置至目标位置路径中是否有离当前定位位置的距离小于预设距离的障碍物；
a2、若有离当前定位位置的距离小于预设距离的障碍物，则根据获取的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物的最短距离；
a3、根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向，根据计算出的运动方向控制机器人的运动姿态，以避开障碍物。
本发明提出的机器人的避障控制系统及方法，通过机器人当前的定位数据检测到有离当前定位位置的距离小于预设距离的障碍物时，根据机器人当前的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物在三维空间的最短距离，并计算出当前机器人应运动的方向，以控制机器人的运动姿态。由于能通过机器人与障碍物在三维空间的最短距离来控制机器人的运动方向，从而实现检测并避开机器人在三维空间中各个方向的障碍物，有效地控制机器人避障。
附图说明
图1为本发明机器人避障方法一实施例的流程示意图；
图2a为本发明机器人避障方法一实施例中对障碍物3d模型扇形均分的示意图；
图2b为本发明机器人避障方法一实施例中标号为k的扇形模型部分的示意图；
图3a为本发明机器人避障方法一实施例中机器人与障碍物的3d模型示意图；
图3b为本发明机器人避障方法一实施例中对立方体障碍物模型进行扇形均分的示意图；
图3c为本发明机器人避障方法一实施例中对模型部分的筛选示意图；
图3d为本发明机器人避障方法一实施例中计算最短距离向量的示意图；
图4为本发明机器人避障方法一实施例中确定有效遮挡区域的示意图。
图5为本发明避障控制系统10较佳实施例的运行环境示意图；
图6为本发明避障控制系统10较佳实施例的功能模块图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚、明白，以下结合附图和实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。
本发明提供一种机器人避障方法。
参照图1，图1为本发明机器人避障方法一实施例的流程示意图。
在一实施例中，该机器人避障方法包括：
步骤s10、机器人的避障控制系统实时或者定时(例如，每隔2秒)获取机器人当前的定位数据(例如在室内的位置、姿态等)，并根据当前定位数据及预先确定的移动区域内各个障碍物的位置数据，确定当前定位位置至目标位置路径中是否有离当前定位位置的距离小于预设距离的障碍物。例如，可依靠机器人自身的传感器来定位并判断与预先确定的移动区域内各个障碍物的距离，如可在机器人上安装接近传感器(例如，超声波、红外、激光等传感器)来判断机器人当前定位位置至目标位置路径中是否有离当前定位位置的距离小于预设距离的障碍物。
步骤s20、若有离当前定位位置的距离小于预设距离的障碍物，则根据获取的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物的最短距离。
在检测到机器人的当前定位位置与预先确定的移动区域内各个障碍物的距离之后，若判断没有障碍物离当前定位位置的距离小于预设距离，则继续沿目标位置路径移动并实时或者定时检测机器人与移动区域内各个障碍物的距离。若判断有离当前定位位置的距离小于预设距离的障碍物，则根据获取的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型计算出机器人与该障碍物的最短距离，以利用该最短距离来判断在三维空间中机器人沿目标位置路径移动时是否会碰撞到该障碍物，从而实现不仅能在机器人的传感器所在高度平面检测到障碍物，还能检测到三维空间中潜在的障碍物，以在机器人安装有传感器的方向和机器人没有安装传感器的其他方向上均能检测到三维空间中各个方向潜在的障碍物。其中，所述预先确定的机器人的3d模型及移动区域内各个障碍物的3d模型可以预先存储于机器人的存储单元中，或者，可以由机器人通过无线通信单元访问物联网系统服务器获取，在此不做限定。
步骤s30、根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向，根据计算出的运动方向控制机器人的运动姿态，以避开三维空间中各个方向潜在的障碍物，有效控制机器人在沿目标位置路径移动时的避障。
本实施例通过机器人当前的定位数据检测到有离当前定位位置的距离小于预设距离的障碍物时，根据机器人当前的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物在三维空间的最短距离，并计算出当前机器人应运动的方向，以控制机器人的运动姿态。由于能通过机器人与障碍物在三维空间的最短距离来控制机器人的运动方向，从而实现检测并避开机器人在三维空间中各个方向的障碍物，有效地控制机器人避障。
进一步地，所述步骤s20包括：
步骤s201、对预先确定的机器人的3d模型及该障碍物的3d模型进行预处理。
步骤s202、对获取的定位数据、预处理后的机器人3d模型数据及预处理后的障碍物3d模型数据，利用预先确定的距离计算规则计算出机器人与障碍物的最短距离。
例如，由于机器人和障碍物一般为非凸体，因此，可对机器人和障碍物的3d模型进行预处理如转换为凸体，以便后续更加准确和快速地计算出最短距离。
进一步地，所述步骤s201中的机器人3d模型预处理包括：针对机器人的每一个关节，直接利用预先确定的算法(例如，quickhull快速凸包算法)找出包围各个关节的最小凸多面体，以将机器人非凸模型转换为凸体模型。通过上述凸处理的机器人3d模型在后续计算最短距离向量时能有效提高计算速度和计算精度。
障碍物3d模型预处理的方式包括三种：第一种、构建非凸多面体的凸包围盒使之转换为凸体进行碰撞检测；第二种、对非凸多面体进行凸分解，使非凸模型转换为多个凸体进行碰撞检测；第三种、对障碍物3d模型扇形均分(即扇形剖分)，然后对均分后的单个扇形进行凸分解，这种先扇形均分再凸分解的方式相对于前两种不仅计算速度更快，而且计算精度更高。
进一步地，所述对障碍物3d模型扇形均分的步骤包括：
建立待扇形均分的障碍物3d模型的球形包围盒，找到球形包围盒的球心；
设定一经过所述球心的初始扇形均分平面，将所述初始扇形均分平面按预设的旋转角绕所述球心进行多次旋转，以将球形包围盒均分为n个扇形部分；该球形包围盒的n个扇形部分作为障碍物3d模型的n个模型部分。
例如，在一种具体实施方式中，可包括如下步骤：
x1、建立要扇形均分的障碍物3d模型m的球形包围盒b，找到球形包围盒b的球心o，然后在球心o处建立三维坐标系oxyz；
x2、过球心o做一条与三维坐标系oxyz中z轴重合的直线l，则xoz平面即为初始扇形均分平面，设xoz平面为α1，α1将障碍物3d模型分为2部分；
x3、将α1绕直线l选择一定角度β(β代表相邻扇形偏角)得到另外一个新平面α2，将新平面继续旋转β可以得到平面α3，旋转m-1次可以得到第m个平面αn；
x4、设β＝180/m，则m个平面可以把球形包围盒b均分为2m部分，障碍物3d模型即被分为2m个模型部分。通过上述步骤可以完成对任意模型，包括非凸模型的简单剖分，并通过哈希表进行管理剖分好的模型部分。
进一步地，所述对均分后的单个扇形进行凸分解的步骤包括：
采用delaunay三角剖分算法对障碍物3d模型进行表面三角剖分，产生三角面片(凸片)集合；并针对每一个三角面片构造与之对应的凸块。例如，将厚度为零的三角面片在其平面法向量方向进行预设厚度的拉伸，变为凸块。
进一步地，所述预先确定的距离计算规则包括：
根据机器人当前的定位数据(如室内位置、姿态等)及预先确定的筛选算法，对障碍物3d模型扇形均分后获得的各个模型部分进行筛选，筛选出待进行距离计算的模型部分；
对获取的定位数据、筛选出的模型部分，利用预先确定的距离计算算法(例如，gjk算法)计算出机器人与筛选出的模型部分的最短距离，该最短距离即为机器人与障碍物3d模型的最短距离。
进一步地，如图2a、2b所示，图2a为本发明机器人避障方法一实施例中对障碍物3d模型扇形均分的示意图。图2b为本发明机器人避障方法一实施例中标号为k的扇形模型部分的示意图。所述预先确定的筛选算法包括：
y1、将障碍物3d模型扇形均分后获得的n个模型部分分别作为障碍物的n个节点，建立key-value键值分别是相对于初始扇形均分平面(即xoz平面)的旋转角即偏角和模型几何信息数据的哈希表，以进行模型节点管理；
y2、对扇形均分获得的各个模型部分进行标号，从1开始进行标号；均分的n个扇形模型部分，相邻扇形偏角为360°/n，根据标号，建立于初始标号为i的扇形模型部分的偏角映射关系，代表所述偏角映射关系的哈希函数为：
hash(i)＝i*(360°/n)
其中，i为标号为i的扇形模型部分，hash(i)代表标号为i的扇形模型部分与障碍物坐标系的x轴正轴的偏角；
y3、建立机器人的运动学，根据建立的运动学计算出机器人各个关节的位姿，从建立的哈希表中查询出机器人附近的障碍物扇形区域。如下图2所示；当机器人在运动过程中，通过机器人运动学，运动学方程为：
ti＝a0a1a2…ai-1ai
其中，ak(k＝1,2,...,i)为机器人关节坐标系之间的齐次变换矩阵(可以通过机器人各关节的d-h参数确定)，a0表示机器人当前位置矩阵(与机器人当前定位数据对应)，ti为第i个关节相对于机器人坐标系的位姿；
通过ti计算出机器人运动过程中各个关节局部坐标系原点坐标的实时更新值qi(x,y,z)，进一步可以得到关节在障碍物坐标系下的偏角α：
α＝f(qi(x,y,z))
其中，qi(x,y,z)表示机器人关节在机器人坐标系下的坐标；tr表示机器人坐标系变换到障碍物坐标系的变换矩阵(为4*4的矩阵，机器人坐标系和障碍物坐标系已确定，该矩阵可以直接计算出来)，则机器人关节在障碍物坐标系下的坐标qi(xt,yt,zt)为：
qi(xt,yt,zt)＝trqi(x,y,z)
假定障碍物坐标系z轴正向朝上，遵循右手坐标系，设关节在障碍物坐标系下的偏角为α，则
向求解三角函数即可得到关节在障碍物坐标系下的偏角为α，获取到偏角α之后，即可根据代表所述偏角映射关系的哈希函数hash(i)计算得到对应标号的扇形模型部分，并基于对应标号的模型部分筛选出待进行距离计算的模型部分。例如，计算得到的扇形模型部分的标号为k，则可选取标号在[k-m,k+n]范围内的扇形模型部分进行最短距离计算。其中m、n为一预设数值，以选取标号为k的扇形模型部分附近的多个扇形模型部分作为待进行最短距离计算的模型部分。
如图3a所示，图3a为本发明机器人避障方法一实施例中机器人与障碍物的3d模型示意图。在一种具体实施方式中，机器人采用只有底盘的运动、没有手臂等其他运动关节的机器人，机器人3d模型采用高为1500mm，运动底盘半径为320mm的机器人3d模型，障碍物3d模型采用一个简单立方体模型，尺寸为2200mm*2200mm*1000mm，在障碍物模型坐标系下机器人当前的坐标为(1800,-100)。
图3b为本发明机器人避障方法一实施例中对立方体障碍物模型进行扇形均分的示意图。对障碍物模型进行预处理中，预处理主要是对障碍物模型进行扇形均分，如图3b所示，障碍物模型被扇形均分为32份，从x轴逆时针对扇形均分的模型部分进行编号，1,2，…，15,16，…，31,32；每一个模型块的夹角为：360/32＝11.25度，可以看出，编号1模型块与x轴正向偏角11.25度，编号2模型块与x轴正向偏角11.25*2＝22.5度，编号为i的模型块与x轴正向偏角：i*(360/32)。
图3c为本发明机器人避障方法一实施例中对模型部分的筛选示意图。在对模型部分的筛选过程中，因本实施例采用的机器人只有底盘的运动，没有手臂等其他运动关节，所以底盘位姿代表机器人的整体位姿，当前机器人的位置为(1800，-100)(相对于障碍物坐标系下坐标)，可以计算出机器人与障碍物坐标系的x轴正轴的偏角为354度；进而计算机器人对应扇形模型部分的标号为354/11.25＝31.5，向上取整得到32，所以待进行距离计算的对应扇形块编号32，也就是说机器人离编号为32的障碍物块最近。接下来选取k＝32附近的障碍物块，采用gjk计算与机器人之间的最短距离及最短距离点；选取m＝1，n＝2，则得到障碍物块范围是[31,34]，编号超过32的需要做简单转换，33转换为对应编号为1的障碍物块，34转换为对应编号为2的障碍物块；如图3c所示，最终选取编号是31,32,1,2的障碍物块进行最短距离计算。
图3d为本发明机器人避障方法一实施例中计算最短距离向量的示意图。在计算最短距离时，通过上述处理，已缩小障碍物块的范围(1,2,31,32)，直接采用gjk算法计算出机器人与障碍物间的最短距离点，如图3d所示，分别为障碍物上的点(x1,y1,z1)＝(1100,-100,-235),机器人上的点(x2,y2,z2)＝(1477,-100,-235)；则机器人与障碍物之间的最短距离向量
进一步地，所述步骤s30包括：
机器人的避障控制系统根据计算的最短距离分析是否需要避障；如若计算的最短距离大于预设距离阈值，则确定不需要避障，或者，若计算的最短距离小于或者等于预设距离阈值，则确定需要避障。若确定需要避障，则机器人的避障控制系统根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向，并根据计算出的运动方向控制机器人的运动姿态。
进一步地，所述根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向的步骤包括：
将机器人及该障碍物投影到同一坐标系平面中；
根据预先确定的投影分析规则及障碍物3d模型投影到所述坐标系平面的投影区域外轮廓各个点的坐标，计算出该障碍物的投影相对于机器人当前位置及目标位置有效遮挡区域的面积；
根据计算的最短距离及有效遮挡区域的面积确定出第一预设类型避障参数(例如，虚拟斥力)，根据目标点位置与机器人当前定位位置的距离确定出第二预设类型避障参数(例如，虚拟引力)，根据第一预设类型避障参数及第二预设类型避障参数确定出机器人当前应运动的方向。
进一步地，如图4所示，图4为本发明机器人避障方法一实施例中确定有效遮挡区域的示意图。所述预先确定的投影分析规则为：
设坐标系平面的p1位置点表示机器人所在位置即当前定位位置，p2位置点表示目标点所在位置即目标位置，投影区域p3表示障碍物3d模型在坐标系平面中的投影，并在坐标系平面中连接p1p2，得到一条直线j；
若直线j与投影区域p3没有交点或者交点只有一个，则确定不存在有效遮挡区域；
若直线j与投影区域p3的交点个数大于1，则直线j将投影分割为两部分(如图4所示的s1区域和s2区域)，在投影区域p3(例如s1或s2区域中)中任意找一点ps，过ps作直线j的垂线，垂线与直线j的交点为pj,进而得到向量
进一步地，所述第一预设类型避障参数为虚拟斥力，所述第二预设类型避障参数为虚拟引力，所述根据计算的最短距离及有效遮挡区域的面积确定出第一预设类型避障参数，根据目标位置与机器人当前定位位置的距离确定出第二预设类型避障参数，根据所述第一预设类型避障参数及所述第二预设类型避障参数确定出机器人当前应运动的方向的步骤包括：
对计算的最短距离和有效遮挡投影区域的面积，利用第一计算规则计算出作用在机器人上的一个虚拟斥力；
对当前定位位置与目标点位置的距离，利用第二计算规则计算出作用在机器人上的一个虚拟引力；
计算出该虚拟引力和虚拟斥力的合力方向，所述合力方向即为机器人当前应运动的方向。
进一步地，所述第一计算规则为：
设定机器人与障碍物的最短距离的向量为
其中，kr、br表示预设的虚拟斥力系数，s0表示预设的有效遮挡区域面积阈值，s0>0；d0表示预设的距离阈值，d0>0；虚拟斥力方向与最短距离方向相同。
依据上述关系式，当机器人与障碍物距离较远，超过设定的距离阈值d0时不进行避障，
进一步地，所述第二计算规则为：
本发明进一步提供一种机器人的避障控制系统。
请参阅图5，是本发明避障控制系统10较佳实施例的运行环境示意图。
在本实施例中，所述的避障控制系统10安装并运行于机器人1中。该机器人1可包括，但不仅限于，存储器11、处理器12及显示器13。图1仅示出了具有组件11-13的机器人1，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。
所述存储器11在一些实施例中可以是所述机器人1的内部存储单元，例如该机器人1的硬盘或内存。所述存储器11在另一些实施例中也可以是所述机器人1的外部存储设备，例如所述机器人1上配备的插接式硬盘，智能存储卡(smartmediacard,smc)，安全数字(securedigital,sd)卡，闪存卡(flashcard)等。进一步地，所述存储器11还可以既包括所述机器人1的内部存储单元也包括外部存储设备。所述存储器11用于存储安装于所述机器人1的应用软件及各类数据，例如所述避障控制系统10的程序代码等。所述存储器11还可以用于暂时地存储已经输出或者将要输出的数据。
所述处理器12在一些实施例中可以是一中央处理器(centralprocessingunit,cpu)，微处理器或其他数据处理芯片，用于运行所述存储器11中存储的程序代码或处理数据，例如执行所述避障控制系统10等。
所述显示器13在一些实施例中可以是led显示器、液晶显示器、触控式液晶显示器以及oled(organiclight-emittingdiode，有机发光二极管)触摸器等。所述显示器13用于显示在所述机器人1中处理的信息以及用于显示可视化的用户界面，例如应用菜单界面、应用图标界面等。所述机器人1的部件11-13通过系统总线相互通信。
请参阅图6，是本发明避障控制系统10较佳实施例的功能模块图。在本实施例中，所述的避障控制系统10可以被分割成一个或多个模块，所述一个或者多个模块被存储于所述存储器11中，并由一个或多个处理器(本实施例为所述处理器12)所执行，以完成本发明。例如，在图6中，所述的避障控制系统10可以被分割成确定模块01、计算模块02、控制模块03。本发明所称的模块是指能够完成特定功能的一系列计算机程序指令段，比程序更适合于描述所述避障控制系统10在所述机器人1中的执行过程。以下描述将具体介绍所述确定模块01、计算模块02、控制模块03的功能。
所述确定模块01，用于实时或者定时(例如，每隔2秒)获取机器人当前的定位数据(例如在室内的位置、姿态等)，并根据当前定位数据及预先确定的移动区域内各个障碍物的位置数据，确定当前定位位置至目标位置路径中是否有离当前定位位置的距离小于预设距离的障碍物。例如，可依靠机器人自身的传感器来定位并判断与预先确定的移动区域内各个障碍物的距离，如可在机器人上安装接近传感器(例如，超声波、红外、激光等传感器)来判断机器人当前定位位置至目标位置路径中是否有离当前定位位置的距离小于预设距离的障碍物。
所述计算模块02，用于若有离当前定位位置的距离小于预设距离的障碍物，则根据获取的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物的最短距离。
在检测到机器人的当前定位位置与预先确定的移动区域内各个障碍物的距离之后，若判断没有障碍物离当前定位位置的距离小于预设距离，则继续沿目标位置路径移动并实时或者定时检测机器人与移动区域内各个障碍物的距离。若判断有离当前定位位置的距离小于预设距离的障碍物，则根据获取的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型计算出机器人与该障碍物的最短距离，以利用该最短距离来判断在三维空间中机器人沿目标位置路径移动时是否会碰撞到该障碍物，从而实现不仅能在机器人的传感器所在高度平面检测到障碍物，还能检测到三维空间中潜在的障碍物，以在机器人安装有传感器的方向和机器人没有安装传感器的其他方向上均能检测到三维空间中各个方向潜在的障碍物。其中，所述预先确定的机器人的3d模型及移动区域内各个障碍物的3d模型可以预先存储于机器人的存储单元中，或者，可以由机器人通过无线通信单元访问物联网系统服务器获取，在此不做限定。
所述控制模块03，用于根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向，根据计算出的运动方向控制机器人的运动姿态，以避开三维空间中各个方向潜在的障碍物，有效控制机器人在沿目标位置路径移动时的避障。
本实施例通过机器人当前的定位数据检测到有离当前定位位置的距离小于预设距离的障碍物时，根据机器人当前的定位数据、预先确定的机器人的3d模型及预先确定的该障碍物的3d模型，计算出机器人与该障碍物在三维空间的最短距离，并计算出当前机器人应运动的方向，以控制机器人的运动姿态。由于能通过机器人与障碍物在三维空间的最短距离来控制机器人的运动方向，从而实现检测并避开机器人在三维空间中各个方向的障碍物，有效地控制机器人避障。
进一步地，上述计算模块02还用于：
对预先确定的机器人的3d模型及该障碍物的3d模型进行预处理；对获取的定位数据、预处理后的机器人3d模型数据及预处理后的障碍物3d模型数据，利用预先确定的距离计算规则计算出机器人与障碍物的最短距离。
例如，由于机器人和障碍物一般为非凸体，因此，可对机器人和障碍物的3d模型进行预处理如转换为凸体，以便后续更加准确和快速地计算出最短距离。
进一步地，上述计算模块02还用于：针对机器人的每一个关节，直接利用预先确定的算法(例如，quickhull快速凸包算法)找出包围各个关节的最小凸多面体，以将机器人非凸模型转换为凸体模型。通过上述凸处理的机器人3d模型在后续计算最短距离向量时能有效提高计算速度和计算精度。
障碍物3d模型预处理的方式包括三种：第一种、构建非凸多面体的凸包围盒使之转换为凸体进行碰撞检测；第二种、对非凸多面体进行凸分解，使非凸模型转换为多个凸体进行碰撞检测；第三种、对障碍物3d模型扇形均分(即扇形剖分)，然后对均分后的单个扇形进行凸分解，这种先扇形均分再凸分解的方式相对于前两种不仅计算速度更快，而且计算精度更高。
进一步地，上述计算模块02还用于：
建立待扇形均分的障碍物3d模型的球形包围盒，找到球形包围盒的球心；设定一经过所述球心的初始扇形均分平面，将所述初始扇形均分平面按预设的旋转角绕所述球心进行多次旋转，以将球形包围盒均分为n个扇形部分；该球形包围盒的n个扇形部分作为障碍物3d模型的n个模型部分。
例如，在一种具体实施方式中，可包括如下步骤：
x1、建立要扇形均分的障碍物3d模型m的球形包围盒b，找到球形包围盒b的球心o，然后在球心o处建立三维坐标系oxyz；
x2、过球心o做一条与三维坐标系oxyz中z轴重合的直线l，则xoz平面即为初始扇形均分平面，设xoz平面为α1，α1将障碍物3d模型分为2部分；
x3、将α1绕直线l选择一定角度β(β代表相邻扇形偏角)得到另外一个新平面α2，将新平面继续旋转β可以得到平面α3，旋转m-1次可以得到第m个平面αn；
x4、设β＝180/m，则m个平面可以把球形包围盒b均分为2m部分，障碍物3d模型即被分为2m个模型部分。通过上述步骤可以完成对任意模型，包括非凸模型的简单剖分，并通过哈希表进行管理剖分好的模型部分。
进一步地，上述计算模块02还用于：
采用delaunay三角剖分算法对障碍物3d模型进行表面三角剖分，产生三角面片(凸片)集合；并针对每一个三角面片构造与之对应的凸块。例如，将厚度为零的三角面片在其平面法向量方向进行预设厚度的拉伸，变为凸块。
进一步地，所述预先确定的距离计算规则包括：
根据机器人当前的定位数据(如室内位置、姿态等)及预先确定的筛选算法，对障碍物3d模型扇形均分后获得的各个模型部分进行筛选，筛选出待进行距离计算的模型部分；
对获取的定位数据、筛选出的模型部分，利用预先确定的距离计算算法(例如，gjk算法)计算出机器人与筛选出的模型部分的最短距离，该最短距离即为机器人与障碍物3d模型的最短距离。
进一步地，如图2a、2b所示，所述预先确定的筛选算法包括：
y1、将障碍物3d模型扇形均分后获得的n个模型部分分别作为障碍物的n个节点，建立key-value键值分别是相对于初始扇形均分平面(即xoz平面)的旋转角即偏角和模型几何信息数据的哈希表，以进行模型节点管理；
y2、对扇形均分获得的各个模型部分进行标号，从1开始进行标号；均分的n个扇形模型部分，相邻扇形偏角为360°/n，根据标号，建立于初始标号为i的扇形模型部分的偏角映射关系，代表所述偏角映射关系的哈希函数为：
hash(i)＝i*(360°/n)
其中，i为标号为i的扇形模型部分，hash(i)代表标号为i的扇形模型部分与障碍物坐标系的x轴正轴的偏角；
y3、建立机器人的运动学，根据建立的运动学计算出机器人各个关节的位姿，从建立的哈希表中查询出机器人附近的障碍物扇形区域。如下图2所示；当机器人在运动过程中，通过机器人运动学，运动学方程为：
ti＝a0a1a2…ai-1ai
其中，ak(k＝1,2,...,i)为机器人关节坐标系之间的齐次变换矩阵(可以通过机器人各关节的d-h参数确定)，a0表示机器人当前位置矩阵(与机器人当前定位数据对应)，ti为第i个关节相对于机器人坐标系的位姿；
通过ti计算出机器人运动过程中各个关节局部坐标系原点坐标的实时更新值qi(x,y,z)，进一步可以得到关节在障碍物坐标系下的偏角α：
α＝f(qi(x,y,z))
其中，qi(x,y,z)表示机器人关节在机器人坐标系下的坐标；tr表示机器人坐标系变换到障碍物坐标系的变换矩阵(为4*4的矩阵，机器人坐标系和障碍物坐标系已确定，该矩阵可以直接计算出来)，则机器人关节在障碍物坐标系下的坐标qi(xt,yt,zt)为：
qi(xt,yt,zt)＝trqi(x,y,z)
假定障碍物坐标系z轴正向朝上，遵循右手坐标系，设关节在障碍物坐标系下的偏角为α，则
向求解三角函数即可得到关节在障碍物坐标系下的偏角为α，获取到偏角α之后，即可根据代表所述偏角映射关系的哈希函数hash(i)计算得到对应标号的扇形模型部分，并基于对应标号的模型部分筛选出待进行距离计算的模型部分。例如，计算得到的扇形模型部分的标号为k，则可选取标号在[k-m,k+n]范围内的扇形模型部分进行最短距离计算。其中m、n为一预设数值，以选取标号为k的扇形模型部分附近的多个扇形模型部分作为待进行最短距离计算的模型部分。
如图3a所示，在一种具体实施方式中，机器人采用只有底盘的运动、没有手臂等其他运动关节的机器人，机器人3d模型采用高为1500mm，运动底盘半径为320mm的机器人3d模型，障碍物3d模型采用一个简单立方体模型，尺寸为2200mm*2200mm*1000mm，在障碍物模型坐标系下机器人当前的坐标为(1800,-100)。
对障碍物模型进行预处理中，预处理主要是对障碍物模型进行扇形均分，如图3b所示，障碍物模型被扇形均分为32份，从x轴逆时针对扇形均分的模型部分进行编号，1,2，…，15,16，…，31,32；每一个模型块的夹角为：360/32＝11.25度，可以看出，编号1模型块与x轴正向偏角11.25度，编号2模型块与x轴正向偏角11.25*2＝22.5度，编号为i的模型块与x轴正向偏角：i*(360/32)。
在对模型部分的筛选过程中，因本实施例采用的机器人只有底盘的运动，没有手臂等其他运动关节，所以底盘位姿代表机器人的整体位姿，当前机器人的位置为(1800，-100)(相对于障碍物坐标系下坐标)，可以计算出机器人与障碍物坐标系的x轴正轴的偏角为354度；进而计算机器人对应扇形模型部分的标号为354/11.25＝31.5，向上取整得到32，所以待进行距离计算的对应扇形块编号32，也就是说机器人离编号为32的障碍物块最近。
接下来选取k＝32附近的障碍物块，采用gjk计算与机器人之间的最短距离及最短距离点；选取m＝1，n＝2，则得到障碍物块范围是[31,34]，编号超过32的需要做简单转换，33转换为对应编号为1的障碍物块，34转换为对应编号为2的障碍物块；如图3c所示，最终选取编号是31,32,1,2的障碍物块进行最短距离计算。
在计算最短距离时，通过上述处理，已缩小障碍物块的范围(1,2,31,32)，直接采用gjk算法计算出机器人与障碍物间的最短距离点，如图3d所示，分别为障碍物上的点(x1,y1,z1)＝(1100,-100,-235),机器人上的点(x2,y2,z2)＝(1477,-100,-235)；则机器人与障碍物之间的最短距离向量
进一步地，上述控制模块03还用于：
根据计算的最短距离分析是否需要避障；如若计算的最短距离大于预设距离阈值，则确定不需要避障，或者，若计算的最短距离小于或者等于预设距离阈值，则确定需要避障。若确定需要避障，则根据获取的定位数据、计算的最短距离及该障碍物的3d模型，计算出当前机器人应运动的方向，并根据计算出的运动方向控制机器人的运动姿态。
进一步地，上述控制模块03还用于：
将机器人及该障碍物投影到同一坐标系平面中；
根据预先确定的投影分析规则及障碍物3d模型投影到所述坐标系平面的投影区域外轮廓各个点的坐标，计算出该障碍物的投影相对于机器人当前位置及目标位置有效遮挡区域的面积；
根据计算的最短距离及有效遮挡区域的面积确定出第一预设类型避障参数(例如，虚拟斥力)，根据目标点位置与机器人当前定位位置的距离确定出第二预设类型避障参数(例如，虚拟引力)，根据第一预设类型避障参数及第二预设类型避障参数确定出机器人当前应运动的方向。
进一步地，如图4所示，所述预先确定的投影分析规则为：
设坐标系平面的p1位置点表示机器人所在位置即当前定位位置，p2位置点表示目标点所在位置即目标位置，投影区域p3表示障碍物3d模型在坐标系平面中的投影，并在坐标系平面中连接p1p2，得到一条直线j；
若直线j与投影区域p3没有交点或者交点只有一个，则确定不存在有效遮挡区域；
若直线j与投影区域p3的交点个数大于1，则直线j将投影分割为两部分(如图4所示的s1区域和s2区域)，在投影区域p3(例如s1或s2区域中)中任意找一点ps，过ps作直线j的垂线，垂线与直线j的交点为pj,进而得到向量
进一步地，所述第一预设类型避障参数为虚拟斥力，所述第二预设类型避障参数为虚拟引力，上述控制模块03还用于：
对计算的最短距离和有效遮挡投影区域的面积，利用第一计算规则计算出作用在机器人上的一个虚拟斥力；
对当前定位位置与目标点位置的距离，利用第二计算规则计算出作用在机器人上的一个虚拟引力；
计算出该虚拟引力和虚拟斥力的合力方向，所述合力方向即为机器人当前应运动的方向。
进一步地，所述第一计算规则为：
设定机器人与障碍物的最短距离的向量为
其中，kr、br表示预设的虚拟斥力系数，s0表示预设的有效遮挡区域面积阈值，s0>0；d0表示预设的距离阈值，d0>0；虚拟斥力方向与最短距离方向相同。
依据上述关系式，当机器人与障碍物距离较远，超过设定的距离阈值d0时不进行避障，
进一步地，所述第二计算规则为：
需要说明的是，在本文中，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括该要素的过程、方法、物品或者装置中还存在另外的相同要素。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到上述实施例方法可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件来实现，但很多情况下前者是更佳的实施方式。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质(如rom/ram、磁碟、光盘)中，包括若干指令用以使得一台终端设备(可以是手机，计算机，服务器，空调器，或者网络设备等)执行本发明各个实施例所述的方法。
以上参照附图说明了本发明的优选实施例，并非因此局限本发明的权利范围。上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。另外，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。
本领域技术人员不脱离本发明的范围和实质，可以有多种变型方案实现本发明，比如作为一个实施例的特征可用于另一实施例而得到又一实施例。凡在运用本发明的技术构思之内所作的任何修改、等同替换和改进，均应在本发明的权利范围之内。
- 还没有人留言评论。精彩留言会获得点赞！