专利名称：一种水下自主式航行器的局部路径规划方法
技术领域：
本发明涉及水下自主式航行器领域，特别涉及一种水下自主式航行器的局部路径规划方法。
背景技术：
水下自主式航行器在对指定的未知海域进行海底勘查与测量时需要在水平投影内沿指定目标航迹行驶。如图I所示，水下自主式航行器一般采取在水平投影上沿“弓”字形航迹来回扫描的方式进行海底勘查与测量。该航迹可被分解为一系列有向直线段。水下自主式航行器在沿目标航迹行驶的过程中，由于受到洋流的干扰、传感器测量误差与自身的控制误差等，会偏离目标航迹行驶，导致水下自主式航行器不能按照目标航迹进行海底扫描测量，从而影响测量效果。水下自主式航行器必须能够减小和消除这种航迹跟踪偏差， 要有自主进行局部路径规划的能力，实现在一定精度范围内的航迹跟踪。这是水下自主式航行器完成水下作业和使命的重要前提。目前水下自主式航行器常用的航迹跟踪方法有神经网络法(参考文献汤莉，AUV神经网络水平面航迹跟踪控制研究，哈尔滨工程大学硕士学位论文，2009)等。但该方法抗瞬时扰动能力和抗恒定洋流干扰能力不是十分理想调节距离长，跟踪里程长，计算复杂、算法的时间和空间开销大，难以满足水下自主式航行器进行海底勘查与测量的需求。
发明内容
本发明提供了一种水下自主式航行器的局部路径规划方法，本方法实现了调节距离短，跟踪里程短，计算简单、时间和空间开销小，满足了水下自主式航行器进行海底勘查与测量的需求，详见下文描述一种水下自主式航行器的局部路径规划方法，所述方法包括以下步骤(I)将全局目标航迹分解为一系列有向直线段PiPwI i = 1，2，3，...，N，并依次将 N个预期航点存储在水下自主式航行器控制系统任务文件里；(2)将Pi和Pi+1定义为一段航迹的起始点和结束点，P为水下自主式航行器当前位置，P到直线PiPw的垂线段的长度d = |ph|定义为当前航迹跟踪偏差，H为垂足，
为目标航迹的期望航向角,％ 为未修正的期望航向角，￥ph为与期望航向角％&垂直的航向，vE为修正以后的期望航向角；(3)从所述水下自主式航行器控制系统任务文件里读取第一个目标航点Pi, i = I ;(4)若i > N，则执行步骤(12)，否则，从所述水下自主式航行器控制系统任务文件里读取下一目标航点Pi+1，确定当前局部目标航迹有向线段PiPw ;(5)从惯性导航系统获得水下自主式航行器当前位置P和当前航向，判断是否已经到达Pi+1，如果是，i = i+1，执行步骤⑷；如果否，执行步骤(6)；(6)计算水下自主式航行器当前航迹跟踪偏差d，判断所述当前航迹跟踪偏差d是否小于等于第一阈值dmin，如果是，执行步骤(7);如果否，执行步骤(8)；(7)修正以后的期望航向角Ve等于，期望航向不做调整，执行步骤(12)；(8)判断所述当前航迹跟踪偏差d是否小于等于第二阈值d_，且大于第一阈值 dmin，如果是，执彳了步骤(9);如果否，执彳了步骤(10);(9)根据第一公式计算辅助动态圆半径r，以所述水下自主式航行器当前位置P为圆心、以！■为半径建立动态辅助圆，根据所述动态辅助圆和所述当前局部目标航迹有向线段PiPw的位置关系选出临时目标航点，计算出修正以后的期望航向￥e;(10)所述当前航迹跟踪偏差d大于所述第二阈值d_，使期望航向Ve= ￥ph;(11)控制所述水下自主式航行器沿Ve方向行驶，重新执行步骤(5)；(12)流程结束。所述第一公式具体为r = f (d), f (d)彡 0，f (dmin) = rmax, f (dmax) = rmin, dmin < dmax其中，r.，rmax分别表示动态圆半径r的最大值和最小值。所述根据所述动态辅助圆和所述当前局部目标航迹有向线段PiPw的位置关系选出临时目标航点，计算出修正以后的期望航向Ve具体为I)当动态辅助圆与直线PiPw相离且P在PiPw上的投影H在PwPi的延长线上时，选择Pi作为临时目标航点，修改期望航向使;2)当动态辅助圆与直线PiPp1相离且P在PiPp1上的投影H在PiPp1上时，选择H 作为临时目标航点，修改期望航向使Ve= Vph;3)当动态辅助圆与直线PiPw相离且P在PiPw上的投影H在PiPw的延长线上时，选择Pi+1作为临时目标航点，修改期望航向使仏=% ;4)当动态辅助圆与直线PiPw相交且P在PiPw上的投影H在PwPi的延长线上时，选择Pi为临时目标航点，修改期望航向使;5)当动态辅助圆和直线PiPw相交于点E、F且P在PiPw上的投影在PiPw上，且 Pi在动态辅助圆内Pi+1在动态辅助圆外时，选择靠近Pi+1的E点为临时目标航点，修改期望航向使Ve = Vpe ;6)当动态辅助圆和直线PiPw相交于点E、F且P在PiPw上的投影在PiPw上，且 1和？1+1在动态辅助圆外时，选择E点为临时目标航点，修改期望航向使Ve= Vpe;7)当P在PiPw上的投影在PiPw上且Pi和Pi+1在动态辅助圆内时，选择Pi+1点为临时目标航点，修改期望航向使仏=;8)当动态辅助圆和直线PiPp1相交，且P在PiPw上的投影在PiPw上，且Pi+1在动态辅助圆内Pi在动态辅助圆外时，选择Pi+1点为临时目标航点，修改期望航向使；9)当动态辅助圆和直线PiPw相交且P在PiPw上的投影在PiPw的延长线上，选择Pi+1点为临时目标航点，修改期望航向使仏=% 。本发明提供的技术方案的有益效果是本发明提供了一种水下自主式航行器的局部路径规划方法，当航迹跟踪偏差d小于等于第二阈值d_，且大于第一阈值dmin时，通过动态辅助圆和当前局部目标航迹有向线段PiPw的位置关系选出临时目标航点，计算出修正以后的期望航向VE，控制水下自主式航行器沿Ve方向行驶，本方法能够有效实现水下自主式航行器的局部路径规划，其抵抗瞬间干扰的能力和抵抗恒定洋流干扰的能力都强于传统的神经网络法，并且具有时空开销小、计算简便的优点，满足了水下自主式航行器进行海底勘查与测量的需求。
图I为现有技术提供的水下自主式航行器欲采取的全局目标航迹的分解与表达示意图；图2为本发明提供的图示说明及相关定义；图3-1与3-2为本发明提供的检验本方法的抗瞬时扰动能力的实验效果图；图4为本发明提供的检验本方法的抗恒定洋流扰动能力的实验效果图；图5-1至5-9为本发明提供的本方法的动态辅助圆与临时局部期望航迹的位置关系及相应的新的期望航点选取的示意图；图6为本发明提供的一种水下自主式航行器的局部路径规划方法的流程图。
具体实施例方式为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地详细描述。为了实现调节距离短，跟踪里程短，计算简单、时间和空间开销小，满足水下自主式航行器进行海底勘查与测量的需求，参见图6，本发明实施例提供了一种水下自主式航行器的局部路径规划方法，详见下文描述101 :将全局目标航迹分解为一系列有向直线段PiPwI i = 1，2，3，...，N，并依次将N个预期航点存储在水下自主式航行器控制系统任务文件里；其中，参见图1，该步骤具体为将水下自主式航行器进行海底测量欲采取的全局目标航迹的各个顶点PiIi = 1，2，3，...，N，按照先后顺序存储于水下自主式航行器控制系统任务文件中，共N个预期航点，N为正整数，每个航点包括预期经度、预期纬度和预期速度
等信息。其中，参见图2，设水下自主式航行器目前行驶在局部目标航迹PiPw附近。102 :将Pi和Pi+1定义为一段航迹的起始点和结束点，P为水下自主式航行器当前位置，P到直线PiPw的垂线段的长度d= |ph|定义为航迹跟踪偏差，h为垂足，为目标航迹的期望航向角，为未修正的期望航向角，VPhS与期望航向角垂直的航向，￥e 为修正以后的期望航向角；103 :从水下自主式航行器控制系统任务文件里读取第一个目标航点Pi, i = I ;104 :若i SN，则执行步骤112，否则，从水下自主式航行器控制系统任务文件里读取下一目标航点Pi+1，确定当前局部目标航迹有向线段PiPw ;其中，通过第一个目标航点Pi和下一目标航点Pi+1从而确定当前局部目标航迹有向线段PiPw。105 :从惯性导航系统获得水下自主式航行器当前位置P和当前航向，判断是否已经到达Pi+1，如果是，i = i+1，执行步骤104 ;如果否，执行步骤106 ；其中，当前位置P通常包括经度和纬度信息，例如经度为117.00000000°，纬度为 39. 00000000。、当前航向为 100. 000°。
106 :计算水下自主式航行器当前航迹跟踪偏差d，判断当前航迹跟踪偏差d是否小于等于第一阈值dmin，如果是，执行步骤107 ;如果否，执行步骤108 ；107 -yE = WPPi+i，期望航向不做调整，执行步骤112 ；108 :判断当前航迹跟踪偏差d是否小于等于第二阈值Clmax,且大于第一阈值dmin， 如果是，执行步骤109 ;如果否，执行步骤110 ；109:根据第一公式计算辅助动态圆半径r，以水下自主式航行器当前位置P为圆心、以r为半径建立动态辅助圆，根据动态辅助圆和当前局部目标航迹有向线段PiPw的位置关系选出临时目标航点，计算出修正以后的期望航向Ve,执行步骤111 ;其中，第一公式具体为r = f (d), f (d) ^ 0, f (dmin) = rmax, f (dmax) = rmin, dmin < dmax (I)其中，rmin, rmax分别表示动态圆半径r的最大值和最小值，dmin, dmax, rmin和rmax的选取根据水下自主式航行器的尺寸、水动力特性、航迹跟踪精度需求确定，具体实现时，本发明实施例对此不做限制。满足(I)式的函数表达式有多种方式，本发明实施例以满足其中的一种形式(2)式为例进行说明。
权利要求
1.一种水下自主式航行器的局部路径规划方法，其特征在于，所述方法包括以下步骤(1)将全局目标航迹分解为一系列有向直线段PiPwIi = 1,2,3,...，N，并依次将N个预期航点存储在水下自主式航行器控制系统任务文件里；(2)将Pi和Pi+1定义为一段航迹的起始点和结束点，P为水下自主式航行器当前位置， P到直线PiPw的垂线段的长度d = PH定义为当前航迹跟踪偏差，H为垂足，％^+1为目标航迹的期望航向角，为未修正的期望航向角，VphS与期望航向角Kfw垂直的航向，Ve为修正以后的期望航向角；(3)从所述水下自主式航行器控制系统任务文件里读取第一个目标航点Pi,i = I ;(4)若i> N，则执行步骤(12)，否则，从所述水下自主式航行器控制系统任务文件里读取下一目标航点Pi+1，确定当前局部目标航迹有向线段PiPw ;(5)从惯性导航系统获得水下自主式航行器当前位置P和当前航向，判断是否已经到达Pi+1，如果是，i = i+1，执行步骤⑷；如果否，执行步骤(6)；(6)计算水下自主式航行器当前航迹跟踪偏差d，判断所述当前航迹跟踪偏差d是否小于等于第一阈值dmin，如果是，执行步骤(7);如果否，执行步骤(8)；(7)修正以后的期望航向角￥E等于，期望航向不做调整，执行步骤(12)；(8)判断所述当前航迹跟踪偏差d是否小于等于第二阈值d_，且大于第一阈值dmin，如果是，执行步骤(9);如果否，执行步骤(10)；(9)根据第一公式计算辅助动态圆半径r，以所述水下自主式航行器当前位置P为圆心、以r为半径建立动态辅助圆，根据所述动态辅助圆和所述当前局部目标航迹有向线段 PiPw的位置关系选出临时目标航点，计算出修正以后的期望航向VE，执行步骤(11)；(10)所述当前航迹跟踪偏差d大于所述第二阈值d_，使期望航向UTe=Vph，执行步骤(11);(11)控制所述水下自主式航行器沿Ve方向行驶，重新执行步骤(5)；(12)流程结束。
2.根据权利要求I所述的一种水下自主式航行器的局部路径规划方法，其特征在于， 所述第一公式具体为r = f (d), f (d) ≤0, f (dmin) = rmax, f (dmax) = Ymin, dmin < dmax其中，^in, rmax分别表示动态圆半径r的最大值和最小值。
3.根据权利要求I所述的一种水下自主式航行器的局部路径规划方法，其特征在于， 所述根据所述动态辅助圆和所述当前局部目标航迹有向线段PiPw的位置关系选出临时目标航点，计算出修正以后的期望航向Ve具体为1)当动态辅助圆与直线PiPw相离且P在PiPw上的投影H在PwPi的延长线上时，选择？1作为临时目标航点，修改期望航向使;2)当动态辅助圆与直线PiPw相离且P在PiPw上的投影H在PiPw上时，选择H作为临时目标航点，修改期望航向使Ve= ￥ph ;3)当动态辅助圆与直线PiPw相离且P在PiPw上的投影H在PiPw的延长线上时，选择Pi+1作为临时目标航点，修改期望航向使仏=￥PPm ；4)当动态辅助圆与直线PiPw相交且P在PiPw上的投影H在PwPi的延长线上时，选择？1为临时目标航点，修改期望航向使;5)当动态辅助圆和直线PiPw相交于点E、F且P在PiPw上的投影在PiPw上，且Pi在动态辅助圆内Pi+1在动态辅助圆外时，选择靠近Pi+1的E点为临时目标航点，修改期望航向使 Ve = Vpe ；6)当动态辅助圆和直线PiPw相交于点E、F且P在PiPw上的投影在PiPw上，且Pi和 Pi+1在动态辅助圆外时，选择E点为临时目标航点，修改期望航向使Ve= Vpe;7)当P在PiPw上的投影在PiPw上且Pi和Pi+1在动态辅助圆内时，选择Pi+1点为临时目标航点，修改期望航向使仏=;8)当动态辅助圆和直线PiPw相交，且P在PiPw上的投影在PiPw上，且Pi+1在动态辅助圆内Pi在动态辅助圆外时，选择Pi+1点为临时目标航点，修改期望航向使仏=% ;9)当动态辅助圆和直线PiPp1相交且P在PiPp1上的投影在PiPp1的延长线上，选择Pi+1 点为临时目标航点，修改期望航向使仏=^+1。
全文摘要
本发明公开了一种水下自主式航行器的局部路径规划方法，涉及水下自主式航行器领域，当航迹跟踪偏差d小于等于第二阈值dmax，且大于第一阈值dmin时，通过动态辅助圆和当前局部目标航迹有向线段PiPi+1的位置关系选出临时目标航点，计算出修正以后的期望航向ψE，控制水下自主式航行器沿ψE方向行驶，本方法能够有效实现水下自主式航行器的局部路径规划，其抵抗瞬间干扰的能力和抵抗恒定洋流干扰的能力都强于传统的神经网络法，并且具有时空开销小、计算简便的优点，满足了水下自主式航行器进行海底勘查与测量的需求。
文档编号G05D1/02GK102541062SQ20121004518
公开日2012年7月4日 申请日期2012年2月24日 优先权日2012年2月24日
发明者曾周末, 李一博, 陈世利, 靳世久, 黄新敬 申请人:天津大学