一种风浪动态数据驱动的无人栈桥自主归位和对接控制方法

本发明涉及无人栈桥对接领域，尤其涉及用于一种风浪动态数据驱动的无人栈桥自主归位和对接控制方法。

背景技术：

1、在对接之前需要将无人栈桥归位，主动船需要实时跟踪被动船，对动态目标的跟踪可以转化为对动态路径的跟踪，这就对用于路径跟踪的制导律的要求变得极为苛刻。传统的los制导律由于制导效果不够好，可能达不到执行任务的要求。具有时变前瞻距离的los制导律存在较大的超调；积分los制导律使得无人栈桥到达期望直线路径时的航迹是平滑的，但要达到与期望路径的最大偏差值需要很长时间。在对接阶段，无人栈桥易受风浪干扰的影响而对接失败。基于这样的需求，本发明提出一种风浪动态数据驱动的无人栈桥自主归位和对接控制方法，改进了具有时变前瞻距离的积分los制导律，直行时引入平滑参数调整了前瞻距离的大小，转弯时引入矢量场的概念，有助于更快到达期望路径，达到稳定，并消除恒定外部干扰造成的影响。为满足对接接受度，引入了视觉制导切换。本发明在风浪干扰下，提高无人栈桥的制导效果，从而完成自主归位和对接，且实现成本较低，易于实现。

技术实现思路

1、本发明的目的在于提供一种风浪动态数据驱动的无人栈桥自主归位和对接控制方法，所述策略方法能提高其制导效果，且实现成本较低，易于实现，gnss/imu多传感器导航系统实时获取无人栈桥的当前位置和偏航角，从而实现无人栈桥的归位，然后切换为单目摄像头实时获取图像数据并进行视觉制导，完成对接。

2、为了达到上述目的，本发明采用的技术方案为：一种风浪动态数据驱动的无人栈桥自主归位和对接控制方法，包括以下步骤：

3、通过无人栈桥上搭载的gnss/imu多传感器导航系统，实时获取无人栈桥的当前位置和偏航角；

4、由地面基站发出远程控制指令，实时生成期望dubins路径，使无人栈桥(主动船)归位，无人栈桥通过制导律计算出期望速度和期望航线角以沿期望路径航行并靠近另一个无人栈桥(被动船)，该制导律为一种基于矢量场的积分视线制导律。dubins路径的转弯半径r保持不变，考虑了无人栈桥的转弯能力。dubins路径的始端考虑了主动船的位置和偏航角，dubins路径的终端考虑了被动船的位置和偏航角，是连接主动船和被动船后方2.5倍被动船船长处的最短路径；

5、根据无人栈桥实际位置和期望路径的偏差，计算出横向轨迹误差，基于风浪的离线和在线数据控制横向轨迹误差逐渐趋向于零，即控制无人栈桥以期望速度收敛并遵循给定的期望路径；

6、主动船到达被动船正后方有2.5倍被动船船长时，进行对接，切换为单目摄像头实时获取图像数据并进行视觉制导，当公接头被母接头的光电信号探测到时，控制主动船的速度与被动船保持一致，执行单头对接，并检测是否对接成功，若不成功则重新进行对接。

7、一种基于矢量场的积分视线制导律，其特征在于，步骤如下：

8、a.跟踪期望直线路径：

9、使用具有时间变化的前瞻距离δ，引入平滑参数μ：

10、

11、其中，ye为横向误差，表示无人栈桥的当前位置与期望路径的最小距离，kδ为收敛率，δmax和δmin分别为δ的上下限，μ使得当无人栈桥将要靠近期望路径时，分配了一个较大的前瞻距离，减小超调，从而使得转向平滑：

12、

13、其中，eλ为一个设计参数，用于增大μ的倍率，ψer为无人栈桥当前航线角与期望路径中距离无人栈桥最近一点的路径切向角γp的差值。

14、当无人栈桥沿期望直线路径行驶时，相应的制导律由下式给出：

15、

16、其中，σ为设计参数，χd为无人栈桥期望航线角。无人栈桥的当前速度u由公式给出，u和v分别为浪涌速度分量和摇摆速度分量。积分作用在运动学水平上，因为它是制导的一部分。横向误差的积分基于风浪的离线和在线数据不断累积，从而产生非零yint。当无人栈桥在所需路径上时，即当ye为零时，ye的积分将产生必要的侧滑角β，允许期望航向角ψd与路径方向不一致，而无人栈桥能够抵消风并精确地遵循该路径，其中ψd＝χd-β。

17、b.跟踪期望曲线路径：

18、期望曲线路径在微分上可以看成由曲率变化的圆弧段组成，每个圆弧段都有一个对应的转弯半径r，无人栈桥与该圆弧段圆心(xc,yc)的径向距离d满足d＝ye+r。无人栈桥沿期望曲线路径行驶的制导律为：

19、

20、其中，rv表示矢量场的场源半径，与转弯半径有关：

21、

22、其中，k和rmin为设计参数。rmin为rv的下限，r为rv的上限，r可由下式计算得出：

23、

24、其中，(xp,yp)表示期望路径中距离无人栈桥最近的一点，其路径切向角为γp，(xp+1,yp+1)表示当前最近的点在期望路径中的预期下一个点，其路径切线角为γp+1。可以通过减小rv的大小来增加矢量场对无人栈桥的制导能力，使得无人栈桥转向更快。由于rv比r小，无人栈桥会有向转弯圆弧内移动的趋势。然而，当无人栈桥将要走到或刚走到转弯圆弧内时，由于期望路径是圆弧不是直线的缘故，会使得无人栈桥又具有向转弯圆弧外移动的趋势，因此无人栈桥将很好地沿着期望路径移动。

25、当无人栈桥沿直线段和圆弧段组成的期望dubins路径行驶时，速度分配采用下式：

26、

27、其中ud表示期望速度，umax和umin分别是速度的上下限。航线角误差由计算得出，其中χ为无人栈桥实际航线角。ymax表示最大允许横向误差，χmax表示最大允许航线角误差。该速度分配使得无人栈桥偏离期望路径时的速度较小，有利于在转向时不会因为速度过快而偏离更多，与期望路径的贴合程度更好。

28、被动船可以是保持固定的，也可以是匀速运动的。当主动船到达被动船正后方有2.5倍被动船船长时，开始对接，切换为单目摄像头进行视觉制导，实时识别被动船上的apriltag码，采用直接线性变换(dlt)算法计算两个无人栈桥之间的相对位置和偏航角误差。控制主动船的四个螺旋桨，使主动船的偏航角趋向于被动船的偏航角，并不断接近被动船。当公接头被母接头的光电信号探测到时，控制主动船的速度与被动船保持一致，执行单头对接，释放母头单元上的电控钩并将其锁定在公头单元上的抓杆上。此时，如果两艘船的横向距离误差小于0.1m，且偏航角误差小于5°，则满足对接接受度，能够对接成功。如果被动船是保持固定的，在对接后，主动船将向后移动，利用视觉检测两艘船之间的距离是否增加；如果被动船是匀速运动的，在对接后，主动船将停止提供推力，利用视觉检测两艘船之间的距离是否增加。如果距离保持不变，则对接成功，否则，收回电控钩，并重新开始对接。

29、本发明的有益效果在于：

30、1)所需无人栈桥搭载的传感器较少，gnss/imu多传感器导航系统实时获取无人栈桥的当前位置和偏航角，单目摄像头实时获取图像数据并进行视觉制导提高精度；

31、2)引入的平滑参数μ使得当无人栈桥将要靠近期望路径时，分配了一个较大的前瞻距离，减小超调，从而使得转向平滑；

32、3)引入的矢量场的场源半径rv使得无人栈桥很好地沿着期望曲线路径行驶；

33、4)速度分配使得无人栈桥偏离期望路径时的速度较小，有利于在转向时不会因为速度过快而偏离更多，与期望路径的贴合程度更好。