br>[0039] 步骤一，航行控制计算机接收到离散航迹点序列P[ i ]后，对离散航迹点序列P[ i ] 进行规划和选取，初始时，选取i = l;
[0040] 步骤二，更新当前目标点Pd(xd，yd)=P[i]，其中xd为当前目标点的经度，yd为当前 目标点的炜度；
[0041 ]步骤三，判断当前目标点是否为离散航迹点序列P [ i ]的最后一个目标航迹点：若 是最后一个目标航迹点，将结束自主航行控制;若不是最后一个目标航迹点，则计算船舶当 前位置点与当前目标点之间的距离Rt;
[0042]步骤四，若Rt < R，执行i = i+l，并回到步骤二;若Rt>R，需要计算船舶的期望航向 角加，并进入下一步骤;其中，R为转弯半径；
[0043]期望航向角的计算公式为：
[0044]
[0045]
[0046] 其中，以为期望航向角加的过渡计算变量，还需要根据船舶当前位置点与当前目 标点的相对位置关系进行象限转换;xd为当前目标点的经度，yd为当前目标点的炜度，xt为 当前位置点的经度，yt为当前位置点的炜度，c为船舶当前位置点、当前目标点与地心所构 成的地心角，COS (C)为其余弦值；
[0047] 以船舶当前位置点为原点，建立直角坐标系，其中，北方为纵轴，与纵轴垂直的正 东方为横轴。
[0048] 若当前目标点位于该直角坐标系的第一象限，则Κ
[0049] 若当前目标点位于该直角坐标系的第二象限，则Κ =360十< ;
[0050] 若当前目标点位于该直角坐标系的第三四象限，则％ =180-乂。图3为期望航向 角计算方法原理图，从图中可以看出期望航向角、航向角以及目标点之间的位置关系。
[0051] 步骤五，通过GPS及航姿测量设备获取船舶的航向角信息，通过航向角与期望航向 角加之间的偏差，计算出舵角控制回路的期望舵角；
[0052] 步骤六，通过舵角传感器获取船舶的舵角信息，计算当前舵角值与步骤五计算得 到的期望舵角之间的偏差，再利用船舶上的舵角执行机构调整船舶舵的方位。
[0053]当步骤一至步骤六执行完毕后，船舶的位置和姿态随之发送变化，船舶当前位置 信息、船舶当前航向角信息和船舶当前舵角信息自动更新。步骤三至步骤六在船舶自主航 行过程中，一直处于实时运行状态，运行的次数和频率取决于所选用的GPS位置及航姿测量 设备和舵角传感器的数据采样率。
[0054]本发明涉及变量及名称的解释如下：
[0055] 离散航迹点序列：由一系列具有先后到达条件的期望航迹点组成的航迹点序列Ρ
[i]，i = l，2,3,…，Ν;其中，P[i] = (xdi，ydi)，xdi为期望航迹点的经度坐标，ydi为期望航迹 点的炜度坐标。
[0056] 当前目标点：船舶航行过程中下一步要到达的目标点，为航迹点序列P[i]中的某 一点。
[0057] 当前位置点:船舶的当前位置点，随着船舶位置的变化由GPS及航姿测量设备实时 获取，记为Pt(Xt，y t)，其中xt为当前位置点的经度，yt为当前位置点的炜度。
[0058]航向角:船舶的纵向中轴线与真北的夹角，可由GPS及航姿测量设备实时获取，记 为Φο
[0059] 期望航向角：船舶当前位置点与当前目标点连线与真北的夹角，是航向控制回路 的期望航向值，记为加。
[0060] 转弯半径:用于当前目标点到达条件的判断，可取值为船长的3~5倍，记为R。
[0061]以上已以较佳实施例公布了本发明，然其并非用以限制本发明，凡采取等同替换 或等效变换的方案所获得的技术方案，均落在本发明的保护范围内。
【主权项】
1. 一种船舶自主航行控制系统，其特征在于:包括航行控制计算机、航向航速控制板、 发动机、舱角执行机构、GPS及航姿测量设备、舱W及舱角传感器； 所述航行控制计算机，通过网络接收航速指令和离散期望航迹点序列，通过RS232串口 接收GPS及航姿测量设备发送的船舶位置及船舶姿态信息，并进行航向控制回路解算和航 速控制回路解算，W数字方式通过PCI总线分别发送舱角指令和油口指令至航向航速控制 板； 所述航速控制板，通过PCI总线接收航行控制计算机发出的航向指令和油口指令，通过 RS422串口接收舱角传感器发送的舱角信息，解算后通过控制总线输出油口模拟电压和启 动指令至发动机，并发送舱角执行指令至舱角执行机构，由舱角执行机构控制舱的方向。2. 根据权利要求1所述的船舶自主航行控制系统，其特征在于：航行控制计算机为 PC104控制板。3. 根据权利要求1所述的船舶自主航行控制系统，其特征在于:所述航向航速控制板为 PCI总线控制板。4. 一种船舶自主航行控制方法，其特征在于:包括W下步骤： 步骤一，航行控制计算机接收到离散航迹点序列P[i]后，对离散航迹点序列P[i]进行 规划和选取，初始时，选取i = 1; 步骤二，更新当前目标点Pd (Xd，yd )= P [ i ]，其中Xd为当前目标点的经度，yd为当前目标 点的缔度； 步骤=，判断当前目标点是否为离散航迹点序列P[i]的最后一个目标航迹点：若是最 后一个目标航迹点，将结束自主航行控制;若不是最后一个目标航迹点，则计算船舶当前位 置点与当前目标点之间的距离Rt; 步骤四，若Rt < R，执行i = i+l，并回到步骤二;若Rt>R，需要计算船舶的期望航向角如， 并进入下一步骤;其中，R为转弯半径； 步骤五，通过GPS及航姿测量设备获取船舶的航向角信息，通过航向角与期望航向角恥 之间的偏差，计算出舱角控制回路的期望舱角； 步骤六，通过舱角传感器获取船舶的舱角信息，计算当前舱角值与步骤五计算得到的 期望舱角之间的偏差，再利用船舶上的舱角执行机构调整船舶舱的方位。5. 根据权利要求4所述的船舶自主航行控制方法，其特征在于:所述步骤四中，期望航 向角如的计算公式为：其中，的为期望航向角恥的过渡计算变量，还需要根据船舶当前位置点与当前目标点 的相对位置关系进行象限转换;Xd为当前目标点的经度，yd为当前目标点的缔度，Xt为当前 位置点的经度，yt为当前位置点的缔度，C为船舶当前位置点、当前目标点与地屯、所构成的 地屯、角，COS(C)为其余弦值； W船舶当前位置点为原点，建立直角坐标系，其中正北方为纵轴，与纵轴垂直的正东方 为横轴； 若当前目标点位于该直角坐标系的第一象限，则的=的； 若当前目标点位于该直角坐标系的第二象限，则I//,,, =360 +的; 若当前目标点位于该直角坐标系的第S四象限，则的。6.根据权利要求4所述的船舶自主航行控制方法，其特征在于：当步骤一至步骤六执行 完毕后，船舶的位置和姿态随之发送变化，船舶当前位置信息、船舶当前航向角信息和船舶 当前舱角信息自动更新。
【专利摘要】本发明提供一种船舶自主航行控制系统及控制方法，针对航向自动舵的功能缺失性和航迹自动舵的使用局限性，避免了跟踪连续的期望航迹，该方法只需要利用期望航迹的一系列关键离散点，即可能够实现船舶的自主航迹跟踪控制需要，无需人工干预，可广泛用以解决于无人值守环境下的船舶自主航行控制问题，尤其适用于水面靶场、高危水域等场合的船舶自主航行需要。该方法在功能上弥补了航向自动舵的缺陷，逼近航迹自动舵的跟踪控制性能，但不需要连续的期望航迹作为跟踪输入。本发明所提出的方法与常规的航迹跟踪控制方法相比，具有很强的工程操作性和实施性，可广泛用于解决船舶的自主航行控制需要。
【IPC分类】G05D1/02
【公开号】CN105511464
【申请号】CN201510869347
【发明人】钱宇光, 于瑞亭, 孙永飞
【申请人】南京长峰航天电子科技有限公司
【公开日】2016年4月20日
【申请日】2015年12月1日