CN112945232B - 一种水下机器人近底地形跟踪的目标值规划方法 - Google Patents

Abstract

本发明涉及一种水下机器人近底地形跟踪的目标值规划方法，包括以下步骤：通过安装在水下机器人前下方的声学测距传感器采集到高度信息，构成最优目标运动轨迹，建立水下机器人垂直面运动学模型，然后依据水下机器人垂直面运动加减速性能和最大速度/角速度限定采样空间在一个可行的滑动范围内，采样多组前向速度和纵倾角速度目标值，并模拟水下机器人在这些目标值下一定时间内的运动轨迹。对多条运动轨迹进行评价，根据与实际地形误差绝对值之和最小为标准选取最优轨迹，其所对应的运动目标时间序列值来作为水下机器人的目标值，发送给底层运动控制器来驱动水下机器人的垂直面运动，从而实现水下机器人近底地形跟踪运动目标值的最优规划。

Description

一种水下机器人近底地形跟踪的目标值规划方法

技术领域

本发明涉及一种水下机器人(Autonomous underwater vehicle，简称为AUV)近底地形跟踪的目标值规划方法，属于水下机器人规划与控制技术领域。

背景技术

水下机器人常应用于近海底的地形测绘、光学拍照、声学探测等作业，而水下近底微地形复杂多变给水下机器人本体的操纵性和垂直面运动控制带来较大的技术挑战，且水下机器人是具有非线性、强时变、强耦合、大时延、多自由度的复杂系统，水动力参数的冗杂性和受风、浪、流等的随机扰动，使得水下机器人的高精度稳定控制难上加难。因此，水下机器人的垂直面运动控制技术研究具有重要的实际意义和应用价值。

传统的运动控制方式是人为提前设置垂直面航行控制的目标深度或高度值，然后发送给底层运动控制器进行闭环控制，驱使实际深度或高度逐渐收敛到目标值，存在的缺点是目标值随实际地形不断变化，当遇到地形高程频繁起伏变化时，容易导致水下机器人经常处于过渡过程的变化过程中，超调量和过渡过程时间等垂直面运动性能指标会恶化；而基于搭载在水下机器人前下方的测高传感器提前测得的地形信息，构造评价函数优化运动目标值，可提前调整水下机器人的目标值，起到预先调节作用，能有效提升垂直面的运动控制性能。

因此，研究实用化的水下机器人运动目标值规划方法，解决目标值优化问题，提高垂直面运动控制能力，是目前水下机器人近底跟踪中一个重点需要解决的实际问题。

发明内容

基于上述背景，本发明针对现有先进规划方法的不足，提出了一种水下机器人近底地形跟踪的目标值规划方法，可以实现机器人快速的垂直面运动目标值优化，提高了水下机器人的垂直面运动性能。

一种水下机器人近底地形跟踪的目标值规划方法，包括以下步骤：

通过安装在水下机器人前下方的声学测距传感器采集到距底距离信息，构成最优目标运动轨迹；

建立水下机器人垂直面运动学模型；

依据水下机器人垂直面运动加减速性能和最大速度/角速度限定采样空间在一个可行的滑动范围内，采样多组前向速度和纵倾角速度目标值，并模拟水下机器人在这些目标值下一段时间内的多组运动轨迹；

对多条运动轨迹进行评价，根据与实际地形误差绝对值之和最小为标准选取最优轨迹；

选取最优轨迹所对应的运动目标距离时间序列值来作为水下机器人的目标值，发送给底层运动控制器来驱动水下机器人的垂直面运动，从而实现水下机器人近底地形跟踪运动目标值的最优规划。

所述目标距离时间序列为水下机器人利用前下方安装的声学测距传感器实测的前方多拍的距底距离值，形成如下的距离时间序列集合：

H＝{h₁,h₂,……,h_n}

其中，h为实测的前方各拍的距底距离值；

由前方各拍距离值和当前位置可计算出相对应的位置值：

其中，x和z表示惯性坐标系下前向和垂向位置，k表示采样点，

为声学测距传感器与水下机器人水平面的安装偏角，θ表示当前纵倾角。

H集合中每个元素按顺序对应于如下目标位置的集合：

所述声学测距传感器为高度计或多普勒测速仪。

所述建立水下机器人垂直面运动学模型，包括：

假设水下机器人轨迹是由多拍直线运动组成，则水下机器人垂直面运动学模型如下：

其中，x和z表示惯性坐标系下前向和垂向位置，Δt表示时间采样步长，k表示采样点，u表示当前前向速度，θ表示当前纵倾角，φ表示当前航向角，q表示当前纵倾角速度；

当选取最优轨迹后，其所对应的垂直面速度(u，q)即为水下机器人的最优运动目标值。

所述依据水下机器人垂直面运动加减速性能和最大速度/角速度限定采样空间在一个可行的滑动范围内，采样多组前向速度和纵倾角速度目标值，并模拟水下机器人在这些目标值下一段时间内的多组运动轨迹，包括：

采样多组垂直面前向速度和纵倾角速度，推算垂直面运动轨迹；

所述垂直面前向速度和纵倾角速度存在如下两个约束：

①水下机器人受自身最大前向速度u和纵倾角速度q的约束为：

u∈[u_min,u_max]，q∈[q_min,u_max]

②由于执行机构所提供的力矩有限，存在最大加减速限制，因此水下机器人轨迹向前模拟的时间段内，存在一个时间滑动窗口，在该时间窗口内的速度是水下机器人能够实际达到的速度：

其中，u_c和q_c分别表示水下机器人的当前前向速度和纵倾角速度，a和

表示水下机器人的最大前向加速度和最大纵倾角加速度。

所述对多条运动轨迹进行评价，根据与实际地形误差绝对值之和最小为标准选取最优轨迹，包括：

在采样的速度组中，有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹作出评价，评价函数如下：

其中，R_i(u,q)采用目标位置

h_i采用垂直面运动学模型中的惯性坐标系下的前向位置x、垂向位置z、当前纵倾角θ，上式用来评价水下机器人在当前设定的采样速度下，模拟轨迹每一拍位置和目标位置之间的距离之和，使得评价函数获得最小值的垂直面速度(u，q)即为最优运动目标值。

本发明具有以下有益效果及意义：

1.本发明构建了最优目标轨迹以及通过建立评价函数求取最优运动目标值，实现了水下机器人运动目标值在线优化的工程应用需求。

2.本发明提出的运动目标值优化方法能够提前预测运动路径且利用未来地形高程信息，使水下机器人垂直面控制具有良好的抗干扰性和鲁棒性。

3.本发明规定了最优运动轨迹评价函数，可实时求取最优运动目标值，能方便快速地应用于实际工程。

附图说明

图1为本发明的水下机器人按最优目标轨迹进行近底地形跟踪示意图。

具体实施方式

下面结合附图对本发明作详细介绍。

本发明的一种水下机器人近底地形跟踪的目标值规划方法，包括以下步骤：

1、构造水下机器人高度目标值窗口时间序列。水下机器人利用前下方安装的高度计或多普勒测速仪等声学测距传感器实测前方多拍的距底距离值，形成如下的距离时间序列集合：

H＝{h₁,h₂,……,h_n}

其中，h为实测的前方各拍的距离值。

由前方各拍距离值和当前位置可计算出相对应的位置值：

其中，x和z表示惯性坐标系下前向和垂向位置，k表示采样点，

为声学测距传感器与水下机器人水平面的安装偏角，θ表示当前纵倾角。

H集合中每个元素按顺序对应于如下目标位置集合：

2、建立水下机器人垂直面运动学模型

若要求得水下机器人的垂直面运动轨迹，就需构建水下机器人的垂直面运动模型。假设水下机器人轨迹是由多拍直线运动组成。具体推导如下：

其中，x和z表示惯性坐标系下前向和垂向位置，Δt表示时间采样步长，k表示采样点，u表示当前前向速度，θ表示当前纵倾角，φ表示当前航向角，q表示当前纵倾角速度。

3、速度采样

水下机器人的垂直运动模型建立后，水平面的目标航向角和垂直面的定高目标值也已提前给定，根据前向速度和纵倾角速度就可推算出垂直面运动轨迹。因此只需采样多组垂直面速度，即前向速度和纵倾角速度，推算垂直面运动轨迹，然后评价这些轨迹是否接近实际地形所形成的目标轨迹。

在垂直面速度(u，q)的二维空间中，存在多组可选速度。根据水下机器人本身的限制和环境限制可以将采样速度控制在一定范围内：

水下机器人受自身最大速度和纵倾角速度的约束：

u∈[u_min,u_max]，q∈[q_min,u_max]

水下机器人受到电机性能的影响：

由于执行机构所提供的力矩有限，存在最大加减速限制，因此水下机器人轨迹向前模拟的时间段内，存在一个时间滑动窗口，在该时间窗口内的速度是水下机器人能够实际达到的速度：

其中，u_c和q_c分别表示水下机器人的当前前向速度和纵倾角速度，a和

表示水下机器人的最大前向加速度和最大纵倾角加速度。

4、构造目标值评价函数

在采样的速度组中，有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹作出评价。构造评价函数如下：

其中，R_i(u,q)采用目标位置

h_i采用垂直面运动学模型中的惯性坐标系下的前向位置x、垂向位置z、当前纵倾角θ，上式用来评价水下机器人在当前设定的采样速度下，模拟轨迹每一拍位置和目标位置之间的距离之和，使得评价函数获得最小值的垂直面速度(u，q)即为最优运动目标值。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰应视为本发明的保护范围。

Claims (3)

1.一种水下机器人近底地形跟踪的目标值规划方法，其特征在于，包括以下步骤：

通过安装在水下机器人前下方的声学测距传感器采集到距底距离信息，构成最优目标运动轨迹；

建立水下机器人垂直面运动学模型；包括：

假设水下机器人轨迹是由多拍直线运动组成，则水下机器人垂直面运动学模型如下：

其中，x和z表示惯性坐标系下前向和垂向位置，Δt表示时间采样步长，k表示采样点，u表示当前前向速度，θ表示当前纵倾角，φ表示当前航向角，q表示当前纵倾角速度；

当选取最优轨迹后，其所对应的垂直面速度(u，q)即为水下机器人的最优运动目标值；

依据水下机器人垂直面运动加减速性能和最大速度/角速度限定采样空间在一个可行的滑动范围内，采样多组前向速度和纵倾角速度目标值，并模拟水下机器人在这些目标值下一段时间内的多组运动轨迹；包括：

采样多组垂直面前向速度和纵倾角速度，推算垂直面运动轨迹；

所述垂直面前向速度和纵倾角速度存在如下两个约束：

①水下机器人受自身最大前向速度u和纵倾角速度q的约束为：

u∈[u_min,u_max]，q∈[q_min,u_max]

②由于执行机构所提供的力矩有限，存在最大加减速限制，因此水下机器人轨迹向前模拟的时间段内，存在一个时间滑动窗口，在该时间窗口内的速度是水下机器人能够实际达到的速度：

u∈[u_c-aΔt,u_c+aΔt]，

其中，u_c和q_c分别表示水下机器人的当前前向速度和纵倾角速度，a和

表示水下机器人的最大前向加速度和最大纵倾角加速度；

对多条运动轨迹进行评价，根据与实际地形误差绝对值之和最小为标准选取最优轨迹；包括：

在采样的速度组中，有若干组轨迹是可行的，因此采用评价函数的方式为每条轨迹作出评价，评价函数如下：

其中，R_i(u,q)采用目标位置

h_i采用垂直面运动学模型中的惯性坐标系下的前向位置x、垂向位置z、当前纵倾角θ，上式用来评价水下机器人在当前设定的采样速度下，模拟轨迹每一拍位置和目标位置之间的距离之和，使得评价函数获得最小值的垂直面速度(u，q)即为最优运动目标值；

选取最优轨迹所对应的运动目标距离时间序列值来作为水下机器人的目标值，发送给底层运动控制器来驱动水下机器人的垂直面运动，从而实现水下机器人近底地形跟踪运动目标值的最优规划。

2.根据权利要求1所述的一种水下机器人近底地形跟踪的目标值规划方法，其特征在于，所述目标距离时间序列为水下机器人利用前下方安装的声学测距传感器实测的前方多拍的距底距离值，形成如下的距离时间序列集合：

H＝{h₁,h₂,……,h_n}

其中，h为实测的前方各拍的距底距离值；

由前方各拍距离值和当前位置可计算出相对应的位置值：

其中，x和z表示惯性坐标系下前向和垂向位置，k表示采样点，

为声学测距传感器与水下机器人水平面的安装偏角，θ表示当前纵倾角；

H集合中每个元素按顺序对应于如下目标位置的集合：

3.根据权利要求1所述的一种水下机器人近底地形跟踪的目标值规划方法，其特征在于，所述声学测距传感器为高度计或多普勒测速仪。

Images