CN117246425A - 四足机器人导航避障与攀爬楼梯方法及系统 - Google Patents
四足机器人导航避障与攀爬楼梯方法及系统 Download PDFInfo
- Publication number
- CN117246425A CN117246425A CN202311462521.4A CN202311462521A CN117246425A CN 117246425 A CN117246425 A CN 117246425A CN 202311462521 A CN202311462521 A CN 202311462521A CN 117246425 A CN117246425 A CN 117246425A
- Authority
- CN
- China
- Prior art keywords
- robot
- map
- coordinate system
- foot
- grid
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 61
- 230000009194 climbing Effects 0.000 title claims abstract description 44
- 206010034701 Peroneal nerve palsy Diseases 0.000 claims abstract description 11
- 238000005259 measurement Methods 0.000 claims description 52
- 239000011159 matrix material Substances 0.000 claims description 26
- 230000006870 function Effects 0.000 claims description 11
- 230000004927 fusion Effects 0.000 claims description 11
- 230000008569 process Effects 0.000 claims description 10
- 230000002787 reinforcement Effects 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 7
- 238000002372 labelling Methods 0.000 claims description 6
- 238000010801 machine learning Methods 0.000 claims description 6
- 230000007613 environmental effect Effects 0.000 claims description 5
- 238000012935 Averaging Methods 0.000 claims description 4
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 4
- 238000001914 filtration Methods 0.000 claims description 4
- 238000011835 investigation Methods 0.000 claims description 4
- 230000003068 static effect Effects 0.000 claims description 4
- 238000004140 cleaning Methods 0.000 claims description 3
- 230000008447 perception Effects 0.000 claims description 3
- 238000012545 processing Methods 0.000 claims description 3
- 238000012549 training Methods 0.000 claims description 3
- 238000013507 mapping Methods 0.000 claims description 2
- 241000282414 Homo sapiens Species 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000008878 coupling Effects 0.000 description 2
- 238000010168 coupling process Methods 0.000 description 2
- 238000005859 coupling reaction Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000005457 optimization Methods 0.000 description 2
- 230000000750 progressive effect Effects 0.000 description 2
- 230000002441 reversible effect Effects 0.000 description 2
- 238000012876 topography Methods 0.000 description 2
- 230000000007 visual effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015572 biosynthetic process Effects 0.000 description 1
- 230000001808 coupling effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 239000006185 dispersion Substances 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 238000003786 synthesis reaction Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
- B62D—MOTOR VEHICLES; TRAILERS
- B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
- B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
- B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
-
- 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)
- Chemical & Material Sciences (AREA)
- Combustion & Propulsion (AREA)
- Transportation (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明提供了一种四足机器人导航避障与攀爬楼梯方法及系统，包括：步骤S1：建立四足机器人预设非结构化环境场景模型；步骤S2：机载双目相机获取并分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；步骤S3：四足机器人根据全局地图和A*路径规划算法实现四足机器人自主导航避障；步骤S4：四足机器人根据非对称Actor‑Critic网络训练，并结合全局地图和局部高程地图规划落足点，从而实现四足机器人运动控制和攀爬楼梯。本发明避免了四足机器人在非结构环境中运动可能出现的失稳、摔倒等问题，有效解决了四足机器人复杂地形下运动的安全隐患，提高了四足机器人的运动能力，确保了特定任务执行的安全及效率。
Description
技术领域
本发明涉及四足机器人技术领域，具体地，涉及四足机器人导航避障与攀爬楼梯方法及系统，更为具体地，涉及一种非结构化环境下四足机器人导航避障与攀爬楼梯方法及系统。
背景技术
四足机器人作为一种典型的足式机器人，相较其他足式机器人，在机械结构稳定性和运动速度方面均有较好的表现，同时具有强大的负载能力。四足机器人有潜力在非结构化环境中协助或替代人类完成探索、搜救、运输等复杂任务，极大增加了人类探索未开发区域的机会。四足机器人因具有良好的稳定性和通用性，目前得到了广泛的关注。
然而，四足机器人作为移动机器人的一种，由于其自身机械耦合结构的复杂性，面向其运动学与动力学系统仿真设计存在着操作难点：从动力学角度来看，四足机器人是一个复杂的动力学耦合系统，在对其动力学建模过程中需要考虑机体-单腿的耦合效应；从运动学角度来看，四足机器人可以被认为是将多个串联机械臂并联而成的移动平台。此外，还需要考虑四足机器人各种外部传感设备。
自主导航与动态避障是四足机器人安全移动与运动的基础与前提。自主导航是实现实时定位与自主地图构建，在非结构化的室内环境中，存在拱形通道、窄巷、草坪、楼梯等复杂场景，使得四足机器人需要在短时间、近距离约束下实时准确地探测到不同障碍物，并及时做出路径调整，这无疑加大了动态避障的难度；而室内光线不均匀，存在多处阴影区域，导致各个障碍物表面曝光度有差异，由于摄像机采集信息时易受光线干扰，甚至在光线条件不好的情况下无法正常工作，这就给四足机器人的视觉导航带来了挑战。虽然激光导航可以解决对室内光线敏感的难题，但激光传感器大部分部署在顶端，导致增加机身高度，同时还可能对低矮障碍物存在探测盲区。融合视觉导航和激光导航的策略提供了新的思路，但具体的融合策略，又必须要结合室内环境的复杂性，有针对性地设计，这也是四足机器人自主导航的又一挑战。
对于四足机器人攀爬楼梯的场景，构建高精度局部高程地图是一个重要的环节。对于攀爬楼梯的场景，目前存在着容易踏到楼梯边缘造成失稳和同一台阶多次踏步等问题，为实现四足机器人平稳自然的攀爬楼梯，需要精确的估计台阶的宽度和高度；对于一些松软崎岖地形，足-地动态接触过程中极易出现明显的足底打滑和地面凹陷等现象，这会造成估计的高度短暂失效等问题，为实现四足机器人整体姿态的稳定，在这些情况下，如何从相机和激光雷达获得的信息中实时有效的构建出高精度的局部高程地图也是一个关键问题。
专利文献CN115840453B(申请号：202310098617.0)公开了一种四足机器人自适应足端避障方法、装置、电子设备及介质，其中，方法包括以下步骤：获取所有参考地面点的地形高度，并基于预设的轨迹方程类型获取四足机器人足端的摆动轨迹；根据所述参考地面点的地形高度和所述摆动轨迹实时调整所述四足机器人足端的移动轨迹，使所述四足机器人足端不与地面碰撞。
发明内容
针对现有技术中的缺陷，本发明的目的是提供一种四足机器人导航避障与攀爬楼梯方法及系统。
根据本发明提供的一种四足机器人导航避障与攀爬楼梯方法，包括：
步骤S1：建立四足机器人预设非结构化环境场景模型；
步骤S2：机载双目相机获取并分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；
步骤S3：四足机器人根据全局地图和A*路径规划算法实现四足机器人自主导航避障；
步骤S4：四足机器人根据非对称Actor-Critic网络训练，并结合全局地图和局部高程地图规划落足点，从而实现四足机器人运动控制和攀爬楼梯。
优选地，所述步骤S2采用：
步骤S2.1：利用机载双目相机获取周围环境RGB图像和深度图像；
步骤S2.2：基于初始地图模板利用机器学习的方式和人工的方式进行分类、标注和验证，得到包含语义信息的全局地图；
步骤S2.3：根据机器人运动数据对所有单元进行更新建立以机器人为中心的高精度局部高程地图。
优选地，所述步骤S2.2采用：
步骤S2.2.1：对传感器数据进行包括整理、分类以及清洗处理，得到没有任何语义信息或注释的初始地图模板；
步骤S2.2.2：利用机器学习和/或人工的方式检测静态对象并对其进行分类和标注，包括：障碍物和楼梯；
步骤S2.2.3：通过人工纠错排查确保地图自动创建过程正确性；
步骤S2.2.4：发布包含语义信息的全局地图。
优选地，所述步骤S2.3采用：
步骤S2.3.1：根据距离传感器数据对有新测量值的单元进行更新得到各单元估计高度和高度方差；
步骤S2.3.2：根据机器人运动数据对所有单元进行更新，保持局部地图始终以机器人为中心，得到各单元估计高度和高度协方差；
步骤S2.3.3：对目标单元进行地图数据融合，得到各单元估计高度和上下置信界限。
优选地，所述步骤S2.3.1采用：
来自传感器的新测量值会作为一个个点映射到高程地图上；具体的某一个点会导致高程地图中的一个单元(x,y)拥有一个新的高度值在地图坐标系{M}中，高度测量值通过高斯概率分布来估计；对于传感器坐标系{S}下的某一点P的位置SrSP，一个单个的测量值可以被转换成地图坐标系{M}下对应的高度测量值；
其中，投影矩阵P＝[0 0 1]将三维测量值映射成高度测量值；为了获得高度测量值的方差推导传感器测量值和传感器坐标框架旋转量的雅可比矩阵JS和JΦ为
其中：
假设则
方差的传播误差为
其中，∑S为距离传感器模型的协方差矩阵；表示传感器旋转量的协方差矩阵；
使用一维卡尔曼滤波将高度测量值和现有的高程地图估计值融合：
其中，上标+表示估计值是在更新之后得到的，上标-表示估计值是在更新之前得到的。
当多个不同高度的测量值落在同一个栅格时，将基于马氏距离融合最高的测量值，并丢弃低于当前估计值一定距离的测量值；当某一区域一直被测量或被重新穿过时，这种融合多个高度测量值的方法仍然有效。
优选地，所述步骤S2.3.2采用：
当栅格单元i收到一个测量更新值时，将其协方差设定为
其中，和是栅格离散化产生的水平不确定性的近似值；对于边长为d的正方形栅格单元，
当栅格单元没有接收到新的测量值时，机器人将根据前一个位姿到当前位姿的变换不断更新协方差矩阵在时间k时，地图参考坐标系{Mk}通过位姿变换可以得到机器人当前参考坐标系在时间k＝2时，点P在地图坐标系{M2}下的位置估计值由k＝1时的点P在地图坐标系{M1}下的位置估计值表示
将坐标统一到参考坐标系{M2}下表示为
不失一般性的，设定参考坐标系{M2}的位姿为
通过如下定义，对齐参考坐标系{M2}和{M1}
此时，定义一个公共参考坐标系{M}，坐标系{M1}和{M2}被统一在一个坐标系{M}下；这样在地图更新时，不必再移动地图中每个栅格的数据，仅需随着机器人的运动改变地图的中心位置即可；
对于协方差从k＝1到k＝2的传播为
其中在k＝1时，协方差∑P,1通过初始化赋值或根据上一次的更新赋值；协方差∑r和∑Φ表示坐标系和的运动估计不确定性，即
计算得到雅可比矩阵
对于地图坐标系统一的情况下，需要改写误差传播式；对于公共参考坐标系{M}，从时刻k到时刻k+1，地图中每个栅格i上点Pi的协方差为
其中
优选地，所述步骤S2.3.3采用：
当需要用到地图时，将每个栅格的高程地图数据转换为hi,max和hi,min表示高度估计值的上下置信区间；hi,max和hi,min的值是从周围的栅格单元数据推断得到，这些栅格单元数据落在满足预设条件的2σ椭圆置信区间内；平均高度估计值通过置信区间范围内的高度值加权平均得到
其中，hk表示第k个栅格单元未融合前的高度值，I表示第i个栅格单元满足预设条件的2σ椭圆置信区间内栅格单元的集合；ωk表示第k个栅格单元的概率权重。
优选地，所述步骤S4采用：
步骤M4.1：使用非对称Actor-Critic强化学习网络训练四足机器人运动和攀爬楼梯，实现落足点规划，并通过PPO算法优化；
步骤M4.2：使用一个上下文辅助估计网络架构来学习状态估计和环境推断的潜在表示信息。
优选地，所述步骤S4.2采用：
上下文辅助估计网络CENet使用混合损失函数进行优化，定义如下：
LCE＝Lest+LVAE
其中，Lest表示身体速度估计量，LVAE表示VAE损失值；对于身体速度状态估计，采用身体速度估计值和真实值vt之间的均方误差损失函数，即
VAE网络是通过标准的β-VAE损失进行训练的，其中包括了重建损失和潜在损失；采用均方误差作为重建损失，KL散度作为潜在损失；因此，VAE损失定义为
其中，是下一个本体感知重建观测值，是潜在状态zt的后验分布值。给定的和p(zt)是由高斯分布参数化的上下文先验分布；因为所有观测值都是经过归一化的，所以选择选择标准正态分布作为先验分布。
根据本发明提供的一种四足机器人导航避障与攀爬楼梯系统，包括：
模块M1：建立四足机器人预设非结构化环境场景模型；
模块M2：机载双目相机获取并分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；
模块M3：四足机器人根据全局地图和A*路径规划算法实现四足机器人自主导航避障；
模块M4：四足机器人根据非对称Actor-Critic网络训练，并结合全局地图和局部高程地图规划落足点，从而实现四足机器人运动控制和攀爬楼梯。
与现有技术相比，本发明具有如下的有益效果：
1、本发明构建了包含语义信息的全局地图和以机器人为中心的高精度局部高程地图，实现了对周围环境的精确感知理解，避免了因为地图精度误差导致的碰撞、摔倒等问题，进而辅助四足机器人实现非结构化环境下的导航运动；
2、本发明针对四足机器人复杂结构设计了非对称Actor-Crit ic强化学习网络，隐式学习了各种地形特征，提高了四足机器人非结构化环境下运动的鲁棒性，有效解决了四足机器人复杂地形下运动的安全隐患，提高了四足机器人的运动能力，确保了特定任务执行的安全及效率。
附图说明
通过阅读参照以下附图对非限制性实施例所作的详细描述，本发明的其它特征、目的和优点将会变得更明显：
图1为四足机器人典型非结构化环境场景图。
图2为一种非结构化环境下四足机器人导航避障与攀爬楼梯方法的流程示意图。
图3为构建以机器人为中心的高精度局部高程地图的流程示意图。
图4为四足机器人运动控制和攀爬楼梯的流程示意图。
图5为四足机器人攀爬楼梯示意图。
图6为四足机器人穿越软垫示意图。
图7为四足机器人穿越窄巷示意图。
具体实施方式
下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变化和改进。这些都属于本发明的保护范围。
实施例1
根据本发明提供的一种四足机器人导航避障与攀爬楼梯方法，包括：
步骤S1：建立四足机器人典型非结构化环境场景模型；
具体地，所述步骤S1包括：
在四足机器人典型非结构化环境场景模型下，具体模型有：宇树Go1四足机器人、拱形门、草坪、楼梯、窄巷、软垫、空水池；
所述草坪在场景模型正中间；宇树Go1四足机器人在草坪右侧；拱形门在宇树Go1四足机器人正前方；楼梯在草坪上方；窄巷在草坪左侧；软垫和空水池在草坪下方；软垫在空水池左侧。
步骤S2：机载双目相机获取并分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；
具体地，所述步骤S2包括：
步骤S2.1：双目相机获取周围环境RGB图像和深度图像；
步骤S2.2：建立包含语义信息的全局地图；
具体地，所述步骤S2.2包括：
步骤S2.2.1：数据处理。对传感器数据进行整理、分类以及清洗从而获得没有任何语义信息或注释的初始地图模板；
步骤S2.2.2：对象检测。利用机器学习的方式，检测大部分常见的静态对象并对其进行分类，包括障碍物、楼梯等。利用人工的方式，标注少量复杂的对象；
步骤S2.2.3：手动验证。通过人工纠错排查，确保地图自动创建过程正确并及时发现问题；
步骤S2.2.4：地图发布。发布包含语义信息的全局地图。
步骤S2.3：建立以机器人为中心的高精度局部高程地图。
具体地，所述步骤S2.3包括：
步骤S2.3.1：根据距离传感器数据对有新测量值的单元进行更新，得到各单元估计高度和高度方差；
具体地，所述步骤S2.3.1包括：
来自传感器的新测量值会作为一个个点映射到高程地图上。具体的某一个点会导致高程地图中的一个单元(x,y)拥有一个新的高度值在地图坐标系{M}中，高度测量值通过高斯概率分布来估计。对于传感器坐标系{S}下的某一点P的位置SrSP，一个单个的测量值可以被转换成地图坐标系{M}下对应的高度测量值
其中投影矩阵P＝[001]将三维测量值映射成高度测量值。为了获得高度测量值的方差推导传感器测量值和传感器坐标框架旋转量的雅可比矩阵JS和JΦ为
其中：
假设则
方差的传播误差为
其中，∑S为距离传感器模型的协方差矩阵。表示传感器旋转量的协方差矩阵(∑IS的子矩阵)。
使用一维卡尔曼滤波可以将高度测量值和现有的高程地图估计值融合：
其中，上标+表示估计值是在更新之后得到的，上标-表示估计值是在更新之前得到的。
当多个不同高度的测量值落在同一个栅格时(比如垂直的墙壁)，将基于马氏距离融合最高的测量值，并丢弃低于当前估计值一定距离的测量值。当某一区域一直被测量或被重新穿过时(比如机器人转弯或反向移动时)，这种融合多个高度测量值的方法仍然有效。
步骤S2.3.2：根据机器人运动数据对所有单元进行更新，保持局部地图始终以机器人为中心，得到各单元估计高度和高度协方差；
具体地，所述步骤S2.3.2包括：
当栅格单元i收到一个测量更新值时，将其协方差设定为
其中和是栅格离散化产生的水平不确定性的近似值，即对于边长为d的正方形栅格单元，
当栅格单元没有接收到新的测量值时，机器人将根据前一个位姿到当前位姿的变换不断更新协方差矩阵在时间k时，地图参考坐标系{Mk}通过位姿变换可以得到机器人当前参考坐标系在时间k＝2时，点P在地图坐标系{M2}下的位置估计值可以由k＝1时的点P在地图坐标系{M1}下的位置估计值表示
将坐标统一到参考坐标系{M2}下可以表示为
不失一般性的，可以设定参考坐标系{M2}的位姿为
通过如下定义，即可对齐参考坐标系{M2}和{M1}
此时，可以定义一个公共参考坐标系{M}，即坐标系{M1}和{M2}被统一在一个坐标系{M}下。这样在地图更新时，不必再移动地图中每个栅格的数据，仅需随着机器人的运动改变地图的中心位置即可。
对于协方差从k＝1到k＝2的传播为
其中在k＝1时，协方差∑P,1通过初始化赋值或根据上一次的更新赋值。协方差∑r和∑Φ表示坐标系和的运动估计不确定性，即
计算得到雅可比矩阵
对于地图坐标系统一的情况下，需要改写误差传播式。对于公共参考坐标系{M}，从时刻k到时刻k+1，地图中每个栅格i上点Pi的协方差为
其中
步骤S2.3.3：对目标单元进行地图数据融合，得到各单元估计高度和上下置信界限。
具体地，所述步骤S2.3.3包括：
地图融合即当需要用到地图时，将每个栅格的高程地图数据转换为hi,max和hi,min表示高度估计值的上下置信区间。hi,max和hi,min的值是从周围的栅格单元数据推断得到，这些栅格单元数据落在95％的2σ椭圆置信区间内。平均高度估计值通过置信区间范围内的高度值加权平均得到
其中，hk表示第k个栅格单元未融合前的高度值，I表示第i个栅格单元95％的2σ椭圆置信区间内栅格单元的集合。ωk表示第k个栅格单元的概率权重。
步骤S3：四足机器人根据全局地图和A*路径规划算法实现自主导航避障；
具体地，所述步骤S3包括：
步骤S3.1：设置导航目标点位置；
步骤S3.2：根据全局地图信息计算周围地图单元对应的路径代价；
步骤S3.3：使用A*算法计算最优路径，实现导航避障。
步骤S4：四足机器人根据非对称Actor-Critic网络训练，并结合全局地图和局部地图规划落足点，从而实现四足机器人运动控制和攀爬楼梯；
具体地，所述步骤S4包括：
步骤S4.1：使用非对称Actor-Critic强化学习网络训练四足机器人运动和攀爬楼梯，实现落足点规划，并通过PPO算法优化；
具体地，所述步骤S4.1包括：
采用了一个非对称的Actor-Critic架构来隐式学习地形特征。通过策略和价值网络之间的相互作用来学习一个鲁棒的运动策略，该策略在给定时间内可以隐式地想象特权信息。策略(Actor)接收部分观测值作为输入，而价值网络(Critic)接收完整状态st。此外，使用了近端策略优化(PPO)算法对策略进行了优化。
使用了一个受游戏启发的课程学习，从而实现复杂地形下的渐进式运动策略学习。地形由平滑、粗糙、离散化和楼梯地形组成，坡度在[0°，22°]以内。此外，利用网格自适应课程进行低速运动，进一步提高了转弯的速度和稳定性。
步骤S4.2：使用一个上下文辅助估计网络(CENet)架构来学习状态估计和环境推断的潜在表示信息。
具体地，所述步骤S4.2包括：
上下文辅助估计网络CENet使用混合损失函数进行优化，定义如下：
LCE＝Lest+LVAE
其中Lest表示身体速度估计量，LVAE表示VAE损失值。对于身体速度状态估计，采用身体速度估计值和真实值vt之间的均方误差损失函数，即
VAE网络是通过标准的β-VAE损失进行训练的，其中包括了重建损失和潜在损失。采用均方误差作为重建损失，KL散度作为潜在损失。因此，VAE损失定义为
其中是下一个本体感知重建观测值，是潜在状态zt的后验分布值。给定的和p(zt)是由高斯分布参数化的上下文先验分布。因为所有观测值都是经过归一化的，所以选择选择标准正态分布作为先验分布。
所述四足机器人典型非结构化环境场景模型包括四足机器人本体模型、环境中的特定场景模型、及运行情况。
首先根据周围环境的三维信息提出了一个全局环境地图，而局部高程地图是根据距离传感器信息和机器人本体运动学数据更新融合得到的，两者结合实现了非结构化环境下的精确导航避障。
基于非对称Actor-Critic强化学习算法，本发明实现了四足机器人的鲁棒控制。本发明最终目的是通过周围环境地图和强化学习算法，完成对周围环境的理解，并有针对性地选择相应的运动控制方案。
本发明还提供一种四足机器人导航避障与攀爬楼梯系统，所述四足机器人导航避障与攀爬楼梯系统可以通过执行所述四足机器人导航避障与攀爬楼梯方法的流程步骤予以实现，即本领域技术人员可以将所述四足机器人导航避障与攀爬楼梯方法理解为所述四足机器人导航避障与攀爬楼梯系统的优选实施方式。
实施例2
实施例2是实施例1的优选例
以下优选例对本发明做进一步详细说明：
如图2所示，本实施例公开了一种非结构化环境下四足机器人导航避障与攀爬楼梯方法，包括如下步骤S00～S30：
S00、建立四足机器人典型非结构化环境场景，如图1所示，即楼梯、窄巷、草坪、软垫等场景；
S10、分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；
S20、根据全局地图进行路径规划，实现四足机器人自主导航避障；
S30、通过强化学习算法规划落足点，实现四足机器人运动控制和攀爬楼梯。
具体来说，所述建立四足机器人典型非结构化环境场景为：
在四足机器人典型非结构化环境场景模型下，具体模型有：宇树Go1四足机器人、拱形门、草坪、楼梯、窄巷、软垫、空水池；
需要说明的是，如图1所示，所述草坪在场景模型正中间；宇树Go1四足机器人在草坪右侧；拱形门在宇树Go1四足机器人正前方；楼梯在草坪上方；窄巷在草坪左侧；软垫和空水池在草坪下方；软垫在空水池左侧。
具体来说，所述分析周围环境信息过程为：
四足机器人上的双目相机扫描周围环境的三维信息，结合机器人自带的编码器和惯性传感器，采集位姿信息，用于机器人实现地图构建。
需要说明的是，四足机器人的双目相机可用于获取周围环境的RGB图像和点云图像，进而可以获取周围物体的深度信息。
具体来说，如图3所示，所述实现以机器人为中心的高精度局部高程地图，包括如下步骤S100～S102：
S100、根据距离传感器数据对有新测量值的单元进行更新，得到各单元估计高度和高度方差，包括：
来自传感器的新测量值会作为一个个点映射到高程地图上。具体的某一个点会导致高程地图中的一个单元(x,y)拥有一个新的高度值在地图坐标系{M}中，高度测量值通过高斯概率分布来估计。对于传感器坐标系{S}下的某一点P的位置SrSP，一个单个的测量值可以被转换成地图坐标系{M}下对应的高度测量值
其中投影矩阵P＝[001]将三维测量值映射成高度测量值。为了获得高度测量值的方差推导传感器测量值和传感器坐标框架旋转量的雅可比矩阵JS和JΦ为
其中：
假设则
方差的传播误差为
其中，∑S为距离传感器模型的协方差矩阵。表示传感器旋转量的协方差矩阵(∑IS的子矩阵)。
使用一维卡尔曼滤波可以将高度测量值和现有的高程地图估计值融合：
其中，上标+表示估计值是在更新之后得到的，上标-表示估计值是在更新之前得到的。
需要说明的是，当多个不同高度的测量值落在同一个栅格时(比如垂直的墙壁)，将基于马氏距离融合最高的测量值，并丢弃低于当前估计值一定距离的测量值。当某一区域一直被测量或被重新穿过时(比如机器人转弯或反向移动时)，这种融合多个高度测量值的方法仍然有效。
S101、根据机器人运动数据对所有单元进行更新，保持局部地图始终以机器人为中心，得到各单元估计高度和高度协方差，包括：
当栅格单元i收到一个测量更新值时，将其协方差设定为
其中和是栅格离散化产生的水平不确定性的近似值，即对于边长为d的正方形栅格单元，
当栅格单元没有接收到新的测量值时，机器人将根据前一个位姿到当前位姿的变换不断更新协方差矩阵在时间k时，地图参考坐标系{Mk}通过位姿变换可以得到机器人当前参考坐标系在时间k＝2时，点P在地图坐标系{M2}下的位置估计值可以由k＝1时的点P在地图坐标系{M1}下的位置估计值表示
将坐标统一到参考坐标系{M2}下可以表示为
不失一般性的，可以设定参考坐标系{M2}的位姿为
通过如下定义，即可对齐参考坐标系{M2}和{M1}
此时，可以定义一个公共参考坐标系{M}，即坐标系{M1}和{M2}被统一在一个坐标系{M}下。这样在地图更新时，不必再移动地图中每个栅格的数据，仅需随着机器人的运动改变地图的中心位置即可。
对于协方差从k＝1到k＝2的传播为
其中在k＝1时，协方差∑P,1通过初始化赋值或根据上一次的更新赋值。协方差∑r和∑Φ表示坐标系和的运动估计不确定性，即
计算得到雅可比矩阵
对于地图坐标系统一的情况下，需要改写误差传播式。对于公共参考坐标系{M}，从时刻k到时刻k+1，地图中每个栅格i上点Pi的协方差为
其中
S102、对目标单元进行地图数据融合，得到各单元估计高度和上下置信界限，包括：
地图融合即当需要用到地图时，将每个栅格的高程地图数据转换为hi,max和hi,min表示高度估计值的上下置信区间。hi,max和hi,min的值是从周围的栅格单元数据推断得到，这些栅格单元数据落在95％的2σ椭圆置信区间内。平均高度估计值通过置信区间范围内的高度值加权平均得到
需要说明的是，其中，hk表示第k个栅格单元未融合前的高度值，I表示第i个栅格单元95％的2σ椭圆置信区间内栅格单元的集合。ωk表示第k个栅格单元的概率权重。
具体来说，如图4所示，所述实现四足机器人运动控制和攀爬楼梯，包括如下步骤S200～S304：
S200、根据全局地图进行路径规划，实现四足机器人定位导航，包括：
首先设置导航目标点位置，再根据全局地图信息计算周围地图单元对应的路径代价，从而使用A*算法计算最优路径，实现导航避障。
S103、根据语义地图判断面前障碍物类型，选择相应运动模式，包括：
对传感器数据进行整理、分类以及清洗从而获得没有任何语义信息或注释的初始地图模板。利用机器学习的方式，检测大部分常见的静态对象并对其进行分类，包括障碍物、楼梯等。利用人工的方式，标注少量复杂的对象。通过人工纠错排查，确保地图自动创建过程正确并及时发现问题。最后，发布包含语义信息的全局地图。
需要说明的是，根据语义地图信息，四足机器人会选择不同的运动模式，从而有针对性的适应不同的地形。
S300、可跨越障碍物，如：草坪、楼梯、软垫等，包括：
对于可跨越障碍物，四足机器人将选择直接穿越，通过自适应的控制算法保持机器人运动过程中的姿态稳定。
如图5所示，四足机器人攀爬楼梯。如图6所示，四足机器人穿越软垫。
需要说明的是，对于楼梯等复杂障碍物，四足机器人在运动过程中会调整自身躯干角度，使其与坡面平行，提高攀爬速度。
S301、采用非对称Actor-Critic强化学习网络实现四足机器人实现落足点规划，包括：
采用了一个非对称的Actor-Critic架构来隐式学习地形特征。通过策略和价值网络之间的相互作用来学习一个鲁棒的运动策略，该策略在给定时间内可以隐式地想象特权信息。策略(Actor)接收部分观测值作为输入，而价值网络(Critic)接收完整状态st。此外，使用了近端策略优化(PPO)算法对策略进行了优化。
需要说明的是，使用了一个受游戏启发的课程学习，从而实现复杂地形下的渐进式运动策略学习。地形由平滑、粗糙、离散化和楼梯地形组成，坡度在[0°，22°]以内。此外，利用网格自适应课程进行低速运动，进一步提高了转弯的速度和稳定性。
S302、使用上下文辅助估计网络架构学习状态估计和环境推断的潜在表示信息，包括：
上下文辅助估计网络CENet使用混合损失函数进行优化，定义如下：
LCE＝Lest+LVAE
需要说明的是，其中Lest表示身体速度估计量，LVAE表示VAE损失值。对于身体速度状态估计，采用身体速度估计值和真实值vt之间的均方误差损失函数，即
VAE网络是通过标准的β-VAE损失进行训练的，其中包括了重建损失和潜在损失。采用均方误差作为重建损失，KL散度作为潜在损失。因此，VAE损失定义为
需要说明的是，其中是下一个本体感知重建观测值，是潜在状态zt的后验分布值。给定的和p(zt)是由高斯分布参数化的上下文先验分布。因为所有观测值都是经过归一化的，所以选择选择标准正态分布作为先验分布。
S303、不可跨越障碍物，如墙体等，包括：
不可跨越的障碍物远高于机器人自身高度，导致四足机器人无法通过攀爬通过。
如图7所示，四足机器人避开墙体穿越窄巷。
需要说明的是，四足机器人在接近该障碍物时，双目相机通常难以完整获得该物体全部信息，故根据语义地图标注信息进行判别。
S304、使用A*算法计算最优路径，实现动态导航避障，包括：
A*算法计算每个节点优先级的公式为：
f(n)＝g(n)+h(n)
需要说明的是，f(n)是节点n的综合优先级。当选择下一个要遍历的节点时，总会选取综合优先级最高(值最小)的节点。g(n)是节点n距离起点的代价。h(n)是节点n距离终点的预计代价，即A*算法的启发函数。
需要进一步说明的是，A*算法在运算过程中，每次从优先队列中选取f(n)值最小(优先级最高)的节点作为下一个待遍历的节点。
本发明通过对四足机器人典型非结构化环境场景模型建模，构建了四足机器人导航避障与攀爬楼梯系统，通过全局地图和局部地图的构建，进一步提高了四足机器人对周围环境的感知能力，从而有针对的对不同环境场景采用不同的运动模式。因此，本发明有效识别了四足机器人周围环境场景模型，为四足机器人鲁棒运动提供了可靠的算法支持，有效解决了四足机器人复杂地形下的运动隐患，提高了四足机器人的运动能力和效率。
本领域技术人员知道，除了以纯计算机可读程序代码方式实现本发明提供的系统及其各个装置、模块、单元以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统及其各个装置、模块、单元以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同功能。所以，本发明提供的系统及其各项装置、模块、单元可以被认为是一种硬件部件，而对其内包括的用于实现各种功能的装置、模块、单元也可以视为硬件部件内的结构；也可以将用于实现各种功能的装置、模块、单元视为既可以是实现方法的软件模块又可以是硬件部件内的结构。
在本申请的描述中，需要理解的是，术语“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本申请和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本申请的限制。
以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。
Claims (10)
1.一种四足机器人导航避障与攀爬楼梯方法，其特征在于，包括：
步骤S1：建立四足机器人预设非结构化环境场景模型；
步骤S2：机载双目相机获取并分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；
步骤S3：四足机器人根据全局地图和A*路径规划算法实现四足机器人自主导航避障；
步骤S4：四足机器人根据非对称Actor-Critic网络训练，并结合全局地图和局部高程地图规划落足点，从而实现四足机器人运动控制和攀爬楼梯。
2.根据权利要求1所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S2采用：
步骤S2.1：利用机载双目相机获取周围环境RGB图像和深度图像；
步骤S2.2：基于初始地图模板利用机器学习的方式和人工的方式进行分类、标注和验证，得到包含语义信息的全局地图；
步骤S2.3：根据机器人运动数据对所有单元进行更新建立以机器人为中心的高精度局部高程地图。
3.根据权利要求2所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S2.2采用：
步骤S2.2.1：对传感器数据进行包括整理、分类以及清洗处理，得到没有任何语义信息或注释的初始地图模板；
步骤S2.2.2：利用机器学习和/或人工的方式检测静态对象并对其进行分类和标注，包括：障碍物和楼梯；
步骤S2.2.3：通过人工纠错排查确保地图自动创建过程正确性；
步骤S2.2.4：发布包含语义信息的全局地图。
4.根据权利要求2所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S2.3采用：
步骤S2.3.1：根据距离传感器数据对有新测量值的单元进行更新得到各单元估计高度和高度方差；
步骤S2.3.2：根据机器人运动数据对所有单元进行更新，保持局部地图始终以机器人为中心，得到各单元估计高度和高度协方差；
步骤S2.3.3：对目标单元进行地图数据融合，得到各单元估计高度和上下置信界限。
5.根据权利要求4所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S2.3.1采用：
来自传感器的新测量值会作为一个个点映射到高程地图上；具体的某一个点会导致高程地图中的一个单元(x,y)拥有一个新的高度值在地图坐标系{M}中，高度测量值通过高斯概率分布来估计；对于传感器坐标系{S}下的某一点P的位置SrSP，一个单个的测量值可以被转换成地图坐标系{M}下对应的高度测量值；
其中，投影矩阵P＝[001]将三维测量值映射成高度测量值；为了获得高度测量值的方差推导传感器测量值和传感器坐标框架旋转量的雅可比矩阵JS和JΦ为
其中：
假设则
方差的传播误差为
其中，∑S为距离传感器模型的协方差矩阵；表示传感器旋转量的协方差矩阵；
使用一维卡尔曼滤波将高度测量值和现有的高程地图估计值融合：
其中，上标+表示估计值是在更新之后得到的，上标-表示估计值是在更新之前得到的。
当多个不同高度的测量值落在同一个栅格时，将基于马氏距离融合最高的测量值，并丢弃低于当前估计值一定距离的测量值；当某一区域一直被测量或被重新穿过时，这种融合多个高度测量值的方法仍然有效。
6.根据权利要求4所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S2.3.2采用：
当栅格单元i收到一个测量更新值时，将其协方差设定为
其中，和是栅格离散化产生的水平不确定性的近似值；对于边长为d的正方形栅格单元，
当栅格单元没有接收到新的测量值时，机器人将根据前一个位姿到当前位姿的变换不断更新协方差矩阵在时间k时，地图参考坐标系{Mk}通过位姿变换可以得到机器人当前参考坐标系在时间k＝2时，点P在地图坐标系{M2}下的位置估计值由k＝1时的点P在地图坐标系{M1}下的位置估计值表示
将坐标统一到参考坐标系{M2}下表示为
不失一般性的，设定参考坐标系{M2}的位姿为
通过如下定义，对齐参考坐标系{M2}和{M1}
此时，定义一个公共参考坐标系{M}，坐标系{M1}和{M2}被统一在一个坐标系{M}下；这样在地图更新时，不必再移动地图中每个栅格的数据，仅需随着机器人的运动改变地图的中心位置即可；
对于协方差从k＝1到k＝2的传播为
其中在k＝1时，协方差∑P,1通过初始化赋值或根据上一次的更新赋值；协方差∑r和∑Φ表示坐标系和的运动估计不确定性，即
计算得到雅可比矩阵
对于地图坐标系统一的情况下，需要改写误差传播式；对于公共参考坐标系{M}，从时刻k到时刻k+1，地图中每个栅格i上点Pi的协方差为
其中
7.根据权利要求4所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S2.3.3采用：
当需要用到地图时，将每个栅格的高程地图数据转换为hi,max和hi,min表示高度估计值的上下置信区间；hi,max和hi,min的值是从周围的栅格单元数据推断得到，这些栅格单元数据落在满足预设条件的2σ椭圆置信区间内；平均高度估计值通过置信区间范围内的高度值加权平均得到
其中，hk表示第k个栅格单元未融合前的高度值，I表示第i个栅格单元满足预设条件的2σ椭圆置信区间内栅格单元的集合；ωk表示第k个栅格单元的概率权重。
8.根据权利要求1所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S4采用：
步骤M4.1：使用非对称Actor-Critic强化学习网络训练四足机器人运动和攀爬楼梯，实现落足点规划，并通过PPO算法优化；
步骤M4.2：使用一个上下文辅助估计网络架构来学习状态估计和环境推断的潜在表示信息。
9.根据权利要求8所述的四足机器人导航避障与攀爬楼梯方法，其特征在于，所述步骤S4.2采用：
上下文辅助估计网络CENet使用混合损失函数进行优化，定义如下：
LCE＝Lest+LVAE
其中，Lest表示身体速度估计量，LVAE表示VAE损失值；对于身体速度状态估计，采用身体速度估计值和真实值vt之间的均方误差损失函数，即
VAE网络是通过标准的β-VAE损失进行训练的，其中包括了重建损失和潜在损失；采用均方误差作为重建损失，KL散度作为潜在损失；因此，VAE损失定义为
其中，是下一个本体感知重建观测值，是潜在状态zt的后验分布值。给定的和p(zt)是由高斯分布参数化的上下文先验分布；因为所有观测值都是经过归一化的，所以选择选择标准正态分布作为先验分布。
10.一种四足机器人导航避障与攀爬楼梯系统，其特征在于，包括：
模块M1：建立四足机器人预设非结构化环境场景模型；
模块M2：机载双目相机获取并分析周围环境信息，建立包含语义信息的全局地图及以机器人为中心的高精度局部高程地图；
模块M3：四足机器人根据全局地图和A*路径规划算法实现四足机器人自主导航避障；
模块M4：四足机器人根据非对称Actor-Critic网络训练，并结合全局地图和局部高程地图规划落足点，从而实现四足机器人运动控制和攀爬楼梯。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311462521.4A CN117246425A (zh) | 2023-11-02 | 2023-11-02 | 四足机器人导航避障与攀爬楼梯方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311462521.4A CN117246425A (zh) | 2023-11-02 | 2023-11-02 | 四足机器人导航避障与攀爬楼梯方法及系统 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117246425A true CN117246425A (zh) | 2023-12-19 |
Family
ID=89126578
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311462521.4A Pending CN117246425A (zh) | 2023-11-02 | 2023-11-02 | 四足机器人导航避障与攀爬楼梯方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117246425A (zh) |
Cited By (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117830991A (zh) * | 2024-03-04 | 2024-04-05 | 山东大学 | 一种基于多模融合的四足机器人复杂场景感知方法及系统 |
| CN117863190A (zh) * | 2024-03-08 | 2024-04-12 | 广州小鹏汽车科技有限公司 | 足式机器人的移动控制方法及足式机器人 |
| CN118192582A (zh) * | 2024-04-02 | 2024-06-14 | 中国人民解放军海军军医大学第三附属医院 | 基于救护转运机器人的运行路径优化方法及系统 |
-
2023
- 2023-11-02 CN CN202311462521.4A patent/CN117246425A/zh active Pending
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117830991A (zh) * | 2024-03-04 | 2024-04-05 | 山东大学 | 一种基于多模融合的四足机器人复杂场景感知方法及系统 |
| CN117830991B (zh) * | 2024-03-04 | 2024-05-24 | 山东大学 | 一种基于多模融合的四足机器人复杂场景感知方法及系统 |
| CN117863190A (zh) * | 2024-03-08 | 2024-04-12 | 广州小鹏汽车科技有限公司 | 足式机器人的移动控制方法及足式机器人 |
| CN118192582A (zh) * | 2024-04-02 | 2024-06-14 | 中国人民解放军海军军医大学第三附属医院 | 基于救护转运机器人的运行路径优化方法及系统 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Grandia et al. | Perceptive locomotion through nonlinear model-predictive control | |
| CN117246425A (zh) | 四足机器人导航避障与攀爬楼梯方法及系统 | |
| Buchanan et al. | Perceptive whole‐body planning for multilegged robots in confined spaces | |
| Belter et al. | Adaptive motion planning for autonomous rough terrain traversal with a walking robot | |
| KR101121763B1 (ko) | 환경 인식 장치 및 방법 | |
| Belter et al. | Rough terrain mapping and classification for foothold selection in a walking robot | |
| Frey et al. | Locomotion policy guided traversability learning using volumetric representations of complex environments | |
| KR20230006507A (ko) | 계단 트랙커를 위한 인식 및 피팅 | |
| Lehner et al. | Exploration with active loop closing: A trade-off between exploration efficiency and map quality | |
| Wang et al. | Actively mapping industrial structures with information gain-based planning on a quadruped robot | |
| CN113433937A (zh) | 基于启发式探索的分层导航避障系统、分层导航避障方法 | |
| Bartoszyk et al. | Terrain-aware motion planning for a walking robot | |
| CN117270551A (zh) | 基于局部环境感知的四足机器人自主攀爬楼梯方法及系统 | |
| Šalanský et al. | Pose consistency kkt-loss for weakly supervised learning of robot-terrain interaction model | |
| CN117733863A (zh) | 机器人运动控制方法、装置、设备、机器人及存储介质 | |
| Jayawardana et al. | Train a robot to climb staircase using vision-base system | |
| Gómez | Map-building and planning for autonomous navigation of a mobile robot | |
| Jordan et al. | Real-time model based path planning for wheeled vehicles | |
| Ulloa et al. | Vision-based collaborative robots for exploration in uneven terrains | |
| McCrory et al. | Humanoid path planning over rough terrain using traversability assessment | |
| Suo et al. | Real time obstacle avoidance and navigation with mobile robot via local elevation information | |
| McCrory et al. | Bipedal Navigation Planning over Rough Terrain using Traversability Models | |
| Gassmann et al. | Real-time 3d map building for local navigation of a walking robot in unstructured terrain | |
| Visser et al. | Amsterdam Oxford Joint Rescue Forces-Team Description Paper-Virtual Robot competition-Rescue Simulation League-RoboCup 2008 | |
| Chen et al. | Proposal for Navigation System Using Three-Dimensional Maps—Self-Localization Using a 3D Map and Slope Detection Using a 2D Laser Range Finder and 3D Map |
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 |