[发明专利]基于ROS机器人操作系统的机器人自动充电对接方法

权利要求书

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)充电：通过充电座上的充电检测模块和机器人本体上的电源管理模块同时检测机器人是否对接成功。