[发明专利]一种基于改进的人工协调场的气垫船避障方法

权利要求书

1.一种基于改进的人工协调场的气垫船避障方法，其特征是：

(1)获取气垫船状态信息、目标点位置以及障碍物位置；

所述气垫船状态信息包括当前时刻位置(x,y)、速度(u,v)、艏向ψ以及回转率r，目标点位置用(x_g,y_g)表示，障碍物位置用(x_o,y_o)表示；

(2)根据气垫船状态和目标点位置设计PID控制律，代替人工协调场中的吸引力；

吸引力F_a为：

Fa=Kp(Pg-P)+Ki∫(Pg-P)dt-KdP·]]>

其中，P_g＝[x_g,y_g]^T，P＝[x,y]^T，U＝[u,v]^T，K_p,K_i,K_d为2×2的增益矩阵；

(3)根据气垫船状态信息和障碍物位置计算风险度，若风险度大于设定的风险度则进入步骤(4)，否则进入步骤(6)；

风险度risk为，

risk=d+ϵ||Po-P||]]>

其中d为设定的障碍物与气垫船安全距离，ε为设定的安全余量；

(4)设计斥力场，并利用气垫船的最大加速度约束设计斥力场的场强参数，进而计算出斥力；

斥力F_r为：

F_r＝k_r(P-P_o)

其中，P_o＝[x_o,y_o]^T；

场强参数k_r如下计算：

kr=1(pos(||P-Po||-ϵ-(pos(Uor·e(Po-P)))2/2amax))m]]>

其中，m≥1，pos(x)＝max(x,0)，U_or为气垫船相对于障碍物的相对速度，因为这里考虑的是静态障碍物，因此有U_or＝U，e(x)表示向量x的单位向量，a_max表示气垫船的最大加速度；

(5)根据斥力计算出协调力；

协调力F_n为：

F_n＝k_n*λ*F_r

其中，k_n为标量的可调参数，λ的作用就是产生垂直于斥力的协调力，避免局部极小值的出现；

(6)将吸引力、排斥力以及协调力合成，算出大地坐标下的合力，然后将所述合力的方向作为气垫船的期望艏向设计艏向PID控制律，将所述合力的大小作为气垫船的纵向控制力对气垫船进行控制；

大地坐标下的合力为F为：

F=Fa+Fr+Fn,ifrisk>r_dFa,else]]>

设二维合力F＝[f1,f2]^T，f1,f2分别为合力在大地坐标下的北向分力和东向分力，则气垫船的期望艏向ψ_d：

ψd=arctan(f2f1)]]>

气垫船的回转控制力矩τ_n根据PID计算为

τ_n＝k_p(ψ_d-ψ)+k_i∫(ψ_d-ψ)dt+k_d(r_d-r)

其中，k_p,k_i,k_d为标量的可调PID参数，ψ_d为期望的艏向，为期望的艏向回转率；

气垫船的纵向控制力τ_x为

τx=f1×f1+f2×f2.]]> 2