一种室内移动机器人充电路径规划与运动控制算法

本发明属于机器人控制领域，特别涉及一种室内移动机器人充电路径规划与运动控制方法。

背景技术：

随着机器人技术的发展，移动机器人已用于室内环境中的各种任务，如地板清洁，购物导引和护理老人等，移动机器人的自主作业是实现这些功能的关键要素。受到移动机器人的尺寸和电池容量的限制，机器人通常需要充电续航操作以实现较长时间的作业。为了实现对机器人在无人条件下也能够高效充电并提高其工作效率，必须确保机器人能自主到达充电点并停靠对接。

机器人对接技术可分为两种：一种是内部对接，指机器人内部的对接；一种是外部对接，涉及范围较大，是最为常用的对接方式。机器人外部对接技术的在生活中的最重要应用就是自动充电技术，机器人都是靠电能驱动的，而电池的容量限制导致了在电量不足的情况下，需要人为对机器人充电进行干预，这大大影响了机器人的工作效率。因此为保证机器人工作的可靠性，需要利用无线充电技术和机器人对接技术，使机器人能自主高效的进行充电。其中环境建模、轨迹规划和机器人运动控制是实现自动充电核心技术。大部分现有的移动机器人应用研究中，多利用激光雷达进行环境的感知，但实际应用中，激光雷达测距范围有限制，难以对近距离地物体精确测量；而高精度的激光雷达成本偏高，难以广泛应用与移动机器人。相较于激光雷达，摄像头成本低并且在近距离感知中具有较高的精度。将激光雷达与摄像头的结合应用应成为较大环境下机器人作业的新思路。

技术实现要素：

本发明解决的技术问题是：为了解决现有移动机器人应用中，激光雷达只能在远距离高精度感知而摄像头感知距离有限的问题，本发明提出一种结合激光雷达远程控制和摄像头近程控制的室内移动机器人充电路径规划与运动控制方法。

本发明的技术方案是：一种室内移动机器人充电路径规划与运动控制方法，包括以下步骤：

步骤1：定义室内移动机器人上带有摄像头和激光雷达；根据激光雷达获取的作业环境信息，并对作业环境进行建模；

步骤2：当室内移动机器人距离充电点距离范围大于n米时，通过激光雷达进行远程控制，包括以下子步骤：

步骤2.1：采用a*算法对充电路径基本规划；

步骤2.2：结合抛物线-直线平滑法对子步骤1得到充电路径的进行轨迹平滑，包括以下子步骤：

步骤2.2.1：由a*算法得到n对离散点pi(xi,yi)(i＝1,2,...,n)，每两个相邻点pi和pi+1之间的轨迹曲线由一个直线段li和两个抛物线段pi和pi+1组成，建立曲线方程为yi＝f(xi)；

步骤2.2.2：两点之间的直线段li的斜率为且该直线段的延长线经过点pi，则有该直线段方程为：

其中，

步骤2.2.3：建立在pi点处的抛物线pi的方程为考虑机器人的机动特性，pi应与直线段li和直线段li+1分别在点和点处相切，则有：

其中，

步骤2.2.4：得到每两个相邻点之间的轨迹方程后，整个轨迹由n-1个直线段和n个抛物线段平滑组成。所有的直线段方程都如上分析所示，在第一个点p1之前和最后一个点pn之后可增加额外的点进行抛物线段的轨迹分析；

步骤2.3：建立移动机器人运动学模型，对子步骤2得到的充电路径进行轨迹跟踪控制，包括以下内容：

步骤2.3.1：建立移动机器人运动学模型，定义移动机器人当前位姿q＝[xyθ]^t，和期望轨迹qr＝[xryrθr]^t，

其中，x为机器人在大地坐标系下x轴的坐标，y为在大地坐标系下y轴的坐标，θ为机器人在大地坐标下的角度；xr为机器人的期望位姿在大地坐标系下x轴的坐标，yr为机器人的期望位姿在大地坐标系下y轴的坐标，θr为机器人的期望位姿在大地坐标下的角度；

建立移动机器人在局部坐标系下的位姿误差eq：

式中，ex,ey,eθ分别表示机器人的期望位置和角度与实际位置和角度在大地坐标系下的偏差；

步骤2.3.2：对机器人充电路径跟踪控制建立如下控制律：

其中kx和ky分别为机器人的位置偏差控制参数，kθ为机器人角度偏差的控制参数；步骤3：当室内移动机器人距离充电点距离范围小于n米时且摄像头获取图像中对接点像素与整体图像的像素比大于m％时，通过摄像头进行近程控制，包括以下子步骤：

步骤3.1：通过摄像头检测到充电站定位目标；

步骤3.2：对机器人行点镇定控制，包括以下步骤：

步骤3.2.1：确定机器人当前位姿q＝[xyθ]^t，和期望位姿qr＝[xryrθr]^t；

步骤3.2.2：在大地坐标系下建立机器人的位姿误差方程，并转化到极坐标系下得到如下位姿误差动力学方程：

式中，ρ为机器人期望位姿与当前位姿在极坐标系下的距离；α为期望位姿与当前位姿的连线与极坐标系水平轴的夹角；为机器人期望位姿与当前位姿的角度偏差；

步骤3.2.3：根据步骤3.2.2，分别得到机器人期望位姿与当前位姿存在位置偏差的第一点镇定控制律：

其中kρ、kα和为机器人的距离偏差和角度偏差的控制参数，和期望位姿与当前位姿不存在位置偏差的第二点镇定控制律：

其中为机器人的角度偏差控制参数；

步骤3.2.4：建立判断准则：以零时刻为初始时刻，若ρ＝0则第一点镇定控制律失效，第二点镇定控制律有效，即机器人初始位置与期望位置重合，机器人有角速度而无线速度；若ρ≠0则第一点镇定控制律有效，直到某时刻ρ＝0；若则第二点镇定控制律有效；

此处的ρ＝0是一个转换条件，表示机器人已经运动到了期望的位置点，但是存在角度偏差，即因此使用第二点镇定控制律，若ρ＝0且说明机器人以及到达期望位姿，则停止运动；

步骤4：室内移动机器人按照规划轨迹和点控制规律，最终到达充电目标点实现机器人与充电点的准确对接。

本发明进一步的技术方案是：所述n取2。

本发明进一步的技术方案是：所述m取1。

本发明进一步的技术方案是：所述步骤1中，作业环境采用栅格法建模，选取1m*1m的栅格大小进行建模。

本发明进一步的技术方案是：对所述步骤2.3.2得到的控制律进行稳定性分析，包括以下步骤：

步骤1：建立轨迹跟踪控制系统

从而确定轨迹跟踪控制系统的稳定控制点为eq＝[000]^t，不稳定平衡点为eq＝[00π]^t；

步骤2：通过对建立的轨迹跟踪控制系统进行局部稳定性分析，确定使控制器局部稳定的参数范围kx＞0,ky＞0,kθ＞0；

步骤3：通过对建立的轨迹跟踪控制系统进行全局稳定性分析，证明在步骤2中求得的参数范围下，所建立的控制器具有全局稳定性，可以用于机器人轨迹跟踪控制。

本发明进一步的技术方案是：对所述步骤3.2.3得到的控制律进行稳定性分析，包括以下步骤：

步骤1：建立在期望位姿与当前位姿存在距离偏差时的点镇定控制系统一

和期望位姿与当前位姿不存在距离偏差、仅存在角度偏差时的点镇定控制系统二

从而确定轨迹跟踪控制系统的稳定控制点为eq＝[000]^t；

步骤2：通过对建立的点镇定控制系统进行局部稳定性分析，确定使点镇定第一控制器局部稳定的参数范围是kρ＞0,确定使点镇定第二控制器局部稳定的参数范围是

步骤3：通过对建立的点镇定控制系统进行全局稳定性分析，证明在步骤2中求得的参数范围下，所建立的控制器具有全局稳定性，可以用于机器人点镇定控制。

发明效果

本发明的技术效果在于：

(1)将抛物线-直线平滑法与a*路径规划方法结合，在平滑轨迹的同时，也大幅减少了由于平滑引起的轨迹增加。

(2)依据移动机器人的运动学模型，建立了轨迹跟踪控制律，并通过稳定性分析选出适合的控制参数，使机器人能有效跟踪规划轨迹。

(3)依据充点电位置和移动机器人的运动学模型，建立了点镇定控制律，并通过稳定性分析选出适合的控制参数，使机器人平滑的运动至充电点位置。

附图说明

图1为本发明所用的激光雷达及其型号示意图

图2为本发明提出的算法流程示意图

图3为本发明的轨迹规划结果图

图4为本发明中轨迹平滑方法与传统轨迹平滑方法的效果对比图

图5为本发明的移动机器人运动学模型示意图

图6为本发明所用的摄像头及其型号示意图

图7为本发明的轨迹跟踪控制示意图

图8为本发明的轨迹跟踪控制结果图，其中(a)为机器人轨迹变化图，(b)为误差结果图

图9为本发明的点镇定跟踪控制结果图，其中(a)为机器人轨迹变化图，(b)为误差结果图

具体实施方式

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。

参见图1-图9，一种室内移动机器人充电路径规划与运动控制算法，具体步骤如下：

步骤1，根据机器人作业环境，结合移动机器人常用的工作环境地图建模方法，对环境地图进行建模，选用的激光雷达型号为hls-lfcd2(lds-01)，生产商家为日本日立-韩国lg，具体参数如图1所示；

步骤2，轨迹规划。选取a*算法作为本算法的基础路径规划方法；结合抛物线-直线平滑法进行轨迹平滑，得到规划轨迹。

对建立的机器人工作场景进行如步骤1的环境建模后，已知起点(0m,0m,0m)和终点(11m,9m,0m)，利用a*算法进行路径规划，并对规划的路径采用抛物线-直线平滑法对规划路径进行平滑，得到如图3所示的轨迹平滑结果图。并将本发明的轨迹平滑方法与三次样条平滑法进行了效果对比，结果如图4所示。在相同的规划路径下，本发明平滑后的轨迹长度比利用三次样条平滑法得到的轨迹长度少了10.8％，可以降低机器人的运动损耗。

步骤3，轨迹跟踪控制。当机器人距离充电点距离大于2米时，建立移动机器人运动学模型如图5所示。在该模型基础上，设计移动机器人轨迹跟踪控制律，并进行稳定性分析，保证机器人能高效跟踪规划轨迹。

步骤4，点镇定对接控制。当机器人距离充电点距离小于2米，且摄像头获取图像中对接点像素与整体图像的像素比大于1％时，由激光雷达切换到摄像头。选用的摄像头为kinect2，具体参数如图6所示。通过摄像头获取充电点的准确信息，在机器人运动学模型的基础上，建立点镇定运动控制律，并进行稳定性分析，使机器人实现与充电点的准确对接。

对机器人作业环境进行栅格法建模时，考虑了机器人的运动特性和作业环境实际情况，选取1m*1m的栅格大小进行建模。接下来由a*算法规划出运动路径点集合，利用抛物线-直线平滑法，在路径点两端处用抛物线平滑而在相邻两路径点中间部分用直线进行拟合，不仅保证了规划轨迹的平滑性，也有效减少了由平滑引起的轨迹长度增加。

机器人轨迹跟踪控制律设计步骤如下：

轨迹跟踪控制示意图如图7所示，机器人在局部坐标系下的机器人位姿误差eq为，

用位姿误差表示轨迹跟踪控制系统的状态空间方程，对公式(1)两边同时求导可得基于该位姿误差动力学方程，为轨迹跟踪控制设计控制律如公式(2)所示。其中,kx、ky和kθ为控制器参数。

对所设计控制律进行稳定性分析，基本流程如下：

(1)闭环系统平衡状态

将公式(2)代入位姿误差动力学方程中可得用状态变量表示的轨迹跟踪控制系统，如公式(3)所示。

其中，ex∈r¹,ey∈r¹,eθ∈(-π,π]。令f(eq)＝0，求解公式(3)所代表的闭环系统的平衡状态,有eq＝0和eq＝[00π]^t使f(0)＝0成立。通过以下局部稳定性证明可知，在满足一定条件下，eq＝[00π]^t为不稳定平衡点，系统仅存在原点平衡状态，即在原点全局渐近稳定。

(2)局部稳定性分析

对如公式(3)所示的非线性定常系统，分别求其在原点平衡状态和eq＝[00π]^t的线性化矩阵a1和a2，可得到两个线性化系统。利用线性系统的稳定性定理进行分析，可得如果控制器参数kx、ky和kθ的取值满足公式(4)所示关系，那么形如公式(3)所示的非线性定常系统，在原点是局部一致渐近稳定的，而在eq＝[00π]^t是不稳定的。

kx＞0,ky＞0,kθ＞0(4)

(3)全局稳定性分析

由局部稳定性分析可知公式(3)所示系统在eq＝[00π]^t是不稳定的，因此仅讨论系统在原点的全局一致渐近稳定性。公式(3)所示的非线性定常系统的状态向量为eq，eq∈eq，eq表示状态空间，

eq＝{[exeyeθ]^t|ex∈r¹,ey∈r¹,eθ∈(-π,π]}。

选取如公式(5)所示的lyapunov候选函数，并且控制器参数kx、ky和kθ的取值满足如公式(4)所示关系。

通过证明，有“v(eq)≥0,当且仅当eq＝0时v(0)＝0”、“当||eq||→∞时，v(eq)→∞”成立。由lyapunov第二法可知，公式(3)所示系统在原点全局一致渐近稳定，选取满足公式(4)情况下的参数，即可对机器人进行轨迹跟踪控制。

本发明选取参数为kx＝1,ky＝2,kθ＝1，对步骤2规划的机器人轨迹进行轨迹跟踪控制。如图8所示，在本发明的轨迹跟踪控制律的作用下，机器人的实际运动终点为(11.01m,9.09m,0m)，与规划路径中的终点(11m,9m,0m)之间的偏差为0.091m，位于机器人偏差范围之内，控制效果较好。

点镇定运动控制律设计步骤如下：

建立机器人在极坐标系下的机器人位姿误差动力学方程：

基于公式(6)所示的位姿误差动力学方程，为点镇定控制设计控制律如公式(8)所示。其中kρ、kα和为控制器参数。

基于公式(7)所示的位姿误差动力学方程，为点镇定控制设计控制律如公式(9)所示。其中为控制器参数。

公式(8)和公式(9)共同构成了点镇定控制的控制律，分析如下：以零时刻为初始时刻，若ρ＝0则控制律(8)不起作用，仅控制律(9)有效，即机器人初始位置与期望位置重合，机器人有角速度而无线速度；若ρ≠0则控制律(8)有效，直到某时刻ρ＝0，若则控制律(9)起作用。随着机器人趋近于期望位姿，控制输入也逐渐减小，到达期望位姿时机器人控制输入为零。

对于所设计的点镇定控制律，需要对该系统进行稳定性分析，基本流程与轨迹跟踪控制律稳定性分析一致。首先求取在设计的控制律作用下闭环系统的平衡状态；然后通过分析在该系统的原点局部一致渐近稳定性和在非原点孤立边界平衡状态的不稳定性，确定控制器参数的取值范围；最后用lypunov候选函数法，得出在该控制律作用下的闭环系统在原点具有全局一致渐近稳定性的结论，此时参数满足条件如下：

由lypunov第二法可知，在公式(8)和公式(9)控制律的作用下，系统在原点全局一致渐近稳定，选取满足公式(10)和公式(11)情况下的参数，即可对机器人进行点镇定控制。

本发明选取参数为kρ＝1,kα＝3,对步骤4的近程控制阶段实施点镇定控制，点镇定控制结果如图9所示。在此实验中，点镇定控制的起始点为(11.01m,9.09m,0m)，终点为(12m,10m,0m)，实际的点镇定控制终点为(11.99m,10m,0m)，偏差为0.01m，位于机器人偏差范围之内，控制效果较好。

与传统的移动机器人运动控制相比，本发明首先将抛物线-直线平滑法与a*路径规划方法结合，在平滑轨迹的同时，也大幅减少了由于平滑引起的轨迹增加。同时依据移动机器人的运动学模型，建立了轨迹跟踪控制律，并通过稳定性分析选出适合的控制参数，使机器人能有效跟踪规划轨迹。最终在找出充电点准确位置基础上，依据移动机器人的运动学模型，建立了点镇定控制律，并通过稳定性分析选出适合的控制参数，使机器人平滑的运动至充电点并实现准确对接。