[发明专利]一种高精度多旋翼机协同定位方法及系统

权利要求书

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

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

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

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

所述n个无人机将采集的信息数据发送至地面站后，地面站对所述信息数据进行处理，获得目标坐标估计值，根据最小二乘模型对所述信息数据进行处理，

对所述信息数据进行处理包括：

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

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

步骤3.1中，设定目标坐标为[x_T，y_T，z_T]^T，无人机自身坐标为[x_U，y_U，z_U]^T，目标坐标与无人机自身坐标之间的关系如下式所示：

其中，为从无人机机体坐标系到导航坐标系的转换矩阵，为从光电吊舱基座坐标系到无人机机体坐标系的转换矩阵，为从光电吊舱视线坐标系到光电吊舱基座坐标系的转换矩阵；L表示无人机与目标之间的距离；

步骤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，

x_U，y_U，z_U表示无人机在大地直角坐标系下的三维坐标，α表示光电吊舱测量获得的目标方位角；β表示光电吊舱测量获得的目标高低角；φ表示无人机的滚转角；θ表示无人机的俯仰角；Ψ表示无人机的偏航角；

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

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

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

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

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

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

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

即：

步骤3.2.2中，设n个无人机共在N个点进行信息数据采集，可得到N组测量数据，得到N个式(3)，整合成矩阵形式，得到：

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

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

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

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

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

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

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

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