CN109839932A - 基于惯性测量单元与gps的自动导航方法 - Google Patents

Abstract

基于惯性测量单元与GPS的自动导航方法，包括：根据待监测区域选择监测点；根据监测点获取监测点坐标；根据监测点先后关系，选取第一个监测点作为目标点，根据目标点与当前航行位置坐标确定目标点相对于当前航行位置的距离和方位角；通过惯性测量单元获取航向后，再结合目标点的方位角，根据角度偏转函数计算出船体需要偏转的角度；根据导航算法进行航向优化，通过PID算法不断调整舵机旋转的角度，寻求航向与目标点方位角一致，直到到达目标点；选取下一个监测点作为目标点，继续航行，直到达到最终目标点。本发明用于监测区域的自动监测，根据设定的监测点进行最优路线航行，电池的利用更加合理化，更加方便快捷地进行监测，定位更加精确。

Description

基于惯性测量单元与GPS的自动导航方法

技术领域

本发明属于水环境监测领域，具体涉及一种基于惯性测量单元与GPS的自动导航方法。

背景技术

近20年来，受中国经济粗放式增长和人口增加等多种因素影响，水环境恶化趋势加剧、水污染事件频发，对经济发展和社会稳定构成严重威胁。松花江硝基苯泄露、太湖蓝藻、镇江货轮苯酚泄露等事件引起国内外广泛关注。对与人类生活密切相关的水环境进行监测可掌握水体污染物分布状况，探明各种污染物质，追溯污染物来源、途径、迁移转化和消长规律，预测水体污染变化趋势，为水环境质量评价和采取科学防治措施提供基础数据和决策依据。因此，水环境监测是水环境治理中不可缺少的一个重要环节。

随着信息技术发展，由监控中心和若干监测子站组成的水环境自动监测系统可对监测区域进行24小时实时监测，得到广泛应用，先后在太湖流域、滇池流域、巢湖、洱海、松花江等重点区域建成了水环境监控和预警系统。这些系统在实时监测水环境变化、及时发现污染事件和污染治理评价等方面发挥了重要作用，但也暴露出建设和运维成本高(建设一个水环境自动监测站点需数百万元，每个站点每年的运维费用也需约10万元)、监测站点位置固定、监测区域有限等不足。因此，国内外展开了遥控船的研究，手动控制船体驶向待监测点，到达待监测点后再手动控制船体采集水质，采集完水质后再手动控制船体驶向下一个待监测点，到达待监测点后再采集水质，如此循环往复后，当采集完最后一个待监测点的水质后，仍需手动将船体返回到站点。手动控制船体不仅耗时耗力，而且需要监测人员实时通过摄像头来观察船体的周围情况，看看是否到达目标点。由此可见，采集一次数据是很不方便的。因此我们开展了船体自动导航系统的研究，以降低监测时操作的复杂度并提高监测能力。研究人员引入多目标优化算法，将船头所需调转的方向作为优化目标，结合椭球几何两点间角度算法，椭球几何两点间距离算法、舵机智能控制算法、电机智能控制算法对自动导航系统进行优化。多目标优化算法综合考虑多个优化目标，目标在多次优化下得到最优值，取得了较好的优化结果。

发明内容

本发明针对现有技术中的不足，提供一种基于惯性测量单元与GPS的自动导航方法。

为实现上述目的，本发明采用以下技术方案：

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

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

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

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

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

为优化上述技术方案，采取的具体措施还包括：

优选地，步骤一中，在待选择区域根据所需的监测情况，按照距离放置船位置的远近依次设定监测点。

优选地，步骤二中，根据用户设定的监测点，通过地图获取这些监测点的坐标，并将监测点的坐标作为无人船航行的目标点输出到无人船控制中心。

优选地，步骤三中，设定无人船到达目标点一定距离即为达到目标点，目标点周围一定距离即为目标点区域，根据无人船的实时位置判断无人船是否到达目标点区域，若无人船到达目标点区域则获取下一监测点作为目标点，并使无人船向着新目标点航行。

优选地，步骤三中，假定从IMU与GPS获取的当前位置信息为p_n(w_n，j_n)，目标点信息为T(w_t，j_t)，则目标点相对于当前位置的距离为：

优选地，步骤三中，假定从IMU与GPS获取的当前位置信息为p_n(w_n，j_n)，目标点信息为T(w_t，j_t)，则目标点相对于当前位置的方位角为：

优选地，步骤四中，假定从IMU获取当前无人船的航向为angleBoat，则无人船需要偏转的角度为：

angleTurn＝angle-angleBoat

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

优选地，不断调整船体的方向具体为：通过调整舵机方向实现船头方向的调整，通过树莓派控制实现舵机方向的调整；综合PID算法与PWM进行航行控制，由PID算法确定转向角度，PWM根据转向角度输出指定波形的PWM波控制舵机旋转，从而控制舵机旋转角度寻求目标点航向一致。

优选地，采用PWM进行航行控制具体为：在使用PWM时先创建一个PWM实例，之后通过占空比来启动PWM，其中占空比是指高电平占周期的百分比，通过ChangeDutyCycle方法来改变占空比，调整占空比改变PWM波形；舵机通过舵机标识符duojiflag.value判断左右转向，当duojiflag.value＝1时，舵机向右偏，无人船右转；当duojiflag.value＝-1时，舵机向左偏，无人船左转；当duojiflag.value＝0时，舵机位置居中，无人船直线行驶。

优选地，采用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表示比例系数，ki表示积分时间常数，k_d表示微分时间常数，e[n]、e[n-1]、e[n-2]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入；

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

本发明的有益效果是：本发明使用于针对监测区域的自动监测，可以根据设定的监测点进行最优路线航行，使电池的利用更加合理化，更加方便快捷地进行监测，定位更加精确。

附图说明

图1是本发明的基于惯性测量单元与GPS的自动导航方法的流程图。

图2是目标点区域航行示意图。

图3是某一位置目标点方位角与当前航向示意图。

图4是导航系统的硬件连线图。

具体实施方式

现在结合附图对本发明作进一步详细的说明。

如图1所示的基于惯性测量单元与GPS的自动导航方法，包含以下步骤：

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

在待选择区域根据所需的监测情况，按照距离放置船位置的远近依次设定监测点。

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

根据用户设定的监测点，通过地图获取这些监测点的坐标，并将监测点的坐标作为无人船航行的目标点输出到无人船控制中心。

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

由于GPS存在一定误差，所以设定无人船到达目标点一定距离即为达到目标点，目标点周围一定距离即为目标点区域。根据无人船的实时位置判断无人船是否到达目标点区域，若无人船到达目标点区域则获取下一目标点，并使无人船向着该目标点航行。如图2所示，圆代表目标点区域，无人船进入圆内即为达到目标点，随后向下一目标点前进。

假定从惯性测量单元与GPS获取的当前位置信息为p_n(w_n，j_n)，目标点信息为T(w_t，j_t)，则目标点相对于当前位置的距离为：

目标点相对于当前位置的方位角为：

惯性测量单元(IMU)是测量物体三轴姿态角(或角速率)以及加速度的装置。一般的，一个IMU包含了三个单轴的加速度计和三个单轴的陀螺，加速度计检测物体在载体坐标系统独立三轴的加速度信号，而陀螺检测载体相对于导航坐标系的角速度信号，测量物体在三维空间中的角速度和加速度，并以此解算出物体的姿态。IMU与树莓派的USB接口直接相连，12V电源通过12V-5V变压器变压后为树莓派供电，IMU与树莓派的硬件连接图如图4所示。

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

假定从IMU获取当前无人船的航向为angleBoat，则无人船需要偏转的角度为：

angleTurn＝angle-angleBoat (3)

如图3所示，左边的有向射线为当前位置与目标位置所在直线，右边的有向射线为船头所朝向的方向。由图可得，此时的偏转角度为左边射线与右边射线的顺时针角度差，偏转角度明显大于180度，顺时针改变船头的方向所做的工作量明显大于逆时针改变船头的方向所做的工作量。因此，根据上述的分析，制定出了相应的改进算法：当求得的偏转角度的值大于180度时，或小于180度时，从反方向进行转向。该算法有如下特点：在遇到图3的情况时，只需逆时针改变船头的方向，不需要顺时针改变船头的方向，减少了舵机与电机的工作量，从而使得电池的利用更加合理化，延长了电池的寿命，减少了不必要的成本，船体的续航能力得到了提高。获取需要偏转的角度后，不断调整船体的方向，寻求航向与目标点方位角一致地向目标点行进，并根据无人船实时位置到目标点的距离判断是否达到目标点，直到到达目标点，选取下一个监测点作为目标点，继续航行，直到达到最终目标点。

调整船头方向是通过调整舵机方向实现的，调整舵机的方向是通过树莓派控制实现的。由于函数得到偏转角度与实际所需的偏转角度有一定的误差以及风力、水流等因素，所以综合PID算法与PWM进行航行控制，即由PID算法确定转向角度，PWM根据转向角度输出指定波形的PWM波控制舵机旋转，从而控制舵机旋转角度，寻求目标点航向一致。

脉宽调整(PWM)是指用微处理器的数字输出来对模拟电路进行控制，是一种对模拟信号电频进行数字编码的方式。在使用PWM时需要先创建一个PWM实例，之后通过占空比来启动PWM，其中占空比通常是指高电平占周期的百分比。可以通过ChangeDutyCycle方法来改变占空比，调整占空比改变PWM波形。舵机通过舵机标识符duojiflag.value判断左右转向，当duojiflag.value＝1时，无人船右转；当duojiflag.value＝-1时，无人船左转；当duojiflag.value＝0时，舵机位置居中，无人船直线行驶。舵机的VCC、GND直接与12V电源相连供电，SNG连接到树莓派的GPIO接受指令；电机驱动螺旋桨，电机的动作具体由电调接受指令并进行控制，电调的VCC、GND直接与12V电源相连供电，SNG连接到树莓派的6PIO接受指令。舵机和电调与树莓派的硬件连接图如图4所示。

PID(比例proportion、积分integral、微分differential)控制是工业中最常用的控制算法。PID控制器的基本思想是读取传感器，然后通过计算比例，积分和微分单元计算所需的执行器输出，并将这三个分量相加以计算输出。PID控制器输出u(t)见下式：

式中k_p表示比例系数，e(t)为输入，k_i表示积分时间常数，k_d表示微分时间常数。PID控制的基础是比例控制，当仅有比例控制时系统输出存在稳态误差；积分控制可消除稳态误差，但可能增加超调；在微分控制中，控制器的输出与输入误差信号的微分(即误差的变化率)成正比关系。

考虑实际条件，使用增量式PID控制进行无人船控制。增量式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]} (5)

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

需要注意的是，发明中所引用的如“上”、“下”、“左”、“右”、“前”、“后”等的用语，亦仅为便于叙述的明了，而非用以限定本发明可实施的范围，其相对关系的改变或调整，在无实质变更技术内容下，当亦视为本发明可实施的范畴。

以上仅是本发明的优选实施方式，本发明的保护范围并不仅局限于上述实施例，凡属于本发明思路下的技术方案均属于本发明的保护范围。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理前提下的若干改进和润饰，应视为本发明的保护范围。

Claims (10)

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]分别表示当前时刻的输入、前一时刻的输入、再前一时刻的输入；

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