CN116466701A - 一种无人水面艇能耗最小轨迹规划方法 - Google Patents

Abstract

本发明提供了一种无人水面艇能耗最小轨迹规划方法，涉及无人水面艇运动规划技术领域，包括如下步骤：基于深度图像信息构建并更新无人水面艇目前所处环境下的三维占据栅格地图；构造带符号的欧式距离场；采用基于船舶运动学的A星算法，基于三维占据栅格地图进行前端路径点的搜索，得到路径点；采用直线对所述路径点进行曲线拟合，得到路径曲线基于欧式距离场，对针对速度、加速度、避障距离、曲线光滑度的无约束优化问题进行求解，得到避障的最优轨迹。本发明基于视觉信息进行无人艇的轨迹规划，可以对开放水域复杂环境下任意形状的障碍物进行避障避碰，无人艇可以根据周围障碍物环境做出实时局部规划。

Description

一种无人水面艇能耗最小轨迹规划方法

技术领域

本发明涉及无人水面艇运动规划技术领域，具体而言，尤其涉及一种无人水面艇能耗最小轨迹规划方法。

背景技术

近年来，对海洋的探索、开发和利用是当前沿海各个国家的发展重点。无人水面艇因其具有小型化、轻量化、自主化等特点，已经成为人们探索、开发海洋的重要工具。无人水面艇最重要的特点是任务执行的自主性。即便没有人类的干预，无人水面艇在确定航线和操作任务后，也能够进行自主导航，智能地避开障碍物，从而帮助人们减少不必要的繁琐工作。

随着无人水面艇的研究不断地深入，许多国家和实验机构开始广泛地研究无人水面艇的轨迹规划，并取得了非常多的进展和成果。根据优化目标的不同，无人水面艇的轨迹规划大致可以划分为基于时间最优的轨迹规划、基于能耗最优的轨迹规划和基于多目标最优的轨迹规划。其中，基于多目标最优的轨迹规划在过去的几年中受到了极大的关注，其目标是选取多个优化目标建立多目标优化模型，并引入权重系数调整各优化目标所占的优化权重以得到无人水面艇行驶的最优轨迹。轨迹规划的实际应用涵盖了广泛的海洋任务，如勘探开发、领海监测、航线规划。

在现有关于无人水面艇轨迹规划方法中，大多通过对通过高清地图或者电子海图进行规划，从现有无人水面艇轨迹规划的算法来看，会存在如下问题：第一，构建地图只是构建了较为简单的占据栅格地图，没有对地图进行进一步的优化，为避障提供更多信息，例如欧氏距离、梯度等，没有对任意形状的复杂障碍物进行避障避碰。第二，轨迹规划算法只是采用较为简单的图搜索算法或者快速探索随机树算法，并没有根据所处环境对速度、加速度、避障距离作进一步的优化，没有考虑运动物体的运动学模型，同时也没有考虑最小能耗和平滑航线的需求。第三，电子海图或者高清地图无法实时的展现周围障碍物信息，对无人水面艇所处的环境信息无法做到实时更新与完善，无法对距离较近的障碍物进行避障避碰，同时受电子海图栅格大小的限制，无法进行高效的轨迹规划。

发明内容

有鉴于此，本发明的目的在于提出一种无人水面艇能耗最小轨迹规划方法，利用基于视觉信息构造带符号的欧氏距离地图，在欧氏距离地图的基础上针对避障距离、速度、加速度、曲线光滑度构造无约束优化问题求解无人水面艇最优轨迹。

本发明采用的技术手段如下：

一种无人水面艇能耗最小轨迹规划方法，包括如下步骤：

使用深度相机采集无人水面艇目前所处环境的深度图像信息，基于深度图像信息构建并更新无人水面艇目前所处环境下的三维占据栅格地图；

采用欧几里得转换算法在所述三维占据栅格地图的基础上构造带符号的欧式距离场；

采用基于船舶运动学的A星算法，基于三维占据栅格地图进行前端路径点的搜索，得到路径点；

采用直线对所述路径点进行曲线拟合，得到路径曲线；

针对路径曲线，基于欧式距离场，构造针对速度、加速度、避障距离、曲线光滑度的无约束优化问题；

基于欧式距离场，对针对速度、加速度、避障距离、曲线光滑度的无约束优化问题进行求解，得到避障的最优轨迹。

进一步地，所述三维占据栅格地图的建立步骤为：

设栅格为m_i，栅格被占据的概率为p(m_i＝1)，栅格空闲的概率为p(m_i＝0)，记第t次的观测设置为z_t，，z₁、z₂、z₃、z₄...z_t，目标为计算占据栅格在已知t次观测上的后验概率，即p(m_i＝1|z_1:t)和p(m_i＝0|z_1:t)，由贝叶斯公式可得：

根据马尔科夫假设可得，当前观测值与前几次观测值独立无关，则有：

p(z_t|z_1:t-1,m_i)＝p(z_t|m_i) (2)

则栅格被占据或者空闲的概率如下所示：

设对栅格m_i，第t次的观测记作l_t(m_i)，前t-1次的观测记作l_t-1(m_i)，则上式整理可得如式：

p(m_i|z_t)为逆传感器模型，将其转换为传感器模型p(z_t|m_i)，转换公式如下所示：

将传感器模型带入(4)得：

其中：l_t(m_i)为对栅格m_i的第t次的观测值，l_t-1(m_i)为对栅格m_i的前t-1次观测值，p(z_t|z_1:t-1,m_i),p(z_t|m_i)为栅格m_i基于前1～t-1次的观测得到的第t次观测值与第t次观测值，p(m_i|z_t),为栅格空闲和栅格占据下的逆传感器模型，p(z_t|m_i),/>为栅格空闲和栅格占据下的传感器模型；

对于固定传感器，每次更新的栅格信息为固定值，随着传感器数据不断更新，则l_t(m_i)随着第t次观测不断更新，从而完成相机范围内三维占据栅格地图在第t次观测上的更新。

进一步地，所述欧式距离场的构造公式为：

D(p)＝min{(p-o)²+f(o)} (7)

其中：D(p)为空闲栅格p离最近障碍物栅格的距离，o为障碍物栅格，f(o)为用于存储构建高维带符号的欧式距离场的距离信息的采样函数。

进一步地，所述路径点的获取方式为：

将起始点及目标点的三维位置转化至栅格地图的索引号，并计算第一个扩展点的代价值；

搜索扩张节点控制空间采样迭代循环，同时计算计算规划到现在所耗的时间；

判断扩张节点判断节点在不在闭集合中，是否发生碰撞，利用庞特里亚金极值计算扩展节点损失值；

判断当前临时扩展节点与当前节点的其他临时扩展节点是否在同一个栅格中；

如果在同一个栅格中，则进行剪枝；要判断当前临时扩展节点的损失值是否比同一个栅格的对比损失值小，若比同一个栅格的对比损失值小，则更新这一栅格节点为当前临时扩展节点；

如果不剪枝，则首先判断当前临时扩展节点是否出现在开集合中，如果不出现在开集合中，则直接将临时扩展节点加入开集合中，如果存在于开集合但还未扩展的话，则比较当前临时扩展节点与对应栅格的损失值，若更小，则更新栅格中的节点。

进一步地，所述采用直线对所述路径点进行曲线拟合，得到路径曲线步骤中：

多段轨迹的变量D为：

多段轨迹构造的A矩阵为：

所述路径曲线的系数矩阵P_i为：

其中：T_i为轨迹之间的实际时间，为位置、速度或者加速度变量其中m代表轨迹的起始时刻或者结尾时刻，i为第i段轨迹，k为第k阶变量，A为多段轨迹起点和终点的位置、速度、加速度所构造的参数矩阵，D为轨迹所有离散点的位置、速度、加速度所构造的参数矩阵，P_in为第i段的第n个参数。

进一步地，针对速度、加速度、避障距离、曲线光滑度的无约束优化问题的形式为：

minλ₁f_s+λ₂f_o+λ₃(f_v+f_a) (20)

其中，关于曲线光滑度f_s构造问题的形式以及雅各比矩阵如下所示：

关于避障距离f_o构造问题的形式以及雅各比矩阵如下所示：

关于速度f_v构造问题的形式以及雅各比矩阵如下所示：

关于加速度f_a构造问题的形式以及雅各比矩阵如下所示：

其中：λ₁，λ₂，λ₃分别为曲线光滑度、避障距离、加速度对应的权重系数，D_f，D_p分别为固定参变量矩阵和待优化参变量矩阵，固定参变量包含起点和终点的位置、速度和加速度，待优化参变量则为起点与终点之间各个离散路径点的位置、速度以及加速度，C_t,A,Q为计算过程中路径曲线的系数矩阵参变量，三者相乘构成的对称矩阵用R_FF,R_PF,R_FP,R_PP进行表示，J_s，J_o，J_v,J_a表示所对应优化变量的一阶导数，c_a，c_v，c表示所对应加速度、速度、避障距离构造的罚函数，表示加速度、速度、避障距离构造罚函数的一阶导数，p_u(t),v_u(t),a_u(t)该段轨迹在t时刻下的避障距离、加速度、速度，G_a,G,F,v_u为计算过程中的中间变量矩阵，τ,δt避分别为第i段轨迹的总时间T_i和离散的时间区间，α为系数参变量。

本发明还提供了一种存储介质，所述存储介质包括存储的程序，其中，所述程序运行时，执行上述任一项无人水面艇能耗最小轨迹规划方法。

本发明还提供了一种电子装置，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器通过所述计算机程序运行执行上述任一项无人水面艇能耗最小轨迹规划方法。

较现有技术相比，本发明具有以下优点：

第一，与现有的无人水面艇运动规划算法相比，本发明基于视觉信息进行无人艇的轨迹规划，可以对开放水域复杂环境下任意形状的障碍物进行避障避碰，同时，该算法具有较好的实时性，无人艇可以根据周围障碍物环境做出实时局部规划。

第二，与现有的无人水面艇中基于A星的运动规划方法相比，本发明考虑了船舶运动学模型，将无人艇转弯的最小半径作为判断依据加入了路径点搜索中，同时设计了后端轨迹优化的部分，优化了无人水面艇转弯的角度等，降低了能耗。

第三，相较于传统的三维占据栅格地图作为运动规划的地图，本发明构建了带符号的欧式距离场，为后续的运动规划算法提供更多的梯度与距离信息，有利于无人水面艇轨迹的优化。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图做以简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。

图1为本发明流程图。

图2为本发明带符号的欧式距离场求解梯度的示意图。

图3为本发明构造无人水面艇周围带符号的欧式距离场的示意图。

图4为本发明得到的路径点进行直线拟合的示意图。

图5为本发明单个无人水面艇在复杂环境下进行自主导航的示意图。

图6为本发明单个无人水面艇在利用柏林算法构造的仿真环境下进行轨迹规划的示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

需要说明的是，本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

本发明提供了一种无人水面艇能耗最小轨迹规划方法，首先基于深度相机采集的深度图像信息，不断更新无人水面艇目前所处环境下的三维占据栅格地图，采用欧几里得转换算法在占据栅格地图的基础上构造带符号的欧式距离场。采用基于船舶运动学的A星算法进行前端路径点的搜索，将得到的路径点用直线进行拟合，利用三线性插值的方法进行梯度和距离的计算并在直线轨迹的基础上构造关于避障距离、速度、加速度、曲线光滑度的无约束优化问题，进行求解进而得到一条光滑、可以避障的最优轨迹。最后，利用柏林算法进行仿真平台的搭建，进行单艘无人水面艇轨迹规划的仿真实验。

A、三维占据栅格地图的建立

设栅格为m_i,栅格被占据的概率为p(m_i＝1)，栅格空闲的概率为p(m_i＝0).记第t次的观测设置为z_t，，z₁、z₂、z₃、z₄...z_t也可以被记作z_1:t，目标为计算占据栅格在已知t次观测上的后验概率，即p(m_i＝1|z_1:t)和p(m_i＝0|z_1:t)。由贝叶斯公式可得：

根据马尔科夫假设可得，当前观测值与前几次观测值独立无关，则有：

p(z_t|z_1:t-1,m_i)＝p(z_t|m_i) (2)

则栅格被占据或者空闲的概率如下所示：

设对栅格m_i，第t次的观测记作l_t(m_i)，前t-1次的观测记作l_t-1(m_i)，则上式整理可得如下式子：

p(m_i|z_t)为逆传感器模型，将其转换为传感器模型p(z_t|m_i)，转换公式如下所示：

将传感器模型带入(4)得：

已知对于固定传感器而言，每次更新的栅格信息为固定值，随着传感器数据不断更新。则l_t(m_i)随着第t次观测不断更新，从而完成相机范围内三维占据栅格地图在第t次观测上的更新。

B、带符号的欧式距离场的构建

设D(p)为空闲栅格p离最近障碍物栅格的距离，障碍物栅格设置为o，f(o)为采样函数，用于存储构建高维带符号的欧式距离场的距离信息，则构造公式如下所示：

D(p)＝min{(p-o)²+f(o)} (7)

将得到的地图进行栅格划分，设定固定的分辨率。将包含障碍物的栅格设置为0，将没有障碍物的栅格设置为1。首先不考虑y和z轴，在x轴求解每个栅格距离障碍物的最短距离，如果没有障碍物则f(o)为正无穷，其次，考虑x轴和y轴在每个有数值的障碍物栅格上，作为障碍物栅格，再次求解值为无穷的栅格距离最近有数值栅格的距离。考虑x轴、y轴和z轴的情况也如上述情况相同。以上的得到的距离场只在没有障碍物的部分构造了正向的距离场，在存在障碍物的部分构造反向距离的距离场则需要求解障碍物栅格距离最近空闲栅格的距离的负数，方法同上，则可以得到障碍物所在栅格距离为负值，空闲栅格距离为正值的带符号的欧式距离地图。通过三线性插值的方式计算轨迹点到达最近障碍物的距离，对所得距离函数进行求导即可得到沿x轴、y轴、z轴方向的梯度。设已知8个栅格的中心点位置的距离分别设为C₀₀₀、C₀₀₁、C₀₁₀、C₀₁₁、C₁₀₀、C₁₀₁、C₁₁₀、C₁₁₁，设插值点的距离为a、b、c、d、e、f、g，g相对于C₀₀₀的位置偏移量为(t_x,t_y,t_z)，则三线性插值求解距离的公式如下所示：

g沿x轴、y轴、z轴的梯度设对grad_x(g),grad_y(g),grad_z(g)，根据其他插值点求解梯度的公式如下所示：

C、基于船舶运动学的A星算法

将起始点及目标点的三维位置转化至栅格地图的索引号，并计算第一个扩展点的代价值，搜索扩张节点控制空间采样迭代循环(根据船舶运动学模型，分为a.最大转向左侧、b.最大转向右侧、c.直行、d.1/2最大转向左侧、e.1/2最大转向右侧。)，同时计算计算规划到现在所耗的时间，规划到现在所耗的时间＝现在的时刻-开始规划的时刻。判断扩张节点判断节点在不在闭集合中，是否发生碰撞，利用庞特里亚金极值计算扩展节点损失值。判断当前临时扩展节点与当前节点的其他临时扩展节点是否在同一个栅格中，如果是的话，就要进行剪枝。要判断当前临时扩展节点的损失值是否比同一个栅格的对比损失值小，如果是的话，则更新这一栅格节点为当前临时扩展节点。如果不剪枝的话，则首先判断当前临时扩展节点是否出现在开集合中，如果不是的话，则可以直接将临时扩展节点加入开集合中。如果存在于开集合但还未扩展的话，则比较当前临时扩展节点与对应栅格的损失值,若更小，则更新栅格中的节点。所依据船舶运动学公式如下所示：

设节点当前状态为(x₀，y₀，ψ₀),扩张之后，节点状态为(x_t，y_t，ψ_t)进行节点扩张的公式如下所示：

基于庞特里亚金极值的代价函数计算，确定扩张点和终点的速度和位置，以最小加速度为目的计算最有代价，设计J(T)为目标代价函数，α_μ、β_μ为系数变量，扩张点的状态变量设置为(p_μc，v_μc)，终点的状态变量设置为(p_μg，v_μg)式如下所示：

其中由上式计算得到最优代价J(T)，作为由扩张点选取路径点的依据。

D、采用直线对路径点进行曲线拟合

上述由基于船舶运动学的A星得到一系列点设置为p₀,p₁,p₂,...,p_n，、由此产生n段轨迹，设第i段矩阵映射矩阵为A_i，设给定速度v_c、给定加速度为a_c、给定时间t_c、给定距离d_c、轨迹之间的实际距离记为s_i、轨迹之间的实际时间记为T_i。则粗略计算该段轨迹的时间公式如下所示：

如果s_i＜2d_c，则根据如下式计算时间：

如果s_i＞2d_c，则根据如下式计算时间：

对于第i段轨迹，构造的A_i矩阵如下所示：

设变量引，m代表轨迹的起始时刻或者结尾时刻，i代表第i段轨迹，k代表第k阶变量，则多段轨迹的变量D表示如下所示：

多段轨迹构造的A矩阵如下所示：

设每段轨迹的系数矩阵为P_i，且第i段轨迹的表达式如下所示：

则所有轨迹的系数矩阵P的表达式计算如下所示：

E、构造针对速度、加速度、避障距离、曲线光滑度的无约束优化问题构造问题的形式如下所示：

minλ₁f_s+λ₂f_o+λ₃(f_v+f_a) (20)

其中关于曲线光滑度f_s构造问题的形式以及雅各比矩阵如下所示：

关于避障距离f_o构造问题的形式以及雅各比矩阵如下所示：

关于速度f_v构造问题的形式以及雅各比矩阵如下所示：

关于加速度f_a构造问题的形式以及雅各比矩阵如下所示：

对上述无约束优化问题进行求解，进而得到最优轨迹。

如图1所示，一种无人水面艇能耗最小轨迹规划方法，包括以下步骤：

第一步，根据所采集的视觉信息根据三线性插值算法构建带符号的欧式距离场，如图2和图3所示，根据距离场提供的信息，采用基于船舶运动学的A星算法进行前端路径点的搜索得到所需要的路径点，基于庞特里亚金极值原理计算扩张路径点距离终点的最优代价进行路径点的选择并进行直线拟合如图3和图4所示。

第二步，在得到拟合直线的基础上构造关于速度、加速度、曲线光滑度、避障距离的无约束优化问题，求解获得最优轨迹。为后续无人水面艇的运动控制提供位置、速度、偏航角等信息。如图4和图5所示，即为无人水面艇行驶过程中的直线轨迹以及优化过后的黑色轨迹。

在开始规划前，无人水面艇初始位姿所在地为世界坐标系的原点，随机进行目标地点的选择，无人水面艇在感知区域的范围内进行带符号的欧式距离场的构建，并不断对所规划的轨迹进行碰撞检测，如果轨迹被检测到碰到障碍物，则会在当前安全位置进行重规划以避开障碍物。当检测到离目标点距离较近时，则轨迹规划停止，并且到达终点的速度接近于0，由于考虑了曲线光滑度作为优化目标，所以尽可能地降低了无人水面艇运动过程中的能量损失。

本发明还提供了一种存储介质，所述存储介质包括存储的程序，其中，所述程序运行时，执行上述任一项无人水面艇能耗最小轨迹规划方法。

本发明还提供了一种电子装置，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器通过所述计算机程序运行执行上述任一项无人水面艇能耗最小轨迹规划方法。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

在本发明的上述实施例中，对各个实施例的描述都各有侧重，某个实施例中没有详述的部分，可以参见其他实施例的相关描述。

在本申请所提供的几个实施例中，应该理解到，所揭露的技术内容，可通过其它的方式实现。其中，以上所描述的装置实施例仅仅是示意性的，例如所述单元的划分，可以为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，单元或模块的间接耦合或通信连接，可以是电性或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可为个人计算机、服务器或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、移动硬盘、磁碟或者光盘等各种可以存储程序代码的介质。

最后应说明的是：以上各实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述各实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分或者全部技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (8)

1.一种无人水面艇能耗最小轨迹规划方法，其特征在于，包括如下步骤：

使用深度相机采集无人水面艇目前所处环境的深度图像信息，基于深度图像信息构建并更新无人水面艇目前所处环境下的三维占据栅格地图；

采用欧几里得转换算法在所述三维占据栅格地图的基础上构造带符号的欧式距离场；

采用基于船舶运动学的A星算法，基于三维占据栅格地图进行前端路径点的搜索，得到路径点；

采用直线对所述路径点进行曲线拟合，得到路径曲线；

针对路径曲线，基于欧式距离场，构造针对速度、加速度、避障距离、曲线光滑度的无约束优化问题；

基于欧式距离场，对针对速度、加速度、避障距离、曲线光滑度的无约束优化问题进行求解，得到避障的最优轨迹。

2.根据权利要求1所述的无人水面艇能耗最小轨迹规划方法，其特征在于，所述三维占据栅格地图的建立步骤为：

设栅格为m_i，栅格被占据的概率为p(m_i＝1)，也可记为p(m_i)栅格空闲的概率为p(m_i＝0)，也可记为记第t次的观测设置为z_t,，z₁、z₂、z₃、z₄...z_t，目标为计算占据栅格在已知t次观测上的后验概率，即/>由贝叶斯公式可得：

根据马尔科夫假设可得，当前观测值与前几次观测值独立无关，则有：

p(z_t|z_1:t-1,m_i)＝p(z_t|m_i) (2)

根据贝叶斯公式，则栅格被占据或者空闲的概率如下所示：

则上式整理可得如式：

将传感器模型与逆传感器模型之间进行转换，转换公式如下所示：

将传感器模型带入(4)得：

其中：l_t(m_i)为对栅格m_i的第t次的观测值，l_t-1(m_i)为对栅格m_i的前t-1次观测值，p(z_t|z_1:t-1,m_i),p(z_t|m_i)为栅格m_i基于前1～t-1次的观测得到的第t次观测值与第t次观测值，为栅格空闲和栅格占据下的逆传感器模型，/>为栅格空闲和栅格占据下的传感器模型；

对于固定传感器，每次更新的栅格信息为固定值，随着传感器数据不断更新，则l_t(m_i)随着第t次观测不断更新，从而完成相机范围内三维占据栅格地图在第t次观测上的更新。

3.根据权利要求1所述的无人水面艇能耗最小轨迹规划方法，其特征在于，所述欧式距离场的构造公式为：

D(p)＝min{(p-o)²+f(o)} (7)

其中：D(p)为空闲栅格p离最近障碍物栅格的距离，o为障碍物栅格，f(o)为用于存储构建高维带符号的欧式距离场的距离信息的采样函数。

4.根据权利要求1所述的无人水面艇能耗最小轨迹规划方法，其特征在于，所述路径点的获取方式为：

将起始点及目标点的三维位置转化至栅格地图的索引号，并计算第一个扩展点的代价值；

搜索扩张节点控制空间采样迭代循环，同时计算计算规划到现在所耗的时间；

判断扩张节点判断节点在不在闭集合中，是否发生碰撞，利用庞特里亚金极值计算扩展节点损失值；

判断当前临时扩展节点与当前节点的其他临时扩展节点是否在同一个栅格中；

如果在同一个栅格中，则进行剪枝；要判断当前临时扩展节点的损失值是否比同一个栅格的对比损失值小，若比同一个栅格的对比损失值小，则更新这一栅格节点为当前临时扩展节点；

如果不剪枝，则首先判断当前临时扩展节点是否出现在开集合中，如果不出现在开集合中，则直接将临时扩展节点加入开集合中，如果存在于开集合但还未扩展的话，则比较当前临时扩展节点与对应栅格的损失值，若更小，则更新栅格中的节点。

5.根据权利要求1所述的无人水面艇能耗最小轨迹规划方法，其特征在于，所述采用直线对所述路径点进行曲线拟合，得到路径曲线步骤中：

多段轨迹的变量D为：

多段轨迹构造的A矩阵为：

所述路径曲线的系数矩阵P_i为：

其中：T_i为轨迹之间的实际时间，为位置、速度或者加速度变量其中m代表轨迹的起始时刻或者结尾时刻，i为第i段轨迹，k为第k阶变量，A为多段轨迹起点和终点的位置、速度、加速度所构造的参数矩阵，D为轨迹所有离散点的位置、速度、加速度所构造的参数矩阵，P_in为第i段的第n个参数。

6.根据权利要求1所述的无人水面艇能耗最小轨迹规划方法，其特征在于，针对速度、加速度、避障距离、曲线光滑度的无约束优化问题的形式为：

minλ₁f_s+λ₂f_o+λ₃(f_v+f_a) (20)

其中，关于曲线光滑度f_s构造问题的形式以及雅各比矩阵如下所示：

关于避障距离f_o构造问题的形式以及雅各比矩阵如下所示：

关于速度f_v构造问题的形式以及雅各比矩阵如下所示：

关于加速度f_a构造问题的形式以及雅各比矩阵如下所示：

其中：λ₁，λ₂，λ₃分别为曲线光滑度、避障距离、加速度对应的权重系数，D_f，D_p分别为固定参变量矩阵和待优化参变量矩阵，固定参变量包含起点和终点的位置、速度和加速度，待优化参变量则为起点与终点之间各个离散路径点的位置、速度以及加速度，C_t,A,Q为计算过程中路径曲线的系数矩阵参变量，三者相乘构成的对称矩阵用R_FF,R_PF,R_FP,R_PP进行表示，J_s，J_o，J_v,J_a表示所对应优化变量的一阶导数，c_a，c_v，c表示所对应加速度、速度、避障距离构造的罚函数，表示加速度、速度、避障距离构造罚函数的一阶导数，p_u(t),v_u(t),a_u(t)该段轨迹在t时刻下的避障距离、加速度、速度，G_a,G,F,v_u为计算过程中的中间变量矩阵，τ,δt避分别为第i段轨迹的总时间T_i和离散的时间区间，α为系数参变量。

7.一种存储介质，其特征在于，所述存储介质包括存储的程序，其中，所述程序运行时，执行所述权利要求1至6中任一项权利要求所述的无人水面艇能耗最小轨迹规划方法。

8.一种电子装置，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器通过所述计算机程序运行执行所述权利要求1至6中任一项权利要求所述的无人水面艇能耗最小轨迹规划方法。