CN112928799B - 基于激光测量的移动机器人自动对接充电方法 - Google Patents

基于激光测量的移动机器人自动对接充电方法 Download PDF

Info

Publication number
CN112928799B
CN112928799B CN202110155802.XA CN202110155802A CN112928799B CN 112928799 B CN112928799 B CN 112928799B CN 202110155802 A CN202110155802 A CN 202110155802A CN 112928799 B CN112928799 B CN 112928799B
Authority
CN
China
Prior art keywords
charging pile
robot
point
line
line segment
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
CN202110155802.XA
Other languages
English (en)
Other versions
CN112928799A (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.)
Beijing University of Technology
Original Assignee
Beijing University of Technology
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 Beijing University of Technology filed Critical Beijing University of Technology
Priority to CN202110155802.XA priority Critical patent/CN112928799B/zh
Publication of CN112928799A publication Critical patent/CN112928799A/zh
Application granted granted Critical
Publication of CN112928799B publication Critical patent/CN112928799B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • H—ELECTRICITY
    • H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
    • H02J—CIRCUIT ARRANGEMENTS OR SYSTEMS FOR SUPPLYING OR DISTRIBUTING ELECTRIC POWER; SYSTEMS FOR STORING ELECTRIC ENERGY
    • H02J7/00—Circuit arrangements for charging or depolarising batteries or for supplying loads from batteries
    • H02J7/0042—Circuit arrangements for charging or depolarising batteries or for supplying loads from batteries characterised by the mechanical construction
    • H02J7/0045—Circuit arrangements for charging or depolarising batteries or for supplying loads from batteries characterised by the mechanical construction concerning the insertion or the connection of the batteries
    • 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/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
    • G05D1/0242—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using non-visible light signals, e.g. IR or UV signals
    • 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
    • H—ELECTRICITY
    • H01—ELECTRIC ELEMENTS
    • H01R—ELECTRICALLY-CONDUCTIVE CONNECTIONS; STRUCTURAL ASSOCIATIONS OF A PLURALITY OF MUTUALLY-INSULATED ELECTRICAL CONNECTING ELEMENTS; COUPLING DEVICES; CURRENT COLLECTORS
    • H01R13/00—Details of coupling devices of the kinds covered by groups H01R12/70 or H01R24/00 - H01R33/00
    • H01R13/66—Structural association with built-in electrical component
    • H—ELECTRICITY
    • H01—ELECTRIC ELEMENTS
    • H01R—ELECTRICALLY-CONDUCTIVE CONNECTIONS; STRUCTURAL ASSOCIATIONS OF A PLURALITY OF MUTUALLY-INSULATED ELECTRICAL CONNECTING ELEMENTS; COUPLING DEVICES; CURRENT COLLECTORS
    • H01R13/00—Details of coupling devices of the kinds covered by groups H01R12/70 or H01R24/00 - H01R33/00
    • H01R13/66—Structural association with built-in electrical component
    • H01R13/665—Structural association with built-in electrical component with built-in electronic circuit
    • H—ELECTRICITY
    • H01—ELECTRIC ELEMENTS
    • H01R—ELECTRICALLY-CONDUCTIVE CONNECTIONS; STRUCTURAL ASSOCIATIONS OF A PLURALITY OF MUTUALLY-INSULATED ELECTRICAL CONNECTING ELEMENTS; COUPLING DEVICES; CURRENT COLLECTORS
    • H01R13/00—Details of coupling devices of the kinds covered by groups H01R12/70 or H01R24/00 - H01R33/00
    • H01R13/66—Structural association with built-in electrical component
    • H01R13/665—Structural association with built-in electrical component with built-in electronic circuit
    • H01R13/6683—Structural association with built-in electrical component with built-in electronic circuit with built-in sensor
    • H—ELECTRICITY
    • H01—ELECTRIC ELEMENTS
    • H01R—ELECTRICALLY-CONDUCTIVE CONNECTIONS; STRUCTURAL ASSOCIATIONS OF A PLURALITY OF MUTUALLY-INSULATED ELECTRICAL CONNECTING ELEMENTS; COUPLING DEVICES; CURRENT COLLECTORS
    • H01R13/00—Details of coupling devices of the kinds covered by groups H01R12/70 or H01R24/00 - H01R33/00
    • H01R13/66—Structural association with built-in electrical component
    • H01R13/665—Structural association with built-in electrical component with built-in electronic circuit
    • H01R13/6691—Structural association with built-in electrical component with built-in electronic circuit with built-in signalling means
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/60—Other road transportation technologies with climate change mitigation effect
    • Y02T10/70—Energy storage systems for electromobility, e.g. batteries

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Microelectronics & Electronic Packaging (AREA)
  • Power Engineering (AREA)
  • Electromagnetism (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种基于激光测量的移动机器人自动对接充电方法涉及激光测量和运动控制领域。具体包括以下几个步骤：激光雷达数据信息处理：使用激光传感器对充电桩周围的环境信息进行激光数据采集，对采集的激光数据信息进行断点检测‑拐点检测‑线段拟合处理，进而将激光传感器获取的充电桩周围的环境信息用几何线段拟合出来；充电桩特征识别：将顺序存储的拟合线段分组依次送入充电桩特征识别算法中，充电桩识别算法将根据充电桩的长度信息、厚度信息以及充电桩与墙两边的平行关系最终将拟合线段中充电桩所在线段提取出来；移动机器人运动控制。本发明实现移动机器人与充电桩高效、准确地对接。

Description

基于激光测量的移动机器人自动对接充电方法
技术领域
本发明涉及激光测量和运动控制领域，是一种基于激光测量的移动机器人自动对接充电方法。
背景技术
随着经济的不断发展以及社会劳动力的短缺，在餐厅、酒店以及银行等服务型场所对机器人的应用也更加广泛，但想让机器人持续不断地服务于人类，这一切都无法脱离电能的支持。只有有效解决机器人在没有人工干预下随时进行自主充电，才能扩大其活动范围并且延长其自主服务时间。因此，对机器人如何识别充电桩以及对接技术进行研究，具有重要的理论意义和工程应用价值。
目前实现移动机器人对充电桩的识别和对接的方法有多种：第一种是将设计的充电装置中放置一个光源，机器人采用跟踪光源的方法来寻找充电桩，但最终通过实验研究表明，由于导航布置较为复杂，且受光线影响较大，总体的对接失败率较高。第二种是利用视觉传感器和三维路标引导机器人实现对接充电，三维路标具有较好的定位效果，但是在近距离对接时，随着机器人距离充电桩越来越近，摄像头的视觉范围会越来越窄，设置的信标会从摄像头的直线视觉中消失，这将严重影响自动充电对接的精度，而且视觉传感器受光照的影响较大。第三种是基于红外传感器探测对接方法，首先通过***自定义红外传感器组合信号的含义，然后利用安装的红外接收管进行多个红外传感器组合信号特征识别，最后驱动机器人探索对接。红外传感器虽然成本较低，但是该传感器信息量单一，无法准确给出机器人相对充电桩的具体坐标关系，因此机器人探索式对接方式会严重降低对接效率。
发明内容
针对基于红外传感器的红外信号引导对接效率低下和基于视觉传感器的色块信标导引对接精度较低的问题，本发明提出了基于激光测量的服务机器人自动对接充电方法。本发明的前提是先利用实时定位与地图创建(Simultaneous Localization AndMapping,SLAM)技术生成室内地图，机器人在执行任务的某一时刻检测到电量不足时，利用地图匹配算法从远程回航至充电桩附近，此时充电桩在激光传感器扫描范围之内。
为实现移动机器人与充电桩精确高效对接，本发明提供的充电桩特征识别和对接的技术方案为：基于激光测量的移动机器人自动对接充电，具体包括以下几个步骤：
1)激光雷达数据信息处理：使用激光传感器对充电桩周围的环境信息进行激光数据采集，对采集的激光数据信息进行断点检测-拐点检测-线段拟合处理，进而将激光传感器获取的充电桩周围的环境信息用几何线段拟合出来；
2)充电桩特征识别：将顺序存储的拟合线段分组依次送入充电桩特征识别算法中，充电桩识别算法将根据充电桩的长度信息、厚度信息以及充电桩与墙两边的平行关系最终将拟合线段中充电桩所在线段提取出来；
3)移动机器人运动控制：找出充电桩所在的拟合线段后，根据拟合线段的特征参数可计算出充电桩的中点坐标，从而得出移动机器人与充电桩的相对位置关系，最后由本发明设计的多模态分阶段镇定控制器，根据机器人相对于充电桩的角度偏差和距离偏差划分不同的特征模态，对机器人所处的不同特征模态设计相应的控制律，从而实现移动机器人与充电桩高效、准确地对接。
步骤1：激光雷达数据信息处理
激光数据信息处理的目的是从原始激光雷达数据中提取线段特征。
1)激光数据坐标转换：移动机器人与外界环境信息之间的联系是基于直角坐标系的，应该首先对原始数据进行数据项处理，把极坐标转换为直角坐标，然后再进行数据处理运算。
2)断点检测：断点检测是通过阈值比较法在多个原始数据点中检测出不连续的数据点，并以此为断点将所有数据点分为若干点集，这些点集相互之间不具有关联性，独自作为一个区域看待，其中检测出的断点即为这些点集的起始点和终结点；
具体步骤如下：
2.1)首先将多个原始数据点视为一个点集，第一个数据点为点集的起始点，最后一个数据点为点集终结点，即得到初始点集p1((X1,Y1),(X2,Y2)…)；
2.2)然后从点集的第一个数据点开始，按照如下公式依次计算连续两点间的距离D：
Figure BDA0002934652120000031
再比较距离D与阈值
Figure BDA0002934652120000032
的关系，如果
Figure BDA0002934652120000033
那么判定这个数据点为断点，此断点将整个点集分为两部分，得到点集p1((X1,Y1)…(Xi,Yi))和点集p2((Xi+1,Yi+1)…)；
Figure BDA0002934652120000034
是阈值，选为200mm；
2.3)最后再按照同样的方法，对点集p2进行断点的分解，得到点集p2和p3；以此类推，把数据点中的所有断点检测出来，并得到互不关联的点集(p1,p2…pn)；
3)拐点检测：拐点检测通过检测每个点集中拐角点的数目，确定此点集内所能用线段拟合的最少点集数，即拐点检测就是对断点检测所得点集的进一步细分，使所得到的每个点集都可用一条线段进行拟合表示。步骤如下：
3.1)首先取出断点检测所得到的一个点集pi，以其首末点pis，pie为端点做一条直，从这个点集的开始，依次计算每个点到这条直线的距离di，同时比较这些距离值，记下距离最大的点pim，若这个最大距离值dmax大于阈值dthr，那么认为pim点为点集pi的一个拐点，可将此点集再分为两部分Li1和Li2，需要用两段不同特征参数的线段去拟合。若最大距离值dmax仍小于阈值dthr，则认为这个点集可以用一条线段表示，不需要再细分。
dthr是阈值，可以在拟合线段长度的8％-10％之间选择，本文dthr所取值是拟合线段长度的10％。
3.2)接着按照以上方法，再对点集Li1和Li2进行拐点检测，最多可得到四个点集Li1、Li2、Li3和Li4。依次类推，直至每个点集的最大距离dmax均小于阈值dthr，即每个点集均可用一条线段去拟合，则完成点集的拐点检测，此时点集pi细分成若干个可用线段拟合的点集(Li1,Li2…Lin)。
3.3)最后重复以上步骤，即可对断点检测后分割的每一个点集再进行细分，最终可得到一系列可以用一条线段完整拟合的点集(L1,L2…Ln)。
4)线段拟合：经过以上断点检测和拐点检测的步骤，可以得到一组点集(L1，L2…Ln)，其中每个点集内的激光数据点均可用一条线段进行线性拟合，从而可以提取出环境地图所需要的几何特征。本发明采用最小二乘法对激光数据点进行线性拟合，激光传感器中的数据排列方式是从右向左，因此在本发明中将拟合的线段从机器人最右边按顺序依次存储在容器line_segments[i]中，其中i＝0,1,2…n。
步骤2：充电桩特征识别
如图6在机器人坐标系下，d、d1和d2是机器人距离每条拟合线段的距离，θ、θ1和θ2是机器人正方向与每条拟合线段的法向量之间的夹角，l、l1和l2是每条拟合线段的长度。从i＝0开始每次从存储容器line_segments[i]中按顺序取三条线段line_segments[i],line_segments[i+1],line_segments[i+2],具体步骤如下：
1)首先，根据下面的公式可判断三条拟合线段是否互相平行且处于同一平面：
|θ1-θ|≤ε
|θ2-θ|≤ε
其中θ为机器人正方向与充电桩所在拟合线段(line_segments[i+1])的法向量之间的夹角，θ1和θ2分别为机器人正方向与充电桩左边墙所在拟合线段(line_segments[i+2])和充电桩右边墙所在拟合线段(line_segments[i])的法向量之间的夹角，ε为最小误差，且ε＝3°；
ε是误差值范围，可在3°-10°之间选取，为了提高精度，本文选取误差不超过3°。
2)然后，由下面公式判断三条线段是否同处在充电桩所在平面：
|d1-d-th|≤δ
|d2-d-th|≤δ
其中d为移动机器人距离充电桩所在拟合线段的距离，d1和d2分别为移动机器人距离充电桩左边墙所在拟合线段(line_segments[i+2])和右边墙所在拟合线段(line_segments[i])的距离，th为充电桩厚度，且th＝0.2m，δ为最小误差，且δ＝0.03m；
th为充电桩厚度，本文设计的充电桩厚度为0.2m,可以根据实际情况改变充电桩的厚度；
δ是误差范围，可在0.02m-0.04m之间选择。
3)最后，若三条线段的中间线line_segments[i+1]的长度l满足下式，可确定line_segments[i+1]为充电桩所在的拟线段：
|l-lactual|≤μ
其中lactual为充电桩的实际长度，且lactual＝0.43m，μ为最小误差，且μ＝0.02m；
lactual为充电桩长度，本发明设计的充电桩长度为0.43m,可以根据实际情况改变充电桩的长度，μ是误差范围，可在0.02m-0.05m之间选择。
4)如果所取的三条拟合线段不符合充电桩识别条件，则i＝i+1依次取下三条拟合线段重复步骤1)～3)，直到最终找到充电桩所在拟合线段。
移动机器人识别到充电桩后，可以根据拟合线段的特征参数确定充电桩的中点坐标pc(xc，yc)：
Figure BDA0002934652120000051
Figure BDA0002934652120000052
其中ps(xs，ys)和pe(xe，ye)分别为此拟合线段的起始坐标和终点坐标。当获取到充电桩中点坐标后，利用ROS将中点坐标发布给多模态分阶段镇定控制器，最终由控制器驱动机器人进行下一步的对接。
步骤3：移动机器人对接充电桩的运动控制
本发明在移动机器人与充电桩对接过程中设计了多模态分阶段镇定控制器，根据机器人在对接过程中与充电桩的角度偏差和距离偏差来划分不同的特征模态，然后对其进行相应的控制参数的设计，使得机器人根据不同特征模态选择相应的控制策略，从而提高服务机器人的运动控制效率，节省对接时间。
根据两轮差分运动学常识可知，本发明中的移动机器人无法实现全向运动，因此将移动机器人从初始位置运动到期望位置的过程划分为姿态调整控制和位置跟踪控制。步骤如下：
1)t0时刻为机器人初始状态，从t0时刻至t1时刻为纯旋转控制，使得机器人正方向垂直于充电桩，这样可通过激光传感器实时获取到移动机器人与充电桩正前方的距离差；
2)t1时刻移动机器人逆时针旋转90°至t2时刻，并直线行驶使得机器人与充电桩正前方的距离差趋于0；
3)t3时刻移动机器人顺时针旋转90°使得机器人正方向垂直于充电桩；由于充电触片位于充电桩背部，因此在充电桩正前方开启直线对接过程中需考虑尾部进行对接，即t3时刻机器人向前直线对接，在距离充电桩40cm处逆时针旋转180°至t4时刻；
40cm是本发明选取的距离长度，可在30cm-60cm之间选取。
4)t4时刻机器人开始缓慢后退接触充电桩直至与充电桩完全接触开启充电。
若机器人在对接过程中对接失败，则移动机器人重新回到对接起点进行充电桩识别，重复1)～4)开启新一轮的对接，直至对接成功。
本发明中调整移动机器人的姿态误差角dθ时，是根据机器人在全局世界坐标系下当前的姿态角θc与下一位置的期望姿态角θe得到姿态误差dθ＝θe-θc，然后通过分段镇定控制器中的旋转控制器如下公式使得dθ趋近于0：
Figure BDA0002934652120000061
其中v表示机器人的线速度，kω>0，为了提高对接效率和准确率，本发明在不同模态kω的值不同，在t0时刻机器人姿态误差角比较小，为了防止调整过度，kω值选取较小，即kω＝1.0；在t1和t3时刻姿态误差角较大，为了实现快速缩小误差，kω值选取较大，即kω＝3.0。
本发明中在缩小移动机器人的距离误差dd时设计位置跟踪控制器，在位置跟踪过程中定义服务机器人坐标系(xc，yc)下的位置误差为(dx，dy)T，则位置误差方程为：
Figure BDA0002934652120000062
其中，(xe，ye)为期望位置，根据位置误差(dx，dy)T可以得到当前位置与期望位置的距离差dd＝((dx)2+(dy)2)1/2，通过下面的位置跟踪控制器使得dd趋近于0：
Figure BDA0002934652120000063
其中kv>0，同样为了提高对接效率和准确率，本发明在不同模态kv的值不同，在t1时刻距离误差较大，为了实现快速缩小误差，kv值选取较大，即kv＝1.5；在t3和t4时刻距离误差较小，为了较小震荡，kv值选取较小，即kv＝0.5。
为验证本发明所述方法的有效性，本发明中引入ROS的Rviz(可视化界面)来直观展示充电桩的识别结果，激光传感器可以把所有的环境信息呈现出来，并且利用线段拟合算法可将周围环境信息利用线段特征表达出来；利用充电桩识别算法找到充电桩中点坐标后，将充电桩中点坐标以Marker的形式发布出去，最后在Rviz下订阅Marker信息；进行大量实验可得出，利用多模态分阶段镇定控制器驱动机器人对接过程中，移动机器人从同一起点运动至不同的目标点以及从不同起点到达同一目标点，其误差均在可接受范围之内，且对接成功率可达到93.54％，对接精度可达到95％。
本发明与现有技术相比具有如下有益的结果：
1、相较组合红外传感器识别充电桩，使用激光传感器识别充电桩后可得到机器人与充电桩之间精确的角度偏差和距离偏差，因此由运动控制算法可以更加精确的驱动移动机器人与充电桩的对接；
2、采用激光传感器采集充电桩周围的环境信息并进行数据处理量远远小于使用视觉传感器识别充电桩的数据处理量，并且激光传感器受光照影响度远小于视觉传感器；
3、根据充电桩的物理模型(长度，厚度以及与墙两边的平行关系)进行识别，提高识别的实时性与准确性；
4、相较于使用机器人操作***(Robot Operating System,ROS)提供的move_base导航功能包根据发送的中点坐标导航至充电桩进行对接，本发明设计的多模态分阶段镇定控制器可根据机器人当前相对于充电桩的角度偏差和距离偏差进行实时调整当前位姿，从而提高对接精度以及对接效率；
5、本发明的自动对接***采用ROS集成的状态机框架，将复杂的对接任务模块化，并且通过状态机提供的可视化工具可以随时查看各级任务之间的跳转以及数据流传递等信息。
附图说明
图1为基于激光测量的移动机器人自动对接算法框架图
图2为本发明的总体流程图
图3为激光数据坐标转换示意图
图4为断点检测流程图
图5为拐点检测示意图
图6为机器人坐标系下拟合线段的特征参数示意图
图7为多模态分阶段镇定控制流程图
图8为移动机器人对接充电桩过程示意图
图9为自动充电对接流程图
具体实施方式
下面结合附图和具体实施例对本发明作详细说明。
本发明提出基于激光测量的移动机器人自动对接充电方法，算法框架如图1所示，具体包括以下步骤：
更具体的：
1)激光数据坐标转换：移动机器人与外界环境信息之间的联系是基于直角坐标系的，应该首先对原始数据进行数据项处理，把极坐标转换为直角坐标，然后再进行数据处理运算；采集的数据点个数为多个，计算出第i个数据点对应的极坐标系下的角度θi：
然后通过以下公式将极坐标(ρi，θi)转化为直角坐标(Xi，Yi)；
Xi＝ρi×cosθi
Yi＝ρi×sinθi
图2为激光数据坐标转换示意图。例子中使用的是UST-10LX激光传感器，它在270°范围内采集的数据点个数为1080个，可以计算出第i个数据点对应的极坐标系下的角度θi：
Figure BDA0002934652120000081
然后通过以下公式将极坐标(ρi，θi)转化为直角坐标(Xi,Yi)。
Xi＝ρi×cosθi
Yi＝ρi×sinθi
270°，1080说明：270°是作者实验室所用激光能够扫描的范围，其他激光产品也有360°的扫描范围，1080是作者实验室所用激光能采集的激光点个数，不同激光产品能采集的激光点个数不同。
2)断点检测：断点检测是通过阈值比较法在多个原始数据点中检测出不连续的数据点，并以此为断点将所有数据点分为若干点集，这些点集相互之间不具有关联性，可以独自作为一个区域看待，其中检测出的断点即为这些点集的起始点和终结点。图4为断点检测程序流程图。
步骤如下：
2.1)首先将1080个原始数据点视为一个点集，第一个数据点为点集的起始点，最后一个数据点为点集终结点，即得到初始点集p1((X1,Y1),(X2,Y2)…(X1080,Y1080))。
2.2)然后从点集的第一个数据点开始，按照如下公式依次计算连续两点间的距离D：
Figure BDA0002934652120000091
再比较距离D与阈值
Figure BDA0002934652120000092
的关系，如果
Figure BDA0002934652120000093
那么判定这个数据点为断点，此断点将整个点集分为两部分，得到点集p1((X1，Y1)…(Xi,Yi))和点集p2((Xi+1，Yi+1)…(X1080，Y1080))。
2.3)最后再按照同样的方法，可对点集p2进行断点的分解，得到点集p2和p3。以此类推，即可把数据点中的所有断点检测出来，并得到互不关联的点集(p1，p2…pn)。在断点检测过程中，可以通过判断孤立点集内的激光点个数并结合充电桩的实际长度来剔除部分点集，从而减少数据处理运算量。
Figure BDA0002934652120000094
是阈值，可以在150mm-350mm之间选择，
Figure BDA0002934652120000095
越大则线段拟合个数越少，
Figure BDA0002934652120000096
越小则拟合线段越多，本文最终使用的阈值为200mm。
3)拐点检测：拐点检测通过检测每个点集中拐角点的数目，确定此点集内所能用线段拟合的最少点集数，即拐点检测就是对断点检测所得点集的进一步细分，使所得到的每个点集都可用一条线段进行拟合表示。图5为拐点检测示意图，步骤如下：
3.1)首先取出断点检测所得到的一个点集pi，以其首末点pis，pie为端点做一条直，从这个点集的开始，依次计算每个点到这条直线的距离di，同时比较这些距离值，记下距离最大的点pim，若这个最大距离值dmax大于阈值dthr，那么认为pim点为点集pi的一个拐点，可将此点集再分为两部分Li1和Li2，需要用两端不同特征参数的线段去拟合。若最大距离值dmax仍小于阈值dthr，则认为这个点集可以用一条线段表示，不需要再细分。dthr是阈值，可以在拟合线段长度的8％-10％之间选择，本文dthr所取值是拟合线段长度的10％。
3.2)接着按照以上方法，再对点集Li1和Li2进行拐点检测，最多可得到四个点集Li1、Li2、Li3和Li4。依次类推，直至每个点集的最大距离dmax均小于阈值dthr，即每个点集均可用一条线段去拟合，则完成点集的拐点检测，此时点集pi细分成若干个可用线段拟合的点集(Li1，Li2…Lin)。
3.3)最后重复以上步骤，即可对断点检测后分割的每一个点集再进行细分，最终可得到一系列可以用一条线段完整拟合的点集(L1，L2…Ln)。
4)线段拟合：经过以上断点检测和拐点检测的步骤，可以得到一组点集(L1，L2…Ln)，其中每个点集内的激光数据点均可用一条线段进行线性拟合，从而可以提取出环境地图所需要的几何特征。本发明采用最小二乘法对激光数据点进行线性拟合，激光传感器中的数据排列方式是从右向左，因此在本发明中将拟合的线段从机器人最右边按顺序依次存储在容器line_segments[i]中，其中i＝0,1,2…n。
步骤2：充电桩特征识别
如图6，在机器人坐标系下，d、d1和d2是机器人距离每条拟合线段的距离，θ、θ1和θ2是机器人正方向与每条拟合线段的法向量之间的夹角，l、l1和l2是每条拟合线段的长度。从i＝0开始每次从存储容器line_segments[i]中按顺序取三条线段line_segments[i],line_segments[i+1],line_segments[i+2],具体步骤如下：
1)首先，根据下面的公式可判断三条拟合线段是否互相平行且处于同一平面：
|θ1-θ|≤ε
|θ2-θ|≤ε
其中θ为机器人正方向与充电桩所在拟合线段(line_segments[i+1])的法向量之间的夹角，θ1和θ2分别为机器人正方向与充电桩左边墙所在拟合线段(line_segments[i+2])和充电桩右边墙所在拟合线段(line_segments[i])的法向量之间的夹角，ε为最小误差，且ε＝3°；
2)然后，由下面公式判断三条线段是否同处在充电桩所在平面：
|d1-d-th|≤δ
|d2-d-th|≤δ
其中d为移动机器人距离充电桩所在拟合线段的距离，d1和d2分别为移动机器人距离充电桩左边墙所在拟合线段(line_segments[i+2])和右边墙所在拟合线段(line_segments[i])的距离，th为充电桩厚度，且th＝0.2m，δ为最小误差，且δ＝0.03m；
3)最后，若三条线段的中间线line_segments[i+1]的长度l满足下式，可确定line_segments[i+1]为充电桩所在的拟线段：
|l-lactual|≤μ
其中lactual为充电桩的实际长度，且lactual＝0.43m，μ为最小误差，且μ＝0.03m；
4)如果所取的三条拟合线段不符合充电桩识别条件，则i＝i+1依次取下三条拟合线段重复步骤1)～3)，直到最终找到充电桩所在拟合线段。
移动机器人识别到充电桩后，可以根据拟合线段的特征参数确定充电桩的中点坐标pc(xc，yc)：
Figure BDA0002934652120000111
Figure BDA0002934652120000112
其中ps(xs，ys)和pe(xe，ye)分别为此拟合线段的起始坐标和终点坐标。当获取到充电桩中点坐标后，利用ROS将中点坐标发布给多模态分阶段镇定控制器，最终由控制器驱动机器人进行下一步的对接。
步骤3：移动机器人对接充电桩的运动控制
本发明在移动机器人与充电桩对接过程中设计了多模态分阶段镇定控制器，根据机器人在对接过程中与充电桩的角度偏差和距离偏差来划分不同的特征模态，然后对其进行相应的控制参数的设计，使得机器人根据不同特征模态选择相应的控制策略，从而提高服务机器人的运动控制效率，节省对接时间，多模态分阶段镇定控制流程如图7。
根据两轮差分运动学常识可知，本发明中的移动机器人无法实现全向运动，因此将移动机器人从初始位置运动到期望位置的过程划分为姿态调整控制和位置跟踪控制。图8为分阶段镇定对接过程示意图，步骤如下：
1)t0时刻为机器人初始状态，从t0时刻至t1时刻为纯旋转控制，使得机器人正方向垂直于充电桩，这样可通过激光传感器实时获取到移动机器人与充电桩正前方的距离差；
2)t1时刻移动机器人逆时针旋转90°至t2时刻，并直线行驶使得机器人与充电桩正前方的距离差趋于0；
3)t3时刻移动机器人顺时针旋转90°使得机器人正方向垂直于充电桩；由于充电触片位于充电桩背部，因此在充电桩正前方开启直线对接过程中需考虑尾部进行对接，即t3时刻机器人向前直线对接，在距离充电桩40cm处逆时针旋转180°至t4时刻；
4)t4时刻机器人开始缓慢后退接触充电桩直至与充电桩完全接触开启充电。
若机器人在对接过程中对接失败，则移动机器人重新回到对接起点进行充电桩识别，重复1)～4)开启新一轮的对接，直至对接成功。
本发明中调整移动机器人的姿态误差角dθ时，是根据机器人在全局世界坐标系下当前的姿态角θc与下一位置的期望姿态角θe得到姿态误差dθ＝θe-θc，然后通过分段镇定控制器中的旋转控制器如下公式使得dθ趋近于0：
Figure BDA0002934652120000121
其中v表示机器人的线速度，kω>0，为了提高对接效率和准确率，本发明在不同模态kω的值不同，在t0时刻机器人姿态误差角比较小，为了防止调整过度，kω值选取较小，即kω＝1.0；在t1和t3时刻姿态误差角较大，为了实现快速缩小误差，kω值选取较大，即kω＝3.0。
本发明中在缩小移动机器人的距离误差dd时设计位置跟踪控制器，在位置跟踪过程中定义服务机器人坐标系(xc,yc)下的位置误差为(dx,dy)T，则位置误差方程为：
Figure BDA0002934652120000122
其中，(xe,ye)为期望位置，根据位置误差(dx,dy)T可以得到当前位置与期望位置的距离差dd＝((dx)2+(dy)2)1/2，通过下面的位置跟踪控制器使得dd趋近于0：
Figure BDA0002934652120000131
其中kv>0，同样为了提高对接效率和准确率，本发明在不同模态kv的值不同，在t1时刻距离误差较大，为了实现快速缩小误差，kv值选取较大，即kv＝1.5；在t3和t4时刻距离误差较小，为了较小震荡，kv值选取较小，即kv＝0.5。
为验证本发明所述方法的有效性，本发明中引入ROS的Rviz(可视化界面)来直观展示充电桩的识别结果，激光传感器可以把所有的环境信息呈现出来，并且利用线段拟合算法可将周围环境信息利用线段特征表达出来；利用充电桩识别算法找到充电桩中点坐标后，将充电桩中点坐标以Marker的形式发布出去，最后在Rviz下订阅Marker信息；进行大量实验可得出，利用多模态分阶段镇定控制器驱动机器人对接过程中，移动机器人从同一起点运动至不同的目标点以及从不同起点到达同一目标点，其误差均在可接受范围之内，且对接成功率可达到93.54％，对接精度可达到95％。

Claims (1)

1.基于激光测量的移动机器人自动对接充电方法，其特征在于包括以下步骤：
步骤1：激光雷达数据信息处理
1)激光数据坐标转换：移动机器人与外界环境信息之间的联系是基于直角坐标系的，应该首先对原始数据进行数据项处理，把极坐标转换为直角坐标，然后再进行数据处理运算；采集的数据点个数为多个，计算出第i个数据点对应的极坐标系下的角度θi：
然后通过以下公式将极坐标(ρi，θi)转化为直角坐标(Xi，Yi)；
Xi＝ρi×cosθi
Yi＝ρi×sinθi
2)断点检测：断点检测是通过阈值比较法在多个原始数据点中检测出不连续的数据点，并以此为断点将所有数据点分为若干点集，这些点集相互之间不具有关联性，独自作为一个区域看待，其中检测出的断点即为这些点集的起始点和终结点；
具体步骤如下：
2.1)首先将多个原始数据点视为一个点集，第一个数据点为点集的起始点，最后一个数据点为点集终结点，即得到初始点集p1((X1，Y1)，(X2，Y2)...)；
2.2)然后从点集的第一个数据点开始，按照如下公式依次计算连续两点间的距离D：
Figure FDA0002934652110000011
再比较距离D与阈值
Figure FDA0002934652110000012
的关系，如果
Figure FDA0002934652110000013
那么判定这个数据点为断点，此断点将整个点集分为两部分，得到点集p1((X1，Y1)...(Xi，Yi))和点集p2((Xi+1，Yi+1)...)；
Figure FDA0002934652110000014
是阈值，选为200mm；
2.3)最后再按照同样的方法，对点集p2进行断点的分解，得到点集p2和p3；以此类推，把数据点中的所有断点检测出来，并得到互不关联的点集(p1，p2...pn)；
3)拐点检测：拐点检测通过检测每个点集中拐角点的数目，确定此点集内所能用线段拟合的最少点集数，即拐点检测就是对断点检测所得点集的进一步细分，使所得到的每个点集都用一条线段进行拟合表示；
步骤如下：
3.1)首先取出断点检测所得到的一个点集pi，以其首末点pis，pie为端点做一条直线 ，从这个点集的开始，依次计算每个点到这条直线的距离di，同时比较这些距离值，记下距离最大的点pim，若这个最大距离值dmax大于阈值dthr，那么认为pim点为点集pi的一个拐点，将此点集再分为两部分Li1和Li2，需要用两端不同特征参数的线段去拟合；若最大距离值dmax仍小于阈值dthr，则认为这个点集用一条线段表示，不需要再细分；
dthr是阈值，dthr取拟合线段长度的10％；
3.2)接着按照以上方法，再对点集Li1和Li2进行拐点检测，最多得到四个点集Li1、Li2、Li3和Li4；依次类推，直至每个点集的最大距离dmax均小于阈值dthr，即每个点集均用一条线段去拟合，则完成点集的拐点检测，此时点集pi细分成若干个用线段拟合的点集(Li1，Li2...Lin)；
3.3)最后重复以上步骤，即对断点检测后分割的每一个点集再进行细分，最终得到一系列用一条线段完整拟合的点集(L1，L2...Ln)；
4)线段拟合：经过以上断点检测和拐点检测的步骤，得到一组点集(L1，L2...Ln)，其中每个点集内的激光数据点均用一条线段进行线性拟合，从而提取出环境地图所需要的几何特征；采用最小二乘法对激光数据点进行线性拟合，激光传感器中的数据排列方式是从右向左，因此将拟合的线段从机器人最右边按顺序依次存储在容器line_segments[i]中，其中i＝0，1，2...n；
步骤2：充电桩特征识别
在机器人坐标系下，d、d1和d2是机器人距离每条拟合线段的距离，θ、θ1和θ2是机器人正方向与每条拟合线段的法向量之间的夹角，l、l1和l2是每条拟合线段的长度；从i＝0开始每次从存储容器line_segments[i]中按顺序取三条线段line_segments[i]，line_segments[i+1]，line_segments[i+2]，具体步骤如下：
1)首先，根据下面的公式判断三条拟合线段是否互相平行且处于同一平面：
|θ1-θ|≤ε
|θ2-θ|≤ε
其中θ为机器人正方向与充电桩所在拟合线段(line_segments[i+1])的法向量之间的夹角，θ1和θ2分别为机器人正方向与充电桩左边墙所在拟合线段(line_segments[i+2])和充电桩右边墙所在拟合线段(line_segments[i])的法向量之间的夹角，ε为最小误差，不超过3°；
2)然后，由下面公式判断三条线段是否同处在充电桩所在平面：
|d1-d-th|≤δ
|d2-d-th|≤δ
其中d为移动机器人距离充电桩所在拟合线段的距离，d1和d2分别为移动机器人距离充电桩左边墙所在拟合线段(line_segments[i+2])和右边墙所在拟合线段(line_segments[i])的距离，th为充电桩厚度，且th＝0.2m，δ为最小误差，且δ＝0.03m；
3)最后，若三条线段的中间线line_segments[i+1]的长度l满足下式，确定line_segments[i+1]为充电桩所在的拟线段：
|l-lactual|≤μ
其中lactual为充电桩的实际长度，且lactual＝0.43m，μ为最小误差，且μ＝0.02m；
4)如果所取的三条拟合线段不符合充电桩识别条件，则i＝i+1依次取下三条拟合线段重复步骤1)～3)，直到最终找到充电桩所在拟合线段；
移动机器人识别到充电桩后，根据拟合线段的特征参数确定充电桩的中点坐标pc(xc，yc)：
Figure FDA0002934652110000031
Figure FDA0002934652110000032
其中ps(xs，ys)和pe(xe，ye)分别为此拟合线段的起始坐标和终点坐标；当获取到充电桩中点坐标后，利用ROS将中点坐标发布给多模态分阶段镇定控制器，最终由控制器驱动机器人进行下一步的对接；
步骤3：移动机器人对接充电桩的运动控制
在移动机器人与充电桩对接过程中设计了多模态分阶段镇定控制器，根据机器人在对接过程中与充电桩的角度偏差和距离偏差来划分不同的特征模态，然后对其进行相应的控制参数的设计，使得机器人根据不同特征模态选择相应的控制策略；
根据两轮差分运动学常识，将移动机器人从初始位置运动到期望位置的过程划分为姿态调整控制和位置跟踪控制；步骤如下：
1)t0时刻为机器人初始状态，从t0时刻至t1时刻为纯旋转控制，使得机器人正方向垂直于充电桩，这样通过激光传感器实时获取到移动机器人与充电桩正前方的距离差；
2)t1时刻移动机器人逆时针旋转90°至t2时刻，并直线行驶使得机器人与充电桩正前方的距离差趋于0；
3)t3时刻移动机器人顺时针旋转90°使得机器人正方向垂直于充电桩；由于充电触片位于充电桩背部，因此在充电桩正前方开启直线对接过程中需考虑尾部进行对接，即t3时刻机器人向前直线对接，在距离充电桩40cm处逆时针旋转180°至t4时刻；
4)t4时刻机器人开始后退接触充电桩直至与充电桩完全接触开启充电；
若机器人在对接过程中对接失败，则移动机器人重新回到对接起点进行充电桩识别，重复1)～4)开启新一轮的对接，直至对接成功；
调整移动机器人的姿态误差角dθ时，是根据机器人在全局世界坐标系下当前的姿态角θc与下一位置的期望姿态角θe得到姿态误差dθ＝θe-θc，然后通过分段镇定控制器中的旋转控制器如下公式使得dθ趋近于0：
Figure FDA0002934652110000041
其中v表示机器人的线速度，kω＞0，为了提高对接效率和准确率，在不同模态kω的值不同，在t0时刻kω＝1.0；在t1和t3时刻kω＝3.0；
在缩小移动机器人的距离误差dd时设计位置跟踪控制器，在位置跟踪过程中定义服务机器人坐标系(xc，yc)下的位置误差为(dx，dy)T，则位置误差方程为：
Figure FDA0002934652110000042
其中，(xe，ye)为期望位置，根据位置误差(dx，dy)T得到当前位置与期望位置的距离差dd＝((dx)2+(dy)2)1/2，通过下面的位置跟踪控制器使得dd趋近于0：
Figure FDA0002934652110000043
其中kv＞0，在t1时刻距离误差较大，kv＝1.5；在t3和t4时刻kv＝0.5。
CN202110155802.XA 2021-02-04 2021-02-04 基于激光测量的移动机器人自动对接充电方法 Active CN112928799B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110155802.XA CN112928799B (zh) 2021-02-04 2021-02-04 基于激光测量的移动机器人自动对接充电方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110155802.XA CN112928799B (zh) 2021-02-04 2021-02-04 基于激光测量的移动机器人自动对接充电方法

Publications (2)

Publication Number Publication Date
CN112928799A CN112928799A (zh) 2021-06-08
CN112928799B true CN112928799B (zh) 2022-08-09

Family

ID=76170329

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110155802.XA Active CN112928799B (zh) 2021-02-04 2021-02-04 基于激光测量的移动机器人自动对接充电方法

Country Status (1)

Country Link
CN (1) CN112928799B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113311844B (zh) * 2021-07-28 2021-11-12 福勤智能科技（昆山）有限公司 一种伺服控制方法、装置、计算机设备及存储介质
CN113541265B (zh) * 2021-07-29 2022-07-29 上海术理智能科技有限公司 用于机器人的自动充电***及充电桩定位导航方法
CN113485357A (zh) * 2021-07-29 2021-10-08 山东新一代信息产业技术研究院有限公司 一种室内机器人自动回桩充电***及方法
CN115700419A (zh) * 2021-07-30 2023-02-07 深圳市普渡科技有限公司 机器人及其自动回充方法、控制装置、存储介质
CN115561730B (zh) * 2022-11-11 2023-03-17 湖北工业大学 基于激光雷达特征识别的定位导航方法
CN115816416A (zh) * 2022-12-15 2023-03-21 锐趣科技(北京)有限公司 一种对接机器人及其对接方法

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103268729B (zh) * 2013-05-22 2015-08-19 北京工业大学 基于混合特征的移动机器人级联式地图创建方法
CN104635730B (zh) * 2013-11-13 2017-07-28 沈阳新松机器人自动化股份有限公司 一种机器人自主充电方法
WO2018176358A1 (en) * 2017-03-31 2018-10-04 Intel Corporation Autonomous tunnel navigation with a robotic system
WO2019190395A1 (en) * 2018-03-28 2019-10-03 Agency For Science, Technology And Research Method and system for returning a displaced autonomous mobile robot to its navigational path
CN109755995B (zh) * 2018-12-14 2020-07-28 华南理工大学 基于ros机器人操作***的机器人自动充电对接方法
CN109696168A (zh) * 2019-02-27 2019-04-30 沈阳建筑大学 一种基于激光传感器的移动机器人几何地图创建方法
CN112217248A (zh) * 2020-09-02 2021-01-12 创新工场(北京)企业管理股份有限公司 充电桩、用于移动机器人的自主充电的方法和装置
CN112247988A (zh) * 2020-09-29 2021-01-22 南京理工大学 基于激光雷达对移动机器人自主充电的方法

Also Published As

Publication number Publication date
CN112928799A (zh) 2021-06-08

Similar Documents

Publication Publication Date Title
CN112928799B (zh) 基于激光测量的移动机器人自动对接充电方法
Yang et al. Cubeslam: Monocular 3-d object slam
CN107239076B (zh) 基于虚拟扫描与测距匹配的agv激光slam方法
Lingemann et al. High-speed laser localization for mobile robots
CN106548486B (zh) 一种基于稀疏视觉特征地图的无人车位置跟踪方法
CN112183133B (zh) 一种基于ArUco码引导的移动机器人自主充电方法
CN110874100A (zh) 用于使用视觉稀疏地图进行自主导航的***和方法
CN111693050B (zh) 基于建筑信息模型的室内中大型机器人导航方法
CN105759829A (zh) 基于激光雷达的微型无人机操控方法及***
CN112819943B (zh) 一种基于全景相机的主动视觉slam***
EP3850456A1 (en) Control and navigation systems, pose optimisation, mapping, and localisation techniques
Dall’Osto et al. Fast and robust bio-inspired teach and repeat navigation
CN114034299B (zh) 一种基于主动激光slam的导航***
US20230419531A1 (en) Apparatus and method for measuring, inspecting or machining objects
CN113721639B (zh) 一种用于小型水下机器人入坞的路径规划方法及控制方法
CN114593739A (zh) 基于视觉检测与参考线匹配的车辆全局定位方法及装置
Li et al. A mobile robotic arm grasping system with autonomous navigation and object detection
CN113674310A (zh) 一种基于主动视觉感知的四旋翼无人机目标跟踪方法
CN113741550B (zh) 移动机器人跟随方法和***
CN115690343A (zh) 一种基于视觉跟随的机器人激光雷达扫描建图方法
Roggeman et al. Embedded vision-based localization and model predictive control for autonomous exploration
Wang et al. A survey of simultaneous localization and mapping on unstructured lunar complex environment
CN116127405A (zh) 一种融合点云地图、运动模型和局部特征的位置识别方法
Han et al. Mobile robot navigation by circular path planning algorithm using camera and ultrasonic sensor
Zhang et al. Triangle codes and tracer lights based absolute positioning method for terminal visual docking of autonomous underwater vehicles

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