[发明专利]一种锚泊自动定位控制方法及控制系统

权利要求书

1.一种锚泊自动定位控制方法，其特征在于，包括以下步骤：

步骤一，首先进行布锚工作，计算出八个锚点的坐标；

第一步：

布好八个锚绳，调整使得八根锚绳都处于松弛状态；

船的坐标轴，刚好与工程海面坐标系的X、Y轴平行，且方向一致；

第二步：

调整1号、2号、3号和4号锚绳，使得四根锚绳处于紧绷状态，记录此时导航坐标系下船体正中心的坐标为(x₀，y₀)，以及1号锚绳和2号锚绳的测量伸出长度为L₁和L₂；

收紧1、2号锚绳的同时，放松3、4号锚绳，使得船首角度不变的情况下，船体正中心的坐标平移到(x₀+Δx，y₀+Δy)，其中，Δy＞0，Δx任意，记录此时1号锚绳和2号锚绳的测量伸出长度为L_1’和L_2’，停止运动时，1、2、3、4号锚绳紧绷；

船体对应出锚点的位移情况：从A点开始运动，到B点运动结束；

锚绳长度和位置的实际变化情况：1号锚绳从P₁A运动为P₁B，其在海平面上的投影长度P₁A’和P₁B’是计算锚绳与船体的水平面夹角的所需长度数据；

h为船体出锚点和锚实际所在点之间的竖直距离，记为水深；

通过计算，得到船体实际移动距离为：

各长度数据之间的关系为：

实际上P₁、B’、A’三点并不一定在同一直线上，将P₁A’近似的等于P1B’+A’B’；

则计算出锚绳的投影长度P₁A'和P₁B’，以及此时的水深h如下：

那么，在水平面俯视下，1号锚绳的计算为：

θ1＝arctan(Δx/Δy)

同理可以得到

接下来，根据三角函数公式，计算出l号锚和2号锚的锚点坐标为P₁(x₀-15+P₁A'cosα₁，y₀+45+P₁A'sinα₁)＝(x₁，y₁)，式中，15是指1号锚的锚点距离船体中心点的横向距离为15米，45是指1号锚的锚点距离船体中心点的纵向距离为45米；

P₂(x₀+15-P₂A’cosα₂，y₀+45+P₂A'sinα₂)＝(x₂，y₂)，式中，15是指2号锚的锚点距离船体中心点的横向距离为15米，45是指2号锚的锚点距离船体中心点的纵向距离为45米；

第三步：

以第二步的终点为起始点，船体中心的坐标记录为(x₀，y₀)，覆盖即可，记录下当前3号和4号锚绳的测量伸出长度为L₃和L₄，随后，开始收紧3、4号锚绳，松开1、2号锚绳；

收紧3、4号锚绳的同时，放松1、2号锚绳，使得船首角度不变的情况下，船体正中心的坐标平移到(x₀+Δx，y₀+Δy)，其中，Δy＜0，Δx任意，记录此时3号锚绳和4号锚绳的测量伸出长度为L_3’和L_4’，停止运动时，1、2、3、4号锚绳紧绷；

定义船体对应出锚点的位移情况：从A点开始运动，到B点运动结束；

定义锚绳长度和位置的实际变化情况：3号锚绳从P₃A运动为P₃B，其在海平面P₃A'上的投影长度P₃A'和P₃B’是计算锚绳与船体的水平面夹角的所需长度数据；

h为船体出锚点和锚实际所在点之间的竖直距离，这里记为水深；

通过计算，得到船体实际移动距离为：

各长度数据之间的关系为：

实际上P₃、B’、A’三点并不一定在同一直线上，将P₃A’近似的等于P₃B’+A’B’；

计算出锚绳的投影长度P₃A’和P₃B’，以及此时的水深h如下：

那么，在水平面俯视下，以3号锚绳的计算为例：

θ3＝arctan(-Δx/--Δy)＝arctan(Δx/Δy)

同理可以得到

接下来，就可以根据三角函数公式，计算出3号锚和4号锚的锚点坐标为：

P₃(x₀+15-P₃A'cosα₃，y₀-45-P₃A’sinα₃)＝(x₃，y₃)，式中，15是指3号锚的锚点距离船体中心点的横向距离为15米，45是指3号锚的锚点距离船体中心点的纵向距离为45米；

P₄(x₀-15+P₄A'cosα₄，y₀-45-P₄A’sinα₄)＝(x₄，y₄)，式中，15是指4号锚的锚点距离船体中心点的横向距离为15米，45是指4号锚的锚点距离船体中心点的纵向距离为45米；

第四步：

松开1、2、3、4号锚绳，拉紧5、6、7、8号锚绳，记录船体中心的坐标(x₀，y₀)，覆盖即可，作为起始点坐标，记录下当前5号和8号锚绳的测量伸出长度为L₅和L₈，随后，开始收紧5、8号锚绳，松开6、7号锚绳；

收紧5、8号锚绳的同时，放松6、7号锚绳，使得船首角度不变的情况下，船体正中心的坐标平移到(x₀+Δx，y₀+Δy)，其中，Δx＜0，Δy任意，记录此时5号锚绳和8号锚绳的测量伸出长度为L_5’和L_8’，停止运动时，5、6、7、8号锚绳紧绷；

定义船体对应出锚点的位移情况：从A点开始运动，到B点运动结束；

定义锚绳长度和位置的实际变化情况：5号锚绳从P₅A运动为P₅B，其在海平面P₅A'上的投影长度P₅A’和P₅B'是计算锚绳与船体的水平面夹角的所需长度数据；

h为船体出锚点和锚实际所在点之间的竖直距离，这里记为水深；

通过计算，得到船体实际移动距离为：

各长度数据之间的关系为：

实际上P5、B’、A’三点并不一定在同一直线上，将P₅A’近似的等于P₅B’+A’B’；

计算出锚绳的投影长度P₅A’和P₅B'，以及此时的水深h如下：

那么，在水平面俯视下，以5号锚绳的计算为例：

θ5＝arctan(-Δx/Δy)

同理可以得到

接下来，根据三角函数公式，计算出5号锚和8号锚的锚点坐标为：

P₅(x₀-15-P₅A'sinβ₁，y₀+45+P₅A’cosβ₁)＝(x₅，y₅)，式中，15是指5号锚的锚点距离船体中心点的横向距离为15米，45是指5号锚的锚点距离船体中心点的纵向距离为45米；

P₈(x₀-15-P₈A'sinβ₄，y₀-45-P₈A’cosβ₄)＝(x₈，y₈)，式中，15是指8号锚的锚点距离船体中心点的横向距离为15米，45是指8号锚的锚点距离船体中心点的纵向距离为45米；

第五步：

以第四步结束之后的船体中心的坐标(x₀，y₀)，覆盖即可，作为起始点坐标，记录下当前6号和7号锚绳的测量伸出长度为L₆和L₇，随后，开始收紧6、7号锚绳，松开5、8号锚绳；

收紧6、7号锚绳的同时，放松5、8号锚绳，使得船首角度不变的情况下，船体正中心的坐标平移到(x₀+Δx，y₀+Δy)，其中，Δx＞0，Δy任意，记录此时6号锚绳和7号锚绳的测量伸出长度为L_6’和L_7’，停止运动时，5、6、7、8号锚绳紧绷；

定义船体对应出锚点的位移情况：从A点开始运动，到B点运动结束；

定义锚绳长度和位置的实际变化情况：锚绳从P₆A运动为P₆B，其在海平面P₆A'上的投影长度P₆A'和P₆B’是计算锚绳与船体的水平面夹角的所需长度数据；

h为船体出锚点和锚实际所在点之间的竖直距离，这里记为水深；

通过计算，得到船体实际移动距离为：

各长度数据之间的关系为：

实际上P₆、B’、A’三点并不一定在同一直线上，将P₆A’近似的等于P₁B’+A’B’；

则可以计算出锚绳的投影长度P₆A'和P₆B’，以及此时的水深h如下：

那么，在水平面俯视图下，以6号锚绳的计算为例：

θ6＝arctan(Δx/Δy)

同理可以得到

接下来，根据三角函数公式，计算出6号锚和7号锚的锚点坐标为：

P₆(x₀+15+P₆A'sinβ₂，y₀+45+P₆A'cosβ₂)＝(x₆，y₆)，式中，15是指6号锚的锚点距离船体中心点的横向距离为15米，45是指6号锚的锚点距离船体中心点的纵向距离为45米；

P₇(x₀+15+P₇A'sinβ₃，y₀-45-P₇A'cosβ₃)＝(x₇，y₇)，式中，15是指7号锚的锚点距离船体中心点的横向距离为15米，45是指7号锚的锚点距离船体中心点的纵向距离为45米；

第六步：

重复第二步至第五步，定义上一次测量某锚点的坐标为P_i-last(x_last，y_last)，定义重复之后所测量得到的某锚点坐标为P_i-now(x_now，y_now)，则更新迭代方法如下：

如果是第一次测量得到数据，则

P_i(x_i，y_i)＝P_i-now(x_now，y_now)

否则，根据以下公式进行迭代

P_i(x_i，y_i)＝P_i(x_i，y_i)+[P_i-now(x_now，y_now)-P_i(x_i，y_i)]×ε

其中，ε为人为设定的常量，范围0.1至0.6均可，视实际效果略作调整即可；

当在某一次迭代完成之后，发现如下情况

P_i-now(x_now，y_now)-P_i(x_i，y_i)＜0.05×P_i(x_i，y_i)

或者，迭代次数超过某个人为设定的上限值时，则完成迭代工作；

至此，八个锚点在工程海面坐标系下的坐标点已经确定；

步骤二，布锚工作完成后，获取此时船舶的位置数据，计算出船艏向，并计算出船体的四个出锚点的坐标，再根据已经测算得到的八个锚点的坐标信息，计算出八根锚绳的投影面长度；

步骤三，根据输入的船舶在坐标系中的目标定位位置，在坐标系中计算出船体当前位置与目标定位位置的偏差量，或者直接输入船舶需要移动的偏差量，根据偏差量和步骤二中的八个锚点的坐标信息、锚绳投影面长度数据计算出八根锚绳的收紧长度控制数据，并生成对各个锚机的运动控制指令，将这些指令发送给对应的锚机控制单元，控制锚机动作，完成船舶的运动到位；

所述偏差量包括X轴偏差量、Y轴偏差量和船艏向偏差角度，对应的，船体移动控制包括以下3个步骤：

X轴平移：

根据X轴偏差量计算得到船体沿X轴平移后新的四个出锚点的坐标，再根据已经确定的八个锚点的坐标信息计算得到八根锚绳新的投影面长度，进而计算得到八根锚绳中需要收紧的4根锚绳的长度改变量；另外4根锚绳为需要放松的锚绳，船体运动过程中使用伺服刹车恒定在某一个拉力值即可；

Y轴平移：

根据Y轴偏差量计算得到船体沿Y轴平移后新的四个出锚点的坐标，再根据已经确定的八个锚点的坐标信息计算得到八根锚绳新的投影面长度，进而计算得到八根锚绳中需要收紧的4根锚绳的长度改变量；另外4根锚绳为需要放松的锚绳，船体运动过程中使用伺服刹车恒定在某一个拉力值即可；

原位旋转：

根据船艏向偏差角度计算得到船体原位旋转后新的四个出锚点的坐标，再根据已经确定的八个锚点的坐标信息计算得到八根锚绳新的投影面长度，进而计算得到八根锚绳中需要收紧的4根锚绳的长度改变量；另外4根锚绳为需要放松的锚绳，船体运动过程中使用伺服刹车恒定在某一个拉力值即可。