本发明涉及一种基于变构型感知装置的足式机器人导航及定位方法，属于足式机器人技术领域。本发明通过设计可变的多传感外参标定、基于激光数据的全局地图构建与定位、基于变构型标定的局部及全局稠密地图构建、基于全局定位的路径导航流程，提出了一种基于变构型感知装置的足式机器人导航及定位方法，解决了足式机器人在未知地形无法完成全局及局部地图不同视角切换的问题，增强了足式机器人感知系统的鲁棒性。
1.一种基于变构型感知装置的足式机器人导航及定位方法，其特征在于，包括以下步骤：
步骤一、实现构型可变的多传感外参标定用于将变构型感知装置的激光雷达、摄像头和惯性测量单元IMU通过传感器外参标定算法来实现信息源的最优化融合；
步骤二、基于步骤一实现的构型可变的多传感外参标定，实现基于激光数据的全局地图构建与定位；
步骤三、根据步骤二中的全局地图构建与定位，实现的基于变构型标定的局部及全局稠密地图构建；
步骤四、根据步骤三中构建的全局稠密地图，实现基于全局定位的路径导航；
步骤1：根据雷达构型变化角度采用最优化的方法优化外参带来的误差项的残差，优化并求解任意激光间的外参，完成构型可变的双激光外参标定；
步骤2：根据步骤1得到的双激光外参标定参数，提出基于时空解耦的激光‑相机外参标定方法，消除多传感间的共同视野及时间同步局限性，基于采集标定板的激光传感器数据和视觉图像数据建立视觉3D点云，模拟激光和相机的全方位信息完成标定；
步骤3：根据步骤2的激光和相机的全方位标定信息，提出基于相机‑惯性测量单元融合的外参标定算法，通过在相机图像中提取特征点完成里程计估计，通过图优化算法进行耦合优化，实现相机‑惯性测量单元外参的标定；
步骤二中，通过实时获取的激光、IMU和里程计数据推算相对于里程计坐标系的相对位姿并构建局部地图，通过匹配局部地图与已构建的环境地图，实时更新里程计坐标系与全局地图坐标系间的相对位姿，从而实现实时的足式机器人定位；
步骤二中提出的基于激光数据的全局地图构建与定位的流程为：
流程一：建立基于拓扑度量的定位框架，基于激光的SLAM根据以一定频率获取的激光数据信息，利用迭代最近点算法ICP估计每一帧激光相对于当前地图的相对位姿，并基于位姿将激光点云数据融入地图中，建立基于激光雷达的定位框架；
流程二：根据流程一的激光定位框架，提出基于滑动窗口优化的自主定位方法，选择以当前位姿为准的一个时间窗口，并将该窗口内的里程计信息、地图信息、激光雷达点云信息进行融合，形成局部定位的优化问题，完成在子地图上的定位，其里程优化代价函数为如下所示：式中， 为里程计前两个时刻信息轨迹的姿态，Xt, 为激光雷达点云的当
依赖相对里程和上一时刻的定位，可以获得当前位置相对于子地图的初值，通过ICP将当前获得的激光和子地图进行点云匹配，生成优化函数如下所示：式中，Lt,Lt‑1为点云与子地图匹配的当前时刻和前一时刻的相对位姿， 为所有子地图之间的相对位姿的集合，Xt,Xt‑1为激光雷达点云的当前时刻和前一时刻的位姿数据；
通过地图构建，获得地图之间的相对位姿关系，此优化函数如下所示：
式中， 为地图之间的相对位姿， Mj为所有子地图之间的相对位姿的集合；
将公式1、2、3的三项优化代价函数进行迭代优化，得到机器人当前位姿相对于当前子地图的最优位姿，即当前里程与地图相对位姿关系的位姿优化函数如下所示：步骤三中，根据生成的全局地图定位信息，通过激光定位将数据发布到里程计，下一步通过激光及IMU里程计融合数据发布反馈到足式机器人参与控制；
步骤31：稠密地图的局部高度地图由距离传感器获得的点云生成，并结合距离传感器获取数据方差和机器人运动状态转移的不确定度进行更新，从而获取准确的局部稠密高度地图；基于已建立完成的局部稠密高度地图完成固定及障碍物检测并判断可行域；基于局部稠密高度地图，通过运动学和惯性测量单元进行本体状态估计定位，并进行地图更新融合来使得局部稠密高度地图更加精确，将地形高程图估计值 通过扩展卡尔曼滤波的方式融合到高度测量值式中， 为前一时刻障碍的高度估计值，为当前时刻障碍的高度测量值，σp,σh, 为高程图估计信息的标准差；
步骤32：利用步骤31计算得到的地形高程图估计值 及高度测量值 实现基
于变构型标定的全局稠密地图构建，基于对局部稠密地图的扩展将部分稠密地图融合到全局一致的坐标系下，实现全局稠密地图的构建；全局地图由子图组成并持续维护，根据SLAM的位姿优化实现子图相应优化，对于地图中的每个栅格，提取其表面法向量和与附近栅格的高度差作为局部及全局地图的表面特征；通过地形特征加权评估地图中每个栅格的可行域权值，完成局部地图及全局地图的导航路径及落足点规划，重新构建出一个全局稠密地图；根据不同的任务属性，选择不同的距离传感器作为系统输入，SLAM系统为建图过程提供实时的位姿估计，以及回环优化后的优化位姿，构建出的全局稠密地图为后续路径导航使用；
步骤四中，首先基于Dijkstra算法生成无碰撞路径，下一步基于多帧规划间的路径顺滑方法生成优化路径，局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，最后基于人工势场法建立足式机器人避障的评价函数帮助完成机器人实现全局路径导航的自主避障；
步骤41：在已知全局地图及机器人定位的情况下，生成一条从机器人当前位置至全局目标点的无碰撞路径，采用Dijkstra算法在局部地图构建输出的实时局部栅格地图中进行路径规划，Dijkstra算法的起点为机器人当前所在栅格，目标点为下一个要到达的目标节点所在栅格，具体步骤如下：(1)记栅格地图为G＝{V,E}，V为栅格顶点，E为两点之间的边，初始时令S＝{V0}，T＝V‑S＝{其余栅格顶点}；
(2)利用步骤(1)中的栅格地图，从T中选取一个与S中顶点有关联边且权值最小的顶点W，加入到S中；
(3)利用步骤(2)中的地图选取范围，对其余T中顶点的距离值进行修改：若加进W作中间顶点，修改V0到Vi的距离缩短值；
(4)重复上述步骤2和步骤3，直到S中包含栅格目标顶点；
步骤42：利用步骤41中得到的目标点所在位置信息，基于多帧规划间的路径顺滑方法，先将栅格路径进行离散采样，再放入TEB算法进行优化处理，采用g2o优化输出移动机器人的路径点以及速度，经过优化后，局部路径相对会更加平滑，并且可以调节距离障碍物之间的距离，使用多帧之间路径顺滑策略的路径规划，通过显式地考虑在前一时刻已经规划出的路径，并将它拼接至下一时刻的路径中，即使目标点发生了跳变，路径在足式机器人处的瞬时运动方向仍然保持连续；
步骤43：利用步骤42中得到的起始点与目标点之间的规划路径，在局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，基于本体传感器采集得到的数据，建立一个以机器人本体质心为圆心，质心到腿部各足末端位置为半径的虚拟机体模型，以包络的虚拟机体模型作为避障运动的采样区域，根据地形高程图得到的数据，计算机器人质心和障碍物之间的垂直距离和质心沿最优方向的距离，基于人工势场法建立足式机器人避障的评价函数实现全局路径导航的自主避障：式中， 为障碍物的位置向量； 为机器人的位置向量； 为目标点的位置向量；
kod为足式机器人和障碍物位置转换间的增益系数；krd为机器人和目标位置转换间的增益系数；ρ为机器人与障碍物之间的最短距离；ρ0为势场范围的阈值常数；n为栅格节点数。
2.如权利要求1所述方法，其特征在于，该感知装置包括：主框架(A)、双目云台(B)、云台动力系统(C)；所述双目云台(B)与主框架(A)连接，云台动力系统(C)安装在主框架(A)上；
所述主框架(A)包括第一雷达固定平台(A‑1)、上盖板(A‑2)、第二下层框架侧板(A‑3)、上层框架侧板(A‑5)、上层框架底座(A‑6)、第一下层框架侧板(A‑7)；所述主框架(A)顶端的第一雷达固定平台(A‑1)通过螺丝连接第一激光雷达(A‑10)，第一雷达固定平台(A‑1)通过其下方两侧的螺丝孔连接两块上层框架侧板(A‑5)，上层框架侧板(A‑5)则插入上层框架底座(A‑6)上方的卡槽内，并通过螺丝进行固定；
所述上层框架底座(A‑6)通过两侧的安装孔与第一下层框架侧板(A‑7)、第二下层框架侧板(A‑3)进行固定，在第一下层框架侧板(A‑7)上安装了两个同步带的张紧轮组(A‑8)与一个云台主动轴(A‑9)，两个张紧轮组(A‑8)则插入第一下层框架侧板(A‑7)上预留的滑槽内，两个张紧轮组(A‑8)的相对位置可调；上盖板(A‑2)通过卡槽加螺丝的方式与第一下层框架侧板(A‑7)、第二下层框架侧板(A‑3)、上层框架底座(A‑6)相连接并形成一个腔体；云台主动轴(A‑9)与第一下层框架侧板(A‑7)之间安装有微型法兰轴承与平面推力轴承，云台主动轴(A‑9)的轴端先套进所述平面推力轴承再套入所述微型法兰轴承，最后插进第一下层框架侧板(A‑7)的末端安装孔内，并用卡簧进行固定；第二下层框架侧板(A‑3)上安装有云台从动轴(A‑4)；
所述双目云台(B)包括第二雷达金属底座(B‑1)、回旋轴承固定座(B‑3)、舵机法兰盘(B‑4)、回旋轴承连接件(B‑5)、回旋轴承(B‑6)、双目摄像头固定架(B‑7)、双目摄像头(B‑
8)；顶端的第二雷达金属底座(B‑1)通过中央的安装孔用螺丝固定安装有第二激光雷达(B‑
9)，第二雷达金属底座(B‑1)下方的凹槽内固定安装有舵机(B‑2)，并且第二雷达金属底座(B‑1)两侧带螺纹孔的凹槽与主框架(A)中的云台主动轴(A‑9)、云台从动轴(A‑4)相连接；
回旋轴承固定座(B‑3)通过螺丝与第二雷达金属底座(B‑1)相连接，同时回旋轴承固定座(B‑3)与回旋轴承(B‑6)外圈相连接，舵机法兰盘(B‑4)通过中央的螺丝孔与舵机(B‑2)相连接，同时舵机法兰盘(B‑4)四周有四个螺纹孔与回旋轴承连接件(B‑5)通过螺丝相连接，回旋轴承连接件(B‑5)与回旋轴承(B‑6)内圈连接，双目摄像头固定架(B‑7)通过中央四个固定孔与回旋轴承(B‑6)内圈连接，双目摄像头(B‑8)固定在双目摄像头固定架(B‑7)一侧的平面上，双目摄像头(B‑8)可在0‑°180°的范围内左右转动；
所述云台动力装置(C)包括云台电机(C‑1)、同步带主动轮(C‑2)、同步带(C‑3)、同步带从动轮(C‑4)；云台电机(C‑1)通过螺丝固定在第一下层框架侧板(A‑7)的上部，同步带主动轮(C‑2)通过顶丝固定在云台电机(C‑1)的输出轴上，同步带从动轮(C‑2)通过顶丝固定在云台主动轴(A‑9)上，同步带(C‑3)以图5的形式固定在同步带主动轮(C‑2)与同步带从动轮(C‑4)之间，可通过张紧轮组(A‑8)下方的螺丝调节同步带(C‑3)的张紧程度；云台电机(C‑
1)转动可带动同步带主动轮(C‑2)转动，再通过同步带(C‑3)，带动同步带从动轮(C‑4)转动，进而带动双目云台(B)转动，双目云台(B)在云台动力装置(C)的控制下可以在0°‑90°的范围内转动。
3.一种如权利要求1至2中任一项所述方法在足式机器人技术领域中的应用。
基于变构型感知装置的足式机器人导航及定位方法
技术领域
[0001] 本发明属于足式机器人技术领域，具体涉及一种基于变构型感知装置的足式机器人导航及定位方法。
背景技术
[0002] 感知导航与定位是足式机器人适应复杂地形、实现自主行走、达到人机交互的核心关键技术，是足式机器人区别于有人装备、成为地面无人系统的关键。
[0003] 目前，我国足式机器人感知与导行控制技术领域起步较晚，足式机器人普遍存在自主性不强，足式机器人无法自由穿梭在城市大街小巷，实现灵活静默侦察等任务，相关感知工作与技术成果少，集成化程度不高，足式机器人成熟适配的感知导航与定位系统较少。我国开展的针对大范围非结构化环境中的三维环境建模较少，没有开展基于变构型结构的全局地图的定位方法，复杂环境障碍检测与无碰撞规划导航开展的工作较少，仍存在感知元素较少甚至误判、导航误差较大、足式机器人步态难以适应感知系统等问题。因此，需要设计一种基于变构型感知装置的足式机器人导航及定位方法。
发明内容
[0004] (一)要解决的技术问题
[0005] 本发明要解决的技术问题是：如何解决足式机器人在未知地形无法完成全局及局部地图不同视角切换的问题，提出一种基于变构型感知装置的足式机器人导航及定位方法。
[0006] (二)技术方案
[0007] 为了解决上述技术问题，本发明提供了一种基于变构型感知装置的足式机器人导航及定位方法，包括以下步骤：
[0008] 步骤一、实现构型可变的多传感外参标定用于将变构型感知装置的激光雷达、摄像头和惯性测量单元IMU通过传感器外参标定算法来实现信息源的最优化融合；
[0009] 步骤二、基于步骤一实现的构型可变的多传感外参标定，实现基于激光数据的全局地图构建与定位；
[0010] 步骤三、根据步骤二中的全局地图构建与定位，实现的基于变构型标定的局部及全局稠密地图构建；
[0011] 步骤四、根据步骤三中构建的全局稠密地图，实现基于全局定位的路径导航。
[0012] 本发明还提供了一种所述方法在足式机器人技术领域中的应用。
[0013] (三)有益效果
[0014] 本发明通过设计可变的多传感外参标定、基于激光数据的全局地图构建与定位、基于变构型标定的局部及全局稠密地图构建、基于全局定位的路径导航流程，提出了一种基于变构型感知装置的足式机器人导航及定位方法，解决了足式机器人在未知地形无法完成全局及局部地图不同视角切换的问题，增强了足式机器人感知系统的鲁棒性。
附图说明
[0015] 图1至图3是足式机器人变构型感知装置的示意图；其中，图1为主框架整体三维图；图2为双目云台整体零件爆炸三维图；图3为云台动力装置整体三维图；
[0016] 图4是本发明一种基于变构型感知装置的足式机器人导航及定位方法的流程图；
[0017] 图5是基于激光数据的全局地图构建与定位流程示意图；
[0018] 图6是激光定位各参考系相对位姿及相应的数据发布示意图；
[0019] 图7是基于激光数据的足式机器人局部稠密地图建立示意图；
[0020] 图8是基于全局定位的路径导航步骤示意图。
具体实施方式
[0021] 为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
[0022] 本发明提供了一种基于变构型感知装置的足式机器人导航及定位方法，其中，变构型感知装置示意图如图1所示，包括：主框架A、双目云台B、云台动力系统C；所述双目云台B与主框架A连接，云台动力系统C安装在主框架A上。
[0023] 所述主框架A包括第一雷达固定平台A‑1、上盖板A‑2、第二下层框架侧板A‑3、上层框架侧板A‑5、上层框架底座A‑6、第一下层框架侧板A‑7；所述主框架A顶端的第一雷达固定平台A‑1通过螺丝连接第一激光雷达A‑10，第一雷达固定平台A‑1通过其下方两侧的螺丝孔连接两块上层框架侧板A‑5，上层框架侧板A‑5则插入上层框架底座A‑6上方的卡槽内，并通过螺丝进行固定；
[0024] 所述上层框架底座A‑6通过两侧的安装孔与第一下层框架侧板A‑7、第二下层框架侧板A‑3进行固定，在第一下层框架侧板A‑7上安装了两个同步带的张紧轮组A‑8与一个云台主动轴A‑9，两个张紧轮组A‑8则插入第一下层框架侧板A‑7上预留的滑槽内，两个张紧轮组A‑8的相对位置可调；上盖板A‑2通过卡槽加螺丝的方式与第一下层框架侧板A‑7、第二下层框架侧板A‑3、上层框架底座A‑6相连接并形成一个腔体；云台主动轴A‑9与第一下层框架侧板A‑7之间安装有微型法兰轴承与平面推力轴承，云台主动轴A‑9的轴端先套进所述平面推力轴承再套入所述微型法兰轴承，最后插进第一下层框架侧板A‑7的末端安装孔内，并用卡簧进行固定；第二下层框架侧板A‑3上安装有云台从动轴A‑4。
[0025] 所述双目云台B包括第二雷达金属底座B‑1、回旋轴承固定座B‑3、舵机法兰盘B‑4、回旋轴承连接件B‑5、回旋轴承B‑6、双目摄像头固定架B‑7、双目摄像头B‑8；顶端的第二雷达金属底座B‑1通过中央的安装孔用螺丝固定安装有第二激光雷达B‑9，第二雷达金属底座B‑1下方的凹槽内固定安装有舵机B‑2，并且第二雷达金属底座B‑1两侧带螺纹孔的凹槽与主框架A中的云台主动轴A‑9、云台从动轴A‑4相连接；回旋轴承固定座B‑3通过螺丝与第二雷达金属底座B‑1相连接，同时回旋轴承固定座B‑3与回旋轴承B‑6外圈相连接，舵机法兰盘B‑4通过中央的螺丝孔与舵机B‑2相连接，同时舵机法兰盘B‑4四周有四个螺纹孔与回旋轴承连接件B‑5通过螺丝相连接，回旋轴承连接件B‑5与回旋轴承B‑6内圈连接，双目摄像头固定架B‑7通过中央四个固定孔与回旋轴承B‑6内圈连接，双目摄像头B‑8固定在双目摄像头固定架B‑7一侧的平面上，双目摄像头B‑8可在0‑°180°的范围内左右转动。
[0026] 所述云台动力装置C包括云台电机C‑1、同步带主动轮C‑2、同步带C‑3、同步带从动轮C‑4；云台电机C‑1通过螺丝固定在第一下层框架侧板A‑7的上部，同步带主动轮C‑2通过顶丝固定在云台电机C‑1的输出轴上，同步带从动轮C‑2通过顶丝固定在云台主动轴A‑9上，同步带C‑3以图5的形式固定在同步带主动轮C‑2与同步带从动轮C‑4之间，可通过张紧轮组A‑8下方的螺丝调节同步带C‑3的张紧程度；云台电机C‑1转动可带动同步带主动轮C‑2转动，再通过同步带C‑3，带动同步带从动轮C‑4转动，进而带动双目云台B转动，双目云台B在云台动力装置C的控制下可以在0°‑90°的范围内转动。
[0027] 当感知装置启动之后，双目云台B会处于初始位置，通过雷达、摄像头与惯性测量单元IMU的自标定，并通过双雷达与双目摄像头B‑8完成地形环境建模，双激光雷达采集的信号需要经过雷达数据采集器传回主控模块完成导航及地图定位的功能，双目摄像头B‑8采集到的信号通过USB Hub导入主控模块中完成人员识别及障碍物检测等功能，激光雷达采集到的点云及里程计信息传入主控模块生成局部及全局的2.5D稠密地图来完成足式机器人导航规划及避障策略，并将导航及定位感知信息传输给足式机器人运动控制单元，实现足式机器人在复杂路面的自主鲁棒行走。
[0028] 可以根据实际需要调节双目云台雷达的倾角，例如通过双目摄像头B‑8判断当前地形不复杂，但突发情况可能较多，此时可以将倾角调小，使得感知装置获得更大范围的感知区域，这样使得机器人有充分的时间应对危险；若当前地形较为复杂，突发情况较少，此时可以将倾角调大，使得机器人能够更好地获得实时的地面信息，找到合适的落脚点，保证前进的稳定性；若地形复杂程度与突发情况概率都一般，此时可以将倾角调到适中，兼顾两者。
[0029] 参考图2至图6，基于该装置实现的上述方法具体是按照以下步骤完成的：
[0030] 步骤一、实现构型可变的多传感外参标定用于将激光雷达、摄像头和惯性测量单元IMU通过高效鲁棒的传感器外参标定算法来实现信息源的最优化融合。实现构型可变的多传感外参标定的流程为：
[0031] 流程一：根据雷达构型变化角度采用最优化的方法优化外参带来的误差项的残差，优化并求解任意激光间的外参，完成构型可变的双激光外参标定；
[0032] 流程二：根据流程一的双激光外参标定参数，提出基于时空解耦的激光‑相机外参标定方法，消除多传感间的共同视野及时间同步局限性，基于采集标定板的激光传感器数据和视觉图像数据建立视觉3D点云，模拟激光和相机的全方位信息完成标定；
[0033] 流程三：根据流程二的激光和相机的全方位标定信息，提出基于相机‑惯性测量单元融合的外参标定算法，通过在相机图像中提取特征点完成里程计估计，通过图优化算法进行耦合优化，实现相机‑惯性测量单元外参的标定。
[0034] 步骤二、根据步骤一实现的构型可变的多传感外参标定，实现基于激光数据的全局地图构建与定位，通过实时获取的激光、IMU和里程计数据推算相对于里程计坐标系的相对位姿并构建局部地图，通过匹配局部地图与已构建的环境地图，实时更新里程计坐标系与全局地图坐标系间的相对位姿,从而实现实时的足式机器人定位。提出的基于激光数据的全局地图构建与定位的流程为：
[0035] 流程一：建立基于拓扑度量的定位框架，基于激光的SLAM根据以一定频率获取的激光数据信息，利用迭代最近点算法(ICP)估计每一帧激光相对于当前地图的相对位姿，并基于位姿将激光点云数据融入地图中，建立基于激光雷达的定位框架；
[0036] 流程二：根据流程一的激光定位框架，提出基于滑动窗口优化的自主定位方法，选择以当前位姿为准的一个时间窗口，并将该窗口内的里程计信息、地图信息、激光雷达点云信息进行融合，形成如图的局部定位的优化问题，完成系统在子地图上的定位，其里程优化代价函数为如下所示：
[0037]
[0038] 式中， 为里程计前两个时刻信息轨迹的姿态， 为激光雷达点云的当前及前两个时刻的位姿数据；
[0039] 依赖相对里程和上一时刻的定位，可以获得当前位置相对于子地图的初值，通过ICP将当前获得的激光和子地图进行点云匹配，生成优化函数如下所示：
[0040]
[0041] 式中，Lt,Lt‑1为点云与子地图匹配的当前时刻和前一时刻的相对位姿， 为所有子地图之间的相对位姿的集合，Xt,Xt‑1为激光雷达点云的当前时刻和前一时刻的位姿数据；
[0042] 通过地图构建，获得地图之间的相对位姿关系，此优化函数如下所示：
[0043]
[0044] 式中， 为地图之间的相对位姿， Mj为所有子地图之间的相对位姿的集合；
[0045] 将公式1、2、3的三项优化代价函数进行迭代优化，得到机器人当前位姿相对于当前子地图的最优位姿，即当前里程与地图相对位姿关系的位姿优化函数如下所示：
[0046]
[0047] 步骤三、根据步骤二中的全局地图构建与定位，实现的基于变构型标定的局部及全局稠密地图构建，其中，根据生成的全局地图定位信息，通过激光定位将数据发布到里程计，下一步通过激光及IMU里程计融合数据发布反馈到足式机器人参与控制，具体流程为：
[0048] 流程一：稠密地图的局部高度地图由距离传感器获得的点云生成，并结合距离传感器获取数据方差和机器人运动状态转移的不确定度进行更新，从而获取准确的局部稠密高度地图。基于已建立完成的局部稠密高度地图完成固定及障碍物检测并判断可行域。基于局部稠密高度地图，通过运动学和惯性测量单元进行本体状态估计定位，并进行地图更新融合来使得局部稠密高度地图更加精确，将地形高程图估计值 通过扩展卡尔曼滤波的方式融合到高度测量值
[0049]
[0050]
[0051] 式中， 为前一时刻障碍的高度估计值， 为当前时刻障碍的高度测量值，为高程图估计信息的标准差；
[0052] 流程二：利用流程一计算得到的地形高程图估计值 及高度测量值实现基于变构型标定的全局稠密地图构建，基于对局部稠密地图的扩展将部分稠密地图融合到全局一致的坐标系下，实现全局稠密地图的构建；全局地图由子图组成并持续维护，根据SLAM的位姿优化实现子图相应优化，对于地图中的每个栅格，提取其表面法向量和与附近栅格的高度差作为局部及全局地图的表面特征；通过地形特征加权评估地图中每个栅格的可行域权值，完成局部地图及全局地图的导航路径及落足点规划，重新构建出一个全局稠密地图；根据不同的任务属性，选择不同的距离传感器作为系统输入，SLAM系统为建图过程提供实时的位姿估计，以及回环优化后的优化位姿，构建出的全局稠密地图为后续路径导航使用。
[0053] 步骤四、根据步骤三中建立的全局稠密地图，实现基于全局定位的路径导航，首先基于Dijkstra算法生成无碰撞路径，下一步基于多帧规划间的路径顺滑方法生成优化路径，局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，最后基于人工势场法建立足式机器人避障的评价函数帮助完成机器人实现全局路径导航的自主避障，具体流程为：
[0054] 流程一：在已知全局地图及机器人定位的情况下，生成一条从机器人当前位置至全局目标点的无碰撞路径，采用Dijkstra算法在局部地图构建输出的实时局部栅格地图中进行路径规划，Dijkstra算法的起点为机器人当前所在栅格，目标点为下一个要到达的目标节点所在栅格，具体步骤如下：
[0055] (1)记栅格地图为G＝{V,E}，V为栅格顶点，E为两点之间的边，初始时令S＝{V0}，T＝V‑S＝{其余栅格顶点}；
[0056] (2)利用步骤(1)中的栅格地图，从T中选取一个与S中顶点有关联边且权值最小的顶点W，加入到S中；
[0057] (3)利用步骤(2)中的地图选取范围，对其余T中顶点的距离值进行修改：若加进W作中间顶点，修改V0到Vi的距离缩短值；
[0058] (4)重复上述步骤2和步骤3，直到S中包含栅格目标顶点；
[0059] 流程二：利用流程一中得到的目标点所在位置信息，基于多帧规划间的路径顺滑方法，先将栅格路径进行离散采样，再放入TEB算法进行优化处理，采用g2o优化输出移动机器人的路径点以及速度，经过优化后，局部路径相对会更加平滑，并且可以调节距离障碍物之间的距离，使用多帧之间路径顺滑策略的路径规划，通过显式地考虑在前一时刻已经规划出的路径，并将它拼接至下一时刻的路径中，即使目标点发生了跳变，路径在足式机器人处的瞬时运动方向仍然保持连续；
[0060] 流程三：利用流程二中得到的起始点与目标点之间的规划路径，在局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，基于本体传感器采集得到的数据，建立一个以机器人本体质心为圆心，质心到腿部各足末端位置为半径的虚拟机体模型，以包络的虚拟机体模型作为避障运动的采样区域，根据地形高程图得到的数据，计算机器人质心和障碍物之间的垂直距离和质心沿最优方向的距离，基于人工势场法建立足式机器人避障的评价函数实现全局路径导航的自主避障：
[0061]
[0062] 式中， 为障碍物的位置向量； 为机器人的位置向量； 为目标点的位置向量；kod为足式机器人和障碍物位置转换间的增益系数；krd为机器人和目标位置转换间的增益系数；ρ为机器人与障碍物之间的最短距离；ρ0为势场范围的阈值常数；n为栅格节点数。
[0063] 以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。
