CN113878578A - 适用于复合型机器人的动态自适应定位方法和系统 - Google Patents

Abstract

本发明提供了一种适用于复合型机器人的动态自适应定位方法和系统，通过激光雷达和驱动电机，驱动机器人进行同步定位与建图、导航和运动规划；在机器人的机械臂本体上配置夹爪，进行物体抓取和转移；通过深度相机获取目标平面与立体的点云数据，并根据点云数据获取目标特征点；所述方法包括如下步骤：步骤1：驱动机器人进行同步定位与建图；步骤2：驱动机器人移动至预设工作地点；步骤3：识别预设工作地点的特征点；步骤4：根据特征点和机械臂构型，优化机器人位姿；步骤5：驱动机器人移动至优化位姿处。本发明能够主动适应工作台面的变化，针对工作太慢的变化自主调整移动机器人位姿，降低了工作中断率，提高了工作效率。

Description

适用于复合型机器人的动态自适应定位方法和系统

技术领域

本发明涉及机器人定位技术领域，具体地，涉及一种适用于复合型机器人的动态自适应定位方法和系统。

背景技术

复合型机器人是一种集成移动机器人和通用机械臂两项功能为一身的新型机器人，移动机器人代替了人腿脚的行走功能，通用机械臂代替了人胳膊的抓取功能。因此，复合型机器人集合了两种类型机器人的优势，移动机器人仅能完成一些定点巡逻，报警等单一任务，而通用机械臂仅能完成一些抓取、码垛、喷涂等固定动作，当二者结合为复合型机器人后，两者的优势相加起到了1+1>2的效果，能够应用于更加复杂的任务场景中，在不同的工作地点之间来回切换，完成多种类型的任务。

专利文献CN109493369A(申请号：CN201811058413.X)公开了一种智能机器人视觉动态定位跟踪方法，所述方法包括以下步骤：S10、特征信息提取步骤；S20、视频获取步骤；S30、视频处理步骤，S40、物体识别步骤；S50、精细化识别步骤，S60、目标定位跟踪步骤。

通常情况下，复合型机器人都需要预先建好地图并标记不同工作地点，当切换不同工作任务时，控制移动底盘运动至指定工作地点并开始工作。但这种解决方案的通用性和适应性极差，当工作台面上的目标发生改变后，机械臂可能无法完成工作，导致工作任务中断。因此，设计一种自适应性的定位方法，帮助复合型机器人在工作台面附近进行位姿微调，以帮助机械臂获得良好的工作环境，这是未来复合型机器人发展的重大趋势。

发明内容

针对现有技术中的缺陷，本发明的目的是提供一种适用于复合型机器人的动态自适应定位方法和系统。

根据本发明提供的适用于复合型机器人的动态自适应定位方法，通过激光雷达和驱动电机，驱动机器人进行同步定位与建图、导航和运动规划；在机器人的机械臂本体上配置夹爪，进行物体抓取和转移；通过深度相机获取目标平面与立体的点云数据，并根据点云数据获取目标特征点；所述方法包括如下步骤：

步骤1：驱动机器人进行同步定位与建图；

步骤2：驱动机器人移动至预设工作地点；

步骤3：识别预设工作地点的特征点；

步骤4：根据特征点和机械臂构型，优化机器人位姿；

步骤5：驱动机器人移动至优化位姿处。

优选的，所述步骤1包括：控制机器人在全地图中运动并遍历地图中的所有空间，根据激光雷达返回的数据建立机器人工作空间的平面地图，并在地图上的工作地点处进行标记。

优选的，所述步骤2包括：根据预设导航控制算法控制机器人运动至预设工作地点，在机器人到达预设工作地点后，将对应指令发送至机械臂控制设备；

所述预设导航控制算法包括A*算法、D*算法以及人工势场法。

优选的，所述步骤3包括：利用三维点云匹配技术获取工作台面的关键特征点，并将关键特征点发送至机械臂控制设备。

优选的，所述步骤4包括：

定义机械臂的可操作度：

其中，J为机械臂的雅克比矩阵，det为矩阵的求模函数；当可操作度M越大，表示机械臂目前的构型离奇异构型越远；

机器人的位姿由[x y θ]^T三个参数决定，其中，x和y表示机器人在平面地图上的位置，θ表示机器人在平面地图上的姿态；

设定遍历步长Δ，在机器人位姿范围内遍历其所有位姿，在每一个位姿工况下，求得关键特征点与机械臂基座的距离，以及所有关键特征点与基座距离的最大值，得到一系列不同位姿工况下的最大距离：{D_max1…D_maxn}；同时，在每一个位姿工况下求得机械臂达到关键特征点构型下的可操作度，以及每一个位姿工况下所有关键特征点下的最小可操作度：{M_min1…M_minn}；

设定可操作度的极限值M_limit和D_limit，当可操作度小于该极限值M_limit，或是关键特征点离基座的最大距离大于D_limit时，直接舍弃该可操作度对应的位姿，剩余的机器人位姿按照公式c＝αM_mini-βD_maxi计算，其中α+β＝1，表示可操作度和工作空间对于位姿优化的权重占比，且均为正数，当计算得到一系列的c值后，其最大值对应的机器人位姿为预设工作地点的优化位姿。

根据本发明提供的适用于复合型机器人的动态自适应定位系统，通过激光雷达和驱动电机，驱动机器人进行同步定位与建图、导航和运动规划；在机器人的机械臂本体上配置夹爪，进行物体抓取和转移；通过深度相机获取目标平面与立体的点云数据，并根据点云数据获取目标特征点；所述系统包括如下模块：

模块M1：驱动机器人进行同步定位与建图；

模块M2：驱动机器人移动至预设工作地点；

模块M3：识别预设工作地点的特征点；

模块M4：根据特征点和机械臂构型，优化机器人位姿；

模块M5：驱动机器人移动至优化位姿处。

优选的，所述模块M1包括：控制机器人在全地图中运动并遍历地图中的所有空间，根据激光雷达返回的数据建立机器人工作空间的平面地图，并在地图上的工作地点处进行标记。

优选的，所述模块M2包括：根据预设导航控制算法控制机器人运动至预设工作地点，在机器人到达预设工作地点后，将对应指令发送至机械臂控制设备；

所述预设导航控制算法包括A*算法、D*算法以及人工势场法。

优选的，所述模块M3包括：利用三维点云匹配技术获取工作台面的关键特征点，并将关键特征点发送至机械臂控制设备。

优选的，所述模块M4包括：

定义机械臂的可操作度：

其中，J为机械臂的雅克比矩阵，det为矩阵的求模函数；当可操作度M越大，表示机械臂目前的构型离奇异构型越远；

机器人的位姿由[x y θ]^T三个参数决定，其中，x和y表示机器人在平面地图上的位置，θ表示机器人在平面地图上的姿态；

设定遍历步长Δ，在机器人位姿范围内遍历其所有位姿，在每一个位姿工况下，求得关键特征点与机械臂基座的距离，以及所有关键特征点与基座距离的最大值，得到一系列不同位姿工况下的最大距离：{D_max1…D_maxn}；同时，在每一个位姿工况下求得机械臂达到关键特征点构型下的可操作度，以及每一个位姿工况下所有关键特征点下的最小可操作度：{M_min1…M_minn}；

设定可操作度的极限值M_limit和D_limit，当可操作度小于该极限值M_limit，或是关键特征点离基座的最大距离大于D_limit时，直接舍弃该可操作度对应的位姿，剩余的机器人位姿按照公式c＝αM_mini-βD_maxi计算，其中α+β＝1，表示可操作度和工作空间对于位姿优化的权重占比，且均为正数，当计算得到一系列的c值后，其最大值对应的机器人位姿为预设工作地点的优化位姿。

与现有技术相比，本发明具有如下的有益效果：

(1)本发明能够主动适应工作台面的变化，针对工作太慢的变化自主调整移动机器人位姿，降低了工作中断率，提高了工作效率；

(2)本发明避免了因工作台面环境的改变导致机械臂的工作空间范围无法覆盖目标，从而导致工作任务中断的问题，也避免了因工作任务包含奇异构型从而导致机械臂无法完成工作的问题；

(3)本发明提高了复合型机器人的环境适应性，能够针对不同的复杂环境调整移动机器人的位姿，便于机械臂完成工作任务。

附图说明

通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：

图1为复合型机器人组成图；

图2为复合型机器人SLAM建图；

图3为机械臂工作空间分布图；

图4为移动机器人位姿优化流程图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。

实施例：

本发明中复合型机器人由三大模块组成：移动机器人模块，机械臂模块和视觉模块。如图1所示，三大模块均是具有独立工作性能的模块，各模块中也分配了相对应的工装。

移动机器人模块：移动机器人模块中配备激光雷达，驱动电机等通用性设备，能够保证移动机器人单独完成常见的SLAM，导航，运动规划等功能。

机械臂模块：机械臂模块上除了机械臂本体外，还包含有夹爪，保证机械臂能够完成抓取、转移物体等操作。

视觉模块：视觉模块主要由深度相机构成，能够提供目标平面与立体的点云数据，并能够根据这些数据获取目标特征点。

根据本发明提供了一种适用于复合型机器人的动态自适应定位方法，包括如下步骤：

步骤1：移动机器人SLAM建图；

步骤2：移动机器人运动至工作地点附近；

步骤3：视觉模块识别关键特征点；

步骤4：根据机械臂构型优化移动机器人位姿；

步骤5：移动机器人运动至优化位姿处。

其中步骤1的详细内容为：手动控制复合型机器人在全地图中运动并遍历完全地图中的所有空间，根据激光雷达返回的数据便可以建立移动机器人工作空间的平面地图，并在地图上的工作地点附近进行简单标记，如图2所示。

其中步骤2的详细内容为：根据导航控制算法控制复合型机器人运动至工作点附近，行业类常用的A*算法，D*算法以及人工势场法等均可以用于控制本发明中涉及到的复合型机器人。当移动机器人运动至工作点附近后，发送相关指令至机械臂模块。

其中步骤3的详细内容为：复合型机器人运动至工作点后视觉模块便能够获取工作台面的图像信息，利用三维点云匹配技术获取工作台面的关键特征点，并将上述关键特征点发送至机械臂模块。

其中步骤4的详细内容为：决定机械臂能否完成工作任务的关键有两点，一是机械臂的任务空间被其工作空间全包围，二是机械臂的任务空间内不含有其奇异构型。

一般而言，机械臂的工作空间由机械臂各连杆长度和各关节运动范围决定，因此当一台机械臂被设计生产出来后，其工作空间便已确定。对于常见的空间六轴机械臂而言，其工作空间为球体，该球体半径由机械臂各连杆长度决定。机械臂工作空间又可分为灵巧工作空间和可达工作空间，灵巧工作空间意味着机械臂可以有多个姿态到达该位置，而可达工作空间意味着机械臂只能以一种姿态到达该位置，机械臂的工作空间分布如图3所示。

机械臂的奇异构型是指在该构型下，机械臂某个方向上的自由度缺失，例如，对于常见的空间六轴机械臂而言，当连杆2和连杆3共线时，机械臂自由度由6个缩减为5个。奇异构型对于机械臂而言是非常危险的，在执行工作任务时，通常都是在笛卡尔空间内对机械臂进行轨迹规划，当机械臂处于奇异构型附近时，笛卡尔空间微小的速度变化都会导致机械臂某些关节速度非常大，理论上，当机械臂处于奇异构型时，某个关节速度将会达到无限大，因此避开机械臂的奇异构型是非常必要的。

本发明定义机械臂的可操作度：

其中J表示为机械臂的雅克比矩阵，det表示为矩阵的求模函数。当可操作度M越大，表示机械臂目前的构型离奇异构型越远。

移动机器人的位姿由[x y θ]^T三个参数决定，其中x和y表示复合型机器人在图2中平面地图上的位置，θ表示复合型机器人在图2平面地图上的姿态。不同移动机器人位姿决定了机械臂的工作空间是否覆盖了工作台面上的关键特征点，同时也决定了机械臂的工作空间是否包含其奇异构型。

由于平面地图上的障碍限制，因此在工作点对移动机器人位姿进行微调时，x、y和θ均有最大值和最小值限制，因此可考虑设定遍历步长Δ，在移动机器人位姿范围内遍历其所有位姿，在每一个位姿工况下，可求得关键特征点与机械臂基座的距离，并且，在每一个位姿工况下都能求得所有关键特征点与基座距离的最大值，按此方案可求得一系列不同位姿工况下的最大距离：{D_max1…D_maxn}；同时，在每一个位姿工况下都能求得机械臂达到关键特征点构型下的可操作度，并且能够求得每一个位姿工况下所有关键特征点下的最小可操作度：{M_min1…M_minn}。

移动机器人位姿优化的选择准则为：上述求得的距离D_max越小越好，上述求得可操作性M_min越大越好。按照该准则，本发明的优化逻辑为：首先设定可操作度的极限值M_limit和D_limit，当上述求得的可操作度小于该极限值M_limit，或是关键特征点离基座的最大距离大于D_limit时直接舍弃该可操作度对应的位姿，剩余的移动机器人位姿按照公式c＝αM_mini-βD_maxi计算，其中α+β＝1，表示可操作度和工作空间对于位姿优化的权重占比，且均为正数，可根据实际需求自行确定。当计算得到一系列的c值后，其最大值对应的移动机器人位姿即是工作点附近的优化位姿。移动机器人位姿优化流程图如图4所示。

其中步骤5的详细内容为：机械臂模块将求解得到的优化位姿传递至移动机器人模块，移动机器人模块控制复合型机器人到达优化位姿处。

本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (10)

1.一种适用于复合型机器人的动态自适应定位方法，其特征在于，通过激光雷达和驱动电机，驱动机器人进行同步定位与建图、导航和运动规划；在机器人的机械臂本体上配置夹爪，进行物体抓取和转移；通过深度相机获取目标平面与立体的点云数据，并根据点云数据获取目标特征点；所述方法包括如下步骤：

步骤1：驱动机器人进行同步定位与建图；

步骤2：驱动机器人移动至预设工作地点；

步骤3：识别预设工作地点的特征点；

步骤4：根据特征点和机械臂构型，优化机器人位姿；

步骤5：驱动机器人移动至优化位姿处。

2.根据权利要求1所述的适用于复合型机器人的动态自适应定位方法，其特征在于，所述步骤1包括：控制机器人在全地图中运动并遍历地图中的所有空间，根据激光雷达返回的数据建立机器人工作空间的平面地图，并在地图上的工作地点处进行标记。

3.根据权利要求1所述的适用于复合型机器人的动态自适应定位方法，其特征在于，所述步骤2包括：根据预设导航控制算法控制机器人运动至预设工作地点，在机器人到达预设工作地点后，将对应指令发送至机械臂控制设备；

所述预设导航控制算法包括A*算法、D*算法以及人工势场法。

4.根据权利要求1所述的适用于复合型机器人的动态自适应定位方法，其特征在于，所述步骤3包括：利用三维点云匹配技术获取工作台面的关键特征点，并将关键特征点发送至机械臂控制设备。

5.根据权利要求1所述的适用于复合型机器人的动态自适应定位方法，其特征在于，所述步骤4包括：

定义机械臂的可操作度：

其中，J为机械臂的雅克比矩阵，det为矩阵的求模函数；当可操作度M越大，表示机械臂目前的构型离奇异构型越远；

机器人的位姿由[x y θ]^T三个参数决定，其中，x和y表示机器人在平面地图上的位置，θ表示机器人在平面地图上的姿态；

设定遍历步长Δ，在机器人位姿范围内遍历其所有位姿，在每一个位姿工况下，求得关键特征点与机械臂基座的距离，以及所有关键特征点与基座距离的最大值，得到一系列不同位姿工况下的最大距离：{D_max1 … D_maxn}；同时，在每一个位姿工况下求得机械臂达到关键特征点构型下的可操作度，以及每一个位姿工况下所有关键特征点下的最小可操作度：{M_min1 … M_minn}；

设定可操作度的极限值M_limit和D_limit，当可操作度小于该极限值M_limit，或是关键特征点离基座的最大距离大于D_limit时，直接舍弃该可操作度对应的位姿，剩余的机器人位姿按照公式c＝αM_mini-βD_maxi计算，其中α+β＝1，表示可操作度和工作空间对于位姿优化的权重占比，且均为正数，当计算得到一系列的c值后，其最大值对应的机器人位姿为预设工作地点的优化位姿。

6.一种适用于复合型机器人的动态自适应定位系统，其特征在于，通过激光雷达和驱动电机，驱动机器人进行同步定位与建图、导航和运动规划；在机器人的机械臂本体上配置夹爪，进行物体抓取和转移；通过深度相机获取目标平面与立体的点云数据，并根据点云数据获取目标特征点；所述系统包括如下模块：

模块M1：驱动机器人进行同步定位与建图；

模块M2：驱动机器人移动至预设工作地点；

模块M3：识别预设工作地点的特征点；

模块M4：根据特征点和机械臂构型，优化机器人位姿；

模块M5：驱动机器人移动至优化位姿处。

7.根据权利要求6所述的适用于复合型机器人的动态自适应定位系统，其特征在于，所述模块M1包括：控制机器人在全地图中运动并遍历地图中的所有空间，根据激光雷达返回的数据建立机器人工作空间的平面地图，并在地图上的工作地点处进行标记。

8.根据权利要求6所述的适用于复合型机器人的动态自适应定位系统，其特征在于，所述模块M2包括：根据预设导航控制算法控制机器人运动至预设工作地点，在机器人到达预设工作地点后，将对应指令发送至机械臂控制设备；

所述预设导航控制算法包括A*算法、D*算法以及人工势场法。

9.根据权利要求6所述的适用于复合型机器人的动态自适应定位系统，其特征在于，所述模块M3包括：利用三维点云匹配技术获取工作台面的关键特征点，并将关键特征点发送至机械臂控制设备。

10.根据权利要求6所述的适用于复合型机器人的动态自适应定位系统，其特征在于，所述模块M4包括：

定义机械臂的可操作度：

其中，J为机械臂的雅克比矩阵，det为矩阵的求模函数；当可操作度M越大，表示机械臂目前的构型离奇异构型越远；

机器人的位姿由[x y θ]^T三个参数决定，其中，x和y表示机器人在平面地图上的位置，θ表示机器人在平面地图上的姿态；

设定遍历步长Δ，在机器人位姿范围内遍历其所有位姿，在每一个位姿工况下，求得关键特征点与机械臂基座的距离，以及所有关键特征点与基座距离的最大值，得到一系列不同位姿工况下的最大距离：{D_max1 … D_maxn}；同时，在每一个位姿工况下求得机械臂达到关键特征点构型下的可操作度，以及每一个位姿工况下所有关键特征点下的最小可操作度：{M_min1 … M_minn}；

设定可操作度的极限值M_limit和D_limit，当可操作度小于该极限值M_limit，或是关键特征点离基座的最大距离大于D_limit时，直接舍弃该可操作度对应的位姿，剩余的机器人位姿按照公式c＝αM_mini-βD_maxi计算，其中α+β＝1，表示可操作度和工作空间对于位姿优化的权重占比，且均为正数，当计算得到一系列的c值后，其最大值对应的机器人位姿为预设工作地点的优化位姿。

Images