[发明专利]一种登标作业艇跟随浮标运动的控制系统及其控制方法

权利要求书

1.一种登标作业艇跟随浮标运动的控制系统，其特征在于：包括登标作业艇A_j的控制系统和浮标A_i的控制系统，所述登标作业艇A_j的控制系统包括控制器、无刷推进器及驱动器、作业艇GPS定位设备、作业艇WiFi通信设备、超声波测距传感器、电子罗盘；所述浮标A_i的控制系统包括控制计算机、浮标GPS定位设备、浮标WiFi通信设备、电子罗盘；

所述控制系统的控制方法包括以下步骤：

(1)在登标作业艇的控制器内建立登标作业艇和浮标之间相对速度和转向角的动态数学模型，将相对速度分别沿着连线l_ij(t)方向以及垂直连线l_ij(t)方向进行矢量分解，得到径向相对速度v_ij(t)和转动角速度ω_ij(t)，得到下列数学模型：

其中，

l_ij(t)为t时刻登标作业艇A_j的GPS定位设备的安装位置到浮标A_i的GPS定位设备的安装位置的距离；

v_ij(t)表示t时刻登标作业艇和浮标之间的相对径向速度，

v_i(t)为t时刻浮标A_i的漂移速度；

v_j(t)为t时刻登标作业艇A_j的速度；

为t时刻浮标A_i和登标作业艇A_j的相对转向角；

ω_i(t)为t时刻浮标A_i的运动角速度；

ω_j(t)为t时刻登标作业艇A_j的运动角速度；

ω_ij(t)表示t时刻登标作业艇和浮标之间的相对角速度

γ_ij(t)表示t时刻A_j的运动速度v_j(t)与l_ij(t)之间的夹角；

d为登标作业艇的GPS定位设备安装位置到登标作业艇中心点的距离，

一旦GPS定位设备安装完毕，则d为一恒定值；

(2)选择矢量表示数学模型的状态矢量，u_i(t)＝[v_i(t)ω_i(t)]^T表示t时刻浮标A_i的运动状态矢量，u_j(t)＝[v_j(t)ω_j(t)]^T表示t时刻登标作业艇A_j的控制量，则式(1)可以写成紧凑形式

其中

登标作业艇和浮标的转向角差值可以表示为β_ij(t)＝θ_i(t)-θ_j(t)，表示浮标A_i和登标作业艇A_j转向角度的差值；进一步可得ω_ij(t)＝ω_i(t)-ω_j(t)，表示浮标和登标作业艇角速度的差值，所以

其中，

θ_i(t)表示t时刻浮标A_i的运动转向角；

θ_j(t)表示t时刻登标作业艇A_j的运动转向角；

(3)选取登标作业艇A_j的控制矢量u_j(t)＝G^-1(p₁-Fu_i(t))，

其中表示辅助控制输入；并且通过选择适当的控制增益k₁,k₂0，使相应的公式(2)可以写为其中最后得到登标作业艇A_j的控制量u_j(t)为：

(4)浮标A_i向登标作业艇A_j发送自身的速度v_i(t)、角度θ_j(t)、角速度ω_i(t)以及相对转向角度其中相对角度是浮标计算机通过计算当前浮标的GPS位置和登标作业艇的GPS位置信息，计算出l_ij(t)在惯性坐标系下的方向信息，并结合当前浮标的速度方向v_i(t)后得到的，其中浮标的速度方向v_i(t)可通过浮标的GPS协议信息解析得到；登标作业艇A_j的控制器通过WiFi模块接收浮标A_i发送的数据，进一步通过计算得到t时刻的γ_ij(t)，并结合通过超声测距传感器测得的l_ij(t)，进行其自身控制量u_j(t)的计算，登标作业艇控制器输出u_j(t)给推进器驱动器，继而通过对登标作业艇两个推进器的转速和差速进行控制，实现对登标作业艇自身的速度v_j(t)和转速ω_j(t)的控制；于是，可以通过控制登标作业艇A_j的径向速度v_j(t)和转向速度ω_j(t)，保证以预定的距离和预定的角度跟踪浮标，实现登标作业艇对浮标实时运动轨迹的跟踪控制。