[发明专利]一种多AUV动态圆弧编队控制方法

摘要

本发明提供一种多AUV动态圆弧编队控制方法，是一种在已知路径下的多自主水下航行器(Autonomous Underwater Vehicles,AUV)动态圆弧编队控制策略，属于船舶控制技术领域。本发明提出了一种新的实现多AUV圆弧编队控制策略，将领航者与跟随者的位姿关系分为相对于坐标轴原点的距离与角度关系，构建姿态角、艏向角、速度误差模型，通过PID控制器控制编队中每个AUV实现圆弧编队运动；跟随者实时接收且只需接收领航者的位姿信息，减少了AUV在多种信息通讯时出现的延时、丢失等不利现象，增强了编队控制的稳定可靠性。

主权项

1.一种多AUV动态圆弧编队控制方法，其特征在于：步骤如下：步骤1：获得领航者和跟随者需跟踪的圆弧路径；步骤2：建立参考坐标系建立表达AUV运动的固定参考坐标系，以步骤1中获取的圆弧路径的圆心为固定参考坐标系的原点O；任意选取圆弧的一条直径所在的直线为坐标轴的纵轴，记为X轴；垂直于该直径且经过圆弧圆心的直线为坐标轴的横轴，记为Y轴；AUV重心为G，速度向量为V_t，在固定坐标系下X轴和Y轴上投影为AUV受到的外力F在固定坐标系下X轴和Y轴方向上的分力为F_X,F_Y；受到的外力矩为T；建立表达AUV运动的运动坐标系，原点取在AUV重心G处；纵轴取在AUV纵中剖面内，指向船首，记为x轴；横轴与AUV纵中剖面垂直，指向右舷，记为y轴；速度向量为Vt，在运动坐标系下x轴和y轴上投影为u,v；AUV受到的外力F在固定坐标系下x轴和y轴方向上的分力为Fx,Fy；受到的外力矩为T；因外力矩而获得的角速度为r；步骤3：建立AUV数学模型固定坐标系的X轴与运动坐标系x轴的夹角为艏向角，记为ψ；AUV速度Vt与固定坐标系X轴的夹角为φ；假设u恒不为0，定义侧滑角β，则：β＝φ‑ψ＝arctan(v/u)得到AUV的动力学模型为：式中：为ψ和φ的导数，V_t为AUV的合速度，且在运动坐标系中，实际流体有黏性，会出现流体惯性阻力效应，相当于AUV质量增加，针对欠驱动AUV，只会在AUV纵向受到水平推力FT和垂直水平面方向转艏力矩Γ，AUV运动时会受到其它因素在水平面对AUV的阻力Fw和垂直水平面方向的力矩Nw，其中Fw在x轴和y轴上的分力为Fxw,Fyw：则AUV在实际情况下的动力学模型为：式中：mu；mvr；mv；mur；mr分别为实际运动中的质量；步骤4：由圆弧视线导引法推导AUV期望艏向角在圆弧路径跟踪中，pk(xk,yk)为AUV到圆心连线与圆弧的交点，p(xt,yt)为AUV的实时位置坐标，ψ(t)为AUV的实时艏向角，e为路径跟踪的横向误差，plos(xlos,ylos)为视线引导法生成的前视点，ψd为AUV期望角，Δ为选取的前视向量pkplos的长度，α(t)为AUV期望运行方向与有向线段Op的夹角，θ为有向线段Op与北向坐标轴的夹角。由几何关系得：θ＝arctan(yt,xt)ψd＝θ‑α(t)式中：α(t)＝arctan(|e|/Δ)；步骤5：构建固定坐标系下的跟随者AUV的误差模型在固定坐标系中，假定AUVi为领航者，其它AUV为跟随者，在编队队形中AUVi将其位姿纵坐标通过水声通信传感器发送给跟随者，跟随者实时采集领航者的纵向坐标信息，AUVi在固定坐标系下的位姿为(Xi,Yi,ψi)；第j个跟随者的坐标为(Xj,Yj,ψj)；当多AUV实现编队后，领航者将跟随步骤1所规划的圆弧路径，半径为R_i，即而跟随者将在圆弧路径所在的同心圆弧上运动，半径为R_j，即其中D_j＝R_i‑R_j则得到以下误差模型：(1)姿态角角度误差定义AUV姿态角θ＝arctan(Y/X)，其中X,Y为AUV在步骤2中固定参考坐标系下的坐标，则有：领航者AUV姿态角θi＝arctan(Yi/Xi)，跟随者AUV姿态角θj＝arctan(Yj/Xj)则对于领航者的位姿角度误差为0；而第j个跟随者的位姿角度误差θej：θej＝θi‑θj‑θsj式中：θsj为第j个跟随者AUV与领航者AUVi的理想姿态角之差；(2)艏向角误差领航者与第j个跟随者的距离误差Ej为：又有：式中：ei和ej可以看作领航者和第j个跟随者AUV路径跟踪中的误差。将ei和ej带入Ej:Ej＝ej‑ei当时，有所以此时领航者和跟随者均做圆弧运动，则将领航者和第j个跟随者AUV路径跟踪中的误差E_j转化为AUV的路径跟踪误差e_j,e_i；而步骤4中圆弧视线导引法又通过将路径跟踪误差得到领航者和第j个跟随者期望艏向角ψdi,ψdj，所以领航者和第j个跟随者的艏向角误差分别为ψei,ψej，且有：ψei＝ψdi‑ψi,ψej＝ψdj‑ψj其中ψi,ψj分别为领航者和第j个跟随者的当前艏向角：(3)速度误差领航者AUVi的速度误差uei为：uei＝ui‑ud式中：ud为领航者期望速度，ui为AUV当前速度；对于跟随者AUVj：uej＝uj‑ucj式中：ucj为为第j个跟随者AUV期望速度，uj为第j个跟随者AUV当前速度；而：ucj＝(Rj/Ri)*ud‑g(θej)在这里函数g(·)在定义域[‑π,π]内满足如下条件：设定：其中a满足umin,umax为AUV的最小速度和最大速度，u为AUV速度；步骤6：AUV运动控制器的设计使步骤5中的3个误差趋于0，则实现多AUV之间的圆弧编队；定义K_p1、K_i1、K_d1π分别为艏向角控制的比例、积分、微分参数，K_p、K_i、K_d分别为速度控制比例、积分，微分参数；以k时刻设计领航者和跟随者的运动控制器：ψei(k)、ψei(k‑1)、ψei(k‑2)分别为领航者AUV在k时刻的艏向角误差、(k‑1)时刻艏向角误差、(k‑2)时刻艏向角误差；uei(k)、uei(k‑1)、uei(k‑2)分别为领航者AUV在k时刻速度误差、(k‑1)时刻的速度误差、(k‑2)时刻的速度误差；ψej(k)、ψej(k‑1)、ψej(k‑2)分别为第j个跟随者AUV在k时刻的艏向角误差、(k‑1)时刻艏向角误差、(k‑2)时刻艏向角误差；uej(k)、uej(k‑1)、uej(k‑2)分别为第j个跟随者AUV在k时刻速度误差、(k‑1)时刻的速度误差、(k‑2)时刻的速度误差；Γi(k‑1)、Γi(k)分别为领航者AUV在(k‑1)时刻、k时刻的转艏力矩；Γj(k‑1)、Γj(k)为第j个跟随者AUV(k‑1)时刻、k时刻的转艏力矩；FTi(k‑1)、FTi(k)为领航者AUV在(k‑1)时刻、k时刻的水平推力；FTj(k‑1)、FTj(k)为第j个跟随者AUV(k‑1)时刻、k时刻的水平推力，则有：(1)艏向角误差距离误差可由步骤4的圆弧视线导引法，通过寻找前向距离，使AUV的距离误差变为艏向角误差，而艏向角可以通过改变领航者AUV的转艏力矩Γi(k)和第j个跟随者AUV的转艏力矩Γj(k)来控制：Γi(k)＝Γi(k‑1)+K_p1*(ψei(k)‑ψei(k‑1))+K_i1*ψei(k)+K_d1*(ψei(k)‑2*ψei(k‑1)+ψei(k‑2))Γj(k)＝Γj(k‑1)+K_p1*(ψej(k)‑ψej(k‑1))+K_i1*ψej(k)+K_d1*(ψej(k)‑2*ψej(k‑1)+ψej(k‑2))其中当k≤0时Γi(k)、ψei(k)、Γi(k)、ψej(k)都为0，(2)角度误差和速度误差角度误差和速度误差均可以通过AUV速度控制来减小，通常会给AUV一个期望速度ud；通过控制领航者AUV纵向受到水平推力FTi(k)和第j个跟随者AUV纵向受到水平推力FTj(k)来控制AUV速度，FTi(k)＝FTi(k‑1)+K_p*(uei(k)‑uei(k‑1))+K_i*uei(k)+K_d*(uei(k)‑2*uei(k‑1)+uei(k‑2))FTj(k‑1)＝FTj(k‑1)+K_p*(uej(k)‑uej(k‑1))+K_i*uej(k)+K_d*(uej(k)‑2*uej(k‑1)+uej(k‑2))其中当k≤0时，FTi(k)、uei(k)、FTi(k)、uej(k)都为0。