CN110470304B - 一种基于无人机光电平台的高精度目标定位测速方法 - Google Patents

Abstract

本发明公开了一种基于无人机光电平台的高精度目标定位测速方法；本发明提出的高精度目标定位测速方法首先根据静止的合作目标对光电平台的方位角误差和高低角误差进行估计，待光电平台方位角误差和高低角误差估计值稳定后，取稳定后的方位角误差和高低角误差的估计值作为光电平台的方位角误差和高低角误差，然后在定位测速过程中实时对光电平台方位角误差和高低角误差进行补偿并输出高精度的目标位置与速度信息。

Description

一种基于无人机光电平台的高精度目标定位测速方法

【技术领域】

本发明属于无人机目标定位领域，具体涉及一种基于无人机光电平台的高精度目标定位测速方法。

【背景技术】

在现代战争中实时把握战场发展的态势和瞬时变化的情报是获取战场主动权的关键。因此，有效的获取战场信息的方式和手段就显得尤为重要。无人机具有体积小、成本低、机动灵活、作战时无人员伤亡风险的优点，目前广泛应用于侦察监视、火炮校射等军事领域。

无人机可以进入阵地前沿和深入敌后，按预定的程序或地面指令进行工作，通过装在机体上的光电平台将所获得的信息和图像实时传送回地面。无人机在执行校射任务时，一般是首先使用光电平台进行搜索并识别目标，将目标的图像和坐标显示在地面控制站的屏幕上，然后将目标数据输入炮兵射击指挥系统。在无人机诸多的军事应用中，无人机对目标定位功能是实现这些应用的前提条件。只有实现高精度的目标定位，才能实现侦察监视、火炮校射等军事任务。

目前国内外各种型号无人机进行目标定位主要有三种方法：基于共线构像的目标定位、基于图像匹配模式的目标定位和基于姿态测量/激光测距的目标定位。现有的三种无人机目标定位方法均有不足之处：

基于共线构像的目标定位：该方法需要获取目标点飞行器的航高，以及摄像机的内、外方位元素，根据共线条件方程计算地面目标位置。在实际使用中，该方法需要假定待测目标区域为平坦地面，复杂地形情况下，目标定位精度较低，并且该方法要求在实施目标定位时，飞机尽量从目标上方飞过，该方法使用局限性较多。

基于图像匹配模式的目标定位：该方法利用可获取的多源图像，在预先建立基准图像条件下，将经过校正的无人机图像与基准图片进行匹配，从而实现目标定位，该方法有较高的定位精度，但基准图片的获取有一定的限制性，且图片匹配实时性较差，实用性不高。

基于姿态测量/激光测距的目标定位：该方法对侦察机的飞行方式没有特殊要求。但是该方法误差来源较多，包括：光电平台方位角误差和高低角误差、激光测距误差、无人机姿态误差以及无人机位置误差等，以上所有误差均会影响该方法定位精度。

【发明内容】

本发明的目的在于克服上述现有技术的缺点，提供一种基于无人机光电平台的高精度目标定位测速方法，该方法不仅能够实时输出目标高精度位置信息还能输出目标的速度信息，解决现有技术中使用局限性较多、定位精度不高以及实时性较差的问题。

为达到上述目的，本发明采用以下技术方案予以实现：

一种基于无人机光电平台的高精度目标定位测速方法，包括以下步骤：

步骤1，采集合作目标的经度、纬度和高度，确定无人机的标定航线；无人机在标定航线中匀速运动，且运动时无人机的航高固定，俯仰角和滚转角均为零度；

步骤2，无人机在标定航线内匀速运动时，针对合作目标，通过六个参数计算得出目标位置量测T_e、光电平台方位角误差量测δα_t及高低角误差量测δβ_t；所述六个参数为：无人机的位置P_e、无人机的姿态

合作目标的位置

光电平台输出的激光测距值_r以及光电平台的方位角α_p和高低角β_p；

通过建立合作目标的状态方程和量测方程得到针对合作目标的k时刻量测向量预测

将T_e、

δα_t和δβ_t代入至卡尔曼滤波器或者UKF滤波器，计算得出光电平台方位角误差估计值δα和高低角误差估计值δβ；

步骤3，对任意目标进行定位和测速时，通过步骤2得出的光电平台方位角误差估计值δα和高低角误差估计值δβ，对无人机输出的跟踪任意目标的光电平台方位角α_p和高低角β_p进行实时补偿，结合无人机的位置P_e、无人机的姿态

和光电平台输出的激光测距值_r，计算出针对任意目标的目标位置量测T_e；通过建立任意目标的状态方程和量测方程得到针对任意目标的k时刻量测向量预测

将任意目标的T_e和

输入至卡尔曼滤波器或者UKF滤波器，计算得到k时刻任意目标的速度和位置。

本发明的进一步改进在于：

优选的，步骤1中，所述标定航线中，合作目标和无人机机身的轴线为一条直线，或合作目标在无人机机身轴线的左侧或右侧。

优选的，步骤2中的目标均指代合作目标，步骤2具体包括以下步骤：

步骤2.1，针对合作目标，建立状态方程，得到目标k时刻状态向量的预测

步骤2.2，通过目标在摄像机坐标系下的坐标值T_c和在大地直角坐标系下的目标位置量测T_e之间的转换关系，构造合作目标的目标位置量测T_e和无人机的位置P_e、无人机的姿态

光电平台输出的激光测距值_r、光电平台的方位角α_p以及高低角β_p的第一关系式，通过第一关系式计算得出合作目标的目标位置量测T_e；

通过目标在摄像机坐标系下的坐标值T_c和在导航坐标系的坐标值T_n之间的转换关系，构造光电平台方位角误差量测δα_t和高低角误差量测δβ_t分别与无人机的位置P_e、合作目标点的位置

无人机的姿态

光电平台的方位角α_p以及高低角β_p的第二关系式，通过第二关系式计算得出光电平台方位角误差量测δα_t和高低角误差量测δβ_t；

步骤2.3，针对合作目标，建立量测方程，对步骤2.1中的目标k时刻状态向量的预测

进行计算，得到目标k时刻量测向量预测

步骤2.4，将步骤2.2得到的目标位置量测T_e、光电平台方位角误差量测δα_t和高低角误差量测δβ_t，以及步骤2.3得到的目标k时刻量测向量预测

输入至卡尔曼滤波器或者UKF滤波器，计算得到k时刻光电平台方位角误差估计值δα和高低角误差估计值δβ。

优选的，步骤2.1中，针对合作目标，目标的状态方程为：

其中：X_k-1—k-1时刻状态向量；Φ_k/k-1—状态转移矩阵；

—k时刻状态向量的预测；t—相邻两帧数据时间间隔。

优选的，步骤2.2中的具体步骤为：

步骤2.2.1，所述第一关系式为：

式中，

其中，目标在摄像机坐标系下的坐标

目标在大地直角坐标系下的目标位置量测T_e＝[p_xe p_ye p_ze]^T；

无人机在大地直角坐标系下的坐标值

—大地直角坐标系e到地理坐标系n的转换矩阵；

—地理坐标系n到无人机机体坐标系b的转换矩阵；

—无人机机体坐标系b到摄像机坐标系c的转换矩阵；

其中：

L—无人机纬度；λ—无人机经度；

—无人机航向角；θ—无人机俯仰角；γ—无人机滚转角；α_p—光电平台输出的方位角，(α_p∈[0,2π))；β_p—光电平台输出的高低角，

步骤2.2.2，所述第二关系式如下所示：

其中，目标在摄像机坐标系的坐标值为T_c，目标在导航坐标系的坐标值为T_n；

假设T_c＝[1 0 0]，则T_n＝[cos(α_v)·cos(β_v) sin(α_v)·cos(β_v) sin(β_v)]，将式(11)展开可得：

右＝[cos(α_true)·cos(β_true) sin(α_true)·cos(β_true) sin(β_true)] (12)

由于左＝右，令左＝T_b，得到光电平台方位角真值α_true和高低角真值β_true的计算公式：

其中：

—无人机航向角；θ—无人机俯仰角；γ—无人机滚转角；α_true—光电平台真实方位角，(α_true∈[0,2π))；β_true—光电平台真实高低角，

α_v—视轴方位角(α_v∈[0,2π))；β_v—视轴高低角，

则光电平台方位角误差量测δα_t和高低角误差量测δβ_t的计算公式如下：

优选的，步骤2.3中，所述量测方程为：

其中：

—k时刻状态向量的预测；H_k—k时刻量测矩阵；

—k时刻量测向量预测。

优选的，步骤3中的目标均指代任意目标，步骤3具体包括以下步骤：

步骤3.1，针对任意目标，建立状态方程，得到目标k时刻状态向量的预测

步骤3.2，通过目标在摄像机坐标系下的坐标值T_c和在大地直角坐标系下的目标位置量测T_e之间的转换关系，构造任意目标的目标位置量测T_e和光电平台方位角α_p、高低角β_p、无人机的位置P_e、无人机的姿态

和光电平台输出的激光测距值_r的第三关系式，计算得出任意目标的目标位置量测T_e；

步骤3.3，建立量测方程，对步骤3.1中目标k时刻状态向量的预测

进行计算，得到目标k时刻量测向量预测

步骤3.4，将任意目标的T_e和

输入至卡尔曼滤波器或者UKF滤波器，计算得到k时刻任意目标的速度和位置。

优选的，步骤3.1中，目标的状态方程为：

其中：

X_k-1—k-1时刻状态向量；Φ_k/k-1—状态转移矩阵；

—k时刻状态向量预测；t—相邻两帧数据时间间隔。

优选的，步骤3.2中，针对任意目标，所述第三关系式为：

式中，

其中，目标在摄像机坐标系下的坐标值

目标在大地直角坐标系下的目标位置量测

无人机在大地直角坐标系下的坐标为

—大地直角坐标系e到地理坐标系n的转换矩阵；

—地理坐标系n到无人机机体坐标系b的转换矩阵；

—无人机机体坐标系b到摄像机坐标系c的转换矩阵；

其中：

L—无人机纬度；λ—无人机经度；

—无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α_{p_bc}——补偿后的光电平台方位角，(α_{p_bc}∈[0,2π))；β_{p_bc}——补偿后的光电平台高低角，

光电平台方位角和高低角实时补偿方法如下式所示：

其中：

δα—步骤2计算得出的光电平台方位角误差估计值；α_p—光电平台输出的方位角(α_p∈[0,2π))；δβ—步骤2计算得出的光电平台方位角误差估计值；β_p—补偿后的光电平台高低角

优选的，步骤3.3中，所述量测方程为：

其中：

—k时刻状态向量预测；H_k—k时刻量测矩阵；

—k时刻量测向量预测。

与现有技术相比，本发明具有以下有益效果：

本发明公开了一种基于无人机光电平台的高精度目标定位测速方法，该方法首先通过构建合作目标的目标位置量测、光电平台方位角误差量测和高低角误差量测，计算出光电平台方位角误差估计值δα和高低角误差估计值δβ；将两个角度的误差估计值带入至下一步中任意目标的速度和位置计算的过程中，对两个角度进行修正，使得输出的目标和速度的精度提高；本发明提出的高精度目标定位测速方法对地形无要求，对航迹无要求，使用没有局限性；可实时输出目标的高精度位置与速度信息；该方法不仅适用于静态目标，还适用于动态目标；可有效减小光电平台方位角误差和高低角误差带来的影响。

【附图说明】

图1是对光电平台的高低角误差和俯仰角误差进行标定时的参考标定航线图；

图2是视轴方位角和高低角示意图；

图3是卡尔曼滤波流程框图；

图4是对静态目标进行1000次定位测速仿真的定位结果分布图；

图5是对静态目标进行1000次定位测速仿真的总位置误差分布图；

图6是对静态目标进行1000次定位测速仿真的总速度误差分布图；

图7是对动态目标进行1000次定位测速仿真的定位结果分布图；

图8是对动态目标进行1000次定位测速仿真的总位置误差分布图；

图9是对动态目标进行1000次定位测速仿真的总速度误差分布图；

1-第一航路；2-第二航路；3-第三航路。

【具体实施方式】

下面结合附图和具体步骤对本发明做进一步详细描述，本发明公开了一种基于无人机光电平台的高精度目标定位测速方法，该方法具体包括以下步骤：

步骤1，采集合作目标点精确的经度、纬度和高度；规划标定航线，标定航线要求无人机保持匀速运动、航向和航高稳定、俯仰角和滚转角为零度。

标定航线的规划要考虑无人机与合作目标的相对位置，使得无人机能充分利用前后左右四个方向对合作目标进行定位，标定航线应采用如图1所示的航线或与之相似的航线。在第一航路1或第三航路3，目标在无人机的机头或机尾；在第二航路2，目标在无人机的左翼或右翼。无人机从合作目标垂直起飞后，到达指定高度，设定合作目标在指定高度的垂直投影点为W点，无人机开始沿远离W点的方向移动，W点始终在无人机机尾的后侧，目标和无人机的机身在同一直线上，无人机在第一航路1上飞行X1米后转向，转至第二航路2，设定第一航路1和第二航路2之间的夹角为o；无人机在第二航路2飞行X2米后转向，转至第三航路3，无人机在第三航路3上朝向W点运动，第三航路3和第二航路2之间的夹角同为o，无人机在第三航路3飞行至W；在整个航路飞行过程中，无人机的高度不变。

完成标定航线的规划与合作目标点精确的经度、纬度和高度采集后，无人机在对光电平台方位角误差和高低角误差进行估计时沿规划完成的标定航线飞行，在飞行过程中光电平台一直跟踪合作目标，该步骤为后续对光电平台方位角误差和高低角误差的估计奠定基础。

步骤2，在第一步规划的标定航线和采集的合作目标点精确的经度、纬度和高度基础上对光电平台的方位角误差和高低角误差进行实时估计，无人机沿第一步规划的标定航线飞行，在飞行过程中光电平台一直跟踪合作目标，并且实时输出光电平台的方位角、高低角以及激光测距信息；根据光电平台输出的方位角、高低角以及激光测距信息并结合无人机自身的位置、姿态以及合作目标点精确的经度、纬度和高度，通过建立的估计模型实时估计光电平台方位角误差和高低角误差，取稳定后的方位角误差和高低角误差的估计值作为光电平台的方位角误差和高低角误差。下述步骤2中所有的目标均指代合作目标；

步骤2.1构造系统的状态方程

选取目标在大地直角坐标系下的位置估计值[p_x p_y p_z]^T、速度估计值[v_x v_y v_z]^T以及光电平台方位角误差估计值δα和高低角误差估计值δβ为状态，建立目标的状态向量即：X＝[p_x p_y p_z v_x v_y v_z δα δβ]^T，其中，p_x为大地直角坐标系下目标x方向位置估计值，p_y为大地直角坐标系下目标y方向位置估计值，p_z为大地直角坐标系下目标z方向位置估计值，v_x为大地直角坐标系下目标x方向速度估计值，v_y为大地直角坐标系下目标y方向速度估计值，v_z为大地直角坐标系下目标z方向速度估计值。

选取目标在大地直角坐标系下的位置[p_xe p_ye p_ze]^T与光电平台方位角误差δα_t和高低角误差δβ_t为量测，即：Z＝[p_xe p_ye p_ze δα_t δβ_t]^T，其中p_xe为大地直角坐标系下目标x方向位置量测，p_ye为大地直角坐标系下目标y方向位置量测，p_ze为大地直角坐标系下目标z方向位置量测。

设定合作目标为静止状态，无人机运动状态为匀速直线运动，则合作目标的系统的状态方程为：

其中：

X_k-1——k-1时刻状态向量；

Φ_k/k-1——状态转移矩阵；

——k时刻状态向量的预测；

t——相邻两帧数据时间间隔；

——k时刻目标x方向位置估计值的预测值；

——k时刻目标y方向位置估计值的预测值；

——k时刻目标z方向位置估计值的预测值；

——k时刻目标x方向速度估计值的预测值；

——k时刻目标y方向速度估计值的预测值；

——k时刻目标z方向速度估计值的预测值；

——k时刻光电平台方位角误差估计值的预测值；

——k时刻光电平台高低角误差估计值的预测值；

——k-1时刻目标x方向位置估计值；

——k-1时刻目标y方向位置估计值；

——k-1时刻目标z方向位置估计值；

——k-1时刻目标x方向速度估计值；

——k-1时刻目标y方向速度估计值；

——k-1时刻目标z方向速度估计值；

δα_k-1——k-1时刻光电平台方位角的误差估计值；

δβ_k-1——k-1时刻光电平台高低角的误差估计值；

步骤2.2计算目标位置量测、光电平台方位角误差量测和高低角误差量测。

由于光电平台直接输出的信息为无人机与目标的距离r以及光电平台的方位角α_p和高低角β_p，都是量测构造过程中的输入；与无人机计算目标位置和速度的模型中直接使用的量测[p_xe p_ye p_ze δα_t δβ_t]^T不同，因此量测需要重新进行构造便于模型中的计算应用，量测构造过程分为目标位置量测[p_xe p_ye p_ze]^T构造与光电平台方位角误差量测δα_t、高低角误差量测δβ_t构造两部分；该步骤中，通过坐标转换得到无人机的位置

无人机的姿态

光电平台输出的激光测距值_r以及光电平台的方位角α_p和高低角β_p这五个参数和目标位置量测、光电平台方位角误差量测δα_t和高低角误差量测δβ_t之间的关系式；

步骤2.2.1构造目标位置量测

假设目标在摄像机坐标系下的坐标

其中_r为激光测距值，

为摄像机坐标系下目标x方向位置，

为摄像机坐标系下目标y方向位置，

为摄像机坐标系下目标z方向位置；目标大地直角坐标系下位置量测为T_e＝[p_xe p_ye p_ze]^T，该步骤中的目标均指代合作目标，无人机在大地直角坐标系下的位置为

则根据坐标转换关系可得：

其中：

——大地直角坐标系e到地理坐标系n的转换矩阵；

——地理坐标系n到无人机机体坐标系b的转换矩阵；

——无人机机体坐标系b到摄像机坐标系c的转换矩阵。将式(8)进行变换可得第一关系式：

式(10)中各个矩阵定义如下：

其中：

L——无人机纬度；

λ——无人机经度；

——无人机航向角；

θ——无人机俯仰角；

γ——无人机滚转角；

α_p——光电平台输出的方位角(α_p∈[0,2π))；

β_p——光电平台输出的高低角

以上是目标位置量测[p_xe p_ye p_ze]^T的构造过程，通过上述构造过程可将无人机的位置

无人机的姿态

光电平台输出的激光测距值_r以及光电平台的方位角α_p和高低角β_p转换为目标位置量测([p_xe p_ye p_ze]^T)，所述光电平台输出的激光测距值r为无人机与目标的距离。

步骤2.2.2构造光电平台方位角误差量测δα_t和高低角误差量测δβ_t。

假设合作目标在摄像机坐标系的坐标值为T_c，目标在导航坐标系的坐标值为T_n，根据坐标转换关系可知两个向量存在如下关系，即为第二关系式：

本发明采用的导航坐标系为北东地坐标系，载体坐标系为前右下坐标系，因此可假设T_c＝[1 0 0]，则T_n＝[cos(α_v)·cos(β_v) sin(α_v)·cos(β_v) sin(β_v)]，将式(11)展开可得：

右＝[cos(α_true)·cos(β_true) sin(α_true)·cos(β_true) sin(β_true)] (12)

由于左＝右，令左＝T_b，可得光电平台方位角真值α_true和高低角真值β_true的计算公式：

其中：

——无人机航向角；

θ——无人机俯仰角；

γ——无人机滚转角；

α_true——光电平台真实方位角(α_true∈[0,2π))；

β_true——光电平台真实高低角

α_v——视轴方位角(α_v∈[0,2π))；

β_v——视轴高低角

视轴方位角和高低角示意图如图2所示，计算方法如下：

其中：

——静止合作目标在大地直角坐标系x方向坐标值；

——静止合作目标在大地直角坐标系y方向坐标值；

——静止合作目标在大地直角坐标系z方向坐标值；

——无人机在大地直角坐标系x方向坐标值；

——无人机在大地直角坐标系y方向坐标值；

——无人机在大地直角坐标系z方向坐标值；

α_v——视轴方位角(α_v∈[0,2π))；

β_v——视轴高低角

计算出光电平台真实的方位角和高低角后与光电平台输出的方位角和高低角做差即可得到光电平台方位角误差量测δα_t和高低角误差量测δβ_t，公式如下：

以上是光电平台方位角误差量测δα_t和高低角误差量测δβ_t的构造过程，通过上述构造过程可将无人机的位置

合作目标点的位置

无人机的姿态

以及光电平台的方位角α_p和高低角β_p转换为光电平台方位角误差量测δα_t和高低角误差量测δβ_t。

步骤2.3建立量测方程

至此完成两部分量测的构造，为了后续在进行滤波时可利用构造的量测对状态进行修正，实时估计出光电平台方位角误差δα和高低角误差δβ，还需要进行量测方程的推导，为了利用步骤2.1中k时刻状态向量的预测

计算k时刻量测向量预测

建立系统的量测方程，，下面介绍量测方程：

其中：

——k时刻状态向量的预测；

H_k——k时刻量测矩阵

——k时刻量测向量预测；

——k时刻目标在大地直角坐标系下x方向位置量测预测值；

——k时刻目标在大地直角坐标系下y方向位置量测预测值；

——k时刻目标在大地直角坐标系下z方向位置量测预测值；

——k时刻光电平台方位角误差量测预测值；

——k时刻光电平台高低角误差量测预测值；

步骤2.4通过步骤2.2得到的目标位置量测T_e与光电平台方位角误差量测δα_t和高低角误差量测δβ_t，并结合步骤2.3得到的k时刻量测向量预测

通过卡尔曼滤波器或者UKF滤波器计算得出k时刻光电平台方位角误差估计值δα和高低角误差估计值δβ。

下面以卡尔曼滤波为例，对该模型的卡尔曼滤波具体实施过程进行介绍，如图3所示，卡尔曼滤波实施过程如下：

1)状态预测

2)状态误差协方差矩阵预测

3)滤波器增益

4)状态更新

5)状态误差协方差矩阵更新

其中：

P_k-1——k-1时刻卡尔曼滤波状态误差协方差矩阵；

——k时刻卡尔曼滤波状态误差协方差矩阵预测值；

Q_k-1——k-1时刻系统噪声矩阵；

R_k——k时刻量测噪声矩阵；

K_k——k时刻滤波器增益矩阵；

P_k——k时刻卡尔曼滤波状态误差协方差矩阵；

通过上述卡尔曼滤波器可估计出光电平台方位角误差δα和高低角误差δβ，在后续进行任意目标定位时可实时对光电平台方位角α_p和高低角β_p进行补偿，提高定位精度。下面对任意目标定位过程进行详细介绍。

步骤3，对任意目标进行高精度定位测速过程中实时补偿光电平台方位角和高低角，提高任意目标定位测速精度。

该步骤利用步骤2估计的光电平台方位角误差估计值δα和高低角误差估计值δβ实时对光电平台输出的方位角α_p和高低角β_p进行实时补偿，并对任意目标进行高精度定位测速；以下涉及到的目标均代指任意目标。

步骤3.1建立系统的状态方程

选取目标大地直角坐标系下的位置估计值[p_x p_y p_z]^T以及速度估计值[v_x v_y v_z]^T为状态，即：X＝[p_x p_y p_z v_x v_y v_z]^T，其中p_x为大地直角坐标系下目标x方向位置估计值，p_y为大地直角坐标系下目标y方向位置估计值，p_z为大地直角坐标系下目标z方向位置估计值，v_x为大地直角坐标系下目标x方向速度估计值，v_y为大地直角坐标系下目标y方向速度估计值，v_z为大地直角坐标系下目标z方向速度估计值。选取目标在大地直角坐标系下的位置[p_xe p_ye p_ze]^T为量测，即：Z＝[p_xe p_ye p_ze]^T，其中p_xe为大地直角坐标系下目标x方向目标位置量测，p_ye为大地直角坐标系下目标y方向目标位置量测，p_ze为大地直角坐标系下目标z方向目标位置量测；本段中目标均指代任意目标；

设定目标为任意可活动的目标，系统状态方程为：

其中：

X_k-1—k-1时刻状态向量；

Φ_k/k-1—状态转移矩阵；

—k时刻状态向量预测；

t—相邻两帧数据时间间隔；

——k时刻目标x方向位置估计值的预测值；

——k时刻目标y方向位置估计值的预测值；

——k时刻目标z方向位置估计值的预测值；

——k时刻目标x方向速度估计值的预测值；

——k时刻目标y方向速度估计值的预测值；

——k时刻目标z方向速度估计值的预测值；

——k-1时刻目标x方向位置估计值；

——k-1时刻目标y方向位置估计值；

——k-1时刻目标z方向位置估计值；

——k-1时刻目标x方向速度估计值；

——k-1时刻目标y方向速度估计值；

——k-1时刻目标z方向速度估计值；

步骤3.2计算目标位置量测、光电平台方位角误差量测和高低角误差量测。

由于光电平台直接输出的信息为无人机与任意目标的距离r以及光电平台的方位角α_p和高低角β_p，与模型中直接使用的量测[p_xe p_ye p_ze]^T不同，因此量测需要重新进行构造便于模型中的计算应用，量测构造过程为目标位置量测[p_xe p_ye p_ze]^T构造的过程；该步骤中，通过坐标转换得到无人机的位置

无人机的姿态

光电平台输出的激光测距值r以及光电平台的方位角α_p和高低角β_p这五个参数和目标位置量测[p_xe p_ye p_ze]^T之间的关系式；

步骤3.2.1构造目标位置量测

假设目标在摄像机坐标系下的坐标值

其中_r为激光测距值；设大地直角坐标系下任意目标位置量测为T_e＝[p_xe p_ye p_ze]^T，无人机在大地直角坐标系下的坐标为

则根据坐标转换关系可得：

其中：

——大地直角坐标系e到地理坐标系n的转换矩阵；

——地理坐标系n到无人机机体坐标系b的转换矩阵；

——无人机机体坐标系b到摄像机坐标系c的转换矩阵。将式(26)进行变换可得第三关系式为：

式(28)中各个矩阵定义如下：

其中：

L——无人机纬度；

λ——无人机经度；

——无人机航向角；

θ——无人机俯仰角；

γ——无人机滚转角；

α_{p_bc}——补偿后的光电平台方位角(α_{p_bc}∈[0,2π))；

β_{p_bc}——补偿后的光电平台高低角

光电平台方位角和高低角实时补偿方法如下：

其中：

δα——步骤2计算得出的光电平台方位角误差估计值；

α_p——光电平台输出的方位角(α_p∈[0,2π))；

α_{p_bc}——补偿后的光电平台方位角(α_{p_bc}∈[0,2π))；

δβ——步骤2计算得出的光电平台方位角误差估计值；

β_p——补偿后的光电平台高低角

β_{p_bc}——补偿后的光电平台高低角

以上是量测[p_xe p_ye p_ze]^T的构造过程，通过上述构造过程可将无人机的位置

无人机的姿态

光电平台输出的无人机与任意目标的距离r以及光电平台的方位角α_p和高低角β_p转换为任意目标的目标位置量测[p_xe p_ye p_ze]^T。

步骤3.3建立量测方程为了后续在进行滤波时可利用构造的量测对状态进行修正，对任意目标位置和速度进行实时高精度的估计，还需要进行量测方程的推导，为了利用步骤3.1中式(25)得到的k时刻状态向量预测

计算k时刻量测向量预测

建立系统的量测方程，，下面介绍量测方程：

其中：

——k时刻状态向量预测；

H_k——k时刻量测矩阵；

——k时刻量测向量预测；

——k时刻目标在大地直角坐标系下x方向位置量测预测值；

——k时刻目标在大地直角坐标系下y方向位置量测预测值；

——k时刻目标在大地直角坐标系下z方向位置量测预测值。

步骤3.4，通过步骤3.2得到的目标位置量测T_e，并结合步骤3.3得到的k时刻量测向量预测

通过卡尔曼滤波器或者UKF滤波器计算得到k时刻任意目标高精度位置和速度信息。卡尔曼滤波具体实施过程参考步骤二，此处不再重复赘余。应用本发明提供的基于无人机光电平台的高精度目标定位测速方法分别对动/静态目标进行定位测速仿真，仿真条件如下：

无人机在目标上方进行绕飞，飞行高度约为3000米，飞行速度约为100米/秒；静态目标静止不动，动态目标以20米/秒速度进行匀速直线运动。

无人机位置误差(δLδλδh)、姿态误差

光电平台方位角误差(δα)和高低角误差(δβ)以及激光测距误差(δr)均为服从零均值高斯分布的随机变量，δL～N(0,0.3)，δλ～N(0,0.3)，δh～N(0,0.5)，δr～N(0,5)单位：米；

δθ～N(0,0.3)，δγ～N(0,0.3)，δα～N(0,0.02)，δβ～N(0,0.02)单位：度。

对动/静态目标分别进行1000次仿真，图4为对静态目标进行1000次仿真静态目标坐标分布图，图7为对动态目标进行1000次仿真动态目标坐标分布图；从图5～图6中可看出在1000次静态目标定位测速仿真中位置误差为21.14米，速度误差为0.72米/秒，从图8～图9中可看出在1000次动态目标定位测速仿真中位置误差为27.38米，速度误差为0.78米。由以上仿真可知，本发明提出的基于无人机光电平台的高精度目标定位测速方法可在目标与无人机相距3000米的状态下实时输出20米精度的动/静态目标位置信息，0.7米/秒精度的动态目标速度信息。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，包括以下步骤：

步骤1，采集合作目标的经度、纬度和高度，确定无人机的标定航线；无人机在标定航线中匀速运动，且运动时无人机的航高固定，俯仰角和滚转角均为零度；

步骤2，无人机在标定航线内匀速运动时，针对合作目标，通过六个参数计算得出目标位置量测T_e、光电平台方位角误差量测δα_t及高低角误差量测δβ_t；所述六个参数为：无人机的位置P_e、无人机的姿态

合作目标的位置

光电平台输出的激光测距值r以及光电平台的方位角α_p和高低角β_p；

通过建立合作目标的状态方程和量测方程得到针对合作目标的k时刻量测向量预测

将T_e、

δα_t和δβ_t代入至卡尔曼滤波器或者UKF滤波器，计算得出光电平台方位角误差估计值δα和高低角误差估计值δβ；

步骤3，对任意目标进行定位和测速时，通过步骤2得出的光电平台方位角误差估计值δα和高低角误差估计值δβ，对无人机输出的跟踪任意目标的光电平台方位角α_p和高低角β_p进行实时补偿，结合无人机的位置P_e、无人机的姿态

和光电平台输出的激光测距值r，计算出针对任意目标的目标位置量测T_e；通过建立任意目标的状态方程和量测方程得到针对任意目标的k时刻量测向量预测

将任意目标的T_e和

输入至卡尔曼滤波器或者UKF滤波器，计算得到k时刻任意目标的速度和位置。

2.根据权利要求1所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤1中，所述标定航线中，合作目标和无人机机身的轴线为一条直线，或合作目标在无人机机身轴线的左侧或右侧。

3.根据权利要求1所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤2中的目标均指代合作目标，步骤2具体包括以下步骤：

步骤2.1，针对合作目标，建立状态方程，得到目标k时刻状态向量的预测

步骤2.2，通过目标在摄像机坐标系下的坐标值T_c和在大地直角坐标系下的目标位置量测T_e之间的转换关系，构造合作目标的目标位置量测T_e和无人机的位置P_e、无人机的姿态

光电平台输出的激光测距值r、光电平台的方位角α_p以及高低角β_p的第一关系式，通过第一关系式计算得出合作目标的目标位置量测T_e；

通过目标在摄像机坐标系下的坐标值T_c和在导航坐标系的坐标值T_n之间的转换关系，构造光电平台方位角误差量测δα_t和高低角误差量测δβ_t分别与无人机的位置P_e、合作目标点的位置

无人机的姿态

光电平台的方位角α_p以及高低角β_p的第二关系式，通过第二关系式计算得出光电平台方位角误差量测δα_t和高低角误差量测δβ_t；

步骤2.3，针对合作目标，建立量测方程，对步骤2.1中的目标k时刻状态向量的预测

进行计算，得到目标k时刻量测向量预测

步骤2.4，将步骤2.2得到的目标位置量测T_e、光电平台方位角误差量测δα_t和高低角误差量测δβ_t，以及步骤2.3得到的目标k时刻量测向量预测

输入至卡尔曼滤波器或者UKF滤波器，计算得到k时刻光电平台方位角误差估计值δα和高低角误差估计值δβ。

4.根据权利要求3所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤2.1中，针对合作目标，目标的状态方程为：

其中：X_k-1—k-1时刻状态向量；Φ_k/k-1—状态转移矩阵；

时刻状态向量的预测；t—相邻两帧数据时间间隔。

5.根据权利要求3所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤2.2中的具体步骤为：

步骤2.2.1，所述第一关系式为：

式中，

其中，目标在摄像机坐标系下的坐标

目标在大地直角坐标系下的目标位置量测T_e＝[p_xe p_ye p_ze]^T；

无人机在大地直角坐标系下的坐标值

—大地直角坐标系e到地理坐标系n的转换矩阵；

—地理坐标系n到无人机机体坐标系b的转换矩阵；

—无人机机体坐标系b到摄像机坐标系c的转换矩阵；

其中：

L—无人机纬度；λ—无人机经度；

—无人机航向角；θ—无人机俯仰角；γ—无人机滚转角；α_p—光电平台输出的方位角，(α_p∈[0,2π))；β_p—光电平台输出的高低角，

步骤2.2.2，所述第二关系式如下所示：

其中，目标在摄像机坐标系的坐标值为T_c，目标在导航坐标系的坐标值为T_n；

假设T_c＝[1 0 0]，则T_n＝[cos(α_v)·cos(β_v) sin(α_v)·cos(β_v) sin(β_v)]，将式(11)展开可得：

由于左＝右，令左＝T_b，得到光电平台方位角真值α_true和高低角真值β_true的计算公式：

其中：

—无人机航向角；θ—无人机俯仰角；γ—无人机滚转角；α_true—光电平台真实方位角，(α_true∈[0,2π))；β_true—光电平台真实高低角，

α_v—视轴方位角(α_v∈[0,2π))；β_v—视轴高低角，

则光电平台方位角误差量测δα_t和高低角误差量测δβ_t的计算公式如下：

6.根据权利要求3所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤2.3中，所述量测方程为：

其中：

—k时刻状态向量的预测；H_k—k时刻量测矩阵；

—k时刻量测向量预测。

7.根据权利要求1所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤3中的目标均指代任意目标，步骤3具体包括以下步骤：

步骤3.1，针对任意目标，建立状态方程，得到目标k时刻状态向量的预测

步骤3.2，通过目标在摄像机坐标系下的坐标值T_c和在大地直角坐标系下的目标位置量测T_e之间的转换关系，构造任意目标的目标位置量测T_e和光电平台方位角α_p、高低角β_p、无人机的位置P_e、无人机的姿态

和光电平台输出的激光测距值r的第三关系式，计算得出任意目标的目标位置量测T_e；

步骤3.3，建立量测方程，对步骤3.1中目标k时刻状态向量的预测

进行计算，得到目标k时刻量测向量预测

步骤3.4，将任意目标的T_e和

输入至卡尔曼滤波器或者UKF滤波器，计算得到k时刻任意目标的速度和位置。

8.根据权利要求7所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤3.1中，目标的状态方程为：

其中：

X_k-1—k-1时刻状态向量；Φ_k/k-1—状态转移矩阵；

—k时刻状态向量预测；t—相邻两帧数据时间间隔。

9.根据权利要求7所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤3.2中，针对任意目标，所述第三关系式为：

式中，

其中，目标在摄像机坐标系下的坐标值

目标在大地直角坐标系下的目标位置量测

无人机在大地直角坐标系下的坐标为

—大地直角坐标系e到地理坐标系n的转换矩阵；

—地理坐标系n到无人机机体坐标系b的转换矩阵；

—无人机机体坐标系b到摄像机坐标系c的转换矩阵；

其中：

L—无人机纬度；λ—无人机经度；

—无人机航向角；θ——无人机俯仰角；γ——无人机滚转角；α_{p_bc}——补偿后的光电平台方位角，(α_{p_bc}∈[0,2π))；β_{p_bc}——补偿后的光电平台高低角，

光电平台方位角和高低角实时补偿方法如下式所示：

其中：

δα—步骤2计算得出的光电平台方位角误差估计值；α_p—光电平台输出的方位角(α_p∈[0,2π))；δβ—步骤2计算得出的光电平台方位角误差估计值；β_p—补偿后的光电平台高低角

10.根据权利要求7所述的一种基于无人机光电平台的高精度目标定位测速方法，其特征在于，步骤3.3中，所述量测方程为：

其中：

时刻状态向量预测；H_k—k时刻量测矩阵；

时刻量测向量预测。

Images