[发明专利]基于激光测量的移动机器人自动对接充电方法

权利要求书

1.基于激光测量的移动机器人自动对接充电方法，其特征在于包括以下步骤：

步骤1：激光雷达数据信息处理

1)激光数据坐标转换：移动机器人与外界环境信息之间的联系是基于直角坐标系的，应该首先对原始数据进行数据项处理，把极坐标转换为直角坐标，然后再进行数据处理运算；采集的数据点个数为多个，计算出第i个数据点对应的极坐标系下的角度θ_i：

然后通过以下公式将极坐标(ρ_i，θ_i)转化为直角坐标(X_i，Y_i)；

X_i＝ρ_i×cosθ_i

Y_i＝ρ_i×sinθ_i

2)断点检测：断点检测是通过阈值比较法在多个原始数据点中检测出不连续的数据点，并以此为断点将所有数据点分为若干点集，这些点集相互之间不具有关联性，独自作为一个区域看待，其中检测出的断点即为这些点集的起始点和终结点；

具体步骤如下：

2.1)首先将多个原始数据点视为一个点集，第一个数据点为点集的起始点，最后一个数据点为点集终结点，即得到初始点集p₁((X₁，Y₁)，(X₂，Y₂)...)；

2.2)然后从点集的第一个数据点开始，按照如下公式依次计算连续两点间的距离D：

再比较距离D与阈值的关系，如果那么判定这个数据点为断点，此断点将整个点集分为两部分，得到点集p₁((X₁，Y₁)...(X_i，Y_i))和点集p₂((X_i+1，Y_i+1)...)；是阈值，选为200mm；

2.3)最后再按照同样的方法，对点集p₂进行断点的分解，得到点集p₂和p₃；以此类推，把数据点中的所有断点检测出来，并得到互不关联的点集(p₁，p₂...p_n)；

3)拐点检测：拐点检测通过检测每个点集中拐角点的数目，确定此点集内所能用线段拟合的最少点集数，即拐点检测就是对断点检测所得点集的进一步细分，使所得到的每个点集都用一条线段进行拟合表示；

步骤如下：

3.1)首先取出断点检测所得到的一个点集p_i，以其首末点p_is，p_ie为端点做一条直线 ，从这个点集的开始，依次计算每个点到这条直线的距离d_i，同时比较这些距离值，记下距离最大的点p_im，若这个最大距离值d_max大于阈值d_thr，那么认为p_im点为点集p_i的一个拐点，将此点集再分为两部分L_i1和L_i2，需要用两端不同特征参数的线段去拟合；若最大距离值d_max仍小于阈值d_thr，则认为这个点集用一条线段表示，不需要再细分；

d_thr是阈值，d_thr取拟合线段长度的10％；

3.2)接着按照以上方法，再对点集L_i1和L_i2进行拐点检测，最多得到四个点集L_i1、L_i2、L_i3和L_i4；依次类推，直至每个点集的最大距离d_max均小于阈值d_thr，即每个点集均用一条线段去拟合，则完成点集的拐点检测，此时点集p_i细分成若干个用线段拟合的点集(L_i1，L_i2...L_in)；

3.3)最后重复以上步骤，即对断点检测后分割的每一个点集再进行细分，最终得到一系列用一条线段完整拟合的点集(L₁，L₂...L_n)；

4)线段拟合：经过以上断点检测和拐点检测的步骤，得到一组点集(L₁，L₂...L_n)，其中每个点集内的激光数据点均用一条线段进行线性拟合，从而提取出环境地图所需要的几何特征；采用最小二乘法对激光数据点进行线性拟合，激光传感器中的数据排列方式是从右向左，因此将拟合的线段从机器人最右边按顺序依次存储在容器line_segments[i]中，其中i＝0，1，2...n；

步骤2：充电桩特征识别

在机器人坐标系下，d、d₁和d₂是机器人距离每条拟合线段的距离，θ、θ₁和θ₂是机器人正方向与每条拟合线段的法向量之间的夹角，l、l₁和l₂是每条拟合线段的长度；从i＝0开始每次从存储容器line_segments[i]中按顺序取三条线段line_segments[i]，line_segments[i+1]，line_segments[i+2]，具体步骤如下：

1)首先，根据下面的公式判断三条拟合线段是否互相平行且处于同一平面：

|θ₁-θ|≤ε

|θ₂-θ|≤ε

其中θ为机器人正方向与充电桩所在拟合线段(line_segments[i+1])的法向量之间的夹角，θ₁和θ₂分别为机器人正方向与充电桩左边墙所在拟合线段(line_segments[i+2])和充电桩右边墙所在拟合线段(line_segments[i])的法向量之间的夹角，ε为最小误差，不超过3°；

2)然后，由下面公式判断三条线段是否同处在充电桩所在平面：

|d₁-d-th|≤δ

|d₂-d-th|≤δ

其中d为移动机器人距离充电桩所在拟合线段的距离，d₁和d₂分别为移动机器人距离充电桩左边墙所在拟合线段(line_segments[i+2])和右边墙所在拟合线段(line_segments[i])的距离，th为充电桩厚度，且th＝0.2m，δ为最小误差，且δ＝0.03m；

3)最后，若三条线段的中间线line_segments[i+1]的长度l满足下式，确定line_segments[i+1]为充电桩所在的拟线段：

|l-l_actual|≤μ

其中l_actual为充电桩的实际长度，且l_actual＝0.43m，μ为最小误差，且μ＝0.02m；

4)如果所取的三条拟合线段不符合充电桩识别条件，则i＝i+1依次取下三条拟合线段重复步骤1)～3)，直到最终找到充电桩所在拟合线段；

移动机器人识别到充电桩后，根据拟合线段的特征参数确定充电桩的中点坐标p_c(x_c，y_c)：

其中p_s(x_s，y_s)和p_e(x_e，y_e)分别为此拟合线段的起始坐标和终点坐标；当获取到充电桩中点坐标后，利用ROS将中点坐标发布给多模态分阶段镇定控制器，最终由控制器驱动机器人进行下一步的对接；

步骤3：移动机器人对接充电桩的运动控制

在移动机器人与充电桩对接过程中设计了多模态分阶段镇定控制器，根据机器人在对接过程中与充电桩的角度偏差和距离偏差来划分不同的特征模态，然后对其进行相应的控制参数的设计，使得机器人根据不同特征模态选择相应的控制策略；

根据两轮差分运动学常识，将移动机器人从初始位置运动到期望位置的过程划分为姿态调整控制和位置跟踪控制；步骤如下：

1)t₀时刻为机器人初始状态，从t₀时刻至t₁时刻为纯旋转控制，使得机器人正方向垂直于充电桩，这样通过激光传感器实时获取到移动机器人与充电桩正前方的距离差；

2)t₁时刻移动机器人逆时针旋转90°至t₂时刻，并直线行驶使得机器人与充电桩正前方的距离差趋于0；

3)t₃时刻移动机器人顺时针旋转90°使得机器人正方向垂直于充电桩；由于充电触片位于充电桩背部，因此在充电桩正前方开启直线对接过程中需考虑尾部进行对接，即t₃时刻机器人向前直线对接，在距离充电桩40cm处逆时针旋转180°至t₄时刻；

4)t₄时刻机器人开始后退接触充电桩直至与充电桩完全接触开启充电；

若机器人在对接过程中对接失败，则移动机器人重新回到对接起点进行充电桩识别，重复1)～4)开启新一轮的对接，直至对接成功；

调整移动机器人的姿态误差角d_θ时，是根据机器人在全局世界坐标系下当前的姿态角θ_c与下一位置的期望姿态角θ_e得到姿态误差d_θ＝θ_e-θ_c，然后通过分段镇定控制器中的旋转控制器如下公式使得d_θ趋近于0：

其中v表示机器人的线速度，k_ω＞0，为了提高对接效率和准确率，在不同模态k_ω的值不同，在t₀时刻k_ω＝1.0；在t₁和t₃时刻k_ω＝3.0；

在缩小移动机器人的距离误差d_d时设计位置跟踪控制器，在位置跟踪过程中定义服务机器人坐标系(x_c，y_c)下的位置误差为(d_x，d_y)^T，则位置误差方程为：

其中，(x_e，y_e)为期望位置，根据位置误差(d_x，d_y)^T得到当前位置与期望位置的距离差d_d＝((d_x)²+(d_y)²)^1/2，通过下面的位置跟踪控制器使得d_d趋近于0：

其中k_v＞0，在t₁时刻距离误差较大，k_v＝1.5；在t₃和t₄时刻k_v＝0.5。