本发明涉及水下机器人技术领域，尤其涉及一种可旋转舵推进器的水下机器人(简称AUV)控制方法，实现对AUV的五自由度(不包含横滚控制)运动控制。
背景技术：
在海洋应用中，水下机器人发挥越来越重要的作用。水下机器人分成两类：一种是遥控式有缆水下机器人(简称ROV)，一种是无人自治水下机器人(简称AUV)。ROV需要水面母船支持，同时受到电缆长度的限制，其作业距离有限，一般只有几百米；而AUV自身携带能源，可以远离母船，活动距离达到几十公里甚至上百公里。所以AUV的研究越来越受到各国的重视，AUV的发展代表了未来水下机器人的发展方向。但是AUV的控制方法比ROV的控制方法复杂，只有设计出好的控制方法才能发挥出AUV强大的作业能力。传统AUV控制方法主要采用经典线性控制理论进行控制和对执行结构进行控制力分配，这种方法最大的好处是算法简单，但是AUV的水动力性能是非线性和时变的，水动力试验也很难获得精确的水动力系数，再加上AUV负载变化引起重心和浮心的改变，导致当AUV的外界条件发生改变时，经典线性控制理论的控制方法控制性能急剧下降。本发明采用经典控制理论与变结构控制相结合的方法，对于水动力参数变化的不敏感性和控制抗干扰性优于传统的控制方法，更加适合AUV这种非线性时变模型系统。
技术实现要素：
为了克服传统线性控制方法在对于水动力系数敏感性较强和抗干扰性较弱的问题，本发明要解决的技术问题提供一种可旋转舵推进器的水下机器人控制方法，降低控制系统对水动力参数敏感性和提高系统的控制抗干扰性。
本发明为实现上述目的所采用的技术方案是：一种可旋转舵推进器的水下机器人控制方法，包括速度控制、航向控制和垂直面控制；
所述速度控制包括速度开环控制和速度闭环控制，所述速度开环控制不考虑速度反馈直接输出控制推进器轴向推力；所述速度闭环控制将速度反馈引入到航速控制器中；
所述航向控制为变结构航向控制；
所述垂直面控制分为强机动控制和弱机动控制，在水下机器人与目标位置的距离大于设定值时，采用强机动控制；否则采用弱机动控制。
所述速度开环控制的控制方法如下：
TX＝-X|u|u|ud|ud
其中，Tx是推进器的前向推力，是控制输出量；ud是AUV的目标速度，是控制输入量；X|u|u是AUV与前向速度相关的水动力系数，是已知参数。
所述速度闭环控制的控制方法如下：
其中，Tx是推进器的前向推力，是控制输出量；ud是AUV的目标速度，u是AUV的反馈速度，ud和u都是控制输入量；X|u|u是AUV与前向速度相关的水动力系数，是AUV的惯性质量，X|u|u和都是已知参数，Kp和Ki是需要整定的控制参数，通过线性控制理论的参数整定方法来计算。
所述变结构航向控制的控制方法如下：
τN＝Kp2(ψ-ψd)+Kd2(r-rd)+ηtanh(σ/0.2)
其中，τN是AUV的水平面转艏力矩，是航向控制的控制输出变量；σ是滑膜面，是航向控制的中间变量；(ψ-ψd)是当前航向角误差，(r-rd)是当前航向角误差的微分，它们是控制输入量；Kp2是航向角误差系数，Kd1是航向角误差微分系数，η是滑膜系数，它们可以通过试验测定，是已知参数。
所述弱机动控制的控制方法如下：
τM＝Kdp(d-dd)+Kpθ(θ-θd)+Kdθq+BGzWsinθ
其中，τM是垂直面的纵倾力矩，是控制输出量；d是AUV的当前深度值， dd是AUV的当前目标深度值，θ是AUV的纵倾角，θd是AUV当前的目标纵倾角，它们是AUV的状态量，通过AUV的深度传感器和姿态传感器可以测量和计算；(d-dd)是AUV的深度误差，(θ-θd)是AUV的纵倾角误差，q是AUV的垂直角速率，它们是控制输入量；Kdp是AUV的深度误差系数，Kpθ是AUV的纵倾角误差系数，Kdθ是AUV的垂直角速率系数，BGzWsinθ是AUV的重力/浮力垂直静力矩，它们可以通过试验测定，是已知参数。
所述强机动控制的控制方法如下：
τM＝Kpθ2(θ-θd)+Kdθ2q+BGzWsinθ
其中，τM是垂直面的纵倾力矩，是控制输出量；θ是AUV的当前纵倾角，θd是AUV当前的目标纵倾角，它们是AUV的状态量，通过AUV的姿态传感器可以测量和计算；(θ-θd)是AUV的纵倾角误差，q是AUV的垂直角速率，它们是控制输入量；Kpθ2是AUV的纵倾角误差系数，Kdθ2是AUV的垂直角速率系数，BGzWsinθ是AUV的重力/浮力垂直静力矩，它们可以通过试验测定，是已知参数。
本发明具有以下有益效果及优点
1.对于水动力系数不敏感。相比传统的控制方法，本方法对于水动力系数不敏感，能够在水动力模型不准确的情况下使用。
2.能够更好地适应外界环境的改变。当外界环境发生改变时，例如AUV负载的变化和外界海流的变化，传统的控制方法会偏离最佳工作状态，降低控制系统性能；本方法具有更好的鲁棒性，更好地适应外界环境的改变。
3.应用范围广。本发明不但可以应用于AUV，还可以用于各种机器人的运动控制。
附图说明
图1是本发明的组成示意图。
具体实施方式
下面结合附图及实施例对本发明做进一步的详细说明。
本发明的硬件要求是一台AUV，AUV的艏部配置2个水平舵，在艏部水平舵上装有2个推进电机；艉部配置2个水平舵，在艉部水平舵上装有2个推进电机；在艉部配置一个垂直舵，如图1所示。
本发明的AUV运动控制方法包括以下几个内容：
速度控制的作用是保持AUV以指定的前向速度航行。速度控制包括两种，一种是速度开环控制，是不考虑速度反馈直接输出控制推进器轴向推力的控制；另一种是速度闭环控制，是将速度反馈引入到航速控制器设计中。速度控制线性性比较高，所以采用经典线性控制方法。
速度开环控制的控制方法如下：
TX＝-X|u|u|ud|ud
其中，Tx是推进器的前向推力，是控制输出量；ud是AUV的目标速度，是控制输入量；X|u|u是AUV与前向速度相关的水动力系数，是已知参数。
速度闭环控制的控制方法如下：
其中，Tx是推进器的前向推力，是控制输出量；ud是AUV的目标速度，u是AUV的反馈速度，ud和u都是控制输入量；X|u|u是AUV与前向速度相关的水动力系数，是AUV的惯性质量，X|u|u和都是已知参数。Kp和Ki是需要整定的控制参数，可以通过线性控制理论的参数整定方法来计算。
航向控制的作用是保持AUV在指定的航向上沿直线稳定航行。在要求AUV进行水平面机动时，航向控制能够及时改变AUV的航向，转到指定的目标航向航行。传统航向控制的线性控制方法对于水动力参数敏感，当外界环境发生改变时，线性控制方法的航向控制就偏离最佳工作状态，增加操舵次数和推进器转速变更次数，造成舵机和推进器磨损，降低执行机构的使用寿命。当线性控制方法的航向控制偏离最佳工作状态时，还会造成周期性的航向震荡，降低AUV 的航行品质。所以本发明设计了变结构航向控制。变结构航向控制设计如下：
首先，设计航向控制的滑膜面，它的作用抑制外界改变对航向控制器的影响，提高航向控制器的鲁棒性：
σ＝Ki1(ψi-ψid)+Kp1(ψ-ψd)+Kd1(r-rd)
其中σ是滑膜面，是航向控制的中间变量；ψi是当前航向角误差的积分，ψid是当前目标航向角的积分，ψ是当前航向角，ψd是当前目标航向角，r是当前航向角的微分，rd是目标航向角微分，它们是AUV的状态量，通过AUV的姿态传感器可以测量和计算；(ψi-ψid)是航向角误差的积分，(ψ-ψd)是当前航向角误差，(r-rd)是当前航向角误差的微分，它们是控制输入量；Ki1是航向角误差积分系数，Kp1是航向角误差系数，Kd1是航向角误差微分系数，它们可以通过水池试验测定，是已知参数。
下一步，根据滑膜面设计的变结构航向控制：
τN＝Kp2(ψ-ψd)+Kd2(r-rd)+ηtanh(σ/0.2)
其中τN是AUV的水平面转艏力矩，是航向控制的控制输出变量；σ是滑膜面，是航向控制的中间变量；(ψ-ψd)是当前航向角误差，(r-rd)是当前航向角误差的微分，它们是控制输入量；Kp2是航向角误差系数，Kd1是航向角误差微分系数，η是滑膜系数，它们可以通过试验测定，是已知参数。
垂直面控制的作用是保持AUV在指定的深度下稳定航行和在垂直面内进行深度机动操纵。表面上看，垂直面控制和水平面控制相同，实际上垂直面控制复杂度高于水平面控制，AUV在垂直面运动，不但像水平面运动受到流体动力的作用外，还受到静力和静力矩的作用。垂直面控制分成强机动控制和弱机动控制，其中强机动控制是指大范围的垂直面机动，弱机动控制的垂直面机动范围比较小。
弱机动控制设计如下：
τM＝Kdp(d-dd)+Kpθ(θ-θd)+Kdθq+BGzWsinθ
其中，τM是垂直面的纵倾力矩，是控制输出量；d是AUV的当前深度值，dd是AUV的当前目标深度值，θ是AUV的纵倾角，θd是AUV当前的目标纵倾角，它们是AUV的状态量，通过AUV的深度传感器和姿态传感器可以测量和计算；(d-dd)是AUV的深度误差，(θ-θd)是AUV的纵倾角误差，q是AUV的垂直角速率，它们是控制输入量；Kdp是AUV的深度误差系数，Kpθ是AUV的纵倾角误差系数，Kdθ是AUV的垂直角速率系数，BGzWsinθ是AUV的重力/浮力垂直静力矩，它们可以通过试验测定，是已知参数。
强机动控制设计如下：
τM＝Kpθ2(θ-θd)+Kdθ2q+BGzWsinθ
其中，τM是垂直面的纵倾力矩，是控制输出量；θ是AUV的当前纵倾角，θd是AUV当前的目标纵倾角，它们是AUV的状态量，通过AUV的姿态传感器可以测量和计算；(θ-θd)是AUV的纵倾角误差，q是AUV的垂直角速率，它们是控制输入量；Kpθ2是AUV的纵倾角误差系数，Kdθ2是AUV的垂直角速率系数，BGzWsinθ是AUV的重力/浮力垂直静力矩，它们可以通过试验测定，是已知参数。
在AUV设计完成后，首先进行水池水动力试验，获得AUV的水动力系数。然后按照本发明方法设计AUV的速度、水平面和垂直面控制器。最后，将使用本发明方法的AUV进行航行试验，验证控制方法的正确性。