[发明专利]基于惯性测量单元与GPS的自动导航方法

权利要求书

1.基于惯性测量单元与GPS的自动导航方法，其特征在于，包括如下步骤：

步骤一：根据待监测区域选择监测点；

步骤二：根据监测点获取所有监测点坐标；

步骤三：根据监测点先后关系，选取第一个监测点作为目标点，根据目标点与当前航行位置坐标确定目标点相对于当前航行位置的距离和方位角；

步骤四：基于确定的距离和方位角，采用导航算法进行航向优化，直到到达目标点，选取下一个监测点作为目标点，继续航行，直到达到最终目标点。

2.如权利要求1所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：步骤一中，在待选择区域根据所需的监测情况，按照距离放置船位置的远近依次设定监测点。

3.如权利要求1所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：步骤二中，根据用户设定的监测点，通过地图获取这些监测点的坐标，并将监测点的坐标作为无人船航行的目标点输出到无人船控制中心。

4.如权利要求1所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：步骤三中，设定无人船到达目标点一定距离即为到达目标点，目标点周围一定距离即为目标点区域，根据无人船的实时位置判断无人船是否到达目标点区域，若无人船到达目标点区域则获取下一监测点作为目标点，并使无人船向着新目标点航行。

5.如权利要求1所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：步骤三中，假定从IMU与GPS获取的当前位置信息为p_n(w_n，j_n)，目标点信息为T(w_t，j_t)，则目标点相对于当前位置的距离为：

6.如权利要求1所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：步骤三中，假定从IMU与GPS获取的当前位置信息为p_n(w_n，j_n)，目标点信息为T(w_t，j_t)，则目标点相对于当前位置的方位角为：

7.如权利要求6所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：步骤四中，假定从IMU获取当前无人船的航向为angleBoat，则无人船需要偏转的角度为：

angleTurn＝angle-angleBoat

当求得的偏转角度的值大于180度或小于-180度时，从反方向进行转向；获取需要偏转的角度后，不断调整船体的方向，寻求航向与目标点方位角一致地向目标点行进，并根据无人船实时位置到目标点的距离判断是否到达目标点，直到到达目标点；选取下一个监测点作为目标点，继续航行，直到达到最终目标点。

8.如权利要求7所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：不断调整船体的方向具体为：通过调整舵机方向实现船头方向的调整，通过树莓派控制实现舵机方向的调整；综合PID算法与PWM进行航行控制，由PID算法确定转向角度，PWM根据转向角度输出指定波形的PWM波控制舵机旋转，从而控制舵机旋转角度寻求目标点航向一致。

9.如权利要求8所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：采用PWM进行航行控制具体为：在使用PWM时先创建一个PWM实例，之后通过占空比来启动PWM，其中占空比是指高电平占周期的百分比，通过ChangeDutyCycle方法来改变占空比，调整占空比改变PWM波形；舵机通过舵机标识符duojiflag.value判断左右转向，当duojiflag.value＝1时，舵机向右偏，无人船右转；当duojiflag.value＝-1时，舵机向左偏，无人船左转；当duojiflag.value＝0时，舵机位置居中，无人船直线行驶。

10.如权利要求8所述的基于惯性测量单元与GPS的自动导航方法，其特征在于：采用PID进行航行控制具体为：使用增量式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]}

式中k_p表示比例系数，k_i表示积分时间常数，k_d表示微分时间常数，e[n]、e[n-1]、e[n-2]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入；

通过三个时刻的船航向与目标航向的差值作为输入，得出舵机转向输出，不断调整转向角度。