人工势场法在航道测量中的应用

史艳霞，贾贵玺，董宏超

（1.天津中德职业技术学院 电气与能源学院，天津300350；2.天津大学 智能电网实验室，天津300072）

航道是重要的水运基础设施，为满足水运工程规划、设计和船舶安全航行的需要，必须定期对现行航道进行常规性的维护测量.航道测量的传统方法精度低、实时性差，不能满足航道测量高精度定位的要求.近年来，基于全球定位系统（GPS）技术的自动导航船不断发展，并逐渐应用于水深测绘工作中，很好地满足了内河航道的测量要求［1］.国内外学者提出了许多理论和方法，其核心是对测量船进行控制，从而快速、无障碍、自主地找到既定航向.目前，已知的导航路径规划算法主要有可视图法、栅格法、自由空间法等［2－4］，但这些算法存在计算时间长，精度不高，对环境变化反应不灵敏，难于应用于实际等缺点.人工势场法（APF）是Khatib于1986年提出的，最早应用于移动机器人的路径规划中［1］.本文提出一种将人工势场法与比例－积分－微分（PID）控制相结合的新型导航算法.

1 控制系统实现

1.1 改进人工势场法

移动物体（如测量船）在一个力场中运动时，存在引力极和斥力极.需要到达的目标位置称为引力极，运动路径中存在的障碍物称为斥力极［5－7］.求解斥力极与引力极间的人工势能可得到运动路径.在运动空间中，通过人工势场法可以求解势场.引力场随运动物体与引力极间距离的增大而增大，其方向从运动物体指向引力极.斥力场的极大值出现于移动物体到达斥力极位置，斥力场的大小随运动物体与斥力极间距离的增大而减小，其方向远离斥力极.总体势场为引力场与斥力场的矢量和.

当测量船、障碍物、目标点在同一条直线上时，这种方法避障效果非常不理想，应对人工势场函数进行改进.当测量船、障碍物和目标点在同一条直线上，且障碍物位于测量船和目标点之间时，将斥力Frep与引力Fatt方向定义为与测量船相切，以保证两力的合力方向不经过障碍点，从而使测量船能够躲避障碍物.设测量船的初始位置为（x（1），y（1）），下一步达到的位置为（x，y）.设测量船与目标的距离为Ls，与X轴的夹角为θc，k为正比例增益［8］.则引力在X，Y轴的分量为

斥力在X，Y轴的分量为

式（2）中：Frep，X为斥力的一个分量，方向从障碍物指向测量船；Frep，Y为斥力的另一个分量，方向从障碍物指向目标；θr为障碍物与测量船连线与X轴的夹角.

合力在X，Y轴的分量为

设θ为合力与X轴的夹角；v为测量船运行速度.则测量船位置为

1.2 航向寻找

测量船寻找航向时，一般行驶到河道中央，沿所测放样线的垂直平分线行驶，待寻找到放样线的中点，调转舵角，沿放样线行驶找到一端端点.此时，可认为完成航向找寻.上述方法中，需保证放样线与河道垂直，但实际河道多呈弯曲状，放样线的设定不能保证其垂直型.鉴于放样线的起始点已经预先设定，采用人工势场算法直接寻找放样线的起点，从而简化过程，节约时间.算法示意图，如图1所示.

测量船自动寻找航向避障仿真图，如图2所示.图2中：放样线共有3条，分别表示为A，B，C；测量船的运动轨迹用直线表示；障碍物用圆点来表示.由图2可知：测量船在人工势场算法作用的情况下，完全实现自主寻找航向，可避开障碍物的干扰，向目标点行进.

图1 人工势场航向寻找算法示意图Fig.1 Diagram of course finding algorithm of APF

图2 测量船自动寻找航向避障仿真图Fig.2 Simulation diagram of the automatic course finding and obstacle－avoiding of the ship

1.3 航迹保持

由图2可知：尽管测量船避开了障碍物，但它偏离了放样线行驶.这种情况是由于传统算法只对下个目标点位置进行识别，而没有综合考虑放样线轨迹造成的.通过在人工势场函数中加入中间目标点，对传统算法进行改进［9］.将检测到的障碍物作为圆心坐标，圆的半径定义为一定的影响范围，圆的方程与放样线的方程相交，交点为下一次需要寻找的目标点.通过上述改进，测量船成功躲开障碍物后，需要先找定义的目标点，然后，返回到放样线上，重新寻找放样线端点.通过这个过程，即可在最小偏差范围内保持既定的航迹.测量船跟踪直线航迹保持示意图，如图3所示.通过仿真验证改进后方法的有效性，如图4所示.通过应用改进的算法，测量船既可以成功避开障碍物，又可以沿着既定的航线行驶，比传统的算法精度更高.

上述方法针对测量线是直线的轨迹，人工势场法平滑且安全，并跟随既定路径.如果所测航线是弯曲形状，人工势场函数下测量船将按照最短路径原则走直线找到目标点，所走路径将是一条折线，与既定曲线路径相差很大，如图5所示.为改善这种不足，将人工势场法与PID 控制［10－11］相结合，设计了新的导航算法，如图6所示.

图3 测量船跟踪直线航迹保持示意图Fig.3 Diagram of the ship in straight line track

图4 测量船跟踪直线航迹仿真图Fig.4 Simulation diagram of the ship in straight line track

图5 人工势场导航算法跟踪曲线航迹仿真图Fig.5 Diagram of the ship with APF in curve track

图6 人工势场－PID 导航算法示意图Fig.6 Diagram of the navigational algorithm in APF－PID

将目标点横坐标X设为当前测量船位置A（x1，y1）的横坐标，将X代入航迹段方程Y＝f（X），可得目标点的坐标C（X1，Y1）.两点间的距离与航向之间的差值为

式（5）中：为预定航向角度；为当前航向度；为航向偏差.

将测量船下一个到达的位置定义为B（x2，y2），可得此时的目标点D（X2，Y2）为

初始值均设定Δφ0，ΔL0为0，有

下一步船舶预定到达的位置坐标为

改进的算法将人工势场法与PID 控制算法相结合，可以平滑跟踪设定的曲线航向，也可以实现对障碍物的自动躲避，比其他智能算法精度更高.仿真比较示意图，如图7所示.

图7 人工势场－PID 导航算法跟踪曲线航迹仿真图Fig.7 Simulation diagram of the ship with APF－PID in curve track

1.4 测量船控制系统结构

测量船控制系统的控制方法为间接控制［12］，通过加入如检测舵机的旋转角度与接收到指令间误差是否为零的反馈控制装置，保证实际值与接收指令的一致性.首先，通过新的导航算法求得预定的航向作为测量船将要行驶的目标航向.然后，由陀螺仪测得此时测量船的船艏，即船头的实际方向φr.最后，将目标航向与实际航向之间的偏差值Δφ输入航向控制器，并设计控制算法，控制舵机达到预定航向需要旋转的角度δ及旋转方向，以减小偏差值.控制方程为

式（10）中：k为控制系数，一般根据测量船掉头时的转弯半径（1.5m）、船体长度（1.2m）、舵机的最大偏舵角度（50°）选取，文中k＝（50／180）×（1.2／1.5）＝0.222.

2 硬件系统

2.1 系统整体组成

测量船的主控系统包括工控机、GPS、测绘模块（主要包括水深仪、姿态仪等）和数据采集模块等.通过船载工控机处理采集的数据，以实现测量船的定位及导航.工控机经由RS232串口将控制指令传递给执行模块，实现对测量船转向以及船速的控制，而且在紧急情况下能发送报警信号.

无人测量船体积较小，行驶过程中容易受到环境因素的影响.因此，设计时需要考虑精度的定义问题.如果精度定义太低，无法实现测量船沿着期望的放样线轨迹运行；如果精度太高，舵机的动作则会受到轻微扰动的影响，导致设备损耗增加.因此，需要综合考虑二者之间的平衡关系.自动舵机控制方向是采用单独电源供电，通过传统的汽油机模式调节舵机桨叶的转动方向和大小，以实现对船体转动方向的控制.方向舵的动力来源为蓄电池，自动舵机的主题为电动舵机.此外，系统还包括控制器、功率放大电路、红外脉冲反馈装置，以及各种传感器.

2.2 装置选型

采用3.5马力，最大功率为2.6kW，最大转速为4 200～5 300r·m－1的汽油机作为动力源.电动舵机的选取受动力装置的影响，可以将链条作为船动力装置中方向舵的滑动连接装置.链条转动带动方向舵机转动，在链条的平行移动转化为舵机转动的过程中，可有效控制测量船的转动方向.

伺服机工作的核心部分为无核心马，用细铜线缠绕于电机的外侧，中心装入磁块，可完成转速快、能耗小的电极构建.伺服机的具体工作流程有以下4个步骤：1）上级控制器发送指令与伺服机；2）测量船转动方向的命令通过电路板进行检测，得到控制器发出的指令；3）马达由功率放大器驱动，马达的输出力矩传至摆臂；4）传感器实时检测实际的转动角度，判断测量船是否按照指令达到指定的位置.

伺服电机选择直流无刷电机，采用直流6V 电源对伺服电机进行供电，产生100～200Hz的脉宽调制（PWM）信号，并通过调整信号的占空比，实现对舵机转动角度的调整.直流无刷电动机动态模型［13］，如图8所示.

图8 直流无刷电机动态模型Fig.8 Dynamic model of brushless DC motor

由动态模型可得伺服电机传递函数为

伺服电机输出电流与PWM 输出信号的关系为

伺服电机转速与输出电流的关系为

系统中的舵机转速、角度反馈检测装置为红外脉冲计数器.

3 软件系统

3.1 GPS定位实现

采用GPS定位技术［14］获取测量船所在位置.在上位机界面显示测量船位置时，需要其二维平面坐标（x，y），而GPS测量装置得到的坐标为WGS－84的三维经纬度坐标（B，L，H），所以需要先对经纬度坐标进行分离，将其转化为能够直接用于上位机显示的施工平面测量坐标.转换模型为

式（14）中：（x，y）为平面直角坐标系的横、纵坐标；（L，B）为GPS返回的经、纬度坐标；S为从赤道到纬度坐标B的经线弧长；N为卯酉圈曲率半径；η2＝e′2cos2B，e′为地球的第二偏心率.

得到转换模型后，可根据此模型编写响应的软件程序，并与商业软件的结果进行比对，设计的程序满足精度要求.

系统中S＝114，当地区不同时，需提前输入经线弧长.将纬度转换成弧度，表示为3 634.785 6，转换成度分秒的格式，表示为36：34：47.136.同理，将经度表示成弧度，形式为11 356.348 9，转化成度分秒的格式，表示为113：56：20.934.仿真结果表明：软件程序计算得到的二维坐标与商业软件得到的坐标值匹配精确.因此，后续应用此程序时，需要将GPS系统测得的坐标先转化为二维平面坐标.

3.2 上位机实现

通过上位机设计图形用户界面，实现用户按照需求选择操作功能.上位机界面的设计流程，如图9所示.调用VC 6.0程序的可以拆分窗口类CSplitterend对以上功能进行开发［15］.

图9 操作界面设计框图Fig.9 Design diagram of operation interface

4 试验结果

通过试验水域中的放样点经纬度坐标确定放样点的数目，测量人员可预先设定测量船的运行轨迹.将测量航线的起点和终点数据实时存储到数据库系统中，并通过电台传输由自动导航算法得到的测量船的目标位置.

最终试验结果，如图10所示.图10中：直线为预设的放样线；曲线为船体行驶轨迹.由图10可知：在无人操作的自动状态下，测量船可以自动寻找到既定航道的起点位置，并沿预定航向及航道进行移动；通过实时上传到上位机数据，控制算法在每次测量结束后，自动寻找下一步运行的方向；此过程完全在无人操作的状态下自动完成.设计的航道间的间隔为60m，每次测量的航道长度为100m.试验结果表明：测量船在15min内可完成全部航道的测量.通过系统中友好的上位机人机接口界面，操作人员只需要通过笔记本电脑实时监控上位机界面中显示的测量船运行方向及位置，提高了系统的可用性.

图10 测量试验上位机界面Fig.10 Interface of upper computer in testing

5 结束语

将人工场势法与PID 控制算法相结合，提出一种新型的导航算法，实现测量船运行航道的自动调整.将此新型导航算法进行试验，证明了算法的有效性.研究表明：此算法满足高精度、完全自主的要求，可广泛地应用于内陆航道及水库的测量工程中.

［1］WEI Jiang，FANG Rui－xian，KHAN J.Performance prediction and dynamic simulation of electric ship hybrid power system［C］∥Proceedings of the IEEE Electric Ship Technologies Sympoisium.［s.l.］：IEEE Press，2007：490－497.

［2］李爱萍，李元宗.机器人路径规划方法的研究［J］.机械工程与自动化，2009（156）：194－197.

［3］朱大奇，颜明重.移动机器人路径规划技术综述［J］.控制与决策，2010，25（7）：961－967.

［4］PARK M G，JEON J H，LEE M C.Obstacle avoidance for mobile robots using artificial potential field approach with simulated annealing［J］.Symposium on Industrial Electronics，2001，3（6）：1530－1535.

［5］KHATID O.Real－time obstacle avoidance for manipulators and mobile robots［J］.International Journal of Robotics Research，1986，5（1）：90－98.

［6］刘春阳，程亿强，柳长安.基于改进势场法的移动机器人避障路径规划［J］.东南大学学报：自然科学版，2009，39（增刊1）：116－120.

［7］张建英，刘暾.基于人工势场法的移动机器人最优路径规划［J］.航空学报，2007（增刊1）：183－188.

［8］胡小平，谢珂，左富勇.基于改进人工势场法的机械手避障规划［J］.测控技术，2012，31（10）：109－116.

［9］于红斌.足球机器人系统的决策推理研究［D］.西安：西北工业大学，2005：87－91.

［10］陶永华.新型PID 控制及其应用［M］.北京：机械工业出版社，1998：57－62.

［11］王永初.智能控制理论与系统的发展评述［J］.华侨大学学报：自然科学版，2004，25（1）：1－4.

［12］詹月林.船舶航迹间接多模态控制方法研究［J］.工业仪表与自动化装置，2003（6）：31－34.

［13］OOSHIMA M，TAKEUCHI C.Magnetic suspension performance of a bearingless brushless DC motor for small liquid pumps［J］.Transactions on Industry Applications，2011，47（1）：72－78.

［14］董绪荣，张守信，华仲春.GPS／INS组合导航定位及其应用［M］.长沙：国防科技大学出版社，1998：25－40.

［15］刘光.地理信息系统二次开发教程：组件篇［M］.北京：清华大学出版社，2002：18－35.