[发明专利]基于方位角定位的三维轨迹规划方法、系统、设备及介质

说明书

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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