CN101833335B - 小型水面机器人装置及自适应水流优化导航方法 - Google Patents

Abstract

本发明涉及小型水面机器人装置及自适应水流优化导航方法，机器人装置包括船体和驱动装置、控制装置，控制装置包括水流速传感器、导航装置和中央控制器；导航装置包括GPS传感器、电子罗盘和惯性导航模块，实现自动导航。自适应水流优化导航方法包括：(1)接收GPS信号，得到绝对坐标；(2)获得实时速度及角度；(3)判断与障碍物的距离；(4)获得前进方向；(5)获得水流速度。(6)控制航向。本发明体积小、重量轻，不仅节约能耗，节省人力，而且成本低、便于使用。本发明的导航方法可优化行驶路径，节约能源，同时防翻，防撞，避障，达到稳定运行的目的。

Description

小型水面机器人装置及自适应水流优化导航方法

技术领域

本发明涉及水面机器人装置及其导航方法，尤其是一种带有自动控制装置的小型水面机器人装置，以及小型水面机器人自适应水流优化导航方法。

背景技术

水面机器人也称作无人船，即为无人操控的智能的水面平台，可搭载各种监控监测勘探设备进行不同应用。其应用目前正逐步得到推广。对于没有人操控而需要完全自主行驶的水面机器人来说，导航方法至关重要。导航算法是指当输入给无人船目的地后，水面机器人根据自身所在位置，驶向目的地。期间要经过数学运算，给自身的螺旋桨、船舵(若是双螺旋桨船体，则是调整两桨的差速)发出指令，不断校正，最终到达目的地的方法。

目前尚没有专门针对小型水面机器人(长度1米左右)的优化导航方法。小型水面机器人体积小、重量轻，航行时受水流影响大，往往难以走出预定的路线。目前的导航方法都是每时每颗保持船头对准目的地。当水面机器人航行在水面时，会受到水流影响。而在目前的导航算法中，由于水流在水面机器人行驶时不方便用传感器准确测量，水流的影响一般都是靠实时校正船的方向来克服。这样，船从起始点到终点的路径会是一条曲线，曲线的曲率主要取决于水流速度、船速以及校正频率。一般对于小型无人船而言，由于最大速度和船本身质量的限制，这个曲率一般都比较大。从而增加了不必要的能耗，并延长了到达目的地的时间。水面波动较大时，若小型水面机器人速度过快转角过大还会出现翻船的危险。若遇到障碍时，还有可能发生碰撞情况。

发明内容

本发明的目的是提供一种带有自动控制装置的小型水面机器人装置，以及小型水面机器人自适应水流优化导航方法。

本发明的目的是通过采用以下技术方案来实现的：

小型水面机器人装置，包括船体和设在船体上的驱动装置和控制装置，所述控制装置包括水流速传感器、导航装置和中央控制器；

所述导航装置包括GPS卫星定位传感器、电子罗盘和惯性导航模块；

所述导航装置与中央控制器电路连接；

所述控制装置与驱动装置电路连接，并实现对船体的自动导航。

作为本发明的优选技术方案，所述惯性导航模块包括三轴陀螺仪或三轴加速度传感器。

作为本发明的优选技术方案，所述控制装置还包括雷达和激光测距传感器。

作为本发明的优选技术方案，所述驱动装置包括电池和驱动电机。

小型水面机器人自适应水流优化导航方法，该方法包括以下步骤：

(1)小型水面机器人通过中央控制器接收GPS定位传感器的信号，得到其绝对坐标；

(2)中央控制器接收惯性导航模块的信号，得到小型水面机器人的加速度、角速度，并对所得加速度或角速度积分，获得实时的速度及角度；

(3)中央控制器接收激光测距传感器或雷达的数据，判断与前方障碍物之间距离；

(4)中央控制器接收电子罗盘的数据，获得前进的方向；

(5)中央控制器接收水流速传感器的数据，获得水流的速度。

(6)中央控制器将上述所得数据进行处理后，通过驱动装置和转向装置控制小型水面机器人的航向。

本发明的有益效果是：相对于现有技术，本发明小型水面机器人的体积小、重量轻，采用电力驱动，自身无污染排放，相比大型船，不仅节约了能耗，节省了人力，而且成本较低。

本发明自适应水流导航优化方法，可以自动适应水流的影响，优化行驶的路径，提高效率，节约能源，有效增加运行时间；同时防翻，防撞，避免了水上交通事故的发生；保证了小型水上机器人及其所载仪器仪表的安全。本发明加入了对水流预测元素的方程建立起来的导航算法，经过实践证明，小型水面机器人的行驶轨迹近似直线，大大减少了不必要的能耗，并且节约了时间，同时可以防翻，避障，达到稳定运行的目的。

附图说明

图1是本发明的结构示意图；

图2是本发明小型水面机器人转弯示意图；

图3是本发明Δt时间内小型水面机器人角速度ω与位移d关系图；

图4是本发明t时刻朝向ht示意图。

图中：1.船体，2.中央控制器，3.GPS卫星定位传感器，4.惯性导航模块，5.雷达，6，激光测距传感器，7.水流速传感器，8.驱动电机，9.电池，10.电子罗盘。

具体实施方式

下面结合附图与具体实施例对本发明作进一步说明：

如图1至图4所示，小型水面机器人装置，包括船体1和设在船体1上的驱动装置和控制装置，所述控制装置包括水流速传感器7、导航装置和中央控制器2；所述导航装置包括GPS卫星定位传感器3、电子罗盘10和惯性导航模块4；所述导航装置与中央控制器2电路连接；所述控制装置与驱动装置电路连接，并实现对船体1的自动导航。

本实施例中，所述惯性导航模块4包括三轴陀螺仪或三轴加速度传感器。所述控制装置还包括雷达5和激光测距传感器6。所述驱动装置包括电池9和驱动电机8。

小型水面机器人自适应水流优化导航方法，该方法包括以下步骤：

(1)小型水面机器人通过中央控制器接收GPS定位传感器的信号，得到其绝对坐标；

(2)中央控制器接收惯性导航模块的信号，得到小型水面机器人的加速度、角速度，并对所得加速度或角速度积分，获得实时的速度及角度；

(3)中央控制器接收激光测距传感器或雷达的数据，判断与前方障碍物之间距离；

(4)中央控制器接收电子罗盘的数据，获得前进的方向；

(5)中央控制器接收水流速传感器的数据，获得水流的速度。

(6)中央控制器将上述所得数据进行处理后，通过驱动装置和转向装置控制小型水面机器人的航向。

本发明的小型水面机器人包括以下几种能力。

自适应水流导航：

当小型水面机器人的中央控制器以某种方式收到目的地的地理坐标以后，会从GPS接收器中读取当前的经纬度坐标，并将其转化为横纵坐标的原点。小型水面机器人接下来会从电子指南针读取自身的朝向，并通过计算，控制驱动装置和转向装置。

在行驶过程中，每隔一定的时间间隔，中央控制器会从GPS接收器读取当前经纬度坐标，并转化到以起始点为原点的横纵坐标系，得到当前k时刻的位置坐标X_k，Y_k；同时中央控制器从电子指南针读取当前k时刻的朝向h_k。这样，中央控制器便可计算出此k时刻小型水面机器人朝向的偏角。通过小型水面机器人的数学模型，可以计算出在下一个时间间隔k+1时刻之前，动力装置和转向装置的指令。

该数学模型基于通过静水实验得到的一些静态方程。如，转弯半径r、角速度ω与螺旋桨转速v、船舵角度θ的关系，或转弯半径r、角速度ω与螺旋桨转速v1、v2的关系(双螺旋桨)。数学表达式为，

$\left[\begin{array}{c}r\\ \mathrm{\ω}\end{array}\right]=g\left(\begin{array}{c}\left[\begin{array}{c}v\\ \mathrm{\θ}\end{array}\right]\end{array}\right)$ 或 $\left[\begin{array}{c}r\\ \mathrm{\ω}\end{array}\right]=g\left(\begin{array}{c}\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]\end{array}\right)$

在t时刻，位置为(Xt，Yt)，朝向为ht。假设控制命令不改变，经过一个很短的时间Δt后，位置可以计算为：

$(X_{t+\mathrm{\Δt}},Y_{t+\mathrm{\Δt}})=(X_t+d\×\sin (\frac{\mathrm{\ω}\·\mathrm{\Δt}}{2}+h_t),Y_t+d\×\cos (\frac{\mathrm{\ω}\·\mathrm{\Δt}}{2}+h_t))$

d为小型水面机器人在Δt时间内走过的位移。d的大小与圆周运动的半径有关，即与螺旋桨转速和舵角的大小有关。我们可以通过静水试验得到以下函数的对应值：

d＝d(r)

我们便可以基于此进一步推导出小型水面机器人的控制模型，使之能精确地到达目的地。对于单螺旋桨小型水面机器人来说，其三自由度非线性静态数学模型如下：

其中，

$r_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{r}_k\\ \mathrm{\ω}\end{array}\right]=\left[\begin{array}{cc}1& 0\end{array}\right]g\left(\begin{array}{c}\left[\begin{array}{c}{v}_k\\ {\mathrm{\θ}}_k\end{array}\right]\end{array}\right)$

${\mathrm{\ω}}_k=\left[\begin{array}{cc}0& 1\end{array}\right]\left[\begin{array}{c}{r}_k\\ {\mathrm{\ω}}_k\end{array}\right]=\left[\begin{array}{cc}0& 1\end{array}\right]g\left(\begin{array}{c}\left[\begin{array}{c}{v}_k\\ {\mathrm{\θ}}_k\end{array}\right]\end{array}\right)$

w_k～N(0，Q_k)，x_k是k时刻的真实状态，且是从(k-1)时刻的状态得来。u_k表示(k-1)到k时刻的控制命令。

GPS接收器和电子罗盘的噪声可以用白高斯噪音模型。我们实现了一个卡尔曼滤波器来减少此类噪声。非线性系统的测量模型为：

z_k＝h(x_k)+m_k；m_k～N(0，R_k)

卡尔曼滤波器分两个阶段。在第一阶段(预测阶段)，我们用以下方程预测下一状态和预测误差协方差：

$x_{k|k-1}=F_k{x}_{k-1|k-1}+B_{k-1}{U}_{k-1}=\left[\begin{array}{c}{X}_{k-1}+d\left(r_k\right)\×\sin ({\mathrm{\ω}}_k/2+h_{k-1})\\ Y_{k-1}+d\left(r_k\right)\×\cos ({\mathrm{\ω}}_k/2+h_{k-1})\\ h_{k-1}+{\mathrm{\ω}}_k\end{array}\right]$

P_k|k-1＝F_kP_k-1|k-1F_k ^T+Q_k-1

在第二阶段(更新阶段)，GPS和电子罗盘数据更新，卡尔曼增益、真实状态和真实误差协方差也同步更新：

K_k＝P_k|k-1H_k ^T(H_kP_k|k-1H_k ^T+R_k)^-1

x_k|k＝x_k|k-1+K_k(z_k-H_kx_k|k-1)

P_k|k＝(I-K_kH_k)F_k|k-1

在实际中，小型水面机器人由于自身重量轻，体积小，对水流的影响非常敏感。我们在数学模型中加入了一个水流差量来抵消水流对预测模型的影响。我们假设相邻两个状态k-1和k时刻的水流方向和速度是不变的。这个假设非常有实际意义，也简化我们的计算量。我们对预测状态x_k|k-1进行了调整，加入了上一状态的水流z_k-1-x_k-1|k-2。

x_k|k＝x_k|k-1+z_k-1-x_k-1|k-2+K_k(z_k-h(x_k|k-1+z_k-1-x_k-1|k-2))

经过严格的数学推到可以证明，上式是收敛的。说明以上方程式可以起到对水流作用的抵消作用。

与传统方法实时保持船头对准目的地不同的是，本算法不校准船头的方向，而是根据上一状态的水流情况，计算出理想的

并通过反函数g^-1求出

或者

使得在k时刻，小型水面机器人的位置尽可能的保持在目的地与起始点的连线上，而不是k-1时刻的位置与目的地的连线。这样，便大幅提高了小型水面机器人的行驶效率，使其行驶路径非常接近直线，从而节省了电量与时间。

防翻能力：

每隔一定的时间间隔，中央控制器会从三周陀螺仪读取角速度数据，通过积分算出小型水面机器人横摇与纵摇的角度。风浪过大时，如果小型水面机器人的速度过快，或者转角过大，都会有翻得危险。定义一个安全的横摇与纵摇的临界角度，若任意一个当前角度超过安全值，则意味着小型水面机器人有翻的危险。此时，中央控制器将通过对动力装置减速和转向装置减小转向角等方式，使小型水面机器人脱离险境。

当遇到电磁干扰，如果电子罗盘发生跳变，小型水面机器人会对陀螺仪检测的角速度对时间做积分，计算出其转动的角度，累加到电子罗盘跳变前的角度上。这样船在任何时候都不会失去方向感。当电子罗盘又能在较长时间内与中央处理器所计算的角度吻合，则认为电磁干扰消失。

当小型水面机器人GPS接收器失去信号，中央处理器会对加速度传感器的数据对时间做积分，可以监测到小型水面机器人在空间三个方向上速度的改变。把这个速度的改变值与失去信号时记录的速度叠加，中央处理器可以准确的计算出小型水面机器人坐标的改变。

避障能力：

每隔一定的时间间隔，中央控制器会从雷达接收数据，计算可得较远处的障碍物的位置，再经过计算可知障碍物是否移动。若不移动，中央控制器会重新规划路径，绕开障碍物；若为移动障碍物，中央控制器会继续前进。继续前行后，障碍物或已驶出，不会导致相撞；或已接近小型水面机器人，而此时由于距离太近雷达已无法识别。中央控制会通过激光测距传感器确认障碍物已无相撞风险或放出指令，停止动力装置及转向装置，等待障碍物离开。

Claims (1)

1.一种小型水面机器人自适应水流优化导航方法，其特征是该方法包括以下步骤：

（1）小型水面机器人通过中央控制器接收GPS定位传感器的信号，得到其绝对坐标；

（2）中央控制器接收惯性导航模块的信号，得到小型水面机器人的加速度、角速度，并对所得加速度或角速度积分，获得实时的速度及角度；

（3）中央控制器接收激光测距传感器或雷达的数据，判断与前方障碍物之间距离；

（4）中央控制器接收电子罗盘的数据，获得前进的方向；

（5）中央控制器接收水流速传感器的数据，获得水流的速度；

（6）中央控制器将上述所得数据进行处理后，通过驱动装置和转向装置控制小型水面机器人的航向，所述数据处理方式如下：

行驶过程中每隔一定的时间间隔，中央控制器会从GPS接收器读取当前经纬度坐标，并转化到以起始点为原点的横纵坐标系，得到当前k时刻的位置坐标X_k，Y_k；同时中央控制器从电子指南针读取当前k时刻的朝向h_k；这样，中央控制器便可计算出此k时刻小型水面机器人朝向的偏角；通过小型水面机器人的数学模型，可以计算出在下一个时间间隔k+1时刻之前，动力装置和转向装置的指令；

该数学模型基于通过静水实验得到的一些静态方程；转弯半径r、角速度ω与螺旋桨转速v、船舵角度θ的关系，或转弯半径r、角速度ω与螺旋桨转速v1、v2的关系，双螺旋桨数学表达式为，

$\left[\begin{array}{c}r\\ \mathrm{\ω}\end{array}\right]=g\left(\left[\begin{array}{c}v\\ \mathrm{\θ}\end{array}\right]\right)$ 或 $\left[\begin{array}{c}r\\ \mathrm{\ω}\end{array}\right]=g\left(\left[\begin{array}{c}{v}_1\\ v_2\end{array}\right]\right)$

在t时刻,位置为(Xt,Yt)，朝向为ht；假设控制命令不改变，经过一个很短的时间Δt后，位置可以计算为：

$(X_{t+\mathrm{\Δt}},Y_{t+\mathrm{\Δt}})=(X_t+d\×\sin (\frac{\mathrm{\ω}\·\mathrm{\Δt}}{2}+h_t),Y_t+d\×\cos (\frac{\mathrm{\ω}\·\mathrm{\Δt}}{2}+h_t))$

d为小型水面机器人在Δt时间内走过的位移；d的大小与圆周运动的半径有关，即与螺旋桨转速和舵角的大小有关；通过静水试验得到以下函数的对应值：d＝d(r)

基于此进一步推导出小型水面机器人的控制模型，使之能精确到达目的地；

对于单螺旋桨小型水面机器人来说，其三自由度非线性静态数学模型如下：

$x_k=f(x_{k-1},u_k)+w_k=f(\left[\begin{array}{c}{X}_{k-1}\\ Y_{k-1}\\ h_{k-1}\end{array}\right],\left[\begin{array}{c}{v}_k\\ {\mathrm{\θ}}_k\\ 0\end{array}\right])+w_k=\left[\begin{array}{c}{X}_{k-1}+d\left(r_k\right)\×\sin ({\mathrm{\ω}}_k/2+h_{k-1})\\ Y_{k-1}+d\left(r_k\right)\×\cos ({\mathrm{\ω}}_k/2+h_{k-1})\\ h_{k-1}+{\mathrm{\ω}}_k\end{array}\right]+w_k;$

其中，

$r_k=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}{r}_k\\ \mathrm{\ω}\end{array}\right]=\left[\begin{array}{cc}1& 0\end{array}\right]g\left(\left[\begin{array}{c}{v}_k\\ {\mathrm{\θ}}_k\end{array}\right]\right)$ ω改为ω_k

${\mathrm{\ω}}_k=\left[\begin{array}{cc}0& 1\end{array}\right]\left[\begin{array}{c}{r}_k\\ {\mathrm{\ω}}_k\end{array}\right]=\left[\begin{array}{cc}0& 1\end{array}\right]g\left(\left[\begin{array}{c}{v}_k\\ {\mathrm{\θ}}_k\end{array}\right]\right)$

w_k是噪声，假定w_k符合均值为零，协方差矩阵为Q_k的正态分布：w_k～N（0，Q_k);x_k是k时刻的真实状态，且是从（k-1）时刻的状态得来；u_k表示（k-1）到k时刻的控制命令；

GPS接收器和电子罗盘的噪声用白高斯噪音模型；实现一个卡尔曼滤波器来减少此类噪声；非线性系统的测量模型为：

z_k＝h(x_k)+m_k;m_k～N(0，R_k)

m_k为测量值，其符合均值为零，协方差矩阵为Rk的正态分布：m_k～N（0，R_k);

卡尔曼滤波器分两个阶段；第一阶段即预测阶段，用以下方程预测下一状态和预测误差协方差：

$x_{k|k-1}=F_k{x}_{k-1|k-1}+B_{k-1}{U}_{k-1}=\left[\begin{array}{c}{X}_{k-1}+d\left(r_k\right)\×\sin ({\mathrm{\ω}}_k/2+h_{k-1})\\ Y_{k-1}+d\left(r_k\right)\×\cos ({\mathrm{\ω}}_k/2+h_{k-1})\\ h_{k-1}+{\mathrm{\ω}}_k\end{array}\right]$

P_k|k-1=F_kP_k-1|k-1F_k ^T+Q_k-1

第二阶段即更新阶段,GPS和电子罗盘数据更新,卡尔曼增益、真实状态和真实误差协方差也同步更新：

K_k=P_k|k-1H_k ^T(H_kP_k|k-1H_k ^T+R_k)^-1

x_k|k＝x_k|k-1+K_k(z_k-H_kx_k|k-1)

P_k|k＝（I-K_kH_k)·F_k|k-1

。

Images