CN116700015B - 水下航行器主动增稳控制方法及装置 - Google Patents
水下航行器主动增稳控制方法及装置 Download PDFInfo
- Publication number
- CN116700015B CN116700015B CN202310938219.5A CN202310938219A CN116700015B CN 116700015 B CN116700015 B CN 116700015B CN 202310938219 A CN202310938219 A CN 202310938219A CN 116700015 B CN116700015 B CN 116700015B
- Authority
- CN
- China
- Prior art keywords
- underwater vehicle
- control
- moment
- variable
- prediction
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 60
- 230000003416 augmentation Effects 0.000 title claims abstract description 55
- 238000005457 optimization Methods 0.000 claims abstract description 103
- 238000012545 processing Methods 0.000 claims abstract description 29
- 230000006870 function Effects 0.000 claims description 49
- 238000004590 computer program Methods 0.000 claims description 13
- 230000001133 acceleration Effects 0.000 claims description 8
- 238000004422 calculation algorithm Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 5
- 230000000087 stabilizing effect Effects 0.000 claims 1
- 239000011664 nicotinic acid Substances 0.000 abstract description 18
- 241000251730 Chondrichthyes Species 0.000 abstract description 13
- 241000251468 Actinopterygii Species 0.000 abstract description 10
- 230000000007 visual effect Effects 0.000 abstract description 9
- 230000008569 process Effects 0.000 abstract description 7
- 238000013519 translation Methods 0.000 abstract description 6
- 230000009182 swimming Effects 0.000 abstract description 4
- 230000001276 controlling effect Effects 0.000 description 6
- 238000004891 communication Methods 0.000 description 5
- 241001471473 Sphyrna zygaena Species 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- 230000004886 head movement Effects 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 230000006641 stabilisation Effects 0.000 description 2
- 238000011105 stabilization Methods 0.000 description 2
- 230000003592 biomimetic effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 238000012804 iterative process Methods 0.000 description 1
- 101150036841 minJ gene Proteins 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 210000000006 pectoral fin Anatomy 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 230000001105 regulatory effect Effects 0.000 description 1
- 230000001020 rhythmical effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B13/00—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion
- G05B13/02—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric
- G05B13/04—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators
- G05B13/042—Adaptive control systems, i.e. systems automatically adjusting themselves to have a performance which is optimum according to some preassigned criterion electric involving the use of models or simulators in which a parameter or coefficient is automatically adjusted to optimise the performance
-
- 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)
- Health & Medical Sciences (AREA)
- Artificial Intelligence (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Evolutionary Computation (AREA)
- Medical Informatics (AREA)
- Software Systems (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Feedback Control In General (AREA)
Abstract
本发明属于机器人控制领域技术领域，提供了一种水下航行器主动增稳控制方法及装置，该水下航行器主动增稳控制方法包括：获取水下航行器的第一时刻的状态变量；将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量；基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，并基于优化解得到水下航行器在第二时刻的控制变量序列。本发明所述方法能够降低水下航行器在仿生摆尾游动过程中的头翼的旋转误差和躯干的平移误差，大幅提升了机器双髻鲨的视觉稳定性，进而提高了图像采集的质量和仿生机器鱼运动时的稳定性。
Description
技术领域
本发明涉及机器人控制领域技术领域，尤其涉及一种水下航行器主动增稳控制方法及装置。
背景技术
仿生水下航行器以生物双髻鲨为仿生对象，模仿其独特的生理结构和运动模态，实现了高机动游动和宽视野感知的有效融合。
相关技术中，仿生机器鱼无论采用身体/尾鳍运动模态（Body and/or CaudalFin，BCF）还是中间/胸鳍运动模态(Median and/or Pectoral Fin, MPF)，均会造成机体在水平或者竖直方向的节律性波动，从而导致传感数据不稳定，这会严重影响图像采集质量，造成检测和定位算法的失败，采用数字增稳技术对仿生机器鱼进行增稳时会丢失采集图像的部分前景信息，且会引入额外的计算复杂度，从而占用边缘计算资源，而采用机械增稳技术则会导致仿生机器鱼稳定性降低。
发明内容
本发明提供一种水下航行器主动增稳控制方法及装置，用以解决现有技术中采用数据增稳对仿生机器鱼进行增稳时会丢失图像信息且计算复杂，采用机械增稳会降低仿生机器鱼稳定性的缺陷，提高了图像采集的质量以及仿生机器鱼运动时的稳定性。
本发明提供一种水下航行器主动增稳控制方法，包括：
获取水下航行器的第一时刻的状态变量，所述状态变量包括所述水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；
将所述状态变量输入至预测模型进行预测处理，得到所述水下航行器的在第二时刻的预测变量，所述预测模型基于所述水下航行器的动力学模型确定，所述第二时刻晚于所述第一时刻；
基于多目标优化函数对所述预测变量进行非线性优化处理，得到优化解，并基于所述优化解得到所述水下航行器在第二时刻的控制变量序列，所述控制变量序列用于控制所述水下航行器在目标控制步数内的头翼运动姿态和机体增稳，所述多目标优化函数基于所述水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
根据本发明提供的一种水下航行器主动增稳控制方法，所述获取水下航行器的第一时刻的状态变量，包括：
获取所述水下航行器的导航指令，所述导航指令包括所述水下航行器的偏航指令和俯仰指令；
基于中枢模式发生器模型对所述导航指令进行转换，得到所述偏航角变化量，并基于目标传感器获取所述侧移速度变化量、机体质心的侧移速度变化量。
根据本发明提供的一种水下航行器主动增稳控制方法，所述多目标优化函数应用如下公式得到：
其中，为预测时刻，/>为所述水下航行器的机体质心偏航角，/>为所述水下航行器的头翼关节的偏航角，/>为指定的目标偏航角，/>为机体质心在载体坐标系下的横移速度，/>为上一时刻头翼旋转角度与当前时刻的头翼旋转角度的差值，/>为预测模型的预测步长总数；Q和R均为正定矩阵，用于调节不同误差的权重关系，且满足/>和/>。
根据本发明提供的一种水下航行器主动增稳控制方法，所述基于多目标优化函数对所述预测变量进行非线性优化处理，得到优化解，包括：
基于所述多目标优化函数构造待优化问题；
基于序列二次规划算法和所述预测变量对所述优化问题进行求解，得到所述优化解。
根据本发明提供的一种水下航行器主动增稳控制方法，所述待优化问题通过应用如下公式获取：
，
其中，为预测时刻，/>为所述多目标优化函数计算相关的状态变量。
根据本发明提供的一种水下航行器主动增稳控制方法，所述目标控制步数包括多个控制步数，所述控制变量序列包括所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角；
在所述基于所述优化解得到所述水下航行器在第二时刻的控制变量序列之后，所述方法还包括：
基于所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角，得到所述水下航行器在所述多个控制步数分别对应的关节旋转角速度和关节旋转角加速度。
本发明还提供一种水下航行器主动增稳控制装置，包括：
获取模块，用于获取水下航行器的第一时刻的状态变量，所述状态变量包括所述水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；
预测模块，用于将所述状态变量输入至预测模型进行预测处理，得到所述水下航行器的在第二时刻的预测变量，所述预测模型基于中枢模式发生器和所述水下航行器的动力学模型确定，所述第二时刻晚于所述第一时刻；
优化模块，用于基于多目标优化函数对所述预测变量进行非线性优化处理，得到优化解，并基于所述优化解得到所述水下航行器在第二时刻的控制变量序列，所述控制变量序列用于控制所述水下航行器在目标控制步数内的头翼运动姿态和机体增稳，所述多目标优化函数基于所述水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
根据本发明提供的一种水下航行器主动增稳控制装置，所述目标控制步数包括多个控制步数，所述控制变量序列包括所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角；所述装置还包括：
计算模块，用于在所述基于所述优化解得到所述水下航行器在第二时刻的控制变量序列之后，基于所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角，得到所述水下航行器在所述多个控制步数分别对应的关节旋转角速度和关节旋转角加速度。
本发明还提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现如上述任一种所述水下航行器主动增稳控制方法。
本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上述任一种所述水下航行器主动增稳控制方法。
本发明还提供一种计算机程序产品，包括计算机程序，所述计算机程序被处理器执行时实现如上述任一种所述水下航行器主动增稳控制方法。
本发明提供的水下航行器主动增稳控制方法及装置，通过获取水下航行器的第一时刻的状态变量，并将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，利用多目标优化函数对预测变量进行非线性优化处理，得到优化解，并根据优化解得到水下航行器在第二时刻的控制变量序列，以控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，能够降低水下航行器在仿生摆尾游动过程中的头翼的旋转误差和躯干的平移误差，大幅提升了机器双髻鲨的视觉稳定性，进而提高了图像采集的质量和仿生机器鱼运动时的稳定性。
附图说明
为了更清楚地说明本发明或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作一简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明提供的水下航行器主动增稳控制方法的流程示意图之一；
图2是本发明提供的水下航行器主动增稳控制方法的流程示意图之二；
图3是本发明提供的水下航行器主动增稳控制装置的结构示意图；
图4是本发明提供的电子设备的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚，下面将结合本发明中的附图，对本发明中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面结合图1-图3描述本发明的水下航行器主动增稳控制方法及装置。
图1是本发明提供的水下航行器主动增稳控制方法的流程示意图之一，如图1所示，该水下航行器主动增稳控制方法，包括如下步骤：
步骤110、获取水下航行器的第一时刻的状态变量，状态变量包括水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量。
在该步骤中，水下航行器可以是适合执行水下探索任务的仿生设备，例如，水下航行器可以是机器双髻鲨或水下机器人等。
在该步骤中，第一时刻可以是当前时刻的上一时刻，也可以是截至当前时刻前的多个时刻（包括当前时刻）。
在该步骤中，状态变量包括水下航行器在一段时间内偏航角、侧移速度的变化量和控制增量的变化量。
在该实施例中，控制增量可以是水下航行器在上一时刻的头翼旋转角度与当前旋转角度的差值。
步骤120、将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，预测模型基于水下航行器的动力学模型确定，第二时刻晚于第一时刻。
在该步骤中，预测模型可以根据水下航行器的截至当前时刻的状态变量预测水下航行器在未来时刻的状态变量。
在该步骤中，可以根据机器双髻鲨全状态动力学模型构造简化的离散预测模型。
在该实施例中，根据已有的机器双髻鲨全状态动力学模型设计离散预测模型，以提升优化算法效率和控制实时性，离散预测模型如下式所示：
其中，表示模型预测要在当前t时刻的基础上向后预测K步；/>代表仿生机器双髻鲨的全状态动力学模型，全速度向量V即为状态变量，/>表示预测的状态量；/>表示控制变量，具体为头翼关节的伺服电机的旋转角度/>、角速度/>和角加速度/>；表示状态变量约束，/>表示控制律的优化约束，即伺服电机控制量不可超过旋转角速度上限/>。
在该实施例中，考虑到头翼关节只能实现偏航方向增稳控制，模型迭代预测阶段仅关注机器双髻鲨的平面线速度和角速度变化。
步骤130、基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，并基于优化解得到水下航行器在第二时刻的控制变量序列，控制变量序列用于控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，多目标优化函数基于水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
在该步骤中，目标控制步数可以根据用户需求设置。
在该步骤中，多目标优化函数是基于多个待优化问题设计的联合优化函数。
在该实施例中，可以采用非线性优化方法对多目标优化问题进行处理，即通过在每次迭代中求解一个二次规划问题来逼近最优解。
在该实施例中，在每次迭代中，利用二次近似来近似原始问题的非线性部分，并求解这个近似问题的最优解，然后利用该最优解来更新当前的解，并计算一个新的近似问题，依次重复上述过程，直到上述迭代过程收敛为止。
在该实施例中，在非线性模型预测控制中，非线性优化方用于求解控制问题的优化部分，即求解控制输入变量的最优取值，以最小化控制误差或者达到控制目标。
在该实施例中，选定未来下一时刻控制量作为实际控制值，控制头翼伺服舵机偏航旋转角变化，以实现头翼运动和机体增稳。
本发明实施例提供的水下航行器主动增稳控制方法，通过获取水下航行器的第一时刻的状态变量，并将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，利用多目标优化函数对预测变量进行非线性优化处理，得到优化解，并根据优化解得到水下航行器在第二时刻的控制变量序列，以控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，能够降低水下航行器在仿生摆尾游动过程中的头翼的旋转误差和躯干的平移误差，大幅提升了机器双髻鲨的视觉稳定性，进而提高了图像采集的质量和仿生机器鱼运动时的稳定性。
在一些实施例中，获取水下航行器的第一时刻的状态变量，包括：获取水下航行器的导航指令，导航指令包括水下航行器的偏航指令和俯仰指令；基于中枢模式发生器模型对导航指令进行转换，得到偏航角变化量，并基于目标传感器获取侧移速度变化量、机体质心的侧移速度变化量。
在该实施例中，导航指令可以控制水下航行器在水下环境或者虚拟水下环境中的运动姿态和运动轨迹。
在该实施例中，目标传感器为至少一种传感器，或者为多种传感器组成的传感系统，例如，目标传感器可以是速度传感器和角度传感器组成的传感系统。
图2是本发明提供的水下航行器主动增稳控制方法的流程示意图之二，在图2所示的实施例中，导航指令发生器生成机器双髻鲨的俯仰角控制指令和偏航角控制指令，并将上述控制指令作为机器双髻鲨底层的中枢模式发生器（Central PatternGenerator，CPG）模型输入，分别得到机器双髻鲨在第/>预测时刻向后预测p个步长时的俯仰角控制量/>和第/>预测时刻向后预测p个步长时的偏航角控制量/>，并将/>和作为偏航角变化量，并且由传感系统采集机器双髻鲨在多个时刻的侧移速度变化量、机体质心的侧移速度变化量/>。
在该实施例中，获取水下航行器的第一时刻的状态变量主要包括如下：
其中，和/>分别为机器双髻鲨的机体质心和头翼关节的偏航角，/>为机体质心在载体坐标系下的横移速度，其本身即可作为平移误差，/>表示上一时刻头翼旋转角度与当前旋转角度的差值，/>为预测模型的预测步长总数。
本发明实施例提供的水下航行器主动增稳控制方法，通过获取水下航行器的第一时刻的多个状态变量，为多个优化目标提供参考量，同时为预测模型提供数据来源。
在一些实施例中，多目标优化函数应用如下公式得到：
其中，表示预测时刻，/>为所述水下航行器的机体质心偏航角，/>为所述水下航行器的头翼关节的偏航角，/>为指定的目标偏航角，/>为机体质心在载体坐标系下的横移速度，/>为上一时刻头翼旋转角度与当前时刻的头翼旋转角度的差值，/>为预测模型的预测步长总数；Q和R均为正定矩阵，用于调节不同误差的权重关系，且满足和/>。
在该实施例中，设计多目标优化函数时考虑以下两方面因素：其一，机器双髻鲨的视觉抖振不仅受到偏航方向的旋转运动影响，同时也受到平面（/>平面与水平面平行）内的往复平移运动的干扰，无论旋转还是平移误差均由头翼关节和机体质心的运动叠加构成，则需要把头翼关节与机体质心的偏航误差和平面横移误差同时作为目标函数的优化对象；其二，受限于机械结构条件，控制增量不宜过大，防止损坏电机设备。
本发明实施例提供的水下航行器主动增稳控制方法，通过水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差构造多目标优化函数，能够改善水下航行器的视觉抖振，且减少电机设备的损耗。
在一些实施例中，基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，包括：基于多目标优化函数构造待优化问题；基于序列二次规划算法和预测变量对优化问题进行求解，得到优化解。
在该实施例中，利用序列二次规划（Sequential Quadratic Programming，SQP）算法对优化问题来进行优化求解；SQP是一种常用的非线性优化方法，它通过在每次迭代中求解一个二次规划问题来逼近最优解。
在该实施例中，在优化问题的每次迭代求解过程中，采用二次近似法来近似待优化问题的非线性部分，并求解这个待优化问题的最优解，并使用这个最优解来更新当前的解，并计算一个新的近似问题。这个过程重复进行，直到满足一定的收敛准则为止。
需要说明的是，在非线性模型预测控制中，SQP通常被用于求解控制问题的优化部分，即求解控制输入变量的最优取值，以最小化控制误差或者达到控制目标。
本发明实施例提供的水下航行器主动增稳控制方法，通过多目标优化函数构造待优化问题，并利用序列二次规划算法和预测变量对优化问题进行求解，得到优化解，能够将水下航行器的姿态控制转换为多个待优化问题，并得到优化解，提高了水下航行器的姿态控制的准确性。
在一些实施例中，待优化问题应用如下公式获取：
，
其中，为预测时刻，/>为多目标优化函数计算相关的状态变量，minJ（）表示求取使得多目标优化函数J最小化的控制变量/>。
本发明实施例提供的水下航行器主动增稳控制方法，通过将待优化问题转换成与多目标优化函数对应状态变量相关的问题模型进行求解，便于对非线性问题进行分步求解，减少了计算量。
在一些实施例中，目标控制步数包括多个控制步数，控制变量序列包括水下航行器在多个控制步数分别对应的头翼关节旋转角；在基于优化解得到水下航行器在第二时刻的控制变量序列之后，该方法还包括：基于水下航行器在多个控制步数分别对应的头翼关节旋转角，得到水下航行器在多个控制步数分别对应的关节旋转角速度和关节旋转角加速度。
在该实施例中，通过优化上述问题的可行解，可得到控制步数内的控制变量序列：
在该实施例中，可以选择上述序列中的第一个数值作为当前时刻的最终控制量，即头翼关节旋转角。
相应地，关节旋转角速度和角加速/>可以通过前向差分法计算获得。
在图2所示的实施例中，将机器双髻鲨偏航角的旋转误差作为优化器的输入，在优化约束和多目标优化函数（对应目标函数）进行联合优化求解后，得到控制变量序列，并选取序列中的最终控制量/>发送至机器双髻鲨进行增稳控制。
在该实施例中，旋转误差为/>与/>之间的角度差值。
本发明实施例提供的水下航行器主动增稳控制方法，通过在基于优化解得到水下航行器在第二时刻的控制变量序列之后，利用水下航行器在多个控制步数分别对应的头翼关节旋转角，得到水下航行器在多个控制步数分别对应的关节旋转角速度和关节旋转角加速度，以实现水下航行器的增稳控制，同时提升了水下航行器的头部运动的可视化效果。
下面对本发明提供的水下航行器主动增稳控制装置进行描述，下文描述的水下航行器主动增稳控制装置与上文描述的水下航行器主动增稳控制方法可相互对应参照。
图3是本发明提供的水下航行器主动增稳控制装置的结构示意图，如图3所示，该水下航行器主动增稳控制装置，包括：获取模块310、预测模块320和优化模块330。
获取模块310，用于获取水下航行器的第一时刻的状态变量，状态变量包括水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；
预测模块320，用于将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，预测模型基于中枢模式发生器和水下航行器的动力学模型确定，第二时刻晚于第一时刻；
优化模块330，用于基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，并基于优化解得到水下航行器在第二时刻的控制变量序列，控制变量序列用于控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，多目标优化函数基于水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
本发明实施例提供的水下航行器主动增稳控制装置，通过获取水下航行器的第一时刻的状态变量，并将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，利用多目标优化函数对预测变量进行非线性优化处理，得到优化解，并根据优化解得到水下航行器在第二时刻的控制变量序列，以控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，能够降低水下航行器在仿生摆尾游动过程中的头翼的旋转误差和躯干的平移误差，大幅提升了机器双髻鲨的视觉稳定性，进而提高了图像采集的质量和仿生机器鱼运动时的稳定性。
在一些实施例中，目标控制步数包括多个控制步数，控制变量序列包括水下航行器在多个控制步数分别对应的头翼关节旋转角；该装置还包括：计算模块，用于在基于优化解得到水下航行器在第二时刻的控制变量序列之后，基于水下航行器在多个控制步数分别对应的头翼关节旋转角，得到水下航行器在多个控制步数分别对应的关节旋转角速度和关节旋转角加速度。
本发明实施例提供的水下航行器主动增稳控制装置，通过在基于优化解得到水下航行器在第二时刻的控制变量序列之后，利用水下航行器在多个控制步数分别对应的头翼关节旋转角，得到水下航行器在多个控制步数分别对应的关节旋转角速度和关节旋转角加速度，以实现水下航行器的增稳控制，同时提升了水下航行器的头部运动的可视化效果。
图4是本发明提供的电子设备的结构示意图，如图4所示，该电子设备可以包括：处理器(processor)410、通信接口(Communications Interface)420、存储器(memory)430和通信总线440，其中，处理器410，通信接口420，存储器430通过通信总线440完成相互间的通信。处理器410可以调用存储器430中的逻辑指令，以执行水下航行器主动增稳控制方法，该方法包括：获取水下航行器的第一时刻的状态变量，状态变量包括水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，预测模型基于中枢模式发生器和水下航行器的动力学模型确定，第二时刻晚于第一时刻；基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，并基于优化解得到水下航行器在第二时刻的控制变量序列，控制变量序列用于控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，多目标优化函数基于水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
此外，上述的存储器430中的逻辑指令可以通过软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器（ROM，Read-Only Memory）、随机存取存储器（RAM，Random Access Memory）、磁碟或者光盘等各种可以存储程序代码的介质。
另一方面，本发明还提供一种计算机程序产品，所述计算机程序产品包括计算机程序，计算机程序可存储在非暂态计算机可读存储介质上，所述计算机程序被处理器执行时，计算机能够执行上述各方法所提供的水下航行器主动增稳控制方法，该方法包括：获取水下航行器的第一时刻的状态变量，状态变量包括水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，预测模型基于中枢模式发生器和水下航行器的动力学模型确定，第二时刻晚于第一时刻；基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，并基于优化解得到水下航行器在第二时刻的控制变量序列，控制变量序列用于控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，多目标优化函数基于水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
又一方面，本发明还提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现以执行上述各方法提供的水下航行器主动增稳控制方法，该方法包括：获取水下航行器的第一时刻的状态变量，状态变量包括水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；将状态变量输入至预测模型进行预测处理，得到水下航行器的在第二时刻的预测变量，预测模型基于中枢模式发生器和水下航行器的动力学模型确定，第二时刻晚于第一时刻；基于多目标优化函数对预测变量进行非线性优化处理，得到优化解，并基于优化解得到水下航行器在第二时刻的控制变量序列，控制变量序列用于控制水下航行器在目标控制步数内的头翼运动姿态和机体增稳，多目标优化函数基于水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定。
以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性的劳动的情况下，即可以理解并实施。
通过以上的实施方式的描述，本领域的技术人员可以清楚地了解到各实施方式可借助软件加必需的通用硬件平台的方式来实现，当然也可以通过硬件。基于这样的理解，上述技术方案本质上或者说对现有技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品可以存储在计算机可读存储介质中，如ROM/RAM、磁碟、光盘等，包括若干指令用以使得一台计算机设备（可以是个人计算机，服务器，或者网络设备等）执行各个实施例或者实施例的某些部分所述的方法。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (9)
1.一种水下航行器主动增稳控制方法，其特征在于，包括：
获取水下航行器的第一时刻的状态变量，所述状态变量包括所述水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；
将所述状态变量输入至预测模型进行预测处理，得到所述水下航行器的在第二时刻的预测变量，所述预测模型基于所述水下航行器的动力学模型确定，所述第二时刻晚于所述第一时刻；
基于多目标优化函数对所述预测变量进行非线性优化处理，得到优化解，并基于所述优化解得到所述水下航行器在第二时刻的控制变量序列，所述控制变量序列用于控制所述水下航行器在目标控制步数内的头翼运动姿态和机体增稳，所述多目标优化函数基于所述水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定；
所述多目标优化函数应用如下公式得到：
；
其中，k为预测时刻，为所述水下航行器的机体质心偏航角，/>为所述水下航行器的头翼关节的偏航角，/>为指定的目标偏航角，/>为机体质心在载体坐标系下的横移速度，/>为上一时刻头翼旋转角度与当前时刻的头翼旋转角度的差值，/>为预测模型的预测步长总数；/>和/>均为正定矩阵，用于调节不同误差的权重关系，且满足/>和；
为所述多目标优化函数计算相关的状态变量，J为多目标优化函数，/>为上一时刻头翼旋转角度与当前时刻的头翼旋转角度的差值。
2.根据权利要求1所述的水下航行器主动增稳控制方法，其特征在于，所述获取水下航行器的第一时刻的状态变量，包括：
获取所述水下航行器的导航指令，所述导航指令包括所述水下航行器的偏航指令和俯仰指令；
基于中枢模式发生器模型对所述导航指令进行转换，得到所述偏航角变化量，并基于目标传感器获取所述侧移速度变化量、机体质心的侧移速度变化量。
3.根据权利要求1所述的水下航行器主动增稳控制方法，其特征在于，所述基于多目标优化函数对所述预测变量进行非线性优化处理，得到优化解，包括：
基于所述多目标优化函数构造待优化问题；
基于序列二次规划算法和所述预测变量对所述优化问题进行求解，得到所述优化解。
4.根据权利要求3所述的水下航行器主动增稳控制方法，其特征在于，所述待优化问题通过应用如下公式获取：
；
其中，k为预测时刻，为所述多目标优化函数计算相关的状态变量；
（）表示求取使得多目标优化函数J最小化的控制变量/>，/>为预测模型的预测步长总数，/>为旋转角速度上限。
5.根据权利要求1所述的水下航行器主动增稳控制方法，其特征在于，所述目标控制步数包括多个控制步数，所述控制变量序列包括所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角；
在所述基于所述优化解得到所述水下航行器在第二时刻的控制变量序列之后，所述方法还包括：
基于所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角，得到所述水下航行器在所述多个控制步数分别对应的关节旋转角速度和关节旋转角加速度。
6.一种水下航行器主动增稳控制装置，其特征在于，包括：
获取模块，用于获取水下航行器的第一时刻的状态变量，所述状态变量包括所述水下航行器的偏航角变化量、侧移速度变化量、机体质心的侧移速度变化量和控制增量变化量；
预测模块，用于将所述状态变量输入至预测模型进行预测处理，得到所述水下航行器的在第二时刻的预测变量，所述预测模型基于中枢模式发生器和所述水下航行器的动力学模型确定，所述第二时刻晚于所述第一时刻；
优化模块，用于基于多目标优化函数对所述预测变量进行非线性优化处理，得到优化解，并基于所述优化解得到所述水下航行器在第二时刻的控制变量序列，所述控制变量序列用于控制所述水下航行器在目标控制步数内的头翼运动姿态和机体增稳，所述多目标优化函数基于所述水下航行器的偏航角误差、侧移速度误差、机体质心的侧移速度误差和控制增量误差确定；
所述多目标优化函数应用如下公式得到：
；
其中，k为预测时刻，为所述水下航行器的机体质心偏航角，/>为所述水下航行器的头翼关节的偏航角，/>为指定的目标偏航角，/>为机体质心在载体坐标系下的横移速度，/>为上一时刻头翼旋转角度与当前时刻的头翼旋转角度的差值，/>为预测模型的预测步长总数；/>和/>均为正定矩阵，用于调节不同误差的权重关系，且满足/>和；
为所述多目标优化函数计算相关的状态变量，J为多目标优化函数，/>为上一时刻头翼旋转角度与当前时刻的头翼旋转角度的差值。
7.根据权利要求6所述的水下航行器主动增稳控制装置，其特征在于，所述目标控制步数包括多个控制步数，所述控制变量序列包括所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角；所述装置还包括：
计算模块，用于在所述基于所述优化解得到所述水下航行器在第二时刻的控制变量序列之后，基于所述水下航行器在所述多个控制步数分别对应的头翼关节旋转角，得到所述水下航行器在所述多个控制步数分别对应的关节旋转角速度和关节旋转角加速度。
8.一种电子设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述程序时实现如权利要求1至5任一项所述水下航行器主动增稳控制方法。
9.一种非暂态计算机可读存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现如权利要求1至5任一项所述水下航行器主动增稳控制方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310938219.5A CN116700015B (zh) | 2023-07-28 | 2023-07-28 | 水下航行器主动增稳控制方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310938219.5A CN116700015B (zh) | 2023-07-28 | 2023-07-28 | 水下航行器主动增稳控制方法及装置 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116700015A CN116700015A (zh) | 2023-09-05 |
| CN116700015B true CN116700015B (zh) | 2023-10-31 |
Family
ID=87843534
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310938219.5A Active CN116700015B (zh) | 2023-07-28 | 2023-07-28 | 水下航行器主动增稳控制方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116700015B (zh) |
Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102163059A (zh) * | 2011-04-27 | 2011-08-24 | 南京航空航天大学 | 推力变向无人机的姿态控制系统及控制方法 |
| CN110286683A (zh) * | 2019-07-15 | 2019-09-27 | 北京科技大学 | 一种履带式移动机器人的自主行驶路径跟踪控制方法 |
| CN110758698A (zh) * | 2019-11-28 | 2020-02-07 | 中国科学院自动化研究所 | 仿生滑翔机器海豚的滑翔深度控制方法、系统、装置 |
| CN111538241A (zh) * | 2020-04-30 | 2020-08-14 | 中国人民解放军国防科技大学 | 一种平流层飞艇水平轨迹智能控制方法 |
| US10935986B1 (en) * | 2019-11-28 | 2021-03-02 | Institute Of Automation, Chinese Academy Of Sciences | Gliding depth control method, system and device for biomimetic gliding robotic dolphin |
| CN112622903A (zh) * | 2020-10-29 | 2021-04-09 | 东北大学秦皇岛分校 | 一种车辆跟随驾驶环境下自主车辆的纵向和横向控制方法 |
| WO2021103392A1 (zh) * | 2019-11-29 | 2021-06-03 | 中国科学院自动化研究所 | 基于对抗结构化控制的仿生机器鱼运动控制方法、系统 |
| CN114721407A (zh) * | 2022-04-12 | 2022-07-08 | 南京工程学院 | 一种水下航行器无线充电系统的控制方法 |
-
2023
- 2023-07-28 CN CN202310938219.5A patent/CN116700015B/zh active Active
Patent Citations (8)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102163059A (zh) * | 2011-04-27 | 2011-08-24 | 南京航空航天大学 | 推力变向无人机的姿态控制系统及控制方法 |
| CN110286683A (zh) * | 2019-07-15 | 2019-09-27 | 北京科技大学 | 一种履带式移动机器人的自主行驶路径跟踪控制方法 |
| CN110758698A (zh) * | 2019-11-28 | 2020-02-07 | 中国科学院自动化研究所 | 仿生滑翔机器海豚的滑翔深度控制方法、系统、装置 |
| US10935986B1 (en) * | 2019-11-28 | 2021-03-02 | Institute Of Automation, Chinese Academy Of Sciences | Gliding depth control method, system and device for biomimetic gliding robotic dolphin |
| WO2021103392A1 (zh) * | 2019-11-29 | 2021-06-03 | 中国科学院自动化研究所 | 基于对抗结构化控制的仿生机器鱼运动控制方法、系统 |
| CN111538241A (zh) * | 2020-04-30 | 2020-08-14 | 中国人民解放军国防科技大学 | 一种平流层飞艇水平轨迹智能控制方法 |
| CN112622903A (zh) * | 2020-10-29 | 2021-04-09 | 东北大学秦皇岛分校 | 一种车辆跟随驾驶环境下自主车辆的纵向和横向控制方法 |
| CN114721407A (zh) * | 2022-04-12 | 2022-07-08 | 南京工程学院 | 一种水下航行器无线充电系统的控制方法 |
Non-Patent Citations (3)
| Title |
|---|
| Model Predictive Control-Based Depth Control in Gliding Motion of a Gliding Robotic Dolphin;Jian Wang et al.;IEEE TRANSACTIONS ON SYSTEMS, MAN, AND CYBERNETICS: SYSTEMS;第51卷(第9期);5466-5477 * |
| 不同通气量下双发航行体水下齐射降载增稳特性研究;高山 等;工程力学;105-112 * |
| 高机动水下仿生航行器研究现状;喻俊志 等;舰船科学技术;第42卷(第12期);8-12、100 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116700015A (zh) | 2023-09-05 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20230330848A1 (en) | Reinforcement and imitation learning for a task | |
| CN108227735B (zh) | 基于视觉飞行自稳定的方法、计算机可读介质和系统 | |
| CN106444806B (zh) | 基于生物速度调节的欠驱动auv三维轨迹跟踪控制方法 | |
| CN109240091B (zh) | 一种基于强化学习的水下机器人控制方法及其进行跟踪的控制方法 | |
| CN112462792B (zh) | 一种基于Actor-Critic算法的水下机器人运动控制方法 | |
| CN112947430B (zh) | 一种移动机器人的智能轨迹跟踪控制方法 | |
| CN115480583B (zh) | 飞行作业机器人的视觉伺服跟踪与阻抗控制方法 | |
| CN115494879A (zh) | 基于强化学习sac的旋翼无人机避障方法、装置及设备 | |
| CN113419424B (zh) | 减少过估计的模型化强化学习机器人控制方法及系统 | |
| CN114967714A (zh) | 一种自主式水下机器人抗扰运动控制方法及系统 | |
| CN114880874A (zh) | 一种水面无人船参数自适应鲁棒估计方法与系统 | |
| CN116700015B (zh) | 水下航行器主动增稳控制方法及装置 | |
| CN114310870A (zh) | 智能体的控制方法、装置、电子设备及存储介质 | |
| CN116560401A (zh) | 一种无人机编队中僚机控制指令的确定方法及终端设备 | |
| CN116954258A (zh) | 未知扰动下多四旋翼无人机编队的分层控制方法及装置 | |
| CN110703792B (zh) | 基于增强学习的水下机器人姿态控制方法 | |
| CN112631320A (zh) | 一种无人机自适应控制方法及系统 | |
| CN113093739A (zh) | 一种多无人艇编队避碰的优化控制器及其结构、设计方法 | |
| CN113050420A (zh) | 基于s面控制和td3的auv路径跟踪方法及系统 | |
| CN115808931B (zh) | 水下机器人运动控制方法、装置、系统、设备和存储介质 | |
| Sun et al. | Unmanned aerial vehicles control study using deep deterministic policy gradient | |
| CN111413995A (zh) | 双刚体特征点间相对位置跟踪与姿态同步控制方法及系统 | |
| CN114326759B (zh) | 多智能体编队控制方法、装置及多智能体系统 | |
| CN114428517B (zh) | 一种无人机无人艇协同平台端对端自主降落控制方法 | |
| CN117351449B (zh) | 基于极坐标加权的道路可通行区域边界优化方法 |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |