一种基于智能预测控制的水面无人船路径跟踪方法与流程

技术特征：
1.一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，具体的实现步骤为：步骤1.下位机接收北斗、惯性导航传感器的数据，并进行解析，得到无人船的当前经纬度坐标、航速、艏向角等信息，设置阈值增量约束改进互补滤波算法，对艏向角进行滤波处理；将滤波后的数据将进行转化，得到北东地坐标系下的状态信息；步骤2.按照无人船作业任务预先规划出航行路径点，然后连接各个航行路径点形成一条期望路径曲线作为跟踪的路径，根据无人船初始状态，计算出每个路径点的自动接纳圆半径；步骤3.根据状态信息进行航向规划并利用自适应视线法解算出期望航向角；步骤4.进行路径段的更新，在当前路径点时，若无人船在自适应的接纳圆内，则跟踪下一路径点；步骤5.在航速给定的情况下，将当前艏向角与期望航向角的误差作为模型预测控制器的输入，设计基于模型预测控制的路径跟踪控制器，通过构建预测模型，以滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，再进行反馈校正，解算出对应推进器的转速指令来调整动作，来缩小并消除误差；步骤6.本发明引入人工鱼群算法，把目标函数倒数作为人工鱼群算法的适应度函数，并对该算法进行改进，在觅食阶段，将视野范围扩大后，在新的视野范围内，再次进行搜索，寻找适应度高的人工鱼，并向该方向前进一步；步骤7.通过改进的人工鱼群算法解算出最优控制增量序列中的第一个控制增量分量，将其作用到无人船，在下一时刻，根据导航传感器的实时状态，判断无人船是否到达终点，如果没有到达，将其信息与下一时刻期望路径点的状态信息的差值作为模型预测控制器的输入，形成反馈，构成闭环控制。2.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤1对艏向角的融合滤波算法具体步骤为：步骤1.1：下位机通过rs232通信协议接收北斗、惯性导航传感器的数据，并进行解析；步骤1.2：初始化姿态参数；步骤1.3：获取加速度计实测值，并进行归一化处理，转换成单位向量；步骤1.4：利用陀螺仪更新方向余弦矩阵：式中，为加速度分量，为四元素分量；步骤1.5：将标准重力加速度向量与加速度测量值向量进行叉乘，得到如下的误差向量：
式中，为误差分量，为角速度分量，为加速度分量；步骤1.6：通过pid控制器对误差进行消除，得到当前时刻无人船艏向角的滤波值，控制器公式如下：式中，e
z
(t)为艏向角误差，δe
z
(t)为艏向角补偿量，k
p
为比例系数，t
i
为积分系数，t
d
为微分系数；步骤1.7：采用一阶龙格库塔法进行四元素更新：式中，为四元素分量，为角速度分量，t为采样时间；步骤1.8：保留上一时刻的滤波值；步骤1.9：本发明对互补滤波算法进行了改进：设置阈值增量约束将当前时刻和上一时刻滤波差值与阈值增量约束进行比较，若满足则输出结果，否则重新执行，判定公式如下：|x
now
‑
x
last
|≤β
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(5)式中，x
now
为当前滤波值，x
last
为上一时刻滤波值，β为阈值增量约束,选取范围为[0,1]；步骤1.10：将机体坐标系下的当前状态信息进行转化处理，得到北东地坐标系下的状态信息。3.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤2自适应调整接纳圆半径的具体步骤为：步骤2.1.将路径离散为按顺序散布在路径上的路径点，提取每个直线路径单元上的实时参考路径：
式中，(x
r
,y
r
)是实时参考路径点坐标，(x
n
,y
n
)为预定的第n个航迹点坐标，(x,y)为无人船实时位置，θ
n
为连接航迹点(x
n
,y
n
)与(x
n 1
,y
n 1
)的路径方位角，为自适应接纳圆半径；步骤2.2.根据无人船初始状态，计算出每个路径点的最优接纳圆半径，以下是自适应接纳圆半径的选取方法：式中，l为船舶的长度，η为待定系数，选取范围为[1,5]，为自适应接纳圆的半径，r
min
为待定系数，选取船舶长度的1～2倍，r
max
为待定系数，选取船舶长度的3～5倍，θ为相邻路径夹角，γ为缓冲系数，选取范围为[
‑
1,1]。4.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤3基于自适应视线法期望航向角解算的具体内容为：利用自适应视线法，以当前位置为中心，选取无人船船长的3～5倍为半径，确定视距点p
los
，解算出期望航向角，公式如下：式中，(x
los
,y
los
)为视距位置，(x,y)为当前位置,ψ
los
为期望航向角。5.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤4基于自适应视线法路径段更新的具体内容为：在选择路径p
n 1
时，判断无人船是否在自适应接纳圆内，如果满足公式，将选择跟踪下一路径p
n 1
，公式如下：式中：为自适应接纳圆的半径，(x
n
,y
n
)为预定的第n个航迹点坐标，(x,y)为当前位置。6.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤5中提出的模型预测控制方法具体步骤为：步骤5.1.考虑坐标原点在重心处，忽略无人船的横摇、纵摇以及升降运动带来的影响，忽略在运动过程中的水流附加质量与水动力带来的附加科里奥利向心力，忽略非线性阻尼的影响，建立3自由度的非线性欠驱动无人船操纵模型：的影响，建立3自由度的非线性欠驱动无人船操纵模型：
式中，x,y,ψ是船舶在北东坐标下的位置，u,v,r是船体坐标系下的速度，τ
u
是推进器的力矩，τ
δ
是舵机或双推进器差速下的偏航力矩，m是船舶的质量，izz是绕o
z
轴的转动惯量，是船舶在纵荡、横荡、艏摇3个方向上加速度引起的附加质量，定义为负数，x
u
,y
v
,n
r
分别为船舶在各个方向上的水动力阻尼系数；步骤5.2.对欠驱动无人船操纵模型在期望路径点(x
r
,u
r
)处进行一阶泰勒展开线性化处理，得到如下的预测模型：式中，a、b为雅可比矩阵；步骤5.3.为了得到离散化状态方程，本发明采用前向欧拉的方法对该模型进行离散化处理，并化简为用控制增量形式表示的状态空间模型：理，并化简为用控制增量形式表示的状态空间模型：式中，步骤5.4.进行约束条件设置，在当前时刻及预测时域内，对控制增量、控制量、输出量进行如下限制：δu
min
≤δu(k i)≤δu
max
,i＝0,1,2,...,n
c
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(20)u
min
≤u(k i)≤u
max
,i＝0,1,2,...,n
c
‑1ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(21)y
min
≤y(k i)≤y
max
,i＝0,1,2,...,n
p
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(22)式中，n
c
是控制时域，n
p
是预测时域，δu(k i)为k i时刻的控制增量，u(k i)为k i时刻的控制量，y(k i)为k i时刻的输出量，δu
min
是控制增量最小值，δu
max
是控制增量最大值，u
min
是控制量最小值，u
max
是控制量最大值，根据推进器机械性能选取。y
min
是输出量最小值，y
max
是输出量最大值；步骤5.5.滚动优化确定当前的最优控制策略，使未来一段时间内被控变量与期望值偏差最小，构建关于系统的状态量偏差和控制增量的目标函数，通过改进的人工鱼群算法优
化最优控制序列，输出第一个控制分量作用到无人船上，目标函数如下：j＝(y
‑
y
ref
)
t
q(y
‑
y
ref
) δu
t
rδu
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(23)式中，y为当前时刻的输出值，y
ref
为自适应视线法处理后的期望值，δu为控制增量，q和r为权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j为性能指标；步骤5.6.等待无人船接收下一时刻的定位、姿态数据作为反馈信息。7.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤6利用改进的人工鱼群算法优化目标函数的具体步骤为：步骤6.1.选取目标函数的倒数作为适应度函数，对鱼群进行初始化，设置最大尝试次数为20，初始视野感知距离为1，拥挤因子δ为0.6，初始移动步长为0.1，最大迭代次数为50，适应度函数如下：式中，h(δu)为适应度值，y为当前时刻的输出值，y
ref
为自适应视线法处理后的期望值，δu为控制增量，q和r为权重矩阵，选取主对角线的值为整数并小于100的对角矩阵，j为性能指标；步骤6.2.计算初始鱼群各个个体的适应度值，取最优人工鱼的状态及其适应度值赋予给公告牌；步骤6.3进行觅食行为，本发明对人工鱼群算法进行了改进，在觅食阶段，当人工鱼用完尝试次数时，将视野范围扩大，增加寻优的机会，改进后的具体觅食方法如下：将视野扩大前的人工鱼适应度值与其视野感知范围内随机一条人工鱼的适应度值进行比较，如果比当前人工鱼的适应度高，则向适应度高的人工鱼方向移动一步，否则，随机选择另一条人工鱼进行比较，反复尝试20次后，如果仍不满足前进条件，将视野范围扩大λ倍，新的视野公式如下：visual
new
＝(1 λ)visual
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(25)式中：visual
new
是扩大以后的视野，visual是扩大前的视野，λ为待定系数，一般取50％～80％，在新的视野范围内，再次进行搜索，尝试次数用完后，如果搜索到适应度更高的位置，则向该方向移动一步，若没有搜索到适应度更高的位置，则执行随机移动一步；步骤6.4.进行聚群行为，在探索当前人工鱼视野范围内，满足下面公式时，表明中心不太拥挤，朝着中心位置前进一步，否则，进行觅食行为，公式如下：y
c
/n
f
＞δy
i1
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(26)式中，y
c
是中心位置人工鱼适应度值，n
f
是当前视野中的人工鱼数目，δ是拥挤因子，y
i1
是人工鱼当前状态的适应度值；步骤6.5.进行追尾行为，在探索当前人工鱼视野范围内，满足下面公式时，表明视野范围内随机选择的人工鱼周围不太拥挤，朝着该方向前进一步，否则，进行觅食行为，公式如下：y
j2
/n
f
＞δy
i2
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(27)式中，y
j2
是视野范围内随机选择的人工鱼适应度值，n
f
是当前视野中的人工鱼数目，δ是拥挤因子，y
i2
是人工鱼当前状态的适应度值；
步骤6.6.进行随机行为，在新的视野范围中随机选择一个状态，然后向该方向移动，公式如下：x
i|next
＝x
i3
round
·
visual
new
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(28)式中：x
i|next
为x
i3
的下一个状态，x
i3
为当前人工鱼的状态，visual
new
是扩大以后的视野，round是[
‑
1,1]区间的随机数；步骤6.7.对所有人工鱼个体进行评价，若某个体优于公告牌，则将公告牌更新为该个体；步骤6.8.当公告牌上最优解达到满意误差界内或者达到迭代次数上限时算法结果时，输出最优控制增量序列：δu
*
＝[δu(t|t)
* δu(t 1|t)
*
···
δu(t n
c
‑
1|t)
*
]
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(29)式中，δu(t i|t)
*
为t i时刻的最优控制增量分量，i＝1,2,
···
,n
c
1，δu
*
为最优控制增量序列。8.根据权利要求1所述的一种基于智能预测控制的水面无人船路径跟踪方法，其特征在于，所述步骤7实现路径跟踪控制器闭环反馈的具体内容为：解算出最优控制增量序列中第一个控制增量分量对应的推进器转速值，将其作用到无人船的推进系统，进行航向调整，在下一时刻，通过北斗和惯性导航传感器接收无人船的状态信息，判断无人船是否到达终点，如果没有到达，将其信息与下一时刻期望路径点的状态信息的差值作为模型预测控制器的输入，形成反馈，构成闭环控制，如果到达最后一个期望路径点，则终止循环。

技术总结
本发明针对水面无人船的欠驱动性和受时变风浪流扰动对其操纵性的影响，公开了一种基于智能预测控制的水面无人船路径跟踪方法。针对上电期间，数据偏差较大和高速航行时，陀螺仪出现数据漂移的问题。本发明提出数据融合算法，设置阈值增量约束改进了互补滤波算法，提高导航数据精度。针对无人船在恶劣环境下容易受到干扰，船体发生较大抖动，进入接纳圆时出现转向提前或滞后的问题。本发明提出自适应视线法，自动调整接纳圆半径，使无人船有充分时间调整航向，平缓的完成转向动作，避免偏离目标路径。针对模型预测控制器中用传统目标函数求解精度不高的问题。本发明提出改进的人工鱼群算法进行全局寻优。提高控制系统的抗干扰能力和算法的收敛速度。力和算法的收敛速度。力和算法的收敛速度。


技术研发人员：曾庆军 翁昱 戴晓强 赵强
受保护的技术使用者：江苏科技大学
技术研发日：2021.11.19
技术公布日：2022/1/4