一种底栖潜水器爬游多任务控制方法及系统与流程
本发明涉及深海机器人运动控制,尤其涉及一种底栖潜水器爬游多任务控制方法及系统。
背景技术:
1、底栖潜水器是一类兼具水下游动与海底爬行能力的深海作业装备,通过融合矢量推力器与多关节机械腿两类异构驱动装置,能够在悬浮悬停、坐底驻留、斜坡爬行等多种模态下完成取样、钻孔、观测等精细作业任务,是深海矿产资源勘探和科学考察的核心装备之一;然而,推力器与机械腿在物理特性上存在本质差异,推力器提供空间旋量,机械腿提供接触反力,两者分别对应刚体动力学和接触动力学两类完全不同的数学描述,这种异构性使得游爬一体化作业中的协同控制面临严峻挑战。
2、现有深海装备控制方法通常将推力分配与机械腿力控解耦为独立模块处理,缺乏将两类异构执行器纳入统一控制框架的有效手段。当潜水器在复杂海况下同时执行位姿锁定与足端防滑等多约束任务时,多任务之间存在冲突,位姿跟踪要求机身调整姿态,防滑约束限制足端力方向,推力分配追求能耗最优,传统加权型单层二次规划方法依赖经验权重协调任务优先级,在极端海况下极易因任务间相互妥协干扰而触发防滑边界,导致机身失稳甚至设备损坏,降低了潜水器游爬一体化作业的稳定性与安全性。
技术实现思路
1、有鉴于此,本发明提出了一种底栖潜水器爬游多任务控制方法及系统,通过构建全域广义雅可比映射将推力器与机械腿统一至同一控制框架,并利用分层二次规划建立任务优先级机制,使多冲突任务在互不干涉的前提下协同完成,提高了底栖潜水器游爬一体化作业的稳定性与安全性。
2、本发明的技术方案是这样实现的:第一方面,本发明提供了一种底栖潜水器爬游多任务控制方法,包括以下步骤:
3、s1,引入附加质量矩阵与非线性水动力阻尼旋量,建立潜水器的水下动力学方程,构建全域广义雅可比映射矩阵,将推力器推力输出与机械腿足端接触反力统一映射为水下动力学方程的动力学控制总旋量;
4、s2,获取状态观测器反馈的潜水器实时状态、导航行为层下发的期望位姿与加速度指令以及硬件物理极限参数,根据控制优先级将任务划分为绝对刚性约束集、作业跟踪主任务集和冗余零空间寻优集,将非线性摩擦锥约束线性化为线性不等式组,生成各层级对应的特征参数矩阵组;
5、s3,根据各层级特征参数矩阵组,按照优先级从高到低的顺序,以零空间投影方式逐级进行二次规划求解,每一级求解在当前级上一级残留的零空间内进行,将求解结果投影并累加至累积最优解,每级完成后更新零空间基矩阵,得到系统广义控制输入最优解;
6、s4,构建受机身姿态调制的流体外动压模型,生成期望微调扭矩,将期望微调扭矩映射至前级任务残留的零空间中推力器子空间内,生成推力器系统的零空间控制补偿增量,以调节机身姿态。
7、在以上技术方案的基础上,优选的,步骤s1包括以下子步骤:
8、s11,建立广义控制输入向量,所述广义控制输入向量包括推力器的期望推力输出向量和机械腿足端的三维接触反力向量;
9、s12,建立潜水器的水下动力学方程,所述水下动力学方程为刚体质量惯性矩阵与附加质量矩阵之和乘以加速度向量,加上科里奥利与向心力矩阵乘以速度向量,加上非线性水动力阻尼旋量,加上恢复力与恢复力矩旋量,所得结果等于作用于质心的动力学控制总旋量;
10、s13,构建全域广义雅可比映射矩阵,所述全域广义雅可比映射矩阵包括推力器静态构型映射矩阵和机械腿平滑接触状态映射矩阵;
11、s14,将广义控制输入向量与全域广义雅可比映射矩阵相乘,得到动力学控制总旋量,使推力器推力输出与机械腿足端接触反力统一映射为向质心的动力学控制总旋量。
12、在以上技术方案的基础上,优选的,步骤s13所述构建全域广义雅可比映射矩阵包括以下步骤:
13、s131,获取各推力器的推力方向空间单位向量和安装位置矢量,以各推力器的推力方向空间单位向量构成推力映射的上半部分,以各推力器安装位置的三维位置矢量与推力方向空间单位向量的叉积构成力矩阵映射的下半部分,组合得到推力器静态构型映射矩阵;
14、s132,获取当前处于接触状态的机械腿数量及各机械腿足端的位置矢量,以各机械腿足端接触力向量乘以对应的接触概率因子构成推力映射的上半部分,以各机械腿足端位置矢量与接触力向量的叉积乘以对应的接触概率因子构成力矩映射的下半部分,组合得到机械腿平滑接触状态映射矩阵;
15、s133,将推力器静态构型映射矩阵与机械腿平滑接触状态映射矩阵沿水平方向拼接,得到全域广义雅可比映射矩阵。
16、在以上技术方案的基础上,优选的,所述接触概率因子的确定方式为:获取足端六轴力传感器测量的法向接触力,以法向接触力减去预设力学死区阈值后乘以预设标度因子,取乘积的相反数作为自然常数e的指数,计算指数函数的倒数加1,再取所得和的倒数,作为当前机械腿足端的接触概率因子。
17、在以上技术方案的基础上,优选的,步骤s2包括以下子步骤:
18、s21,获取状态观测器反馈的潜水器实时位姿和速度、导航行为层下发的期望位姿与加速度指令、足端力传感器测量的接触状态以及硬件物理极限参数;
19、s22,将任务划分为三个优先级集合,第一优先级为绝对刚性约束集,包括推力器推力输出的上下限约束和机械腿足端法向力的最小值约束;第二优先级为作业跟踪主任务集,将期望位姿与加速度跟踪偏差映射为基于全域广义雅可比矩阵的等式约束;第三优先级为冗余零空间寻优集,优化目标为广义控制输入向量的二次型能耗代价函数与动密封压差调控代价之和,能耗代价用于优化推力分配效率,动密封压差调控代价用于主动调节机身俯仰角以降低密封压差;
20、s23,将非线性摩擦锥约束内接降维为正多边形棱锥,将正多边形棱锥解耦为多组线性绝对值不等式,建立线性化后的摩擦锥约束;
21、s24,将第一优先级的推力极值约束、足端法向力约束和线性化后的摩擦锥约束作为不等式安全约束,将第二优先级的位姿跟踪等式作为等式任务目标,将第三优先级的二次型能耗代价函数作为优化目标,分别生成各层级对应的特征参数矩阵组,所述特征参数矩阵组包括等式任务目标特征矩阵与目标期望向量、不等式安全约束特征矩阵与安全边界向量。
22、在以上技术方案的基础上,优选的,步骤s3包括以下子步骤:
23、s31,设定零向量为起始基准解,单位矩阵为起始零空间基矩阵,并获取各层级特征参数矩阵组;
24、s32,对第一优先级任务,在起始零空间基矩阵空间内进行二次规划求解,所述二次规划的目标函数为第一级等式任务目标特征矩阵乘以当前累积最优解与零空间基矩阵乘决策变量之和再减去目标期望向量的范数平方的二分之一,加上决策变量的正则化阻尼项的二分之一;
25、所述二次规划的约束条件为第一级不等式安全约束特征矩阵乘以当前累积最优解与零空间基矩阵乘决策变量之和小于或等于安全边界向量,求解得到第一级最优控制步长,将第一级最优控制步长投影至起始零空间基矩阵并累加至起始基准解,得到第一级累积最优解;
26、s33,根据第一级等式任务特征矩阵与起始零空间基矩阵的乘积,计算乘积的广义伪逆,将起始零空间基矩阵乘以单位矩阵减去广义伪逆再乘以乘积的差,得到第二级可用零空间基矩阵;
27、s34,对第二优先级任务,以第一级累积最优解为当前累积最优解,以第二级可用零空间基矩阵为当前零空间基矩阵,进行第二级二次规划求解,得到第二级最优控制步长,将第二级最优控制步长投影至第二级可用零空间基矩阵并累加至第一级累积最优解,得到第二级累积最优解,根据第二级等式任务特征矩阵与第二级可用零空间基矩阵的乘积,根据步骤s33方式更新零空间基矩阵,得到第三级可用零空间基矩阵;
28、s35,对第三优先级任务,以第二级累积最优解为当前累积最优解,以第三级可用零空间基矩阵为当前零空间基矩阵,进行第三级二次规划求解,得到第三级最优控制步长,将第三级最优控制步长投影至第三级可用零空间基矩阵并累加至第二级累积最优解,得到系统广义控制输入最优解。
29、在以上技术方案的基础上,优选的,步骤s4包括以下子步骤:
30、s41,获取深海绝对静水压、深海海水密度、机身当前俯仰角、潜水器当前速度矢量幅值和舱内容积补偿器内部实时压力,建立受机身俯仰角调制的局部流体外动压模型,以深海绝对静水压加上二分之一的深海海水密度乘以流体动压分布系数再乘以潜水器当前潜浮绝对速度矢量幅值的平方,计算得到机身外部动密封局部区域的实时流体动静压总和;
31、s42,计算实时流体动静压总和与舱内实时压力及最优安全工作内外压差基准值之差的平方,求取平方对俯仰角的偏导数,以偏导数乘以预设比例反馈增益的相反数作为期望俯仰微调差动扭矩;
32、s43,获取第二级任务更新后的零空间基矩阵中对应推力器的子块矩阵,获取推力器静态构型映射矩阵中对应机身俯仰自由度的行向量,以行向量乘以子块矩阵的广义伪逆再乘以期望俯仰微调差动扭矩,得到推力器系统的零空间控制补偿增量;
33、s44,将零空间控制补偿增量叠加至推力器系统的原始控制指令上,使推力器在维持原轨迹跟踪的基础上产生差动推力,以调节机身俯仰角。
34、第二方面,本发明还提供了一种底栖潜水器爬游多任务控制系统,采用底栖潜水器爬游多任务控制方法所实现,包括:
35、全域映射模块,用于引入附加质量矩阵与非线性水动力阻尼旋量,建立潜水器的水下动力学方程,构建全域广义雅可比映射矩阵,将推力器推力输出与机械腿足端接触反力统一映射为水下动力学方程的动力学控制总旋量;
36、多任务分层模块,用于获取状态观测器反馈的潜水器实时状态、导航行为层下发的期望位姿与加速度指令以及硬件物理极限参数,根据控制优先级将任务划分为绝对刚性约束集、作业跟踪主任务集和冗余零空间寻优集,将非线性摩擦锥约束线性化为线性不等式组,生成各层级对应的特征参数矩阵组;
37、规划求解模块,用于根据各层级特征参数矩阵组,按照优先级从高到低的顺序,以零空间投影方式逐级进行二次规划求解,每一级求解在当前级上一级残留的零空间内进行,将求解结果投影并累加至累积最优解,每级完成后更新零空间基矩阵,得到系统广义控制输入最优解;
38、空间调控模块,用于构建受机身姿态调制的流体外动压模型,生成期望微调扭矩,将期望微调扭矩映射至前级任务残留的零空间中推力器子空间内,生成推力器系统的零空间控制补偿增量,以调节机身姿态。
39、第三方面,本发明还提供了一种终端设备,终端设备包括:存储器、处理器以及存储在存储器上并可在处理器上运行的底栖潜水器爬游多任务控制方法程序,底栖潜水器爬游多任务控制方法程序配置为实现底栖潜水器爬游多任务控制方法的步骤。
40、第四方面,本发明还提供了一种计算机可读存储介质,所述存储介质上存储有一种底栖潜水器爬游多任务控制方法程序,所述一种底栖潜水器爬游多任务控制方法程序被执行时实现底栖潜水器爬游多任务控制方法。
41、本发明的底栖潜水器爬游多任务控制方法及系统相对于现有技术具有以下有益效果:
42、(1)通过构建全域广义雅可比映射将推力器与机械腿两类异构执行器统一至质心动力学控制总旋量,利用分层二次规划建立防滑硬约束、位姿跟踪和能耗优化三个严格非干涉的优先级集合,并采用零空间投影逐级求解,使防滑约束、位姿跟踪和能耗优化三类冲突任务在严格优先级框架下协同完成,消除了任务间相互妥协干扰,避免了极端海况下触发防滑边界失稳的风险,提升了底栖潜水器游爬一体化作业的稳定性与安全性;
43、(2)通过建立包含推力器推力输出与机械腿足端接触反力的广义控制输入向量,结合推力器静态构型映射矩阵与机械腿平滑接触状态映射矩阵拼接为全域广义雅可比映射矩阵,使推力器与机械腿两类异构执行器在质心动力学空间统一表达,有效消除了两类执行器因物理特性差异导致的协同控制壁垒;同时引入水下附加质量矩阵与非线性水动力阻尼旋量修正深海流体效应,使控制模型精确匹配深海水动力环境;并采用sigmoid接触概率函数实现游爬模态切换时雅可比矩阵的平滑演变,有效避免了模态硬切换对控制系统激发的冲击震荡,使全身控制算法在深海极端流体环境下始终具备良好的收敛特性与控制稳定性;
44、(3)通过将任务按物理硬约束、作业跟踪和冗余优化划分为三个严格非干涉的优先级集合,结合零空间投影技术使高优先级任务结果被绝对冻结、低优先级任务在残余零空间内寻优,并采用零空间级联逐级求解与广义伪逆投影基更新机制,彻底避免了经验权重调参在极端工况下导致的任务妥协干扰;同时将二阶锥非线性摩擦约束内接降维为线性绝对值不等式组以适配高频实时求解,并在某层无可行解时直接丢弃该层目标并保持前级结果,实现了柔性降维与平滑降级,保证了极端工况下的基本安全控制能力与高频实时求解需求。
版权声明:本站专利文献数据主要来源于国家知识产权局等公开渠道,由 技术新讯 进行整理汇编。页面设计及数据汇编的著作权归本站所有。全部内容仅供个人学习、科研评估与技术交流参考,严禁用于任何商业用途(包括但不限于直接生产制造、技术转让、倒卖数据等)。
免责条款:专利技术具有时效性,本站不对资料的绝对准确性、完整性和可实施性提供任何明示或暗示的担保。因参考本站内容造成的直接或间接损失,本站不承担相关法律责任。若您认为本站内容有侵权问题,请及时提供权属证明与我们联系,本站将在核实后依法处理。反馈意见:yyfuon@163.com