CN114234982A

CN114234982A - 基于方位角定位的三维轨迹规划方法、系统、设备及介质

Info

Publication number: CN114234982A
Application number: CN202111562286.9A
Authority: CN
Inventors: 邹逸群; 高碧璐; 唐夏菲
Original assignee: Central South University
Current assignee: Central South University
Priority date: 2021-12-20
Filing date: 2021-12-20
Publication date: 2022-03-25
Anticipated expiration: 2041-12-20
Also published as: CN114234982B

Abstract

本公开实施例中提供了一种基于方位角定位的三维轨迹规划方法、系统、设备及介质，属于控制技术领域，具体包括：获取位置信息以及角度信息；利用极大似然函数计算定位点的估计位置；根据估计位置，最大化FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在目标函数中添加多个约束条件后求解目标传感器在下一时刻的最佳位置；在获取最佳位置后，更新位置信息和角度信息并计算下一相邻时刻的最佳位置直到角度传感器满足最大采样次数，输出轨迹数据。通过本公开的方案，利用过去所有时刻的传感器位置信息，有效地估计定位点位置，并在三维空间中添加必要约束的前提下规划传感器的运动轨迹，提高了轨迹规划方法的效率、精准度和适应性。

Description

基于方位角定位的三维轨迹规划方法、系统、设备及介质

技术领域

本公开实施例涉及控制技术领域，尤其涉及一种基于方位角定位的三维轨迹规划方法、系统、设备及介质。

背景技术

目前，随着科技的飞速发展，飞行器飞行任务的难度系数不断增加，以及现代局部战争频繁爆发，推动了无人驾驶飞行器(无人机)在军事领域的快速发展。目标定位是通过利用测量的数据来获得运动或者静止目标的位置、速度以及其他运动参数的实时估计过程。无人机可提高对风险目标的突防能力，对目标进行定位，进而控制作战空间。为完成搜索、救援以及监视等任务，无人机需要在复杂的环境中实现自主飞行，通常需要必备优秀的自主导航能力，那么无人机的运动轨迹规划问题是无人机自主导航过程中要解决的关键问题。在军事和民事应用中，无人机可以自主计算起点到终点的最佳运动轨迹，从而准确搜索出目标位置。使用简单的二维轨迹规划算法无法获取高度方向上的信息，因此迫切需要三维航迹规划算法以适应复杂的三维场景。但是现有的大多数研究是关于二维空间中传感器位置的最优部署问题。因此当传感器为无人驾驶飞行器时，仅仅采用二维空间中的传感器轨迹规划策略无法准确的对目标进行定位。此外在规划传感器的位置时未充分利用已有传感器的位置信息。

可见，亟需一种高效精准适应性强的基于方位角定位的三维轨迹规划方法。

发明内容

有鉴于此，本公开实施例提供一种基于方位角定位的三维轨迹规划方法、系统、设备及介质，至少部分解决现有技术中存在定位精准度不够，轨迹规划效率低不能适应三维场景的问题。

第一方面，本公开实施例提供了一种基于方位角定位的三维轨迹规划方法，包括：

获取角度传感器在之前时刻内的位置信息，以及所述角度传感器与定位点之间的角度信息；

根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置；

根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置，其中，所述多个约束条件包括单位时间内所述角度传感器的运动最大距离约束和所述角度传感器接近所述定位点的最小距离约束；

在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据。

根据本公开实施例的一种具体实现方式，所述角度信息包括所述角度传感器与所述定位点之间的方位角和和俯仰角。

根据本公开实施例的一种具体实现方式，所述根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置的步骤，包括：

根据所述方位角和所述俯仰角计算所述定位点的二维平面坐标值；

采用最小二乘法确定所述定位点的Z轴坐标值；

根据所述二维平面坐标值和所述Z轴坐标值形成三维坐标值；

将所述三维坐标值作为所述极大似然函数的初始值输入，得到所述估计位置。

根据本公开实施例的一种具体实现方式，所述根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置的步骤，包括：

根据所述估计位置得到所述方位角对应的FIM矩阵和所述俯仰角对应的FIM矩阵；

根据所述方位角对应的FIM矩阵和所述俯仰角对应的FIM矩阵形成所述FIM行列式；

最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数并添加所述约束条件，其中，所述约束条件包括单位时间内所述角度传感器的运动最大距离约束和所述角度传感器接近所述定位点的最小距离约束。

根据本公开实施例的一种具体实现方式，所述在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据的步骤，包括：

根据获取下一时刻所述角度传感器的最佳位置，重新利用极大似然函数计算所述定位点的估计位置；

更新所述位置信息和角度信息，计算下一相邻时刻所述角度传感器的最佳位置，直到所述角度传感器满足最大采样次数，输出所述轨迹数据。

第二方面，本公开实施例提供了一种基于方位角定位的三维轨迹规划系统，包括：

获取模块，用于获取角度传感器在之前时刻内的位置信息，以及所述角度传感器与定位点之间的角度信息；

计算模块，用于根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置；

求解模块，用于根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置，其中，所述多个约束条件包括单位时间内所述角度传感器的运动最大距离约束和所述角度传感器接近所述定位点的最小距离约束；

更新模块，用于在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据。

第三方面，本公开实施例还提供了一种电子设备，该电子设备包括：

至少一个处理器；以及，

与该至少一个处理器通信连接的存储器；其中，

该存储器存储有可被该至少一个处理器执行的指令，该指令被该至少一个处理器执行，以使该至少一个处理器能够执行前述第一方面或第一方面的任一实现方式中的基于方角位定位的三维轨迹规划方法。

第四方面，本公开实施例还提供了一种非暂态计算机可读存储介质，该非暂态计算机可读存储介质存储计算机指令，该计算机指令用于使该计算机执行前述第一方面或第一方面的任一实现方式中的基于方位角定位的三维轨迹规划方法。

第五方面，本公开实施例还提供了一种计算机程序产品，该计算机程序产品包括存储在非暂态计算机可读存储介质上的计算程序，该计算机程序包括程序指令，当该程序指令被计算机执行时，使该计算机执行前述第一方面或第一方面的任一实现方式中的基于方位角定位的三维轨迹规划方法。

本公开实施例中的基于方位角定位的三维轨迹规划方案，包括：获取角度传感器在之前时刻内的位置信息，以及所述角度传感器与定位点之间的角度信息；根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置；根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置，其中，所述多个约束条件包括单位时间内所述角度传感器的运动最大距离约束和所述角度传感器接近所述定位点的最小距离约束；在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据。

本公开实施例的有益效果为：通过本公开的方案，利用过去所有时刻的传感器位置信息，从而有效地估计定位点位置，并在三维空间中添加必要约束的前提下，合理规划传感器的运动轨迹，提高了轨迹规划方法的效率、精准度和适应性。

附图说明

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

图1为本公开实施例提供的一种基于方位角定位的三维轨迹规划方法的流程示意图；

图2为本公开实施例提供的一种三维平面中定位点与传感器角度关系示意图；

图3为本公开实施例提供的一种二维平面中定位点与传感器角度关系示意图；

图4为本公开实施例提供的一种三维空间中传感器运动区和定位点安全区示意图；

图5为本公开实施例提供的一种基于方位角定位的三维轨迹规划方法涉及的轨迹规划过程示意图；

图6为本公开实施例提供的一种基于方位角定位的三维轨迹规划系统的结构示意图；

图7为本公开实施例提供的电子设备示意图。

具体实施方式

下面结合附图对本公开实施例进行详细描述。

以下通过特定的具体实例说明本公开的实施方式，本领域技术人员可由本说明书所揭露的内容轻易地了解本公开的其他优点与功效。显然，所描述的实施例仅仅是本公开一部分实施例，而不是全部的实施例。本公开还可以通过另外不同的具体实施方式加以实施或应用，本说明书中的各项细节也可以基于不同观点与应用，在没有背离本公开的精神下进行各种修饰或改变。需说明的是，在不冲突的情况下，以下实施例及实施例中的特征可以相互组合。基于本公开中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本公开保护的范围。

需要说明的是，下文描述在所附权利要求书的范围内的实施例的各种方面。应显而易见，本文中所描述的方面可体现于广泛多种形式中，且本文中所描述的任何特定结构及/或功能仅为说明性的。基于本公开，所属领域的技术人员应了解，本文中所描述的一个方面可与任何其它方面独立地实施，且可以各种方式组合这些方面中的两者或两者以上。举例来说，可使用本文中所阐述的任何数目个方面来实施设备及/或实践方法。另外，可使用除了本文中所阐述的方面中的一或多者之外的其它结构及/或功能性实施此设备及/或实践此方法。

还需要说明的是，以下实施例中所提供的图示仅以示意方式说明本公开的基本构想，图式中仅显示与本公开中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制，其实际实施时各组件的型态、数量及比例可为一种随意的改变，且其组件布局型态也可能更为复杂。

另外，在以下描述中，提供具体细节是为了便于透彻理解实例。然而，所属领域的技术人员将理解，可在没有这些特定细节的情况下实践所述方面。

随着科技的飞速发展，飞行器飞行任务的难度系数不断增加，以及现代局部战争频繁爆发，推动了无人驾驶飞行器(无人机)在军事领域的快速发展。目标定位是通过利用测量的数据来获得运动或者静止目标的位置、速度以及其他运动参数的实时估计过程。无人机可提高对风险目标的突防能力，对目标进行定位，进而控制作战空间。为完成搜索、救援以及监视等任务，无人机需要在复杂的环境中实现自主飞行，通常需要必备优秀的自主导航能力，那么无人机的运动轨迹规划问题是无人机自主导航过程中要解决的关键问题。在军事和民事应用中，无人机可以自主计算起点到终点的最佳运动轨迹，从而准确搜索出目标位置。使用简单的二维轨迹规划算法无法获取高度方向上的信息，因此迫切需要三维航迹规划算法以适应复杂的三维场景。

传感器所处位置对定位精度存在较大影响，因此对传感器运动轨迹优化策略的研究具有重要意义。虽然目标跟踪受到了相当多的关注，但在大多数情况下，所涉及的传感器是静态的，重点是对可用信息的最佳处理，与使用静态传感器相比，部署移动传感器进行跟踪具有显著优势。相对二维空间中的传感器轨迹规划问题，较少有研究处理三维空间中的轨迹规划问题。本公开实施例提供一种基于方位角定位的三维轨迹规划方法，所述方法可以应用于位置追踪与规划场景的无人机运动轨迹规划过程中。

参见图1，为本公开实施例提供的一种基于方位角定位的三维轨迹规划方法的流程示意图。如图1所示，所述方法主要包括以下步骤：

S101，获取角度传感器在之前时刻内的位置信息，以及所述角度传感器与定位点之间的角度信息；

可选的，所述角度信息包括所述角度传感器与所述定位点之间的方位角和和俯仰角。

例如，如图2所示，在三维空间中，在已知传感器前几个时刻位置前提下，对定位点进行定位，所述定位点的位置未知T＝(x_t,y_t,z_t)'，所述角度传感器

位置已知。

如图3所示，已知二维平面上所述角度传感器的位置信息s_i，其中，

表示第i个时刻所述角度传感器在x，y轴方向的坐标值，其中，t＝(x_t y_t)′表示所述定位点在x，y轴方向的坐标值，所述角度传感器与所述定位点之间方位角θ_i(i＝1,2,…,n)的测量值(即

与(x_t,y_t)'之间的夹角)，测量的角度含有误差项e_i(i＝1,2,…,n)，即(测量方位角等于真实方位角加误差项)：

其中，e_i是均值为零、方差为σ_i ²的独立同分布高斯白噪声。传感器S_i与T之间俯仰角φ_i(i＝1,2,…,n)的测量值已知，测量的角度含有误差项δ_i(i＝1,2,…,n)，即：

其中，δ_i是均值为零、方差为γ_i ²的独立同分布高斯白噪声。

已知过去的多个时刻内所述角度传感器的位置信息，传感器与定位点之间的方位角和俯仰角分别为θ_i(i＝1,2,…,n)、φ_i(i＝1,2,…,n)。

S102，根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置；

具体实施时，考虑到需要根据所述定位点的具体坐标来确定在三维空间内规划所述传感器的运动轨迹，则可以根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置。

S103，根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置，其中，所述多个约束条件包括单位时间内所述角度传感器的运动最大距离约束和所述角度传感器接近所述定位点的最小距离约束；

具体实施时，在基于所述极大似然函数求解出的定位点位置，来确定所述角度传感器在下一时刻的位置。

例如，根据给定带噪声的方位角及俯仰角、过去k个时刻传感器的位置，来确定传感器在k+1时刻的最佳位置，充分利用过去所有时刻所述角度传感器的位置信息，以最小化所述定位点估计的不确定性，最大化以角度传感器与定位点之间的FIM行列式作为求解下一时刻角度传感器位置的目标函数。

考虑到实际应用时，所述定位点可能存在人或者物体，在规划角度传感器移动轨迹时需要避免配置所述角度传感器的飞行器或者无人机等与所述定位点内的人体或者物体发生碰撞，则可以添加多个所述约束条件，例如，考虑传感器运动区域(传感器最大运动距离)和定位点安全区域(禁止传感器侵入该区域)约束，然后在所述约束条件下通过所述目标函数计算所述角度传感器在下一时刻的最佳位置。

S104，在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据。

具体实施时，每获取一个时刻下所述角度传感器的位置值时，即增加一个坐标信息以更新所述位置信息和所述角度信息，并重新定位所述定位点的位置，根据更多时刻的位置信息和角度信息来预测所述角度传感器的运动轨迹，直到所述角度传感器满足最大采样次数，然后根据计算的每个时刻的位置信息输出形成所述轨迹数据，具体更新过程如图4所示。

根据前k个时刻传感器的位置来设计第k+1时刻传感器的放置策略，充分利用了过去所有时刻的传感器位置信息，相较于仅使用过去两个时刻位置信息来进行轨迹规划，能够获取更多的费雪信息，能有效的提升定位精度。

本实施例提供的基于方位角定位的三维轨迹规划方法，通过利用过去所有时刻的传感器位置信息，从而有效地估计定位点位置，并在三维空间中添加必要约束的前提下，合理规划传感器的运动轨迹，提高了轨迹规划方法的效率、精准度和适应性。

在上述实施例的基础上，步骤S102所述的，根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置，包括：

采用最小二乘法确定所述定位点的Z轴坐标值；

根据所述二维平面坐标值和所述Z轴坐标值形成三维坐标值；

例如，在二维定位问题上，所述定位点与所述角度传感器在n个不同的位置信息的伪线性方程表示为如下：

最小二乘法采用的状态矩阵A和B如下所示：

求得所述定位点在二维平面上的坐标值，为：

t_ls＝(A′A)^-1A′B

在三维空间中有以下关系：

其中，

三维空间中，最小二乘法采用的状态矩阵Y和F如下所示：

可以采用最小二乘法来定位所述定位点在z轴方向的坐标，可得三维方向上的坐标为：z_ls＝(Y′F)^-1Y′F

然后可以采用最小二乘法求解出的三维坐标值

作为所述极大似然函数求解所述定位点位置的初始值，考虑方位角和俯仰角测量的最大似然代价函数，由下式给出：

其中，θ_i(x₀,y₀)、φ_i(x₀,y₀,z₀)分别为第i个时刻时所述角度传感器与所述定位点之间方位角和俯仰角的测量值。

分别为：

然后可以使用最小二乘法求解出的三维坐标值

作为求解所述极大似然函数的初始值。x_t、y_t、z_t为采用极大似然估计算法求解的定位点位置坐标。

所述最大似然代价函数可写成两个代价函数之和，即：

其中，

中的

是通过最小化J_n(x₀,y₀)得出的，假设俯仰角对估计(x₀,y₀)的影响非常小，这种方法能够实现(x₀,y₀)与z₀坐标之间的解耦，以使得计算效率高。

进一步的，步骤S103所述的，根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置，包括：

具体实施时，基于所述极大似然函数求解出的定位点估计位置，来确定所述角度传感器在下一时刻的位置。可以根据给定带噪声的方位角及俯仰角、过去k个时刻传感器的位置，来确定传感器在k+1时刻的最佳位置，充分利用过去所有时刻所述目标传感器的位置信息，以最小化所述定位点估计的不确定性，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数。

矩阵M₁代表方位角对应的FIM矩阵，即对应于三维定位问题投影到xy平面，矩阵M₂是俯仰角对应的FIM矩阵。因此在三维空间中的方位角目标定位问题中，所述FIM行列式可表示为：

FIM＝M₁+M₂

其中，θ′_i和φ′_i分别为定位点位置值与第i个时刻角度传感器之间的方位角和俯仰角。在求解第k+1时刻所述角度传感器位置时，FIM中的θ′_i和φ′_i计算方法为如下：

其中，

分别为已知k个传感器位置信息时采用极大似然估计求解出的定位点在x，y，z轴方向上的坐标。d_i和D_i分别为定位点与第i个时刻角度传感器在二维、三维空间中的距离，即：

令：

其中，

分别为第i和第k+1时刻角度传感器的FIM。根据闵可夫斯基不等式，对于两个半正定矩阵D和H，存在如下关系：

det(D+H)≥det(D)+det(H)

因此：

上式右端是所述FIM行列式的下界，其中右端的第一项是常数，因此可以通过求解右端第二项来求解所述FIM行列式。右端第二项与传感器下一时刻位置有关，可通过其求解出传感器运动轨迹的最优解，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻角度传感器位置的目标函数，所述目标函数即可转化为如下形式：

然后根据当前时刻所述定位点的估计位置，结合所述运动最大距离约束和所述最小距离约束，得到所述角度传感器在下一时刻的最佳位置。

具体实施时，如图5所示，在三维空间中，设置所述角度传感器运动区域(S_k+1仅在以S_k为圆心，r为半径的球内及其边缘运动，该区域是指传感器在每个角度上可以从S_k位置移动距离的物理限制。)和所述定位点的安全区域(S_k+1的位置还应当满足在以

为球心，R为半径的球外，所述角度传感器禁止侵入该区域，以避免所述角度传感器与所述定位点发生碰撞)，用公式可表示为：

||S_k+1-S_k||²≤r

其中，S_k、S_k+1分别为第k时刻、第k+1时刻传感器的位置，

为已知k个传感器位置信息时采用极大似然估计求解出的定位点位置，r为传感器在单位时间内运动的最大距离，R为传感器与定位点之间需要保持的最小安全距离。在所述目标函数基础上添加上述两个约束条件，可求解出所述角度传感器在k+1时刻的位置。

在上述实施例的基础上，步骤S104所述的，在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据，包括：

在得到下一时刻所述角度传感器的最佳位置后，根据获取下一时刻所述角度传感器的最佳位置，重新利用极大似然函数计算所述定位点的估计位置，然后更新所述角度传感器的位置信息和角度信息，计算下一相邻时刻所述角度传感器的最佳位置，直到所述角度传感器满足最大采样次数，输出所述轨迹数据。

与上面的方法实施例相对应，参见图6，本公开实施例还提供了一种基于方位角定位的三维轨迹规划系统60，包括：

获取模块601，用于获取角度传感器在之前时刻内的位置信息，以及所述角度传感器与定位点之间的角度信息；

计算模块602，用于根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置；

求解模块603，用于根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置，其中，所述多个约束条件包括单位时间内所述角度传感器的运动最大距离约束和所述角度传感器接近所述定位点的最小距离约束；

更新模块604，用于在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据。

图6所示系统可以对应的执行上述方法实施例中的内容，本实施例未详细描述的部分，参照上述方法实施例中记载的内容，在此不再赘述。

参见图7，本公开实施例还提供了一种电子设备70，该电子设备包括：至少一个处理器以及与该至少一个处理器通信连接的存储器。其中，该存储器存储有可被该至少一个处理器执行的指令，该指令被该至少一个处理器执行，以使该至少一个处理器能够执行前述方法实施例中的基于方位角定位的三维轨迹规划方法。

本公开实施例还提供了一种非暂态计算机可读存储介质，该非暂态计算机可读存储介质存储计算机指令，该计算机指令用于使该计算机执行前述方法实施例中的基于方位角定位的三维轨迹规划方法。

本公开实施例还提供了一种计算机程序产品，该计算机程序产品包括存储在非暂态计算机可读存储介质上的计算程序，该计算机程序包括程序指令，当该程序指令被计算机执行时，使该计算机执行前述方法实施例中的基于方位角定位的三维轨迹规划方法。

下面参考图7，其示出了适于用来实现本公开实施例的电子设备70的结构示意图。本公开实施例中的电子设备可以包括但不限于诸如移动电话、笔记本电脑、数字广播接收器、PDA(个人数字助理)、PAD(平板电脑)、PMP(便携式多媒体播放器)、车载终端(例如车载导航终端)等等的移动终端以及诸如数字TV、台式计算机等等的固定终端。图7示出的电子设备仅仅是一个示例，不应对本公开实施例的功能和使用范围带来任何限制。

如图7所示，电子设备70可以包括处理装置(例如中央处理器、图形处理器等)701，其可以根据存储在只读存储器(ROM)702中的程序或者从存储装置708加载到随机访问存储器(RAM)703中的程序而执行各种适当的动作和处理。在RAM 703中，还存储有电子设备70操作所需的各种程序和数据。处理装置701、ROM 702以及RAM 703通过总线704彼此相连。输入/输出(I/O)接口705也连接至总线704。

通常，以下装置可以连接至I/O接口705：包括例如触摸屏、触摸板、键盘、鼠标、图像传感器、麦克风、加速度计、陀螺仪等的输入装置706；包括例如液晶显示器(LCD)、扬声器、振动器等的输出装置707；包括例如磁带、硬盘等的存储装置708；以及通信装置709。通信装置709可以允许电子设备70与其他设备进行无线或有线通信以交换数据。虽然图中示出了具有各种装置的电子设备70，但是应理解的是，并不要求实施或具备所有示出的装置。可以替代地实施或具备更多或更少的装置。

特别地，根据本公开的实施例，上文参考流程图描述的过程可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在这样的实施例中，该计算机程序可以通过通信装置709从网络上被下载和安装，或者从存储装置708被安装，或者从ROM 702被安装。在该计算机程序被处理装置701执行时，执行本公开实施例的方法中限定的上述功能。

需要说明的是，本公开上述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(EPROM或闪存)、光纤、便携式紧凑磁盘只读存储器(CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本公开中，计算机可读存储介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本公开中，计算机可读信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读信号介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：电线、光缆、RF(射频)等等，或者上述的任意合适的组合。

上述计算机可读介质可以是上述电子设备中所包含的；也可以是单独存在，而未装配入该电子设备中。

上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备可以执行上述方法实施例的相关步骤。

或者，上述计算机可读介质承载有一个或者多个程序，当上述一个或者多个程序被该电子设备执行时，使得该电子设备可以执行上述方法实施例的相关步骤。

可以以一种或多种程序设计语言或其组合来编写用于执行本公开的操作的计算机程序代码，上述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网(LAN)或广域网(WAN)—连接到用户计算机，或者，可以连接到外部计算机(例如利用因特网服务提供商来通过因特网连接)。

附图中的流程图和框图，图示了按照本公开各种实施例的系统、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的基于硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。

描述于本公开实施例中所涉及到的单元可以通过软件的方式实现，也可以通过硬件的方式来实现。

应当理解，本公开的各部分可以用硬件、软件、固件或它们的组合来实现。

以上所述，仅为本公开的具体实施方式，但本公开的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本公开揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本公开的保护范围之内。因此，本公开的保护范围应以权利要求的保护范围为准。

Claims

1.一种基于方位角定位的三维轨迹规划方法，其特征在于，包括：

2.根据权利要求1所述的方法，其特征在于,所述角度信息包括所述角度传感器与所述定位点之间的方位角和和俯仰角。

3.根据权利要求2所述的方法，其特征在于,所述根据所述位置信息和所述角度信息，利用极大似然函数计算所述定位点的估计位置的步骤，包括：

采用最小二乘法确定所述定位点的Z轴坐标值；

根据所述二维平面坐标值和所述Z轴坐标值形成三维坐标值；

4.根据权利要求3所述的方法，其特征在于,所述根据所述估计位置，最大化以所述角度传感器与所述定位点之间的FIM行列式作为求解下一时刻所述角度传感器位置的目标函数，并在所述目标函数中添加多个约束条件后求解所述角度传感器在下一时刻的最佳位置的步骤，包括：

5.根据权利要求4所述的方法，其特征在于,所述在获取下一时刻所述角度传感器的最佳位置后，更新所述位置信息和所述角度信息并计算下一相邻时刻的最佳位置直到所述角度传感器满足最大采样次数，输出轨迹数据的步骤，包括：

6.一种基于方位角定位的三维轨迹规划系统，其特征在于，包括：

7.一种电子设备，其特征在于，所述电子设备包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行前述权利要求1-5中任一项所述的基于方位角定位的三维轨迹规划方法。

8.一种非暂态计算机可读存储介质，该非暂态计算机可读存储介质存储计算机指令，该计算机指令用于使该计算机执行前述权利要求1-5中任一项所述的基于方位角定位的三维轨迹规划方法。