1.一种基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:该方法包括下列顺序的步骤:(1)建立水下机器人的运动学动力学模型,建立最优化问题的目标函数,设定全局路径参考点,利用贝塞尔曲线插值获得全局跟踪路径 计算得到艏摇角;
(2)测量水下机器人当前的位置,读取艏摇角,通过扩张状态观测器计算出当前水下机器人的速度,估计出外部扰动,同时采用声呐对水下障碍物状态进行扫描,并将探测结果输入卡尔曼滤波器;
(3)利用建立的运动学动力学模型,根据当前水下机器人的速度和外部扰动,对水下机器人未来N个控制周期的运动状态进行预测,并同时预测水下障碍物的短时间内的轨迹;
(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障碍物的短时间内的轨迹,利用求解器对最优化问题进行求解并获得最优控制序列,并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号;
(5)如果声呐扫描周期结束,通过声呐测量的水下障碍运动状态更新,则卡尔曼滤波器的输入更新,计算卡尔曼滤波器新的先验状态量与后验状态量,并且对水下障碍物的不确定性同样进行更新计算,如果声呐扫描周期没有结束则卡尔曼滤波器与水下障碍物不确定性不进行更新,保持不变;
(6)重复步骤(2)至步骤(5),直至水下机器人进行路径跟踪且能完成避障。
2.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(1)具体是指:建立水下机器人三自由度下的运动学动力学模型:
T
其中, 为状态向量,V=[u r v] 表示水下机器人的
速度,x和y代表大地坐标系下水下机器人的位置, 表示水下机器人艏摇角,u为水下机器人前 进 速度 ,v为 水 下机 器人 横 移速 度 ,r 为 水下 机 器人 艏 摇速 度 ;
为控制输入量,fu为控制输入纵倾力,fv为横向驱动力,水下机T
器人为欠驱动系统,fv=0;fr为偏航力矩;τd=[du dv dr]为水下机器人外部扰动,du为水下机器人前进方向扰动力,dv为水下机器人横移方向扰动力,dr为艏摇方向的外部环境扰动力矩,M为水下机器人惯性质量矩阵,D为水下机器人阻尼力对角矩阵,C(v)为向心力和科里奥利矩阵,其公式如下:其中,m11、m22、m33均为水下机器人的附加质量参数,d11、d22、d33均为水动力系数;
是将体坐标系下的水下机器人横移速度v转换为大地坐标系速度 的旋转矩阵,具体为:建立最优化问题的目标函数:
subject to u(t)∈U,x(0)=x(t0) (4)其中,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),采用扩张状态观测器计算水下机器人速度、外部扰动状态:其中,l0至l8皆为正整数,b0,b1为补偿因子, 与χ=v·t分别代表体坐标系下的纵向与横向位移; 表示水下机器人艏摇角,u为水下机器人前进速度,v为水下机器人横移速度,r为水下机器人艏摇速度;fu为控制输入纵倾力,fr为偏航力矩;m11、m22、m33均为水下机器人的附加质量参数,d11、d22、d33均为水动力系数;通过分别求解式(6)、(7)、(8),得出水下机器人当前的纵向速度 横向速度 艏摇速度 以及纵向 横向 艏摇 三个方向外部扰动的估计值,“∧”表示计算估计值,“·”表示微分符号。
4.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(3)具体是指:利用水下机器人的运动学动力学模型对水下机器人未来状态进行计算预测得到 并且通过水下障碍物直线运动模型对水下障碍物状态进行预测:
其中, 为水下障碍物i的预测位置,Ts为采样周期,pobs i(t0)为水下障碍物当前时o‑刻的测量位置,v 为水下障碍物测量先验速度,N为预测步数,t0为预测开始当下时刻,往后预测n个Ts采样周期;
(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态进行预测;采用公式(9),通过声呐测量出的当前水下障碍物的位置,加上水下障碍物测量先验速度乘上采样周期Ts,获得未来n个Ts周期内的水下障碍物直线运动位置。
5.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征*在于:所述步骤(4)具体是指:利用求解器对最优化问题进行求解并获得最优控制序列u (t+nTs),n=1,...,N‑1,并将最优控制序列首位 转换作为水下机器人波动鳍推进器的控制*输入信号,u表示通过求解器求解出来的最优控制序列。
6.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法,其特征在于:所述步骤(5)具体是指:如果声呐扫描周期结束,即水下障碍物的测量状态量更新,则对卡尔曼滤波器的水下障碍物先验状态量 与后验状态量Pt更新计算,并且对水下障碍物的不确定性rth,k(t)进行计算更新,否则卡尔曼滤波器与水下障碍物不确定性不进行更新;
其中卡尔曼滤波器分为如下两大部分:
预测:
更新:
T
其中,xt=[xobs,yobs,vobs]是水下障碍物的状态量,xobs,yobs为水下障碍物的坐标位置,o o o T o ovobs为水下障碍物的速度,Zt=[x ,y ,v] 是在t时刻的测量的水下障碍物状态量,x ,y 为o水下障碍物测量的坐标位置,v 为水下障碍物的测量速度,F是状态转移矩阵,H是观测矩阵,Qt是水下障碍物运动噪声协方差矩阵,Rt为观测噪声协方差矩阵,I为单位矩阵;
通过求解计算 最大特征值δi(t),通过公式(12)描述第i个水下障碍物在t时刻的不确定性:
rth,i(t)=rth+10δi(t) (12)其中,rth为常量参数。