CN114216463B

CN114216463B - 一种路径优化目标定位方法及装置、存储介质、无人设备

Info

Publication number: CN114216463B
Application number: CN202111300175.0A
Authority: CN
Inventors: 梁睿; 张颖; 徐升; 李长玉; 马肖一; 仝新宇; 宋兴旺; 吴琪; 李明泰
Original assignee: State Grid Tianjin Electric Power Co Chengxi Power Supply Branch; State Grid Corp of China SGCC; State Grid Tianjin Electric Power Co Ltd
Current assignee: State Grid Tianjin Electric Power Co Chengxi Power Supply Branch; State Grid Corp of China SGCC; State Grid Tianjin Electric Power Co Ltd
Priority date: 2021-11-04
Filing date: 2021-11-04
Publication date: 2024-05-28
Anticipated expiration: 2041-11-04
Also published as: CN114216463A

Abstract

本发明公开了一种路径优化目标定位方法及装置、存储介质、无人设备，通过提出的新型路径优化技术，实现单个无人机/无人艇在区域安防中的(如异物违规侵入、人员搜救等)目标快速精确估计定位。其中主要以路径优化的策略改善目标的“定位速度”、“定位精度”的问题，并且解决传统梯度法路径优化技术中的局部最优陷阱问题。

Description

一种路径优化目标定位方法及装置、存储介质、无人设备

技术领域

本发明涉及无人机应用技术领域，尤其涉及一种路径优化目标定位方法及装置、存储介质、无人设备。

背景技术

无人机、无人艇目前开始被广泛应用于区域安防、目标搜救、军事监控等实际应用中。在这些应用中，对于特定目标位置、运动状态信息的精确获取愈发迫切，特别在应急、安防、军事场景下对于定位精度的要求更加严格，迫切需要本专利快速准确的确定目标的信息。

目前所流行的目标定位技术，主要分为主动式及被动式两种。顾名思义，主动定位技术即是通过无人机/艇所搭载的智能设备主动探测发出相关信号测定目标位置信息，与之相反，被动定位方法仅通过采集目标的相关信息或发出的信号，进而估计目标所在位置的关键信息。而这关键区别在于是否需要主动发出某种特殊信号。借助方位角(Angle-of-Arrival,AOA)定位是一种经典的被动目标定位方法，已广泛应用于军事和民用领域。在进行AOA定位时，根据多次不同位置测量的三角几何关系确定目标的位置、速度等信息。换而言之，利用AOA测量，可以通过跟踪估计器算法来估计目标状态量。这意味着传感器的测量值和传感器的位置分布对目标定位起着至关重要的作用，因此搭载AOA传感器的无人机/艇的路径优化十分重要。

相较其他定位技术，AOA量测可以直接通过相机、声呐、雷达接收器获取，而且AOA定位技术对于周围环境没有特殊要求，系统复杂性较低，可以实现较高精度的定位，避免了定位的模糊性，被广泛应用于各种定位领域，尤其在区域入侵目标的快速追踪、搜救等领域。然而要实现快速高精度的AOA目标定位需要对观测位置开展相关优化。目前关于搭载传感器路径优化的技术，主要围绕遍历搜寻，而以改善目标估计精度的路径优化较少，主要应用的技术为梯度下降法、寻点法优化。现有方法存在以下主要问题亟待解决：1、现有区域巡逻安防技术大多不考虑目标定位精度的提升，仅以遍历区域为目的；2、现有以改善目标估计精度为首要目标的梯度路径优化技术存在局部最优陷阱的问题；3、现有方法目标估计精度改善速度不够快。

因此，设计新型路径优化策略，以实现入侵目标估计精度的快速提升，尤为重要。

发明内容

本申请的目的是提供一种路径优化目标定位方法及装置、存储介质、无人设备，通过提出的新型路径优化技术，实现单个无人机/无人艇在区域安防中的(如异物违规侵入、人员搜救等)目标快速精确估计定位。其中主要以路径优化的策略改善目标的“定位速度”、“定位精度”的问题，并且解决传统梯度法路径优化技术中的局部最优陷阱问题。

为实现本申请的目的，本申请提供的技术方案如下：

第一方面

本申请提供了一种路径优化目标定位方法，包括如下步骤：

步骤1：初始化目标的位置估计值和协方差矩阵值；

步骤2：

初次计算时：获取基于无人设备所在位置的AOA传感器量测值及无人设备当前位置信息，结合步骤1得到的位置估计值和协方差矩阵值，运行伪线性卡尔曼滤波算法，求解得到新的位置估计值及协方差矩阵值并更新；

循环计算时：获取基于无人设备所在位置的AOA传感器量测值及无人设备当前位置信息，结合上一次计算时得到的位置估计值和协方差矩阵值，运行伪线性卡尔曼滤波算法，求解得到新的位置估计值及协方差矩阵值并更新；

步骤3：包括如下步骤：

步骤3.1计算初始步长；

步骤3.2基于梯度下降的优化算法，结合步骤3.1以及步骤2中输出的数据，求解一个无人设备最佳观测位置。

步骤3.3根据步骤3.2的位置，确定多个其他可能的最佳观测位置，并求解所有可能最优位置点相应的协方差阵；

步骤3.4将所有位置进行对比，最终确定最佳观测位置。

步骤4：无人设备移动至步骤3中的获得的最佳观测位置，并跳转至步骤2，循环计算，直至协方差矩阵值达到要求或使用者终止。

其中，所述无人设备为无人机或无人艇。

其中，所述初始步长为百米级。

其中，在步骤3.3中，采用寻点法确定多个其他可能的最佳观测位置，额外选取在所述步骤3.2中确定好的最佳观测位置的对面以及左右90度位置的3个点作为最佳观测位置点。

第二方面

本申请提供了一种路径优化目标定位装置，包括如下单元：

第一单元，所述第一单元用于初始化目标的位置估计值和协方差矩阵值；

第二单元，所述第二单元用于：

初次计算时：获取基于无人设备所在位置的AOA传感器量测值及无人设备当前位置信息，结合第一单元得到的位置估计值和协方差矩阵值，运行伪线性卡尔曼滤波算法，求解得到新的位置估计值及协方差矩阵值并更新；

第三单元，所述第三单元用于：计算初始步长；基于梯度下降的优化算法，结合初始布长以及新的位置估计值及协方差矩阵值、AOA传感器量测值、无人设备当前位置信息，求解第一个无人设备最佳观测位置；根据确定的最佳观测位置，确定额外多个其他可能的最佳观测位置，并求解所有可能最佳观测位置相应的协方差阵；将所有位置进行对比，最终确定最佳观测位置；无人设备移动至最终确定的最佳观测位置。

其中，所述无人设备为无人机或无人艇。

其中，所述初始步长为百米级。

其中，第三单元采用寻点法确定多个其他可能的最佳观测位置，额外选取第一个无人设备最佳观测位置的对面以及左右90度位置的3个点作为最佳观测位置点。

第三方面

本申请提供了一种存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现上述的路径优化目标定位方法。

第四方面

本申请提供了一种终端设备，其特征在于，所述终端设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现上述的路径优化目标定位方法。

与现有技术相比，本发明的有益效果为，本专利在现有梯度路径优化法基础上进行了步长自适应的改进，以及设计了额外寻点的双保险，旨在更加快速的发现对于提升目标估计精度的最佳观测位置。因此，使得现有技术在目标估计精度的改善速度方面产生明显提高，即又快又准的实现单无人机/艇区域对入侵目标运动位置、速度的估计。

附图说明

图1所示为本申请的方法流程示意图；

图2所示为本申请实施例中单架无人机目标追踪及方位角量测示意图；

图3所示为现有梯度法路径优化技术的两个问题示意图；

图4所示为不同适步长对路径优化在寻找全局最优点的作用示意图；

图5所示为应用不同值的实时梯度值示意图；

图6所示为寻点策略中的额外点确定示意图；

图7所示为第一情况下采用所提方案与现有定步长梯度法的仿真效果比较图；

图8所示为第二情况下采用所提方案与现有定步长梯度法的仿真效果比较图。

具体实施方式

需要说明的是，在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。

以下结合附图和具体实施例对本发明作进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用属于“包含”和/或“包括”时，其指明存在特征、步骤、操作、部件或者模块、组件和/或它们的组合。

应该理解，当本申请文件中称部件被“连接”到另一部件时，它可以直接连接到其他元件，或者也可以存在中间部件。这里使用的措辞“和/或”包括一个或更多个相关联的列出项的任一单元和全部组合。

本发明主要解决在无人机(或水下无人艇)区域安防巡逻中的入侵目标追踪侦察等中的目标位置快速精确定位问题。较现有方法、技术，本策略估计算法更具稳定性，路径优化技术较现有梯度法更为高效，能够更加快速的改善目标估计精度。

在本发明中，首先介绍基于目标方位角量测的伪线性卡尔曼滤波目标估计算法，进而详细分析现有基于梯度法的路径优化技术中局部陷阱问题。其次，本专利提出了一种结合梯度试算步长自适应的改进型无人机/无人艇路径优化方法，同时设计了一种额外路径点寻点算法，以弥补在目标估计初期，目标估计不准确时梯度路径优化法精确性欠佳的问题。利用所提出的方法，采用单架无人机/无人艇便可以快速实现目标的准确定位。

如图1所示，本申请提供的一种路径优化目标定位方法，包括如下步骤：

步骤1：初始化目标的位置估计值和协方差矩阵值；

初始化目标的位置估计值，初始化协方差矩阵值，以上两个初始状态用于步骤2中。

步骤2：

初次计算过程：获取基于无人设备所在位置的AOA传感器量测值及无人设备当前位置信息，结合步骤1得到的位置估计值和协方差矩阵值，运行伪线性卡尔曼滤波算法，求解得到新的位置估计值及协方差矩阵值并更新；

循环计算过程：获取基于无人设备所在位置的AOA传感器量测值及无人设备当前位置信息，结合上一次计算时得到的位置估计值和协方差矩阵值，运行伪线性卡尔曼滤波算法，求解得到新的位置估计值及协方差矩阵值并更新；

步骤3：包括如下步骤：

步骤3.1计算初始步长；本步骤中，采用本专利提出的梯度算法中初始步长的解算方法，求出初始步长输入下一步算法；采用本专利提出的步长自适应算法，编写程序整体作为下一步算法的一部分。

步骤3.2基于梯度下降的优化算法，结合步骤3.1以及步骤2中输出的数据，求解一个无人设备最佳观测位置。本步骤中，运行梯度下降优化算法，求出一个无人机最佳观测位置。

步骤3.4将所有位置进行对比，最终确定最佳观测位置。

下面提供了一种无人机/无人艇路径优化目标定位方法的具体实施例。

本实施例中，采用一架装备了AOA(Angle-of-arrival，目标方向角)传感器的无人机/无人艇，其采用恒定标量值的移动速度，机载传感器可以在离散时刻k＝1,2,3…获取关于目标的不同方位角量测值，进而通过结合解析几何的方法以及相应的估计算法解算目标状态。角度测量示意图如图2所示。

具体而言，理想的方向角测量数学模型如下：

其中p_k＝[p_xk,p_yk]和r_k＝[r_xk,r_yk]分别是在时间k时刻的目标和无人机自身的位置。||·||是欧几里得范数，tan^-1是四象限反正切函数。不可避免的，无人机飞行时受风、发动机震动等因素影响，机载传感器量测中含有较大高斯噪声，而会影响目标定位精度。具体而言，包含噪声的方位角测量模型如下：

其中n和m分别是均值为零，方差为和/>的独立加性高斯白噪声。无人机自身位置和速度可由机载导航设备获取，明确已知。观察方向角测量模型公式，可发现角度测量和目标状态之间具有非线性关系。本专利的目标便是由这些具有噪声的非线性量测信息中获取多个目标的准确位置、速度估计值。

本专利以二维环境为例，该技术同样可以推广至三维空间相关问题。对于二维环境空间中目标的状态向量定义为该目标的动态模型满足

其中T表示离散时刻k与k+1之间的时间步长。q_k表示系统过程噪声。Q_k是过程噪声的协方差矩阵，W_k|k表示更新的估计协方差矩阵。因此，伪线性卡尔曼滤波(pseudolinearKalmanfilter,PLKF)的算法如下：

第一步，状态预测：

x_k|k-1＝Ux_k-1|k-1

W_k|k-1＝UW_k-1|k-1U^T+Q_k-1

第二步，量测增益解算：

第三步，结合状态预测及量测增益进行最终状态更新：

x_k|k＝x_k|k-1+k_ky_k

路径优化目的是通过改变无人机自身位置，以获取更加有效的量测信息，进而快速提高目标状态估计的精度。在k时刻时，无人机获得目标的方位角测量值，在获得下一时刻量测前，执行估计、路径优化算法并改变飞行路径以抵达更佳的观测位置。

首先，为了提高目标估计精度，选取PLKF估计器中表示目标定位精度的协方差矩阵的迹为优化目标函数，即随着协方差迹的减小，目标定位估计的精度将获得改善。其数学模型为：

J(r_k)＝tr(W_k|k) (5.6)

其中，tr(·)表示矩阵求迹。为了最小化(5.6)，很多数学优化算法可以使用，例如梯度下降法。各个时刻的飞行路径点满足

r_k+1-r_k＝-vT·ΔJ (5.7)

其中，vT表示无人机一定时间内的飞行距离，ΔJ为基于估计协方差阵的代价函数梯度。二维问题中，该梯度可表示为：

其中，需要用到的假想位置量测可以通过实时估计状态进行解算，δ表示一段距离，为梯度法中的测试距离，当其接近于0时可以近似为本地梯度。下面本专利对J(r_xk+δ)的解算进行实例求解。首先，可以获得

其中(1)及(3)表示前面矢量的第1及第3个元素。进而将(5.9)中的结果带入(5.4)式可以求解h_xk+δ以及ξ_xk+δ，进而根据

求出所需要的结果：

J(r_xk+δ)＝tr(W_k|k,xk+δ) (5.11)

类似的其他三个变量也可求得，从而根据公式(5.8)及(5.7)求解出下一时刻的最优位置点，实现目标追踪估计改善中的路径优化。

由于梯度路径优化法中的下一刻最优路径点的解算依赖本地实时的目标状态估计值，因此当目标位置估计不准确时，路径优化的结果并不理想，尤其在目标估计追踪初期最为严重。另外，在梯度算法中，步长距离δ的选择将直接影响梯度法的性能。其中(1)如果步长距离设置过小，则在梯度路径优化的最佳路径点解算中容易陷入局部最优陷阱点，而由于步长过小的限制，无法摆脱。而(2)如果步长距离设置过大，则在最佳路径解算中可能错过全局最优点，在其周围往复盘旋。图3中直观表示了现有梯度法路径优化技术的这两个问题。

而图4中直观表示了采用不用合适步长时对于整体路径优化在寻找全局最优点的作用。

为了克服上述梯度路径优化法中的问题，本专利设计并开发了一套改进技术，主要包括两个部分：(1)步长自适应策略及初始步长的测试确定法，及(2)特殊方向寻点核算。

根据现有梯度法优化结合伪线性卡尔曼目标估计精度的特点，即目标的估计精度会随路径优化时间的进展而改善，因此局部最优仅在目标估计追踪的初期影响明显，而后期则需要保障步长较小能够抵达全局最优点。因此，本专利设计

其中K表示运行时间上限，δ₀表示初始步长，将影响目标估计及路径优化的最终效果。为了确定初始步长，本专利提出了一种基于仿真测试的方法。结合实际应用，确定在几平方千米区域内，设置x_0|0＝[1400,16,800,23]^T，W_0|0＝diag(10⁴,10⁴,10⁴,10⁴)，并且目标自(1000,1000)m匀速移动，速度为(5,5)m/s。之后本专利采用不同的δ值进行仿真，并解算出k＝3时的梯度代价函数值，如图5所示。根据上图可以发现，采用线性段变化的步长可以避免梯度值得量级过大或过小引发的跳变，进而产生过多的局部最优陷阱。因此本专利在此类问题中，选用初始步长为百米级，如，此数量级的初始步长为本专利所提出的一个核心技术细节。

之后，为了进一步避免陷入局部最优陷阱，本专利设计了四个额外点优化代价函数的寻点法解算方法。当改进的梯度法解算完成后，本专利在已确定好的梯度法路径优化点的对面以及左右90度位置额外选取3个点，解算该4个路径点的代价函数值，4个点的选取如图6所示，

至此，采用寻点法解算

具体而言，将4个待定点的位置带入公式(5.8)至(5.11)解算各个位置的代价函数，最终进行对比选用最小的位置，作为下一时刻最佳路径点。

本发明中的技术经过了MATLAB仿真模拟验证。仿真模拟中，应用单个无人机来追踪定位移动目标。仿真中模拟了两种不同情况，目标运动状态及初始位置均不同，以验证所提方案的泛用性能。在目标追踪任务中，无人机初始航向与x轴正方向相同。模拟中传感器采样时间间隔为1秒，路径优化间隔也为1秒。

仿真验证中，所提出的改进型路径优化方法的目标估计精度始终最优，这验证了所提技术有效性。根据仿真获得的目标估计的代价函数(即PLKF的协方差阵的迹)和无人机与目标的完整运动轨迹结果之后，本专利可以得出结论，所提方法可以又快又准的实现目标估计。两次不同目标估计追踪任务的仿真验证效果如图7和8所示。

另外，与本申请提供的方法相对应，本申请还提供一种路径优化目标定位装置，包括如下单元：

第二单元，所述第二单元用于：

另外，本申请还提供了一种存储介质，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现所述的路径优化目标定位方法。

另外，本申请还提供了一种终端设备，所述终端设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现所述的路径优化目标定位方法。终端设备为无人机或无人艇或其他无人设备。

需要说明的是，本申请中未详述的技术方案，采用公知技术。

以上所述仅是本发明的优选实施方式，应当指出的是，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims

1.一种路径优化目标定位方法，其特征在于，包括如下步骤：

步骤1：初始化目标的位置估计值和协方差矩阵值；

步骤2：

步骤3：包括如下步骤：

步骤3.1计算初始步长；

步骤3.2基于梯度下降的优化算法，结合步骤3.1以及步骤2中输出的数据，通过步长自适应策略计算优化步长，根据优化步长，求解一个无人设备最佳观测位置；其中，步长自适应策略如下：

；

其中，表示运行时间上限，/>表示初始步长，初始步长基于仿真测试的方法确定，所述初始步长为百米级；

步骤3.3根据步骤3.2的位置，确定多个其他可能的最佳观测位置，并求解所有可能最优位置点相应的协方差阵；其中，在步骤3.3中，采用寻点法确定多个其他可能的最佳观测位置，额外选取在所述步骤3.2中确定好的最佳观测位置的对面以及左右90度位置的3个点作为最佳观测位置点，所述寻点法的解算方法，具体如下：

；

解算各个位置的代价函数，最终进行对比选用最小的位置，作为下一时刻最佳路径点；

步骤3.4将所有位置进行对比，最终确定最佳观测位置；

2.根据权利要求1所述的一种路径优化目标定位方法，其特征在于，所述无人设备为无人机或无人艇。

3.一种路径优化目标定位装置，其特征在于，包括如下单元：

第二单元，所述第二单元用于：

第三单元，所述第三单元用于：计算初始步长；基于梯度下降的优化算法，结合初始布长以及新的位置估计值及协方差矩阵值、AOA传感器量测值、无人设备当前位置信息，通过步长自适应策略计算优化步长，根据优化步长，求解一个无人设备最佳观测位置；其中，步长自适应策略如下：

；其中，/>表示运行时间上限，表示初始步长，初始步长基于仿真测试的方法确定，所述初始步长为百米级；

根据确定的最佳观测位置，确定额外多个其他可能的最佳观测位置，并求解所有可能最佳观测位置相应的协方差阵；第三单元采用寻点法确定多个其他可能的最佳观测位置，额外选取第一个无人设备最佳观测位置的对面以及左右90度位置的3个点作为最佳观测位置点；所述寻点法的解算方法，具体如下：

；

将所有位置进行对比，最终确定最佳观测位置；无人设备移动至最终确定的最佳观测位置。

4.根据权利要求3所述的一种路径优化目标定位装置，其特征在于，所述无人设备为无人机或无人艇。

5.一种存储介质，其特征在于，所述存储介质中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由处理器加载并执行以实现如权利要求1至2中任一项权利要求所述的路径优化目标定位方法。

6.一种终端设备，其特征在于，所述终端设备包括处理器和存储器，所述存储器中存储有至少一条指令、至少一段程序、代码集或指令集，所述至少一条指令、所述至少一段程序、所述代码集或指令集由所述处理器加载并执行以实现如权利要求1至2中任一项权利要求所述的路径优化目标定位方法。