CN109755995B - 基于ros机器人操作系统的机器人自动充电对接方法 - Google Patents

Abstract

本发明公开了一种基于ROS机器人操作系统的机器人自动充电对接方法，该方法是使用激光雷达采集数据，对数据进行预处理，使用基点生长算法对数据进行聚合，以机器人充电座模型作为充电座识别特征，以六关键点算法计算充电座实际位置并计算运动指令，以ROS消息队列发布运动指令，进一步提高自动充电过程的成功率。本发明合理利用ROS机器人系统和ROS的导航框架进行实现，方便各模块之间的消息传递和功能实现，构建一个机器人自动充电的子系统，同时也使机器人在自动充电对接过程中具备一定的导航避障能力。

Description

基于ROS机器人操作系统的机器人自动充电对接方法

技术领域

本发明涉及移动机器人自动充电的技术领域，是指一种基于ROS机器人操作系统的机器人自动充电对接方法。

背景技术

近年来，移动机器人技术迅速发展，成果显著，越来越多的移动机器人代替人力在各个领域发挥了重要作用。例如家庭服务机器人，导购机器人，扫地机器人等。随着人工智能(AI)时代的到来，这些技术愈加的成熟，机器人技术已经国家高科技发展程度的一项重要标志。机器人技术的发展也带来了另一个问题，机器人自动充电技术成为了一个亟待解决的问题，几乎所有的移动机器人都需要实现自动充电功能才算是一个完整的机器人系统。因此，对机器人充电座识别及对接技术进行研究，具有非常重要的意义。

目前实现机器人充电座的识别及对接的方法有很多，其中应用最多的是基于红外传感器的机器人充电充电座自动识别及对接技术。通过程序内定义红外传感器组合信号的意义，然后进行多个红外传感器信号组合特征的识别，最后实现充电座的自动对接充电。但是基于红外传感器的自动充电过程，受红外传感器信号单一、系统结构复杂庞大，在一些实际的应用场合并不是特别合理。而另一种是基于机器视觉的充电座自动识别及对接方法，但是此种方法受光照、摄像机方向范围的影响，识别对接效果在一些场合并不是很理想。第三种是基于激光雷达的充电座自动识别及对接技术，机器人本体配备的激光雷达能够较精确的获取周围环境的二维距离信息(有部分激光雷达本身带有激光返回强度信息，使识别更加容易，但是本设计不依靠激光强度返回信息)，且受光照等环境条件影响较小，但由于激光雷达只能采集二维距离信息且没有强度返回信息，特征并不是特别明显，因而对算法的精度要求较高，所以需要对机器人充电座识别及对接算法进行优化。

综合以上论述，提出一种新的精度较高、适用环境范围广的移动机器人充电座识别及对接方法具有较高的实际应用价值。

发明内容

本发明的目的在于克服传统的基于红外传感器和基于激光雷达的机器人自动充电技术的不足，提出了一种基于ROS机器人操作系统的机器人自动充电对接方法，识别阶段采用基点生长的方法，充分利用激光雷达特征点的距离信息，并利用ROS机器人操作系统进行实现，合理利用了ROS的Navigation框架，使机器人在自动充电的最开始阶段中具备一定的路径规划和避障能力，导航到充电座附近位置。初期的返回导航过程利用ROS机器人操作系统提供的move_base导航框架。move_base是ROS机器人操作系统提供的一个功能包，根据给定的机器人的导航目标，激光雷达、环境地图的数据，通过设置导航参数，调用导航框架提供的全局路径规划和局部路径规划模块，即可实现机器人的自主导航。这里订阅initial_pose/goal(机器人出发位置)话题，获得到达初始充电座附近的导航目标点，默认机器人出发位置为充电座位置。设置导航参数选用A*最短路径算法规划出全局路径，使用dwa动态窗口算法规划局部路径，避开障碍物，实现机器人充电对接的初期返回过程的导航。

为实现自动充电目的，本发明提供的技术方案为：基于ROS机器人操作系统的机器人自动充电对接方法，包括以下步骤：

1)激光雷达数据整合：使用激光雷达采集数据，对数据进行预处理，剔除那些超出预设范围的数据和误差数据，并将结果记录保存；

2)特征提取：使用基点生长算法，对数据进行直线簇提取，去除噪声点并对直线簇进行处理；

3)充电座识别：对提取后的结果进行充电座特征识别，利用定义好的充电座特征模型识别充电座，然后计算现实中充电座在激光雷达坐标系/laser_scan下的位置，记为charge_scan，记录保存位置，并计算charge_scan位置和机器人的位置(机器人本体坐标系/base_link或激光雷达坐标系/laser_scan的原点)之间的距离，记为charge2robot_disten，若charge2robot_disten大于某一阈值，则重新进行识别，若满足阈值条件，则进行自动对接步骤；

4)自动对接：根据之前充电座识别后计算出的位置信息，通过自动对接算法计算第一个对接导航点，发送运动控制指令，到达第一个对接导航点后，重新进行充电座识别，更新充电座位置信息，再次通过自动对接算法计算第二个对接导航点，发送运动控制指令，再次更新充电座位置信息，满足位置要求，则进行最后对接，不满足则退回充电座1m外重新开始对接过程；

5)充电：通过充电座上的充电检测模块和机器人本体上的电源管理模块同时检测机器人是否对接成功。

在步骤1)中，使用激光雷达的数据，对数据进行预处理，将激光雷达有效数据限制在max_disten范围内：

0≤use_scan_data≤max_disten

其中，max_disten代表激光雷达数据有效的最大距离，use_scan_data代表有效数据。

在步骤2)中，使用基点生长算法进行直线簇提取，步骤如下：

2.1)根据设定的种子数seed_point，从激光雷达扫描到的初始点开始算，数出一组种子点簇，利用最小二乘法拟合出一条直线，得出直线参数：k(斜率)，b(截距)，进而计算出直线方程：y＝kx+b；

2.2)使用刚得出的直线方程，计算种子点簇最后一个点和种子点簇之后下一个位置点之间的距离point_disten，种子点簇之后下一个位置点和直线之间的垂直距离error_disten，若error_disten和point_disten均小于阈值point_limit和line_limit，则认为种子点簇之后的这个点属于此直线，否则认为此点不属于此直线；

将上一段直线之后的第一个点作为下一组种子点的起点，重新进行直线拟合和下一轮迭代，直至扫描点到达最后一个点；

2.3)重新计算所有相邻直线簇之间的距离，合并相邻距离小于一定数值的直线簇；

根据每个直线簇的点的个数进行筛选，保留直线簇中点的个数在[num_min,num_max]之间的直线簇，其中num_min、num_max分别代表最小和最大数据点个数，合并相邻距离小于cluster_disten_limit的直线簇，cluster_disten_limit为距离限制阈值；

最后提取直线簇的起点和终点，分别记为cluster_pointstart和cluster_pointend，提取的所有端点按照激光雷达数据点的扫描顺序排列成数组，保存在数组cluster[i]中，i＝0,1,......,n，数组中每两个点表示一段直线。

在步骤3)中，以六关键点算法识别充电座模型，以定义好的充电座模型形状信息为模型进行识别：

3.1)对于充电座模型的形状定义，定义了六个定位点即三段相邻直线之间的约束条件，根据上面计算出来的保存有直线起点和终点的数组cluster[i]，迭代计算三段直线的六个点之间的直线距离为：

其中，cluster[i]代表cluster数组中的第i个点，其中i＝0,2,4,......,n-5；L_0,1，L_2,3，L_4,5表示三段相邻直线的长度；L_1,2，L_3,4表示相邻直线段之间的垂直距离；

若同时满足条件0.07≤L_0,1≤0.09和0.04≤L_2,3≤0.08和0.07≤L_4,5≤0.09和以下两个条件中的一个0.07≤L_1,2≤0.012和0.07≤L_2,3≤0.12，则认为线段L_2,3的中点处即为充电座的位置charge_scan，若不满足以上条件则迭代计算数组cluster[i]中后续直线段，直到最后一个线段，若在整个数组cluster[i]中都未找到充电座特征，则等待下一组cluster[i]；

3.2)计算charge_scan位置和机器人的位置(机器人本体坐标系/base_link或激光雷达坐标系/laser_scan的原点)之间的距离，记为charge2robot_disten，若charge2robot_disten大于某一阈值，则重新进行识别，若满足阈值条件，则进行自动对接步骤。

在步骤4)中，根据步骤3)计算出的充电座位置信息和六个定位点的位置信息，使用六关键点算法，进行机器人与充电座的对接过程，步骤为：

4.1)首先计算三段直线的中点，L_0,1线段中点记为P₁,L_2,3线段中点记为P₂，L_4,5线段中点记为P₃，由点P₁和点P₃组成的线段中点记为P₄，在/laser_scan坐标系下计算向量R_1,3和向量R_2,4，然后计算两个向量之间的夹角记为orthogo_angle，如果80°≤orthogo_angle≤90°则认为两向量正交，符合要求，否则等待6个定位点的下一次更新，方向向量R_1,3和R_2,4为：

R_1,3＝P₃-P₁

R_2,4＝P₄-P₂

其中P₁，P₂，P₃，P₄为激光雷达坐标系下充电座特征点；

4.2)以点P₄为圆心，以radius(人为给定)为半径，做出一个圆，将圆的轨迹离散化成360个点，计算圆上点到机器人现在位置的距离，并找到离机器人最近的位置点作为P₅，其中圆的离散化计算：

X＝radius*cos(i)+P₄·x Y＝radius*sin(i)+P₄·y i＝1,2,3......360

其中radius为可人为给定半径，否则为默认值radius＝0.6m，min_disten_point为圆上点到机器人位置之间的直线距离；

每次计算一个点都会对比min_disten_point是否小于历史最小距离，更新最小距离点的位置，最终位置记为P₅；

4.3)因为机器人本体在激光雷达坐标系/laser_scan下即为原点，由于在机器人本体底盘坐标系/base_link下机器人本体正前方方向为向量(1,0,0)，故可通过/tf坐标系转换关系将机器人本体正前方方向转换到激光雷达坐标系/laser_scan下，记为front_vector；

在激光雷达坐标系下计算点P₅与坐标系/laser_scan原点(机器人本体)之间形成的向量和距离，记为move_vector和move_disten，然后计算向量move_vector和向量front_vector之间的夹角，记为move_agl。

move_vector＝-P₅

其中m_a代表move_agl，单位为度

m_v代表move_vector

f_v代表front_vector

m_v.x,m_v.y,f_v.x,f_v.y分别代表方向向量在三维坐标系下的三个正交轴的分量值；

4.4)根据定位点P₁和P₃的位置判断机器人本体位置，在激光雷达坐标系下计算定位点P₁和P₃分别和激光雷达坐标系/laser_scan原点之间的距离，可以确定机器人本体靠近距离短的点的一侧，机器人向点距离短的一侧转动move_agl的度数，然后像后方后退move_disten的距离，到达第一个对接导航点；

4.5)返回步骤3.1)，重新进行充电座的识别及距离计算，将寻找点P₅的半径缩小，即缩小radius的值，更新所有对接导航数据，判断向量R_1,3和R_2,4之间的夹角orthogo_angle是否符合正交条件即80°＜orthogo_angle＜100°，且夹角move_agl满足条件0°≤move_agl≤15°，否则退回到充电座1m之外重新进行对接过程；

如果满足以上条件，则进行第二步对接，重复上一次对接过程，重新计算转动角度move_agl和后退距离move_disten，向机器人本体所带的控制器发送运动指令；

4.6)经过两次导航运动，到达第二个对接导航点，此时机器人本体后radius距离处为充电座所在位置，最后一步对接可以控制机器人本体向后方直线后退radius的距离即可触发充电动作。

在步骤5)中，机器人的运动控制指令是利用ROS机器人操作系统提供的move_base导航框架，实现机器人的控制功能，根据输入方向速度和运行时间来对机器人运动进行控制，即通过向话题cmd_vel/Twist上发布速度、方向和发布时长来控制机器人运动。

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

1、使用激光雷达采集数据，相对于视觉的充电座识别系统数据处理量较小，实时性更高。

2、使用定义好的充电座模型，充电座识别率较高，有效的解决了单个激光雷达特征较少，充电座识别精度低的缺点。

3、提出了实时性较高的充电座识别算法，在充电座的自动识别和对接过程中，分三步子过程，子过程中能够根据识别的反馈信息，及时调整对接策略，提高了对接的成功率。

4、以ROS机器人操作系统为基础进行实现，合理利用了ROS机器人操作系统话题消息机制，最终完成了各部分之间的数据传递和功能实现。

附图说明

图1为本发明的系统框图及自动对接流程图。

其中，cmd_vel/Twist话题消息代表运动控制参数，包括运动速度和角速度，发布时间由cmd_vel/Twist话题消息的持续时间决定。/tf坐标系框架代表机器人本体及地图中的坐标系关系，通过/tf坐标系框架可以完成各个坐标系之间的实时坐标转换。/laser_scan代表激光雷达坐标系，scan_data为激光雷达采集到的障碍物信息。move_base/goal话题消息代表路径规划的终点，即在机器人本体坐标系下的目标点位置信息。数据降噪指的是对激光雷达采集的信息进行错误数据剔除操作，以减小识别误差。

图2为本发明充电座模型和机器人本体示意图。

图3为本发明的自动充电坐标系示意图。

其中，/base_link坐标系是机器人本体坐标系，/laser_scan坐标系为激光雷达坐标系，/map坐标系是全局地图坐标系。

图4为本发明的自动充电对接过程示意图。

图5为本发明的自动充电对接过程流程图。

图6为本发明的/tf坐标系框架(坐标系关系图)，即保存有各个坐标系之间实时转换关系的坐标系结构图。

其中，/map为全局地图坐标系，/odom里程计坐标系，/base_link为机器人本体坐标系(x轴朝前，y轴朝左，z轴朝上)，/imu_link惯性传感器坐标系，/laser_scan为激光雷达坐标系。

具体实施方式

下面结合具体实施例对本发明做进一步的说明。

如图1所示，为本实施例所提供的基于ROS机器人操作系统的机器人自动充电对接方法用到的系统框架，包括Robot、激光雷达数据、数据预处理、自动充电对接以及路径规划五部分。下面分别介绍每部分的具体实现方式：

(1)Robot部分：本示例选用的是EAI科技的Dashgo移动机器人平台，控制器选用的是一台装有Ubuntu14.04的笔记本，在Linux中装有ROS indigo机器人操作系统。

(2)激光数据部分：整个系统的数据由激光雷达采集，数据流向有自动充电对接模块，以及路径规划模块。选用的是EAI科技的Flash Lidar。扫描角度范围为360度，扫描频率为10Hz，角度分辨率β＝0.6°，激光雷达安装机器人正上方0.3m处，测量有效距离5m。激光雷达的数据通过laser_scan_topic发布，消息类型为scan_data。

(3)数据预处理部分：通过订阅laser_scan_topic，从激光雷达获得扫描数据，对数据进行预处理，对数据进行阈值控制，只对[0,max_disten]范围内的数据进行充电座检测，0、max_disten分别代表最小和最大距离的有效激光雷达数据。

(4)自动充电对接部分包括以下三部分：

a、基点生长：使用基点生长算法对数据进行聚类。

b、充电座识别：以之前定义好的充电座模型作为充电座识别特征，计算出充电座在现实环境中的位置，并记录。

c、运动指令：根据充电座的识别数据和位置信息，计算机器人的对接运动指令，消息类型为cmd_vel/Twist，并发送给路径规划。

(5)路径规划部分：这里使用的是ROS机器人操作系统的move_base导航框架，主要包括全局路径规划和局部路径规划。订阅move_base/goal话题，获得下一时刻的目标，使用A*算法规划出全局地图中的路径，订阅laser_Scan话题，使用dwa算法规划局部路径，合理控制机器人速度和加速度，并实时避开障碍物。

本实施例所提供的基于ROS机器人操作系统的机器人自动充电对接方法，自动充电对接过程流程图如图5，具体包括以下步骤：

1)激光雷达数据提取，对数据进行预处理，将激光雷达有效数据限制在max_disten范围内：

0≤use_scan_data≤max_disten

其中，max_disten＝3m代表激光雷达数据有效的最大距离，use_scan_data代表有效数据；

2)特征提取：使用基点生长算法，对数据进行直线簇提取，去除噪声点并对直线簇进行处理，步骤如下：

2.1)根据设定的种子数seed_point＝5，从激光雷达扫描到的初始点开始算，数出一组种子点簇，作为基点簇，利用最小二乘法拟合出一条直线，得出直线参数：k(斜率)，b(截距)，进而计算出直线方程：y＝kx+b。

2.2)使用刚得出的直线方程，计算种子点簇最后一个点和种子点簇之后下一个位置点之间的距离point_disten，种子点簇之后下一个位置点和直线之间的垂直距离error_disten，若error_disten和point_disten均小于阈值point_limit＝0.02m和line_limit＝0.04m，则认为种子点簇之后的这个点属于此直线，否则认为此点不属于此直线。

将上一段直线之后的第一个点作为下一组种子点的起点，重新进行直线拟合和下一轮迭代，直至扫描点到达最后一个点。

2.3)重新计算所有相邻直线簇之间的距离，删除不符合要求的直线簇，合并相邻距离小于一定阈值的直线簇；

根据每个直线簇的点的个数进行筛选，保留直线簇中点的个数在[num_min,num_max]之间的直线簇，其中num_min＝5、num_max＝80分别代表最小和最大数据点个数，合并相邻距离小于cluster_disten_limit＝0.02m的直线簇，cluster_disten_limit为距离限制阈值。

最后提取直线簇的起点和终点，分别记为cluster_pointstart和cluster_pointend，提取的所有端点按照激光雷达数据点的扫描顺序排列成数组，保存在数组cluster[i]中，i＝0,1,......,n，，数组中每两个点表示一段直线。

3)充电座识别：对提取后的结果进行充电座特征识别，利用定义好的充电座特征模型(如图2所示)识别充电座，然后计算现实中充电座在激光雷达坐标系(如图3所示)/laser_scan下的位置，记为charge_scan，记录保存位置，并计算charge_scan位置和机器人的位置(机器人本体坐标系/base_link或激光雷达坐标系/laser_scan的原点)之间的距离，记为charge2robot_disten，若charge2robot_disten大于某一阈值，则重新进行识别，若满足阈值条件，则进行自动对接步骤。

其中，以形状识别算法识别充电座模型，以定义好的充电座模型形状信息为模型进行识别：

3.1)对于充电座模型的形状(如图2所示)，定义了六个定位点即三段相邻直线之间的约束条件，根据上面计算出来的保存有直线起点和终点的数组cluster[i]，迭代计算三段直线的六个点之间的直线距离为：

其中，cluster[i]代表起点和终点数组中的第i个点，其中i＝0,2,4,......,n-5。L_0,1，L_2,3，L_4,5表示三段相邻直线的长度，L_1,2，L_3,4表示相邻直线段之间的垂直距离。

若同时满足条件0.07≤L_0,1≤0.09和0.04≤L_2,3≤0.08和0.07≤L_4,5≤0.09和以下两个条件中的一个0.07≤L_1,2≤0.012和0.07≤L_2,3≤0.12，则认为线段L_2,3的中点处即为充电座的位置charge_scan，若不满足以上条件则迭代计算数组cluster[i]中后续直线段，直到最后一个线段。若在整个数组cluster[i]中都未找到充电座特征，则等待下一组cluster[i]。

3.2)计算charge_scan位置和机器人的位置(机器人本体坐标系/base_link或激光雷达坐标系/laser_scan的原点)之间的距离，记为charge2robot_disten，若charge2robot_disten>1.2m，则重新进行识别，若满足阈值条件，则进行自动对接步骤。

4)自动对接：根据之前充电座识别后计算出的位置信息，通过自动对接算法计算第一个对接导航点，发送运动控制指令，到达第一个对接导航点后，重新进行充电座识别，更新充电座位置信息，再次通过自动对接算法计算第二个对接导航点，发送运动控制指令，再次更新充电座位置信息，满足位置要求，则进行最后对接，不满足则退回充电座1m外重新开始对接过程。

在步骤4)中，根据步骤3)中计算出的充电座位置信息和六个定位点的位置信息，进行机器人与充电座的对接过程(如图4所示)，步骤为：

4.1)首先计算三段直线的中点，L_0,1线段中点记为P₁,L_2,3线段中点记为P₂，L_4,5线段中点记为P₃，由点P₁和点P₃组成的线段中点记为P₄，在/laser_scan坐标系下计算向量R_1,3和向量R_2,4，然后计算两个向量之间的夹角记为orthogo_angle，如果80°≤orthogo_angle≤90°则认为两向量正交，符合要求，否则等待6个定位点的下一次更新。方向向量R_1,3和R_2,4为：

R_1,3＝P₃-P₁

R_2,4＝P₄-P₂

其中P₁，P₂，P₃，P₄为激光雷达坐标系下充电座特征点；

4.2)以点P₄为圆心，以radius＝0.6m为半径，做出一个圆，将圆的轨迹离散化成360个点，计算圆上点到机器人现在位置的距离，并找到离机器人最近的位置点作为P₅。其中圆的离散化计算：

X＝radius*cos(i)+P₄·x Y＝radius*sin(i)+P₄·y i＝1,2,3......360

其中radius为可人为给定半径，否则为默认值radius＝0.6m，min_disten_point为圆上点到机器人位置之间的直线距离。

每次计算一个点都会对比min_disten_point是否小于历史最小距离，更新最小距离点的位置，最终位置记为P₅。

4.3)因为机器人本体在/laser_scan坐标系下即为原点，由于在/base_link坐标系下机器人本体正前方为向量(1,0,0)的方向，故通过/tf坐标系框架(如图6)的坐标系转换关系将机器人本体正前方方向转换到坐标系/laser_scan下，记为front_vector。

在激光雷达坐标系下计算点P₅与坐标系/laser_scan原点(机器人本体)之间形成的向量和距离，记为move_vector和move_disten，然后计算向量move_vector和向量front_vector之间的夹角，记为move_agl。

move_vector＝-P₅

其中m_a代表move_agl，单位为度；

m_v代表move_vector；

f_v代表front_vector；

m_v.x,m_v.y,f_v.x,f_v.y分别代表方向向量在三维坐标系下的三个正交轴的分量值。

4.4)根据定位点P₁和P₃的位置判断机器人本体位置，在激光雷达坐标系下计算定位点P₁和P₃分别和激光雷达坐标系/laser_scan原点之间的距离，可以确定机器人本体靠近距离短的点的一侧，机器人向点距短的一侧转动move_agl的度数，然后像后方后退move_disten的距离，到达第一个对接导航点。

4.5)返回步骤3.1)，重新进行充电座的识别及距离计算，将寻找点P₅的半径更改为radius＝0.3m，更新所有对接导航数据，判断向量R_1,3和R_2,4之间的夹角orthogo_angle是否符合正交条件即80°＜orthogo_angle＜100°，且夹角move_agl满足条件0°≤move_agl≤15°，否则退回到充电座1m之外重新进行对接过程。

如果满足以上条件，则进行第二步对接，计算转动角度move_agl和后退距离move_disten，向机器人本体所带的控制器发送运动指令。

4.6)经过两次导航运动，到达第二个对接导航点，此时机器人本体后方radius＝0.3m处为充电座所在位置，最后一步对接可以控制机器人本体向后方直线后退radius＝0.3m即可触发充电动作。

5)导航：根据充电座目标确定导航目标，使用A*算法规划出全局地图中的路径，并使用dwa动态窗口算法规划局部路径，控制机器人朝导航目标运动，并实时避开障碍物。

具体是：利用ROS提供的move_base导航框架，订阅move_base/goal话题，获得导航目标，通过订阅laser_scan_topic话题获得激光雷达信息消息，使用A*算法规划全局地图中的路径，使用dwa动态窗口算法规划局部路径，避开障碍物，实现了机器人的导航。

以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所作的变化，均应涵盖在本发明的保护范围内。

Claims (4)

1.基于ROS机器人操作系统的机器人自动充电对接方法，其特征在于，包括以下步骤：

1)激光雷达数据预处理：使用激光雷达采集数据，对数据进行预处理，剔除那些超出预设范围的数据和误差数据，并将结果记录保存；

2)特征提取：使用基点生长算法，对数据进行直线簇提取，去除噪声点并对直线簇进行处理；

3)充电座识别：对提取后的结果进行充电座特征识别，利用定义好的充电座特征模型识别充电座，然后计算充电座在激光雷达坐标系/laser_scan下的位置，记为charge_scan，记录保存位置，并计算charge_scan位置和机器人的位置之间的距离，记为charge2robot_disten，若charge2robot_disten大于某一阈值，则重新进行识别，若满足阈值条件，则进行自动对接步骤；其中，机器人的位置为机器人本体坐标系/base_link或激光雷达坐标系/laser_scan的原点；

以形状识别算法识别充电座模型，以定义好的充电座模型形状信息为模型进行识别，步骤为：

3.1)对于充电座模型的形状，定义了六个定位点即三段相邻直线之间的约束条件，根据上面计算出来的保存有直线起点和终点的数组cluster[i]，迭代计算三段直线的六个点之间的直线距离为：

其中，cluster[i]代表cluster数组中的第i个点，其中i＝0,2,4,......,n-5；L_0,1，L_2,3，L_4,5表示三段相邻直线的长度；L_1,2，L_3,4表示相邻直线段之间的垂直距离；

若同时满足条件0.07≤L_0,1≤0.09和0.04≤L_2,3≤0.08和0.07≤L_4,5≤0.09和以下两个条件中的一个0.07≤L_1,2≤0.012和0.07≤L_2,3≤0.12，则认为线段L_2,3的中点处即为充电座的位置charge_scan，若不满足以上条件则迭代计算数组cluster[i]中后续直线段，直到最后一个线段，若在整个数组cluster[i]中都未找到充电座特征，则等待下一组cluster[i]；

3.2)计算charge_scan位置和机器人的位置之间的距离，记为charge2robot_disten，若charge2robot_disten大于某一阈值，则重新进行识别，若满足阈值条件，则进行自动对接步骤；

4)自动对接：根据之前充电座识别后计算出的位置信息，通过自动对接算法计算第一个对接导航点，发送运动控制指令，到达第一个对接导航点后，重新进行充电座识别，更新充电座位置信息，再次通过自动对接算法计算第二个对接导航点，发送运动控制指令，再次更新充电座位置信息，满足位置要求，则进行最后对接，不满足则退回充电座1m外重新开始对接过程；

计算出的充电座位置信息和六个定位点的位置信息，进行机器人与充电座的对接过程，步骤为：

4.1)首先计算三段直线的中点，L_0,1线段中点记为P₁,L_2,3线段中点记为P₂，L_4,5线段中点记为P₃，由点P₁和点P₃组成的线段中点记为P₄，在/laser_scan激光雷达坐标系下计算向量R_1,3和向量R_2,4，然后计算两个向量之间的夹角记为orthogo_angle，如果80°≤orthogo_angle≤90°则认为两向量正交，符合要求，否则等待6个定位点的下一次更新；方向向量R_1,3和R_2,4为：

R_1,3＝P₃-P₁

R_2,4＝P₄-P₂

其中P₁，P₂，P₃，P₄为激光雷达坐标系下充电座特征点；

4.2)以点P₄为圆心，以radius为半径，做出一个圆，将圆的轨迹离散化成360个点，计算圆上点到机器人现在位置的距离，并找到离机器人最近的位置点作为P₅，其中圆的离散化计算：

X＝radius*cos(i)+P_4·x Y＝radius*sin(i)+P_4·y i＝1,2,3......360

其中radius为人为给定半径，否则为默认值radius＝0.6m，min_disten_point为圆上点到机器人位置之间的直线距离；

每次计算一个点都会对比min_disten_point是否小于历史最小距离，更新最小距离点的位置，最终位置记为P₅；

4.3)因为机器人本体在/laser_scan坐标系下即为原点，由于在/base_link坐标系下机器人本体正前方为向量(1,0,0)的方向，故通过/tf坐标系框架关系将机器人本体正前方方向向量转换到激光雷达坐标系/laser_scan下，记为front_vector；

在激光雷达坐标系下计算点P₅与坐标系/laser_scan原点即机器人本体之间形成的向量和距离，记为move_vector和move_disten，然后计算向量move_vector和向量front_vector之间的夹角，记为move_agl；

move_vector＝-P₅

其中m_a代表move_agl，单位为度

m_v代表move_vector

f_v代表front_vector

m_v.x,m_v.y,f_v.x,f_v.y分别代表方向向量在三维坐标系下的三个正交轴的分量值；

4.4)根据定位点P₁和P₃的位置判断机器人本体位置，在激光雷达坐标系下计算定位点P₁和P₃分别和坐标系/laser_scan原点之间的距离，确定机器人本体靠近距离短的点的一侧，机器人向点距短的一侧转动move_agl的度数，然后像后方后退move_disten的距离，到达第一个对接导航点；

4.5)返回步骤3.1)，重新进行充电座的识别及距离计算，将寻找点P₅的半径缩小，即缩小radius的值，更新所有对接导航数据，判断向量R_1,3和R_2,4之间的夹角orthogo_angle是否符合正交条件即80°＜orthogo_angle＜100°，且夹角move_agl满足条件0°≤move_agl≤15°，否则退回到充电座1m之外重新进行对接过程；

如果满足以上条件，则进行第二步对接，重复上一次对接过程，重新计算转动角度move_agl和后退距离move_disten，向机器人本体所带的控制器发送运动指令；

4.6)经过两次导航运动，到达第二个对接导航点，此时机器人本体后方radius距离处为充电座所在位置，最后一步对接能够控制机器人本体向后方直线后退radius的距离即可触发充电动作；

5)充电：通过充电座上的充电检测模块和机器人本体上的电源管理模块同时检测机器人是否对接成功。

2.根据权利要求1所述的基于ROS机器人操作系统的机器人自动充电对接方法，其特征在于，在步骤1)中，利用激光雷达的数据，使用基点生长算法对数据进行预处理，将激光雷达有效数据限制在max_disten范围内：

0≤use_scan_data≤max_disten

其中，max_disten代表激光雷达数据有效的最大距离，use_scan_data代表有效数据。

3.根据权利要求1所述的基于ROS机器人操作系统的机器人自动充电对接方法，其特征在于，在步骤2)中，使用基点生长算法进行直线簇提取，步骤如下：

2.1)根据设定的种子数seed_point，从激光雷达扫描到的初始点开始算，数出一组种子点簇，作为基点簇，利用最小二乘法拟合出一条直线，得出直线参数：斜率k和截距b，进而计算出直线方程：y＝kx+b；

2.2)使用刚得出的直线方程，计算种子点簇最后一个点和种子点簇之后下一个位置点之间的距离point_disten，种子点簇之后下个位置点和直线之间的垂直距离error_disten，若error_disten和point_disten均小于阈值point_limit和line_limit，则认为种子点簇之后的这个点属于此直线，否则认为此点不属于此直线；

将上一段直线之后的第一个点作为下一组种子点的起点，重新进行直线拟合和及下一轮迭代，直至扫描点到达最后一个点；

2.3)重新计算所有相邻直线簇之间的距离，删除不符合要求的直线簇，合并相邻距离小于预定阈值的直线簇；

根据每个直线簇的点的个数进行筛选，保留直线簇中点的个数在[num_min,num_max]之间的直线簇，其中num_min、num_max分别代表最小和最大数据点个数，合并相邻距离小于cluster_disten_limit的直线簇，cluster_disten_limit为距离限制阈值；

最后提取直线簇的起点和终点，分别记为cluster_pointstart和cluster_pointend，提取的所有端点按照激光雷达数据点的扫描顺序排列成数组保存在数组cluster[i]中，i＝0,1,......,n，数组中每两个点表示一段直线。

4.根据权利要求1所述的基于ROS机器人操作系统的机器人自动充电对接方法，其特征在于：在步骤5)中，机器人的运动控制指令是利用ROS机器人操作系统提供的move_base导航框架，实现机器人的控制功能，根据输入方向速度和运行时间来对机器人运动进行控制，即通过向话题cmd_vel/Twist上发布速度、方向和发布时长来控制机器人运动。

Images