[发明专利]一种基于改进角度势场法的无人船自主避障方法

权利要求书

1.一种基于改进角度势场法的无人船自主避障方法，其特征在于，包括以下步骤：

步骤一：根据用户输入的目标点顺序，获取第一个目标点信息；

步骤二：通过IMU获取当前无人船位置和航向信息；

步骤三：根据雷达获取当前位置的环境信息进行环境建模；

步骤四：根据改进角度势场法避障航行直到航行至当前目标点；

步骤五：根据用户输入的目标点顺序，更新目标点位置信息，继续航行，直到航行至最终目标点。

2.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述步骤一具体为：

用户根据实际需求，按照顺序依次输入目标点的位置信息；

根据用户输入的目标点顺序，获取第一个目标点位置信息，并设为当前航行目标点。

3.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，步骤二所述IMU集成AHRS和GPS功能，所述AHRS通过测量三轴方向上的加速度、角速率和磁场，获知无人船当前的三维姿态；所述GPS提供自身位置信息；AHRS和GPS功能的结合用于获取无人船实时位置和行进方向。

4.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述步骤三具体为：

激光雷达可获取当前位置360度的环境信息，所述环境信息包括障碍物所在角度以及障碍物与当前位置相距的距离；

将激光雷达提供的环境信息根据当前航向转化为无人船视角范围内的障碍物距离、角度信息。

5.根据权利要求1所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述步骤四具体为：

根据环境信息进行可航行空间判断；

若当前位置存在可航行空间，则根据环境信息计算通行程度，确定输出角度，根据无人船特性确定待转向角度，使用PID算法进行舵机角度调整航行；若无人船当前位置不存在可航行空间，则进行反向避障；

直到到达当前目标点。

6.根据权利要求5所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述通行程度K_通行计算公式如下：

式中，K_引(θ)、K_阻(θ)分别表示角度θ受到的引力和阻力；

所述输出角度为最大通行程度所在的角度；

所述待转向角度确定方法为：若输出角度大于无人船1秒内转向角度δ，则待转向角度为δ，若输出角度小于δ，则待转向角度为输出角度。

7.根据权利要求6所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述K_引(θ)、K_阻(θ)的计算公式如下：

K_引(θ)＝cos(θ-θ_obj) (12)

式中，为对于角度的障碍点在角度θ产生的阻力，θ_obj为目标点相对于当前位置的方位角。

8.根据权利要求5所述的一种基于改进角度势场法的无人船自主避障方法，其特征在于，所述PID算法为增量式PID，其输出Δu[n]计算公式如下：

Δu[n]＝k_p×{e[n]-e[n-1]}+k_i×e[n]+k_d×{e[n]-2e[n-1]+e[n-2]} (4)

式中k_p表示比例系数，k_i表示积分时间常数，k_d表示微分时间常数，e[n]、e[n-1]、e[n-2]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入；通过三个时刻的船航行与目标航行的差值作为输入得出舵机转向输出，不断调整转向角度。