一种无人船自主巡航控制系统及巡航方法

1.本发明涉及无人船自主航向技术领域，具体是一种无人船自主巡航控制系统及巡航方法。


背景技术：

2.现有的无人船多是使用slam系统进行自主巡航控制，但是无人船在slam系统控制下进行自主巡航时，往往会出现误差，导致无人船实际航向与姿态与规划的路线有偏差。


技术实现要素：

3.本发明的目的在于提供一种无人船自主巡航控制系统及巡航方法，以解决上述背景技术中提出的问题。
4.为实现上述目的，本发明提供如下技术方案：
5.一种无人船自主巡航方法，包括以下步骤：
6.s1、船载slam系统通过无人船上传感器采集数据并处理生成无人船的位姿和地图构建更新，激光雷达生成点云数据并处理对无人船周围的障碍识别和边界位姿识别，进行生成网格地图；
7.s2、无人船接收巡航指令；
8.s3、无人船主控系统根据巡航指令、位姿和地图进行线路规划，并形成参考巡航路径；
9.s4、无人船根据参考巡航路径开始进行巡航，巡航的过程中，通过无人船非线性控制器对无人船动力模型的最优解进行求解；
10.s5、根据无人船动力模型的最优解以及参考巡航路径来修正无人船的实际航向。
11.作为本发明进一步的方案：所述非线性控制器采用最小二乘法求解最优解，然后通最优解惩罚给定预测时域窗口nc(tj≤t≤tj nc)内，预测状态(qk)和控制(uk)轨迹与其指定参考的偏差:
[0012][0013][0014]qk 1
＝f(qk，uk)，k＝j，
…
，j n
c-1
[0015]qk，min
≤qk≤q
k，max
，k＝j，
…
，j nc[0016]uk，min
≤uk≤u
k，max
，k＝j，
…
，j n
c-1
[0017]
其中表示船舶状态，表示控制输入，表示当前状态估计，和分别表示时变状态和控制参考；表示终端参考状态；和是正定权重矩阵，用于惩罚与期望值的偏差；q
k，min
和q
k，max
分别表示状态的下界和上界；u
k，min
和u
k，max
分别表示控制输入的下限和上限。
[0018]
作为本发明进一步的方案：所述qk＝[x y ψ u v r]
t
，uk＝[f
1 f2]
t
[0019]
其中x y ψ分别是船舶沿x轴、y轴、z轴方向的纵荡、横荡、垂荡位置，u v r分别是浪涌速度(u)、摇摆速度(v)和偏航率(r)；
[0020]
f1和f2分别是2台船舶推进器的推力。
[0021]
作为本发明进一步的方案：所述无人船采用六自由度模型来描述其运动，六自由度模型考虑了无人船所处真实水域环境中复杂运动情况的全部自由度，无人船的状态矢量由等式1表示，包含船体沿x轴、y轴、z轴方向的纵荡、横荡、垂荡以及绕x轴、y轴、z轴纵摇、横摇以及艏摇，六自由度模型有基于惯性坐标性和载体坐标系两种描述方式，惯性坐标系(o0x0y0z0)以地球中心为坐标原点，x0，y0，z0轴分别指向正北、正东、地心，载体坐标系(oxyz)以船体质心为坐标原点，x轴沿船首方向，y轴垂直于x轴沿船体右侧，z轴垂直于xoy平面指向地心；
[0022][0023]
式中，x0，y0，z0是惯性坐标系下无人船的位置，θ，ψ为载体坐标系下的欧拉角，可以由船载的rtk和ins组合导航仪测得的线速度和角速度积分计算出，无人船运动状态在两种坐标系下的转换关系如下所示：
[0024][0025][0026][0027][0028]
式中，u，v，w是线速度的测量值，z
p
，zq，zr是角速率测量值，tv，ta是转换矩阵，联立等式2-5可得等式6；
[0029][0030]
作为本发明进一步的方案：所述无人船通过级联无人船的状态向量xv和地图参数向量xm来拓展状态向量x，以达到同时估计无人船的运动状态以及环境中的障碍物信息的目的：
[0031][0032]
其中x
mi
表示第i个障碍物平面结构的地图参数，该地图参数向量的维度随检测到的平面曲面的数量而增加，最终，无人船的系统运动学方程可以由等式8表示，其中z
dr
为船载感知系统的测量值，联立等式6可得f(xv，z
dr
)，w为零均值的高斯白噪声；
[0033][0034]
再利用船载传感器的观测信息来推断无人船的位置，测距传感器的观测量z是某个环境特征相对于传感器的位置和角度信息，系统的测量方程表示如下：
[0035][0036]
式中，d
x
和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角；e是转换矩阵，而v是测量噪声。
[0037]
一种无人船自主巡航控制系统，所述无人船上设有船载传感器部、slam系统以及无人船主控系统，所述无人船主控系统连接有控制终端，所述无人船主控系统与所述slam系统连通，所述无人船主控系统内设有船舶非线性控制器，所述船舶非线性控制器内设有线路优化器，所述线路优化器内设有代价函数模块、控制模型、约束函数模块。
[0038]
作为本发明进一步的方案：所述传感器部包括激光雷达、惯性和卫星组合导航模块、摄像头、毫米波雷达，所述激光雷达包括机械激光雷达，所述机械激光雷达通过网格地图生成器与所述船载主控系统连通。
[0039]
与现有技术相比，本发明的有益效果是：本发明通过在无人船上设置船舶非线性控制器，船舶非线性控制器内设置有包括代价函数、控制模型、约束函数的优化器，能够对
无人船的实际航向进行优化控制，使得无人船的实际航行路径更加贴近有规划的循环路径。
附图说明
[0040]
图1为本实施例无人船自主控制与决策系统构架；
具体实施方式
[0041]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0042]
请参阅图1，本发明实施例中，所述无人船上设有船载传感器部、slam系统以及无人船主控系统，所述无人船主控系统连接有控制终端，所述无人船主控系统与所述slam系统连通，所述无人船主控系统内设有船舶非线性控制器，所述船舶非线性控制器内设有线路优化器，所述线路优化器内设有代价函数模块、控制模型、约束函数模块，所述传感器部包括激光雷达、惯性和卫星组合导航模块、摄像头、毫米波雷达，所述激光雷达包括机械激光雷达，所述机械激光雷达通过网格地图生成器与所述船载主控系统连通。
[0043]
一种无人船自主巡航方法，包括以下步骤：
[0044]
s1、船载slam系统通过无人船上传感器采集数据并处理生成无人船的位姿和地图构建更新，激光雷达生成点云数据并处理对无人船周围的障碍识别和边界位姿识别，进行生成网格地图；
[0045]
s2、无人船接收巡航指令；
[0046]
s3、无人船主控系统根据巡航指令、位姿和地图进行线路规划，并形成参考巡航路径；
[0047]
s4、无人船根据参考巡航路径开始进行巡航，巡航的过程中，通过无人船非线性控制器对无人船动力模型的最优解进行求解；
[0048]
s5、根据无人船动力模型的最优解以及参考巡航路径来修正无人船的实际航向。
[0049]
实施例1
[0050]
在本实施例中，无人船是一个欠驱动系统，因此采用六自由度模型来描述其运动，六自由度模型考虑了无人船所处真实水域环境中复杂运动情况的全部自由度，无人船的状态矢量由等式1表示，包含船体沿x轴、y轴、z轴方向的纵荡、横荡、垂荡以及绕x轴、y轴、z轴纵摇、横摇以及艏摇。六自由度模型有基于惯性坐标性和载体坐标系两种描述方式，惯性坐标系(o0x0y0z0)以地球中心为坐标原点，x0，y0，z0轴分别指向正北、正东、地心，载体坐标系(oxyz)以船体质心为坐标原点，x轴沿船首方向，y轴垂直于x轴沿船体右侧，z轴垂直于xoy平面指向地心。
[0051][0052]
式中，x0，y0，z0是惯性坐标系下无人船的位置，θ，ψ为载体坐标系下的欧拉角，可以由船载的rtk和ins组合导航仪测得的线速度和角速度积分计算出。无人船运动状态在两种坐标系下的转换关系如下所示：
[0053][0054][0055][0056][0057]
式中，u，v，w是线速度的测量值，zo，zq，zr是角速率测量值，tv，ta是转换矩阵，联立等式(2-5)可得等式(6)。
[0058][0059][0060]
通过级联无人船的状态向量xv和地图参数向量xm来拓展状态向量x，以达到同时估计无人船的运动状态以及环境中的障碍物信息的目的。
[0061][0062]
其中x
mi
表示第i个障碍物平面结构的地图参数。该地图参数向量的维度随检测到的平面曲面的数量而增加。最终，无人船的系统运动学方程可以由等式(8)表示，其中z
dr
为船载感知系统的测量值，联立等式(6)可得f(xv，z
dr
)，w为零均值的高斯白噪声。
[0063][0064]
再利用船载传感器的观测信息来推断无人船的位置，测距传感器的观测量z是某个环境特征相对于传感器的位置和角度信息，系统的测量方程表示如下：
[0065]
[0066]
式中，d
x
和dy表示无人船与当前观测地标之间的相对位置，γ表示无人船航向角。e是转换矩阵，而v是测量噪声。
[0067]
对于涉及无人船动力学学习的轨迹跟踪问题，拟采用最小二乘函数的形式为(nonlinear model predictive control，nmpc)制定了最优控制问题，以惩罚给定预测时域窗口nc(tj≤t≤tj nc)内，预测状态(qk)和控制(uk)轨迹与其指定参考的偏差：
[0068]
qk[x y ψ u v r]
t
，其中x y ψ分别是船舶沿x轴、y轴、z轴方向的纵荡、横荡、垂荡位置，u v r分别是浪涌速度(u)、摇摆速度(v)和偏航率(r)；
[0069]
uk＝[f
1 f2]
t
[0070]
f1和f2分别是2台船舶推进器的推力，通过控制船舶推进器的动力输出，进而可以实现前进、减速、转向控制，可以进一步控制船舶的航迹，在本实施例中以具有两台船舶推进器的无人船为例，通过两个。
[0071][0072][0073]qk 1
＝f(qk，uk)，k＝j，
…
，j n
c-1
[0074]qk，min
≤qk≤q
k，max
，k＝j，
…
，j nc[0075]uk，min
≤uk≤u
k，max
，k＝j，
…
，j n
c-1
[0076]
其中表示船舶状态，表示控制输入，表示当前状态估计，和分别表示时变状态和控制参考；表示终端参考状态；和是正定权重矩阵，用于惩罚与期望值的偏差。对某一确定尺度的船舶，某些权重矩阵假定为常数，如p是模型中的参数向量，即船舶的有效载荷。此外，q
k，min
和q
k，max
分别表示状态的下界和上界；u
k，min
和u
k，max
分别表示控制输入的下限和上限。
[0077]
加权矩阵wq、wu和w
nc
为将根据实际测试的结果进行设置和优化，预测时域nc也将根据船舶的动力特性进行选择，wq、wu和w
nc
为正交矩阵，是通过测试后的经验数据。
[0078]
对于本领域技术人员而言，显然本发明不限于上述示范性实施例的细节，而且在不背离本发明的精神或基本特征的情况下，能够以其他的具体形式实现本发明。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本发明的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化囊括在本发明内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。
[0079]
此外，应当理解，虽然本说明书按照实施方式加以描述，但并非每个实施方式仅包含一个独立的技术方案，说明书的这种叙述方式仅仅是为清楚起见，本领域技术人员应当将说明书作为一个整体，各实施例中的技术方案也可以经适当组合，形成本领域技术人员可以理解的其他实施方式。