[发明专利]室外无人机协同室内无人机进行三维定位的方法

权利要求书

1.一种室外无人机协同室内无人机进行三维定位的方法，其特征在于，包括如下：

(1)室外中心无人机中心GPS定位坐标X，根据无人机的尺寸计算出室外中心无人机机翼的GPS坐标P：P＝{p_k}，其中p_k表示室外中心无人机第k个机翼的GPS定位坐标，k＝1,2,...,L，L为室外无人机机翼的数量；

(2)利用超宽带UWB测距模块，通过信号到达时间或是接收信号强度测试每个机翼之间的距离，并将测得的距离信息d_ij发送给室外中心无人机，将无人机之间传输的数据发送到电脑终端，便于观测者掌握实时数据，其中，d_ij为第i个机翼与第j个机翼之间的距离，i,j＝1,2,...,N，N为无人机机翼总数量；

(3)室外中心无人机用接收的距离信息构建成距离矩阵D，根据距离矩阵D采用改进的多维尺度分析算法，计算室内室外无人机机翼相对坐标C；

(4)删除室内室外无人机机翼相对坐标C中室外无人机机翼相对坐标得到室内无人机机翼相对坐标C'；

(5)室外中心无人机采用最小二乘原则将室内无人机机翼相对坐标C'变换到绝对坐标Z中，得到室内第j个无人机机翼的中心绝对坐标Q_j，并利用高度补偿的方法将该坐标Q_j转化为室内第j个无人机中心GPS坐标Y_j。

2.根据权利要求1所述的方法，其中(3)中的距离矩阵D，表示如下：

其中，d_ii＝0，d_ij＝d_ji，i,j＝1,2,…,N，N为无人机机翼总数量。

3.根据权利1要求所述的方法，其中(3)采用多维尺度分析算法，计算室内室外无人机机翼的相对坐标C，实现如下：

(3a)计算相对坐标C的内积矩阵：B＝{b_ij}，b_ij为内积矩阵B中第i行第j列的元素，i,j＝1,2,…,N，b_ij通过如下公式计算：

其中，d_ij为距离矩阵D的第i行第j列元素

(3b)对内积矩阵B进行如下特征分解：

B＝VΛV^T，

其中，Λ＝diag(λ₁,λ₂,…,λ_i,…,λ_N)为内积矩阵B的特征值构成的对角矩阵，λ₁≥λ₂≥…≥λ_i≥…≥λ_N，i＝1,2,…,N，V为特征值对应的特征向量构成的特征向量矩阵，(·)^T表示矩阵的转置；

(3c)用内积矩阵B最大的前三个的特征值构成对角矩阵Λ′＝diag(λ₁,λ₂,λ₃)，并用这三个最大特征值λ₁,λ₂,λ₃对应的特征向量构成特征向量矩阵V'，得到室内室外无人机机翼的相对坐标C为：

4.根据权利要求1所述的方法，其中(5)中所述的室外中心无人机采用最小二乘原则将室内无人机机翼的相对坐标C'转化为绝对坐标Z中，其实现如下：

(5a)对室内无人机机翼相对坐标C'和室外中心无人机机翼GPS定位坐标P按下式进行去均值操作，得到去均值操作后的坐标矩阵：

其中，为室内无人机机翼相对坐标C'进行去均值操作后的坐标矩阵，为室外中心无人机机翼的GPS坐标P进行去均值操作后的坐标矩阵，M为室内无人机机翼数量，1_L＝[1,1,...,1]^T为用L个1组成的矩阵，1_M＝[1,1,...,1]^T为用M个1组成的矩阵，为相对坐标C'的平均值，c_i为室内无人机第i个机翼的相对坐标，为室外中心无人机机翼的GPS坐标的平均值，p_k为室外中心无人机第k个机翼的GPS坐标；

(5b)对(5a)中的两个坐标矩阵和进行相乘，并对相乘结果进行如下奇异值分解：

其中，U为左奇异矩阵，V为右奇异矩阵，(·)^T表示矩阵的转置，Σ＝diag(σ₁,σ₂,..σ_n.,..σ_M)为的奇异值构成的对角矩阵，σ₁≥σ₂≥..≥σ_n≥..≥σ_M，n＝1,2,...,M；

(5c)根据左奇异矩阵U和右奇异矩阵V计算旋转因子R:

R＝UV^T，

(5d)对室内无人机机翼相对坐标C'进行旋转，得到旋转后的室内无人机机翼坐标C”＝RC'，并计算该坐标C”的平均值μ_c”：

其中，c″_i为室内无人机第i个机翼的相对坐标旋转后的坐标；

(5e)根据室外中心无人机机翼的GPS坐标的均值和室内无人机机翼旋转后的坐标均值计算平移因子t：

t＝μ_p-μ_c”，

(5f)根据(5c)和(5e)的结果计算室内无人机机翼的绝对坐标：

Z＝RC'+1_M·t，

其中，1_M＝[1,1,...,1]^T为用M个1组成的列向量；

(5g)根据绝对坐标Z的第j个子矩阵中第i个行向量Z_ij，计算室内第j个无人机的机翼中心绝对坐标Q_j：

其中，W为室内第j个无人机机翼的数量；

(5h)根据机翼中心绝对坐标Q_j计算室内第j个无人机中心的GPS坐标：

其中，为室内第j个无人机中心GPS坐标Y_j的水平方向坐标，为室内第j个无人机机翼中心绝对坐标Q_j的水平方向坐标，为室内第j个无人机中心GPS坐标Y_j的高度坐标，为室内第j个无人机机翼中心绝对坐标Q_j的高度坐标，h为机翼与无人机中心的高度差。