[发明专利]基于低精度GPS、激光测距传感器与方位传感器信息融合的蟹塘自动作业船组合导航方法

摘要

本发明公开了一种基于低精度GPS、激光测距传感器与方位传感器信息融合的蟹塘自动作业船组合导航方法，用于实现蟹塘自动作业船的导航。自动作业船的导航方式是从某点出发，沿塘堤平行，间隔一定行程的内螺旋路径。该发明通过低精度的GPS系统确定导航轨迹拐点的位置以及轨迹航向，通过方位传感器确定当前自动作业船的运动方向，通过激光测距传感器确定自动作业船与近侧塘堤的距离。利用作业船与塘堤的距离，结合当前航向角，通过一定的数学计算，获得当前航向角与期望航向角的差值。将角度偏差值导入明轮电机驱动控制系统，利用PD控制方法调节产生左右明轮的转速差，实现作业船的航向控制，从而实现蟹塘自动作业船的导航。

主权项

1.一种基于低精度GPS、激光测距与方位传感器的蟹塘自动作业船组合导航方法，其特征在于，包括以下步骤：通过GPS系统的数据规划作业船的目标航线、目标航线与距离塘堤的距离L、下一位置的目标点以及转弯点的位置信息，当作业船行驶至相关位置时，开始转向或者切换相应的航迹线；通过方位传感器确定作业船的当前航向角α，结合目标航线的航向信息，判断当前偏离目标航向的角度，为作业船的转向控制提供数据；安装激光测距传感器，并通过激光测距传感器获得作业船与近侧塘堤的距离L₁，结合当前航向角α，计算获得作业船与堤岸的垂直距离L₂，实时监测L₁的值，判断其数值大小，避免作业船与蟹塘堤岸相撞，对作业船造成损坏；分析激光测距传感器所得距离L₁与GPS系统获得的定位信息，当二者的误差大于一定值时，剔除激光传感器的数据；当误差小于一定值时，选择激光测距传感器所得的数据作为航向角计算的依据；利用激光测距传感器所得距离L₁和目标航线与距离塘堤的距离L，结合方位传感器当前航向角α，得作业船当前位置在航线l₁上的投影位置A，沿航线l₁向前延伸一段距离h，获得目标点B；当目标点B超过当前航迹线CD的终点，则切换航迹线，当前航迹线CD与下一航迹线DE交于点D，以作业船当前位置O做航迹线DE的投影点O'。O'沿航迹线DE向前延伸h作为新的目标点B；利用作业船当前位置与目标点B的位置信息，获得目标航向角β，结合方位传感器所得当前航向角α，通过数学计算，得到航向角偏差e；判断作业船当前位置与目标点的距离OB，当OB距离较大时，作业船距离目标位置较远，此时给定较大的电机共模电流，作业船选择高航速运行；当OB较小时，作业船距离目标点位置较近，此时给定较小的电机共模电流，作业船选择低航速运行；将航向角偏差值e导入电机驱动控制系统，利用PID调节器产生左右明轮的差模电流信号，将差模电流信号导入左右明轮电机，产生左右电机的转速差，控制作业船转向。