CN111596654A - 基于改进d星路径规划算法的电缆沟机器人导航避障方法 - Google Patents

基于改进d星路径规划算法的电缆沟机器人导航避障方法 Download PDF

Info

Publication number
CN111596654A
CN111596654A CN202010306134.1A CN202010306134A CN111596654A CN 111596654 A CN111596654 A CN 111596654A CN 202010306134 A CN202010306134 A CN 202010306134A CN 111596654 A CN111596654 A CN 111596654A
Authority
CN
China
Prior art keywords
robot
cable trench
path
algorithm
coordinate system
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.)
Granted
Application number
CN202010306134.1A
Other languages
English (en)
Other versions
CN111596654B (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.)
Zhuzhou Power Supply Branch Of State Grid Hunan Electric Power Co ltd
State Grid Corp of China SGCC
State Grid Hunan Electric Power Co Ltd
Changsha University of Science and Technology
Original Assignee
Zhuzhou Power Supply Branch Of State Grid Hunan Electric Power Co ltd
State Grid Corp of China SGCC
State Grid Hunan Electric Power Co Ltd
Changsha University of Science and 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 Zhuzhou Power Supply Branch Of State Grid Hunan Electric Power Co ltd, State Grid Corp of China SGCC, State Grid Hunan Electric Power Co Ltd, Changsha University of Science and Technology filed Critical Zhuzhou Power Supply Branch Of State Grid Hunan Electric Power Co ltd
Priority to CN202010306134.1A priority Critical patent/CN111596654B/zh
Publication of CN111596654A publication Critical patent/CN111596654A/zh
Application granted granted Critical
Publication of CN111596654B publication Critical patent/CN111596654B/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/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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
    • 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/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

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

Abstract

本发明提供了一种基于改进D星路径规划算法的电缆沟机器人导航避障方法，该导航避障方法通过对电缆沟机器人进行数学建模和对D星路径规划算法进行改进，并对重要转折点之间的路径进行了平滑处理，确保机器人采用最短路径行走巡检时，与沟内的障碍维持安全距离，使得电缆沟机器人进行圆弧移动时更好的在电缆沟内部自主导航避障，从而能够避开电缆沟环境内的石头等诸多障碍，并提高巡检效率。

Description

基于改进D星路径规划算法的电缆沟机器人导航避障方法
技术领域
本发明属于机器人导航领域，更具体地，涉及一种基于改进D星(即D*Lite)路径规划算法的电缆沟机器人导航避障方法。
背景技术
地下电缆沟作为地下输配电网重要组成部分之一，是电网建设中必不可少的设施。沟内电力电缆长时间运行会导致电缆外绝缘老化放电，加上电缆沟中一些动植物尸体经过微生物分解产生的甲烷等可燃性气体积累，最终可能引起地下电缆沟发生火灾爆炸。由于地下电缆沟内部环境狭小复杂，工作人员巡检尤为困难。因此，使用机器人代替人工加强对地下电缆沟的巡检，对尽早发现地下电缆沟内部隐患，降低地下电缆故障率，提高电网运行的稳定性有着至关重要的意义。
但是，由于地下电缆沟内部环境狭小复杂，仅使用D星(即D*Lite)路径规划算法作为电缆沟机器人自主导航避障方法时，规划出的路径并不平滑，电缆沟机器人面对静态环境时规划的预设路径与障碍物非常容易接近，很可能会引发碰撞，且环境发生动态变化时，更是大大增加了碰撞的风险。
因此，亟需设计一种具备抗干扰强、针对电缆隧道复杂环境、稳定性好、精度较高的电缆沟机器人自动导航避障方法，以使得电缆沟机器人在电缆隧道复杂环境中自如的行走。
发明内容
(一)要解决的技术问题
基于上述背景技术，本发明设计了一种基于改进D星路径规划算法的电缆沟机器人导航避障方法，该方法可以在多种障碍物多的场景中应用，解决了D星路径规划算法频繁转弯且导航路径过于接近障碍物的问题，便于使得搭载外形较大的设备和机械臂的电缆沟机器人等进行导航，且不与环境中的障碍物产生碰撞。
(二)技术方案
本发明公开了一种基于改进D星路径规划算法的电缆沟机器人导航避障方法，该方法包括以下步骤：
步骤1：通过电缆沟机器人的本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息；
步骤2：基于步骤1中采集的信息建立电缆沟机器人的数学模型，并用SLAM算法在自身定位的基础上建立环境地图；
步骤3：电缆沟机器人通过路径规划算法得到一条最优路径，使机器人按照最优路径移动到目标位置；所述步骤3中具体包括以下步骤：
步骤3.1：获取环境信息后，规划最优路径；在所述规划最优路径时，改进D星路径规划算法加入演化算法中的映射操作，让电缆沟机器人可以对电缆沟内规划行走区域的栅格进行安全判定，模运算映射规则见下式：
Figure BDA0002455855450000031
其中，
Figure BDA0002455855450000032
为在k维度平面上第i个超出边界烟花的位置，
Figure BDA0002455855450000033
和
Figure BDA0002455855450000034
为在k维度平面上的上下边界值，％为模运算符号；
步骤3.2：选择重要转折点，在使用D星路径规划算法的过程中，从第二个路径点开始，如果此路径点的方向和前一个节点的父节点相同，则认为此路径点为重复，将此路径点删除并重新规划路径点组合，按这个方法规划所有路径节点，得到只有起点、转折点和终点的组合，称为重要转折点；
步骤3.3：构建烟花算法适应度函数，适应度函数的选择会影响算法的收敛速度以及是否可以找到近似最优解，考虑到机器人行走路径的长度，用下式(10)来表达路径的代价值：
Figure BDA0002455855450000035
其中，pij为从i节点到j节点线段的代价值；下式(11)作为个体适应度函数
Figure BDA0002455855450000036
其中，Pmax是一个相对较大且合适的常数；
步骤3.4：使用烟花算法二次规划重要转折点之间的路径，基于适应度函数生成全局最优路径作为最短路径；
步骤3.5：电缆沟机器人做出决策，按照最短路径出发行走，移动到下一节点并指定该节点为起始点。
进一步的，所述步骤3.4中具体包括：使用两相邻的重要转折点作为路径的起点和终点，并再次栅格化两点之间的空间，通过爆炸算子使烟花爆炸，生成N个烟花，每个烟花代表一条起点与终点之间的连线，然后使用适应度函数算出所有烟花的适应度，并重复该过程直到找到终点，通过烟花算法中的烟花的爆炸、火花的选择，即可在两个相邻的重要转折点之间生成近似最佳解，所有的重要转折点之间路径优化过后，即生成全局最优路径作为最短路径。
进一步的，所述步骤2中具体包括以下步骤：
步骤2.1：进行电缆沟机器人的坐标变换；
通过建立地下电缆沟状态监测机器人全局坐标系和局部坐标系之间的关系，以描述地下电缆沟状态监测机器人在二维空间中的位姿，其中{Xt,Yt}为全局坐标系，原点以运动起点机器人的第一关节几何中心定义，Xt为起点时机器人的正前方向，Yt为起点时机器人车体第一关节垂直方向，{Xr,Yr}为局部坐标系，原点以机器人第一关节当前位置几何中心定义，Xr为当前机器人的正前方向，Yr为当前机器人车体第一关节垂直方向，在全局坐标系下，电缆沟机器人的位置为(x,y)，β为机器人的偏航角，由位姿以及全局坐标系与局部坐标系旋转角度差得出最终全局坐标系和局部坐标系映射公式如下式(2)：
Figure BDA0002455855450000051
步骤2.2：进行电缆沟机器人的航迹推理，并得到电缆沟机器人的数学模型；设在t时刻状态监测机器人的相对于当前机器人的正前方向的前进角度为θt，线速度为vt，角速度为ωt，规定机器人前进时vt为正，后退时vt为负；机器人逆时针转动时ωt为正，顺时针旋转时ωt为负，则可以求出全局坐标系下地下电缆沟状态监测机器人的位姿增量为
Figure BDA0002455855450000052
此时，电缆沟机器人在全局坐标系下做近似圆弧运动。
进一步的，所述步骤2中还包括以下步骤2.3：
步骤2.3：在使用SLAM算法的基础上，采用自适应蒙特卡罗定位算法对机器人自身进行自定位，并使用自适应蒙特卡洛定位算法输出定位坐标；其中，电缆沟机器人采用的自适应蒙特卡洛定位算法包括：
步骤一：进行初始化离散工作后，检测路标，生产路标的不变性表示，并测量距离和角度。
步骤二：驱动电缆沟机器人运动，并根据运动模型估计下一运动，检测路标，计算机器人的相对位置，根据预测值和观测值计算各粒子的偏差；
步骤三：对粒子进行跟踪后，判断粒子是否集中，如果是，则结束定位，如果否，则回到步骤二。
进一步的，所述步骤3.5中还包括：按照起始点行走后，机器人每次移动后判断是否达到目标点，如果达到目标点，则结束导航，如果未达到目标点，则进一步判断环境是否发生变化，如果环境发生变化，则更新节点参数并返回步骤3.1，以重新规划最短路径，如果环境未发生变化，则继续按照原有的最短路径行走到下一节点。
在另外一方面，本发明公开了一种非暂态计算机可读存储介质，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如上述任一项所述的基于改进D星路径规划算法的电缆沟机器人导航避障方法。
(三)有益效果
1)本发明的导航避障方法在对机器人工作环境进行分析后，以机器人为核心建立自身运动模型，主要包括机器人的坐标变换和航迹推理两部分。坐标变换是描述机器人的空间位置，是把一种坐标系变换成另一种坐标系，建立两个坐标系之间的对应关系。航迹推理是根据机器人本体感知传感器计算出机器人的运动轨迹，以便控制机器人完成圆弧巡检运动的自主导航任务。与现有技术相比，本发明的基于改进D*Lite路径规划算法的电缆沟机器人自主导航避障方法，抗干扰强、实施简单、稳定性好、精度较高，可以在复杂环境下运行，通过实时采集电缆隧道内的相关环境信息，根据实时情况作出调整，使得电缆沟机器人可以适应各种复杂环境下的电缆隧道的检修工作。
2)本发明的导航避障方法通过引入烟花算法解决了D星路径规划算法频繁转弯，且导航路径过于接近障碍物的问题，便于机器人搭载外形较大的设备和机械臂而不与障碍物产生碰撞，且可以在多种场景中获得应用，主要应用如下：a)地下电缆沟的巡检，对于地下电缆沟狭小且复杂的地形，人工巡检非常困难，若巡检机器人采用基于改进的D*Lite路径规划算法进行自主导航避障方法，不仅节省了人力成本，而且降低了机器人在电缆沟内发生碰撞而引发事故的风险。b)变电站的巡检，对于地形相对开阔且环境信息的动态变化较剧烈的环境，使用机器人巡检时，同样可以采用基于改进的D*Lite路径规划算法进行自主导航避障，减少不必要的转弯并且提高巡检效率。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例所需要使用的附图进行简单地介绍：
图1为本发明实施例中电缆沟机器人的自主导航系统框图；
图2为本发明实施例中电缆沟机器人导航避障控制策略流程图；
图3为本发明实施例中的电缆沟机器人做近似圆弧运动的坐标图；
图4为本发明中机器人采用AMCL(自适应蒙特卡罗定位)算法的流程图；
图5为本发明中通过烟花算法改进的D*Lite算法规划路径效果对比示意图；
图6为本发明中基于改进D星路径规划算法的电缆沟机器人导航避障方法的流程图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明中的技术方案进行清楚、完整地描述。显然，所描述的实施例是本发明的一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都属于本发明保护的范围。
如图1-2所示，本发明提供了一种电缆沟机器人的自主导航系统，该系统通过机器人本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息，使得电缆沟机器人能够在运动过程中使用定位与地图构建(SimultaneousLocalization And Mapping，简称SLAM)算法在自身定位的基础上建立地图，并通过全局路径规划算法得到一条最优路径，机器人按照最优路径进行避障的同时从起始点导航移动到目标点，并实现电缆沟的巡检。
如图2-6所示，本发明中基于改进D星路径规划算法的电缆沟机器人导航避障方法，包括以下步骤：
步骤1：通过电缆沟机器人的本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息；
步骤2：基于步骤1中采集的信息建立电缆沟机器人的数学模型，并用SLAM算法在自身定位的基础上建立环境地图；
进一步的，步骤2包括以下步骤2.1～步骤2.3：
具体的，以电缆沟机器人为核心建立符合自身运动特点的数学模型(也可称运动模型)包括机器人的坐标变换和航迹推理两部分，坐标变换是描述机器人的空间位置，是把一种坐标系变换成另一种坐标系，建立两个坐标系之间的对应关系；航迹推理是根据机器人本体感知传感器计算出机器人的运动轨迹，以便控制机器人完成自主导航任务，。
步骤2.1：进行电缆沟机器人的坐标变换；
对于本发明控制对象的电缆沟机器人来说，因为工作的特殊性，故其与一般的机器人的数学模型不同，在建立数学模型时需把电缆机器人的第一关节看作一个刚体，忽略机器人后面几关节，作为一个刚体看待时，其运动模型可以由6个变量描述，包括三维坐标系和相对全局坐标系的俯仰角、翻滚角和偏航角，由于本发明的电缆沟机器人理论上可以看作在一个二维平面上运动，故可以将三维坐标系的Z轴和俯仰角、翻滚角忽略，只用二维平面坐标和偏航角三个变量来描述电缆沟机器人的数学模型，其位姿主要描述地下电缆沟内的电缆沟器人在二维平面坐标系的位置和机器人运动方向。
如图3所示，通过建立地下电缆沟状态监测机器人全局坐标系和局部坐标系之间的关系，以描述地下电缆沟状态监测机器人在二维空间中的位姿，其中{Xt,Yt}为全局坐标系，原点以运动起点机器人的第一关节几何中心定义，Xt为起点时机器人的正前方向，Yt为起点时机器人车体第一关节垂直方向，{Xr,Yr}为局部坐标系，原点以机器人第一关节当前位置几何中心定义，Xr为当前机器人的正前方向，Yr为当前机器人车体第一关节垂直方向。
在全局坐标系下，电缆沟机器人的位置为(x,y)，β为机器人的偏航角，由全局坐标系与局部坐标系旋转角度差得出。由此，机器人的位姿由向量
Figure BDA0002455855450000101
表示。
通常称β为机器人的航向，β＝0°时，机器人的航向为Xr轴方向；β＝90°时，机器人的航向为Yr轴方向，没有航向的位姿称为位置，位置关系到机器人在地下电缆沟中对四周的感知，位置用一个二维向量
Figure BDA0002455855450000102
表征。
在建立了机器人在全局坐标系和局部坐标系中的位姿后，机器人局部坐标系与全局坐标系的映射关系为下式(1)，使用正交旋转矩阵R(β)把机器人全局坐标系中的位姿对应到局部坐标系下：
Figure BDA0002455855450000103
其中作为航向的旋转角度β由于直接影响全局坐标系和局部坐标系之间的映射，所以求解机器人的航向β很重要。
根据位姿的向量，最终得出全局坐标系和局部坐标系映射公式如下式(2)：
Figure BDA0002455855450000111
步骤2.2：进行电缆沟机器人的航迹推理，并得到电缆沟机器人的数学模型；
进行航迹推理时，因为电缆沟机器人需要围绕第一关节运动巡检，故将机器人的运动分割成一段一段的圆弧进行积分来获得机器人的运动轨迹。
如图2所示，设在t时刻状态监测机器人的线速度为vt，角速度为ωt，规定机器人前进时vt为正，后退时vt为负；机器人逆时针转动时ωt为正，顺时针旋转时ωt为负，则对机器人的控制量为
Figure BDA0002455855450000112
在全局坐标系中，假设机器人在(t,t+1)内的控制量ut保持不变，机器人围绕一个固定半径圆弧做运动，可知机器人的运动半径r为
Figure BDA0002455855450000113
如图2所示，为在全局坐标系下机器人做圆弧移动近似图，t+1时刻的位姿xt+1＝(xt+1,yt+1,θt+1)由t时刻的位姿推出，则可有图可知∠xt+1xtD为
Figure BDA0002455855450000121
如附图2可知，假设地下电缆沟状态监测机器人在(t,t+1)中线速度恒定，则机器人移动的距离为
xt+1xt＝vtΔt (6)
则可以求出全局坐标系下地下电缆沟状态监测机器人的位姿增量为
Figure BDA0002455855450000122
由上式(7)得出的位姿增量，可得到电缆沟机器人在全局坐标系下做近似圆弧运动的数学模型，如下式(8)
Figure BDA0002455855450000123
步骤2.3：在使用SLAM算法的基础上，本发明采用自适应蒙特卡罗定位算法(AMCL,全称Adaptive Monte Carlo Localization)对机器人自身进行自定位，并使用自适应蒙特卡洛定位算法输出定位坐标，以使得电缆沟机器人在狭窄的电缆沟内的定位更加准确，本发明中采用的自适应蒙特卡罗定位算法的步骤如附图4所示。
如图4所示，电缆沟机器人采用的自适应蒙特卡洛定位算法包括：步骤一：进行初始化离散工作后，检测路标，生产路标的不变性表示，并测量距离和角度；步骤二：驱动电缆沟机器人运动，并根据运动模型估计下一运动，检测路标，计算机器人的相对位置，根据预测值和观测值计算各粒子的偏差；步骤三：对粒子进行跟踪后，判断粒子是否集中，如果是，则结束定位，如果否，则回到步骤二。
步骤3：电缆沟机器人通过路径规划算法得到一条最优路径，使机器人按照最优路径移动到目标位置。
为使得电缆沟机器人能够高效的完成巡检工作，本发明提出采用烟花算法与D*Lite路径规划算法融合，通过烟花算法确保机器人采用的路径与障碍维持安全距离，并对重要转折点之间的路径进行平滑处理，使机器人能够更好的在电缆沟内部自主导航避障，从而在机器人做圆弧移动时能够保持距离以避开电缆沟内的石头等诸多小障碍。
具体的，改进D星算法实现方法的流程如附图6所示，可知，与现有技术中的D星(D*Lite)算法相比，本发明中采用的改进D星路径规划算法中包括如下步骤：
步骤3.1：获取环境信息后，规划最优路径；在所述规划最优路径时，改进D星路径规划算法加入演化算法中的映射操作，让电缆沟机器人可以对电缆沟内规划行走区域的栅格进行安全判定，模运算映射规则见下式：
Figure BDA0002455855450000131
其中，
Figure BDA0002455855450000132
为在k维度平面上第i个超出边界烟花的位置，
Figure BDA0002455855450000133
和
Figure BDA0002455855450000134
为在k维度平面上的上下边界值，％为模运算符号。
步骤3.2：选择重要转折点，在使用D星路径规划算法的过程中，从第二个路径点开始，如果此路径点的方向和前一个节点的父节点相同，则认为此路径点为重复，将此路径点删除并重新规划路径点组合，按这个方法规划所有路径节点，得到只有起点、转折点和终点的组合，称为重要转折点。
步骤3.3：构建烟花算法适应度函数，适应度函数的选择会影响算法的收敛速度以及是否可以找到近似最优解，考虑到机器人行走路径的长度，用下式(10)来表达路径的代价值：
Figure BDA0002455855450000141
其中，pij为从i节点到j节点线段的代价值；下式(11)作为个体适应度函数
Figure BDA0002455855450000142
其中，Pmax是一个相对较大且合适的常数。
步骤3.4：使用烟花算法二次规划重要转折点之间的路径，以实现重要转折点之间的路径平滑，使用两相邻的重要转折点作为路径的起点和终点，并再次栅格化两点之间的空间，通过爆炸算子使烟花爆炸，生成N个烟花，每个烟花代表一条起点与终点之间的连线，然后使用适应度函数算出所有烟花的适应度，并重复该过程直到找到终点，通过烟花算法中的烟花的爆炸、火花的选择，即可在两个相邻的重要转折点之间生成近似最佳解，所有的重要转折点之间路径优化过后，即生成全局最优路径。如图5所示，为通过烟花算法改进D星路径规划算法得到的平滑处理效果示意图，图5中左图为不加烟花算法的D星路径规划算法示意图，右图为改进D星路径规划算法平滑处理后的示意图。
步骤3.5：电缆沟机器人做出决策，按照最短路径出发行走，移动到下一节点并指定该节点为起始点。
进一步的，步骤3.5中还进一步包括，按照起始点(即Sstart)行走后，机器人每次移动后判断是否达到目标点(即Sgoal)，如果达到目标点，则结束导航，如果未达到目标点，则进一步判断环境是否发生变化，如果环境发生变化，则更新节点参数并回到步骤3.1重新规划最短路径，如果环境未发生变化，则继续按照原有的最短路径行走到下一节点。
需要指出的是，图5中判断的环境是否发生变化指的是电缆沟机器人前进过程中，检查路径上相关格子是否发生变化，当环境信息发生变化且这种变化对最优路径上格子的参数有影响时，才更新节点参数并重新规划最短路径，如果电缆沟内的环境信息变化，但是这种改变对已有最优路径无影响，即不会影响现有的路径规划结果，则算法不更新路径，并继续按照原有的最短路径行走。
另外，在本发明各个实施例中的各步骤对应的功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用硬件加软件功能单元的形式实现。以软件功能单元的形式实现的集成的单元，可以存储在一个计算机可读取存储介质中。上述软件功能单元存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施例所述方法的部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read-Only Memory，ROM)、随机存取存储器(Random Access Memory，RAM)、磁碟或者光盘等各种可以存储程序代码的介质。
最后说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (6)

1.一种基于改进D星路径规划算法的电缆沟机器人导航避障方法，其特征在于，该方法包括以下步骤：
步骤1：通过电缆沟机器人的本体运动感知传感器结合环境感知传感器测量得到机器人在空间的具体位置、姿态及周围环境的信息；
步骤2：基于步骤1中采集的信息建立电缆沟机器人的数学模型，并用SLAM算法在自身定位的基础上建立环境地图；
步骤3：电缆沟机器人通过路径规划算法得到一条最优路径，使机器人按照最优路径移动到目标位置；所述步骤3中具体包括以下步骤：
步骤3.1：获取环境信息后，规划最优路径；在所述规划最优路径时，改进D星路径规划算法加入演化算法中的映射操作，让电缆沟机器人可以对电缆沟内规划行走区域的栅格进行安全判定，模运算映射规则见下式：
Figure FDA0002455855440000011
其中，
Figure FDA0002455855440000012
为在k维度平面上第i个超出边界烟花的位置，
Figure FDA0002455855440000013
和
Figure FDA0002455855440000014
为在k维度平面上的上下边界值，％为模运算符号；
步骤3.2：选择重要转折点，在使用D星路径规划算法的过程中，从第二个路径点开始，如果此路径点的方向和前一个节点的父节点相同，则认为此路径点为重复，将此路径点删除并重新规划路径点组合，按这个方法规划所有路径节点，得到只有起点、转折点和终点的组合，称为重要转折点；
步骤3.3：构建烟花算法适应度函数，适应度函数的选择会影响算法的收敛速度以及是否可以找到近似最优解，考虑到机器人行走路径的长度，用下式(10)来表达路径的代价值：
Figure FDA0002455855440000021
其中，pij为从i节点到j节点线段的代价值；下式(11)作为个体适应度函数
Figure FDA0002455855440000022
其中，Pmax是一个相对较大且合适的常数。
步骤3.4：使用烟花算法二次规划重要转折点之间的路径，基于适应度函数生成全局最优路径作为最短路径；
步骤3.5：电缆沟机器人做出决策，按照最短路径出发行走，移动到下一节点并指定该节点为起始点。
2.根据权利要求1所述的导航避障方法，其特征在于，所述步骤3.4中具体包括：使用两相邻的重要转折点作为路径的起点和终点，并再次栅格化两点之间的空间，通过爆炸算子使烟花爆炸，生成N个烟花，每个烟花代表一条起点与终点之间的连线，然后使用适应度函数算出所有烟花的适应度，并重复该过程直到找到终点，通过烟花算法中的烟花的爆炸、火花的选择，即可在两个相邻的重要转折点之间生成近似最佳解，所有的重要转折点之间路径优化过后，即生成全局最优路径作为最短路径。
3.根据权利要求1所述的导航避障方法，其特征在于，所述步骤2中具体包括以下步骤：
步骤2.1：进行电缆沟机器人的坐标变换；
通过建立地下电缆沟状态监测机器人全局坐标系和局部坐标系之间的关系，以描述地下电缆沟状态监测机器人在二维空间中的位姿，其中{Xt,Yt}为全局坐标系，原点以运动起点机器人的第一关节几何中心定义，Xt为起点时机器人的正前方向，Yt为起点时机器人车体第一关节垂直方向，{Xr,Yr}为局部坐标系，原点以机器人第一关节当前位置几何中心定义，Xr为当前机器人的正前方向，Yr为当前机器人车体第一关节垂直方向，在全局坐标系下，电缆沟机器人的位置为(x,y)，β为机器人的偏航角，由位姿以及全局坐标系与局部坐标系旋转角度差得出最终全局坐标系和局部坐标系映射公式如下式(2)：
Figure FDA0002455855440000031
步骤2.2：进行电缆沟机器人的航迹推理，并得到电缆沟机器人的数学模型；设在t时刻状态监测机器人的相对于当前机器人的正前方向的前进角度为θt，线速度为vt，角速度为ωt，规定机器人前进时vt为正，后退时vt为负；机器人逆时针转动时ωt为正，顺时针旋转时ωt为负，则可以求出全局坐标系下地下电缆沟状态监测机器人的位姿增量为
Figure FDA0002455855440000041
此时，电缆沟机器人在全局坐标系下做近似圆弧运动。
4.根据权利要求3所述的导航避障方法，其特征在于，所述步骤2.2之后还包括以下步骤2.3：
步骤2.3：在使用SLAM算法的基础上，采用自适应蒙特卡罗定位算法对机器人自身进行自定位，并使用自适应蒙特卡洛定位算法输出定位坐标；其中，电缆沟机器人采用的自适应蒙特卡洛定位算法包括：
步骤一：进行初始化离散工作后，检测路标，生产路标的不变性表示，并测量距离和角度；
步骤二：驱动电缆沟机器人运动，并根据运动模型估计下一运动，检测路标，计算机器人的相对位置，根据预测值和观测值计算各粒子的偏差；
步骤三：对粒子进行跟踪后，判断粒子是否集中，如果是，则结束定位，如果否，则回到步骤二。
5.根据权利要求1所述的导航避障方法，其特征在于，所述步骤3.5中还包括：按照起始点行走后，机器人每次移动后判断是否达到目标点，如果达到目标点，则结束导航，如果未达到目标点，则进一步判断环境是否发生变化，如果环境发生变化，则更新节点参数并返回步骤3.1，以重新规划最短路径，如果环境未发生变化，则继续按照原有的最短路径行走到下一节点。
6.一种非暂态计算机可读存储介质，其特征在于，所述非暂态计算机可读存储介质存储计算机指令，所述计算机指令使所述计算机执行如权利要求1至5任一项所述的基于改进D星路径规划算法的电缆沟机器人导航避障方法。
CN202010306134.1A 2020-04-17 2020-04-17 基于改进d星路径规划算法的电缆沟机器人导航避障方法 Active CN111596654B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010306134.1A CN111596654B (zh) 2020-04-17 2020-04-17 基于改进d星路径规划算法的电缆沟机器人导航避障方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010306134.1A CN111596654B (zh) 2020-04-17 2020-04-17 基于改进d星路径规划算法的电缆沟机器人导航避障方法

Publications (2)

Publication Number Publication Date
CN111596654A true CN111596654A (zh) 2020-08-28
CN111596654B CN111596654B (zh) 2023-07-11

Family

ID=72188951

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010306134.1A Active CN111596654B (zh) 2020-04-17 2020-04-17 基于改进d星路径规划算法的电缆沟机器人导航避障方法

Country Status (1)

Country Link
CN (1) CN111596654B (zh)

Cited By (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112327856A (zh) * 2020-11-13 2021-02-05 云南电网有限责任公司保山供电局 一种基于改进A-star算法的机器人路径规划方法
CN112362063A (zh) * 2020-11-13 2021-02-12 四川大学 基于碰撞类型划分的多机器人路径规划方法和系统
CN112509168A (zh) * 2020-11-26 2021-03-16 深圳市中研安创科技发展有限公司 高效巡检方法、装置、设备及可读存储介质
CN112748733A (zh) * 2020-12-16 2021-05-04 广东电网有限责任公司 电缆放线车路径规划方法、装置、设备及存储介质
CN113050642A (zh) * 2021-03-19 2021-06-29 长沙理工大学 一种多传感器融合的电缆沟智能巡检机器人自动导航方法
CN113075686A (zh) * 2021-03-19 2021-07-06 长沙理工大学 一种基于多传感器融合的电缆沟智能巡检机器人建图方法
CN113534823A (zh) * 2021-09-16 2021-10-22 季华实验室 种植机器人路径规划方法、装置、电子设备和存储介质
CN113624230A (zh) * 2021-06-22 2021-11-09 安克创新科技股份有限公司 用于移动机器人的导航路径生成方法和移动机器人
CN117128975A (zh) * 2023-10-24 2023-11-28 国网山东省电力公司济南供电公司 一种开关柜巡检作业机器人导航方法、系统、介质及设备

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015024407A1 (zh) * 2013-08-19 2015-02-26 国家电网公司 基于电力机器人的双目视觉导航系统及方法
WO2016045615A1 (zh) * 2014-09-25 2016-03-31 科沃斯机器人有限公司 机器人静态路径规划方法
CN105651276A (zh) * 2015-12-30 2016-06-08 天津盛购科技发展有限公司 一种基于动态线路规划仓库路径的系统
CN106092111A (zh) * 2016-06-03 2016-11-09 山东师范大学 一种车辆路径动态规划方法、服务器及导航系统
WO2017041730A1 (zh) * 2015-09-09 2017-03-16 北京进化者机器人科技有限公司 一种移动机器人避障导航的方法和系统
CN109828579A (zh) * 2019-02-25 2019-05-31 广东工业大学 一种目标增量移动的移动机器人路径规划方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015024407A1 (zh) * 2013-08-19 2015-02-26 国家电网公司 基于电力机器人的双目视觉导航系统及方法
WO2016045615A1 (zh) * 2014-09-25 2016-03-31 科沃斯机器人有限公司 机器人静态路径规划方法
WO2017041730A1 (zh) * 2015-09-09 2017-03-16 北京进化者机器人科技有限公司 一种移动机器人避障导航的方法和系统
CN105651276A (zh) * 2015-12-30 2016-06-08 天津盛购科技发展有限公司 一种基于动态线路规划仓库路径的系统
CN106092111A (zh) * 2016-06-03 2016-11-09 山东师范大学 一种车辆路径动态规划方法、服务器及导航系统
CN109828579A (zh) * 2019-02-25 2019-05-31 广东工业大学 一种目标增量移动的移动机器人路径规划方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
刘彪;柏林;周科;: "警用巡逻机器人导航系统设计及关键技术研究" *
张贺;胡越黎;王权;燕明;: "基于改进D*算法的移动机器人路径规划" *

Cited By (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112362063A (zh) * 2020-11-13 2021-02-12 四川大学 基于碰撞类型划分的多机器人路径规划方法和系统
CN112327856B (zh) * 2020-11-13 2022-12-06 云南电网有限责任公司保山供电局 一种基于改进A-star算法的机器人路径规划方法
CN112327856A (zh) * 2020-11-13 2021-02-05 云南电网有限责任公司保山供电局 一种基于改进A-star算法的机器人路径规划方法
CN112509168A (zh) * 2020-11-26 2021-03-16 深圳市中研安创科技发展有限公司 高效巡检方法、装置、设备及可读存储介质
CN112748733A (zh) * 2020-12-16 2021-05-04 广东电网有限责任公司 电缆放线车路径规划方法、装置、设备及存储介质
CN112748733B (zh) * 2020-12-16 2024-05-07 广东电网有限责任公司 电缆放线车路径规划方法、装置、设备及存储介质
CN113075686B (zh) * 2021-03-19 2024-01-12 长沙理工大学 一种基于多传感器融合的电缆沟智能巡检机器人建图方法
CN113050642A (zh) * 2021-03-19 2021-06-29 长沙理工大学 一种多传感器融合的电缆沟智能巡检机器人自动导航方法
CN113075686A (zh) * 2021-03-19 2021-07-06 长沙理工大学 一种基于多传感器融合的电缆沟智能巡检机器人建图方法
CN113050642B (zh) * 2021-03-19 2024-05-07 长沙理工大学 一种多传感器融合的电缆沟智能巡检机器人自动导航方法
CN113624230B (zh) * 2021-06-22 2023-07-18 安克创新科技股份有限公司 用于移动机器人的导航路径生成方法和移动机器人
CN113624230A (zh) * 2021-06-22 2021-11-09 安克创新科技股份有限公司 用于移动机器人的导航路径生成方法和移动机器人
CN113534823A (zh) * 2021-09-16 2021-10-22 季华实验室 种植机器人路径规划方法、装置、电子设备和存储介质
CN117128975A (zh) * 2023-10-24 2023-11-28 国网山东省电力公司济南供电公司 一种开关柜巡检作业机器人导航方法、系统、介质及设备
CN117128975B (zh) * 2023-10-24 2024-03-12 国网山东省电力公司济南供电公司 一种开关柜巡检作业机器人导航方法、系统、介质及设备

Also Published As

Publication number Publication date
CN111596654B (zh) 2023-07-11

Similar Documents

Publication Publication Date Title
CN111596654B (zh) 基于改进d星路径规划算法的电缆沟机器人导航避障方法
CN110703762B (zh) 一种复杂环境下水面无人艇混合路径规划方法
CN110471426B (zh) 基于量子狼群算法的无人驾驶智能车自动避碰方法
Sudhakara et al. Trajectory planning of a mobile robot using enhanced A-star algorithm
CN109917817B (zh) 多水下机器人协同路径规划方法
Magid et al. Voronoi-based trajectory optimization for UGV path planning
CN115373399A (zh) 一种基于空地协同的地面机器人路径规划方法
CN113587930B (zh) 基于多传感融合的自主移动机器人室内外导航方法及装置
Li et al. Mobile robot path planning based on improved genetic algorithm with A-star heuristic method
CN111998858B (zh) 一种基于改进a*算法的无人机航路规划方法
Hu et al. Path planning of UGV based on Bézier curves
Li et al. A New Hybrid Method for Mobile Robot Dynamic Local Path Planning in Unknown Environment.
CN117705123B (zh) 一种轨迹规划方法、装置、设备及存储介质
Huang et al. Path tracking based on improved pure pursuit model and pid
Zhou et al. Slam algorithm and navigation for indoor mobile robot based on ros
CN117109620A (zh) 基于采样的车辆行为与环境交互的自动驾驶路径规划方法
CN116009558A (zh) 一种结合运动学约束的移动机器人路径规划方法
Yuan et al. A Q-learning approach based on human reasoning for navigation in a dynamic environment
CN114609925B (zh) 水下探索策略模型的训练方法及仿生机器鱼水下探索方法
Zhong et al. FGP-Astar Algorithm Based on Path Planning for Mobile Robots
Brumitt et al. Dynamic trajectory planning for a cross-country navigator
Jafari et al. Reactive path planning for emergency steering maneuvers on highway roads
Wang et al. Motion Planning Method for Car-Like Autonomous Mobile Robots in Dynamic Obstacle Environments
Suo et al. Real time obstacle avoidance and navigation with mobile robot via local elevation information
Wang et al. Application of ant colony algorithm in path planning of the data center room robot

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
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Ling Zhiyong

Inventor after: Wu Changjiang

Inventor after: Li Guoxiong

Inventor after: Liu Huafei

Inventor after: Chen Bin

Inventor after: Liu Zheng

Inventor after: Li Zhiqiang

Inventor after: Shen Weiqiang

Inventor after: Tan Liu

Inventor before: Ling Zhiyong

Inventor before: Li Guoxiong

Inventor before: Wu Changjiang

Inventor before: Liu Zheng

Inventor before: Chen Bin

Inventor before: Liu Huafei

Inventor before: Li Zhiqiang

Inventor before: Shen Weiqiang

Inventor before: Tan Liu

GR01 Patent grant
GR01 Patent grant