一种管道机器人复合行走自适应控制方法及系统与流程

本发明涉及管道机器人运动控制，尤其涉及一种管道机器人复合行走自适应控制方法及系统。

背景技术：

1、在管道机器人运动控制技术领域内，现有管道机器人通常设置轮式、履带式、腿式或复合驱动机构，并配置惯性测量单元、压力传感器和关节执行器；控制器根据姿态参数、足端接触压力、运动命令及预设运动模式生成足端轨迹或关节控制量，驱动机器人在直管段、弯管段、坡面管段及局部障碍位置进行行走、滚动或越障。现有四足机器人控制方案还常采用周期步态、姿态反馈、足端接触检测和逆运动学求解完成关节角生成。

2、现有方案多按照单个控制周期内的姿态参数、压力阈值或运动命令直接切换运动模式，较少对不同遥测数据的采样时刻和传输时延进行统一约束。当姿态参数、角速度数据和各腿足端接触压力的更新时间不一致，或者遥测数据发生延迟、中断时，控制器可能继续采用上一时刻的数据进行运行状态判定；当测量值在阈值附近波动时，运动模式及其对应的步态参数也容易频繁切换，导致运行状态判定、模式参数调用和步态周期推进之间出现衔接不连续。

3、此外，现有姿态控制和足端接触控制通常分别形成关节修正量或执行器控制量，姿态偏差与各腿接触压力偏差对应的修正结果缺少统一的足端位置表达。当两类修正量分别作用于关节指令时，后续修正可能改变前序修正结果，且组合后的足端目标难以同时接受足端可达工作空间和逆运动学关系的约束。因此，需要围绕管道机器人复合运动控制，解决遥测数据存在时效差异且姿态偏差与各腿接触压力偏差并存时，运行状态、足端目标修正和关节逻辑角生成之间难以连续衔接的问题。

技术实现思路

1、针对以上问题，本发明提供了一种管道机器人复合行走自适应控制方法及系统，用于解决如何在管道机器人遥测数据存在时效差异且姿态偏差与各腿接触压力偏差并存时，通过稳定运行状态门控和逆运动学前的足端补偿合成，形成受足端可达工作空间约束的关节逻辑角的问题。

2、为实现上述目的，本发明采用的技术方案是：

3、一种管道机器人复合行走自适应控制方法，包括：

4、s100、接收姿态参数、角速度数据、各腿足端接触压力、运动命令和遥测时间戳，计算数据年龄；根据姿态扰动量、角速度扰动量、压力不均量和数据年龄生成候选运行状态，按照状态转换方向的第一确认时间或第二确认时间确定稳定运行状态，数据年龄超过数据超时阈值时将稳定运行状态更新为安全状态；

5、s200、基于所述稳定运行状态和运动命令确定目标运动模式，调用包括步态周期、摆动占空比、各腿周期偏移和各腿足端参考位置偏移的模式参数集，生成基础足端轨迹；

6、s300、根据姿态参数与标定姿态的偏差及角速度数据生成姿态补偿量，根据各腿足端接触压力与目标压力的压力误差生成各腿接触补偿量；将两类补偿量转换为足端参考位置修正量，与所述基础足端轨迹叠加，经足端可达工作空间修正和逆运动学求解生成关节逻辑角；

7、s400、将所述关节逻辑角映射为硬件目标角并按关节行程范围限幅，生成舵机控制指令；所述稳定运行状态为安全状态或指令发送失败时，停止步态周期推进并生成安全姿态指令。

8、进一步地，所述姿态参数包括俯仰角和横滚角；

9、所述姿态扰动量根据所述俯仰角和横滚角相对于标定姿态的偏差生成；

10、所述角速度扰动量根据x轴角速度和y轴角速度生成；

11、所述压力不均量根据各腿足端接触压力与对应目标压力的压力误差生成；

12、所述数据年龄为当前时刻与最近有效的遥测时间戳之差。

13、进一步地，除所述安全状态外，所述候选运行状态的等级高于当前稳定运行状态时采用所述第一确认时间，并在进入阈值持续满足所述第一确认时间后更新所述稳定运行状态；

14、所述候选运行状态的等级低于当前稳定运行状态时采用所述第二确认时间，并在退出阈值持续满足所述第二确认时间后更新所述稳定运行状态；

15、所述第二确认时间长于所述第一确认时间，所述候选运行状态发生改变时重置对应的确认时间。

16、进一步地，所述目标运动模式选自安全站立模式、常规行走模式、高通过行走模式、纵向滚动模式和横向滚动模式；

17、所述高通过行走模式对应姿态补偿启用状态，所述横向滚动模式对应接触补偿启用状态；

18、当前运动模式与所述目标运动模式不同时生成模式切换状态，调用所述当前运动模式与目标运动模式之间的中间姿态，并在所述中间姿态完成后更新当前运动模式。

19、进一步地，生成所述基础足端轨迹包括：

20、将步态周期内的时间与对应腿的周期偏移相加并按所述步态周期归一化，生成各腿局部周期；

21、将所述各腿局部周期与所述摆动占空比比较，确定对应腿处于摆动相或支撑相；

22、在所述摆动相生成抬足轨迹和前移轨迹，在所述支撑相生成回退轨迹；

23、根据各腿摆幅增益修正对应轨迹，并叠加各腿足端参考位置偏移。

24、进一步地，生成所述姿态补偿量包括：

25、计算俯仰角和横滚角相对于所述标定姿态的姿态偏差；

26、根据所述姿态偏差和角速度数据进行比例微分计算并按最大补偿量限幅；

27、将俯仰角对应的姿态补偿量分别转换为前侧腿部和后侧腿部的足端参考位置修正量，将横滚角对应的姿态补偿量分别转换为左侧腿部和右侧腿部的足端参考位置修正量。

28、进一步地，生成各腿接触补偿量包括：

29、计算各腿足端接触压力与对应目标压力的压力误差；

30、所述压力误差位于压力死区内时保持对应的接触补偿量；

31、所述压力误差位于所述压力死区外时，根据所述压力误差和采样时间间隔对对应的接触补偿量进行离散累加；

32、对所述接触补偿量按最大补偿量限幅，并转换为对应腿沿管壁法向的足端参考位置修正量。

33、进一步地，所述足端可达工作空间根据腿部连杆长度和关节行程范围确定；

34、两类补偿量与所述基础足端轨迹叠加后的足端目标超出所述足端可达工作空间时，将所述足端目标修正至所述足端可达工作空间的边界；

35、对修正后的足端目标进行逆运动学求解，并按照执行器通道映射关系、安装方向、零位和比例关系将所述关节逻辑角转换为硬件目标角；

36、根据关节行程范围和单控制周期最大角度变化量对所述硬件目标角进行限幅。

37、进一步地，在所述安全状态下，保持所述安全姿态指令，重置所述候选运行状态的确认时间，停止更新所述姿态补偿量和各腿接触补偿量；

38、连续接收的姿态参数、角速度数据和足端接触压力对应的数据年龄未超过所述数据超时阈值时，按照所述第二确认时间将所述稳定运行状态由所述安全状态恢复。

39、进一步地，一种管道机器人复合行走自适应控制系统，包括：运行状态确定模块、运动模式与足端轨迹生成模块、足端补偿与逆运动学求解模块、执行器映射与安全控制模块；所述系统用于实现上述任一项所述的方法。

40、本发明的关键创新点包括：

41、（1）将姿态扰动量、角速度扰动量、压力不均量与数据年龄共同用于生成候选运行状态，并根据状态转换方向分别调用第一确认时间或第二确认时间形成稳定运行状态；数据年龄超过数据超时阈值时，将稳定运行状态更新为安全状态。

42、（2）以稳定运行状态和运动命令作为目标运动模式的共同匹配条件，调用包括步态周期、摆动占空比、各腿周期偏移和各腿足端参考位置偏移的模式参数集；根据各腿局部周期划分摆动相和支撑相，生成与各腿参数对应的基础足端轨迹。

43、（3）根据姿态参数与标定姿态的偏差及角速度数据生成姿态补偿量，根据各腿足端接触压力与目标压力的压力误差生成各腿接触补偿量；将两类补偿量转换为足端参考位置修正量并与基础足端轨迹叠加，对叠加后的足端目标依次进行足端可达工作空间修正、逆运动学求解、硬件目标角映射和关节行程限幅。

44、以下为其主要的有益效果：

45、（1）针对不同遥测数据更新时间不一致以及运行特征在阈值附近波动时状态切换衔接不连续的问题，通过数据年龄校验、候选运行状态持续确认和非对称确认时间，使超过数据超时阈值的遥测数据不再维持普通运行状态，并使目标运动模式采用经持续确认的稳定运行状态作为调用依据。

46、（2）针对运动模式切换与各腿步态参数、足端参考位置参数更新不同步的问题，通过稳定运行状态与模式参数集的关联调用，使目标运动模式、各腿局部周期和各腿足端参考位置偏移在同一控制过程中更新，形成与当前运行状态和运动命令对应的基础足端轨迹。

47、（3）针对姿态补偿和接触补偿分别作用于关节控制量时修正结果难以统一约束的问题，通过将两类补偿量转换至足端参考位置并在逆运动学前叠加，使姿态偏差和各腿接触压力偏差共同作用于同一足端目标；叠加后的足端目标统一接受足端可达工作空间、逆运动学关系和关节行程范围的约束，形成与修正足端目标对应的关节逻辑角和硬件目标角。