CN112013841B - 一种室内动态环境下的语义slam服务机器人导航方法 - Google Patents
一种室内动态环境下的语义slam服务机器人导航方法 Download PDFInfo
- Publication number
- CN112013841B CN112013841B CN202010874270.0A CN202010874270A CN112013841B CN 112013841 B CN112013841 B CN 112013841B CN 202010874270 A CN202010874270 A CN 202010874270A CN 112013841 B CN112013841 B CN 112013841B
- Authority
- CN
- China
- Prior art keywords
- service robot
- semantic
- wolf
- image
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 59
- 241000282461 Canis lupus Species 0.000 claims abstract description 61
- 230000011218 segmentation Effects 0.000 claims abstract description 13
- 230000003068 static effect Effects 0.000 claims abstract description 10
- 238000005457 optimization Methods 0.000 claims abstract description 9
- 239000000126 substance Substances 0.000 claims description 17
- 230000007613 environmental effect Effects 0.000 claims description 15
- 230000008569 process Effects 0.000 claims description 14
- 238000005070 sampling Methods 0.000 claims description 12
- 238000011156 evaluation Methods 0.000 claims description 11
- 238000011084 recovery Methods 0.000 claims description 9
- 239000013598 vector Substances 0.000 claims description 9
- 241000282421 Canidae Species 0.000 claims description 8
- 230000001133 acceleration Effects 0.000 claims description 6
- 238000009825 accumulation Methods 0.000 claims description 5
- 238000013507 mapping Methods 0.000 claims description 5
- 230000007547 defect Effects 0.000 claims description 4
- 230000003213 activating effect Effects 0.000 claims description 3
- 230000007423 decrease Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000007246 mechanism Effects 0.000 claims description 3
- 239000002245 particle Substances 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000019771 cognition Effects 0.000 abstract description 5
- 238000000605 extraction Methods 0.000 abstract 1
- 230000006870 function Effects 0.000 description 40
- 230000003930 cognitive ability Effects 0.000 description 4
- 230000006872 improvement Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000001502 supplementing effect Effects 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 230000006978 adaptation Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000004888 barrier function Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008901 benefit Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
- G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/20—Image preprocessing
- G06V10/26—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion
- G06V10/267—Segmentation of patterns in the image field; Cutting or merging of image elements to establish the pattern region, e.g. clustering-based techniques; Detection of occlusion by performing operations on regions, e.g. growing, shrinking or watersheds
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V10/00—Arrangements for image or video recognition or understanding
- G06V10/40—Extraction of image or video features
- G06V10/44—Local feature extraction by analysis of parts of the pattern, e.g. by detecting edges, contours, loops, corners, strokes or intersections; Connectivity analysis, e.g. of connected components
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
- G06V20/00—Scenes; Scene-specific elements
- G06V20/10—Terrestrial scenes
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Multimedia (AREA)
- Theoretical Computer Science (AREA)
- Automation & Control Theory (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
本发明提供了一种室内动态环境下的语义SLAM服务机器人导航方法,包括如下步骤:(1)信息语义分割并提取特征点;(2)移动一致性检查,剔除动态点,再利用稳定的静态特征点进行相机位姿估计;(3)语义八叉树地图的构建;(4)初始化服务机器人自身位姿;(5)通过结合灰狼优化算法的DWA局部路径规划算法,求解最优速度轨迹,确定服务机器人行驶路径……解决现有的传统SLAM场景认知能力弱,导致服务机器人定位准确性不高和传统DWA局部动态规划易陷入局部最优位置的问题,提出了结合灰狼优化算法的DWA局部路径规划方法,结合语义信息实现动态避障,保障服务机器人行进时对环境的认知能力,能够提供更准确的速度信息,同时增加安全可靠性。
Description
技术领域
本发明是一种室内动态环境下的语义SLAM服务机器人导航方法,涉及服务机器人自主导航领域,具体涉及语义SLAM服务机器人导航方法领域。
背景技术
随着当代社会生活水平的不断提高,人们对生活质量的需求也不断提高,各种智能服务型机器人不断出现,更好地研究服务机器人变得日益重要。移动机器人SLAM方法是智能服务机器人的基础。早期传统的SLAM方法主要以静态环境假设为前提,近年来人工智能方法尤其是机器学习方法的出现,使得移动机器人在动态环境下的表现得到了显著提升,极大增强了对环境中可移动物体的认知能力,整个过程也更加智能化和安全可靠。
室内场景中存在可移动的目标,比如人、宠物、玩具汽车及其他移动机器人等,对于动态场景,传统的SLAM方法的定位和场景认知能力较弱,准确性大大降低,因为移动的物体会极大地干扰场景结构和定位,结果可能是完全不可靠的。传统视觉SLAM结合语义信息能消除动态特征,提高准确性。路径规划是服务机器人导航的基本环节,能使服务机器人在运动过程中安全、无碰撞地绕过所有障碍物,反映了服务机器人对动态环境地综合性判断,更好地进行智能决策。传统的DWA局部路径规划易陷入局部最优位置,无法满足更精准的导航。
发明内容
本发明所要解决的技术问题是,提供了一种室内动态环境下的语义SLAM服务机器人导航方法,解决现有的传统SLAM场景认知能力弱,导致服务机器人定位准确性不高和传统DWA局部动态规划易陷入局部最优位置的问题,提出了结合灰狼优化算法的DWA局部路径规划方法,结合语义信息实现动态避障,保障服务机器人行进时对环境的认知能力,能够提供更准确的速度信息,同时增加安全可靠性,实现服务机器人动态环境下的位姿估计和路线的有效规划,也能满足服务机器人的实时避障要求。
为解决上述技术问题,本发明采用的技术方案为:
一种室内动态环境下的语义SLAM服务机器人导航方法,其特征在于,
该方法包括以下步骤:
步骤(1):利用服务机器人携带的传感器所获取的信息进行语义分割并提取特征点;
步骤(2):根据所述步骤(1)中提取的特征点进行移动一致性检查,剔除动态点,再利用稳定的静态特征点进行相机位姿估计;
步骤(3):根据所述步骤(2)中的静态特征点在构建好的三维点云地图基础上进行语义八叉树地图的构建;
步骤(4):根据所述步骤(3)中语义八叉树地图,初始化服务机器人自身位姿;
步骤(5):根据所述步骤(4)中确定的服务机器人位姿信息进行定位的同时,通过结合灰狼优化算法的DWA局部路径规划算法,求解最优速度轨迹,确定服务机器人行驶路径;
步骤(6):根据所述步骤(5)中的最优速度轨迹,将生成的速度指令发送给服务机器人底盘执行机构,到达目标区域;
步骤(7):根据所述步骤(6)中的速度指令,服务机器人在到达目标区域过程中若陷入局部障碍区域,则启动恢复模式,使其脱离被困区域,重新导航。
所述步骤(1)包括如下步骤:
步骤(1-1):利用服务机器人携带的传感器获取原始图像信息Ij,使用PSPNet进行语义分割,得到的语义图像Cj中所有相同类别标记为同一类,将语义图像Cj中每个像素点乘以该像素点对应的语义先验权重,用结果得分的中值代替每个像素点的结果得分,得到语义一致的结果图像
步骤(1-2):将语义图像Cj分割为k*k的块,对于每个块,根据公式(1)计算中值si作为该块的被选择的权重,其中,i为块的编号,一幅图像中不只一块,si越高,则此块被选择的概率越大,然后在选择的块内根据梯度值进一步提取特征点,若梯度值小于50,则剔除特征点。
所述步骤(2)中的移动一致性检查包括:
如果匹配的特征点接近图像Ij边缘或者与中心处图像块的像素相差大,则丢弃当前的匹配点,再通过RANSAC算法求取基础矩阵,计算当前帧的极线,判断从匹配点到对应极线的距离是否小于阈值,如果距离大于阈值,则确定匹配点是移动的。
特征点接近传感器获取的原始图像Ij边缘是与原始图像Ij同心,半径为原始图像Ij半径的0.95的同心圆与原始图像Ij相离的部分,中心处图像块的像素相差大是该特征点与中心处像素值之差与中心处像素值比值大于0.8,阈值为0.4倍原始图像Ij尺寸。
5.根据权利要求1所述的一种室内动态环境下的语义SLAM服务机器人导航方法,其特征在于,步骤(3)包括如下步骤:
步骤(3-1):在步骤(2)剔除动态点之后,利用静态特征点,***自动生成三维点云地图;
步骤(3-2):在八叉树中,假设时刻t=1,2,…,T,传感器观测的数据为Z1,Z2,…,ZT,则第n个子节点的语义信息P(n|z1:T)为:
其中,P(n|z1:T)为在观测数据Z1到ZT下第n个子节点的占用概率,P(n|zT)为仅根据观测数据ZT获得的第n个子节点的占用概率,P(n|z1:T-1)为在T时刻之前的观测数据下第n个子节点的占用概率,P(n)为无观测数据下第n个子节点的占用概率;
其中,E为概率对数值,p为上述P(n|z1:T)的简称,
-子节点占据情况的概率对数形式L(n|z1:T+1)为::
L(n|z1:T+1)=L(n|z1:T1)+L(n|z1:T) (5)
其中,L(n|z1:T+1)为在观测数据Z1到ZT+1下第n个子节点的对数占用概率,L(n|z1:T-1)为在观测数据Z1到ZT-1下第n个子节点的对数占用概率,L(n|z1:T)为在观测数据Z1到ZT下第n个子节点的对数占用概率;
所述步骤(4)包括如下步骤:
步骤(4-1):服务机器人自身搭载有Linux操作***,服务机器人导航开始时初始化服务机器人位姿使其位于路线起始点半径0.1m范围内;
步骤(4-2):自动建图导航算法通过传感器观测环境信息,利用贝叶斯估计即粒子滤波方法,获取服务机器人每一时刻的位姿估计:
预测:由t-1时刻的后验概率密度函数计算p(xt-1|y1:t-1),得到t时刻的先验概率密度函数p(xt|y1:t-1),
p(xt|y1:t-1)=∫p(xt|xt-1)p(xt-1|y1:t-1)dxt-1 (6)
其中,xt表示***t时刻的位姿状态估计,xt-1表示***t-1时刻的位姿状态估计,y1:t-1表示1到t-1时刻的传感器观测环境信息;
更新:由t时刻得到的先验概率密度函数p(xt|y1:t-1),结合环境观测量yt,针对上一步的观测过程,获取后验概率密度p(xt|y1:t),
其中,p(yt|y1:t-1)为归一化常数,表示为
p(yt|y1:t-1)=∫p(yt|xt)p(xt|y1:t-1)dxt (8)
其中,yt表示t时刻的传感器观测的环境信息,y1:t-1表示1到t-1时刻的传感器观测环境信息,xt表示***t时刻的位姿状态估计,p(yt|y1:t-1)表示1到t-1时刻的传感器观测环境信息已知情况下对t时刻的传感器观测的环境信息的估计,p(yt|xt)表示***t时刻的位姿状态估计下的传感器观测的环境信息,p(xt|y1:t-1)表示1到t-1时刻的传感器观测环境信息已知情况下***t时刻的位姿状态估计。
所述步骤(5)包括如下步骤:
步骤(5-1):在步骤(4-2)完成服务机器人位姿信息估计之后,随着时间的累积,确定服务机器人当前时刻位姿信息,累积之前所有时刻位姿信息,得知当前时刻所处位置,完成定位;
步骤(5-2):初始化α,β,δ狼的位置和目标函数值,利用生成对抗网络对狼群位置进行扩充,初步预测狼群位置,补充局部未知区域样本数不足的缺陷;
步骤(5-3):计算最终评价函数得分,如果目标函数值小于α狼的目标函数值,则将α狼的目标函数值更新为最优目标函数值,同时将α狼的位置更新为最优位置,如果目标函数值介于α狼和β狼的目标函数值之间,则将β狼的目标函数值更新为最优目标函数值,同时更新β狼的位置,如果目标函数值介于β狼和δ狼的目标函数值之间,则将δ狼的目标函数值更新为最优目标函数值,同时更新δ狼的位置;
步骤(5-5):包围猎物,更新α,β,δ狼的位置
其中,分别为α,β,δ狼的位置,分别表示α,β,δ狼与猎物之间的距离,C3和A3分别表示α,β,δ狼根据式(9)和(10)求出的随机系数向量,分别表示α,β,δ狼的移动距离,则更新的下一时刻的位置为:
所述步骤(6)包括如下步骤:
步骤(6-1):根据服务机器人线速度及角速度,生成速度可行区间Ss,
Ss={(l,r)|l∈[0,lmax],r∈[-rmax,rmax]} (14)
其中,lmax,rmax分别为最大线速度和角速度;
步骤(6-2):由于服务机器人电机性能限制,同时考虑单个速度采样间隔Δt,计算服务机器人真实速度变化范围Sd,
步骤(6-3):考虑到服务机器人传感器自身存在噪声误差及采样间隔内的位姿误差等,且需保障服务机器人与障碍物保持足够安全距离Sa,使得服务机器人及时在障碍物前停止运动,应满足
其中dmin为(l,r)状态下的模拟速度轨迹距离障碍物的最近距离,η1,η2是添加的无碰撞安全系;
步骤(6-4):服务机器人最终的速度采样空间应满足Ss、Sd和Sa范围限制,取其交集Sr得:
Sr=Ss∩Sd∩Sa (17)
步骤(6-5):服务机器人根据生成的速度采样空间Sr进行速度采样,并解算每一时间间隔内对应的所有的速度轨迹,通过评价得到最优速度轨迹,轨迹评价函数C(l,r)如下,
C(1r)=ah(1r)+bo(1r)+c s(1r) (18)
其中,a,b,c为权重系数,l和r分别表示服务机器人的线速度与角速度,h(l,r)描述服务机器人与目标区域的朝向一致性,o(l,r)表示当前运行速度轨迹中距离障碍物的最小距离,s(l,r)衡量服务机器人当前运行速度,轨迹评价函数C(l,r)的最大值即为最优速度轨迹。所述步骤(7)包括如下步骤:
步骤(7-1):在服务机器人自主导航的过程中,若陷入局部障碍物区域,则激活行为恢复模式;
步骤(7-2):启动行为恢复模式后,服务机器人通过传感器重新获取信息,观测周边环境并重新匹配已有语义地图;
步骤(7-3):当服务机器人重新规划出下一时刻的速度轨迹即局部路径时,表示服务机器人脱离被困区域,则继续向目标点行进。
所述步骤(6-3)中η1的取数区间为(0.6,1.0),η2的取数区间为(0.8,1.2)。
本发明的有益效果:
本发明所要解决的技术问题是,提供了一种室内动态环境下的语义SLAM服务机器人导航方法,解决传统SLAM场景认知能力弱,导致服务机器人定位准确性不高和传统DWA局部动态规划易陷入局部最优位置的问题,提出了结合灰狼优化算法的DWA局部路径规划方法,结合语义信息实现动态避障,保障服务机器人行进时对环境的认知能力,同时增加安全可靠性,实现服务机器人动态环境下的位姿估计和路线的有效规划,也能满足服务机器人的实时避障要求。
(1)在DWA局部路径规划方法中引入灰狼优化算法,通过比较不同狼的位置与目标函数,不断更新最终评价函数,得到最佳DWA参数,得到了更加准确的速度信息,使得服务机器人导航更加精准,为今后发展更为智能化的室内服务机器人提供依据。
(2)在传统SLAM方法中引入语义信息,使得服务机器人获取的环境信息更加丰富,对于场景的认知能力有效提高,能够减少因为欠缺对场景的理解带来的弊端,比如在走廊中容易迷失等。
(3)图像中的动态点对于特征点的匹配影响很大,会造成误匹配,造成定位和建图精度不高,通过去除动态点,减少对定位和建图的影响,对于提高导航的精度有很大帮助。
附图说明
图1是本发明服务机器人自主导航方法实施例的流程图;
图2是本发明服务机器人语义SLAM方法中构建的语义八叉树地图;
图3是本发明服务机器人自主局部路径规划方法中灰狼优化算法位置更新示意图。
具体实施方式
下面结合附图以及具体实施例对本发明一种室内动态环境下的语义SLAM服务机器人导航方法作进一步详细说明。
如图1至图3,请参阅图1,本发明提供一种室内动态环境下的语义SLAM服务机器人导航方法,包括步骤为:
(1)利用服务机器人携带的传感器所获取的信息进行语义分割并提取特征点,具体步骤如下:
1)利用服务机器人携带的传感器获取原始图像信息Ij,使用PSPNet进行语义分割,得到的语义图像Cj中所有相同类别标记为同一类,比如所有背景为同一类,人为一类,动物为一类等,将每个像素点乘以该像素点对应的语义先验权重,用结果得分的中值代替每个像素点的结果得分,得到语义一致的结果图像
2)将语义图像Cj分割为3*3的块,对于每个块,根据公式(1)计算中值si作为该块的被选择的权重,其中,i为块的编号,一幅图像中不只一块,si越高,则此块被选择的概率越大,然后在选择的块内根据梯度值进一步提取特征点,若梯度值小于50,则剔除特征点。
(2)根据所述步骤(1)中提取的特征点进行移动一致性检查,剔除动态点,再利用稳定的静态特征点进行相机位姿估计,具体步骤如下:
1)如果匹配的特征点接近图像Ij边缘或者与中心处图像块的像素相差大,则丢弃当前的匹配点,特征点接近传感器获取的原始图像Ij边缘是与原始图像Ij同心,半径为原始图像Ij半径的0.95的同心圆与图像相离的部分,中心处图像块的像素相差大是该特征点与中心处像素值之差与中心处像素值比值大于0.8,再通过RANSAC算法求取基础矩阵,计算当前帧的极线,该极线为相机光点连线形成的基线与图像交点和特征点在图像上投影组成的连线,q=e×m,,其中q为极线,e为基线与图像交点,m为特征点在图像上投影,×表示叉乘,判断从匹配点到对应极线的距离是否小于阈值,阈值为0.4倍原始图像Ij尺寸,如果距离大于阈值,则确定匹配点是移动的。
(3)根据所述步骤(2)中的特征点在构建好的三维点云地图基础上进行语义八叉树地图的构建,具体步骤如下:
1)在步骤(2)剔除动态点之后,利用静态特征点,***自动生成三维点云地图;
2)在八叉树中,如图2,假设时刻t=1,2,…,T,传感器观测的数据为Z1,Z2,…,ZT,则第n个子节点的语义信息P(n|z1:T)为:
其中,P(n|z1:T)为在观测数据Z1到ZT下第n个子节点的占用概率,P(n|zT)为仅根据观测数据ZT获得的第n个子节点的占用概率,P(n|z1:T-1)为在T时刻之前的观测数据下第n个子节点的占用概率,P(n)为无观测数据下第n个子节点的占用概率。
其中,E为概率对数值,p为上述P(n|z1:T)的简称,
节点占据情况的概率对数形式L(n|z1:T+1)为::
L(n|z1:T+1)=L(n|z1:T1)+L(n|z1:T) (5)
其中,L(n|z1:T+1)为在观测数据Z1到ZT+1下第n个子节点的对数占用概率,L(n|z1:T-1)为在观测数据Z1到Zr-1下第n个子节点的对数占用概率,L(n|z1:T)为在观测数据Z1到ZT下第n个子节点的对数占用概率。
(4)根据所述步骤(3)中地图三维点云地图信息,初始化服务机器人自身位姿,具体步骤如下:
1)服务机器人自身搭载有Linux操作***,服务机器人导航开始时用AMCL自适应蒙特卡洛定位初始化服务机器人位姿使其位于路线起始点半径0.1m内,利用自身位姿和语义八叉树地图规划起始点到目标点的室内全局路线;
2)自动建图导航算法,一种机器人的高级导航算法,在机器人避开障碍物的同时进行建图,区别于先把地图建立好再导航的算法,通过传感器观测环境信息,利用贝叶斯估计即粒子滤波方法,获取服务机器人每一时刻的位姿估计:
预测:由t-1时刻的后验概率密度函数计算p(xt-1|y1:t-1),得到t时刻的先验概率密度函数p(xt|y1:t-1),
p(xt|y1:t-1)=∫p(xt|xt-1)p(xt-1|y1:t-1)dxt-1 (6)
其中,xt表示***t时刻的位姿状态估计,xt-1表示***t-1时刻的位姿状态估计,y1:t-1表示1到t-1时刻的传感器观测环境信息;
更新:由t时刻得到的先验概率密度函数p(xt|y1:t-1),结合环境观测量yt,针对上一步的观测过程,获取后验概率密度p(xt|y1:t),
其中,p(yty1:t-1)为归一化常数,表示为
p(yt|y1:t-1)=∫p(yt|xt)p(xt|y1:t-1)dxt(8)
其中,yt表示t时刻的传感器观测的环境信息,y1:t-1表示1到t-1时刻的传感器观测环境信息,xt表示***t时刻的位姿状态估计,p(yt|y1:t-1)表示1到t-1时刻的传感器观测环境信息已知情况下对t时刻的传感器观测的环境信息的估计,p(yt|xt)表示***t时刻的位姿状态估计下的传感器观测的环境信息,p(xt|y1:t-1)表示1到t-1时刻的传感器观测环境信息已知情况下***t时刻的位姿状态估计。
(5)根据所述步骤(4)中确定的服务机器人位姿信息进行定位的同时,通过结合如图3所示灰狼优化算法的DWA局部路径规划算法,解算最优速度轨迹,确定服务机器人行驶路径,具体步骤如下:
1)在步骤(4)的2)完成服务机器人位姿信息估计之后,随着时间的累积,确定服务机器人当前时刻位姿信息,累积之前所有时刻位姿信息,得知当前时刻所处位置,完成定位;
2)初始化α,β,δ狼的位置和目标函数值,利用生成对抗网络对狼群位置进行扩充,初步预测狼群位置,补充局部未知区域样本数不足的缺陷;
3)计算最终评价函数得分,如果目标函数值小于α狼的目标函数值,则将α狼的目标函数值更新为最优目标函数值,同时将α狼的位置更新为最优位置,如果目标函数值介于α狼和β狼的目标函数值之间,则将β狼的目标函数值更新为最优目标函数值,同时更新β狼的位置,如果目标函数值介于β狼和δ狼的目标函数值之间,则将δ狼的目标函数值更新为最优目标函数值,同时更新δ狼的位置;
5)包围猎物,更新α,β,δ狼的位置
其中,分别为α,β,δ狼的位置,分别表示α,β,δ狼与猎物之间的距离,C3和A3分别表示α,β,δ狼根据式(9)和(10)求出的随机系数向量,分别表示α,β,δ狼的移动距离,则更新的下一时刻的位置为:
(6)根据所述步骤(5)中速度轨迹,将生成的速度指令发送给服务机器人底盘执行机构,到达目标区域,具体步骤如下:
1)根据服务机器人线速度及角速度,生成速度可行区间Ss,
Ss={(l,r)|l∈[0,lmax],r∈[-rmax,rmax]} (14)
其中,lmax,rmax分别为最大线速度和角速度;
2)由于服务机器人电机性能限制,同时考虑单个速度采样间隔Δc,计算服务机器人真实速度变化范围Sd,
3)考虑到服务机器人传感器自身存在噪声误差及采样间隔内的位姿误差等,且需保障服务机器人与障碍物保持足够安全距离Sa,使得服务机器人及时在障碍物前停止运动,应满足
其中dmin为(l,r)状态下的模拟速度轨迹距离障碍物的最近距离,η1,η2是添加的无碰撞安全系数,η1的取数区间为(0.6,1.0),η2的取数区间为(0.8,1.2);
4)服务机器人最终的速度采样空间应满足Ss、Sd和Sa范围限制,取其交集Sr得:
Sr=Ss∩Sd∩Sa (17)
5)服务机器人根据生成的速度采样空间Sr进行速度采样,并解算每一时间间隔内对应的所有的速度轨迹,通过评价得到最优速度轨迹,轨迹评价函数C(l,r)如下,
C(1r)=ah(1r)+b o(1r)+c s(1r) (18)
其中,a,b,c为权重系数,l和r分别表示服务机器人的线速度与角速度,h(l,r)描述服务机器人与目标区域的朝向一致性,o(l,r)表示当前运行速度轨迹中距离障碍物的最小距离,s(l,r)衡量服务机器人当前运行速度,轨迹评价函数C(l,r)的最大值即为最优速度轨迹。
6)根据得出的最优速度轨迹,使得服务机器人最终到达目标点。
(7)根据所述步骤(6)中的速度指令,服务机器人在到达目标区域过程中若陷入局部障碍区域,则启动恢复模式,使其脱离被困区域,重新导航,具体步骤如下:
1)在服务机器人自主导航的过程中,若陷入局部障碍物区域,则激活行为恢复模式;
2)启动行为恢复模式后,服务机器人通过传感器重新获取信息,观测周边环境并重新匹配已有语义地图;
3)当服务机器人重新规划出下一时刻的速度轨迹即局部路径时,表示服务机器人脱离被困区域,则继续向目标点行进。
在此处所提供的说明书中,说明了大量具体细节。然而,能够理解,本发明的实施例可以在没有这些具体细节的情况下被实践。在一些实例中,并未详细示出公知的方法、结构和技术,以便不模糊对本说明书的理解。
在本发明的描述中,需要理解的是,术语“上”、“下”、“前”、“后”、“左”、“右”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。
类似地,应当理解,为了精简本公开并帮助理解各个发明方面中的一个或多个,在上面对本发明的示例性实施例的描述中,本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而,并不应将该公开的方法解释成反映如下意图:即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多特征。更确切地说,如权利要求书所反映的那样,发明方面在于少于前面公开的单个实施例的所有特征。因此,遵循具体实施方式的权利要求书由此明确地并入该具体实施方式,其中每个权利要求本身都作为本发明的单独实施例。
如在此所使用的那样,除非另行规定,使用序数词“第一”、“第二”、“第三”等等来描述普通对象仅仅表示涉及类似对象的不同实例,并且并不意图暗示这样被描述的对象必须具有时间上、空间上、排序方面或者以任意其它方式的给定顺序。
尽管根据有限数量的实施例描述了本发明,但是受益于上面的描述,本技术领域内的技术人员明白,在由此描述的本发明的范围内,可以设想其它实施例。此外,应当注意,本说明书中使用的语言主要是为了可读性和教导的目的而选择的,而不是为了解释或者限定本发明的主题而选择的。因此,在不偏离所附权利要求书的范围和精神的情况下,对于本技术领域的普通技术人员来说许多修改和变更都是显而易见的。对于本发明的范围,对本发明所做的公开是说明性的,而非限制性的,本发明的范围由所附权利要求书限定。
以上所述仅是本发明的优选实施方式,应当指出:对于本技术领域的普通技术人员来说,在不脱离本发明原理的前提下,还可以做出若干改进和润饰,这些改进和润饰也应视为本发明的保护范围。
Claims (7)
1.一种室内动态环境下的语义SLAM服务机器人导航方法,其特征在于,
该方法包括以下步骤:
步骤(1):利用服务机器人携带的传感器所获取的信息进行语义分割并提取特征点;
步骤(2):根据所述步骤(1)中提取的特征点进行移动一致性检查,剔除动态点,再利用稳定的静态特征点进行相机位姿估计;
步骤(3):根据所述步骤(2)中的静态特征点在构建好的三维点云地图基础上进行语义八叉树地图的构建;
步骤(4):根据所述步骤(3)中语义八叉树地图,初始化服务机器人自身位姿;
步骤(5):根据所述步骤(4)中确定的服务机器人位姿信息进行定位的同时,通过结合灰狼优化算法的DWA局部路径规划算法,求解最优速度轨迹,确定服务机器人行驶路径;
步骤(6):根据所述步骤(5)中的最优速度轨迹,将生成的速度指令发送给服务机器人底盘执行机构,到达目标区域;
步骤(7):根据所述步骤(6)中的速度指令,服务机器人在到达目标区域过程中若陷入局部障碍区域,则启动恢复模式,使其脱离被困区域,重新导航;
其中,所述步骤(1)包括如下步骤:
步骤(1-1):利用服务机器人携带的传感器获取原始图像信息,使用PSPNet进行语义分割,得到的语义图像中所有相同类别标记为同一类,将语义图像中每个像素点乘以该像素点对应的语义先验权重,用结果得分的中值代替每个像素点的结果得分,得到语义一致的结果图像:
步骤(1-2):将语义图像分割为的块, 对于每个块,根据公式(1)计算中值作为该块的被选择的权重,其中,i为块的编号,一幅图像中不止一块,越高,则此块被选择的概率越大,然后在选择的块内根据梯度值进一步提取特征点,若梯度值小于50,则剔除特征点;
所述步骤(4)包括如下步骤:
步骤(4-1):服务机器人自身搭载有Linux操作***,服务机器人导航开始时初始化服务机器人位姿使其位于路线起始点半径0.1m范围内;
步骤(4-2):自动建图导航算法通过传感器观测环境信息,利用贝叶斯估计即粒子滤波方法,获取服务机器人每一时刻的位姿估计:
其中,表示t时刻的传感器观测的环境信息,表示1到t-1时刻的传感器观测环境信息,表示***t时刻的位姿状态估计,表示1到t-1时刻的传感器观测环境信息已知情况下对t时刻的传感器观测的环境信息的估计,表示***t时刻的位姿状态估计下的传感器观测的环境信息,表示1到t-1时刻的传感器观测环境信息已知情况下***t时刻的位姿状态估计;
所述步骤(5)包括如下步骤:
步骤(5-1):在步骤(4-2)完成服务机器人位姿信息估计之后,随着时间的累积,确定服务机器人当前时刻位姿信息,累积之前所有时刻位姿信息,得知当前时刻所处位置,完成定位;
步骤(5-3):计算最终评价函数得分,如果目标函数值小于狼的目标函数值,则将狼的目标函数值更新为最优目标函数值,同时将狼的位置更新为最优位置,如果目标函数值介于狼和狼的目标函数值之间,则将狼的目标函数值更新为最优目标函数值,同时更新狼的位置,如果目标函数值介于狼和狼的目标函数值之间,则将狼的目标函数值更新为最优目标函数值,同时更新狼的位置;
4.根据权利要求1所述的一种室内动态环境下的语义SLAM服务机器人导航方法,其特征在于,步骤(3)包括如下步骤:
步骤(3-1):在步骤(2)剔除动态点之后,利用静态特征点,***自动生成三维点云地图;
5.根据权利要求1所述的一种室内动态环境下的语义SLAM服务机器人导航方法,其特征在于,所述步骤(6)包括如下步骤:
6.根据权利要求1所述的一种室内动态环境下的语义SLAM服务机器人导航方法,其特征在于,所述步骤(7)包括如下步骤:
步骤(7-1):在服务机器人自主导航的过程中,若陷入局部障碍物区域,则激活行为恢复模式;
步骤(7-2):启动行为恢复模式后,服务机器人通过传感器重新获取信息,观测周边环境并重新匹配已有语义地图;
步骤(7-3):当服务机器人重新规划出下一时刻的速度轨迹即局部路径时,表示服务机器人脱离被困区域,则继续向目标点行进。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010874270.0A CN112013841B (zh) | 2020-08-26 | 2020-08-26 | 一种室内动态环境下的语义slam服务机器人导航方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010874270.0A CN112013841B (zh) | 2020-08-26 | 2020-08-26 | 一种室内动态环境下的语义slam服务机器人导航方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN112013841A CN112013841A (zh) | 2020-12-01 |
CN112013841B true CN112013841B (zh) | 2021-08-06 |
Family
ID=73504071
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010874270.0A Active CN112013841B (zh) | 2020-08-26 | 2020-08-26 | 一种室内动态环境下的语义slam服务机器人导航方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN112013841B (zh) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN112348029B (zh) * | 2021-01-08 | 2021-08-10 | 禾多科技(北京)有限公司 | 局部地图调整方法、装置、设备和计算机可读介质 |
CN113050626B (zh) * | 2021-03-01 | 2023-04-11 | 苏州澜途科技有限公司 | 服务机器人物理禁区检测和全局重定位方法 |
CN113253717B (zh) * | 2021-03-17 | 2022-10-11 | 武汉科技大学 | 一种基于动态障碍物的室内移动机器人局部路径规划方法 |
CN113029176B (zh) * | 2021-03-19 | 2023-08-15 | 深蓝汽车科技有限公司 | 面向多层级体验的电动汽车最优充电路径规划方法 |
CN113190012B (zh) * | 2021-05-10 | 2022-08-12 | 山东大学 | 一种机器人任务自主规划方法及*** |
CN113296520B (zh) * | 2021-05-26 | 2023-07-14 | 河北工业大学 | 融合a*与改进灰狼算法的巡检机器人路径规划方法 |
CN113547523B (zh) * | 2021-08-05 | 2022-04-15 | 河北工业大学 | 基于人工势场法和灰狼算法的冗余机械臂求逆解方法 |
CN117315175B (zh) * | 2023-09-28 | 2024-05-14 | 广东拓普视科技有限公司 | 一种基于机器人的构图定位装置及其方法 |
CN117068207B (zh) * | 2023-10-17 | 2024-01-12 | 合肥焕智科技有限公司 | 一种基于激光slam导航的车载控制*** |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108596974A (zh) * | 2018-04-04 | 2018-09-28 | 清华大学 | 动态场景机器人定位建图***及方法 |
CN110608743A (zh) * | 2019-10-18 | 2019-12-24 | 南京航空航天大学 | 基于多种群混沌灰狼算法的多无人机协同航路规划方法 |
CN111457929A (zh) * | 2019-12-31 | 2020-07-28 | 南京工大数控科技有限公司 | 一种基于地理信息***的物流车辆自主路径规划与导航方法 |
CN111457927A (zh) * | 2020-04-26 | 2020-07-28 | 北京工商大学 | 动态障碍物下的无人巡航船多目标路径规划方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111174798A (zh) * | 2020-01-17 | 2020-05-19 | 长安大学 | 一种足式机器人路径规划方法 |
CN111368728A (zh) * | 2020-03-04 | 2020-07-03 | 上海眼控科技股份有限公司 | 安全监测方法、装置、计算机设备和存储介质 |
-
2020
- 2020-08-26 CN CN202010874270.0A patent/CN112013841B/zh active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108596974A (zh) * | 2018-04-04 | 2018-09-28 | 清华大学 | 动态场景机器人定位建图***及方法 |
CN110608743A (zh) * | 2019-10-18 | 2019-12-24 | 南京航空航天大学 | 基于多种群混沌灰狼算法的多无人机协同航路规划方法 |
CN111457929A (zh) * | 2019-12-31 | 2020-07-28 | 南京工大数控科技有限公司 | 一种基于地理信息***的物流车辆自主路径规划与导航方法 |
CN111457927A (zh) * | 2020-04-26 | 2020-07-28 | 北京工商大学 | 动态障碍物下的无人巡航船多目标路径规划方法 |
Non-Patent Citations (3)
Title |
---|
Grey Wolf Optimizer;Seyedali Mirjalili et al;《Advances in Engineering Software》;20140121;第69卷;第48-51页 * |
基于语义分割的室内动态场景同步定位与语义建图;席志红等;《计算机应用》;20191031;第39卷(第10期);第2847-2851页 * |
席志红等.基于语义分割的室内动态场景同步定位与语义建图.《计算机应用》.2019,第39卷(第10期),第2847-2851页. * |
Also Published As
Publication number | Publication date |
---|---|
CN112013841A (zh) | 2020-12-01 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN112013841B (zh) | 一种室内动态环境下的语义slam服务机器人导航方法 | |
CN114384920B (zh) | 一种基于局部栅格地图实时构建的动态避障方法 | |
US11694356B2 (en) | Methods and systems for joint pose and shape estimation of objects from sensor data | |
CN112212872B (zh) | 基于激光雷达和导航地图的端到端自动驾驶方法及*** | |
GB2568286A (en) | Method of computer vision based localisation and navigation and system for performing the same | |
JP7078021B2 (ja) | 物体検出装置、物体検出方法及び物体検出用コンピュータプログラム | |
CN110675307A (zh) | 基于vslam的3d稀疏点云到2d栅格图的实现方法 | |
US10402978B1 (en) | Method for detecting pseudo-3D bounding box based on CNN capable of converting modes according to poses of objects using instance segmentation and device using the same | |
TW202020811A (zh) | 用於基於障礙物檢測校正高清晰度地圖的系統和方法 | |
WO2019175533A1 (en) | Vehicle tracking | |
WO2021056499A1 (zh) | 数据处理方法、设备和可移动平台 | |
CN114454875A (zh) | 一种基于强化学习的城市道路自动泊车方法及*** | |
CN113903011B (zh) | 一种适用于室内停车场的语义地图构建及定位方法 | |
CN113516664A (zh) | 一种基于语义分割动态点的视觉slam方法 | |
US20230343222A1 (en) | System for sparsely representing and storing geographic and map data | |
CN107610235B (zh) | 一种基于深度学***台导航方法和装置 | |
US10796571B2 (en) | Method and device for detecting emergency vehicles in real time and planning driving routes to cope with situations to be expected to be occurred by the emergency vehicles | |
WO2022087751A1 (en) | Method and system for graph neural network based pedestrian action prediction in autonomous driving systems | |
KR20230036858A (ko) | 3차원 포인트 클라우드 맵을 이용한 차선 레벨 지도 구축 방법 및 시스템 | |
US10445611B1 (en) | Method for detecting pseudo-3D bounding box to be used for military purpose, smart phone or virtual driving based-on CNN capable of converting modes according to conditions of objects and device using the same | |
CN113034504B (zh) | Slam建图过程中的平面特征融合方法 | |
Ghintab et al. | CNN-based visual localization for autonomous vehicles under different weather conditions | |
JP2021026683A (ja) | 距離推定装置 | |
Shapovalov et al. | Robust localization of a self-driving vehicle in a lane | |
RU2808469C2 (ru) | Способ управления роботизированным транспортным средством |
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 |