一种基于参数辨识的水下机器人垂直面运动控制方法

1.本发明涉及水下机器人的运动控制技术领域，尤其涉及变结构水下机器人运动控制方法，实现变结构水下机器人在机械臂运动条件下的高精度垂直面运动控制。


背景技术：

2.水下机器人在海洋工程、海洋科学研究的重要工程装备，水下机器人主要包括遥控水下机器人和自主水下机器人，其中遥控水下机器人依托脐带电缆作为能源供给和通信链路，搭载机械臂作为水下作业工具，承担水下科学目标抓取、水下工程装备的部署和安装等任务；自主水下机器人本身携带能源，不依托脐带电缆供电和通信，搭载多种水下探测载荷，承担水下远距离勘察探测任务。遥控水下机器人的优点是作业能力强，缺点是受到脐带电缆的约束，作业半径有限；自主水下机器人的优点是不受脐带电缆的约束，作业半径较大，缺点是能源有限，对本体流体线型阻力要求较高，无法搭载破坏流体线型的机械手，作业能力较弱。与传统水下机器人不同，变结构水下机器人是兼容自主水下机器人、遥控水下机器人的新概念水下机器人，它的技术特征是自身携带能源、无脐带电缆约束、低阻力流线外形、携带机械手、具有变形能力。当工况是长距离航渡或勘察时，变结构水下机器人回收机械手，变形为自主水下机器人模型，执行航行和勘察任务；当工况是小区域内精确作业时，变结构水下机器人释放机械手，变形为遥控水下机器人模式，执行高强度作业任务。变结构水下机器人实现了远距离航渡、大范围勘察、小区域目标作业等工作流程的一体化，极大地提升了海上作业效率，节约了海洋工程的宝贵时间成本。而变结构水下机器人的主要技术难点是如何克服变结构水下机器人在机械手运动过程中静力学参数的变化对垂直面运动控制的影响。传统机械臂的分析方法是直接从静力学模型角度计算机械臂运动过程中的静力学参数，它的缺点是机械臂运动过程中浮力分布的静力学参数理论计算精度较低，降低了变结构水下机器人控制方法的控制精度。


技术实现要素：

3.当变结构机器人承担长距离航渡或勘察任务时，变结构水下机器人将处于艏部的机械臂回收到机器人内部，变形为自主水下机器人模式；当变结构机器人承担承担小区域精确作业任务时，变结构水下机器人将机械臂从内部摆到艏部，变形为遥控水下机器人模式。与传统水下机器人只有单工作模式且静力学参数基本固定不同，变结构水下机器人在变形前后和机械臂运动过程中的静力学参数是时变参数，面临的最大技术难题是如何克服机械臂运动对变结构水下机器人的垂直面运动控制扰动。本发明创新地从参数辨识角度实时估计机械臂运动中时变静力学参数，采用基于动力学模型的在线参数辨识技术，创新地在参数估计过程中引入阻尼因子和遗忘因子，克服随机测量噪声对参数识别的抖动干扰，同时降低量测数据累积的“数据饱和”风险，提高了时变参数辨识的适应性和稳定性，将参数辨识代入垂直面运动控制器，实现变结构水下机器人的垂直面运动控制。首先将机械臂运动和水下机器人本体运动统一放入滤波器进行建模；然后根据水下机器人姿态角和速度
测量值反馈校正静力学参数估计，同时设计参数递推阻尼，防止参数辨识过程的过参数化，获得静力学参数的实时精确估计；最后将时变静力学参数和动力学模型结合，实现水下机器人在机械臂运动情况下的高精度垂直面运动控制。
4.本发明为实现上述目的所采用的技术方案是：
5.一种基于参数辨识的水下机器人垂直面运动控制方法，包括以下步骤：
6.1)基于水下机器人当前时刻的状态数据，预测水下机器人在当前时刻的扶正力矩；
7.2)计算当前时刻扶正力矩的预测精度；
8.3)基于水下机器人的状态数据以及预测精度，计算扶正力矩的新息误差、量测矩阵协方差、量测雅各比矩阵；
9.4)根据步骤3)得到的结果，更新水下机器人当前时刻的浮心位置估计及估计精度；
10.5)根据水下机器人的状态数据以及浮心位置估计，计算水下机器人垂直面控制的纵倾力矩，进而完成对水下机器人的实时运动控制。
11.所述步骤1)具体为：
12.根据水下机器人当前时刻tk的加速度和姿态角预测变结构水下机器人在时刻tk的扶正力矩gm(k)，具体为：
[0013][0014]
其中，m表示垂直面的附加质量，d表示垂直面的阻尼系数，c(k)表示时刻tk垂直面的向心力矩阵，v
uwq
(k)表示水下机器人在时刻tk的垂直面速度向量，表示水下机器人在时刻tk的垂直面速度向量的导数，uk、wk分别是前向速度和垂向速度，qk是纵倾角速度；分别是前向线加速度、垂向加速度，即前向速度的导数、垂向速度的导数，是纵倾角速度的导数，τm(k)是水下机器人在时刻tk输入的纵倾角控制力矩，m表示水下机器人的质量，xg和zg分别表示水下机器人重心的前向位置、垂向位置，mu、mw、mq、是水下机器人的水动力系数，iy是水下机器人的转动惯量。
[0015]
所述步骤2)具体为：
[0016]
根据水下机器人时刻tk加速度误差协方差、姿态角误差协方差，计算变结构水下机器人在时刻tk的扶正力矩gm(k)的预测精度p(gm(k))，具体为：
[0017][0018]
其中，r(m)、r(d)和r(c(k))分别表示垂直面附加质量、垂直面阻尼、垂直面向心力矩阵在时刻tk对计算扶正力矩均方误差的影响，q(k)表示时刻tk垂直面向心力矩阵对垂直面速度向量的导数，σa2是水下机器人捷联惯导测量的加速度误差方差，表示水下机器人捷联惯导的纵倾角速度的随机误差方差，和分别表示水下机器人多普勒计程仪的前向速度测量随机误差的方差、右向速度测量误差的方差。
[0019]
所述步骤3)具体为：
[0020]
根据水下机器人当前时刻tk姿态角、扶正力矩gm(k)、扶正力矩预测精度，采用误差传递原理构建扶正力矩观测模型，计算扶正力矩量测的新息误差εm(k)、量测矩阵协方差r1(k)、量测雅各比矩阵h1(k)，具体为：
[0021][0022]
其中，m1(k)是中间变量，和分别表示时刻t
k-1
水下机器人浮心前向位置的估计、垂向位置的估计，xb(k)和zb(k)分别表示时刻tk水下机器人浮心前向位置的预测、垂向位置的预测，δx(k)和δh(k)分别表示时刻tk水下机器人浮心与重心的前向位置间距预测、垂向位置间距预测，θk和φk是捷联惯导在时刻tk的测量值，和分别表示水下机器人捷联惯导测量的纵倾角、横滚角的随机误差方差，w和b分别表示水下机器人的重力和浮力。
[0023]
所述步骤4)具体为：
[0024]
根据水下机器人当前时刻tk扶正力矩的新息误差、量测矩阵协方差，计算当前时刻水下机器人的浮心位置和对应浮心的精度估计
[0025][0026]
其中，和分别表示时刻tk水下机器人浮心前向位置的估计、垂向位置的估计，表示时刻tk水下机器人浮心前向位置和垂向位置的预测估计精度，表示时刻t
k-1
水下机器人浮心位置的估计精度，sk是时刻tk水下机器人的新息协方差，表示时刻t
k-1
水下机器人浮心的估计，是已知量；水下机器人浮心的估计，是已知量；表示时刻t
k-2
水下机器人浮心的估计，i是单位矩阵，μ表示浮心修正的阻尼系数，α表示融合扶正力矩的遗忘系数，q([xb,zb]
t
)表示浮心变化驱动噪声。
[0027]
本发明具有以下有益效果及优点：
[0028]
1.相对于传统水下机器人控制方法，本发明具有在线估计机械臂作业过程对机器人控制的影响能力，具有更好的算法鲁棒性和环境适应性，实现了变结构水下机器人在机械臂运动状态下的高精度垂直面控制，提升变结构水下机器人的水下作业能力。
[0029]
2.应用范围广。本发明不但可以应用于变结构水下机器人航渡和水下作业工况，还可以用于遥控水下机器人水下作业和自主水下机器人勘察探测应用场景。
[0030]
3.经济成本较低。本发明不需额外增加传感器而只依托机器人本身传感器即可实现水下机器人控制，降低了水下机器人的设计成本。
附图说明
[0031]
图1是本发明的变结构水下机器人结构图；
[0032]
图2是本发明的变结构水下机器人垂直面控制方法的流程图。
具体实施方式
[0033]
下面结合附图及实施例对本发明做进一步的详细说明。
[0034]
整个系统组成包括变结构水下机器人、变结构水下机器人搭载的多普勒计程仪、捷联惯导、机械臂等设备。其中变结构水下机器人是搭载多普勒计程仪、捷联惯导、机械臂的潜航器，多普勒计程仪、捷联惯导是变结构水下机器人的导航传感器，多普勒计程仪的作用是测量变结构水下机器人相对水底的运动速度，它包括前向速度、垂向速度等；捷联惯导的作用是测量变结构水下机器人的姿态角，角速度率、线加速度等速度信息，它包括航向角、纵倾角、横滚角、前向线加速度即前向速度的导数、垂向线加速度即垂向速度的导数、纵
倾角速度等，其中纵倾角速度导数可以通过差分相邻时刻纵倾角速度间接获得；机械臂的作用是承担水下设备安装和水下样品目标抓取等水下作业任务。
[0035]
系统组成如图1所示。
[0036]
如图2所示，整个系统按照如下流程工作：
[0037]
为了阐述方便，将本文的符号变量进行如下定义：定义t表示时间变量，tk表示第k个时刻，k表示时间索引，且t
k-1
≤tk。定义u、w、q分别表示水下机器人的前向速度、垂向速度、纵倾角速度；定义uk、wk、qk分别表示水下机器人在时刻tk的前向速度、垂向速度、纵倾角速度；定义分别表示水下机器人前向速度的导数、垂向速度的导数、纵倾角速度的导数；定义分别表示水下机器人在时刻tk前向速度的导数、垂向速度的导数、纵倾角速度的导数；定义θ、φ分别表示水下机器人的纵倾角、横滚角。
[0038]
定义mu、mw、mq、是水下机器人的水动力系数，它们是已知量；定义xg和zg分别水下机器人重心的前向位置、垂向位置，它们是已知量；定义xb(k)和zb(k)分别表示水下机器人浮心在时刻tk的前向位置、垂向位置；定义θk、φk分别表示水下机器人在时刻tk的纵倾角、横滚角。
[0039]
定义i
x
、iy、iz是水下机器人的转动惯量，它们是已知量；定义m表示水下机器人的质量，是已知量；定义w和b分别表示水下机器人的重力和浮力，是已知量。
[0040]
为了方便描述矩阵和向量的运算，定义t表示矩阵或向量的转置算子，定义dlg(
·
)表示对角矩阵算子，定义p(
·
)表示求解括号内变量的误差协方差，定义c(
·
)表示计算余弦函数算子，定义s(
·
)表示计算正弦函数算子，定义表示括号内变量的随机误差，定义r(
·
)表示括号内变量的均方误差。
[0041]
第一步，预测水下机器人在当前时刻的扶正力矩
[0042]
根据水下机器人当前时刻tk的加速度和姿态角预测变结构水下机器人在时刻tk的扶正力矩gm(k)，它的计算方法如下：
[0043][0044]
其中gm(k)是待求解变量，它是重力和浮力合力产生的扶正力矩；定义m表示垂直面的附加质量，它是中间变量；定义d表示垂直面的阻尼系数，它是中间变量；定义c(k)表示时刻tk垂直面的向心力矩阵，它是中间变量；定义v
uwq
(k)表示水下机器人在时刻tk的垂直面速度向量，它是中间变量；表示水下机器人在时刻tk的垂直面速度向量的导数，它是中间变量；uk、wk是多普勒计程仪在时刻tk的测量值，是已知量测；qk是捷联惯导在时刻tk的测量值，是已知量测；是捷联惯导在时刻tk的测量值，是已知量测；是通过捷联惯
导在时刻tk和时刻t
k-1
的纵倾角角速度差分获得，它是间接测量，是已知量；τm(k)是水下机器人在时刻tk输入的纵倾角控制力矩，它是上一个周期的计算结果，它是实时变化的已知输入量；xg和zg分别水下机器人重心的前向位置、垂向位置，它们是已知量。
[0045]
第二步，计算当前时刻扶正力矩的预测精度
[0046]
根据水下机器人时刻tk加速度误差协方差、姿态角误差协方差，计算变结构水下机器人在时刻tk的扶正力矩gm(k)的预测精度p(gm(k))，它的计算方法如下：
[0047][0048]
其中p(gm(k))是待求解变量，它表示水下机器人在时刻tk的扶正力矩gm(k)误差的预测协方差p(gm(k))，即扶正力矩在时刻tk的预测精度；r(m)、r(d)和r(c(k))分别表示垂直面附加质量、垂直面阻尼、垂直面向心力矩阵在时刻tk对计算扶正力矩均方误差的影响，它们是中间变量；q(k)表示时刻tk垂直面向心力矩阵对垂直面速度向量的导数，它是中间变量；定义和分别表示水下机器人捷联惯导测量的的误差方差，它是捷联惯导的设备参数，是已知量，通常情况下二者值是相等的，即的设备参数，是已知量，通常情况下二者值是相等的，即是水下机器人捷联惯导测量的加速度误差方差；定义表示水下机器人捷联惯导的纵倾角速度的随机误差方差，它是捷联惯导的设备参数，是已知量；和表示水下机器人多普勒计程仪的前向速度测量随机误差的方差、右向速度测量误差的方差，它是多普勒计程仪的设备参数，是已知量，通常情况下二者值是相等的，即
[0049]
第三步，计算扶正力矩的新息误差、量测矩阵协方差、量测雅各比矩阵
[0050]
在实际使用中，利用水下机器人的重力和浮力近似相等对观测模型进行化简，根据水下机器人当前时刻tk姿态角、扶正力矩gm(k)、扶正力矩预测精度，采用误差传递原理构建扶正力矩观测模型，计算扶正力矩量测的新息误差、量测矩阵协方差、量测雅各比矩阵，它们的计算方法如下：
[0051][0052]
其中εm(k)、r1(k)和h1(k)是待求解变量，εm(k)表示时刻tk扶正力矩的新息误差；其中定义表示时刻tk扶正力矩的随机误差变量；r1(k)表示时刻tk扶正力矩的量测矩阵协方差，即h1(k)表示时刻tk扶正力矩的量测雅各比矩阵。m1(k)是中间变量，没有物理含义量；和表示时刻t
k-1
水下机器人浮心前向位置的估计、垂向位置的估计，是上一时刻的计算结果，是已知量；xb(k)和zb(k)表示时刻tk水下机器人浮心前向位置的预测、垂向位置的预测，它是中间变量；δx(k)和δh(k)表示时刻tk水下机器人浮心与重心的前向位置间距预测、垂向位置间距预测，它是中间变量；定义θk和φk是捷联惯导在时刻tk的测量值，是已知量；和分别表示水下机器人捷联惯导测量的纵倾角、横滚角的随机误差方差，它们是捷联惯导的设备参数，是已知量；p(gm(k))是水下机器人在时刻tk扶正力矩gm(k)的预测精度，它是上一步的计算结果，是已知量；gm(k)是水下机器人在时刻tk的扶正力矩，它是上一步的计算结果，是已知量；w和b分别表示水下机器人的重力和浮力，水下机器人的重力和浮力近似相等，即w＝b，它们是已知量；θk、φk是捷联惯导在时刻tk的测量值，是已知量测；。
[0053]
第四步，更新水下机器人当前时刻的浮心估计及估计精度
[0054]
根据水下机器人当前时刻tk扶正力矩的新息误差、量测矩阵协方差，设计阻尼系数和遗忘因子分别改善系统稳定性和参数时变适应性，计算当前时刻水下机器人的浮心和对应浮心的精度估计，它们的计算方法如下：
[0055][0056]
其中是待求解变量，和表示时刻tk水下机器人浮心
前向位置的估计、垂向位置的估计，是待求解变量；表示时刻tk水下机器人浮心前向位置和垂向位置的估计精度，是待求解变量；表示时刻tk水下机器人浮心前向位置和垂向位置的预测估计精度，是中间变量；表示时刻t
k-1
水下机器人浮心位置的估计精度，是上一个周期的计算结果，它是已知量；r1(k)表示时刻tk扶正力矩的量测矩阵协方差，是上一步计算结果，它是已知量；sk是时刻tk水下机器人的新息协方差，它是中间变量；εm(k)表示时刻tk扶正力矩的新息误差，是上一步的计算结果，是已知量；表示时刻t
k-1
水下机器人浮心的估计，是已知量；表示时刻t
k-2
水下机器人浮心的估计，是已知量；i是单位矩阵。μ表示浮心修正的阻尼系数，它是用户设定变量，建议取值范围是0到0.1，它是已知量；α表示融合扶正力矩的遗忘系数，它是用户设定变量，建议取值范围是1.0到1.2，它是已知量；q([xb,zb]
t
)表示浮心变化驱动噪声，它是用户设定变量，它是已知量。
[0057]
第五步，计算水下机器人垂直面控制的纵倾力矩
[0058]
τm(k 1)是水下机器人在时刻t
k 1
输入的纵倾角
[0059]
根据浮心的位置估计、水下机器人的纵倾角、纵倾角速度、目标纵倾角，采用标准pid控制器，计算水下机器人在时刻t
k 1
输入的纵倾角控制力矩τm(k 1)，它的计算方法如下：
[0060][0061]
其中τm(k 1)是待求解变量，τm(k 1)表示水下机器人在时刻t
k 1
输入的纵倾角控制力矩；qk是捷联惯导在时刻tk的测量值，是已知量测；θk是捷联惯导在时刻tk的测量值，是已知量测；θi是捷联惯导在时刻ti的测量值，是已知量测；θd是水下机器人的目标纵倾角，是已知量。表示时刻tk水下机器人浮心的估计，是上一步的计算结果，它是已知量；k
p
、kd、ki是水下机器人垂直面纵倾角控制参数，它们是用户设定值，是已知量。
[0062]
本发明是周期循环迭代调用步骤一到步骤五，本周期的输入是上一个周期的计算结果，本周期的输出是下一个周期的输入，实现变结构水下机器人在线垂直面运动控制。