拒止环境下的多无人机自身定位与目标定位方法与流程

1.本发明涉及通过无人机对其自身及目标定位的方法，具体涉及一种在拒止环境下的多无人机自身定位与目标定位方法。


背景技术：

2.多旋翼无人机由于具有机动性、灵活性好的特点，现已被广泛应用于军事和民用领域。对地面目标的侦查与定位技术是目前多旋翼无人机应用的关键技术之一。
3.随着多机协同技术的发展，基于多个无人机的定位问题已成为目前的研究热点。多机定位问题可分为多机自身定位问题和多机目标定位问题。其中多机自身定位问题解决的是在拒止环境下，各无人机通过测量与基站和邻近无人机之间的相对位置信息，改善自身的绝对位置定位精度。多无人机协同目标定位问题指的是通过多个无人机从不同位置获取与目标之间的相对信息，快速准确地实现对目标的定位，但在实际应用场景中，可能由于同频信号干扰或建筑物遮蔽等原因导致无人机自身的导航系统失效，从而无法对目标进行准确定位。这两个问题是存在耦合的，在目标定位问题中，目标定位的精度取决于无人机自身的定位精度和相对信息测量的精度；各无人机协同目标定位的结果反过来也能提高无人机自身定位的质量；
4.然而，在现有技术中，尚没有针对拒止环境的无人机及目标定位的有效方案，所以目前亟需一种能够在拒止环境下提供无人机及目标准确位置的方法。
5.由于上述原因，本发明人对现有的无人机及目标定位方法做了深入研究，以期待设计出一种能够解决上述问题的拒止环境下的多无人机自身定位与目标定位方法。


技术实现要素：

6.为了克服上述问题，本发明人进行了锐意研究，设计出一种拒止环境下的多无人机自身定位与目标定位方法，所述拒止环境是指屏蔽卫星信号的环境，在该环境下利用卫星进行定位的系统都不能正常接收到卫星信号，不能正常工作；该方法中，通过在地面设立绝对位置已知的基站，并由基站提供与其通信的无人机之间的相对距离和角度信息，将基站提供的信息与无人机之间测量的相对位置信息以及无人机探测得到的目标信息用卡尔曼滤波算法进行融合，从而获得无人机、目标位置信息，再通算出观测无人机和中转无人机的航向角，控制观测无人机和中转无人机进一步调整位置，到达新的位置再次探测得到新的探测信息，持续重复该过程，逐步提升获得的无人机、目标位置信息精度，以使得满足使用需求，从而完成本发明。
7.具体来说，本发明的目的在于提供拒止环境下的多无人机自身定位与目标定位方法，该方法中：
8.设置至少两个观测无人机、至少一个中转无人机和一个已知位置的基站，
9.所述观测无人机用于探测获得：每个观测无人机和目标之间的距离、任意两个观测无人机彼此之间的距离；
10.所述中转无人机用于探测获得：中转无人机和每个观测无人机之间的距离；
11.所述基站用于探测获得：基站与中转无人机之间的距离、基站与每个观测无人机之间的距离、基站与中转无人机之间的视线角；
12.该方法包括如下步骤：
13.步骤1，通过观测无人机、中转无人机和基站探测得到观测向量；
14.步骤2，用卡尔曼滤波算法对观测向量做融合处理；
15.步骤3，求解观测无人机和中转无人机的航向角，并据此控制观测无人机和中转无人机到达下一个航点；
16.步骤4，重复上述步骤1、步骤2和步骤3，直至状态估计的误差方差矩阵的对角线元素之和减小到设定阈值。
17.其中，在步骤3中，在k时刻预测第k n时刻的状态估计误差方差矩阵p
k n
的逆矩阵，即信息矩阵j
k n
，求解使得信息矩阵j
k n
最大化的观测无人机和中转无人机的航向角。
18.其中，在步骤3中，通过下式(一)获得观测无人机和中转无人机的航向角；
[0019][0020]
其中，表示包含三个航向角的向量；
[0021]
j
k n
表示信息矩阵，通过下式(二)获得：
[0022][0023]
其中，所述p0表示卡尔曼滤波算法中状态估计误差方差矩阵的初值；
[0024]
所述i
i
表示i时刻的观测向量中与待估计的状态向量相关的信息量，通过下式(三)获得：
[0025][0026]
其中，h
i
为观测向量对状态向量求偏导得到的雅可比矩阵，通过下式(四)获得：
[0027][0028]
所述观测向量包括每个观测无人机和目标之间的距离、任意两个观测无人机彼此之间的距离、中转无人机和每个观测无人机之间的距离、基站与中转无人机之间的距离、基站与每个观测无人机之间的距离、基站与中转无人机之间的视线角；
[0029]
所述状态向量包括目标的位置、中转无人机的位置和每个观测无人机的位置；
[0030]
所述x
i
表示第i时刻的状态向量，包含无人机和目标的位置信息；
[0031]
所述x
b
表示基站位置；
[0032]
所述r表示观测传感器的噪声方差矩阵；
[0033]
其中，在步骤4中，所述状态估计的误差方差p通过下式(五)获得：
[0034][0035]
其中，p
k
表示k时刻的状态估计的误差方差，p
‑
k
表示预测的k时刻状态误差方差矩
阵；
[0036][0037]
a表示系统状态转移矩阵，q表示状态误差方差矩阵，表示预测的k
‑
1时刻状态误差方差矩阵；
[0038]
k
k
表示卡尔曼增益：
[0039][0040]
其中，在步骤4中，所述设定阈值为50～80。
[0041]
本发明所具有的有益效果包括：
[0042]
(1)根据本发明提供的拒止环境下的多无人机自身定位与目标定位方法，该方法迭代过程简单，迭代速度快，整体执行过程简单可控；
[0043]
(2)根据本发明提供的拒止环境下的多无人机自身定位与目标定位方法，该方法在执行过程中能够实时获得目标位置和无人机位置信息，并且获得的信息越来越精确，即能够在未获得精确信息前提供相对准确的位置信息，以满足特殊的信息需求。
附图说明
[0044]
图1示出根据本发明一种优选实施方式的拒止环境下的多无人机自身定位与目标定位方法整体逻辑图；
[0045]
图2示出根据本发明一种优选实施方式的在二维平面上的观测关系示意图；
[0046]
图3示出本发明实施例中三个无人机航点轨迹图；
[0047]
图4示出本发明实施例中目标定位误差变化示意图；
[0048]
图5示出本发明实施例中三个无人机定位误差变化示意图；
[0049]
图6示出本发明实施例中误差方差矩阵的对角线元素之和的变化规律示意图。
具体实施方式
[0050]
下面通过附图和实施例对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。
[0051]
在这里专用的词
″
示例性
″
意为
″
用作例子、实施例或说明性
″
。这里作为
″
示例性
″
所说明的任何实施例不必解释为优于或好于其它实施例。尽管在附图中示出了实施例的各种方面，但是除非特别指出，不必按比例绘制附图。
[0052]
根据本发明提供的一种拒止环境下的多无人机自身定位与目标定位方法，所述拒止环境是指屏蔽卫星信号的环境，在该环境下利用卫星进行定位的系统都不能正常接收到卫星信号，不能正常工作；该方法中：
[0053]
设置至少两个观测无人机、至少一个中转无人机和一个已知位置的基站，
[0054]
所述观测无人机用于探测获得：每个观测无人机和目标之间的距离、至少两个观测无人机彼此之间的距离；
[0055]
所述中转无人机用于探测获得：中转无人机和每个观测无人机之间的距离；
[0056]
所述基站用于探测获得：基站与中转无人机之间的距离、基站与每个观测无人机
之间的距离、基站与中转无人机之间的视线角；
[0057]
如图1中所示，该方法包括如下步骤：
[0058]
步骤1，通过观测无人机、中转无人机和基站探测得到观测向量z
k
；
[0059]
步骤2，用卡尔曼滤波算法对观测向量做融合处理，得到状态向量
[0060]
步骤3，求解观测无人机和中转无人机的航向角，并根据航向角分别控制观测无人机和中转无人机到达新的航点；
[0061]
步骤4，重复上述步骤1、步骤2和步骤3，直至状态估计的误差方差矩阵的对角线元素之和减小到设定阈值。
[0062]
优选地，在每个无人机上都搭载有激光测距仪，用以探测与其他无人机或者目标之间的距离，在每个无人机还设置有光电吊舱，能够直接获取方位角，进而用以获得基站与中转无人机之间的视线角；在所述基站上也设置有用于测量距离的激光测距仪。
[0063]
在一个优选的实施方式中，在步骤1中，所述观测向量z
k
包括角度测量和距离测量信息，具体来说，观测向包括每个观测无人机和目标之间的距离、任意两个观测无人机彼此之间的距离、中转无人机和每个观测无人机之间的距离、基站与中转无人机之间的距离、基站与每个观测无人机之间的距离、基站与中转无人机之间的视线角。当目标、观测无人机、中转无人机和基站都在同一个二维平面内，且所述观测无人机设置为两个，中转无人机设置为一个时，如图2中所示，此时的观测向量z
k
可表示为：
[0064]
z
k
＝[ρ
2t
，ρ
3t
，φ
10
，ρ
10
，ρ
20
，ρ
30
，ρ
12
，ρ
13
，ρ
23
]
t
[0065]
ρ
2t
和ρ
3t
分别表示两个观测无人机探测得到的观测无人机与目标之间的距离；
[0066]
φ
10
表示基站与中转无人机之间的视线角；
[0067]
ρ
10
表示基站与中转无人机之间的距离；
[0068]
ρ
20
和ρ
30
分别表示基站与两个观测无人机之间的距离；
[0069]
ρ
12
和ρ
13
分别表示中转无人机与两个观测无人机之间的距离；
[0070]
ρ
23
表示两个观测无人机之间的距离。
[0071]
所述基站的位置信息已知，即x
b
＝[x0，y0]
t
；
[0072]
所述观测向量z
k
与下述观测方程等价，所述观测方程为：
[0073]
[0074]
其中，v
k
表示量测噪声，服从均值为0，方差为r的正态分布；
[0075]
(x
t
，y
t
)表示目标位置，(x1，y1)表示中转无人机位置，(x2，y2)和(x3，y3)分别表示观测无人机位置。
[0076]
优选地，在步骤1中，第一次通过观测无人机、中转无人机和基站进行探测时，给出一组估计得到的目标位置、中转无人机位置和观测无人机位置，用以解算获得初始的观测方程；在后续循环迭代过程中逐步获得更为精确的上述位置信息。所述目标初始位置根据两个探测目标的无人机获得的距离利用三角交汇推算目标粗略位置，无人机初始位置由机上的惯性导航设备给出，拒止环境下无卫星导航辅助的惯性导航漂移误差比较大，通过本技术中的方法能够在短时间内消除该误差。
[0077]
本技术中，上述观测向量是实时获得，其探测/获得频率是1hz，无人机每接收到一次航向角指令，会据此飞行1s，在完成该航向角指令后再次探测获得上述观测向量。
[0078]
在根据航向角飞行时，无人机按预设速度定速飞行，航向角的接收频率1hz。
[0079]
本技术中，所述中转无人机和观测无人机都在同一时刻通过探测器探测获得用于构成观测向量的数据信息，中转无人机和观测无人探测信息时所处的位置即为所述航点，在同一时刻，每个无人机所在的航点都是不同的。每次解算获得的航向角都有多个，每个无人机都对应有一个航向角，各个无人机获得的航向角可以是彼此不同的，当无人机接收到航向角以后，都会按照航向角进行移动飞行，其飞行时间为1s，从而到达一个新的空间位置，并在新的空间位置再次进行探测，进而得到新的观测向量，所述新的空间位置即为新的航点。
[0080]
优选地，在每个无人机上都设置有数传电台，从而实时地将其观测到的信息传递给基站，并接收基站传递来的航向角指令；在所述基站上也设置有数传电台，从而实时地接收观测向量，并将航向角指令传递给每一架无人机。
[0081]
在一个优选的实施方式中，在步骤2中，通过扩展卡尔曼滤波算法进行所述融合处理，通过该融合算法能够得到状态向量。
[0082]
在一个优选的实施方式中，在步骤3中，在k时刻预测第k n时刻的状态估计误差方差矩阵p
k n
的逆矩阵，即信息矩阵j
k n
，求解使得信息矩阵j
k n
最大化的观测无人机和中转无人机的航向角。
[0083]
优选地，k时刻的状态向量为：
[0084]
x
k
＝[x
t
，y
t
，x1，y1，x2，y2，x3，y3]
kt
，
[0085]
相应地估计k 1时刻的状态向量为：
[0086]
x
k 1
＝x
k
[0，0，v
0 cosψ
k1
，v
0 sinψ
k1
，v
0 cosψ
k2
，v
0 sinψ
k2
，v
0 cosψ
k3
，v
0 sinψ
k3
]
t
ω
k
[0087]
本技术中，在优化航向角时，需要预测以后n个时刻(k 1到k n时刻)目标函数的值，目标函数的表达式中包括x
k 1
到x
k n
，所以需要估计后续时刻的状态向量。
[0088]
其中，ω
k
表示过程噪声，ψ
k1
表示中转无人机在k时刻的航向角；ψ
k2
和ψ
k3
分别表示两个观测无人机在k时刻的航向角。
[0089]
优选地，在步骤3中，通过下式(一)获得观测无人机和中转无人机的航向角；
[0090]
[0091]
其中，表示三个航向角组成的优化向量，即ψ
k1
、ψ
k2
和ψ
k3
组成的优化向量；
[0092]
为数学符号，表示使右侧括号内目标函数取最大值；
[0093]
j
k n
表示信息矩阵，通过下式(二)获得：
[0094][0095]
其中，所述p0表示卡尔曼滤波算法中状态的初始误差方差，其取值如下：
[0096]
[500，0，0，0，0，0，0，0；0，500，0，0，0，0，0，0；0，0，100，0，0，0，0，0；0，0，0，100，0，0，0，0；0，0，0，0，100，0，0，0；0，0，0，0，0，100，0，0；0，0，0，0，0，0，100，0；0，0，0，0，0，0，0，100；]
[0097]
所述i
i
表示i时刻的观测向量中与待估计的状态向量相关的信息量，通过下式(三)获得：
[0098][0099]
其中，h
i
为i时刻观测向量对状态向量求偏导得到的雅可比矩阵，通过下式(四)获得：
[0100][0101]
所述观测向量为：
[0102]
z
k
＝[ρ
2t
，ρ
3t
，φ
10
，ρ
10
，ρ
20
，ρ
30
，ρ
12
，ρ
13
，ρ
23
]
t
；
[0103]
所述状态向量为x
k
＝[x
t
，y
t
，x1，y1，x2，y2，x3，y3]
kt
；
[0104]
所述x
i
表示第i时刻的状态向量，包含无人机位置信息和目标的位置信息；优选包含三个无人机位置信息和一个目标位置信息；
[0105]
所述x
b
表示基站位置；
[0106]
所述r表示传感器噪声方差矩阵；
[0107]
在一个优选的实施方式中，在步骤4中，所述状态估计的误差方差通过下式(五)获得：
[0108][0109]
其中，p
k
表示k时刻的状态估计的误差方差；
[0110]
p
‑
k
表示预测的k时刻状态误差方差矩阵；
[0111][0112]
a表示系统状态转移矩阵，q表示状态误差方差矩阵，表示预测的k
‑
1时刻状态误差方差矩阵；
[0113]
k
k
表示卡尔曼增益：
[0114][0115]
在一个优选的实施方式中，在步骤4中，所述设定阈值为，30～100，优选为50～80，
更优选为50，本技术中，态估计的误差方差矩阵的对角线元素之和小于该阈值时，即为每个状态对应的误差方差之和小于该阈值，此时各状态量均已收敛，定位误差达到米级，认为此时的误差在可接受范围之内。
[0116]
在一个优选的实施方式中，当所述状态估计的误差方差矩阵的对角线元素之和小于所述设定阈值时，对应的状态向量即为准确的输出值，从而获得准确的目标位置信息和无人机位置信息。
[0117]
在一个优选的实施方式中，当目标、观测无人机、中转无人机和基站都在同一个三维空间内，基站和目标都是固定在地面不动的，观测无人机和中转无人机都能够在三维空间内飞行或悬停，此时的观测向量、观测方程和状态向量为：
[0118]
z
k
＝[ρ
2t
，ρ
3t
，φ
10
，ρ
10
，ρ
20
，ρ
30
，ρ
12
，ρ
13
，ρ
23
]
t
[0119][0120]
x
k
＝[x
t
，y
t
，x1，y1，x2，y2，x3，y3]
kt
[0121]
实施例
[0122]
选择三架相同的无人机，其飞行速度都为10m/s，分别编号为uav1、uav2和uav3，对应的初始坐标分别为[10m，10m]、[0m，
‑
20m]、[
‑
20m，0m]；设置基站坐标为[0m，0m]，目标真实坐标为[200m，200m]。uav1为中转无人机，uav2和uav3为观测无人机。
[0123]
其中，在uav1、uav2和uav3上都安装有激光测距仪和数传电台；在基站上设置有激光测距仪、光电吊舱和数传电台。
[0124]
在控制过程中，三架无人机的转弯速率约束为10
°
/s，状态向量的初始值为：
[0125]
x0＝[180，220，12，12，6，
‑
22，
‑
23，7]
t
[0126]
即控制过程的初始时刻目标坐标是[180m，220m]，uav1坐标为[12m，12m]，uav2坐标为[6m，
‑
22m]，uav3坐标为[
‑
23m，7m]。
[0127]
对上述三个无人机做如下控制：
[0128]
步骤1，通过uav1、uav2和uav3和基站探测得到观测向量，即uav1与uav2、uav3之间的距离，基站与uav1、uav2和uav3之间的距离，基站与uav1之间的视线角，uav2与目标之间的距离，uav3与目标之间的距离，uav2和uav3之间的距离；
[0129]
步骤2，用卡尔曼滤波算法对量测向量做融合处理，得到状态向量；
[0130]
所述状态向量中包括目标位置信息和所有无人机的位置信息，即目标、uav1、uav2和uav3的坐标信息；
[0131]
通过扩展卡尔曼滤波算法进行融合处理；
[0132]
步骤3，求uav1、uav2和uav3的航向角，并据此控制uav1、uav2和uav3到达新的航点；
[0133]
其中，通过下式(一)获得观测无人机uav2、uav3和中转无人机uav1的航向角；
[0134][0135]
其中，表示三个航向角组成的优化向量；
[0136]
j
k n
通过下式(二)获得
[0137][0138]
其中，p0取值为
[0139]
[500，0，0，0，0，0，0，0；0，500，0，0，0，0，0，0；0，0，100，0，0，0，0，0；0，0，0，100，0，0，0，0；0，0，0，0，100，0，0，0；0，0，0，0，0，100，0，0；0，0，0，0，0，0，100，0；0，0，0，0，0，0，0，100；]
[0140]
ii通过下式(三)获得
[0141][0142]
其中，h
i
通过下式(四)获得
[0143][0144]
得到：uav1的航向角为
‑
120
°
，uav2的航向角为50
°
，uav3的航向角为30
°
，根据航向角控制uav1、uav2和uav3都以10m/s的速度移动1秒，然后再次探测得到观测向量，据此得到新的航向角；
[0145]
步骤4，重复50次上述步骤1、步骤2和步骤3；得到的无人机uav1、uav2和uav3的运动轨迹即无人机的航点如图3中所示，相应地，每个航点对应的目标定位误差变化情况如图4中所示；三个无人机的定位误差变化情况如图5中所示；
[0146]
由图4可知，目标定位误差在15秒内即可收敛到10米以内；
[0147]
根据图5可知，在没有卫星导航提供绝对坐标的情况下，无人机的位置误差维持在3米以内。
[0148]
进一步地，在步骤4中，实时通过下式(五)解算状态估计的误差方差，
[0149][0150]
其中，p
k
表示k时刻的状态估计的误差方差；p
‑
k
表示预测的k时刻状态误差方差矩阵；
[0151][0152]
a表示系统状态转移矩阵，q表示状态误差方差矩阵，表示预测的k
‑
1时刻状态
误差方差矩阵；
[0153]
k
k
表示卡尔曼增益：
[0154][0155]
每个航点对应的状态估计的误差方差矩阵的对角线元素之和如图6中所示，
[0156]
从而可知，在状态估计的误差方差矩阵的对角线元素之和达到50时，停止迭代计算，此时对应的目标定位误差为5m，无人机uav1、uav2和uav3的定位误差分别为2.5m、0.8m、2.4m，在状态估计的误差方差矩阵的对角线元素之和达到50，进而停止迭代计算时得到的目标、uav1、uav2和uav3的位置信息能够满足控制需求。
[0157]
以上结合了优选的实施方式对本发明进行了说明，不过这些实施方式仅是范例性的，仅起到说明性的作用。在此基础上，可以对本发明进行多种替换和改进，这些均落入本发明的保护范围内。