CN114442621A - 一种基于四足机器人的自主探索和建图系统 - Google Patents
一种基于四足机器人的自主探索和建图系统 Download PDFInfo
- Publication number
- CN114442621A CN114442621A CN202210048450.2A CN202210048450A CN114442621A CN 114442621 A CN114442621 A CN 114442621A CN 202210048450 A CN202210048450 A CN 202210048450A CN 114442621 A CN114442621 A CN 114442621A
- Authority
- CN
- China
- Prior art keywords
- path
- exploration
- local
- quadruped robot
- robot
- 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.)
- Withdrawn
Links
- 238000013507 mapping Methods 0.000 title claims abstract description 27
- 238000000034 method Methods 0.000 claims abstract description 30
- 238000005457 optimization Methods 0.000 claims abstract description 19
- 238000005070 sampling Methods 0.000 claims abstract description 7
- 238000006243 chemical reaction Methods 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 5
- 230000005021 gait Effects 0.000 claims description 5
- 238000005096 rolling process Methods 0.000 claims description 5
- 238000011217 control strategy Methods 0.000 claims description 4
- 238000011156 evaluation Methods 0.000 claims description 4
- 230000004888 barrier function Effects 0.000 claims description 3
- 239000002245 particle Substances 0.000 claims description 3
- 238000012216 screening Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 description 11
- 230000008447 perception Effects 0.000 description 8
- 238000010586 diagram Methods 0.000 description 6
- 230000008569 process Effects 0.000 description 5
- 230000007613 environmental effect Effects 0.000 description 3
- 230000000694 effects Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 239000011159 matrix material Substances 0.000 description 2
- 244000025254 Cannabis sativa Species 0.000 description 1
- 244000290594 Ficus sycomorus Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000002567 autonomic effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 235000019994 cava Nutrition 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 239000002131 composite material Substances 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000037230 mobility Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000008707 rearrangement Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000012360 testing method Methods 0.000 description 1
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/0234—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons
- G05D1/0236—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using optical markers or beacons in combination with a laser
-
- 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/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/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
- G05D1/0251—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 extracting 3D information from a plurality of images taken from different locations, e.g. stereo vision
-
- 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
-
- 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/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0289—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling with means for avoiding collisions between vehicles
-
- 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/0287—Control of position or course in two dimensions specially adapted to land vehicles involving a plurality of land vehicles, e.g. fleet or convoy travelling
- G05D1/0291—Fleet control
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Remote Sensing (AREA)
- Radar, Positioning & Navigation (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Optics & Photonics (AREA)
- Multimedia (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种基于四足机器人的自主探索和建图系统，将四足机器人的机载传感器检测的周围的环境信息分割为多个子空间；使用遍历路径生成算法得到遍历各个子空间的最短遍历路径即全局路径；在四足机器人为中心的局部区域内均匀采样得到局部探索点并评价选出较优的探索点集合；针对探索点集合利用遍历路径生成算法得到遍历各个探索点的最短的局部路径，与全局路径结合得到完整的路径；根据机载传感器信息得到局部可通行性地图，从局部路径中提取局部目标点使用A*算法得到粗略的最短无碰撞路径作为轨迹初始值，利用轨迹优化方法得到平滑轨迹用于计算四足机器人的运动控制指令实时控制；判断是否还存在未探索的子空间，如存在则返回步骤一重复探索直至结束。
Description
技术领域
本发明属于机器人的技术领域，具体涉及一种基于四足机器人的自主探索和建图系统。
背景技术
近年来，随着传感器技术和人工智能等技术的快速发展,移动机器人在日常的生产生活中有着更加广泛的应用，尤其在军事探查、探险救援等领域发挥着越来越重要的作用。之前的研究大多是预先对周围环境进行建模，得到已知地图后再进行路径规划，但是实际应用中，机器人通常面对的是环境信息未知或者部分未知的情况，比如灾后救援、地形勘探等等，并且人员如果直接进入会出现伤亡等严重的后果，为了避免这种情况，可以让机器人先对未知的环境进行自主探索并且建图，方便人员开展工作。因此，移动机器人在未知环境中的自主探索有着重要的意义。
目前已知的已经存在一些关于自主探索问题的研究。如中国专利申请号CN201911374291.X的发明专利公开了一种基于边界的自主探索方法，该方法通过获取激光点云数据来提取地面信息，进而检测出边界，转换为移动机器人的目标点，但是该方法没有介绍如何针对选出的边界来提取探索目标点；中国专利号CN202110582540.5公开了一种复合式边界检测的机器人自主探索方法，该方法使用采样的方法来提取边界点，并对其进行筛选转换为探索目标点，但是该方法适合只考虑目前的规划范围，仅规划局部路径，没有考虑全局的最优性，会出现路径冗余度较高、地图完整度较低的问题。
此外，在上述场景中，比如灾后现场、洞穴、丛林等，地形杂乱、不平坦并且崎岖，如果使用传统的轮式或者履带式机器人，至多能涉足一半的环境，无法实现自主探索任务。而四足机器人作为一种特殊类型的机器人，有着高效的机动性和灵活的横向运动能力。尤其在未知环境的探索中，面对变化的地形，四足机器人有着优越性和不可替代性。
因此现有的机器人的自主探索方法仍然存在缺陷。
发明内容
为了解决现有机器人应用中存在的上述问题，提供一种基于四足机器人的自主探索和建图系统，为对周围环境实现高质量的感知和建图，分为全局探索和局部探索、轨迹优化三个层面，能够实时、自主、鲁棒地实现对环境的探索。
为了达到上述发明目的，本发明采用以下技术方案：一种基于四足机器人的自主探索和建图系统，包括以下步骤：
步骤一、利用四足机器人的机载传感器检测周围的环境信息，并将周围的环境信息分割为多个子空间；
步骤二、使用遍历路径生成算法得到一条遍历各个子空间的最短遍历路径，即全局路径；
步骤三、在以四足机器人为中心的局部区域内，通过均匀采样得到局部探索点，并引入包括信息增益和定位精度因素的信息理论来评价局部探索点，选出较优的探索点集合；
步骤四、根据较优的探索点集合，利用遍历路径生成算法得到遍历各个探索点的最短的局部路径，局部路径与全局路径结合得到完整的路径；
步骤五、在以四足机器人为中心的局部区域内，根据机载传感器信息得到局部可通行性地图，并从局部路径中提取局部目标点，使用A*算法得到一条粗略的最短无碰撞路径；
步骤六、将最短无碰撞路径作为轨迹初始值，利用基于四足机器人模型的轨迹优化方法得到平滑轨迹；
步骤七、使用滚动时域控制策略，利用平滑轨迹计算四足机器人的运动控制指令，实时控制机器人行进；
步骤八、判断是否还存在未探索的子空间，如存在则返回步骤一重复探索步骤，直至不存在未探索的子空间结束。
进一步的，所述步骤一具体为：
S11、检测周围环境，四足机器人的机载传感器包含多线激光雷达、惯性传感器和深度相机，利用机载传感器感知周围环境，周围环境包括已知无障碍区域、已知有障碍区域和未知区域，周围环境的边界定义为已知无障碍区域和其他区域，其他区域包含未知区域和含已知有障碍物的区域；
S12、将检测到的周围的环境分割为多个子空间；
S13、为了实现完整探索地图，需要将包含边界的区域即未完成探索的区域进行标记，标记为正在探索的子空间。
进一步的，所述步骤二具体为，
S21、根据当前环境中正在探索的子空间，使用A*算法计算各个子空间中心点的最短无碰撞路径的长度；
S22、将遍历各个子空间的问题建模为旅行商问题，构建带权无向图G＝(V,w)，带权无向图的顶点是各个子空间的顶点，边的权值为两个顶点间最短路径的长度；
S23、根据带权无向该图，使用克里斯托菲德斯算法得到一条遍历各个子空间中心点的最短遍历路径，即全局路径。
进一步的，所述步骤三具体为，
S31、在以四足机器人为中心的局部区域内，通过均匀采样的方法得到一系列局部探索点，为了实现高质量建图，要求探索点在一定距离范围和角度范围内，根据该要求对局部探索点进行筛选；
S32、针对筛选后的局部探索点，引入信息理论对其进行评估，信息理论主要考虑信息增益、定位精度和通行成本，使用优先队列选出较优的探索点集合。
进一步的，所述步骤四具体为，对于选出的探索点集合，使用克里斯托菲德斯算法得到一条遍历各个局部探索点的最短的局部路径；并且将局部路径和全局路径根据周围环境信息进行适应性结合，从而得到完整的路径。
进一步的，所述步骤五具体为，
S51、在以四足机器人为中心的局部区域内，根据机载传感器的信息利用地形评估函数得到局部可通行性地图，即包含通行性代价的栅格地图；
S52、将四足机器人建模为质点模型，从局部路径中提取局部目标点，使用A*算法将地形可通行性考虑在内，得到从当前位置到局部目标点的粗略的最短无碰撞路径。
进一步的，所述步骤六具体为，
S61、对四足机器人的动力学模型进行简化，建立支撑腿的地面反作用力和运动学参数的映射关系；
S62、以步骤五得到的最短无碰撞路径作为轨迹初始值，利用基于四足机器人模型的轨迹优化方法得到平滑轨迹；基于四足机器人模型的轨迹优化方法具体为：综合考虑地面反作用力约束、运动学参数和障碍物约束，使用非线性优化算法得到平滑的轨迹路径，并且可以被控制模块执行。
进一步的，所述步骤七具体为，利用步骤六的平滑轨迹来计算四足机器人的控制指令，控制指令包含速度和加速度；滚动时域控制策略具体为：将计算的第一组期望控制指令发送给步态控制器，步态控制器对四足机器人进行实时控制。
进一步的，所述步骤八具体为，返回步骤一，如果无法分割出未完成探索的子空间，则认为探索任务结束；否则从步骤一开始重复探索步骤，继续探索未完成探索的子空间。
采用本发明技术方案，其有益效果为：通过利用四足机器人的进行探索，采用本发明的系统，分为全局遍历路径规划和局部探索点的遍历路径规划，并且使用结合四足机器人动力学模型的轨迹优化方法，既保证了对环境探索的完整度和感知建图的质量，又保证了规划算法的效率和实时性，并使用在线滚动时域控制，提高了实际探索过程的鲁棒性；该系统既能保证全局探索的完整度，又能提高感知建图的质量，同时保证了轨迹的可执行性，使四足机器人可以在各种复杂地形中充分发挥其运动能力，高效地实现对未知环境的探索。
附图说明
图1a是绝影X20四足机器人的立体图；
图1b是绝影X20四足机器人硬件安装的结构示意图；
图2是自主探索和建图系统的软件系统和结构示意图；
图3是自主探索算法的子空间、全局路径和局部路径的示意图；
图4a是自主探索过程中的全局示意图；
图4b是图4a中对应位姿的局部可通行地图和对应生成的轨迹序列图；
图5a是针对室内办公环境应用本发明的系统得到的自主探索路径；
图5b是针对室内办公环境应用本发明的系统得到的点云地图。
具体实施方式
结合附图对本发明具体方案具体实施例作进一步的阐述，使得本技术方案更加清楚、明白。本领域技术人员可由本说明书所揭露的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本发明的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。
本实施例涉及一种基于四足机器人的自主探索和建图系统。如图1a和1b所示为本实施例采用的四足机器人绝影X20的结构图，在四足机器上使用的机载传感器，包括一台速腾聚创的16线旋转式激光雷达，在机器人顶部前侧水平放置，垂直视场角-15°～15°，可探测最远100m的障碍物；两台立体视觉深度相机，型号分别为英特尔的Realsense D435i和英特尔的Realsense D4355，可用于检测2m距离内的地面障碍物；一台惯性测量传感器(IMU)Xsens MTi-300用于获取机器人身体的姿态角和运动状态；一个提供传感器同步信号的同步主板，以及一台Intel NUC小型计算机用于运行机器人的感知以及路径规划算法。
如图2-图5b所示，本自主探索和建图系统的具体实现步骤如下。
步骤一：四足机器人自带的机载传感器包括激光雷达、深度相机两种类型的传感器，既能检测近处的地形，又可以感知较远的环境。由于单帧数据感知范围有限，所以会对多帧的历史点云进行拼接，针对拼接后的点云地图，我们将周围环境分为已知无障碍区域、已知有障碍区域和未知区域，边界定义为已知无障碍区域和其他区域，其他区域既包含未知区域也包含已知但有障碍物的区域。将检测到的周围的环境分割为多个子空间，如图4a中实体方块均为未完成探索的子区间，标记该子区间有助于实现全局的完整性。
步骤二：根据检测到的未完成探索的子区间，通过A*算法得到各个子区间中心点之间的最短无碰撞路径以及长度，对基础A*算法进行改进，提高搜索的效率，优化路径长度。
A*算法计算地图中某个节点n的总代价函数，即估计从起点经过节点n到达终点的最小成本，即：
f(n)＝g(n)+h(n)
其中g(n)是从起点到当前节点n的最小成本，h(n)是估计的从当前节点n到终点的最小成本，本发明对h(n)进行改进：
首先计算两个节点的最短移动距离：
Δx＝abs(ncurr，x-nend，x)
Δy＝abs(ncurr，y-nend，y)
由于转向运动对四足机器人腿足的执行机构带来更多的负担，所以我们更倾向于使四足机器人沿着直线运动，所以计算起点-终点的矢量角和当前节点-终点的矢量角的角度差：
Δx1＝ncurr，x-nend，x
Δy1＝ncurr，y-nend，y
Δx2＝nstart，x-nend，x
Δy2＝nstart，y-nend，y
结合上述公式确定估计代价函数h(n)为：
其中α是一个可调节的参数，用来调节对转向运动增加的成本比例。根据该函数来改进基础A*算法，使用A*算法计算各个子空间中心点的最短路径及长度。
本实施例中的遍历路径生成算法采用的是克里斯托菲德斯算法。
将遍历各个子空间的问题建模为旅行商问题，根据A*算法计算出的最短路径首先构建带权无向图G＝(V，w)，图的顶点是各个子空间的顶点，边的权值为两个顶点间最短路径的长度；根据该带权无向图，使用克里斯托菲德斯算法得到一条遍历各个子空间中心点的最短遍历路径，即全局路径。
克里斯托菲德斯算法具体流程为：(1)以任意顶点作为起点，在多项式时间内得到最小生成树；(2)得到所有度数为奇数的顶点(奇度顶点)的集合；(3)对于奇度顶点的集合建立最小完全匹配模型，找到最小成本匹配；(4)得到一个欧拉图，基于该图找到欧拉回路，在欧拉回路中去除重复的点，只保留第一次出现的顶点，找到哈密尔顿圈，即找到遍历路径，即全局路径。A*算法得到的路径是最短的，克里斯托菲德斯算法得到的路径是遍历路径，结合就是最短遍历路径，通过上面步骤完成对基础A算法的改进。
步骤三：在当前规划范围内(局部区域内，即以四足机器人为中心，机载传感器的当前感知范围为依据)，通过均匀采样的方法得到一系列局部探索点，为了实现高质量建图，要求探索点在一定距离范围和角度范围内，首先依据该要求对上面的一系列局部探索点筛选；
|p-pf|≤d
对于筛选后的局部探索点，引入信息理论对其进行评估，主要考虑信息增益、定位精度和通行成本等因素来计算收益值。信息增益I被定义为在局部探索点时预期观察到的未知区域的面积，引入信息论中的熵来计算。对于探索点v来说，将传感器对环境的感知建模为以该位姿为起点的一组射线Rv，对于每条射线r来说，在到达其终点过程中经过的所有栅格为Gr，每个栅格g的信息增益为Ig，那么该点v的信息增益
是所有射线经过的所有栅格的信息增益的总和：
对于每个栅格g来说，其占据概率为Po(g)，那么从信息理论角度来说，该栅格的熵为：
H(g)＝-Po(g)lnPo(g)-(1-Po(g))ln(1-Po(g))
栅格gn的可见性概率为沿着射线已经经过的所有栅格的占用概率的乘积：
栅格gn的信息增益为可见性概率和熵的乘积：
Ig＝Pv(g)H(g)
定位精度
即机器人对自身位姿的估计精度，引入信息理论的费雪矩阵：
对机器人的位姿P的导数分解为对x，y，θ三个方向的导数，即
得到费雪信息矩阵：
使用D(P)的行列式作为评价定位精度的指标，即：
通行成本
定义为四足机器人从当前位置到局部探索点的预期移动成本。综上，定义局部探索点的收益R为：
步骤四：将探索点的收益值R作为优先值加入到优先队列中，之后从队列中选取候选点加入到探索点集合中，被选到的概率和其收益值成正比关系，当选择一个探索点后，其他探索点收益值会受该探索点的影响，所以需要对其余探索点的收益值重新计算。当优先队列为空或者边际收益小于阈值后，不再选择新的探索点。
对于选出的探索点集合，同样使用最短遍历路径生成算法得到遍历各个探索点的路径，即首先用A*算法得到各个探索点的最短路径，再根据该路径构成带权无向图G＝(V，w)，使用克里斯托菲德斯算法得到一条遍历各个局部探索点的最短遍历路径，即局部路径。
接着需要将全局路径和局部路径结合得到完整路径，具体方式为：计算全局路径和当前规划范围的连接点，将两个连接点作为局部路径的起点和终点，即实现根据环境的自适应性结合。
步骤五：在更加精细的局部范围内根据地形评估函数得到局部可通行性地图，即包含通行性代价的栅格地图；地形评估函数为：
其中，粗糙度r表示地形的不平整度和粗糙度，比如室外的草地、碎石堆等，通常根据栅格内所有点云的方差来计算；坡度s表示地形的倾斜程度，比如斜坡等，通过斜率来计算；高度h表示当前栅格相对于当前区域的高度差。三个特征对应的阈值rthre，sthre和hthre每一项允许的最大值，需要根据四足机器人的运动能力来设置，设置为机器人能够达到的最大不平整度、倾斜程度和高度。如果某一项超过其最大值，那么该栅格的可通行性最低，可通行性成本Ct最高，设置为1。
在局部轨迹优化之前，从局部路径中提取局部目标点，并且将四足机器人建模为质点模型，使用改进A*算法，根据可通行性地图计算出的成本值，得到一条当前位置到局部目标点的最短无碰撞路径。由于周围环境的变化和机器人移动的不确定性，在每个规划周期内更新当前的地图，当可通行性地图更新或者局部目标点变化时重新生成局部路径，作为后续轨迹优化的初始值。
步骤六：对四足机器人的动力学模型进行简化，建立支撑腿的地面反作用力和运动学参数的映射关系；假定地面反作用力是作用四足机器人上的唯一外力，机器人质心CoM(Centre of Mass)处的线速度
线加速度
角速度ωb、角加速度
的计算方法如下：
其中
为机器人的质量，
为重力加速度向量，
为中心转动惯量，
为机器人质心到第i个支撑腿的足端的向量。
将局部轨迹优化问题中建模为非线性优化问题，即将轨迹表示为包含一系列的n个机器人位姿xi＝(xi，yi，θi)T以及n-1个时间间隔
其中
表示机器人在栅格地图中的二维坐标，θi∈S1表示机器人的方向角，ΔTi表示机器人从位姿xi运动到下一位姿xi+1的时间间隔：
主要考虑的约束为：(1)动力学模型约束，即将地面反作用力在世界坐标系下分解为三个分量，需要满足摩擦锥约束：
|fz|≤fmax，|fx|≤μ|fz|，|fy|≤μ|fz|
(2)运动学约束，将线性速度在机器人坐标系下被分解为纵向的vx和横向的vy，则需要满足如下不等式约束：
vmin≤vx≤vmax
|vy|≤|vbound|，|ω|≤|ωbound|
其中不同速度的阈值需要根据实际机器人的情况来设定。(3)障碍物约束，即要考虑到障碍物的最小距离，需要满足如下不等式约束，其阈值同样需要根据实际机器人的情况来设定。
dk≥dmin
由于希望四足机器人快速移动的同时，保持运动的平稳，所以将轨迹优化的目标函数设置为最小化运动时间间隔的平方和以及加速度的平方和：
以步骤五得到的粗糙的离散路径作为轨迹初始值，并综合考虑以上约束和目标函数，即：
a＝[F2(vi)，F2(ωi)]T
c＝[ΔTi，||ai||]T
其中a、b、c分别代表不同的优化目标项：运动动力学约束的目标函数、障碍物约束的目标函数和目标函数。
使用非线性优化算法得到平滑的轨迹，并且可以被控制模块执行；既发挥了灵活的运动特性，又保证了轨迹的安全可执行。
步骤七：基于步骤六得到的轨迹来计算四足机器人的速度和加速度，将计算的第一组期望控制量发送给步态控制器，实现对四足机器人的实时控制。
步骤八：在步骤一中，如果无法分割出未完成探索的子空间，则认为探索任务结束；否则从步骤一开始重复步骤，继续探索未完成探索的子空间。判断方法为：如果未完成探索的子空间的数量为0，则不存在未完成探索的子空间，进而结束探索过程。
图5a是用四足机器人使用该方法在室内环境中的探索情况，该环境大约为45米*55米，图中的实现为探索路径，路径长度为482米，探索时间为572秒。图5b是探索完成后的建图情况，可以看到建图较为完整。
本系统根据四足机器人的机载传感器检测到周围环境信息，并自主规划路线实现对环境的自主探索和实时建图。实现方法为：(1)结合感知的分层的自主探索算法：将该问题分解为基于稀疏地图的全局路径规划和考虑感知的局部路径规划，全局层面遍历各个未完成探索的子空间，可以有效地提高建图的完整度；局部层面中引入信息理论来评价局部探索点的感知质量,可以实现对当前规划范围的高质量感知。并且两层规划算法根据环境自适应性结合,大大减少了往复的冗余路径,提高了探索的速度和效率。(2)根据四足机器人的模型来优化不同地形的局部轨迹：将动力学约束和运动学、障碍物约束等一起考虑到优化算法中,可以得到平滑、可行的轨迹；并且定量衡量地形的可通行性,满足不同地形的适应性。(3)完整的自主探索系统：该系统结合自主探索算法和轨迹优化算法，部署在四足机器人上，在实际测试中实现了实时、自主且鲁棒的探索效果,并且可以适应多种复杂地形。
本实施例涉及注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。
Claims (9)
1.一种基于四足机器人的自主探索和建图系统，其特征在于：包括以下步骤：
步骤一、利用四足机器人的机载传感器检测周围的环境信息，并将周围的环境信息分割为多个子空间；
步骤二、使用遍历路径生成算法得到一条遍历各个子空间的最短遍历路径，即全局路径；
步骤三、在以四足机器人为中心的局部区域内，通过均匀采样得到局部探索点，并引入包括信息增益和定位精度因素的信息理论来评价局部探索点，选出较优的探索点集合；
步骤四、根据较优的探索点集合，利用遍历路径生成算法得到遍历各个探索点的最短的局部路径，局部路径与全局路径结合得到完整的路径；
步骤五、在以四足机器人为中心的局部区域内，根据机载传感器信息得到局部可通行性地图，并从局部路径中提取局部目标点，使用A*算法得到一条粗略的最短无碰撞路径；
步骤六、将最短无碰撞路径作为轨迹初始值，利用基于四足机器人模型的轨迹优化方法得到平滑轨迹；
步骤七、使用滚动时域控制策略，利用平滑轨迹计算四足机器人的运动控制指令，实时控制机器人行进；
步骤八、判断是否还存在未探索的子空间，如存在则返回步骤一重复探索步骤，直至不存在未探索的子空间结束。
2.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤一具体为：
S11、检测周围环境，四足机器人的机载传感器包含多线激光雷达、惯性传感器和深度相机，利用机载传感器感知周围环境，周围环境包括已知无障碍区域、已知有障碍区域和未知区域，周围环境的边界定义为已知无障碍区域和其他区域，其他区域包含未知区域和含已知有障碍物的区域；
S12、将检测到的周围的环境分割为多个子空间；
S13、为了实现完整探索地图，需要将包含边界的区域即未完成探索的区域进行标记，标记为正在探索的子空间。
3.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤二具体为，
S21、根据当前环境中正在探索的子空间，使用A*算法计算各个子空间中心点的最短无碰撞路径的长度；
S22、将遍历各个子空间的问题建模为旅行商问题，构建带权无向图G＝(V,w)，带权无向图的顶点是各个子空间的顶点，边的权值为两个顶点间最短路径的长度；
S23、根据带权无向该图，使用克里斯托菲德斯算法得到一条遍历各个子空间中心点的最短遍历路径，即全局路径。
4.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤三具体为，
S31、在以四足机器人为中心的局部区域内，通过均匀采样的方法得到一系列局部探索点，为了实现高质量建图，要求探索点在一定距离范围和角度范围内，根据该要求对局部探索点进行筛选；
S32、针对筛选后的局部探索点，引入信息理论对其进行评估，信息理论主要考虑信息增益、定位精度和通行成本，使用优先队列选出较优的探索点集合。
5.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤四具体为，对于选出的探索点集合，使用克里斯托菲德斯算法得到一条遍历各个局部探索点的最短的局部路径；并且将局部路径和全局路径根据周围环境信息进行适应性结合，从而得到完整的路径。
6.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤五具体为，
S51、在以四足机器人为中心的局部区域内，根据机载传感器的信息利用地形评估函数得到局部可通行性地图，即包含通行性代价的栅格地图；
S52、将四足机器人建模为质点模型，从局部路径中提取局部目标点，使用A*算法将地形可通行性考虑在内，得到从当前位置到局部目标点的粗略的最短无碰撞路径。
7.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤六具体为，
S61、对四足机器人的动力学模型进行简化，建立支撑腿的地面反作用力和运动学参数的映射关系；
S62、以步骤五得到的最短无碰撞路径作为轨迹初始值，利用基于四足机器人模型的轨迹优化方法得到平滑轨迹；基于四足机器人模型的轨迹优化方法具体为：综合考虑地面反作用力约束、运动学参数和障碍物约束，使用非线性优化算法得到平滑的轨迹路径，并且可以被控制模块执行。
8.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤七具体为，利用步骤六的平滑轨迹来计算四足机器人的控制指令，控制指令包含速度和加速度；滚动时域控制策略具体为：将计算的第一组期望控制指令发送给步态控制器，步态控制器对四足机器人进行实时控制。
9.根据权利要求1所述的一种基于四足机器人的自主探索和建图系统，其特征在于：所述步骤八具体为，返回步骤一，如果无法分割出未完成探索的子空间，则认为探索任务结束；否则从步骤一开始重复探索步骤，继续探索未完成探索的子空间。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210048450.2A CN114442621A (zh) | 2022-01-17 | 2022-01-17 | 一种基于四足机器人的自主探索和建图系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202210048450.2A CN114442621A (zh) | 2022-01-17 | 2022-01-17 | 一种基于四足机器人的自主探索和建图系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN114442621A true CN114442621A (zh) | 2022-05-06 |
Family
ID=81368454
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202210048450.2A Withdrawn CN114442621A (zh) | 2022-01-17 | 2022-01-17 | 一种基于四足机器人的自主探索和建图系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114442621A (zh) |
Cited By (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115129057A (zh) * | 2022-07-07 | 2022-09-30 | 福州大学 | 基于通行效率评估的机器人分层式自主探索方法 |
| CN115167433A (zh) * | 2022-07-22 | 2022-10-11 | 华南理工大学 | 融合全局视觉的机器人自主探索环境信息的方法及系统 |
| CN115456249A (zh) * | 2022-08-16 | 2022-12-09 | 上海聚水潭网络科技有限公司 | 一种分拣行走路径优化方法及系统 |
| CN115866807A (zh) * | 2022-11-17 | 2023-03-28 | 华东交通大学 | 一种基于地形信息的无线传感器网络节点部署方法 |
| CN116263605A (zh) * | 2023-04-21 | 2023-06-16 | 杭州国辰机器人科技有限公司 | 基于非线性优化的移动机器人平滑、实时碰撞规避方法 |
| CN116449391A (zh) * | 2023-04-17 | 2023-07-18 | 深圳直角设计工程有限公司 | 一种基于3d点云的室内全景成像方法与系统 |
| CN116578088A (zh) * | 2023-05-04 | 2023-08-11 | 浙江大学 | 室外自主移动机器人连续轨迹生成方法、系统、介质及设备 |
| CN115129057B (zh) * | 2022-07-07 | 2024-06-07 | 福州大学 | 基于通行效率评估的机器人分层式自主探索方法 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150314443A1 (en) * | 2014-04-30 | 2015-11-05 | Shenzhen Mercury Optoelectronics Research Institute | Visual-based obstacle detection method and apparatus for mobile robot |
| CN110414722A (zh) * | 2019-07-08 | 2019-11-05 | 中国人民解放军陆军工程大学 | 基于能耗公平的无人机协同侦察路径规划方法 |
| US20200158526A1 (en) * | 2018-11-19 | 2020-05-21 | International Business Machines Corporation | Combined route planning and opportunistic searching in variable cost enviroments |
| CN111487960A (zh) * | 2020-03-26 | 2020-08-04 | 北京控制工程研究所 | 一种基于定位能力估计的移动机器人路径规划方法 |
| WO2020233724A1 (zh) * | 2019-05-23 | 2020-11-26 | 全球能源互联网研究院有限公司 | 一种基于视觉slam的电网作业环境地图的构建方法及系统 |
| CN113110522A (zh) * | 2021-05-27 | 2021-07-13 | 福州大学 | 一种基于复合式边界检测的机器人自主探索方法 |
| CN113703462A (zh) * | 2021-09-02 | 2021-11-26 | 东北大学 | 一种基于四足机器人的未知空间自主探索系统 |
| CN113805590A (zh) * | 2021-09-23 | 2021-12-17 | 云南民族大学 | 一种基于边界驱动的室内机器人自主探索方法及系统 |
| CN113848912A (zh) * | 2021-09-28 | 2021-12-28 | 北京理工大学重庆创新中心 | 基于自主探索的室内地图建立方法及装置 |
-
2022
- 2022-01-17 CN CN202210048450.2A patent/CN114442621A/zh not_active Withdrawn
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US20150314443A1 (en) * | 2014-04-30 | 2015-11-05 | Shenzhen Mercury Optoelectronics Research Institute | Visual-based obstacle detection method and apparatus for mobile robot |
| US20200158526A1 (en) * | 2018-11-19 | 2020-05-21 | International Business Machines Corporation | Combined route planning and opportunistic searching in variable cost enviroments |
| WO2020233724A1 (zh) * | 2019-05-23 | 2020-11-26 | 全球能源互联网研究院有限公司 | 一种基于视觉slam的电网作业环境地图的构建方法及系统 |
| CN110414722A (zh) * | 2019-07-08 | 2019-11-05 | 中国人民解放军陆军工程大学 | 基于能耗公平的无人机协同侦察路径规划方法 |
| CN111487960A (zh) * | 2020-03-26 | 2020-08-04 | 北京控制工程研究所 | 一种基于定位能力估计的移动机器人路径规划方法 |
| CN113110522A (zh) * | 2021-05-27 | 2021-07-13 | 福州大学 | 一种基于复合式边界检测的机器人自主探索方法 |
| CN113703462A (zh) * | 2021-09-02 | 2021-11-26 | 东北大学 | 一种基于四足机器人的未知空间自主探索系统 |
| CN113805590A (zh) * | 2021-09-23 | 2021-12-17 | 云南民族大学 | 一种基于边界驱动的室内机器人自主探索方法及系统 |
| CN113848912A (zh) * | 2021-09-28 | 2021-12-28 | 北京理工大学重庆创新中心 | 基于自主探索的室内地图建立方法及装置 |
Non-Patent Citations (1)
| Title |
|---|
| ZHEN ZHANG等: "Efficient Motion Planning Based on Kinodynamic Model for Quadruped Robots Following Persons in Confined Spaces", 《IEEEASME TRANSACTIONS ON MECHATRONICS》 * |
Cited By (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115129057A (zh) * | 2022-07-07 | 2022-09-30 | 福州大学 | 基于通行效率评估的机器人分层式自主探索方法 |
| CN115129057B (zh) * | 2022-07-07 | 2024-06-07 | 福州大学 | 基于通行效率评估的机器人分层式自主探索方法 |
| CN115167433B (zh) * | 2022-07-22 | 2024-03-19 | 华南理工大学 | 融合全局视觉的机器人自主探索环境信息的方法及系统 |
| CN115167433A (zh) * | 2022-07-22 | 2022-10-11 | 华南理工大学 | 融合全局视觉的机器人自主探索环境信息的方法及系统 |
| CN115456249A (zh) * | 2022-08-16 | 2022-12-09 | 上海聚水潭网络科技有限公司 | 一种分拣行走路径优化方法及系统 |
| CN115456249B (zh) * | 2022-08-16 | 2024-03-22 | 上海聚水潭网络科技有限公司 | 一种分拣行走路径优化方法及系统 |
| CN115866807A (zh) * | 2022-11-17 | 2023-03-28 | 华东交通大学 | 一种基于地形信息的无线传感器网络节点部署方法 |
| CN115866807B (zh) * | 2022-11-17 | 2023-10-27 | 华东交通大学 | 一种基于地形信息的无线传感器网络节点部署方法 |
| CN116449391A (zh) * | 2023-04-17 | 2023-07-18 | 深圳直角设计工程有限公司 | 一种基于3d点云的室内全景成像方法与系统 |
| CN116449391B (zh) * | 2023-04-17 | 2024-05-17 | 深圳直角设计工程有限公司 | 一种基于3d点云的室内全景成像方法与系统 |
| CN116263605A (zh) * | 2023-04-21 | 2023-06-16 | 杭州国辰机器人科技有限公司 | 基于非线性优化的移动机器人平滑、实时碰撞规避方法 |
| CN116578088B (zh) * | 2023-05-04 | 2024-02-09 | 浙江大学 | 室外自主移动机器人连续轨迹生成方法及系统 |
| CN116578088A (zh) * | 2023-05-04 | 2023-08-11 | 浙江大学 | 室外自主移动机器人连续轨迹生成方法、系统、介质及设备 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN114442621A (zh) | 一种基于四足机器人的自主探索和建图系统 | |
| WO2017028653A1 (zh) | 一种移动机器人室内自建地图的方法和系统 | |
| Frey et al. | Locomotion policy guided traversability learning using volumetric representations of complex environments | |
| CN111474953B (zh) | 多动态视角协同的空中目标识别方法及系统 | |
| CN111258316B (zh) | 一种动态环境下趋势感知的机器人轨迹规划方法 | |
| CN112684807A (zh) | 无人机集群三维编队方法 | |
| Levine et al. | Information-rich path planning with general constraints using rapidly-exploring random trees | |
| CN114022824A (zh) | 一种面向狭窄环境的四足机器人运动规划方法 | |
| CN114998276B (zh) | 一种基于三维点云的机器人动态障碍物实时检测方法 | |
| Pan et al. | GPU accelerated real-time traversability mapping | |
| CN112762957A (zh) | 一种基于多传感器融合的环境建模及路径规划方法 | |
| KR20210063791A (ko) | 장애물의 특성을 고려한 dqn 및 slam 기반의 맵리스 내비게이션 시스템 및 그 처리 방법 | |
| CN112652001B (zh) | 基于扩展卡尔曼滤波的水下机器人多传感器融合定位系统 | |
| KR20160048530A (ko) | 자율 이동 차량의 경로 생성 방법 및 경로 생성 장치 | |
| CN114119920A (zh) | 三维点云地图构建方法、系统 | |
| Li et al. | Hybrid filtering framework based robust localization for industrial vehicles | |
| CN113110455A (zh) | 一种未知初始状态的多机器人协同探索方法、装置及系统 | |
| CN115639823A (zh) | 崎岖起伏地形下机器人地形感知与移动控制方法及系统 | |
| Choi et al. | Improved CNN-based path planning for stairs climbing in autonomous UAV with LiDAR sensor | |
| Leng et al. | Multi-UAV surveillance over forested regions | |
| CN116136417B (zh) | 一种面向越野环境的无人驾驶车辆局部路径规划方法 | |
| Helble et al. | 3-d path planning and target trajectory prediction for the oxford aerial tracking system | |
| CN113433937A (zh) | 基于启发式探索的分层导航避障系统、分层导航避障方法 | |
| CN116352722A (zh) | 多传感器融合的矿山巡检救援机器人及其控制方法 | |
| Wettergreen et al. | Developing a framework for reliable autonomous surface mobility |
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 | ||
| WW01 | Invention patent application withdrawn after publication | ||
| WW01 | Invention patent application withdrawn after publication |
Application publication date: 20220506 |