一种足式人形机器人的全身避障方法及系统与流程

本发明涉及机器人环境感知与运动控制，尤其涉及一种足式人形机器人的全身避障方法及系统。

背景技术：

1、目前业内已公开多项人形机器人避障及运动标定相关技术方案，各类现有技术均存在场景适配单一、技术维度缺失的问题，难以兼顾足式人形机器人高自由度动态外形、实时动态避障、高精度安全容错、分层差异化控制的核心作业需求，无法实现全维度、高精度的全身避障效果。公开号cn119088030a、cn118915773a、cn118836877a均为传统路径规划类避障技术，适配性短板显著。其中cn119088030a依托视觉分割实现路径规划，全程将机器人简化为平面运动质点或固定包络模型，完全忽略人形机器人多连杆三维立体结构特性，无法适配机体运动过程中外形动态变化的避障场景。cn118915773a基于模型预测控制实现障碍跨越避障，仅依靠障碍物高度作为单一触发条件，仅能实现固定跨越动作，未对机器人各连杆运动过程中的碰撞风险进行检测与防护，且仅依托二维半地势图开展环境感知，空间感知维度存在缺失。cn118836877a借助生成对抗网络完成路径规划，以整机单一标量距离作为避障触发依据，无法精准定位风险连杆位置，且技术效果高度依赖训练数据集，泛化性差，无法与机器人关节级精细化运动控制深度耦合。

2、公开号为cn122253272a与cn116985184a的专利聚焦机器人自碰撞检测领域，仅可实现机器人本体连杆间的碰撞排查，不具备外部环境动态避障能力。cn122253272a依靠离线仿真环境完成检测工作，仅输出布尔值碰撞判定结果，无法提供碰撞距离、避让方向等有效调控信息，且必须依托高性能图形处理器算力加持，无法适配机器人嵌入式机载运行场景。cn116985184a采用固定关节包络模型开展自碰撞检测，模型精度无法适配人形机器人动态运动场景，同时未搭建实时环境地图，无法应对外部动态障碍，且未区分机体行走、上肢运动两套独立执行通道，无法实现分部位差异化避障调控。

3、公开号为cn122346163a与cn116909284a的专利属于整机层级粗略避障技术，精细化控制能力缺失。cn122346163a依托建筑信息模型先验地图实现避障导航，极度依赖预设静态地图，无法适配未知动态环境，且将机器人作为单一整体开展避障判定，不区分躯干、上下肢、头部等不同运动部位的避障需求，仅能调整整机步态参数，无法实现上肢独立避障运动。cn116909284a通过限定机体固定坐标系速度阈值实现避障控制，仅基于整机障碍数据调控机体行走速度，未关联单根连杆的实时风险状态，仅能控制机体运动，无法独立管控上肢关节运动，难以实现机体行走与上肢避让同步进行的作业场景。

4、公开号为cn119871388a、cn118305796a、cn117718958a的专利为传统机器人运动学标定技术，应用场景与技术逻辑均存在明显局限。该类方案核心目标仅为提升工业机器人末端定位精度，多依赖外置标定工装完成离线标定作业，无法适配人形机器人现场实时标定需求。同时，此类技术仅完成参数辨识与精度修正，直接舍弃标定过程生成的参数残差协方差数据，未将参数偏差、装配误差等不确定因素转化为避障安全余量，无法解决远端连杆误差累积、近端连杆安全余量冗余的行业痛点。

5、综上，现有技术零散单一、各有短板，无法系统性解决人形机器人动态外形适配、参数偏差容错、低算力实时检测、双通道分层避障的核心技术难题。

技术实现思路

1、本发明的目的在于克服上述现有技术的问题，提供了一种足式人形机器人的全身避障方法及系统，用于解决现有固定包络避障方案两难困境，解决人形机器人多连杆动态外形下的全身碰撞判定难题，兼顾标定不确定度、硬件算力约束与机器人双通道执行架构，实现不同肢体差异化避障处置技术问题。

2、上述目的是通过以下技术方案来实现：

3、本发明一方面提供了一种足式人形机器人的全身避障方法，包括如下步骤：

4、步骤s1：开展全身关节与连杆几何标定，保留标定得到的参数残差协方差，将所述参数残差协方差经连杆位姿雅可比传播，折算得到各连杆对应的膨胀余量；

5、步骤s2：构建连杆碰撞体三级层次结构，对关节状态与滑动窗口占据地图做关节状态时间对齐，广播各级碰撞体的位姿；

6、步骤s3：执行分层剪枝筛选得到风险子链，对风险子链对应的感兴趣区域内占据体素进行贪心合并，结合未观测体素分部位处置策略，执行逐连杆有符号距离查询，得到逐连杆的有符号距离、最近点对以及由两最近点连线确定的单位方向；

7、步骤s4：将查询结果按运动学子链进行分流，分别生成腿躯干子链对应的机体速度约束、上肢头部子链对应的上肢关节避障约束，经仲裁与失效降级处理后，下发至对应的执行单元。

8、作为本方法的优化，步骤s1中所述开展全身关节与连杆几何标定，包括：依靠机载感知部件观测固定在机器人连杆上的本体标志物，采集多组构型下本体标志物观测值与关节读数配对，辨识关节零位偏差与连杆几何偏差，输出修正后的运动学参数以及所述参数残差协方差；将所述参数残差协方差经连杆位姿雅可比矩阵传播，得到连杆笛卡尔空间的位姿协方差，由所述位姿协方差折算得到连杆的各向异性膨胀余量，远端连杆的膨胀余量大于近基座连杆的膨胀余量，并将所述膨胀余量沿连杆凸包外法向叠加至对应连杆凸包。

9、作为本方法的优化，步骤2中所述连杆碰撞体三级层次结构，包括：第零级整机包围球、第一级子链包络胶囊体和第二级逐连杆凸包；所述第零级整机包围球由全部连杆凸包顶点生成；所述第一级子链包络胶囊体按照运动学子链聚合生成；所述第二级逐连杆凸包为叠加对应连杆膨胀余量后的连杆凸包；预先剔除相邻连杆对以及常态接触连杆对，生成检测对白名单，后续距离查询跳过所述检测对白名单内的检测对。

10、作为本方法的优化，所述分层剪枝包括第零级粗筛和第一级风险子链筛选；

11、所述第零级粗筛具体为：判断所述整机包围球与占据体素集合是否相交；

12、若无交，则本周期跳过全部精确查询；

13、若相交，执行第一级风险子链筛选：逐个子链判断所述子链包络胶囊体与占据体素集合是否相交，相交的子链标记为风险子链；无相交则本周期跳过全部精确查询。

14、作为本方法的优化，步骤s3中所述未观测体素分部位处置策略包括：归属腿躯干子链的未观测体素直接计入障碍；归属上肢头部子链的未观测体素，仅当连杆运动趋势朝向障碍时计入障碍，连杆向本体收拢运动时不计入障碍；所述贪心合并为沿三个坐标轴执行行程编码贪心合并，将离散占据体素合并为长方体块并构造为碰撞对象。

15、作为本方法的优化，步骤s3中所述逐连杆有符号距离查询，是将叠加膨胀余量后的第二级逐连杆凸包与所述长方体块并构造为碰撞对象执行凸体有符号距离运算；相分离时距离取正值，相互穿透时距离取穿透深度负值；输出有符号距离、连杆与障碍物上的最近点对、由两最近点连线确定的单位方向，查询过程跳过所述检测对白名单内的检测对。

16、作为本方法的优化，步骤s4中所述生成腿躯干子链对应的机体速度约束，包括：获取腿躯干子链最小有符号距离以及对应的最近点连线方向；将机体速度指令分解为沿所述最近点连线方向的法向分量以及切向分量，仅对法向分量做衰减缩放，切向分量保持不变；缩放完成后对每个速度通道做可执行校验，若通道速度幅值大于0且小于足式平台最小可执行速度幅值，则将该通道置零并置位避障停车标志。

17、作为本方法的优化，步骤s4中所述生成上肢头部子链对应的上肢关节避障约束，包括：利用连杆位姿雅可比，将所述最近点连线方向投影到关节速度空间，构造关节速度的线性避障不等式；以任务期望上肢关节速度作为主任务，在零空间投影框架下求解二次规划；当二次规划求解不可行时，将该子链全部关节置零，驱动上肢向预设收拢构型回退。

18、本发明另一方面提供了一种足式人形机器人的全身避障系统，用于实现上述任一项所述的全身避障方法，包括：

19、全身关节零位与连杆几何标定模块，用于遍历多组标定构型，利用机载感知部件观测本体标志物，辨识关节零位偏差与连杆几何偏差，输出修正后的运动学参数以及参数残差协方差；

20、标定残差传播与逐连杆膨胀余量解算模块，用于接收所述参数残差协方差，经位姿雅可比传播折算得到逐连杆膨胀余量表；

21、连杆碰撞体三级层次离线构建模块，用于接收所述逐连杆膨胀余量表，构建连杆碰撞体三级层次结构；

22、自碰撞对与常态接触对剔除模块，用于剔除关节直接相连的相邻连杆对以及常态接触连杆对，输出检测对白名单；

23、关节状态时间对齐与前向运动学解算模块，用于将关节状态对齐至滑动窗口占据地图时间戳，并基于修正后运动学参数解算全身连杆位姿；

24、全身碰撞体坐标变换广播模块，用于为各级碰撞体挂载坐标系，逐周期广播各级碰撞体的位姿；

25、整机包围球与子链包络胶囊分级粗筛模块，用于执行第零级、第一级分层剪枝，筛选得到风险子链；

26、滑动窗口占据地图构建模块，用于维护以机器人为中心、内存占用恒定的三维滑动窗口占据地图；

27、概率融合与传感器噪声余量膨胀模块，用于对体素做概率融合，仅施加传感器噪声对应的地图膨胀量；

28、未知体素分部位处置模块，用于针对感兴趣区域内未观测体素按照运动学子链执行分部位取舍处置；

29、感兴趣区域体素贪心合并与碰撞对象构造模块，用于对感兴趣区域内体素做行程编码贪心合并，生成长方体块并构造为碰撞对象；

30、逐连杆有符号距离与最近点查询模块，用于结合所述检测对白名单执行逐连杆凸体距离运算，输出逐连杆有符号距离、最近点对、由两最近点连线确定的单位方向；

31、距离结果按运动学子链分流模块，用于将距离查询结果按照运动学子链分流为腿躯干组、上肢头部组两组数据；

32、机体速度方向性缩放模块，用于基于所述腿躯干组数据生成机体速度约束；

33、上肢关节避障不等式构造与零空间求解模块，用于基于所述上肢头部组数据构造避障不等式并零空间投影求解二次规划，输出上肢关节避障约束；

34、全身避障仲裁与失效降级模块，用于合并所述机体速度约束、所述上肢关节避障约束，校验系统状态时效性，执行失效降级；

35、状态与逐部位最小距离上报模块，用于对外上报避障限速状态、各子链最小距离、触发连杆、剪枝层级及求解状态。

36、本系统中各模块按照如下数据流及调用关系工作如下：

37、离线阶段：所述全身关节零位与连杆几何标定模块输出参数残差协方差送入所述标定残差传播与逐连杆膨胀余量解算模块；所述标定残差传播与逐连杆膨胀余量解算模块输出逐连杆膨胀余量表送入所述连杆碰撞体三级层次离线构建模块；所述连杆碰撞体三级层次离线构建模块与所述自碰撞对与常态接触对剔除模块完成离线运算，输出三级碰撞体配置、检测对白名单供在线模块调用；

38、在线阶段，关节编码器数据送入所述关节状态时间对齐与前向运动学解算模块，所述关节状态时间对齐与前向运动学解算模块输出连杆位姿送入所述全身碰撞体坐标变换广播模块；环境感知与位姿估计数据送入所述滑动窗口占据地图构建模块，地图数据依次送入所述概率融合与传感器噪声余量膨胀模块、所述未知体素分部位处置模块；

39、所述全身碰撞体坐标变换广播模块输出的碰撞体位姿信息、所述未知体素分部位处置模块输出的体素处置信息共同送入所述整机包围球与子链包络胶囊分级粗筛模块；粗筛输出风险子链信息送入所述感兴趣区域体素贪心合并与碰撞对象构造模块；

40、所述感兴趣区域体素贪心合并与碰撞对象构造模块输出长方体块并构造为碰撞对象，结合离线得到的所述检测对白名单一同送入所述逐连杆有符号距离与最近点查询模块；距离查询结果输入所述距离结果按运动学子链分流模块；

41、所述距离结果按运动学子链分流模块分别向所述机体速度方向性缩放模块、所述上肢关节避障不等式构造与零空间求解模块分发对应组别的数据；两个模块输出约束共同输入所述全身避障仲裁与失效降级模块；所述全身避障仲裁与失效降级模块将最终执行指令下发至外部足式、上肢执行单元，同时驱动所述状态与逐部位最小距离上报模块对外上报系统状态。

42、本发明所提供的一种足式人形机器人的全身避障方法及系统，通过构建动态更新的连杆碰撞体三级层次结构实现摆脱固定机体包络的限制，通过将标定残差传播转化逐连杆差异化膨胀余量实现远端连杆安全可靠同时保留近端连杆通行能力，通过分层剪枝与体素贪心合并实现嵌入式平台下的实时几何查询，通过运动学子链分流生成双通道约束实现行走与上肢避让同步开展，通过未观测体素分部位处置与速度可执行校验，实现地图不完备场景可靠运行并规避步态无效指令，整体提升足式人形机器人复杂环境下避障的实用性与安全性。得益于连杆碰撞体三级层次结构随机器人关节构型动态更新，并且滑动窗口占据地图仅施加传感器噪声余量膨胀，机体几何全部由机器人侧凸包模型承载。相较于现有技术可带来如下有益效果：

43、第一，消除了固定机体包络在“过保守”与“漏检”之间的两难。机器人一侧的几何由逐连杆凸包如实表达并随构型逐周期更新，机器人在双臂收拢时判定可通行的通道宽度即接近躯干实际宽度，而在双臂前伸时手臂本身也受保护。通道通行能力不再由全构型下的最差包络决定，手臂、抬起的腿与前倾的头部亦不再脱离保护。

44、第二，碰撞判据由一个比特升级为可支撑分化处置的完整几何信息。有符号距离给出“还差多少”，最近点连线方向给出“该朝哪儿退”，连杆标识给出“是谁面临碰撞”。据此，手臂面临碰撞时收臂、躯干面临碰撞时限制指向障碍方向的机体运动这两类处置得以分别实施，机器人不再只能整体停车。

45、第三，逐连杆膨胀余量使远端连杆的判据可信而不牵累近端连杆。膨胀余量由该连杆自身的标定残差经位姿雅可比传播得到，手掌等远端连杆获得较大余量，躯干等近基座连杆获得较小余量。相较于按全身最不确定连杆整体取值的统一安全距离，近端连杆在通道通行能力上的损失被显著减小，同时远端连杆的判据仍覆盖其真实不确定度。

46、第四，精确几何查询的开销与环境空旷程度相适应，可在嵌入式平台按控制周期运行。第零级与第一级剪枝把空旷环境下的每周期开销降至常数级；风险子链筛选把贴墙行走等常见工况下进入精确查询的连杆数限制在单侧肢体范围内；感兴趣区域内的体素贪心合并把碰撞对象数量由数千量级降至数十量级。三者叠加，使连杆网格与占据体素之间的精确距离查询无需图形处理器并行加速。

47、第五，机器人可以“收臂通过”而非“停车等待”。上肢避障约束经位姿雅可比构造在关节速度空间内，并以零空间投影与任务期望速度共存，因而手臂的让位运动不占用机体速度通道、不改变行走指令。机器人穿越货架间隙时表现为一边正常行走、一边把手臂收到躯干侧面，而非在间隙前停下。

48、第六，贴障碍的切向运动得以保留。机体速度只在指向最近点连线方向的法向分量上被缩减，切向分量不缩减，因而机器人在紧邻墙面、货架或桌沿时仍可沿其表面平移或绕行，不会因法向距离小而被整体锁死。

49、第七，避障限速与足式平台的最小可执行速度幅值相容。方向性缩放之后逐通道检查幅值是否落入不可执行区间，落入者明确置零并置位停车标志。系统只存在“明确不动”与“以可执行速度运动”两种状态，不会下发一个幅值虽为正、但步态层无法产生迈步的指令，从而避免机器人在障碍附近出现“已发出指令却不迈步”的停滞与随后的走走停停。

50、第八，建图不完整时不发生全局冻结。未观测体素按其所面对的子链分别取舍：躯干与足端方向上仍按障碍处理以保证安全裕度，上肢在向本体收拢方向上的运动则可以适度乐观。机器人在刚进入新区域、地图大面积未观测时仍可行走与收臂，而不是原地静止等待建图完成。

51、第九，机器人与环境的两类几何表示在同一时刻对齐。关节位置在解算连杆位姿之前先内插或外推到滑动窗口占据地图时间戳，避免手臂摆动过程中机器人几何与环境几何相差一个数据链路延时；该延时对应的末端位置偏差与膨胀余量同量级，若不对齐，则标定得到的余量将被延时抵消。

52、第十，标定可在作业现场以机载部件完成。标定的观测源为机载视觉感知部件、被观测对象为固定于机器人自身连杆上的本体标志物，不需架设位置敏感探测器、结构光装置或外置标定板。机器人在现场因碰撞或维护导致关节零位变化后可就地重新标定，膨胀余量随之更新。