一种深海水下机器人的悬停控制方法与流程

本发明涉及一种深海水下机器人运动控制方法，尤其涉及一种深海水下机器人的悬停控制方法。

背景技术：

目前，深海水下机器人悬停控制主要有两类方式，一种是依靠执行机构产生直接的动力来控制载体垂直方向运动，一种是通过改变载体重力或浮力来控制载体垂直方向运动。

由于深海水下机器人深度改变范围大，海水密度变化及载体压缩等因素导致载体浮力与重力不平衡，即水下机器人载体具有一定的剩余浮力，并且该剩余浮力很难事先准确计算，另外处于安全考虑，水下机器人载体会人为的保留一定的剩余浮力，以避免坐底危险。

配置了垂向推进器的深海水下机器人，某些作业任务需要水下机器人能够保持一定深度不动(定深悬停)或保持距离水底一定的高度不动(定高悬停)，从而完成对环境信息或目标物的测量、观察、记录等操作。然而由于剩余浮力的影响，水下机器人的升沉控制可能产生振荡或者产生问题误差，导致悬停控制品质降低。因此，需要找到一种能够克服剩余浮力影响的深海水下机器人悬停控制方法。

技术实现要素：

本发明的目的是为了提供一种深海水下机器人的悬停控制方法，采用深沉运动速度控制与位置控制自动切换、位置控制调整项自动调整实现对深度偏差的调节，进而实现定深悬停或定高悬停控制目的。

本发明的目的是这样实现的：步骤如下：

步骤一：偏差产生器给出统一的深度偏差ed，即向下为正方向的垂直位置偏差；在定深悬停时由规划系统给出的目标值深度值dd与机器人深度计测量的深度值d做差，得到深度偏差ed:

ed＝dd-d

在定高悬停时由规划系统给出的目标值高度值hd与机器人高度计测量的距水底高度值h做差，得到高度偏差eh:

eh＝hd-h

由于高度信息定义的正方向与深度信息定义的正方向相反，在定高悬停时将高度偏差转化为深度偏差，即：

ed＝-eh

然后对深度偏差ed进行微分得到深度偏差变化率e`d：

步骤二：切换控制器根据深度偏差ed的绝对值大小自动选择使用速度控制器或者位置控制器：当深度偏差ed的绝对值大于阈值ey时，即|ed|>ey时，采用速度控制器，否则采用位置控制器；

步骤三：速度控制器采用积分算法，深度偏差ed线性映射为目标升沉速度vd：

vd＝kved

与深度偏差变化率e`d做差得到速度偏差ev：

ev＝vd-e`d

对ev进行积分计算后乘以积分系数ki作为控制输出o，实现对机器人定深或定高悬停控制：

o(t)＝o(t-1)+kiev(t)

步骤四：位置控制器采用比例微分算法，深度偏差ed乘以积分系数kp加上深度偏差变化率e`d乘以微分系数kd作为位置控制器输出ol：

ol(t)＝kped(t)+kde`d(t)

位置控制器输出ol与位置控制调整项a耦合实现对机器人定深或定高悬停控制：

o(t)＝ol(t)+a(t)

当速度控制器切换到位置控制器时，将速度控制器最后输出作为位置控制调整项：

a(t)＝o(t)

当位置控制器切换到速度控制器时，将位置控制器输出与位置控制调整项最后的耦合值作为速度控制器积分初始值，然后位置控制器输出与位置控制调整项耦合置0：

o(t)＝a(t)，a(t)＝0

当位置控制器有效时，位置控制调整项根据位置控制器输出自动调整，调整规则为，若一段时间内位置控制器输出值方差小于阈值dy，则位置控制调整项a累加当前位置控制器输出值o与比例参数k的乘积：

a(t)＝a(t-1)+k*o(t)

当深度偏差满足控制精度并且保持稳定后，位置控制调整项的大小可视为剩余浮力的大小。

与现有技术相比，本发明的有益效果是：本发明将水下机器人悬停控制根据深度偏差的大小分为两种情况，深度偏差较大的情况下采用速度控制，其积分作用允许垂向输出达到执行器的最大输出值，此时能够发挥出水下机器人全部的抵抗剩余浮力的能力，并且水下机器人以较小的速度垂向运动时，其控制输出接近剩余浮力大小，以此作为位置控制的调整项可以加快深度偏差收敛速度；深度偏差较小的情况下采用位置控制，可以保证控制的响应速度与控制精度，同时位置控制调整项可消除稳态误差；在速度控制和位置控制切换时，由于位置控制调整项的存在，控制输出时连续的，可避免产生冲击，保证控制输出的连续性；位置控制调整项还表征了实际的剩余浮力大小，对于调整水下机器人压载配置或者研究机器人与深海环境的相互作用提供了参考。

附图说明

图1为深海水下机器人的悬停控制方法原理图；

图2为1000米定深悬停控制曲线；

图3为5米定高悬停控制曲线。

具体实施方式

下面结合附图与具体实施方式对本发明作进一步详细描述。

结合图1至图3，本发明是一种用于深海水下机器人的悬停控制方法，包括偏差产生器、切换控制器、速度控制器、位置控制器及位置控制调整项、耦合器；

所述偏差产生器定义了统一的深度偏差ed，即向下为正方向的垂直位置偏差；在定深悬停时由规划系统给出的目标值深度值与机器人深度计测量的深度值做差，得到深度偏差ed；在定高悬停时由规划系统给出的目标值高度值与机器人高度计测量的距水底高度值做差，得到高度偏差eh，由于高度信息定义的正方向与深度信息定义的正方向相反，为了后续控制器设计方便，控制器输入统一使用深度偏差，在定高悬停时将高度偏差转化为深度偏差，即ed＝-eh；然后对深度偏差ed进行微分得到深度偏差变化率e`d；

所述切换器根据深度偏差ed的绝对值大小自动选择使用速度控制器或者位置控制器；当深度偏差ed的绝对值大于阈值ey时，即|ed|>ey时，采用速度控制器，否则采用位置控制；

所述速度控制器采用积分算法，深度偏差ed线性映射为目标升沉速度vd，然后与深度偏差变化率e`d做差得到速度偏差ev，对ev进行积分计算后乘以积分系数ki作为控制输出，实现对机器人定深或定高悬停控制；

所述位置控制器采用比例微分算法，深度偏差ed乘以积分系数kp加上深度偏差变化率e`d乘以微分系数kd作为位置控制器输出，位置控制器输出与位置控制调整项耦合实现对机器人定深或定高悬停控制；

所述位置控制调整项只在位置控制器有效时参与控制；当速度控制器切换到位置控制器时，将速度控制器最后输出作为位置控制调整项；当位置控制器切换到速度控制器时，将位置控制器输出与位置控制调整项最后的耦合值作为速度控制器积分初始值，然后位置控制器输出与位置控制调整项耦合置0；当位置控制器有效时，位置控制调整项根据位置控制器输出自动调整，调整规则为，若一段时间内位置控制器输出值方差小于阈值dy，则位置控制调整项a累加当前位置控制器输出值o与比例参数k的乘积，即a(t)＝a(t-1)+k*o(t)；当深度偏差满足控制精度并且保持稳定后，位置控制调整项的大小可视为剩余浮力的大小。

本发明提供了一种深海水下机器人的悬停控制方法，如图1所示：包括偏差产生器、切换控制器、速度控制器、位置控制器及位置控制调整项、耦合器；

所述偏差产生器定义了统一的深度偏差ed，即向下为正方向的垂直位置偏差；在定深悬停时由规划系统给出的目标值深度值dd与机器人深度计测量的深度值d做差，得到深度偏差ed:

ed＝dd-d

在定高悬停时由规划系统给出的目标值高度值hd与机器人高度计测量的距水底高度值h做差，得到高度偏差eh:

eh＝hd-h

由于高度信息定义的正方向与深度信息定义的正方向相反，为了后续控制器设计方便，控制器输入统一使用深度偏差，在定高悬停时将高度偏差转化为深度偏差，即：

ed＝-eh

然后对深度偏差ed进行微分得到深度偏差变化率e`d：

所述切换控制器根据深度偏差ed的绝对值大小自动选择使用速度控制器或者位置控制器；当深度偏差ed的绝对值大于阈值ey时，即|ed|>ey时，采用速度控制器，否则采用位置控制；

所述速度控制器采用积分算法，深度偏差ed线性映射为目标升沉速度vd：

vd＝kved

然后与深度偏差变化率e`d做差得到速度偏差ev：

ev＝vd-e`d

对ev进行积分计算后乘以积分系数ki作为控制输出o，实现对机器人定深或定高悬停控制：

o(t)＝o(t-1)+kiev(t)

所述位置控制器采用比例微分算法，深度偏差ed乘以积分系数kp加上深度偏差变化率e`d乘以微分系数kd作为位置控制器输出ol：

ol(t)＝kped(t)+kde`d(t)

位置控制器输出ol与位置控制调整项a耦合实现对机器人定深或定高悬停控制；

o(t)＝ol(t)+a(t)

所述位置控制调整项只在位置控制器有效时参与控制；当速度控制器切换到位置控制器时，将速度控制器最后输出作为位置控制调整项：

a(t)＝o(t)

当位置控制器切换到速度控制器时，将位置控制器输出与位置控制调整项最后的耦合值作为速度控制器积分初始值，然后位置控制器输出与位置控制调整项耦合置0：

o(t)＝a(t)，a(t)＝0

当位置控制器有效时，位置控制调整项根据位置控制器输出自动调整，调整规则为，若一段时间内位置控制器输出值方差小于阈值dy，则位置控制调整项a累加当前位置控制器输出值o与比例参数k的乘积：

a(t)＝a(t-1)+k*o(t)

当深度偏差满足控制精度并且保持稳定后，位置控制调整项的大小可视为剩余浮力的大小。

图2为海中1000米定深悬停控制曲线，图3为海中5米定高悬停控制曲线。

综上，本发明公开了一种深海水下机器人的悬停控制方法。本发明采用深沉运动速度控制与位置控制自动切换、位置控制调整项自动调整实现对深度偏差的调节，进而实现定深悬停或定高悬停控制目的。速度控制可使水下机器人发挥最大的垂向控制能力，位置控制保证深度偏差的调节精度与响应速度，位置控制调整项可消除垂向稳态误差。本发明能够使水下机器人克服剩余浮力的影响，实现定深或定高悬停，并能够反映剩余浮力的大小。