1.基于声光联合导引重型auv动/静态目标自主对接方法,其特征在于,包括以下步骤:
1)基阵位置搜索阶段:auv搜寻目标上的基阵发出的声学定位数据,确定目标位置;
2)向对接航线起点航行阶段:根据目标位置规划对接航线,向对接航线起点航行;
3)远距离导引阶段:根据目标位置调整对接航线,使auv沿对接航线航行;
4)末端距离导引阶段:使用声学定位数据,提高目标位置更新频率,当收到光学定位数据时,则使用光学导引使auv到达目标位置。
2.根据权利要求1所述的基于声光联合导引重型auv动/静态目标自主对接方法,其特征在于,所述确定目标位置具体为:
x_d=-(cos(h)(cos(p)*x_z-sin(p)(sin(r)*y_z+cos(r)*z_z))-sin(h)(cos(r)*y_z-sin(r)*z_z))
y_d=-(sin(h)(cos(p)*x_z-sin(p)(sin(r)*y_z+cos(r)*z_z))+cos(h)(cos(r)*y_z-sin(r)*z_z))
z_d=sin(p)*x_z+cos(p)*(sin(r)*y_z+cos(r)*z_z)
其中,h、p、r分别表示auv的航向、纵倾和横滚角度,x_z、y_z和z_z分别为auv在载体坐标系下的北向、东向和垂向距离,x_d、y_d和z_d分别为目标在大地坐标系下的东向、北向和垂向距离。
3.根据权利要求1所述的基于声光联合导引重型auv动/静态目标自主对接方法,其特征在于,所述根据目标位置调整对接航线,使auv沿对接航线航行,具体为:根据auv当前航迹偏航角与目标航迹偏航角的差值,得到航向偏差;将偏差与auv当前航向求和得到目标航向,根据目标航向更新对接航线,auv沿更新后的对接航线航行。
4.根据权利要求3所述的基于声光联合导引重型auv动/静态目标自主对接方法,其特征在于,所述根据auv当前航迹偏航角与目标航迹偏航角的差值,得到航向偏差,具体为:
其中,δhdis为航迹偏航角,kp、ki、kd分别为比例系数、积分系数、微分系数,δδdis为当前auv与目标航线距离值;
航向偏差δh′如下:
δh′=δhdock-δhdis
其中,δh′为航向偏差,由auv当前航向与目标轴线夹角δhdock与航迹偏航角δhdis计算得出。
5.根据权利要求1所述的基于声光联合导引重型auv动/静态目标自主对接方法,其特征在于,步骤4)中,当auv获取到光学导引数据后,使用光学导引数据更新目标位置。