CN115480580A - 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 - Google Patents
一种基于nmpc的水下机器人路径跟踪与避障的控制方法 Download PDFInfo
- Publication number
- CN115480580A CN115480580A CN202211161234.5A CN202211161234A CN115480580A CN 115480580 A CN115480580 A CN 115480580A CN 202211161234 A CN202211161234 A CN 202211161234A CN 115480580 A CN115480580 A CN 115480580A
- Authority
- CN
- China
- Prior art keywords
- underwater
- underwater robot
- obstacle
- robot
- speed
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 25
- 238000005457 optimization Methods 0.000 claims abstract description 14
- 230000004888 barrier function Effects 0.000 claims abstract description 9
- 239000011159 matrix material Substances 0.000 claims description 33
- 238000005070 sampling Methods 0.000 claims description 12
- 238000004364 calculation method Methods 0.000 claims description 8
- 238000005259 measurement Methods 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 4
- 238000006073 displacement reaction Methods 0.000 claims description 4
- 238000013016 damping Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 238000004088 simulation Methods 0.000 description 8
- 238000001914 filtration Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 3
- 238000002474 experimental method Methods 0.000 description 3
- 239000011664 nicotinic acid Substances 0.000 description 3
- 241000282414 Homo sapiens Species 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000011217 control strategy Methods 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 1
- 238000013528 artificial neural network Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000003592 biomimetic effect Effects 0.000 description 1
- 230000002950 deficient Effects 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/04—Control of altitude or depth
- G05D1/06—Rate of change of altitude or depth
- G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于NMPC的水下机器人路径跟踪与避障的控制方法,包括:建立水下机器人的运动学动力学模型;计算出当前水下机器人的速度,估计出外部扰动;对水下机器人未来N个控制周期的运动状态进行预测,预测水下障碍物的短时间内的轨迹;对最优化问题进行求解并获得最优控制序列;声呐扫描周期结束,计算卡尔曼滤波器新的先验状态量与后验状态量,对水下障碍物的不确定性同样进行更新计算,否则不进行更新,保持不变;直至水下机器人进行路径跟踪且能完成避障。本发明采用改进的扩张状态观测器进行扰动观测,大大提升水下机器人抗干扰能力,运动的鲁棒性;使得水下机器人碰撞概率降低,缩短行驶距离且扩大了与障碍物的安全距离。
Description
技术领域
本发明涉及水下仿生机器人路径跟踪技术领域,尤其是一种基于NMPC的 水下机器人路径跟踪与避障的控制方法。
背景技术
海洋是地球最大的资源宝藏,由于陆地资源不断被挖掘匮乏,人类对海洋 不断地进行研究与开发,水下机器人就成为了开发海洋资源的重要工具之一, 它可代替人类在海洋中完成高难度高风险的资源探测与采集等一系列工作,其 中水下机器人的相关技术,最核心的就是运动控制,来完成高精度路径跟踪与 自主动态避障。
几十年来,水下机器人或水面无人船(USV)的路径跟踪控制一直是世界各 国学者共同关注的问题,并提出了许多优秀的控制策略:如自适应控制、反步 控制、滑模方法、神经网络控制、模型预测控制等。这些传统控制方法对于模 型参数不确定性和未知扰动情况下的水下机器人或水面舰船的路径跟踪问题 都有一定作用效果,但很少有在控制方法中能考虑到避障问题,以及对障碍物 未来位置的有效预测,然而,在海洋自然环境扰动中,一旦偏离预定轨迹,发 生碰撞的风险将极大增加,水下机器人如何能够安全避免碰撞,并保持对预定 路径的跟踪,这对于仅依靠冲击力和偏航力矩的欠驱动水下机器人来说是一个严峻的挑战。此外,依赖波动鳍推进的仿生水下机器人控制输入不同于传统的 螺旋桨推进水下机器人和水面舰艇,导致模型的不确定性和未知外部干扰的估 计变得更加复杂。
发明内容
本发明的目的在于提供一种大大提升水下机器人抗干扰能力和运动的鲁 棒性,对短周期内的障碍物未来位置进行预测,使得水下机器人碰撞概率降低, 缩短行驶距离且扩大了与障碍物的安全距离的基于NMPC的水下机器人路径跟 踪与避障的控制方法。
为实现上述目的,本发明采用了以下技术方案:一种基于NMPC的水下机 器人路径跟踪与避障的控制方法,该方法包括下列顺序的步骤:
(2)测量水下机器人当前的位置,读取艏摇角,通过扩张状态观测器计 算出当前水下机器人的速度,估计出外部扰动,同时采用声呐对水下障碍物状 态进行扫描,并将探测结果输入卡尔曼滤波器;
(3)利用建立的运动学动力学模型,根据当前水下机器人的速度和外部 扰动,对水下机器人未来N个控制周期的运动状态进行预测,并同时预测水下 障碍物的短时间内的轨迹;
(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障 碍物的短时间内的轨迹,利用求解器对最优化问题进行求解并获得最优控制序 列,并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号;
(5)如果声呐扫描周期结束,通过声呐测量的水下障碍运动状态更新, 则卡尔曼滤波器的输入更新,计算卡尔曼滤波器新的先验状态量与后验状态量, 并且对水下障碍物的不确定性同样进行更新计算,如果声呐扫描周期没有结束 则卡尔曼滤波器与水下障碍物不确定性不进行更新,保持不变;
(6)重复步骤(2)至步骤(5),直至水下机器人进行路径跟踪且能完 成避障。
所述步骤(1)具体是指:
建立水下机器人三自由度下的运动学动力学模型:
其中,为状态向量,V=[u r v]T表示水下机 器人的速度,x和y代表大地坐标系下水下机器人的位置,表示水下机器人艏 摇角,u为水下机器人前进速度,v为水下机器人横移速度,r为水下机器人艏摇 速度;为控制输入量,fu为控制输入纵倾力,fv为横向驱 动力,水下机器人为欠驱动系统,fv=0;fr为偏航力矩;τd=[du dv dr]T为 水下机器人外部扰动,du为水下机器人前进方向扰动力,dv为水下机器人横移 方向扰动力,dr为艏摇方向的外部环境扰动力矩,M为水下机器人惯性质量矩 阵,D为水下机器人阻尼力对角矩阵,C(v)为向心力和科里奥利矩阵,其公式 如下:
其中,m11、m22、m33均为水下机器人的附加质量参数,d11、d22、d33均为 水动力系数;
建立最优化问题的目标函数:
其中,Jx(x(t),xref(t))为期望状态代价,其公式为:
Ju(αu(t))为控制输入量代价,其公式为:
Jc(x(t)为碰撞代价,其公式为:
其中,T为水下机器人采样周期,f(x,u)是描述水下机器人连续非线性方 程,di(t)表示为与第i个水下障碍物的欧氏距离,ki>0为平滑避障轨迹的参 数;为期望状态代价项的权重系数矩阵,为控制输入量代 价的权重系数矩阵,Qc表示碰撞代价系数;U是控制量力与力矩可输入范围, 如下式所示:
水下机器人是欠驱动系统,则有fvmin=fvmax=0;fvmin为水下机器人横 向驱动力最小值,fvmax为水下机器人横向驱动力最大值,fumin为水下机器人 纵向驱动力的最小值,fumax为水下机器人纵向驱动力的最大值,frmin为水下 机器人艏摇力矩的最小值,frmax为水下机器人艏摇力矩最大值。
所述步骤(2)具体是指:
测量与计算水下机器人状态量x(t0),采用扩张状态观测器计算水下机器人 速度、外部扰动状态:
其中,l0至l8皆为正整数,b0,b1为补偿因子,ζ=u·t与χ=v·t分别代表 体坐标系下的纵向与横向位移;表示水下机器人艏摇角,u为水下机器人前 进速度,v为水下机器人横移速度,r为水下机器人艏摇速度;fu为控制输入纵 倾力,fr为偏航力矩;m11、m22、m33均为水下机器人的附加质量参数,d11、 d22、d33均为水动力系数;通过分别求解式(6)、(7)、(8),得出水下 机器人当前的纵向速度横向速度艏摇速度以及纵向横向艏摇三个方向外部扰动的估计值,“∧”表示计算估计值,“·”表示微分符号。
其中,为水下障碍物i的预测位置,Ts为采样周期,pobs i(t0)为水下 障碍物当前时刻的测量位置,vo-为水下障碍物测量先验速度,N为预测步数, t0为预测开始当下时刻,往后预测n个Ts采样周期;
(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态 进行预测;采用公式(9),通过声呐测量出的当前水下障碍物的位置,加上 水下障碍物测量先验速度乘上采样周期Ts,获得未来n个Ts周期内的水下障碍 物直线运动位置。
所述步骤(4)具体是指:利用求解器对最优化问题进行求解并获得最优 控制序列u*(t+nTs),n=1,...,N-1,并将最优控制序列首位转换作为水下 机器人波动鳍推进器的控制输入信号,u*表示通过求解器求解出来的最优控制 序列。
所述步骤(5)具体是指:如果声呐扫描周期结束,即水下障碍物的测量 状态量更新,则对卡尔曼滤波器的水下障碍物先验状态量与后验状态量Pt更新计算,并且对水下障碍物的不确定性rth,k(t)进行计算更新,否则卡尔曼滤 波器与水下障碍物不确定性不进行更新;
其中卡尔曼滤波器分为如下两大部分:
其中,xt=[xobs,yobs,vobs]T是水下障碍物的状态量,xobs,yobs为水下障 碍物的坐标位置,vobs为水下障碍物的速度,Zt=[xo,yo,vo]T是在t时刻的测 量的水下障碍物状态量,xo,yo为水下障碍物测量的坐标位置,vo为水下障碍 物的测量速度,F是状态转移矩阵,H是观测矩阵,Qt是水下障碍物运动噪声 协方差矩阵,Rt为观测噪声协方差矩阵,I为单位矩阵;
rth,i(t)=rth+10δi(t) (12)
其中,rth为常量参数。
由上述技术方案可知,本发明的有益效果为:第一,本发明对于仿生水下 机器人由于独特的推进方式而导致模型参数不确定性以及外部环境干扰,采用 改进的扩张状态观测器进行扰动观测,并将其作为非线性模型预测控制扰动项 进行不断计算更新,大大提升水下机器人抗干扰能力,运动的鲁棒性;第二, 本发明针对动态障碍物状态观测不确定性,即观测误差,声呐扫描周期过长, 通过结合卡尔曼滤波先验估计,对短周期内的障碍物未来位置进行预测,使得 水下机器人碰撞概率降低,缩短行驶距离且扩大了与障碍物的安全距离。
附图说明
图1是路径跟踪示意图;
图2是本发明的方法框架示意图;
图3、4、5、6均是仿真1过程的重要时刻截图;
图7是曲线跟踪对比结果示意图;
图8、9、10、11、12、13均是多动态水下障碍物仿真截图。
具体实施方式
如图2所示,一种基于NMPC的水下机器人路径跟踪与避障的控制方法, 该方法包括下列顺序的步骤:
(1)建立水下机器人的运动学动力学模型,建立最优化问题的目标函数, 设定全局路径参考点,利用贝塞尔曲线插值获得全局跟踪路径计算得到艏 摇角;系统参数初始化,包含系列全局跟踪路径全局跟踪路径参考点为 如图1所示,其中,ex表示水下机器当前位置坐标x与期望位置坐 标xd之间的误差,ey表示水下机器当前位置坐标y与期望位置坐标yd之间的误 差,表示水下机器当前水下机器人艏摇角与期望艏摇角参考角之间的误 差;xB和yB为水下机器人体坐标系坐标轴,xI和yI为大地坐标系坐标轴。
(2)测量水下机器人当前的位置,读取艏摇角,通过扩张状态观测器计 算出当前水下机器人的速度,估计出外部扰动,同时采用声呐对水下障碍物状 态进行扫描,并将探测结果输入卡尔曼滤波器;
(3)利用建立的运动学动力学模型,根据当前水下机器人的速度和外部 扰动,对水下机器人未来N个控制周期的运动状态进行预测,并同时预测水下 障碍物的短时间内的轨迹;
(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障 碍物的短时间内的轨迹,利用求解器对最优化问题进行求解并获得最优控制序 列,并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号;
(5)如果声呐扫描周期结束,通过声呐测量的水下障碍运动状态更新, 则卡尔曼滤波器的输入更新,计算卡尔曼滤波器新的先验状态量与后验状态量, 并且对水下障碍物的不确定性同样进行更新计算,如果声呐扫描周期没有结束 则卡尔曼滤波器与水下障碍物不确定性不进行更新,保持不变;
(6)重复步骤(2)至步骤(5),直至水下机器人进行路径跟踪且能完 成避障。
在本发明中,路径规划是指:在具有障碍物的环境中,给定机器人的起始 点与目标点,然后要求机器人从某一处出发,按照某种控制规律到达该路径上, 并实现其跟踪运动。
NMPC即非线性模型预测控制:是一种基于非线性模型的闭环优化控制策 略,对于强非线性系统,线性模型难以描述复杂动态过程,线性化模型不太适 用,造成线性预测控制难以运用,故提出的非线性系统模型预测控制。
卡尔曼滤波:一种利用线性的系统状态方程,通过系统输入输出观测数据, 对系统状态进行最优估计的算法。
扩张状态观测器:是通用而实用的扰动观测器。
所述步骤(1)具体是指:
建立水下机器人三自由度下的运动学动力学模型:
其中,为状态向量,V=[u r v]T表示水下机 器人的速度,x和y代表大地坐标系下水下机器人的位置,表示水下机器人艏 摇角,u为水下机器人前进速度,v为水下机器人横移速度,r为水下机器人艏摇 速度;为控制输入量,fu为控制输入纵倾力,fv为横向驱 动力,水下机器人为欠驱动系统,fv=0;fr为偏航力矩;τd=[du dv dr]T为 水下机器人外部扰动,du为水下机器人前进方向扰动力,dv为水下机器人横移 方向扰动力,dr为艏摇方向的外部环境扰动力矩,M为水下机器人惯性质量矩 阵,D为水下机器人阻尼力对角矩阵,C(v)为向心力和科里奥利矩阵,其公式 如下:
其中,m11、m22、m33均为水下机器人的附加质量参数,d11、d22、d33均为 水动力系数;
建立最优化问题的目标函数:
其中,Jx(x(t),xref(t))为期望状态代价,其公式为:
Ju(αu(t))为控制输入量代价,其公式为:
Jc(x(t)为碰撞代价,其公式为:
其中,T为水下机器人采样周期,f(x,u)是描述水下机器人连续非线性方 程,di(t)表示为与第i个水下障碍物的欧氏距离,ki>0为平滑避障轨迹的参 数;为期望状态代价项的权重系数矩阵,为控制输入量代 价的权重系数矩阵,Qc表示碰撞代价系数;U是控制量力与力矩可输入范围, 如下式所示:
水下机器人是欠驱动系统,则有fvmin=fvmax=0;fvmin为水下机器人横 向驱动力最小值,fvmax为水下机器人横向驱动力最大值,fumin为水下机器人 纵向驱动力的最小值,fumax为水下机器人纵向驱动力的最大值,frmin为水下 机器人艏摇力矩的最小值,frmax为水下机器人艏摇力矩最大值。
所述步骤(2)具体是指:
测量与计算水下机器人状态量x(t0),采用扩张状态观测器计算水下机器人 速度、外部扰动状态:
其中,l0至l8皆为正整数,b0,b1为补偿因子,ζ=u·t与χ=v·t分别代表 体坐标系下的纵向与横向位移;表示水下机器人艏摇角,u为水下机器人前 进速度,v为水下机器人横移速度,r为水下机器人艏摇速度;fu为控制输入纵 倾力,fr为偏航力矩;m11、m22、m33均为水下机器人的附加质量参数,d11、 d22、d33均为水动力系数;通过分别求解式(6)、(7)、(8),得出水下 机器人当前的纵向速度横向速度艏摇速度以及纵向横向艏摇三个方向外部扰动的估计值,“∧”表示计算估计值,“·”表示微分符号。
其中,为水下障碍物i的预测位置,Ts为采样周期,pobs i(t0)为水下 障碍物当前时刻的测量位置,vo-为水下障碍物测量先验速度,N为预测步数, t0为预测开始当下时刻,往后预测n个Ts采样周期;
(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态 进行预测;采用公式(9),通过声呐测量出的当前水下障碍物的位置,加上 水下障碍物测量先验速度乘上采样周期Ts,获得未来n个Ts周期内的水下障碍 物直线运动位置。
所述步骤(4)具体是指:利用求解器对最优化问题进行求解并获得最优 控制序列u*(t+nTs),n=1,...,N-1,并将最优控制序列首位转换作为水下 机器人波动鳍推进器的控制输入信号,u*表示通过求解器求解出来的最优控制 序列。
所述步骤(5)具体是指:如果声呐扫描周期结束,即水下障碍物的测量 状态量更新,则对卡尔曼滤波器的水下障碍物先验状态量与后验状态量Pt更新计算,并且对水下障碍物的不确定性rth,k(t)进行计算更新,否则卡尔曼滤 波器与水下障碍物不确定性不进行更新;
其中卡尔曼滤波器分为如下两大部分:
其中,xt=[xobs,yobs,vobs]T是水下障碍物的状态量,xobs,yobs为水下障 碍物的坐标位置,vobs为水下障碍物的速度,Zt=[xo,yo,vo]T是在t时刻的测 量的水下障碍物状态量,xo,yo为水下障碍物测量的坐标位置,vo为水下障碍 物的测量速度,F是状态转移矩阵,H是观测矩阵,Qt是水下障碍物运动噪声 协方差矩阵,Rt为观测噪声协方差矩阵,I为单位矩阵;
rth,i(t)=rth+10δi(t) (12)
其中,rth为常量参数。
仿真实验,本发明选择了单动态水下障碍物与多动态水下障碍物两种情况 进行仿真实验。参数设置:m11=57.5,m22=61.3,m33=1.15,d11=53,d22= 58,以及d33=3.1。
系统初始状态量均为0,额外扰动为:τd=[0.6*sin(0.075*t),0.5* sin(0.075*t),0.4*sin(0.075*t)]TN·m,非线性模型预测控制相关参数为: Ts=0.2s,Np=25,Nc=1,相关参数权重: Qx=diag{55.0,55.0,5.0,1.0,800,5.0}以及 Qu=diag{10.0,25.0},碰撞代价Qc=20,期望纵移速度为0.2m/s。 扩张状态观测器参数分别为:l1=3.6,l2=4.32,l3=1.728,l4=2.4,l5= 1.92,l6=1.516,l7=2.4,l8=1.92,l9=0.528。初始障碍物坐标 运动速度为-0.15m/s,并添加高斯分布变化为N(0,0.02)。
图3至图6展示了在添加外部扰动情况下进行路径跟踪与一个动态障碍物避 障时的关键时刻序列图,曲线为期望路径,小半径圆圈为水下机器人,大半径 圆圈为动态水下障碍物,圆圈上方的标记符号表示此时的偏航角,其中,图3为 t=35.0s,图4为t=37.2s,图5为t=46.0s,图6为t=52.6s,大圆圈紧邻着的表示 基于卡尔曼滤波的后验位置。在图3中可以看到,由于此时水下机器人仿真模型 的右侧前方有障碍物,因此它选择偏离左侧的预定轨迹以避免碰撞;值得注意 的是,在图4中,结合卡尔曼滤波中的先验信息预测水下障碍物轨迹时,选择向 右行驶,碰撞概率较低。图5、图6表示在避免动态水下障碍物之后,水下机器 人很快回归到期望路径。
在图7中,可以看出,没有扩张状态观测器的路径跟踪总是存在明显的误差, 而采用本发明可以实现无误差跟踪。另外,可以看到,当水下机器人能够访问 到水下障碍物预测的未来轨迹时,它会选择与水下障碍物相反的运动方向(正x 轴方向)来避免碰撞。
为了进一步验证所提发明算法的有效性,在相同实验场景下添加多水下障 碍物进行进一步验证。
表1多障碍物仿真实验数据
从图8至图13多水下障碍物仿真截图来看,本发明可以稳定的实现在具备外 部扰动条件下跟踪曲线跟踪的同时稳定的避开多障碍物。其中,图8为t=7s,图9为t=10s,图10为t=17s,图11为t=23s,图12为t=28s,图13为t=37.2s,在水下 机器人预定的期望轨迹附近添加多个动态水下障碍物,对于每个添加的水下障 碍物,连续进行10次模拟实验,场景实验结果如表1表示,在将卡尔曼滤波的先 验结果融合进入非线性模型预测控制的预测模块中,在较短周期内对障碍物的 位置进行预测之后,会降低发生碰撞的概率,以及加大与障碍物的最近距离, 大大提升避障的鲁棒性。在图8至图13中,水下机器人仿真模型的起点在坐标系 的原点(0,0);图8至图13中的曲线为全局跟踪路径,各个圆圈表示初始位置不同的动态水下障碍物。
综上所述,本发明对于仿生水下机器人由于独特的推进方式而导致模型参 数不确定性以及外部环境干扰,采用改进的扩张状态观测器进行扰动观测,并 将其作为非线性模型预测控制扰动项进行不断计算更新,大大提升水下机器人 抗干扰能力,运动的鲁棒性;本发明针对动态障碍物状态观测不确定性,即观 测误差,声呐扫描周期过长,通过结合卡尔曼滤波先验估计,对短周期内的障 碍物未来位置进行预测,使得水下机器人碰撞概率降低,缩短行驶距离且扩大 了与障碍物的安全距离。
Claims (6)
1.一种基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:该方法包括下列顺序的步骤:
(2)测量水下机器人当前的位置,读取艏摇角,通过扩张状态观测器计算出当前水下机器人的速度,估计出外部扰动,同时采用声呐对水下障碍物状态进行扫描,并将探测结果输入卡尔曼滤波器;
(3)利用建立的运动学动力学模型,根据当前水下机器人的速度和外部扰动,对水下机器人未来N个控制周期的运动状态进行预测,并同时预测水下障碍物的短时间内的轨迹;
(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障碍物的短时间内的轨迹,利用求解器对最优化问题进行求解并获得最优控制序列,并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号;
(5)如果声呐扫描周期结束,通过声呐测量的水下障碍运动状态更新,则卡尔曼滤波器的输入更新,计算卡尔曼滤波器新的先验状态量与后验状态量,并且对水下障碍物的不确定性同样进行更新计算,如果声呐扫描周期没有结束则卡尔曼滤波器与水下障碍物不确定性不进行更新,保持不变;
(6)重复步骤(2)至步骤(5),直至水下机器人进行路径跟踪且能完成避障。
2.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(1)具体是指:
建立水下机器人三自由度下的运动学动力学模型:
其中,为状态向量,V=[u r v]T表示水下机器人的速度,x和y代表大地坐标系下水下机器人的位置,表示水下机器人艏摇角,u为水下机器人前进速度,v为水下机器人横移速度,r为水下机器人艏摇速度;为控制输入量,fu为控制输入纵倾力,fv为横向驱动力,水下机器人为欠驱动系统,fv=0;fr为偏航力矩;τd=[du dv dr]T为水下机器人外部扰动,du为水下机器人前进方向扰动力,dv为水下机器人横移方向扰动力,dr为艏摇方向的外部环境扰动力矩,M为水下机器人惯性质量矩阵,D为水下机器人阻尼力对角矩阵,C(v)为向心力和科里奥利矩阵,其公式如下:
其中,m11、m22、m33均为水下机器人的附加质量参数,d11、d22、d33均为水动力系数;
建立最优化问题的目标函数:
其中,Jx(x(t),xref(t))为期望状态代价,其公式为:
Ju(Δu(t))为控制输入量代价,其公式为:
Jc(x(t)为碰撞代价,其公式为:
其中,T为水下机器人采样周期,f(x,u)是描述水下机器人连续非线性方程,di(t)表示为与第i个水下障碍物的欧氏距离,ki>0为平滑避障轨迹的参数;为期望状态代价项的权重系数矩阵,为控制输入量代价的权重系数矩阵,Qc表示碰撞代价系数;U是控制量力与力矩可输入范围,如下式所示:
水下机器人是欠驱动系统,则有fvmin=fvmax=0;fvmin为水下机器人横向驱动力最小值,fvmax为水下机器人横向驱动力最大值,fumin为水下机器人纵向驱动力的最小值,fumax为水下机器人纵向驱动力的最大值,frmin为水下机器人艏摇力矩的最小值,frmax为水下机器人艏摇力矩最大值。
3.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(2)具体是指:
测量与计算水下机器人状态量x(t0),采用扩张状态观测器计算水下机器人速度、外部扰动状态:
4.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(3)具体是指:利用水下机器人的运动学动力学模型对水下机器人未来状态进行计算预测得到并且通过水下障碍物直线运动模型对水下障碍物状态进行预测:
(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态进行预测;采用公式(9),通过声呐测量出的当前水下障碍物的位置,加上水下障碍物测量先验速度乘上采样周期Ts,获得未来n个Ts周期内的水下障碍物直线运动位置。
6.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(5)具体是指:如果声呐扫描周期结束,即水下障碍物的测量状态量更新,则对卡尔曼滤波器的水下障碍物先验状态量与后验状态量Pt更新计算,并且对水下障碍物的不确定性rth,k(t)进行计算更新,否则卡尔曼滤波器与水下障碍物不确定性不进行更新;
其中卡尔曼滤波器分为如下两大部分:
其中,xt=[xobs,yobs,vobs]T是水下障碍物的状态量,xobs,yobs为水下障碍物的坐标位置,vobs为水下障碍物的速度,Zt=[xo,yo,vo]T是在t时刻的测量的水下障碍物状态量,xo,yo为水下障碍物测量的坐标位置,vo为水下障碍物的测量速度,F是状态转移矩阵,H是观测矩阵,Qt是水下障碍物运动噪声协方差矩阵,Rt为观测噪声协方差矩阵,I为单位矩阵;
rth,i(t)=rth+10δi(t) (12)
其中,rth为常量参数。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211161234.5A CN115480580A (zh) | 2022-09-23 | 2022-09-23 | 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202211161234.5A CN115480580A (zh) | 2022-09-23 | 2022-09-23 | 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN115480580A true CN115480580A (zh) | 2022-12-16 |
Family
ID=84394367
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202211161234.5A Pending CN115480580A (zh) | 2022-09-23 | 2022-09-23 | 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN115480580A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115657689A (zh) * | 2022-12-26 | 2023-01-31 | 安徽大学 | 一种基于轨迹预测的自主水下航行器目标跟踪控制方法 |
CN118534781A (zh) * | 2024-07-26 | 2024-08-23 | 西南石油大学 | 一种欠驱动波动鳍水下机器人的自适应反步滑模控制方法 |
-
2022
- 2022-09-23 CN CN202211161234.5A patent/CN115480580A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN115657689A (zh) * | 2022-12-26 | 2023-01-31 | 安徽大学 | 一种基于轨迹预测的自主水下航行器目标跟踪控制方法 |
CN118534781A (zh) * | 2024-07-26 | 2024-08-23 | 西南石油大学 | 一种欠驱动波动鳍水下机器人的自适应反步滑模控制方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN107168312B (zh) | 一种补偿uuv运动学和动力学干扰的空间轨迹跟踪控制方法 | |
Sun et al. | Mapless motion planning system for an autonomous underwater vehicle using policy gradient-based deep reinforcement learning | |
CN108427414B (zh) | 一种自主水下航行器水平面自适应轨迹跟踪控制方法 | |
CN111650948B (zh) | 一种可底栖式auv的水平面轨迹快速跟踪控制方法 | |
Hadi et al. | Deep reinforcement learning for adaptive path planning and control of an autonomous underwater vehicle | |
CN109765929B (zh) | 一种基于改进rnn的uuv实时避障规划方法 | |
CN105807789B (zh) | 基于t-s模糊观测器补偿的uuv控制方法 | |
CN115480580A (zh) | 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 | |
CN108829099A (zh) | 一种基于受限反步法控制的欠驱动无人船轨迹跟踪方法 | |
CN113093804B (zh) | 基于反演滑模控制的无人艇编队控制方法和控制系统 | |
CN113467231B (zh) | 基于侧滑补偿ilos制导律的无人艇路径跟踪方法 | |
CN109857124A (zh) | 基于速度与航向双重制导的无人船精确路径跟踪控制方法 | |
CN111240345A (zh) | 一种基于双bp网络增强学习框架的水下机器人轨迹跟踪方法 | |
CN111930141A (zh) | 一种水下机器人三维路径可视化跟踪方法 | |
CN114115262B (zh) | 基于方位角信息的多auv执行器饱和协同编队控制系统和方法 | |
CN111857165B (zh) | 一种水下航行器的轨迹跟踪控制方法 | |
CN112068440B (zh) | 基于模型预测控制的auv回收对接动力定位控制方法 | |
CN113741433B (zh) | 一种水面无人船的分布式编队方法 | |
CN115657689B (zh) | 一种基于轨迹预测的自主水下航行器目标跟踪控制方法 | |
CN117311142A (zh) | 一种融合粒子群算法与神经网络预测控制的dp船舶运动控制与推力分配协同控制方法 | |
CN114967714A (zh) | 一种自主式水下机器人抗扰运动控制方法及系统 | |
CN114397899A (zh) | 一种仿生机器鱼三维路径跟踪控制方法及装置 | |
CN113485389A (zh) | 一种基于自适应预测的水下机器人三维轨迹跟踪控制方法 | |
CN117111614A (zh) | 一种基于改进人工势场法的船舶编队避碰避障与跟踪控制方法 | |
CN113110512B (zh) | 一种减弱未知干扰与抖振影响的可底栖式auv自适应轨迹跟踪控制方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |