一种双足机器人跨模态可信度约束的速度协调方法及系统与流程

本发明涉及运动控制，尤其涉及一种双足机器人跨模态可信度约束的速度协调方法及系统。

背景技术：

1、双足机器人因其类人的移动方式和复杂地形的适应能力，近年来成为机器人学研究的热点。

2、传统移动机器人在感知信号丢失时通常采用速度归零的硬刹车策略，或直接将置信度与期望速度相乘作为指令输出。此类方法对于四轮质心尚可接受，但对于具有高自由度和欠驱动特性的双足机器人，当综合可信度瞬间跳变时，速度指令的阶跃变化会产生无穷大的加速度，导致质心倾覆力矩瞬间越限，直接引发发散运动分量失稳乃至摔倒。高速运动或强风等极限场景下，空气阻力对双足机器人的姿态稳定与电机出力具有显著影响，但现有方法通常将此类气动力视为不可建模的随机扰动予以忽略，未充分利用其作为环境辅助资源的潜在价值，致使电机在不必要的高负荷状态下运行，能耗高且易过热保护；现有控制架构缺乏主动截断和熔断保护能力，异常信号沿链路逐级放大传导至底层执行器，整体生存能力不足。

技术实现思路

1、本发明解决的技术问题是：现有技术难以解决感知突变时的速度指令平滑连续问题，异常信号沿链路逐级放大传导至底层执行器，缺乏主动截断和熔断保护能力。

2、为解决上述技术问题，本发明提供如下技术方案：

3、第一方面，一种双足机器人跨模态可信度约束的速度协调方法，包括如下步骤：

4、步骤s1，通过环境感知数据与目标运动数据计算得到综合可信度和时间变化率，对所述时间变化率进行分段增益处理，得到原始导纳参数；

5、步骤s2，基于虚拟能量池机制对所述原始导纳参数进行校验，得到虚拟质量和虚拟阻尼系数，构建影子质心模型，对所述目标运动数据进行滤波，得到第一速度参数；

6、步骤s3，获取双足机器人的当前位姿与电机参数，通过所述综合可信度进行收缩，得到安全包络，并对所述第一速度参数进行校验，得到第二速度参数与连续边界条件；

7、步骤s4，通过约束耦合与逆动力学优化，对所述安全包络、连续边界条件和第二速度参数进行计算，得到足端接触力和关节力矩指令。

8、优选地，所述步骤s1包括如下子步骤：

9、步骤s11，获取双足机器人的环境感知数据和目标运动数据，计算视觉观测位置和雷达观测位置的马氏距离，得到空间一致性置信度，利用扩展卡尔曼滤波器对目标运动数据进行递推，得到预测速度，并与当前实测的目标速度进行比对，得到时间连贯性置信度；

10、步骤s12，将所述空间一致性置信度与时间连贯性置信度通过联合概率模型进行融合，得到归一化后的综合可信度，对所述归一化后的综合可信度进行时间差分，提取平滑后的可信度变化率；

11、步骤s13，根据所述可信度变化率，构建分段映射函数，将所述可信度变化率映射为原始导纳参数，所述原始导纳参数包括原始虚拟质量和原始虚拟阻尼系数。

12、优选地，所述分段映射函数包括第一增益函数和第二增益函数，处理逻辑为：

13、当所述可信度变化率0时，触发第一增益计算，通过第一增益函数对所述可信度变化率进行处理，得到对应的原始虚拟质量和原始虚拟阻尼系数；

14、当所述可信度变化率0时，触发第二增益计算，通过第二增益函数对所述可信度变化率进行帆布里计算，得到对应的原始虚拟质量和原始虚拟阻尼系数。

15、优选地，所述步骤s2包括如下子步骤：

16、步骤s21，构建虚拟能量池机制，根据双足机器人的当前速度、跟随偏差和原始虚拟质量，计算瞬态虚假功率，对所述瞬态虚假功率进行离散积分处理，得到虚拟能量池的剩余能量状态；

17、步骤s22，将所述剩余能量状态与预设的安全能量阈值进行对比：

18、当所述剩余能量状态大于安全能量阈值时，判定能量充裕，将所述原始导纳参数作为目标导纳参数，目标导纳参数包括目标虚拟质量和目标虚拟阻尼系数；

19、当所述剩余能量状态小于等于安全能量阈值时，判定无源性被破坏，触发截断机制，将所述原始虚拟质量修正为安全质量常数作为目标虚拟质量，并对所述原始虚拟阻尼系数进行超限制动增益，并作为目标虚拟阻尼系数；

20、步骤s23，利用目标导纳参数构建影子质心模型，并对所述目标运动数据进行微分计算，得到质心期望速度、质心期望加速度和质心期望加加速度，并作为第一速度参数。

21、优选地，所述步骤s3包括如下子步骤：

22、步骤s31，获取双足机器人的当前位姿、电机参数和相对气流速度，根据所述相对气流速度计算当前时刻的气动阻力估计值，将所述气动阻力估计值代入电机功率边界模型进行修正，得到修正功率边界，同步融合几何步长边界，得到初始运动边界；

23、步骤s32，将所述综合可信度作为收缩比例因子，对所述初始运动边界进行处理，生成安全包络；

24、步骤s33，获取连续相位变量的接触状态切换临界点和高动态减速区间，计算第一速度参数的惯性冲击力，将所述惯性冲击力和气动阻力估计值进行叠加运算，得到净惯性残差力，对所述净惯性残差力进行处理，输出第二速度参数和连续边界条件。

25、优选地，对所述净惯性残差力进行处理，输出第二速度参数和连续边界条件的处理逻辑为：

26、提取双足机器人当前加速度的单位方向向量，将所述气动阻力估计值与单位方向向量进行点积求值运算，得到气动阻力在加速度方向上的投影标量；

27、将所述第一速度参数的惯性冲击力标量值减去投影标量，得到净惯性残差力；

28、将所述净惯性残差力与预设的力值阈值进行比对：

29、当所述净惯性残差力小于或等于所述力值阈值时，判定当前惯性冲击在安全范围以内，输出所述第一速度参数作为第二速度参数；

30、当所述净惯性残差力大于所述力值阈值时，判定当前惯性冲击超出安全范围，执行伸展处理，所述伸展处理包括：

31、计算所述净惯性残差力与力值阈值的差值，并将差值除以力值阈值，得到超限比例系数；获取双足机器人的基准步态执行周期，计算安全步态执行周期所述安全步态执行周期=基准步态执行周期超限比例系数，通过安全步态执行周期对所述连续相位变量进行修正，得到当前动作执行速率；

32、计算所述净惯性残差力对应的初始加加速度数值，将所述初始加加速度数值与预设的加加速度上限值进行比对，若超出，则将所述初始加加速度数值强制截断修正为所述加加速度上限值；

33、基于当前动作执行速率进行轨迹重构，输出第二速度参数和连续边界条件，所述连续边界条件用于限制接触状态切换临界点足端速度为零。

34、优选地，所述步骤s4包括如下子步骤：

35、步骤s41，根据所述第二速度参数、安全包络和连续边界条件构建约束优化模型；

36、步骤s42，获取双足机器人的关节空间质量矩阵与偏置力矩阵，将所述关节空间质量矩阵、偏置力矩阵代入约束优化模型中，逆运算得到足端接触力与关节加速度；

37、步骤s43，根据所述足端接触力与关节加速度，结合双足机器人的执行器选择矩阵与接触雅可比矩阵，计算得到关节力矩指令，将所述足端接触力与所述关节力矩指令下发至双足机器人的关节驱动器执行。

38、优选地，所述步骤s42还包括如下子步骤：

39、步骤s421，获取双足机器人当前的实际关节数据，所述实际关节数据包括关节位置和关节速度，基于多体刚体动力学算法计算得到关节空间质量矩阵，计算偏置力矩阵，解算与双足机器人足端接触点相对应的接触雅可比矩阵和对应的时间导数矩阵；

40、步骤s422，利用所述关节空间质量矩阵、偏置力矩阵和接触雅可比矩阵，构建双足机器人的动力学方程，利用接触雅可比矩阵和时间导数矩阵，构建从关节加速度空间向足端笛卡尔空间转换的运动映射方程，将所述动力学方程与所述运动映射方程作为约束条件，输入所述约束优化模型中；

41、步骤s423，将足端接触力与关节加速度设定为优化决策变量，启动二次规划求解器，对所述约束优化模型进行收敛计算，输出最优解向量，从最优解向量分解得到足端接触力与关节加速度。

42、优选地，将所述足端接触力与所述关节力矩指令下发至双足机器人的关节驱动器执行的处理过程为：

43、在底层伺服控制器内部构建力位复合控制回路，所述力位复合控制回路包括前馈通道和补偿反馈通道，将所述关节力矩指令输入所述前馈通道，利用双足机器人的接触雅可比矩阵转置，将所述足端接触力映射为关节阻抗补偿力矩，并输入所述补偿反馈通道；

44、获取双足机器人各关节编码器实时反馈的实际关节角度与实际关节速度，基于目标关节角度与目标关节速度计算运动偏差，利用pd调节器对所述运动偏差进行计算，得到闭环镇定力矩；将所述前馈力矩、关节阻抗补偿力矩和闭环镇定力矩进行求和，生成综合力矩控制量；

45、将所述综合力矩控制量按照底层伺服控制周期进行离散化打包，得到力矩控制数据包，通过实时工业通信总线，将力矩控制数据包同步下发至双足机器人各个物理关节的末端伺服驱动器，通过所述末端伺服驱动器内部的电流环进行解析与电流放大，驱动对应关节的电机执行器输出所述综合力矩控制量，完成双足机器人运动姿态与接触力分配。

46、第二方面，一种双足机器人跨模态可信度约束的速度协调系统，包括映射模块、能量校验模块、安全包络模块和控制约束模块；

47、所述映射模块，通过环境感知数据与目标运动数据计算得到综合可信度和时间变化率，对所述时间变化率进行分段增益处理，得到原始导纳参数；

48、所述能量校验模块，基于虚拟能量池机制对所述原始导纳参数进行校验，得到虚拟质量和虚拟阻尼系数，构建影子质心模型，对所述目标运动数据进行滤波，得到第一速度参数；

49、所述安全包络模块，获取双足机器人的当前位姿与电机参数，通过所述综合可信度进行收缩，得到安全包络，并对所述第一速度参数进行校验，得到第二速度参数与连续边界条件；

50、所述控制约束模块，通过约束耦合与逆动力学优化，对所述安全包络、连续边界条件和第二速度参数进行计算，得到足端接触力和关节力矩指令；

51、本发明的有益效果：本发明通过分段增益函数将可信度变化率映射为虚拟质量与阻尼系数，利用物理惯性固有的低通滤波特性平滑吸收指令突变，实现了质心速度、加速度与加加速度的全程c2连续过渡。分段函数赋予系统环境恶化时虚拟质量毫秒级激增对抗突发扰动，环境恢复时线性缓释避免前冲失稳，当传感器硬件故障引发导纳参数高频震荡时自动熔断保护，在信息域与物理域之间建立了能量隔离带。气动阻力模型的引入将环境风力从随机干扰转变为可控辅助资源，逆风时降低电机负荷，顺风时提前预判制动困难。可信度驱动的安全包络动态缩放，为控制系统主动预留紧急制动余量，实现了感知质量与运动极限的实时耦合。时域伸展处理以时间换空间策略将加速度峰值摊薄至安全红线内，配合加加速度硬截断与足端零速落地约束。