一种基于正交配置的无人艇最短航路优化方法及系统与流程

本发明属于无人艇自主决策，更具体地，涉及一种基于正交配置的无人艇最短航路优化方法及系统。

背景技术：

1、随着无人化技术的发展，无人平台的自主智能化水平也在不断提高，无人艇航路规划作为无人化领域的热门研究方向，受到各研究院所、高校的广泛关注，相关理论研究主要集中在航行路径的可行性方面，例如在已知起点信息和终点信息的条件下规划出一条安全可行的航行路径，而对于在带有复杂约束条件的无人艇实际航行场景中，如何合理高效规划出使得航行距离等指标优化的研究较少。

2、动态优化作为最优化的一个分支，研究的主要问题是从一类可行方案中选出使得优化目标最优的一个方案，且同时满足指定约束要求，使得动态系统从给定的初始状态运行到终端状态。例如，在航天器航路设计问题中，在已知航天器自身属性及大气环境的条件下，通过动态优化方法求得从航行起点到终点满足航行约束且燃料消耗最少的路径。由此可见，无人艇航路规划问题即可处理为动态优化问题。在实际应用场景中由于动态系统的复杂性及不确定性等因素，导致相关的数学模型通常表现为强非线性，且对于大规模的应用问题，构建出的模型具有维度高、规模大的特点，增加了问题求解的难度。近些年，针对复杂动态优化问题的求解难点，提出快速可行的优化方案是学术领域和工业领域的重要研究方向。

3、正交配置方法作为求解动态优化问题的主流方法，通过离散化思想，将和时间有关的无限维的动态优化问题转化为有限维非线性规划问题，通过插值多项式逼近动态优化问题中的状态向量和控制向量，将微分方程转化为代数形式，从而避免了对微分方程进行数值积分带来的耗时问题，提升问题求解效率。基于动态优化领域的相关研究成果，研究适用于无人艇路径规划问题的有效方法是目前亟需解决的技术问题。

技术实现思路

1、针对现有技术的以上缺陷或改进需求，本发明提出了一种基于正交配置的无人艇最短航路优化方法及系统，针对无人艇执行航行任务时的最短航路规划问题，规划出一条满足航行约束且路径最短的航路。

2、为实现上述目的，按照本发明的一个方面，提供了一种基于正交配置的无人艇最短航路优化方法，包括：

3、获取无人艇在初始时刻的位置坐标、速度、航向角、航向角角速度、加速度，同时获取t时刻的位置坐标、速度、航向角、航向角角速度、加速度；

4、通过分析无人艇的动力学模型，构建动力学方程，通过分析无人艇在任务执行过程中受环境的影响，构建目标函数与约束条件；

5、对无人艇的速度、航向角、航向角角速度、加速度进行初始化；

6、对运动时域进行离散化，通过lagrange插值逼近状态向量和控制向量，获得近似微分方程及目标函数，最后，将无人艇最短航路动态优化问题转换为非线性规划问题，并通过非线性规划问题进行求解，得到最短航路轨迹。

7、在一些可选的实施方案中，由构建目标函数j与约束条件，其中，t0表示初始时刻，tf表示最终时刻，v(t)表示无人艇在t时刻的速度矢量的模长，状态向量为x(t)＝[x(t),y(t),θ(t),v(t)]t，(x(t),y(t))表示无人艇在t时刻的位置坐标，控制向量为u(t)＝[ω(t),a(t)]t，动力学方程简记作θ(t)表示无人艇在t时刻的航向角，ω(t)表示无人艇在t时刻的航向角的角速度，a(t)表示无人艇在t时刻的加速度，x(0)＝[x0,y0,θ0,v0]表示无人艇初始状态条件，x0,y0无人艇在初始时刻的位置坐标，θ0表示无人艇在初始时刻的航向角，v0表示无人艇在初始时刻的速度矢量的模长，(xbar,ybar)表示禁航区的圆心位置坐标，rbar表示禁航区的半径，θmin表示无人艇的最小航向角，θmax表示无人艇的最大航向角，vmin表示无人艇速度矢量模长的最小值，vmax表示无人艇速度矢量模长的最大值，ωmax表示无人艇航向角的角速度的最大值，amax表示无人艇加速度的最大值，(x(tf),y(tf))表示无人艇在tf时刻的位置坐标，(xter,yter)表示无人艇终端位置坐标。

8、在一些可选的实施方案中，所述对运动时域进行离散化，通过lagrange插值逼近状态向量和控制向量，获得近似微分方程及目标函数，包括：

9、将时域[t0,tf]划分为n段：t0＜t1＜…＜tk-1＜tk＜…＜tn＝tf，将时间变量t∈[t0,tf]转换到新的时域[-1,1]上，转换后的新时间节点为τki(i＝1,2,…,nk)，nk是[tk-1,tk]上的插值节点数；

10、利用lagrange插值多项式近似时间段[tk-1,tk]上的状态向量和控制向量：其中，xk(τki)和uk(τki)分别为状态向量和控制向量在[tk-1,tk]上第τki个节点处的值，简记为xki和uki，lki(τ)和分别为nk次和nk-1次lagrange插值多项式的基函数；

11、对近似后的状态向量求导得到其中，xk0为状态向量在第k个时间段初始时刻的值，代入时间节点τkj(j＝1,2,…,nk)，有构成微分矩阵dk，其中，n+1为微分矩阵dk的列数；

12、由微分方程得其中，分别为矩阵的第一列元素、剩余元素构成的矩阵；简记为δk为时间段[tk-1,tk]长度；

13、利用gauss_legendre求积公式将目标函数表示为：其中，ωkj为gauss_legendre求积系数。

14、在一些可选的实施方案中，由将无人艇最短航路动态优化问题转换为非线性规划问题，其中，ce、ci分别为原问题中的等式约束条件和不等式约束条件，ul、uu分别为控制向量的上下界。

15、按照本发明的另一方面，提供了一种基于正交配置的无人艇最短航路优化系统，包括：

16、信息采集模块，用于获取无人艇在初始时刻的位置坐标、速度、航向角、航向角角速度、加速度，同时获取t时刻的位置坐标、速度、航向角、航向角角速度、加速度；

17、模型构建模块，用于通过分析无人艇的动力学模型，构建动力学方程，通过分析无人艇在任务执行过程中受环境的影响，构建目标函数与约束条件；

18、相关参数初始化模块，用于对无人艇的速度、航向角、航向角角速度、加速度进行初始化；

19、系统状态求解模块，用于对运动时域进行离散化，通过lagrange插值逼近状态向量和控制向量，获得近似微分方程及目标函数，最后，将无人艇最短航路动态优化问题转换为非线性规划问题；

20、非线性规划问题求解模块，用于通过非线性规划问题进行求解，通过最优策略输出模块输出最短航路轨迹。

21、在一些可选的实施方案中，由构建目标函数j与约束条件，其中，t0表示初始时刻，tf表示最终时刻，v(t)表示无人艇在t时刻的速度矢量的模长，状态向量为x(t)＝[x(t),y(t),θ(t),v(t)]t，(x(t),y(t))表示无人艇在t时刻的位置坐标，控制向量为u(t)＝[ω(t),a(t)]t，动力学方程简记作θ(t)表示无人艇在t时刻的航向角，ω(t)表示无人艇在t时刻的航向角的角速度，a(t)表示无人艇在t时刻的加速度，x(0)＝[x0,y0,θ0,v0]表示无人艇初始状态条件，x0,y0无人艇在初始时刻的位置坐标，θ0表示无人艇在初始时刻的航向角，v0表示无人艇在初始时刻的速度矢量的模长，(xbar,ybar)表示禁航区的圆心位置坐标，rbar表示禁航区的半径，θmin表示无人艇的最小航向角，θmax表示无人艇的最大航向角，vmin表示无人艇速度矢量模长的最小值，vmax表示无人艇速度矢量模长的最大值，ωmax表示无人艇航向角的角速度的最大值，amax表示无人艇加速度的最大值，(x(tf),y(tf))表示无人艇在tf时刻的位置坐标，(xter,yter)表示无人艇终端位置坐标。

22、在一些可选的实施方案中，所述对运动时域进行离散化，通过lagrange插值逼近状态向量和控制向量，获得近似微分方程及目标函数，包括：

23、将时域[t0,tf]划分为n段：t0＜t1＜…＜tk-1＜tk＜…＜tn＝tf，将时间变量t∈[t0,tf]转换到新的时域[-1,1]上，转换后的新时间节点为τki(i＝1,2,…,nk)，nk是[tk-1,tk]上的插值节点数；

24、利用lagrange插值多项式近似时间段[tk-1,tk]上的状态向量和控制向量：其中，xk(τki)和uk(τki)分别为状态向量和控制向量在[tk-1,tk]上第τki个节点处的值，简记为xki和uki，lki(τ)和分别为nk次和nk-1次lagrange插值多项式的基函数；

25、对近似后的状态向量求导得到其中，xk0为状态向量在第k个时间段初始时刻的值，代入时间节点τkj(j＝1,2,…,nk)，有构成微分矩阵dk，其中，n+1为微分矩阵dk的列数；

26、由微分方程得其中，分别为矩阵的第一列元素、剩余元素构成的矩阵；简记为δk为时间段[tk-1,tk]长度；

27、利用gauss_legendre求积公式将目标函数表示为：其中，ωkj为gauss_legendre求积系数。

28、在一些可选的实施方案中，由将无人艇最短航路动态优化问题转换为非线性规划问题，其中，ce、ci分别为原问题中的等式约束条件和不等式约束条件，ul、uu分别为控制向量的上下界。

29、按照本发明的另一方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述任一项所述方法的步骤。

30、总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

31、能够根据无人艇运动学模型、关键约束行相继条件、目标函数等合理构建无人艇最短航路动态优化模型。基于正交配置的无人艇最短航路优化方法通过lagrange插值多项式逼近原问题中的状态向量和控制向量，从而将原问题转化为有限维的非线性规划问题，提升无人艇航路优化问题求解效率。