CN112197761A - 一种高精度多旋翼机协同定位方法及系统 - Google Patents

Abstract

本发明提供一种高精度多旋翼机协同定位方法及系统，该方法包括：选定需要进行定位的目标；n个无人机进行信息数据的采集，所述信息数据包括无人机自身参数信息、目标视线角信息以及无人机与目标的距离信息；对所述信息数据进行处理，获得目标位置的估计值。本发明所提供的方法采用多无人机协同采集信息，将光电吊舱和组合导航模块之间的安装误差引入最小二乘模型中，实现了在惯性导航元件精度较低的情况下、在短时间内对远距离目标的高精度定位，定位精度高、时间短。

Description

一种高精度多旋翼机协同定位方法及系统

技术领域

本发明涉及目标定位领域，具体涉及一种高精度多旋翼机协同定位方法及系统。

背景技术

多旋翼机如无人机由于具有灵活性、机动性好的特点，现已被广泛应用于军事和民用领域。对地面目标的侦查与高精度定位技术是目前无人机应用的关键技术之一。主流的目标定位方法采用机载光电设备获取目标的相对位置信息，从而根据无人机自身位置对目标的大地系坐标进行解算。

基于光电载荷设备的无人机定位方法分为主动定位与被动定位两类。其中被动定位方法主要包括地图匹配法和角度定位法。地图匹配法将光电吊舱获取的包含目标的图像与已有的参考地图进行图像匹配，从而得到目标坐标，当环境背景较为单一或地图无法获取时，该方法的使用受限。角度定位法仅根据光电吊舱提供的目标角度信息，求解共线方程组来获得目标坐标，该方法在近距离目标定位中效果较好且成本低，但当无人机无法接近目标，需要进行远距离定位时，该方法的定位误差较大。主动定位方法与被动定位相比，加入了测距传感器，可获得目标距离信息，多用于远距离目标定位问题中，同时采用机载导航模块提供无人机姿态信息，实现目标定位，但由于机载导航模块精度的限制，单点定位方法得到的定位结果不能满足高精度定位需求。

发明内容

为了克服上述问题，本发明人进行了锐意研究，提供一种高精度多旋翼机协同定位方法及系统，该方法包括：选定需要进行定位的目标；n个无人机进行信息数据的采集，所述信息数据包括无人机自身参数信息、目标视线角信息以及无人机与目标的距离信息；对所述信息数据进行处理，获得目标位置的估计值。本发明所提供的方法采用多无人机协同采集信息，将光电吊舱和组合导航模块之间的安装误差引入最小二乘模型中，通过迭代运算，获得目标坐标估计值，该方法实现了在惯性导航元件精度较低的情况下、在短时间内对远距离目标的高精度定位，定位精度高、时间短，从而完成本发明。

本发明的目的一方面在于提供一种高精度多旋翼机协同定位方法，该方法包括：

选定需要进行定位的目标；

n个无人机进行信息数据的采集，所述信息数据包括无人机自身参数信息、目标视线角信息以及无人机与目标的距离信息；

对所述信息数据进行处理，获得目标位置的估计值。

本发明的另一方面提供一种高精度多旋翼机协同定位系统，所述系统包括无人机和地面站，所述无人机上搭载有组合导航模块和光电吊舱，所述光电吊舱上设有激光测距仪，

所述无人机与地面站通讯连接，

所述组合导航模块与光电吊舱连接，

所述组合导航模块用于采集无人机的位置和姿态信息，所述组合导航模块包括卫星导航模块与惯性导航模块，

所述光电吊舱用于采集目标的视线角信息，所述视线角信息包括方位角和高低角，

所述激光测距仪用于采集无人机与目标之间的距离信息。

本发明所具有的有益效果为：

(1)本发明所提供的高精度多旋翼机协同定位方法，采用主动定位方法，根据无人机自身参数信息，结合目标视线角即距离信息，利用实现远距离目标的定位；

(2)本发明的方法采用多无人机协同采集目标点信息，采用多点定位消除传感器随机误差，与单个无人机目标定位相比，具有显著的时间优势和精度优势；

(3)本发明的方法将光电吊舱和导航模块之间的安装误差引入到最小二乘模型中，实现了短时间对远距离目标的精确定位；

(4)本发明的方法和系统能够在惯性导航元件精度较低的情况下达到较高精度的定位结果，降低了测量设备的成本，与单个无人机执行目标定位任务相比，能够更快速、精确地求解出目标坐标。

附图说明

图1示出无人机与目标位置的示意图；

图2示出本发明一种优选实施方式的高精度多旋翼机协同定位系统的示意图；

图3示出本发明的目标坐标与无人机坐标的坐标关系；

图4示出本发明的目标坐标与无人机坐标转换关系示意图；

图5示出本发明一种优选实施方式的高精度多旋翼机协同定位方法流程示意图；

图6示出本发明实施例所得多个无人机目标定位迭代CEP曲线；

图7示出本发明实施例所得单个无人机目标定位迭代CEP曲线。

具体实施方式

下面通过附图和优选实施方式对本发明进一步详细说明。通过这些说明，本发明的特点和优点将变得更为清楚明确。

根据本发明，提供一种高精度多旋翼机协同定位方法，如图5所示，该方法包括：

步骤1、选定需要进行定位的目标；

步骤2、n个无人机进行信息数据的采集，所述信息数据包括无人机自身参数信息、目标视线角信息以及无人机与目标距离信息；

步骤3、根据最小二乘模型对所述信息数据进行处理，获得目标位置的估计值。

根据本发明，步骤1中，所述选定需要进行定位的目标包括：将n个无人机悬停在探测区域内，在无人机视野内搜寻目标，选定目标，对目标进行锁定，并设置无人机在目标上方飞行。

本发明中，目标为地面静止目标。

通过地面站软件操控机载光电吊舱实现在无人机视野内搜寻目标，通过地面站软件操控机载光电吊舱并对发现的目标进行框选来锁定目标。

本发明的定位方法主要针对远距离目标采用多旋翼机(无人机)协同定位，能够实现高精度目标定位。

根据本发明，无人机数量n≥3。

根据本发明，设置无人机在目标上方飞行，使无人机围绕目标飞行，从而实现无人机在各个角度采集信息数据。

根据本发明，无人机与目标的距离在4000m以上，或无人机飞行高度在150m以上，优选在200m以上，多旋翼机在该设定高度范围内能够实现高精度定位。

根据本发明，步骤2中，选定目标后，多个无人机按照设定飞行轨迹围绕目标飞行，采集信息数据，实现对目标位置的高精度定位，即无人机在飞行过程中同时采集自身参数信息、目标视线角信息及无人机与目标距离信息，同时将信息数据进行存储，优选地，无人机在多个位置采集信息，从而使得无人机对目标的多个角度进行测试，实现目标的精准定位，如图1所示。

根据本发明，每间隔一定的采样时间，各无人机将各自所采集的信息数据通过数据链路发送至地面站，各无人机采集的信息为各无人机在多个位置采集的信息数据。

根据本发明，由地面站控制光电吊舱转动并在视野内搜索目标，对目标进行框选锁定，同时，激光测距仪开启，对目标进行持续测距。

根据本发明，无人机自身导航参数包括无人机位置和无人机姿态信息，无人机的位置即无人机的坐标，无人机姿态信息包括无人机姿态角，无人机的偏航角ψ、俯仰角θ、和滚转角φ，无人机自身导航参数由组合导航模块获得。

根据本发明，目标视线角信息包括目标方位角和高低角，目标视线角信息由光电吊舱提供，光电吊舱可进行二自由度运动，即俯仰旋转和方位旋转。

根据本发明，无人机与目标距离信息包括无人机与目标之间的距离L，由激光测距仪测得。

根据本发明，步骤3中，n个无人机将采集的信息数据发送至地面站后，地面站对所述信息数据进行处理，获得目标坐标估计值，进而得到目标位置的估计值，优选地，根据最小二乘模型对所述信息数据进行处理，获得目标坐标的估计值。

本发明中，定义以下坐标系：

(1)大地坐标系

大地坐标系(O_A-X_AY_AZ_A)原点O_A与地球质心重合，Z_A轴指向地球北极，X_A轴指向格林尼治平均子午面与赤道的交点，Y_A轴垂直于X_AO_AZ_A平面构成右手坐标系。在大地系中某位置用(B,L,H)表达，其中B,L,H分别代表该位置的纬度、经度和高度。

(2)地理坐标系(导航坐标系)

参考系(O_n-X_nY_nZ_n)原点位于无人机的质心，X_n轴指向北极，Z_n轴垂直于地表指向地面，Y_n正向指向东方，并与X_nO_nZ_n面垂直。

(3)机体坐标系

参考系(O_b-X_bY_bZ_b)原点位于载机的质心，X_b正向指向载机正前向，Y_b正向指向机体正右方，Z_b正向与X_bO_bYb按照右手法则确定。

(4)光电吊舱基座坐标系

参考系(O_g-X_gY_gZ_g)与机体坐标系(O_b-X_bY_bZ_b)之间固连，若两者之间没有安装角偏差时，机体坐标系(O_b-X_bY_bZ_b)与参考系(O_g-X_gY_gZ_g)能够通过平移实现完全重合；在实际使用过程中，安装角偏差可通过标校获得。

(5)光电吊舱视线坐标系(光电球坐标系)

原点位于光学传感器的光轴与镜片平面的交点上，X_c为光电球镜头的光轴方向，方向指向前方，Y_c指向右侧，Z_c与X_cO_cY_c组成右手坐标系。

本发明中，通过上述各坐标系进行转换，获得目标坐标，光电吊舱视线坐标系转换成光电基座坐标系，进一步转换为机体坐标系，转换为地理坐标系，再转换为大地坐标系，转换关系如图3和4所示。

图3中，R_T表示目标位置的矢量，R_U表示无人机机位置的矢量，R表示目标与飞机之间相对位置的矢量。

与常用目标定位坐标转换模型不同的是，本发明考虑了光电吊舱基座和机载导航设备，即机体坐标系之间的安装误差角，在无人机起飞前标定各个无人机的安装角，即光电球航向安装角和俯仰安装角。

根据本发明，步骤3中，对所述信息数据进行处理包括：

步骤3.1、确定目标坐标与无人机自身坐标之间的坐标转换关系，

步骤3.2、确定目标坐标的估计值。

根据本发明，步骤3.1中，设定目标坐标为[x_T，y_T，z_T]^T，x_T、y_T、z_T为目标在大地直角坐标系下的坐标，无人机坐标为[x_U，y_U，z_U]^T，x_U、y_U、z_U为无人机在大地直角坐标系下的坐标，目标坐标与无人机坐标之间的关系如下式所示：

其中，

为从无人机机体坐标系到导航坐标系的转换矩阵，

为从光电吊舱基座坐标系到无人机机体坐标系的转换矩阵，

为从光电吊舱视线坐标系到光电吊舱基座坐标系的转换矩阵，

其中，ψ、θ、φ分别表示无人机的偏航角、俯仰角和俯仰角，ξ、ζ分别表示光电吊舱中光电球航向安装角和俯仰安装角，α、β分别表示目标方位角和高低角。

根据本发明，步骤3.2包括：

步骤3.2.1、确定目标坐标与无人机自身坐标的非线性方程；

步骤3.2.2、根据非线性方程建立最小二乘模型，得到目标坐标的估计值。

根据本发明，步骤3.2.1中，

设定目标坐标的参数表示为：

Θ₁＝[x_T,y_T,z_T]^T

设由测量量构成的向量为y，

设某架无人机发送至地面站的真实测量量的向量为z，

z＝y+v,v～N(0,R₀)

其中，v为测量噪声，服从高斯分布，协方差矩阵为9×9的矩阵R₀；

将目标坐标与无人机坐标之间的关系表示为式(2)：

Θ₁＝f(y)＝f(z-v) 式(2)，

其中，f为上述坐标转换关系式表示的非线性函数。

对式(2)进行泰勒展开，得到：

即：

根据本发明，步骤3.2.2中，设n个无人机共在N个点进行信息采集，N≥n，可得到N组测量数据，从而得到N个上述表达式，整合成矩阵形式，得到：

其中V为误差项，其协方差矩阵为：

由加权最小二乘法求解上述线性估计问题，得到目标坐标估计量的表达式如式(5)所示：

其中，z_k为第k个点的真实数据信息构成的向量，k＝1、2、……N，A_k为f对测量量z_k求偏导得到的雅可比矩阵，即

所得目标坐标估计值的估计误差协方差矩阵为：

根据本发明，步骤3.2.2中，对目标坐标估计值的估计误差协方差矩阵P进行判断，若|P|小于设定阈值，则说明当前所得目标坐标估计值满足要求，则以该估计值为目标的最终坐标，则定位结束，无人机停止飞行任务，否则，循环步骤2-3，进行迭代过程，直至|P|小于设定阈值，无人机飞行任务停止，所得目标坐标的估计值即为目标的最终坐标。

根据本发明一种优选的实施方式，采用蒙特卡洛仿真，得到迭代过程的CEP(圆概率误差)收敛曲线，横坐标为采样时间，纵坐标为目标位置绝对误差，CEP收敛曲线误差的方差即|P|，|P|越小，CEP收敛曲线收敛的越好。

根据本发明一种优选的实施方式，当|P|小于设定阈值时，则确定目标估计值为目标的最终坐标，优选地，设定阈值在2×10^-6以下，优选在4×10^-6以下，目标位置绝对误差(目标定位精度)达到20m以下，则无人机任务停止。

根据本发明，通过上述方法得到目标在大地直角坐标系的坐标估计值，通过坐标转换即可得到目标的经度、纬度、高度信息，实现对目标位置的确定。根据本发明，提供一种高精度多旋翼机协同定位系统，该系统包括多个无人机和地面站，无人机上搭载有组合导航模块和光电吊舱，光电吊舱中集成有激光测距仪。

根据本发明，组合导航模块包括卫星导航模块与惯性导航模块，组合导航模块提供无人机自身导航参数，包括无人机的位置和姿态信息。

根据本发明，卫星导航模块包括卫星导航接收机和卫星天线，惯性导航模块包括加速度计和/或陀螺仪，卫星导航模块确定无人机坐标，惯性导航模块确定无人机偏航角、俯仰角和滚转角。

根据本发明，组合导航模块与光电吊舱连接，优选固连安装。

根据本发明，所述光电吊舱用于采集目标的视线角信息，所述视线角信息包括方位角和高低角。

根据本发明，激光测距仪采集无人机与目标之间的距离信息。

根据本发明，各无人机将所采集的无人机的位置和姿态信息、目标的视线角信息和无人机与目标之间的距离信息数据通过数据链路发送至地面站，地面站包括数据处理模块，数据处理模块根据最小二乘模型对无人机发送的信息数据进行处理，获得目标位置的估计值，进而确定目标位置。

本发明所提供的高精度多旋翼机协同定位方法和系统，通过多无人机协同采集目标点信息，采用多点定位消除传感器随机误差的方法，同时将光电吊舱与组合导航模块之间的安装误差引入模型中，实现了短时间内对远距离目标的精确定位，与单机目标定位方法相比，本发明所提供的方法，在相同采样时间的条件下，定位精度有显著提升，在相同的CEP误差要求下，需要的定位收敛时间显著减少。

实施例

对多无人机定位目标进行仿真，设定无人机数量为3个，无人机在距离目标3000m以外时，通过光电吊舱的摄像头探测到目标，对目标进行锁定，激光测距仪开始对目标测距，无人机自身以10m/s的飞行速度采集数据，并记录下各传感器(包括组合导航模块、光电吊舱、激光测距仪)的数据。

各传感器的噪声服从高斯分布，为均值为零的高斯白噪声，各传感器参数的误差水平如表1所示：

表1

为加快目标位置估计误差曲线的收敛，无人机在稳定锁定目标之后，自身也进行机动，以获得飞机在不同相对位置时对目标的测量值。设三个无人机的速度值为10m/s，飞行高度为300m，检测到目标时，与目标距离为3100m，数据采集频率为1Hz，各无人机将采集的自身参数信息、目标视线角信息和无人机与目标的距离信息发送至地面站。

根据最小二乘模型，地面站对收集到的信息进行处理，具体地，设目标坐标的参数表示为：

Θ₁＝[x_T,y_T,z_T]^T

由测量量构成的向量y，

设单个无人机发送至地面站的真实测量量的向量为z，

z＝y+v,v～N(0,R₀)

其中，v为测量噪声，服从高斯分布，为均值为零的高斯白噪声；

目标坐标与无人机坐标之间的关系为：

Θ₁＝f(y)＝f(z-v)，

其中，f为上述坐标转换关系式表示的非线性函数，

对上式进行泰勒展开，得到：

即：

3个无人机在N个点进行信息采集，可得到N组测量数据，整合成矩阵形式，得到：

其中，N为600，

其中V为误差项，其协方差矩阵为：

由加权最小二乘法求解上述线性估计问题，得到目标坐标的估计值：

所得目标坐标估计值的估计误差协方差矩阵为：

对估计值的估计误差协方差矩阵进行判断，判断|P|是否小于设定阈值4×10^-6，若是，则当前所得到的目标坐标估计值为目标的最终坐标；若否，则无人机继续飞行采集信息，重复上述过程，直至|P|小于设定阈值4×10^-6，任务结束。

对三个无人机同时在不同方位采集到的目标数据进行处理，求解目标位置，仿真时间200s，进行1000次蒙特卡洛仿真后，所得迭代过程的CEP收敛曲线如图6所示，其中，横坐标为采样时间(单位为s)，纵坐标为CEP收敛曲线的误差即目标位置的绝对误差(单位为m)。

作为对比，采用上述仿真条件，将3个无人机替换为单个无人机进行仿真，即，单个无人机的速度值为10m/s，飞行高度为300m，检测到目标时，距离为3100m，数据采集频率为1Hz，各传感器的噪声服从高斯分布，为均值为零的高斯白噪声，各传感器参数的误差水平如表1所示，单个无人机将采集的自身参数信息、目标视线角信息和无人机与目标的距离信息发送至地面站，同样地，地面站对单个无人机采集到的信息数据进行处理，求解目标位置，仿真时间200s，通过1000次蒙特卡洛仿真后，所得迭代过程的CEP收敛曲线如图7所示。

如图6和图7，对比单个无人机和多个无人机的CEP收敛曲线，可以看出，由于单个无人机机在短时间内自身活动范围的限制，采集到的数据不足以使目标位置误差快速收敛，在t＝200s时，目标位置的绝对误差依然在CEP 40m以上。而多无人机机定位增加了飞机在不同方位观测到的有效目标信息，可以使目标的位置绝对误差快速收敛，在t＝79s时，目标位置绝对误差已经达到CEP 10m左右，达到了在短时间、远距离条件下高精度定位的目的。

以上结合优选实施方式和范例性实例对本发明进行了详细说明。不过需要声明的是，这些具体实施方式仅是对本发明的阐述性解释，并不对本发明的保护范围构成任何限制。在不超出本发明精神和保护范围的情况下，可以对本发明技术内容及其实施方式进行各种改进、等价替换或修饰，这些均落入本发明的保护范围内。本发明的保护范围以所附权利要求为准。

Claims (10)

1.一种高精度多旋翼机协同定位方法，其特征在于，所述方法包括：

步骤1、选定需要进行定位的目标；

步骤2、n个无人机进行信息数据的采集，所述信息数据包括无人机自身参数信息、目标视线角信息以及无人机与目标的距离信息；

步骤3、对所述信息数据进行处理，获得目标位置的估计值。

2.根据权利要求1所述的方法，其特征在于，步骤1，所述选定需要进行定位的目标包括：将n个无人机悬停在探测区域内，在无人机视野内搜寻目标，选定目标，并设置无人机在目标上方飞行。

3.根据权利要求1所述的方法，其特征在于，步骤2中，所述无人机自身参数信息由组合导航模块获得，所述无人机自身参数信息包括无人机位置信息和无人机姿态信息，

所述无人机位置信息为无人机坐标，所述无人机姿态信息包括无人机偏航角、俯仰角和滚转角，

所述目标视线角信息由光电吊舱获得，所述目标视线角信息包括目标方位角和高低角。

4.根据权利要求1所述的方法，其特征在于，步骤3中，所述n个无人机将采集的信息数据发送至地面站后，地面站对所述信息数据进行处理，获得目标坐标估计值，优选地，根据最小二乘模型对所述信息数据进行处理，

更优选地，对所述信息数据进行处理包括：

步骤3.1、确定目标坐标与无人机自身坐标之间的坐标转换关系，

步骤3.2、确定目标坐标的估计值。

5.根据权利要求4所述的方法，其特征在于，步骤3.1中，设定目标坐标为[x_T，y_T，z_T]^T，无人机自身坐标为[x_U，y_U，z_U]^T，目标坐标与无人机自身坐标之间的关系如下式所示：

其中，

为从无人机机体坐标系到导航坐标系的转换矩阵，

为从光电吊舱基座坐标系到无人机机体坐标系的转换矩阵，

为从光电吊舱视线坐标系到光电吊舱基座坐标系的转换矩阵。

6.根据权利要求4所述的方法，其特征在于，步骤3.2包括：

步骤3.2.1、确定目标坐标与无人机自身坐标的非线性方程；

步骤3.2.2、根据非线性方程建立最小二乘模型，得到目标坐标的估计值；

优选地，步骤3.2.1中：

设定目标坐标为Θ₁＝[x_T，y_T，z_T]^T，

设由测量量构成的向量为y，y＝[x_U,y_U,z_U,α,β,φ,θ,ψ,L]^T，

设无人机发送至地面站的真实测量量的向量为z，

z＝y+v,v～N(0,R₀)

其中，v为测量噪声，服从高斯分布，协方差矩阵为9×9的矩阵R₀，

将目标坐标与无人机坐标之间的关系表示为下式：

Θ₁＝f(y)＝f(z-v) 式(2)，

其中，f为上述坐标转换关系式表示的非线性函数，

对式(2)进行泰勒展开，得到：

即：

7.根据权利要求6所述的方法，其特征在于，步骤3.2.2中，设n个无人机共在N个点进行信息数据采集，可得到N组测量数据，得到N个式(3)，整合成矩阵形式，得到：

其中V为误差项，其协方差矩阵为：

由加权最小二乘法求解上述线性估计问题，得到目标坐标估计量的表达式如式(5)所示：

其中，z_k为第k个点真实数据信息构成的向量，k＝1、2、……N，A_k为f对z_k求偏导得到的雅可比矩阵，即

所得目标坐标估计值的估计误差协方差矩阵为：

8.根据权利要求7所述的方法，其特征在于，步骤3.2.2中，对目标坐标估计值的估计误差协方差矩阵P进行判断，若|P|小于设定阈值，则无人机飞行任务停止；否则，循环步骤2-3，直至|P|小于设定阈值，无人机飞行任务停止，所得目标坐标的估计值即为目标的最终坐标。

9.一种高精度多旋翼机协同定位系统，其特征在于，所述系统包括无人机和地面站，所述无人机上搭载有组合导航模块和光电吊舱，所述光电吊舱上设有激光测距仪，

所述组合导航模块与光电吊舱连接，

所述组合导航模块用于采集无人机的位置和姿态信息，所述组合导航模块包括卫星导航模块与惯性导航模块，

所述光电吊舱用于采集目标的视线角信息，所述视线角信息包括方位角和高低角，

所述激光测距仪用于采集无人机与目标之间的距离信息。

10.根据权利要求9所述的系统，其特征在于，所述无人机将无人机的位置和姿态信息、目标的视线角信息和无人机与目标之间的距离信息数据通过数据链路发送至地面站，所述地面站包括数据处理模块，所述数据处理模块根据最小二乘模型对无人机发送的信息数据进行处理，获得目标位置的估计值。

Images