CN109933087A - 无人机与地面机动目标虚拟编队战位保持控制方法 - Google Patents

Abstract

本发明提供了一种无人机与地面机动目标“虚拟编队”战位保持控制方法，建立地面机动目标运动模型，对目标运动信息进行估计，建立“虚拟编队”模型及模糊状态机，根据目标运动信息维护虚拟编队点运动信息，利用虚拟编队点运动信息和无人机运动信息计算滚转指令，由滚转角指令计算无人机副翼控制指令及方向舵控制指令，并送入执行机构进行无人机姿态控制。本发明使得无人机对地面机动目标的跟踪运用方式变得灵活多样，满足多种跟踪需求；降低了系统不确定性与测量不确定性对无人机的影响，从而抑制了无人机姿态抖动；提高了无人机对地面机动目标的跟踪能力，同时也保障了飞行安全。

Description

无人机与地面机动目标虚拟编队战位保持控制方法

技术领域

本发明涉及无人机领域，尤其是一种无人机与地面机动目标的控制方法。

背景技术

侦察型无人机一般携带可见光/红外传感器及激光测距/照射设备完成对地面目标的侦察、定位及照射引导等功能。要完成这些功能，需要无人机的光电设备能稳定的跟踪地面目标，而固定翼无人机在跟踪地面机动目标时，由于需要保持空速，无法悬停，并且无人机的飞行速度一般大于地面目标的运动速度，因此固定翼无人机对地面机动目标的跟踪，需要在无人机与地面机动目标建立一种“虚拟编队”的战位来实现稳定跟踪。

传统的无人机跟踪地面目标的方法是以目标为圆心在目标上空进行盘旋，这种方法实现简单，但不够灵活，无法满足需要无人机在目标侧面进行侦察照射的使用方式要求；若采用无人机在目标侧面进行机动飞行的方式，既增加地面操作人员的工作负荷，又不能精确保证无人机与目标之间的位置关系。因此需要根据无人机载侦察照射设备的激光作用距离、照射引导方式及跟踪角速度限制等因素，建立一种无人机与地面机动目标所组成的“虚拟编队”战位。

由于地面目标运动模式的不确定性，以及侦察设备获取的目标运动信息中的噪声信号，使得如果单纯根据侦察照射设备获取的目标运动信息来调整无人机制导指令，控制无人机航迹，则将会出现无人机制导指令的抖动，从而导致无人机姿态控制指令的抖动，导致无人机姿态频繁变化，这样就不能为侦察照射设备提供稳定的工作环境，严重地，还会诱发振荡，导致飞机失控，影响任务执行并危及无人机飞行安全。

发明内容

为了克服现有技术的不足，实现固定翼无人机对地面机动目标实现灵活稳定的跟踪，解决固定翼无人机由于目标运动信息的不确定性以及侦察设备的测量不确定性对航迹控制的影响问题，本发明提出一种无人机与地面机动目标“虚拟编队”战位保持控制方法。

本发明解决其技术问题所采用的技术方案的步骤如下：

步骤1：建立地面机动目标运动模型，并对目标运动信息进行估计；

对于地面机动目标，采用Singer模型进行描述，即：

式中x，分别为目标的位置、速度、加速度和加加速度；ω(t)是均值为0，方差为σ²的高斯白噪声，α为机动系数；

无人机上装载的机载侦察设备通过对地面机动目标进行测距定位，得到地面机动目标相对于无人机的位置，再根据已知的无人机位置得到地面机动目标的实时位置；

建立离散卡尔曼滤波器，取状态量为[p_x,v_x,a_x,p_y,v_y,a_y]^T，其中 p_x,v_x,a_x,p_y,v_y,a_y分别为目标位置x轴向分量，目标速度x轴向分量，目标加速度x 轴向分量，目标位置y轴向分量，目标速度y轴向分量，目标加速度y轴向分量，目标建立如下卡尔曼滤波器，用于估计目标位置和速度：

P_k,k-1＝AP_k-1,k-1A^T+Q

K_k＝P_k,k-1H^T(HP_k,k-1H^T+R)^-1

P_k,k＝P_k,k-1-K_kHP_k,k-1

其中，和分别为k-1时刻和k时刻的后验状态估计值，A为状态矩阵， A^T为状态矩阵的转置，Q为过程噪声矩阵，H为观测矩阵，H^T为观测矩阵的转置，R 为观测噪声矩阵；P_k-1,k-1表示k-1时刻的后验估计协方差，P_k,k-1表示k时刻的先验估计协方差，K_k为滤波增益矩阵，为k时刻的先验状态估计值，z_k为观测值，P_k,k为 k时刻的后验估计协方差；

步骤2：建立“虚拟编队”模型及模糊状态机，根据目标运动信息维护虚拟编队点运动信息；

引入虚拟编队点O，对于地面机动目标T，O点的位置通过相对于T的方位角ψ，俯仰角θ以及距离r确定，由于目标T不断运动，O点的位置也不断更新，看作O点与目标T编队运动，将这种无人机与地面机动目标之间的相对稳定编队称为“虚拟编队”战位；

无人机对地面机动目标的跟踪问题分解为O点与目标T的非合作编队问题和无人机对以O点为圆心的圆形轨迹的跟踪问题；

将虚拟编队点O与目标T间的距离误差D_e和速度误差V_e作为模糊状态机的输入，设D_e的取值范围为[-D_max,D_max]，V_e的取值范围为[-V_max,V_max]，对距离误差D_e和速度误差V_e作归一化处理：

设d_e与v_e的模糊语言取值均为{负大，小，正大}，对应的模糊隶属度函数均为三角隶属度函数，三角隶属度函数形式为：

其中，该函数参数分别为：负大(a＝-2,b＝-1,c＝-0.1)，小(a＝-0.5,b＝0,c＝0.5)，负大(a＝0.1,b＝1,c＝2)；

将虚拟编队点O的速度指令V_cmd作为模糊状态机的输出，其模糊语言取值设为{负大，小，正大}，其中负大对应-V_cmd，小对应0，正大对应V_cmd；

建立如下模糊规则库：

根据所建立的规则库，采用乘积推理机进行模糊推理：

μ(u)＝μ(d_e)×μ(v_e)

式中，μ(d_e)和μ(v_e)分别为d_e和v_e的隶属度函数；

对于模糊推理的结果，采用重心法进行反模糊化，计算方法如下：

式中，μ_i指第i条规则对应的输出，μ_N(u_i)指该输出的隶属度函数值，得到最终的虚拟编队点O的速度指令u；

步骤3：利用虚拟编队点运动信息和无人机运动信息计算滚转指令；

无人机目标轨迹为以虚拟编队点O为圆心，半径为R_cmd的圆轨迹，虚拟编队点O 的速度为步骤2中计算得出的u，无人机当前位置为P，当前速度为V，与虚拟编队点O的速度方向夹角为ψ_p，该虚拟编队点即为无人机跟踪的地面机动目标，无人机的横向加速度指令由以下步骤计算得到：

(1)无人机运动时间常数为T，阻尼为ξ，ω、k_d、k_d均为增益系数，按以下公式计算：

k_d=ω²,

(2)由无人机与圆心间距离d和半径指令R_cmd得到位置误差d_err，由无人机速度V 沿OP投影可得到速度误差

(3)计算由位置误差得到的横向加速度指令a_cmd1：

(4)计算由动点O运动所需的横向加速度指令a_cmd2：

a_cmd2＝k_cucosψ_p

(5)计算无人机圆周运动所需的横向加速度指令a_cmd3：

(6)计算总的横向加速度指令a_cmd：

a_cmd＝a_cdm1+a_cdm2+a_cdm3

(7)计算滚转角指令φ_cmd，g为重力加速度：

步骤4：

由滚转角指令φ_cmd根据下式计算无人机副翼控制指令δ_a及方向舵控制指令δ_r：

δ_a＝k_φ·(φ-φ_cmd)+k_pp

δ_r＝k_φr·(φ-φ_cmd)+k_rr

式中，p为无人机滚转角速率，r为无人机偏航角速率，φ为无人机滚转角，k_φ，k_φr，k_p，k_r均为控制增益，将计算得到的无人机副翼控制指令δ_a及方向舵控制指令δ_r送入执行机构进行无人机姿态控制，从而实现固定翼无人机与地面机动目标“虚拟编队”战位的保持控制。

本发明的有益效果在于本发明提出的无人机与地面机动目标“虚拟编队”战位保持控制方法，具有以下3个优点：

1.通过构建“虚拟编队”，将无人机对地面机动目标的跟踪问题分解为一中心点与目标的非合作编队问题和无人机对以该中心点为圆心的圆形轨迹的跟踪问题，使得无人机对地面机动目标的跟踪运用方式变得灵活多样，满足多种跟踪需求；

2.对定位所得的目标位置进行卡尔曼滤波，并运用模糊状态机对虚拟编队中心点的速度量进行求解，降低了系统不确定性与测量不确定性对无人机的影响，从而抑制了无人机姿态抖动；

3.运用L1制导算法解决了无人机姿态控制与对机动目标跟踪间的矛盾，提高了无人机对地面机动目标的跟踪能力，同时也保障了飞行安全。

附图说明

图1是本发明发明方法实现框图。

图2是本发明虚拟编队点示意图。

图3是本发明模糊系统隶属度函数。

图4是本发明无人机跟踪动圆示意图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

步骤1：建立地面机动目标运动模型，并对目标运动信息进行估计；

对于地面机动目标，采用Singer模型进行描述，即：

式中x，分别为目标的位置、速度、加速度和加加速度；ω(t)是均值为0，方差为σ²的高斯白噪声，α为机动系数；

无人机上装载的机载侦察设备通过对地面机动目标进行测距定位，得到地面机动目标相对于无人机的位置，再根据已知的无人机位置得到地面机动目标的实时位置；

建立离散卡尔曼滤波器，取状态量为[p_x,v_x,a_x,p_y,v_y,a_y]^T，其中

p_x,v_x,a_x,p_y,v_y,a_y分别为目标位置x轴向分量，目标速度x轴向分量，目标加速度x轴向分量，目标位置y轴向分量，目标速度y轴向分量，目标加速度y轴向分量，目标建立如下卡尔曼滤波器，用于估计目标位置和速度：

P_k,k-1＝AP_k-1,k-1A^T+Q

K_k＝P_k,k-1H^T(HP_k,k-1H^T+R)^-1

P_k,k＝P_k,k-1-K_kHP_k,k-1

其中，和分别为k-1时刻和k时刻的后验状态估计值，A为状态矩阵， A^T为状态矩阵的转置，Q为过程噪声矩阵，H为观测矩阵，H^T为观测矩阵的转置，R 为观测噪声矩阵；P_k-1,k-1表示k-1时刻的后验估计协方差，P_k,k-1表示k时刻的先验估计协方差，K_k为滤波增益矩阵，为k时刻的先验状态估计值，z_k为观测值，P_k,k为 k时刻的后验估计协方差；

步骤2：建立“虚拟编队”模型及模糊状态机，根据目标运动信息维护虚拟编队点运动信息；

引入虚拟编队点O，对于地面机动目标T，O点的位置通过相对于T的方位角ψ，俯仰角θ以及距离r确定，由于目标T不断运动，O点的位置也不断更新，看作O点与目标T编队运动，将这种无人机与地面机动目标之间的相对稳定编队称为“虚拟编队”战位，其示意图如图2。

无人机对地面机动目标的跟踪问题可以分解为O点与目标T的非合作编队问题和无人机对以O点为圆心的圆形轨迹的跟踪问题；

将虚拟编队点O与目标T间的距离误差D_e和速度误差V_e作为模糊状态机的输入，设D_e的取值范围为[-D_max,D_max]，V_e的取值范围为[-V_max,V_max]，对距离误差D_e和速度误差V_e作归一化处理：

设d_e与v_e的模糊语言取值均为{负大，小，正大}，对应的模糊隶属度函数均为三角隶属度函数，三角隶属度函数形式为：

其中，该函数图如图3，参数分别为：负大(a＝-2,b＝-1,c＝-0.1)，小(a＝-0.5,b＝ 0,c＝0.5)，负大(a＝0.1,b＝1,c＝2)；

将虚拟编队点O的速度指令V_cmd作为模糊状态机的输出，其模糊语言取值设为{负大，小，正大}，其中负大对应-V_cmd，小对应0，正大对应V_cmd；

建立如下模糊规则库：

根据所建立的规则库，采用乘积推理机进行模糊推理：

μ(u)＝μ(d_e)×μ(v_e)

式中，μ(d_e)和μ(v_e)分别为d_e和v_e的隶属度函数；

对于模糊推理的结果，采用重心法进行反模糊化，计算方法如下：

式中，μ_i指第i条规则对应的输出，μ_N(u_i)指该输出的隶属度函数值，得到最终的虚拟编队点O的速度指令u；

步骤3：

利用虚拟编队点运动信息和无人机运动信息计算滚转指令；

无人机目标轨迹为以虚拟编队点O为圆心，半径为R_cmd的圆轨迹，虚拟编队点O 的速度为步骤2中计算得出的u，无人机当前位置为P，当前速度为V，与虚拟编队点O的速度方向夹角为ψ_p，该虚拟编队点即为无人机跟踪的地面机动目标，无人机的横向加速度指令由以下步骤计算得到：

(1)无人机运动时间常数为T，阻尼为ξ，ω、k_d、k_d均为增益系数，按以下公式计算：

k_d＝ω²,

(2)由无人机与圆心间距离d和半径指令R_cmd得到位置误差d_err，由无人机速度V 沿OP投影可得到速度误差

(3)计算由位置误差得到的横向加速度指令a_cmd1：

(4)计算由动点O运动所需的横向加速度指令a_cmd2：

a_cmd2＝k_cucosψ_p

(5)计算无人机圆周运动所需的横向加速度指令a_cmd3：

(6)计算总的横向加速度指令a_cmd：

a_cmd＝a_cdm1+a_cdm2+a_cdm3

(7)计算滚转角指令φ_cmd，g为重力加速度：

步骤4：

由滚转角指令φ_cmd根据下式计算无人机副翼控制指令δ_a及方向舵控制指令δ_r：

δ_a＝k_φ·(φ-φ_cmd)+k_pp

δ_r＝k_φr·(φ-φ_cmd)+k_rr

式中，p为无人机滚转角速率，r为无人机偏航角速率，φ为无人机滚转角，k_φ，k_φr，k_p，k_r均为控制增益，将计算得到的无人机副翼控制指令δ_a及方向舵控制指令δ_r送入执行机构进行无人机姿态控制，从而实现固定翼无人机与地面机动目标“虚拟编队”战位的保持控制。

本发明的实施例如下：

步骤1：建立地面机动目标运动模型,并对目标运动信息进行估计。

对于地面机动目标，采用Singer模型来对机动目标进行描述，即：

式中x，分别为目标的位置、速度、加速度和加加速度；ω(t)是均值为0，方差为σ²的高斯白噪声，α取值为0.05。

机载侦察设备通过对目标进行测距定位，得到目标相对于飞机的位置，再根据已知的飞机位置可得到目标的实时位置[p_x,p_y]。

建立离散卡尔曼滤波器，取状态量为[p_x,v_x,a_x,p_y,v_y,a_y]^T，其中 p_x,v_x,a_x,p_y,v_y,a_y分别为：目标位置x轴向分量，目标速度x轴向分量，目标加速度 x轴向分量，目标位置y轴向分量，目标速度y轴向分量，目标加速度y轴向分量，目标建立如下卡尔曼滤波器，用于估计目标位置和速度。

P_k,k-1＝AP_k-1,k-1A^T+Q

K_k＝P_k,k-1H^T(HP_k,k-1H^T+R)^-1

P_k,k＝P_k,k-1-K_kHP_k,k-1

其中：

过程噪声方差取值为200，观测噪声方差取值为350。

步骤2：引入虚拟编队点O，对于地面机动目标T，点O至目标T的方向定义为0°，则点O的位置相对于T的方位角范围为[-38°，38°]，俯仰角范围为[30°，60°]，距离设为3km。

将虚拟编队点O与目标T间的距离误差D_e和速度误差V_e作为模糊状态机的输入。设D_e的取值范围为[-D_max,D_max]，V_e的取值范围为[-V_max,V_max]，对距离误差D_e和速度误差V_e作归一化处理：

设d_e与v_e的模糊语言取值均为{负大，小，正大}，对应的模糊隶属度函数均为三角隶属度函数，参数分别为：负大(a＝-2,b＝-1,c＝-0.1)，小(a＝-0.5,b＝0,c＝0.5)，负大(a＝0.1,b＝1,c＝2)。

三角隶属度函数形式为：

将虚拟编队点O的速度指令作为模糊状态机的输出，其模糊语言取值也设为{负大，小，正大}，其中负大对应-V_cmd，小对应0，正大对应V_cmd。

建立规则库如下表：

根据上述规则库，采用乘积推理机进行模糊推理：

μ(u)＝μ(d_e)×μ(v_e)

式中，μ(d_e)和μ(v_e)分别为d_e和v_e的隶属度函数。

对于模糊推理的结果，采用重心法进行反模糊化，计算方法如下：

得到最终的虚拟编队点O的速度指令u。

步骤3：利用虚拟编队点运动信息和无人机运动信息计算滚转指令。

无人机目标轨迹为以虚拟编队点O为圆心，半径为R_cmd的圆轨迹，虚拟编队点O 的速度为u，无人机当前位置为P，当前速度为V，与虚拟编队点O的速度方向夹角为ψ_p，该虚拟编队点即为无人机跟踪的地面机动目标，无人机的横向加速度指令可以由以下步骤计算得到：

(1)无人机运动时间常数T取值为25，阻尼ξ取值为0.6，动点补偿系数k_c取值为1.1，则可计算得到以下增益系数：

k_d＝ω²＝0.0625

(2)由无人机与圆心间距离d和半径指令R_cmd可得到位置误差d_err，由无人机速度 V沿OP投影可得到速度误差

(3)计算由位置误差得到的横向加速度指令a_cmd1：

(4)计算由动点O运动所需的横向加速度指令a_cmd2：

a_cmd2＝k_cucosψ_p

(5)计算无人机圆周运动所需的横向加速度指令a_cmd3：

(6)计算总的横向加速度指令a_cmd：

a_cmd＝a_cdm1+a_cdm2+a_cdm3

(7)计算滚转角指令φ_cmd，g为重力加速度：

步骤4：

由滚转角指令根据下式计算无人机副翼控制指令δ_a及方向舵控制指令δ_r：

δ_a＝k_φ·(φ-φ_cmd)+k_pp

δ_r＝k_φr·(φ-φ_cmd)+k_rr

式中，p为无人机滚转角速率，r为无人机偏航角速率，φ为无人机滚转角，k_φ，k_φr，k_p，k_r等均为控制增益，在本例中取值分别为0.8，0.2，0.2，0.15。最后，将计算得到的舵面偏转指令送入执行机构进行无人机姿态控制。从而实现固定翼无人机与地面机动目标“虚拟编队”战位的保持控制。

Claims (1)

1.一种无人机与地面机动目标“虚拟编队”战位保持控制方法，其特征在于包括下述步骤：

步骤1：建立地面机动目标运动模型，并对目标运动信息进行估计；

对于地面机动目标，采用Singer模型进行描述，即：

式中x，分别为目标的位置、速度、加速度和加加速度；ω(t)是均值为0，方差为σ²的高斯白噪声，α为机动系数；

无人机上装载的机载侦察设备通过对地面机动目标进行测距定位，得到地面机动目标相对于无人机的位置，再根据已知的无人机位置得到地面机动目标的实时位置；

建立离散卡尔曼滤波器，取状态量为[p_x,v_x,a_x,p_y,v_y,a_y]^T，其中p_x,v_x,a_x,p_y,v_y,a_y分别为目标位置x轴向分量，目标速度x轴向分量，目标加速度x轴向分量，目标位置y轴向分量，目标速度y轴向分量，目标加速度y轴向分量，目标建立如下卡尔曼滤波器，用于估计目标位置和速度：

P_k,k-1＝AP_k-1,k-1A^T+Q

K_k＝P_k,k-1H^T(HP_k,k-1H^T+R)^-1

P_k,k＝P_k,k-1-K_kHP_k,k-1

其中，和分别为k-1时刻和k时刻的后验状态估计值，A为状态矩阵，A^T为状态矩阵的转置，Q为过程噪声矩阵，H为观测矩阵，H^T为观测矩阵的转置，R为观测噪声矩阵；P_k-1,k-1表示k-1时刻的后验估计协方差，P_k,k-1表示k时刻的先验估计协方差，K_k为滤波增益矩阵，为k时刻的先验状态估计值，z_k为观测值，P_k,k为k时刻的后验估计协方差；

步骤2：建立“虚拟编队”模型及模糊状态机，根据目标运动信息维护虚拟编队点运动信息；

引入虚拟编队点O，对于地面机动目标T，O点的位置通过相对于T的方位角ψ，俯仰角θ以及距离r确定，由于目标T不断运动，O点的位置也不断更新，看作O点与目标T编队运动，将这种无人机与地面机动目标之间的相对稳定编队称为“虚拟编队”战位；

无人机对地面机动目标的跟踪问题分解为O点与目标T的非合作编队问题和无人机对以O点为圆心的圆形轨迹的跟踪问题；

将虚拟编队点O与目标T间的距离误差D_e和速度误差V_e作为模糊状态机的输入，设D_e的取值范围为[-D_max,D_max]，V_e的取值范围为[-V_max,V_max]，对距离误差D_e和速度误差V_e作归一化处理：

设d_e与v_e的模糊语言取值均为{负大，小，正大}，对应的模糊隶属度函数均为三角隶属度函数，三角隶属度函数形式为：

其中，该函数参数分别为：负大(a＝-2,b＝-1,c＝-0.1)，小(a＝-0.5,b＝0,c＝0.5)，负大(a＝0.1,b＝1,c＝2)；

将虚拟编队点O的速度指令V_cmd作为模糊状态机的输出，其模糊语言取值设为{负大，小，正大}，其中负大对应-V_cmd，小对应0，正大对应V_cmd；

建立如下模糊规则库：

根据所建立的规则库，采用乘积推理机进行模糊推理：

μ(u)＝μ(d_e)×μ(v_e)

式中，μ(d_e)和μ(v_e)分别为d_e和v_e的隶属度函数；

对于模糊推理的结果，采用重心法进行反模糊化，计算方法如下：

式中，μ_i指第i条规则对应的输出，μ_N(u_i)指该输出的隶属度函数值，得到最终的虚拟编队点O的速度指令u；

步骤3：利用虚拟编队点运动信息和无人机运动信息计算滚转指令；

无人机目标轨迹为以虚拟编队点O为圆心，半径为R_cmd的圆轨迹，虚拟编队点O的速度为步骤2中计算得出的u，无人机当前位置为P，当前速度为V，与虚拟编队点O的速度方向夹角为ψ_p，该虚拟编队点即为无人机跟踪的地面机动目标，无人机的横向加速度指令由以下步骤计算得到：

(1)无人机运动时间常数为T，阻尼为ξ，ω、k_d、均为增益系数，按以下公式计算：

k_d＝ω²,

(2)由无人机与圆心间距离d和半径指令R_cmd得到位置误差d_err，由无人机速度V沿OP投影可得到速度误差

(3)计算由位置误差得到的横向加速度指令a_cmd1：

(4)计算由动点O运动所需的横向加速度指令a_cmd2：

a_cmd2＝k_cu cosψ_p

(5)计算无人机圆周运动所需的横向加速度指令a_cmd3：

(6)计算总的横向加速度指令a_cmd：

a_cmd＝a_cdm1+a_cdm2+a_cdm3

(7)计算滚转角指令φ_cmd，g为重力加速度：

步骤4：

由滚转角指令φ_cmd根据下式计算无人机副翼控制指令δ_a及方向舵控制指令δ_r：

δ_a＝k_φ·(φ-φ_cmd)+k_pp

δ_r＝k_φr·(φ-φ_cmd)+k_rr

式中，p为无人机滚转角速率，r为无人机偏航角速率，φ为无人机滚转角，k_φ，k_φr，k_p，k_r均为控制增益，将计算得到的无人机副翼控制指令δ_a及方向舵控制指令δ_r送入执行机构进行无人机姿态控制，从而实现固定翼无人机与地面机动目标“虚拟编队”战位的保持控制。