水下检测与作业机器人动力定位方法技术

本发明专利技术公开了一种水下检测与作业机器人动力定位方法，本发明专利技术解决动力定位受到波浪干扰而产生的不确定性问题，结合所研制的新型水下检测与作业机器人，基于流体动力学数字模拟辨识参数法建立动力学模型，进行六自由度动力定位分析。通过传感器获得水下机器人的位置和艏向，采用自适应无迹卡尔曼滤波算法实时估计水下机器人的状态，并采用非奇异终端滑模控制对推力予以补偿，以减小由波浪等随机带来的影响，根据定位误差设计力和力矩分配策略。该方法具有良好的动力定位效果，并且在受扰动后可以迅速调整动力分配策略，减小其带来的负面影响。

【技术实现步骤摘要】

本专利技术涉及一种水下检测与作业机器人，尤其涉及一种水下检测与作业机器人动力定位方法，属于机器人

技术介绍
随着陆地资源的枯竭，海洋资源的开发利用对人类发展和社会进步起着巨大的推动作用。水下机器人(ROV)作为开发海洋的手段之一，一直得到广泛的关注和研究。近几年来，国内外多数水下机器人只有浮游能力或爬行能力，能够实现浮游和爬壁的两种模式的水下机器人较为少见。水下检测与作业机器人，是一种新颖的模块化、多功能、带缆遥控水下机器人，可以在浮游和爬行模态之间自由切换，并进行水下作业，被广泛应用于水下观察、浮游勘察、海底勘探、水下结构检修、海底管线检测以及水下安装等作业，已经成为海洋工程水下结构安全检测及维护的关键装备。随着水下机器人的研究与发展，水下机器人的功能已经不仅仅局限于在水下低速或高速航行，而且需要具有如：悬停、旋转及爬潜等更高的机动性能。在水下机器人执行任务的过程中，不但要求水下航行器在环境扰动作用下按照预定的轨迹运动，而且在许多情况下需要利用水下机器人对目标物进行更细致的观察和作业，这就需要水下机器人相对于目标物的位置保持不变，即要求水下机器人具有能够抵抗环境扰动的动力定位能力。由于动力定位系统具有不受水深限制，投入和撤离迅速等优点并且可以实现上述的精确的机动性能。因此，为使水下机器人在深海中具有更强的能力，研究水下机器人的动力定位技术是十分有必要的。水下机器人在水下运动，会受到波浪、洋流和各种噪声的干扰。对于跟踪系统而言,由于外界干扰、加速度的物理特性和人为操纵等因素影响,很难用一个准确的统计特性来表示系统噪声,所以它的统计特性往往是未知和时变的。申请号为“201410090281.4”的专利文献公开了一种“船舶动力定位系统的控制方法及其控制系统”，但所采用的PID控制算法收敛性较差，使得船舶接近目标点较慢，并且容易发生振荡；申请号为“201510016643.X”的专利文献公开了“一种基于改进强跟踪滤波状态观测器的船舶动力定位方法”，其改进强跟踪滤波状态观测器关于不确定模型的鲁棒性较差，状态估计精度不高，容易出现发散等问题。
技术实现思路
本专利技术的目的在于提供一种水下检测与作业机器人动力定位方法，该水下机器人动力定位方法基于自适应无迹卡尔曼滤波状态观测器和非奇异终端滑模控制器，以提高水下机器人稳定性。为解决上述问题，本专利技术采用的技术方案是：一种水下检测与作业机器人动力定位方法，包括以下步骤：步骤1：在有环境干扰的情况下，通过传感器系统来采集动力定位水下机器人的位置和艏向信息，通过位置和艏向信息建立出水下机器人运动学数学模型为： M v · + C R B ( v ) v + C A ( v ξ ) v ξ + D ( v ξ ) v ξ + g ( η ) = τ + ξ η · = J ( η ) v - - - ( 1 ) ]]>式中，为水下机器人相对于固定坐标系的位置和姿态角，其中为横滚角，θ为俯仰角，ψ为航向角；v＝[u v w p q r]T，为水下机器人在艇体坐标系内的速度和角速度；J(η)为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R6×6；C(v)＝CRB+CA，为水下机器人附加质量的科氏力及向心力矩阵，CRB为水下机器人附加质量的科氏力，CA为水下机器人向心力矩阵，C(v)∈R6×6；D(vξ)是水下机器人流体阻力矩阵，D(vξ)∈R6×6；vξ＝v-vd为去除环境干扰力时水下机器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R6×1；τ是水下机器人推进器提供的推力，τ∈R6×1；ξ是环境干扰力，ξ∈R6×1；将式(1)整理变换为： η · = J ( η ) v v · 本文档来自技高网...

【技术保护点】
一种水下检测与作业机器人动力定位方法，其特征在于，该方法包含下列步骤：步骤1：在有环境干扰的情况下，通过传感器系统来采集动力定位水下机器人的位置和艏向信息，通过位置和艏向信息建立出水下机器人运动学数学模型为：Mv·+CRB(v)v+CA(vξ)vξ+D(vξ)vξ+g(η)=τ+ξη·=J(η)v---(1)]]>式中，为水下机器人相对于固定坐标系的位置和姿态角，其中为横滚角，θ为俯仰角，ψ为航向角；v＝[u v w p q r]T，为水下机器人在艇体坐标系内的速度和角速度；J(η)为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R6×6；C(v)＝CRB+CA，为水下机器人附加质量的科氏力及向心力矩阵，CRB为水下机器人附加质量的科氏力，CA为水下机器人向心力矩阵，C(v)∈R6×6；D(vξ)是水下机器人流体阻力矩阵，D(vξ)∈R6×6；vξ＝v‑vd为去除环境干扰力时水下机器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R6×1；τ是水下机器人推进器提供的推力，τ∈R6×1；ξ是环境干扰力，ξ∈R6×1；将式(1)整理变换为：η·=J(η)vv·=M-1[-CRB-v-CAvξ-Dvξ-g(η)+τ]---(2)]]>自适应无迹卡尔曼滤波算法基于如下非线性离散状态空间模型：xk|k-1=fk(xk-1|k-2)+Bu+wkyk|k-1=hkxk|k-1+vk---(3)]]>式中，k表示系统离散采样时间点，k≥0；xk|k‑1＝[η v]T为k时刻的系统估计状态量；fk(xk‑1|k‑2)＝[J(η)v‑M‑1CRBv‑M‑1CAvξ‑M‑1Dvξ‑M‑1g]T，fk(xk‑1|k‑2)为xk‑1到xk的一步转移矩阵；B＝[06×6M‑1]T为系数矩阵；u∈R6×1是推进器推力；wk为系统激励高斯白噪声序列；hk为量测矩阵，反应量测量和估计量之间的数学关系；vk为量测值高斯白噪声序列；yk为k时刻的传感器量测值；步骤2：自适应无迹卡尔曼滤波状态观测器通过以下设计步骤实现：1)状态初始条件为x‾0=E(x0)---(4)]]>P0=E((x0-x‾0)(x0-x‾0)T)---(5)]]>Q^0=Q^0---(6)]]>其中,x0为状态初始值；为x0的均值；P0为预测协方差初始值；为系统噪声初始值；2)预测更新对于给定的和Pk‑1|k‑1，根据式(7)—(13)用UT法求状态预测和(1)计算Sigma点χk-1=x^k-1|k-1x^k-1|k-1±((n+λ)Pk-1|k-1)12---(7)]]>其中，χk‑1为k‑1时刻的Sigma点；为k‑1时刻的状态值；Pk‑1|k‑1为k‑1时刻的预测协方差；n为大于1正整数，λ为比例系数；(2)预测更新χk|k‑1＝f(χk‑1)     (8)x‾k|k-1=Σi=02nWimχi,k|k-1---(9)]]>Pk|k-1=Σi=02nWic[χi,k|k-1-x‾k|k-1][χi,k|k-1-x‾k|k-1]T+Q^k-1---(10)]]>y‾k|k-1=Wimh(χi,k|k-1)---(11)]]>其中，f(χk‑1)为χk‑1到χk的一步转移矩阵；和为均值和协方差的加权值；为k‑1时刻的系统噪声；h(χi,k|k‑1)为量测矩阵；预测协方差为：Pk|k-1=Σi=02nWic[χi,k|k-1-x‾k|k-1][χi,k|k-1-x‾k|k-1]T+Q^k-1---(12)]]>3)发散判断由式判断是否发散,如发散按照式(14)—(16)修正Pk|k‑1,不发散则进入下一步，其中为残差序列；Pk|k-1=λkΣi=02nWic[χi,k|k-1-x‾k|k-1][χi,k|k-1-x‾k|k-1]T+Q^k-1---(13)]]>λk=λ0,λ0≥11,λ0<1---(14)]]>λ0=tr(...

【技术特征摘要】
1.一种水下检测与作业机器人动力定位方法，其特征在于，该方法包含下列步骤：步骤1：在有环境干扰的情况下，通过传感器系统来采集动力定位水下机器人的位置和艏向信息，通过位置和艏向信息建立出水下机器人运动学数学模型为： M v · + C R B ( v ) v + C A ( v ξ ) v ξ + D ( v ξ ) v ξ + g ( η ) = τ + ξ η · = J ( η ) v - - - ( 1 ) ]]>式中，为水下机器人相对于固定坐标系的位置和姿态角，其中为横滚角，θ为俯仰角，ψ为航向角；v＝[u v w p q r]T，为水下机器人在艇体坐标系内的速度和角速度；J(η)为坐标转换矩阵；M为水下机器人惯性矩阵，M∈R6×6；C(v)＝CRB+CA，为水下机器人附加质量的科氏力及向心力矩阵，CRB为水下机器人附加质量的科氏力，CA为水下机器人向心力矩阵，C(v)∈R6×6；D(vξ)是水下机器人流体阻力矩阵，D(vξ)∈R6×6；vξ＝v-vd为去除环境干扰力时水下机器人的速度；g(η)是由重力和浮力组成的回复力矩阵，g(η)∈R6×1；τ是水下机器人推进器提供的推力，τ∈R6×1；ξ是环境干扰力，ξ∈R6×1；将式(1)整理变换为： η · = J ( η ) v v · = M - 1 [ - C R B - v - C A v ξ - Dv ξ - g ( η ) + τ ] - - - ( 2 ) ]]>自适应无迹卡尔曼滤波算法基于如下非线性离散状态空间模型： x k | k - 1 = f k ( x k - 1 | k - 2 ) + B u + w k y k | k - 1 = h k x k | k - 1 + v k - - - ( 3 ) ]]>式中，k表示系统离散采样时间点，k≥0；xk|k-1＝[η v]T为k时刻的系统估计状态量；fk(xk-1|k-2)＝[J(η)v-M-1CRBv-M-1CAvξ-M-1Dvξ-M-1g]T，fk(xk-1|k-2)为xk-1到xk的一步转移矩阵；B＝[06×6M-1]T为系数矩阵；u∈R6×1是推进器推力；wk为系统激励高斯白噪声序列；hk为量测矩阵，反应量测量和估计量之间的数学关系；vk为量测值高斯白噪声序列；yk为k时刻的传感器量测值；步骤2：自适应无迹卡尔曼滤波状态观测器通过以下设计步骤实现：1)状态初始条件为 x ‾ 0 = E ( x 0 ) - - - ( 4 ) ]]> P 0 = E ( ( x 0 - x ‾ 0 ) ( x 0 - x ‾ 0 ) T ) - - - ( 5 ) ]]> Q ^ 0 = Q ^ 0 - - - ( 6 ) ]]>其中,x0为状态初始值；为x0的均值；P0为预测协方差初始值；为系统噪声初始值；2)预测更新对于给定的和Pk-1|k-1，根据式(7)—(13)用UT法求状态预测和(1)计算Sigma点 χ k - 1 = x ^ k - 1 | k - 1 x ^ k - 1 | k - 1 ± ( ( n + λ ) P k - 1 | k - 1 ) 1 2 - - - ( 7 ) ]]>其中，χk-1为k-1时刻的Sigma点；为k-1时刻的状态值；Pk-1|k-1为k-1时刻的预测协方差；n为大于1正整数，λ为比例系数；(2)预测更新χk|k-1＝f(χk-1) (8) x ‾ k | k - 1 = Σ i = 0 2 n W i m χ i , k | k - 1 - - - ( 9 ) ]]> P k | k - 1 = Σ i = 0 2 n W i c [ χ i , k | k - 1 - x ‾ k | k - 1 ] [ χ i , k | k - 1 - x ‾ k | k - 1 ] T + Q ^ k - 1 - - - ( 10 ) ]]> y ‾ k | k - 1 = W i m h ( χ i , k | k - 1 ) - - - ( 11 ) ]]>其中，f(χk-1)为χk-1到χk的一步转移矩阵；和为均值和协方差的加权值；为k-1时刻的系统噪声；h(χi,k|k-1)为量测矩阵；预测协方差为： P k | k - 1 = Σ i = 0 2 n W i c [ χ i , k | k - 1 - x ‾ k | k - 1 ] [ ...

【专利技术属性】
技术研发人员：曾庆军，梁凇，刘慧婷，张明，
申请(专利权)人：江苏科技大学，
类型：发明
国别省市：江苏;32