CN113390411A - 基于变构型感知装置的足式机器人导航及定位方法 - Google Patents

基于变构型感知装置的足式机器人导航及定位方法 Download PDF

Info

Publication number
CN113390411A
CN113390411A CN202110647041.XA CN202110647041A CN113390411A CN 113390411 A CN113390411 A CN 113390411A CN 202110647041 A CN202110647041 A CN 202110647041A CN 113390411 A CN113390411 A CN 113390411A
Authority
CN
China
Prior art keywords
map
global
path
laser
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.)
Granted
Application number
CN202110647041.XA
Other languages
English (en)
Other versions
CN113390411B (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.)
Intelligent Mobile Robot Zhongshan Research Institute
China North Vehicle Research Institute
Original Assignee
Intelligent Mobile Robot Zhongshan Research Institute
China North Vehicle Research Institute
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 Intelligent Mobile Robot Zhongshan Research Institute, China North Vehicle Research Institute filed Critical Intelligent Mobile Robot Zhongshan Research Institute
Priority to CN202110647041.XA priority Critical patent/CN113390411B/zh
Publication of CN113390411A publication Critical patent/CN113390411A/zh
Application granted granted Critical
Publication of CN113390411B publication Critical patent/CN113390411B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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
    • G01C21/1652—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 with ranging devices, e.g. LIDAR or RADAR
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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/38—Electronic maps specially adapted for navigation; Updating thereof
    • G01C21/3804—Creation or updating of map data
    • G01C21/3833—Creation or updating of map data characterised by the source of data
    • G01C21/3848—Data obtained from both position sensors and additional sensors
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders

Landscapes

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

Abstract

本发明涉及一种基于变构型感知装置的足式机器人导航及定位方法，属于足式机器人技术领域。本发明通过设计可变的多传感外参标定、基于激光数据的全局地图构建与定位、基于变构型标定的局部及全局稠密地图构建、基于全局定位的路径导航流程，提出了一种基于变构型感知装置的足式机器人导航及定位方法，解决了足式机器人在未知地形无法完成全局及局部地图不同视角切换的问题，增强了足式机器人感知系统的鲁棒性。

Description

基于变构型感知装置的足式机器人导航及定位方法
技术领域
本发明属于足式机器人技术领域，具体涉及一种基于变构型感知装置的足式机器人导航及定位方法。
背景技术
感知导航与定位是足式机器人适应复杂地形、实现自主行走、达到人机交互的核心关键技术，是足式机器人区别于有人装备、成为地面无人系统的关键。
目前，我国足式机器人感知与导行控制技术领域起步较晚，足式机器人普遍存在自主性不强，足式机器人无法自由穿梭在城市大街小巷，实现灵活静默侦察等任务，相关感知工作与技术成果少，集成化程度不高，足式机器人成熟适配的感知导航与定位系统较少。我国开展的针对大范围非结构化环境中的三维环境建模较少，没有开展基于变构型结构的全局地图的定位方法，复杂环境障碍检测与无碰撞规划导航开展的工作较少，仍存在感知元素较少甚至误判、导航误差较大、足式机器人步态难以适应感知系统等问题。因此，需要设计一种基于变构型感知装置的足式机器人导航及定位方法。
发明内容
(一)要解决的技术问题
本发明要解决的技术问题是：如何解决足式机器人在未知地形无法完成全局及局部地图不同视角切换的问题，提出一种基于变构型感知装置的足式机器人导航及定位方法。
(二)技术方案
为了解决上述技术问题，本发明提供了一种基于变构型感知装置的足式机器人导航及定位方法，包括以下步骤：
步骤一、实现构型可变的多传感外参标定用于将变构型感知装置的激光雷达、摄像头和惯性测量单元IMU通过传感器外参标定算法来实现信息源的最优化融合；
步骤二、基于步骤一实现的构型可变的多传感外参标定，实现基于激光数据的全局地图构建与定位；
步骤三、根据步骤二中的全局地图构建与定位，实现的基于变构型标定的局部及全局稠密地图构建；
步骤四、根据步骤三中构建的全局稠密地图，实现基于全局定位的路径导航。
本发明还提供了一种所述方法在足式机器人技术领域中的应用。
(三)有益效果
本发明通过设计可变的多传感外参标定、基于激光数据的全局地图构建与定位、基于变构型标定的局部及全局稠密地图构建、基于全局定位的路径导航流程，提出了一种基于变构型感知装置的足式机器人导航及定位方法，解决了足式机器人在未知地形无法完成全局及局部地图不同视角切换的问题，增强了足式机器人感知系统的鲁棒性。
附图说明
图1至图3是足式机器人变构型感知装置的示意图；其中，图1为主框架整体三维图；图2为双目云台整体零件爆炸三维图；图3为云台动力装置整体三维图；
图4是本发明一种基于变构型感知装置的足式机器人导航及定位方法的流程图；
图5是基于激光数据的全局地图构建与定位流程示意图；
图6是激光定位各参考系相对位姿及相应的数据发布示意图；
图7是基于激光数据的足式机器人局部稠密地图建立示意图；
图8是基于全局定位的路径导航步骤示意图。
具体实施方式
为使本发明的目的、内容、和优点更加清楚，下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。
本发明提供了一种基于变构型感知装置的足式机器人导航及定位方法，其中，变构型感知装置示意图如图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°的范围内转动。
当感知装置启动之后，双目云台B会处于初始位置，通过雷达、摄像头与惯性测量单元IMU的自标定，并通过双雷达与双目摄像头B-8完成地形环境建模，双激光雷达采集的信号需要经过雷达数据采集器传回主控模块完成导航及地图定位的功能，双目摄像头B-8采集到的信号通过USB Hub导入主控模块中完成人员识别及障碍物检测等功能，激光雷达采集到的点云及里程计信息传入主控模块生成局部及全局的2.5D稠密地图来完成足式机器人导航规划及避障策略，并将导航及定位感知信息传输给足式机器人运动控制单元，实现足式机器人在复杂路面的自主鲁棒行走。
可以根据实际需要调节双目云台雷达的倾角，例如通过双目摄像头B-8判断当前地形不复杂，但突发情况可能较多，此时可以将倾角调小，使得感知装置获得更大范围的感知区域，这样使得机器人有充分的时间应对危险；若当前地形较为复杂，突发情况较少，此时可以将倾角调大，使得机器人能够更好地获得实时的地面信息，找到合适的落脚点，保证前进的稳定性；若地形复杂程度与突发情况概率都一般，此时可以将倾角调到适中，兼顾两者。
参考图2至图6，基于该装置实现的上述方法具体是按照以下步骤完成的：
步骤一、实现构型可变的多传感外参标定用于将激光雷达、摄像头和惯性测量单元IMU通过高效鲁棒的传感器外参标定算法来实现信息源的最优化融合。实现构型可变的多传感外参标定的流程为：
流程一：根据雷达构型变化角度采用最优化的方法优化外参带来的误差项的残差，优化并求解任意激光间的外参，完成构型可变的双激光外参标定；
流程二：根据流程一的双激光外参标定参数，提出基于时空解耦的激光-相机外参标定方法，消除多传感间的共同视野及时间同步局限性，基于采集标定板的激光传感器数据和视觉图像数据建立视觉3D点云，模拟激光和相机的全方位信息完成标定；
流程三：根据流程二的激光和相机的全方位标定信息，提出基于相机-惯性测量单元融合的外参标定算法，通过在相机图像中提取特征点完成里程计估计，通过图优化算法进行耦合优化，实现相机-惯性测量单元外参的标定。
步骤二、根据步骤一实现的构型可变的多传感外参标定，实现基于激光数据的全局地图构建与定位，通过实时获取的激光、IMU和里程计数据推算相对于里程计坐标系的相对位姿并构建局部地图，通过匹配局部地图与已构建的环境地图，实时更新里程计坐标系与全局地图坐标系间的相对位姿,从而实现实时的足式机器人定位。提出的基于激光数据的全局地图构建与定位的流程为：
流程一：建立基于拓扑度量的定位框架，基于激光的SLAM根据以一定频率获取的激光数据信息，利用迭代最近点算法(ICP)估计每一帧激光相对于当前地图的相对位姿，并基于位姿将激光点云数据融入地图中，建立基于激光雷达的定位框架；
流程二：根据流程一的激光定位框架，提出基于滑动窗口优化的自主定位方法，选择以当前位姿为准的一个时间窗口，并将该窗口内的里程计信息、地图信息、激光雷达点云信息进行融合，形成如图的局部定位的优化问题，完成系统在子地图上的定位，其里程优化代价函数为如下所示：
Figure BDA0003110309870000071
式中，
Figure BDA0003110309870000072
为里程计前两个时刻信息轨迹的姿态，
Figure BDA0003110309870000073
为激光雷达点云的当前及前两个时刻的位姿数据；
依赖相对里程和上一时刻的定位，可以获得当前位置相对于子地图的初值，通过ICP将当前获得的激光和子地图进行点云匹配，生成优化函数如下所示：
Figure BDA0003110309870000074
式中，Lt,Lt-1为点云与子地图匹配的当前时刻和前一时刻的相对位姿，
Figure BDA0003110309870000075
为所有子地图之间的相对位姿的集合，Xt,Xt-1为激光雷达点云的当前时刻和前一时刻的位姿数据；
通过地图构建，获得地图之间的相对位姿关系，此优化函数如下所示：
Figure BDA0003110309870000076
式中，
Figure BDA0003110309870000077
为地图之间的相对位姿，
Figure BDA0003110309870000078
Mj为所有子地图之间的相对位姿的集合；
将公式1、2、3的三项优化代价函数进行迭代优化，得到机器人当前位姿相对于当前子地图的最优位姿，即当前里程与地图相对位姿关系的位姿优化函数如下所示：
Figure BDA0003110309870000079
步骤三、根据步骤二中的全局地图构建与定位，实现的基于变构型标定的局部及全局稠密地图构建，其中，根据生成的全局地图定位信息，通过激光定位将数据发布到里程计，下一步通过激光及IMU里程计融合数据发布反馈到足式机器人参与控制，具体流程为：
流程一：稠密地图的局部高度地图由距离传感器获得的点云生成，并结合距离传感器获取数据方差和机器人运动状态转移的不确定度进行更新，从而获取准确的局部稠密高度地图。基于已建立完成的局部稠密高度地图完成固定及障碍物检测并判断可行域。基于局部稠密高度地图，通过运动学和惯性测量单元进行本体状态估计定位，并进行地图更新融合来使得局部稠密高度地图更加精确，将地形高程图估计值
Figure BDA0003110309870000081
通过扩展卡尔曼滤波的方式融合到高度测量值
Figure BDA0003110309870000082
Figure BDA0003110309870000083
Figure BDA0003110309870000084
式中，
Figure BDA0003110309870000085
为前一时刻障碍的高度估计值，
Figure BDA0003110309870000086
为当前时刻障碍的高度测量值，
Figure BDA0003110309870000087
为高程图估计信息的标准差；
流程二：利用流程一计算得到的地形高程图估计值
Figure BDA0003110309870000088
及高度测量值
Figure BDA0003110309870000089
实现基于变构型标定的全局稠密地图构建，基于对局部稠密地图的扩展将部分稠密地图融合到全局一致的坐标系下，实现全局稠密地图的构建；全局地图由子图组成并持续维护，根据SLAM的位姿优化实现子图相应优化，对于地图中的每个栅格，提取其表面法向量和与附近栅格的高度差作为局部及全局地图的表面特征；通过地形特征加权评估地图中每个栅格的可行域权值，完成局部地图及全局地图的导航路径及落足点规划，重新构建出一个全局稠密地图；根据不同的任务属性，选择不同的距离传感器作为系统输入，SLAM系统为建图过程提供实时的位姿估计，以及回环优化后的优化位姿，构建出的全局稠密地图为后续路径导航使用。
步骤四、根据步骤三中建立的全局稠密地图，实现基于全局定位的路径导航，首先基于Dijkstra算法生成无碰撞路径，下一步基于多帧规划间的路径顺滑方法生成优化路径，局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，最后基于人工势场法建立足式机器人避障的评价函数帮助完成机器人实现全局路径导航的自主避障，具体流程为：
流程一：在已知全局地图及机器人定位的情况下，生成一条从机器人当前位置至全局目标点的无碰撞路径，采用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中包含栅格目标顶点；
流程二：利用流程一中得到的目标点所在位置信息，基于多帧规划间的路径顺滑方法，先将栅格路径进行离散采样，再放入TEB算法进行优化处理，采用g2o优化输出移动机器人的路径点以及速度，经过优化后，局部路径相对会更加平滑，并且可以调节距离障碍物之间的距离，使用多帧之间路径顺滑策略的路径规划，通过显式地考虑在前一时刻已经规划出的路径，并将它拼接至下一时刻的路径中，即使目标点发生了跳变，路径在足式机器人处的瞬时运动方向仍然保持连续；
流程三：利用流程二中得到的起始点与目标点之间的规划路径，在局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，基于本体传感器采集得到的数据，建立一个以机器人本体质心为圆心，质心到腿部各足末端位置为半径的虚拟机体模型，以包络的虚拟机体模型作为避障运动的采样区域，根据地形高程图得到的数据，计算机器人质心和障碍物之间的垂直距离和质心沿最优方向的距离，基于人工势场法建立足式机器人避障的评价函数实现全局路径导航的自主避障：
Figure BDA0003110309870000101
式中，
Figure BDA0003110309870000102
为障碍物的位置向量；
Figure BDA0003110309870000103
为机器人的位置向量；
Figure BDA0003110309870000104
为目标点的位置向量；kod为足式机器人和障碍物位置转换间的增益系数；krd为机器人和目标位置转换间的增益系数；ρ为机器人与障碍物之间的最短距离；ρ0为势场范围的阈值常数；n为栅格节点数。
以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明技术原理的前提下，还可以做出若干改进和变形，这些改进和变形也应视为本发明的保护范围。

Claims (10)

1.一种基于变构型感知装置的足式机器人导航及定位方法，其特征在于，包括以下步骤：
步骤一、实现构型可变的多传感外参标定用于将变构型感知装置的激光雷达、摄像头和惯性测量单元IMU通过传感器外参标定算法来实现信息源的最优化融合；
步骤二、基于步骤一实现的构型可变的多传感外参标定，实现基于激光数据的全局地图构建与定位；
步骤三、根据步骤二中的全局地图构建与定位，实现的基于变构型标定的局部及全局稠密地图构建；
步骤四、根据步骤三中构建的全局稠密地图，实现基于全局定位的路径导航。
2.如权利要求1所述的方法，其特征在于，步骤1中，实现构型可变的多传感外参标定的流程为：
步骤1：根据雷达构型变化角度采用最优化的方法优化外参带来的误差项的残差，优化并求解任意激光间的外参，完成构型可变的双激光外参标定；
步骤2：根据步骤1得到的双激光外参标定参数，提出基于时空解耦的激光-相机外参标定方法，消除多传感间的共同视野及时间同步局限性，基于采集标定板的激光传感器数据和视觉图像数据建立视觉3D点云，模拟激光和相机的全方位信息完成标定；
步骤3：根据步骤2的激光和相机的全方位标定信息，提出基于相机-惯性测量单元融合的外参标定算法，通过在相机图像中提取特征点完成里程计估计，通过图优化算法进行耦合优化，实现相机-惯性测量单元外参的标定。
3.如权利要求2所述的方法，其特征在于，步骤二中，通过实时获取的激光、IMU和里程计数据推算相对于里程计坐标系的相对位姿并构建局部地图，通过匹配局部地图与已构建的环境地图，实时更新里程计坐标系与全局地图坐标系间的相对位姿，从而实现实时的足式机器人定位。
4.如权利要求3所述的方法，其特征在于，步骤二中提出的基于激光数据的全局地图构建与定位的流程为：
流程一：建立基于拓扑度量的定位框架，基于激光的SLAM根据以一定频率获取的激光数据信息，利用迭代最近点算法ICP估计每一帧激光相对于当前地图的相对位姿，并基于位姿将激光点云数据融入地图中，建立基于激光雷达的定位框架；
流程二：根据流程一的激光定位框架，提出基于滑动窗口优化的自主定位方法，选择以当前位姿为准的一个时间窗口，并将该窗口内的里程计信息、地图信息、激光雷达点云信息进行融合，形成局部定位的优化问题，完成在子地图上的定位，其里程优化代价函数为如下所示：
Figure FDA0003110309860000021
式中，
Figure FDA0003110309860000022
为里程计前两个时刻信息轨迹的姿态，Xt,
Figure FDA0003110309860000023
为激光雷达点云的当前及前两个时刻的位姿数据；
依赖相对里程和上一时刻的定位，可以获得当前位置相对于子地图的初值，通过ICP将当前获得的激光和子地图进行点云匹配，生成优化函数如下所示：
Figure FDA0003110309860000031
式中，Lt,Lt-1为点云与子地图匹配的当前时刻和前一时刻的相对位姿，
Figure FDA0003110309860000032
为所有子地图之间的相对位姿的集合，Xt,Xt-1为激光雷达点云的当前时刻和前一时刻的位姿数据；
通过地图构建，获得地图之间的相对位姿关系，此优化函数如下所示：
Figure FDA0003110309860000033
式中，
Figure FDA0003110309860000034
为地图之间的相对位姿，
Figure FDA0003110309860000035
Mj为所有子地图之间的相对位姿的集合；
将公式1、2、3的三项优化代价函数进行迭代优化，得到机器人当前位姿相对于当前子地图的最优位姿，即当前里程与地图相对位姿关系的位姿优化函数如下所示：
Figure FDA0003110309860000036
5.如权利要求4所述的方法，其特征在于，步骤三中，根据生成的全局地图定位信息，通过激光定位将数据发布到里程计，下一步通过激光及IMU里程计融合数据发布反馈到足式机器人参与控制。
6.如权利要求5所述的方法，其特征在于，步骤三具体流程为：
步骤31：稠密地图的局部高度地图由距离传感器获得的点云生成，并结合距离传感器获取数据方差和机器人运动状态转移的不确定度进行更新，从而获取准确的局部稠密高度地图。基于已建立完成的局部稠密高度地图完成固定及障碍物检测并判断可行域。基于局部稠密高度地图，通过运动学和惯性测量单元进行本体状态估计定位，并进行地图更新融合来使得局部稠密高度地图更加精确，将地形高程图估计值
Figure FDA0003110309860000041
通过扩展卡尔曼滤波的方式融合到高度测量值
Figure FDA0003110309860000042
Figure FDA0003110309860000043
Figure FDA0003110309860000044
式中，
Figure FDA0003110309860000045
为前一时刻障碍的高度估计值，
Figure FDA0003110309860000046
为当前时刻障碍的高度测量值，σp,σh,
Figure FDA0003110309860000047
为高程图估计信息的标准差；
步骤32：利用步骤31计算得到的地形高程图估计值
Figure FDA0003110309860000048
及高度测量值
Figure FDA0003110309860000049
实现基于变构型标定的全局稠密地图构建，基于对局部稠密地图的扩展将部分稠密地图融合到全局一致的坐标系下，实现全局稠密地图的构建；全局地图由子图组成并持续维护，根据SLAM的位姿优化实现子图相应优化，对于地图中的每个栅格，提取其表面法向量和与附近栅格的高度差作为局部及全局地图的表面特征；通过地形特征加权评估地图中每个栅格的可行域权值，完成局部地图及全局地图的导航路径及落足点规划，重新构建出一个全局稠密地图；根据不同的任务属性，选择不同的距离传感器作为系统输入，SLAM系统为建图过程提供实时的位姿估计，以及回环优化后的优化位姿，构建出的全局稠密地图为后续路径导航使用。
7.如权利要求6所述的方法，其特征在于，步骤四中，首先基于Dijkstra算法生成无碰撞路径，下一步基于多帧规划间的路径顺滑方法生成优化路径，局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，最后基于人工势场法建立足式机器人避障的评价函数帮助完成机器人实现全局路径导航的自主避障。
8.如权利要求6所述的方法，其特征在于，步骤四具体流程为：
步骤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中得到的起始点与目标点之间的规划路径，在局部及全局动态的路径规划过程中，对障碍物进行膨胀、显式地创建位姿空间，基于本体传感器采集得到的数据，建立一个以机器人本体质心为圆心，质心到腿部各足末端位置为半径的虚拟机体模型，以包络的虚拟机体模型作为避障运动的采样区域，根据地形高程图得到的数据，计算机器人质心和障碍物之间的垂直距离和质心沿最优方向的距离，基于人工势场法建立足式机器人避障的评价函数实现全局路径导航的自主避障：
Figure FDA0003110309860000061
式中，
Figure FDA0003110309860000062
为障碍物的位置向量；
Figure FDA0003110309860000063
为机器人的位置向量；
Figure FDA0003110309860000064
为目标点的位置向量；kod为足式机器人和障碍物位置转换间的增益系数；krd为机器人和目标位置转换间的增益系数；ρ为机器人与障碍物之间的最短距离；ρ0为势场范围的阈值常数；n为栅格节点数。
9.如权利要求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°的范围内转动。
10.一种如权利要求1至9中任一项所述方法在足式机器人技术领域中的应用。
CN202110647041.XA 2021-06-10 2021-06-10 基于变构型感知装置的足式机器人导航及定位方法 Active CN113390411B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110647041.XA CN113390411B (zh) 2021-06-10 2021-06-10 基于变构型感知装置的足式机器人导航及定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110647041.XA CN113390411B (zh) 2021-06-10 2021-06-10 基于变构型感知装置的足式机器人导航及定位方法

Publications (2)

Publication Number Publication Date
CN113390411A true CN113390411A (zh) 2021-09-14
CN113390411B CN113390411B (zh) 2022-08-09

Family

ID=77620195

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110647041.XA Active CN113390411B (zh) 2021-06-10 2021-06-10 基于变构型感知装置的足式机器人导航及定位方法

Country Status (1)

Country Link
CN (1) CN113390411B (zh)

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114104139A (zh) * 2021-09-28 2022-03-01 北京炎凌嘉业机电设备有限公司 一种仿生足式机器人步足平台融合越障及自主跟随系统
CN114131610A (zh) * 2021-12-15 2022-03-04 深圳亿嘉和科技研发有限公司 基于人类行为识别感知的机器人人机动作交互系统及方法
CN115143964A (zh) * 2022-07-05 2022-10-04 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115502971A (zh) * 2022-09-15 2022-12-23 杭州蓝芯科技有限公司 一种应对定位切换跳变的导航对接方法、系统以及设备
CN117496464A (zh) * 2023-10-23 2024-02-02 广东灵锶智能科技有限公司 一种足式机器人地面探测方法及装置
CN117496464B (zh) * 2023-10-23 2024-05-24 广东灵锶智能科技有限公司 一种足式机器人地面探测方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107505644A (zh) * 2017-07-28 2017-12-22 武汉理工大学 基于车载多传感器融合的三维高精度地图生成系统及方法
CN110207714A (zh) * 2019-06-28 2019-09-06 广州小鹏汽车科技有限公司 一种确定车辆位姿的方法、车载系统及车辆
CN111486864A (zh) * 2019-01-28 2020-08-04 北京工商大学 基于立体正八边结构的多源传感器联合标定方法
US20200309529A1 (en) * 2019-03-29 2020-10-01 Trimble Inc. Slam assisted ins

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN107505644A (zh) * 2017-07-28 2017-12-22 武汉理工大学 基于车载多传感器融合的三维高精度地图生成系统及方法
CN111486864A (zh) * 2019-01-28 2020-08-04 北京工商大学 基于立体正八边结构的多源传感器联合标定方法
US20200309529A1 (en) * 2019-03-29 2020-10-01 Trimble Inc. Slam assisted ins
CN110207714A (zh) * 2019-06-28 2019-09-06 广州小鹏汽车科技有限公司 一种确定车辆位姿的方法、车载系统及车辆

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114104139A (zh) * 2021-09-28 2022-03-01 北京炎凌嘉业机电设备有限公司 一种仿生足式机器人步足平台融合越障及自主跟随系统
CN114131610A (zh) * 2021-12-15 2022-03-04 深圳亿嘉和科技研发有限公司 基于人类行为识别感知的机器人人机动作交互系统及方法
CN114131610B (zh) * 2021-12-15 2023-11-10 深圳亿嘉和科技研发有限公司 基于人类行为识别感知的机器人人机动作交互系统及方法
CN115143964A (zh) * 2022-07-05 2022-10-04 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115143964B (zh) * 2022-07-05 2024-05-10 中国科学技术大学 一种基于2.5d代价地图的四足机器人自主导航方法
CN115502971A (zh) * 2022-09-15 2022-12-23 杭州蓝芯科技有限公司 一种应对定位切换跳变的导航对接方法、系统以及设备
CN117496464A (zh) * 2023-10-23 2024-02-02 广东灵锶智能科技有限公司 一种足式机器人地面探测方法及装置
CN117496464B (zh) * 2023-10-23 2024-05-24 广东灵锶智能科技有限公司 一种足式机器人地面探测方法及装置

Also Published As

Publication number Publication date
CN113390411B (zh) 2022-08-09

Similar Documents

Publication Publication Date Title
CN113390411B (zh) 基于变构型感知装置的足式机器人导航及定位方法
Mansouri et al. Cooperative coverage path planning for visual inspection
Zhang et al. 2d lidar-based slam and path planning for indoor rescue using mobile robots
KR20190064311A (ko) 라이다를 이용한 지도 생성 방법 및 장치
Chambers et al. Perception for a river mapping robot
CN104236548A (zh) 一种微型无人机室内自主导航方法
Siegwart et al. Autonomous mobile robots
Zhou et al. A lidar odometry for outdoor mobile robots using ndt based scan matching in gps-denied environments
Oriolo et al. Vision-based odometric localization for humanoids using a kinematic EKF
Sundram et al. Development of a miniature robot for multi-robot occupancy grid mapping
Choi et al. Online 3D coverage path planning using surface vector
Anousaki et al. Simultaneous localization and map building of skid-steered robots
Lamon et al. Mapping with an autonomous car
Elisha et al. Active online visual-inertial navigation and sensor calibration via belief space planning and factor graph based incremental smoothing
Básaca-Preciado et al. Intelligent transportation scheme for autonomous vehicle in smart campus
CN117234203A (zh) 一种多源里程融合slam井下导航方法
Marlow et al. Local terrain mapping for obstacle avoidance using monocular vision
Selkäinaho Adaptive autonomous navigation of mobile robots in unknown environments
Alkhawaja et al. Low-cost depth/IMU intelligent sensor fusion for indoor robot navigation
Xu et al. Probabilistic membrane computing-based SLAM for patrol UAVs in coal mines
CN114740869A (zh) 多传感器融合估计与迭代预搜索的机器人避障方法及系统
CN114003041A (zh) 一种多无人车协同探测系统
Wang et al. Information-fusion based robot simultaneous localization and mapping adapted to search and rescue cluttered environment
Li et al. A method for collision free sensor network based navigation of flying robots among moving and steady obstacles
Guo et al. A Bio-inspired SLAM System for a Legged 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
GR01 Patent grant
GR01 Patent grant