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

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

Info

Publication number
CN113156944B
CN113156944B CN202110329509.0A CN202110329509A CN113156944B CN 113156944 B CN113156944 B CN 113156944B CN 202110329509 A CN202110329509 A CN 202110329509A CN 113156944 B CN113156944 B CN 113156944B
Authority
CN
China
Prior art keywords
robot
point
control
mobile robot
pose
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110329509.0A
Other languages
English (en)
Other versions
CN113156944A (zh
Inventor
李艳丽
李乐
刘卫东
雷小康
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202110329509.0A priority Critical patent/CN113156944B/zh
Publication of CN113156944A publication Critical patent/CN113156944A/zh
Application granted granted Critical
Publication of CN113156944B publication Critical patent/CN113156944B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0246—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using a video camera in combination with image processing means
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0225—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving docking at a fixed facility, e.g. base station or loading bay
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Multimedia (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

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

Description

一种室内移动机器人充电路径规划与运动控制算法
技术领域
本发明属于机器人控制领域，特别涉及一种室内移动机器人充电路径规划与运动控制算法。
背景技术
随着机器人技术的发展，移动机器人已用于室内环境中的各种任务，如地板清洁，购物导引和护理老人等，移动机器人的自主作业是实现这些功能的关键要素。受到移动机器人的尺寸和电池容量的限制，机器人通常需要充电续航操作以实现较长时间的作业。为了实现对机器人在无人条件下也能够高效充电并提高其工作效率，必须确保机器人能自主到达充电点并停靠对接。
机器人对接技术可分为两种：一种是内部对接，指机器人内部的对接；一种是外部对接，涉及范围较大，是最为常用的对接方式。机器人外部对接技术的在生活中的最重要应用就是自动充电技术，机器人都是靠电能驱动的，而电池的容量限制导致了在电量不足的情况下，需要人为对机器人充电进行干预，这大大影响了机器人的工作效率。因此为保证机器人工作的可靠性，需要利用无线充电技术和机器人对接技术，使机器人能自主高效的进行充电。其中环境建模、轨迹规划和机器人运动控制是实现自动充电核心技术。大部分现有的移动机器人应用研究中，多利用激光雷达进行环境的感知，但实际应用中，激光雷达测距范围有限制，难以对近距离地物体精确测量；而高精度的激光雷达成本偏高，难以广泛应用与移动机器人。相较于激光雷达，摄像头成本低并且在近距离感知中具有较高的精度。将激光雷达与摄像头的结合应用应成为较大环境下机器人作业的新思路。
发明内容
本发明解决的技术问题是：为了解决现有移动机器人应用中，激光雷达只能在远距离高精度感知而摄像头感知距离有限的问题，本发明提出一种结合激光雷达远程控制和摄像头近程控制的室内移动机器人充电路径规划与运动控制算法。
本发明的技术方案是：一种室内移动机器人充电路径规划与运动控制算法，包括以下步骤：
步骤1：定义室内移动机器人上带有摄像头和激光雷达；根据激光雷达获取的作业环境信息，并对作业环境进行建模；
步骤2：当室内移动机器人距离充电点距离范围大于N米时，通过激光雷达进行远程控制，N为室内移动机器人进行远程控制和近程控制切换的临界值，该值在本发明中为2。包括以下子步骤：
步骤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的斜率为
Figure GDA0004035581450000021
且该直线段的延长线经过点Pi，则有该直线段方程为：
Figure GDA0004035581450000022
其中，
Figure GDA0004035581450000023
步骤2.2.3：设建立在Pi点处的抛物线pi的方程为
Figure GDA0004035581450000024
Figure GDA0004035581450000025
Δ1,Δ2为需要求解的方程参数。考虑机器人的机动特性，pi应与直线段li和直线段li+1分别在点
Figure GDA0004035581450000026
和点
Figure GDA0004035581450000027
处相切，则有：
Figure GDA0004035581450000031
其中，Δxi＝xi+1-xi；即
Figure GDA0004035581450000032
步骤2.2.4：得到每两个相邻点之间的轨迹方程后，整个轨迹由n-1个直线段和n个抛物线段平滑组成。所有的直线段方程都如上分析所示，在第一个点P1之前和最后一个点Pn之后可增加额外的点进行抛物线段的轨迹分析；
步骤2.3：建立移动机器人运动学模型，对子步骤2得到的充电路径进行轨迹跟踪控制，包括以下内容：
步骤2.3.1：建立移动机器人运动学模型，定义移动机器人当前位姿q＝[x y θ]T，和期望轨迹qr＝[xr yr θr]T，
其中，x为机器人在大地坐标系下x轴的坐标，y为在大地坐标系下y轴的坐标，θ为机器人在大地坐标下的角度；xr为机器人的期望位姿在大地坐标系下x轴的坐标，yr为机器人的期望位姿在大地坐标系下y轴的坐标，θr为机器人的期望位姿在大地坐标下的角度；
建立移动机器人在局部坐标系下的位姿误差eq：
Figure GDA0004035581450000033
式中，ex,ey,eθ分别表示机器人的期望位置和角度与实际位置和角度在大地坐标系下的偏差；
步骤2.3.2：对机器人充电路径跟踪控制建立如下控制律：
Figure GDA0004035581450000034
其中，vr和ωr分别为移动机器人期望轨迹下的线速度和角速度，v和ω则分别为通过轨迹跟踪求解出的机器人实时线速度和角速度，kx和ky分别为机器人的位置偏差控制参数，kθ为机器人角度偏差的控制参数；
步骤3：当室内移动机器人距离充电点距离范围小于N米时且摄像头获取图像中对接点像素与整体图像的像素比大于M％时，通过摄像头进行近程控制，其中，N为室内移动机器人进行远程控制和近程控制切换的临界值，该值在本发明中为2；M为机器人能够准确识别到目标时，该目标的图像区域在整个图像中的面积占比，在本发明取值为30。包括以下子步骤：
步骤3.1：通过摄像头检测到充电站定位目标；
步骤3.2：对机器人行点镇定控制，包括以下步骤：
步骤3.2.1：确定机器人当前位姿q＝[x yθ]T，和期望位姿qr＝[xr yrθr]T；步骤3.2.2：在大地坐标系下建立机器人的位姿误差方程，并转化到极坐标系下得到如下位姿误差动力学方程：
Figure GDA0004035581450000041
Figure GDA0004035581450000042
式中，ρ为机器人期望位姿与当前位姿在极坐标系下的距离；α为期望位姿与当前位姿的连线与极坐标系水平轴的夹角；
Figure GDA0004035581450000043
为机器人期望位姿与当前位姿的角度偏差；
步骤3.2.3：根据步骤3.2.2，分别得到机器人期望位姿与当前位姿存在位置偏差的第一点镇定控制律：
Figure GDA0004035581450000044
其中kρ、kα和
Figure GDA0004035581450000051
为机器人的距离偏差和角度偏差的控制参数，和期望位姿与当前位姿不存在位置偏差的第二点镇定控制律：
Figure GDA0004035581450000052
其中
Figure GDA0004035581450000053
为机器人的角度偏差控制参数；
步骤3.2.4：建立判断准则：以零时刻为初始时刻，若ρ＝0则第一点镇定控制律失效，第二点镇定控制律有效，即机器人初始位置与期望位置重合，机器人有角速度而无线速度；若ρ≠0则第一点镇定控制律有效，直到某时刻ρ＝0；若
Figure GDA0004035581450000054
则第二点镇定控制律有效；
此处的ρ＝0是一个转换条件，表示机器人已经运动到了期望的位置点，但是存在角度偏差，即
Figure GDA0004035581450000055
因此使用第二点镇定控制律，若ρ＝0且
Figure GDA0004035581450000056
说明机器人以及到达期望位姿，则停止运动；
步骤4：室内移动机器人按照规划轨迹和点控制规律，最终到达充电目标点实现机器人与充电点的准确对接。
本发明进一步的技术方案是：所述N取2。
本发明进一步的技术方案是：所述M取30。
本发明进一步的技术方案是：所述步骤1中，作业环境采用栅格法建模，选取1m*1m的栅格大小进行建模。
本发明进一步的技术方案是：对所述步骤2.3.2得到的控制律进行稳定性分析，包括以下步骤：
步骤1：建立轨迹跟踪控制系统
Figure GDA0004035581450000057
从而确定轨迹跟踪控制系统的稳定控制点为eq＝[000]T，不稳定平衡点为eq＝[00π]T；
步骤2：通过对建立的轨迹跟踪控制系统进行局部稳定性分析，确定使控制器局部稳定的参数范围kx＞0,ky＞0,kθ＞0；
步骤3：通过对建立的轨迹跟踪控制系统进行全局稳定性分析，证明在步骤2中求得的参数范围下，所建立的控制器具有全局稳定性，可以用于机器人轨迹跟踪控制。
本发明进一步的技术方案是：对所述步骤3.2.3得到的控制律进行稳定性分析，包括以下步骤：
步骤1：建立在期望位姿与当前位姿存在距离偏差时的点镇定控制系统一
Figure GDA0004035581450000061
和期望位姿与当前位姿不存在距离偏差、仅存在角度偏差时的点镇定控制系统二
Figure GDA0004035581450000062
从而确定轨迹跟踪控制系统的稳定控制点为eq＝[000]T；
步骤2：通过对建立的点镇定控制系统进行局部稳定性分析，确定使点镇定第一控制器局部稳定的参数范围是
Figure GDA0004035581450000063
确定使点镇定第二控制器局部稳定的参数范围是
Figure GDA0004035581450000064
步骤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米，且摄像头获取图像中对接点像素与整体图像的像素比大于30％时，由激光雷达切换到摄像头。选用的摄像头为Kinect2，具体参数如图6所示。通过摄像头获取充电点的准确信息，在机器人运动学模型的基础上，建立点镇定运动控制律，并进行稳定性分析，使机器人实现与充电点的准确对接。
对机器人作业环境进行栅格法建模时，考虑了机器人的运动特性和作业环境实际情况，选取1m*1m的栅格大小进行建模。接下来由A*算法规划出运动路径点集合，利用抛物线-直线平滑法，在路径点两端处用抛物线平滑而在相邻两路径点中间部分用直线进行拟合，不仅保证了规划轨迹的平滑性，也有效减少了由平滑引起的轨迹长度增加。
机器人轨迹跟踪控制律设计步骤如下：
轨迹跟踪控制示意图如图7所示，机器人在局部坐标系下的机器人位姿误差eq为，
Figure GDA0004035581450000091
用位姿误差表示轨迹跟踪控制系统的状态空间方程，对公式(1)两边同时求导可得
Figure GDA0004035581450000092
基于该位姿误差动力学方程，为轨迹跟踪控制设计控制律如公式(2)所示。其中,kx、ky和kθ为控制器参数。
Figure GDA0004035581450000093
对所设计控制律进行稳定性分析，基本流程如下：
(1)闭环系统平衡状态
将公式(2)代入位姿误差动力学方程中可得用状态变量表示的轨迹跟踪控制系统，如公式(3)所示。
Figure GDA0004035581450000094
其中，ex∈R1,ey∈R1,eθ∈(-π,π]。令f(eq)＝0，求解公式(3)所代表的闭环系统的平衡状态,有eq＝0和eq＝[0 0 π]T使f(0)＝0成立。通过以下局部稳定性证明可知，在满足一定条件下，eq＝[0 0 π]T为不稳定平衡点，系统仅存在原点平衡状态，即在原点全局渐近稳定。
(2)局部稳定性分析
对如公式(3)所示的非线性定常系统，分别求其在原点平衡状态和eq＝[0 0 π]T的线性化矩阵A1和A2，可得到两个线性化系统。利用线性系统的稳定性定理进行分析，可得如果控制器参数kx、ky和kθ的取值满足公式(4)所示关系，那么形如公式(3)所示的非线性定常系统，在原点是局部一致渐近稳定的，而在eq＝[0 0 π]T是不稳定的。
kx＞0,ky＞0,kθ＞0 (4)
(3)全局稳定性分析
由局部稳定性分析可知公式(3)所示系统在eq＝[0 0 π]T是不稳定的，因此仅讨论系统在原点的全局一致渐近稳定性。公式(3)所示的非线性定常系统的状态向量为eq，eq∈Eq，Eq表示状态空间，
Eq＝{[ex ey eθ]T|ex∈R1,ey∈R1,eθ∈(-π,π]}。
选取如公式(5)所示的Lyapunov候选函数，并且控制器参数kx、ky和kθ的取值满足如公式(4)所示关系。
Figure GDA0004035581450000101
通过证明，有“V(eq)≥0,当且仅当eq＝0时V(0)＝0”、“V(eq)≤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，位于机器人偏差范围之内，控制效果较好。
点镇定运动控制律设计步骤如下：
建立机器人在极坐标系下的机器人位姿误差动力学方程：
Figure GDA0004035581450000111
Figure GDA0004035581450000112
基于公式(6)所示的位姿误差动力学方程，为点镇定控制设计控制律如公式(8)所示。其中kρ、kα和
Figure GDA0004035581450000117
为控制器参数。
Figure GDA0004035581450000113
基于公式(7)所示的位姿误差动力学方程，为点镇定控制设计控制律如公式(9)所示。其中
Figure GDA0004035581450000114
为控制器参数。
Figure GDA0004035581450000115
公式(8)和公式(9)共同构成了点镇定控制的控制律，分析如下：以零时刻为初始时刻，若ρ＝0则控制律(8)不起作用，仅控制律(9)有效，即机器人初始位置与期望位置重合，机器人有角速度而无线速度；若ρ≠0则控制律(8)有效，直到某时刻ρ＝0，若
Figure GDA0004035581450000116
则控制律(9)起作用。随着机器人趋近于期望位姿，控制输入也逐渐减小，到达期望位姿时机器人控制输入为零。
对于所设计的点镇定控制律，需要对该系统进行稳定性分析，基本流程与轨迹跟踪控制律稳定性分析一致。首先求取在设计的控制律作用下闭环系统的平衡状态；然后通过分析在该系统的原点局部一致渐近稳定性和在非原点孤立边界平衡状态的不稳定性，确定控制器参数的取值范围；最后用Lypunov候选函数法，得出在该控制律作用下的闭环系统在原点具有全局一致渐近稳定性的结论，此时参数满足条件如下：
Figure GDA0004035581450000121
Figure GDA0004035581450000122
由Lypunov第二法可知，在公式(8)和公式(9)控制律的作用下，系统在原点全局一致渐近稳定，选取满足公式(10)和公式(11)情况下的参数，即可对机器人进行点镇定控制。
本发明选取参数为
Figure GDA0004035581450000123
对步骤4的近程控制阶段实施点镇定控制，点镇定控制结果如图9所示。在此实验中，点镇定控制的起始点为(11.01m,9.09m,0m)，终点为(12m,10m,0m)，实际的点镇定控制终点为(11.99m,10m,0m)，偏差为0.01m，位于机器人偏差范围之内，控制效果较好。
与传统的移动机器人运动控制相比，本发明首先将抛物线-直线平滑法与A*路径规划方法结合，在平滑轨迹的同时，也大幅减少了由于平滑引起的轨迹增加。同时依据移动机器人的运动学模型，建立了轨迹跟踪控制律，并通过稳定性分析选出适合的控制参数，使机器人能有效跟踪规划轨迹。最终在找出充电点准确位置基础上，依据移动机器人的运动学模型，建立了点镇定控制律，并通过稳定性分析选出适合的控制参数，使机器人平滑的运动至充电点并实现准确对接。

Claims (6)

1.一种室内移动机器人充电路径规划与运动控制算法，其特征在于，包括以下步骤：
步骤1：定义室内移动机器人上带有摄像头和激光雷达；根据激光雷达获取的作业环境信息，并对作业环境进行建模；
步骤2：当室内移动机器人距离充电点距离范围大于N米时，通过激光雷达进行远程控制，N为室内移动机器人进行远程控制和近程控制切换的临界值，该值在本算法中为2；包括以下子步骤：
步骤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的斜率为
Figure FDA0004017405090000011
且该直线段的延长线经过点Pi，则有该直线段方程为：
Figure FDA0004017405090000012
其中，
Figure FDA0004017405090000013
步骤2.2.3：设建立在Pi点处的抛物线pi的方程为
Figure FDA0004017405090000014
y0，
Figure FDA0004017405090000015
Δ1,Δ2为需要求解的方程参数；考虑机器人的机动特性，pi应与直线段li和直线段li+1分别在点
Figure FDA0004017405090000016
和点
Figure FDA0004017405090000017
处相切，则有：
Figure FDA0004017405090000018
其中，Δxi＝xi+1-xi；即y0＝yi,
Figure FDA0004017405090000019
Δ1＝xi,
Figure FDA00040174050900000110
步骤2.2.4：得到每两个相邻点之间的轨迹方程后，整个轨迹由n-1个直线段和n个抛物线段平滑组成；所有的直线段方程都如上分析所示，在第一个点P1之前和最后一个点Pn之后可增加额外的点进行抛物线段的轨迹分析；
步骤2.3：建立移动机器人运动学模型，对子步骤2得到的充电路径进行轨迹跟踪控制，包括以下内容：
步骤2.3.1：建立移动机器人运动学模型，定义移动机器人当前位姿q＝[x y θ]T，和期望轨迹qr＝[xr yr θr]T，
其中，x为机器人在大地坐标系下x轴的坐标，y为在大地坐标系下y轴的坐标，θ为机器人在大地坐标下的角度；xr为机器人的期望位姿在大地坐标系下x轴的坐标，yr为机器人的期望位姿在大地坐标系下y轴的坐标，θr为机器人的期望位姿在大地坐标下的角度；
建立移动机器人在局部坐标系下的位姿误差eq：
Figure FDA0004017405090000021
式中，ex,ey,eθ分别表示机器人的期望位置和角度与实际位置和角度在大地坐标系下的偏差；
步骤2.3.2：对机器人充电路径跟踪控制建立如下控制律：
Figure FDA0004017405090000022
其中，vr和ωr分别为移动机器人期望轨迹下的线速度和角速度，v和ω则分别为通过轨迹跟踪求解出的机器人实时线速度和角速度，kx和ky分别为机器人的位置偏差控制参数，kθ为机器人角度偏差的控制参数；
步骤3：当室内移动机器人距离充电点距离范围小于N米时且摄像头获取图像中对接点像素与整体图像的像素比大于M％时，通过摄像头进行近程控制，其中，N为室内移动机器人进行远程控制和近程控制切换的临界值，该值在本算法中为2；M为机器人能够准确识别到目标时，该目标的图像区域在整个图像中的面积占比，在本算法取值为30；包括以下子步骤：
步骤3.1：通过摄像头检测到充电站定位目标；
步骤3.2：对机器人行点镇定控制，包括以下步骤：
步骤3.2.1：确定机器人当前位姿q＝[x y θ]T，和期望位姿qr＝[xr yr θr]T；
步骤3.2.2：在大地坐标系下建立机器人的位姿误差方程，并转化到极坐标系下得到如下位姿误差动力学方程：
Figure FDA0004017405090000031
Figure FDA0004017405090000032
式中，ρ为机器人期望位姿与当前位姿在极坐标系下的距离；α为期望位姿与当前位姿的连线与极坐标系水平轴的夹角；
Figure FDA0004017405090000033
为机器人期望位姿与当前位姿的角度偏差；
步骤3.2.3：根据步骤3.2.2，分别得到机器人期望位姿与当前位姿存在位置偏差的第一点镇定控制律：
Figure FDA0004017405090000034
其中kρ、kα和
Figure FDA0004017405090000035
为机器人的距离偏差和角度偏差的控制参数，和期望位姿与当前位姿不存在位置偏差的第二点镇定控制律：
Figure FDA0004017405090000041
其中
Figure FDA0004017405090000042
为机器人的角度偏差控制参数；
步骤3.2.4：建立判断准则：以零时刻为初始时刻，若ρ＝0则第一点镇定控制律失效，第二点镇定控制律有效，即机器人初始位置与期望位置重合，机器人有角速度而无线速度；若ρ≠0则第一点镇定控制律有效，直到某时刻ρ＝0；若
Figure FDA0004017405090000043
则第二点镇定控制律有效；
此处的ρ＝0是一个转换条件，表示机器人已经运动到了期望的位置点，但是存在角度偏差，即
Figure FDA0004017405090000044
因此使用第二点镇定控制律，若ρ＝0且
Figure FDA0004017405090000045
说明机器人以及到达期望位姿，则停止运动；
步骤4：室内移动机器人按照规划轨迹和点控制规律，最终到达充电目标点实现机器人与充电点的准确对接。
2.如权利要求1所述的一种室内移动机器人充电路径规划与运动控制算法，其特征在于，N为室内移动机器人进行远程控制和近程控制切换的临界值，该值在本算法中为2；取该数值的原因是，对于本算法，经过多次视觉检测，当充电点与移动机器人的距离在2米以内是，对于充点电的检测和定位更加准确，因此N值为2。
3.如权利要求1所述的一种室内移动机器人充电路径规划与运动控制算法，其特征在于，N为室内移动机器人进行远程控制和近程控制切换的临界值，该值在本算法中为2；M为机器人能够准确识别到目标时，该目标的图像区域在整个图像中的面积占比，在本算法取值为30；M取值原因为，对于本算法，当移动机器人距离充电点为2米时，充电点在图像中所占像素比例为30％，此时移动机器人已经可以识别出充电点，在距离缩短时，该比例会增加，定位准确率也能得到保证。
4.如权利要求1所述的一种室内移动机器人充电路径规划与运动控制算法，其特征在于，所述步骤1中，作业环境采用栅格法建模，选取1m*1m的栅格大小进行建模。
5.如权利要求1所述的一种室内移动机器人充电路径规划与运动控制算法，其特征在于，对所述步骤2.3.2得到的控制律进行稳定性分析，包括以下步骤：
步骤1：建立轨迹跟踪控制系统
Figure FDA0004017405090000051
从而确定轨迹跟踪控制系统的稳定控制点为eq＝[0 0 0]T，不稳定平衡点为eq＝[0 0π]T；
步骤2：通过对建立的轨迹跟踪控制系统进行局部稳定性分析，确定使控制器局部稳定的参数范围kx＞0,ky＞0,kθ＞0；
步骤3：通过对建立的轨迹跟踪控制系统进行全局稳定性分析，证明在步骤2中求得的参数范围下，所建立的控制器具有全局稳定性，可以用于机器人轨迹跟踪控制。
6.如权利要求1所述的一种室内移动机器人充电路径规划与运动控制算法，其特征在于，对所述步骤3.2.3得到的控制律进行稳定性分析，包括以下步骤：
步骤1：建立在期望位姿与当前位姿存在距离偏差时的点镇定控制系统一
Figure FDA0004017405090000052
和期望位姿与当前位姿不存在距离偏差、仅存在角度偏差时的点镇定控制系统二
Figure FDA0004017405090000053
从而确定轨迹跟踪控制系统的稳定控制点为eq＝[0 0 0]T；
步骤2：通过对建立的点镇定控制系统进行局部稳定性分析，确定使点镇定第一控制器局部稳定的参数范围是kρ＞0,
Figure FDA0004017405090000061
确定使点镇定第二控制器局部稳定的参数范围是
Figure FDA0004017405090000062
步骤3：通过对建立的点镇定控制系统进行全局稳定性分析，证明在步骤2中求得的参数范围下，所建立的控制器具有全局稳定性，可以用于机器人点镇定控制。
CN202110329509.0A 2021-03-28 2021-03-28 一种室内移动机器人充电路径规划与运动控制算法 Active CN113156944B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110329509.0A CN113156944B (zh) 2021-03-28 2021-03-28 一种室内移动机器人充电路径规划与运动控制算法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110329509.0A CN113156944B (zh) 2021-03-28 2021-03-28 一种室内移动机器人充电路径规划与运动控制算法

Publications (2)

Publication Number Publication Date
CN113156944A CN113156944A (zh) 2021-07-23
CN113156944B true CN113156944B (zh) 2023-03-07

Family

ID=76885158

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110329509.0A Active CN113156944B (zh) 2021-03-28 2021-03-28 一种室内移动机器人充电路径规划与运动控制算法

Country Status (1)

Country Link
CN (1) CN113156944B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115280964B (zh) * 2022-08-19 2024-03-19 江苏大学 一种茎叶蔬菜收获机自动作业行驶方法及系统和收获机
CN117506934B (zh) * 2024-01-03 2024-03-12 创新奇智(北京)科技有限公司 工业机器人自动化控制系统及方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104460668A (zh) * 2014-11-03 2015-03-25 沈阳化工大学 一种提高足球机器人射门效率的方法
CN107030697A (zh) * 2017-04-28 2017-08-11 广州大学 一种机器人笛卡尔空间平滑轨迹的规划方法
CN110262495A (zh) * 2019-06-26 2019-09-20 山东大学 可实现移动机器人自主导航与精确定位的控制系统及方法
CN110286683A (zh) * 2019-07-15 2019-09-27 北京科技大学 一种履带式移动机器人的自主行驶路径跟踪控制方法
CN112183133A (zh) * 2020-08-28 2021-01-05 同济大学 一种基于ArUco码引导的移动机器人自主充电方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2015535373A (ja) * 2012-10-05 2015-12-10 アイロボット コーポレイション 移動ロボットを含むドッキングステーション姿勢を決定するためのロボット管理システムとこれを用いる方法
CN105511457B (zh) * 2014-09-25 2019-03-01 科沃斯机器人股份有限公司 机器人静态路径规划方法
CN110162063A (zh) * 2019-06-12 2019-08-23 北京洛必德科技有限公司 一种用于机器人自动充电的路径规划方法和装置
CN111650929B (zh) * 2020-03-02 2023-03-31 南阳师范学院 一种自适应滑模控制方法、系统及移动机器人控制器

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104460668A (zh) * 2014-11-03 2015-03-25 沈阳化工大学 一种提高足球机器人射门效率的方法
CN107030697A (zh) * 2017-04-28 2017-08-11 广州大学 一种机器人笛卡尔空间平滑轨迹的规划方法
CN110262495A (zh) * 2019-06-26 2019-09-20 山东大学 可实现移动机器人自主导航与精确定位的控制系统及方法
CN110286683A (zh) * 2019-07-15 2019-09-27 北京科技大学 一种履带式移动机器人的自主行驶路径跟踪控制方法
CN112183133A (zh) * 2020-08-28 2021-01-05 同济大学 一种基于ArUco码引导的移动机器人自主充电方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Path Planning Method for AUV Docking Based on Adaptive Quantum-Behaved Particle Swarm Optimization;Li Le;《IEEE Access》;20190613;第7卷;第78665-78674页 *
基于认知差异的多机器人协同信息趋向烟羽源搜索方法;雷小康;《控制与决策》;20180131;第33卷(第1期);第45-52页 *

Also Published As

Publication number Publication date
CN113156944A (zh) 2021-07-23

Similar Documents

Publication Publication Date Title
CN113156944B (zh) 一种室内移动机器人充电路径规划与运动控制算法
Zhang et al. 2d lidar-based slam and path planning for indoor rescue using mobile robots
Savkin et al. A simple biologically inspired algorithm for collision-free navigation of a unicycle-like robot in dynamic environments with moving obstacles
CN112183133B (zh) 一种基于ArUco码引导的移动机器人自主充电方法
Moon et al. Kinodynamic planner dual-tree RRT (DT-RRT) for two-wheeled mobile robots using the rapidly exploring random tree
CN108983603B (zh) 一种机器人与物体的对接方法及其机器人
CN111307147B (zh) 一种融合定位反光板与激光特征的agv高精度定位方法
CN106774310A (zh) 一种机器人导航方法
CN106346129B (zh) 一种基于激光焊缝跟踪传感器的机器人焊接运动控制方法
Xu et al. Image-based visual impedance force control for contact aerial manipulation
Narváez et al. Autonomous vtol-uav docking system for heterogeneous multirobot team
CN109976164B (zh) 一种多旋翼无人机能量优化视觉覆盖轨迹规划方法
CN109900273B (zh) 一种室外移动机器人导引方法及导引系统
Park et al. Feedback motion planning via non-holonomic RRT* for mobile robots
Pfrunder et al. A proof-of-concept demonstration of visual teach and repeat on a quadrocopter using an altitude sensor and a monocular camera
CN112083727B (zh) 基于速度障碍物的多自主体系统分布式避碰编队控制方法
Poonawala et al. Formation control of wheeled robots with vision-based position measurement
McIsaac et al. Experiments in closed-loop control for an underwater eel-like robot
Lee et al. Autonomous landing system for aerial mobile robot cooperation
Nieuwenhuisen et al. Local multiresolution trajectory optimization for micro aerial vehicles employing continuous curvature transitions
Klančar et al. Combined stochastic-deterministic predictive control using local-minima free navigation
CN114355933A (zh) 机器人对接充电桩的运动控制方法
CN110231823B (zh) 一种双轮机器人的直接控制方法
Paromtchik et al. Optical guidance system for multiple mobile robots
Effati et al. An equivalent time-optimal problem to find energy-optimal paths for skid-steer rovers

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant