本发明涉及水下机器人运动控制，具体地说是涉及一种水下机器人抗扰控制方法。
背景技术：
：：1、海洋面积广阔无垠，蕴藏着丰富的生物资源、油气资源和矿产资源等，水下机器人是海洋勘测和资源开发不可缺少的重要技术装备，而水下机器人正常作业的关键技术之一是机器人的深度控制。深度控制是水下机器人自主作业的基础核心，也是决定水下机器人工作状态是否稳定与可靠的关键。由于水下机器人工作环境具有未知性，其运动具有强非线性、耦合性和时变性等特点，无法获取水下机器人准确的数学模型，目前的深度控制算法还无法实现水下机器人的快速精准的运动控制，所以水下机器人高精度抗扰深度控制一直是一项待解决的难点问题。2、针对水下机器人运动控制问题，国内外众多学者开展了较多研究，提出了较多的运动控制方法。目前，比例-积分-微分（proportional-integral-derivative，pid）算法由于简单便于实现的特点被普遍使用。自抗扰控制器是在pid控制器的基础上发展而来，由于自抗扰控制算法不依赖精确模型，并对外部扰动有着较高适应性，该算法受到了研究人员和工程师的广泛关注。目前已有学者将自抗扰技术应用到了水下机器人的控制器中，如采用双闭环自抗扰控制器实现水下机器人姿态控制，采用自抗扰控制器进行水下机器人定深控制。3、但是，目前将自抗扰技术应用到水下机器人控制中的策略，没有解决自抗扰控制器参数调节困难的问题，在实际应用过程中，只能根据前人的经验采用试错法对参数进行调节，耗费大量的时间精力，粗略得到符合要求的参数。技术实现思路1、本发明的目的在于提供一种水下机器人抗扰控制方法，实现控制器参数自动整定，以提高水下机器人的抗扰能力与控制精度。2、为了达到上述目的，本发明所采用的技术解决方案如下：3、一种水下机器人抗扰控制方法，所述方法包括如下步骤：4、步骤1、建立水下机器人的运动学与动力学模型；5、步骤2、构建自抗扰控制器；6、步骤3、建立评价函数；7、步骤4、改进星雀优化算法进行参数整定与优化。8、进一步地，所述步骤1的具体过程如下：9、步骤11、建立水下机器人自身坐标系与惯性坐标系；10、步骤12、建立水下机器人六自由度非线性运动学模型：11、；式（1）12、其中，表示水下机器人在惯性坐标系中的位置和姿态矢量，v表示水下机器人自身坐标系中的线速度和角速度矢量；13、；式（2）14、步骤13、建立水下机器人动力学模型：15、；式（3）16、其中，m为包含附加质量的惯性矩阵；为v的一阶导，即水下机器人在该方向的运动速度；c(v)为科里奥利力矩阵和向心力；d(v)为流体阻力矩阵；为重力和浮力矩阵；为推力矩阵。17、进一步地，所述步骤2的具体过程如下：18、含扰动的二阶系统表示为：19、；式（4）20、其中，为水下机器人垂向位置，为水下机器人垂向速度，为水下机器人垂向加速度，o为系统控制量；ω 为扰动量；g 表示被控系统模型变量之间的耦合关系；b 为控制量增益， b 用 估计；21、设为广义系统扰动，由式（4）可得：22、；式（5）23、总扰动f，设，用状态变量分别表示，如式（6）：24、；式（6）25、得到状态方程如下：26、；式（7）27、其中，d为状态变量的估计值；28、矩阵<mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi>f</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>=</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mo>[</mo><mstyledisplaystyle="true"mathcolor="#000000"><mtable><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>1</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>1</mi></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr></mtable></mstyle><mo>]</mo></mstyle></mstyle>；矩阵<mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi>g</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>=</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mo>[</mo><mstyledisplaystyle="true"mathcolor="#000000"><mtable><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>b</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><msub><mrow/><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></msub></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr></mtable></mstyle><mo>]</mo></mstyle></mstyle>；矩阵<mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi>e</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>=</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mo>[</mo><mstyledisplaystyle="true"mathcolor="#000000"><mtable><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>1</mi></mstyle></mtd></mtr></mtable></mstyle><mo>]</mo></mstyle></mstyle>；矩阵<mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi>h</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>=</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mo>[</mo><mstyledisplaystyle="true"mathcolor="#000000"><mtable><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>1</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></mtd></mtr></mtable></mstyle><mo>]</mo></mstyle></mstyle>；29、设计扩张状态观测器为：30、；式（8）31、其中， l扩张状态观测器反馈增益矩阵，<mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi>l</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>=</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mo>[</mo><mstyledisplaystyle="true"mathcolor="#000000"><mtable><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>β</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><msub><mrow/><mstyledisplaystyle="true"mathcolor="#000000"><mi>1</mi></mstyle></msub></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>β</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><msub><mrow/><mstyledisplaystyle="true"mathcolor="#000000"><mi>2</mi></mstyle></msub></mstyle></mtd></mtr><mtr><mtd><mstyledisplaystyle="true"mathcolor="#000000"><mi>β</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><msub><mrow/><mstyledisplaystyle="true"mathcolor="#000000"><mi>3</mi></mstyle></msub></mstyle></mtd></mtr></mtable></mstyle><mo>]</mo></mstyle></mstyle>；32、设计状态反馈及系统的最终控制量分别为：33、；式（9）34、；式（10）35、其中，、为控制器增益系数；为输入量；为中间控制量；为状态变量的估计值；36、根据带宽法，将状态误差反馈和扩张状态观测器参数配置在极点处可得式（11）（12），37、；式（11）38、；式（12）39、其中，为控制器带宽，为观测器带宽；40、并且，对自抗扰控制器进行参数配置即为对的值进行选取。41、进一步地，所述步骤3的具体过程如下：42、设定；式（13）43、其中，；44、选取转为整定，设参数向量如下：45、<mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi>x</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>=</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mo>[</mo><mstyledisplaystyle="true"mathcolor="#000000"><mstyledisplaystyle="true"mathcolor="#000000"><mi> </mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>b</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><msub><mrow/><mstyledisplaystyle="true"mathcolor="#000000"><mi>0</mi></mstyle></msub></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi> </mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi> </mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>ω</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><msub><mrow/><mstyledisplaystyle="true"mathcolor="#000000"><mi>c</mi></mstyle></msub></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi> </mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi> </mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi>n</mi></mstyle><mstyledisplaystyle="true"mathcolor="#000000"><mi> </mi></mstyle></mstyle><mo>]</mo></mstyle></mstyle>；式（14）46、设置求解函数为：47、；式（15）48、其中，err为稳态误差；con为总能耗；max为最大超调量；、、 为对应指标的权重。49、进一步地，所述步骤4的具体过程如下：50、采用佳点集生成种群：51、；式（16）52、；式（17）53、其中， 为佳点集；n为种群数量；d为种群维度；为表佳点；p 为满足的最小素数；i为从1到n的整数；54、引入佳点集后的种群初始化过程如式（18）：55、；式（18）56、其中， 为第t代中第i个胡桃夹子的第j个位置；为上限；为下限；57、通过ε-贪婪算法选择策略：58、；式（19）59、其中， 为0到1之间的随机数； 为第t代种群策略选择的概率；60、；式（20）61、其中，分别为ε的开始概率和结束概率；t为当前种群迭代次数；为种群迭代的最大次数；62、觅食与存储策略包括探索阶段和开发阶段，探索阶段过程如式（21），开发阶段过程如式（22），探索阶段和开发阶段再通过式（23）进行选择：63、；式（21）64、；式（22）65、；式（23）66、其中， 为t代种群中第i个星雀的新位置；γ 为根据莱维飞行生成的随机数；a、b、c分别为从群体中随机选择的三个不同度量值；是第 t 代所有解的第 j 维平均值；、、为介于 0 和 1 之间的随机数，为正态分布的随机值，根据莱维飞行生成的随机数；67、缓存搜索与恢复策略包括缓存阶段、探索阶段和利用阶段，缓存阶段过程如式（24）和式（25），探索阶段和利用阶段的区别在于缓存的选择不同，探索阶段和利用阶段的优化分别如式（26）和式（27），探索阶段和利用阶段再通过式（28）进行选择：68、；式（24）69、；式（25）70、；式（26）71、；式（27）72、；式（28）73、其中，为果壳虫缓存位置的当代迭代中的对象；为范围[0,π] 内的随机数；为第 t 次迭代中的当前最佳缓存位置；c为在总体中随机选择的一个解；74、通过比较新迭代的解和上一次迭代的解，保留高质量的解：75、；式（29）76、经过次迭代，得到x最优解，作为自抗扰控制器中的参数输入，并进行仿真与湖试验证。77、本发明的有益技术效果是：78、1、本发明引入佳点集与ε-贪婪算法改进星雀优化算法，并运用该算法改进自抗扰控制器，实现控制器参数自动整定；综合考虑稳态误差、超调量、能耗等控制器性能指标，合理分配权重，设置求解函数，迭代优化算法，得到使控制效果达到所需的最优情况对应的控制器参数；79、2、本发明相较于将传统的自抗扰控制器用于水下机器人深度控制策略，解决了自抗扰控制器参数调节困难的问题，提高了参数选取的科学性与可靠性，提升了水下机器人在定深作业过程中的控制精度与抗扰能力。当前第1页12当前第1页12