一种无稳态误差的远程水下航行器定深控制方法与流程

本发明涉及一种无稳态误差的远程水下航行器的定深控制方法，属于水下航行器运动控制领域。

背景技术：

目前，水下航行器广泛采用“俯仰角+深度”的双环输出反馈定深控制方法，该方法简单有效。但是，对于一般的水下航行器，其重力和浮力不能完全相等，而且不具备理想的上下对称性，此时在定深直航状态下具有非零的平衡俯仰角和平衡舵角，造成深度控制的稳态误差。传统方法采取设置自由角的办法，即在原有双环反馈控制的基础上叠加一个恰当的固定角度，抵消由深度稳态误差产生的操舵，从而消除定深误差。

由于加工制造时的误差，同一型号水下航行器的流体动力参数和衡重参数存在一定差异，而且在不同海域、深度下海水密度也会发生变化，不能保证水下航行器平衡状态完全一致，这在低速情况下差异尤其显著，因此无法选择合适的自由角满足各种情况下的定深误差设计指标要求，为控制系统的研制带来了困难。

技术实现要素：

为了避免现有技术的不足之处，本发明提出一种无稳态误差的远程水下航行器定深控制方法，利用控制规律估计深度补偿，在实际航行中对深度补偿进行修正，保证无稳态误差的远程水下航行器定深航行。

一种无稳态误差的远程水下航行器定深控制方法，包括以下步骤：

步骤1：开始航行前，根据水下航行器模型参数计算的平衡攻角和平衡舵角标称值α_∞、δ_∞，依据控制规律δ_∞＝c_yΔy_∞+c_θα_∞，计算深度补偿其中，c_y为大于零的深度控制参数，依据定深爬潜过程的姿态要求选择，c_θ为大于零的俯仰角控制参数；

步骤2：水下航行器开始航行后，在每一个控制周期内，对深度信息进行采样，计算深度偏差其中：y(k)由深度传感器测量得到的航行深度，y_d为指定深度，Δy_max＞0为深度偏差限幅值；

步骤3：观测记录Δy(k)的值，计数值n＝n+1；

步骤4：如果数据量n＝N，N为设计的采样个数，计算深度偏差平均值和方差并令n＝0；如果判断深度稳定，为正的参数，的取值应大于深度测量噪声的方差，进行深度补偿的修正计算：Δy_∞＝Δy_∞+E(Δy(k))；

步骤5：计算横舵角操舵指令δ_e(k)＝c_y(y(k)-y_d+Δy_∞)+c_θθ(k)，其中，δ_e操下舵为正，θ(k)为姿态传感器获得的当前控制周期k的俯仰角测量值，以抬头为正；

步骤6：按照水下航行器最大舵角进行限幅：其中，δ_min、δ_max为舵角最大最小值，并将操舵指令发送个操舵机构；

步骤7：k＝k+1，重复步骤2，直至航行时间到或航行任务完成。

在本发明的一个优选实施例中，所述控制方法利用深度偏差的方差，判断水下航行器是否达到深度稳定，如果达到深度稳定，利用深度偏差的平均值，修正深度补偿，保证水下航行器无稳态误差的定深航行。

在本发明的一个优选实施例中，所述c_y取值范围0.01～0.05，c_θ取值范围0.5～5。

在本发明的一个优选实施例中，所述Δy_max取值范围为5m～20m。

通过以上方案，本发明的效果在于：

本发明采用深度补偿自适应机制，取代了传统控制中固定自由角的方法，消除了水下航行器不同平衡态造成的深度控制误差。

本发明适用于远程水下航行器的纵向运动控制，具有参数数目少、结构简单、自适应能力强、便于调节、控制精度高的优点，可用于水下机器人控制系统设计，进行海洋环境监测、水下设施的维护等。

附图说明

图1：是无稳态误差水下航行器定深控制流程图。

具体实施方式

现结合实施例、附图1对本发明作进一步描述：

本发明依据水下航行器的深度、俯仰角测量值，进行控制规律计算，产生舵角指令，对于水下航行器进行定深控制。其中，深度测量由压力传感器获得，俯仰角测量由倾斜仪或惯性导航系统获得。

一种无稳态误差的远程水下航行器定深控制方法，包括以下步骤：

步骤一：设置平衡攻角估计初值α_∞，平衡舵角估计初值δ_∞，深度补偿Δy_∞。其中，α_∞、δ_∞分别为根据水下航行器模型参数计算的平衡攻角和平衡舵角标称值，Δy_∞是依据控制规律δ_∞＝c_yΔy_∞+c_θα_∞计算的深度补偿，c_y为大于零的深度控制参数，依据定深爬潜过程的姿态要求选择，本实施例期望偏差10米时以30度(π/6)角度爬潜，则c_y＝π/60。

本实施例中控制周期为0.1秒，在每个控制循环k≥1中执行以下步骤。

步骤二：计算深度偏差，并作限幅处理。通过AD采样端口读取深度传感器输出的模拟电压，通过比例变化获得航行深度y(k)，设定深度y_d，水下航行为负。于是，深度偏差为

其中，Δy_max＞0为深度偏差限幅，避免深度误差产生过大的纵平面机动，一般取值5m～20m，本实施例选择Δy_max＝10。

步骤三：观测记录Δy(k)的值，计数值n＝n+1。

步骤四：如果数据量n＝N，N为设计的采样个数，计算深度偏差平均值和方差并令n＝0。如果判断深度稳定，为正的参数，的取值应大于深度测量噪声的方差，进行深度补偿的修正计算：Δy_∞＝Δy_∞+E(Δy(k))；本实施例选择N＝100，

步骤五：更新横舵角操舵指令δ_e(k)＝c_y(y(k)-y_d+Δy_∞)+c_θθ(k)，其中，δ_e操下舵为正，c_θ为大于零的俯仰角控制参数，取值范围0.5～5，依据水下航行器俯仰运动模型确定，本实施例选择c_θ＝3.0，θ(k)为姿态传感器获得的当前控制周期k的俯仰角测量值，以抬头为正。按照水下航行器最大舵角进行限幅

其中，δ_max、δ_min为舵角最大最小值，本实施例取δ_max＝0.349(20度)、δ_min＝-0.349(-20度)。

步骤六：k＝k+1，在下一个控制周期中重复步骤二，直至航行时间到或航行任务完成。

本实施例中，当初始值α_∞＝0.087(5度)，δ_∞＝-0.52(-3度)，在第1控制周期中，y(1)＝-5.0，深度指令y_d＝-20，Δy(1)＝10，俯仰角测量θ(1)为0.174(10度)，δ_e(1)＝2.102，限幅后δ_e(1)＝0.349。