CN112731500B - 室外无人机协同室内无人机进行三维定位的方法 - Google Patents

Abstract

本发明公开了一种室外无人机协同室内无人机进行三维定位的方法，主要解决现有方法在室内未安装特定装置下不能获得室内无人机GPS信息的问题。其方案是：室外中心无人机获得中心GPS坐标，计算其机翼GPS坐标；通过无人机安每个机翼安装的UWB测距模块测出机翼间距离，通过无人机安装的通信模块将距离信息发送给室外中心无人机和地面基站，室外中心无人机根据距离信息构建距离矩阵，获得室内外机翼相对坐标，并根据室外机翼GPS坐标将室内机翼相对坐标转为绝对坐标，获得室内无人机中心GPS坐标，确定出室内无人机位置。本发明能通过室外无人机协同工作确定出室内无人机的GPS坐标，可用于对普通室内环境下的室内无人机准确定位。

Description

室外无人机协同室内无人机进行三维定位的方法

技术领域

本发明属于无人机技术领域，具体涉及一种无人机室内定位方法，可用于普通室内环境下获得准确的室内无人机GPS坐标信息。

背景技术

现阶段，无人机技术的进步带来无人机的需求逐渐增加。无人机最初应用在军事领域，因其具有成本低、机动性能好、不易被发现的特点成为军方侦查、打击的新兴武器。无人机技术的不断普及，无人机进入到民众的生活中。在民用方面，无人机可以完成拍摄、快递运输、灾害检测等多种生活方面的任务，为民众提供了便捷，提高了人民的生活质量。但是，目前无人机的应用场景大多在户外，通过GPS技术获得无人机的位置信息。一旦无人机进入室内，便会失去无人机GPS定位信号，导致获取无人机的位置信息失败，无法对室内的无人机进行路线规划、任务分配，导致室内的无人机无法继续执行飞行任务到达指定地点。

目前，多采用室内构建三维坐标，每一个坐标轴的正向安装超宽带UWB基站，无人机安装UWB标签，测量基站与标签的距离信息，利用双曲线定位算法确定无人机室内位置同时利用摄像机观察室内，辅助确定室内位置。但是这种方法需要根据算法需求提前在室内架设UWB基站，如果无人机进入普通的室内环境，没有UWB基站和任何可以参考的相关信息，仅通过机载摄像机无法获取室内无人机准确的位置信息，就不能准确控制室内无人机的飞行位置，导致室内无人机无法继续执行后续的飞行任务。

发明内容

本发明的目的在于针对上述现有技术的不足，提出一种室外无人机协同室内无人机进行三维定位的方法，以融合GPS技术的室外无人机协同室内无人机进行三维定位的方法，实现无人机在普通室内环境下，准确获得室内无人机的GPS坐标信息。

本发明的技术方案是：对多维尺度分析MDS算法进行改进，通过改进后的算法计算室内无人机机翼的相对坐标，并采用最小二乘原则将该室内无人机机翼相对坐标变换到GPS的定位坐标，获得室内无人机中心的GPS坐标，其实现步骤包括如下：

(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。

本发明具有以下优点：

1.本发明由于在室内室外无人机上安装UWB测距模块和通信模块，用来测试每个机翼之间的距离，完成室内室外无人机之间数据传输，并通过室外中心无人机利用多维尺度算法和自身GPS坐标信息进行计算，得到室内无人机中心的GPS坐标，实现室外无人机协同室内无人机进行三维定位的功能。

2.本发明的室外中心无人机利用自身GPS信息和室内室外无人机传输的距离信息，就可计算室内无人机的GPS坐标，无需室内安装定位设备，解决普通室内环境下无法获取室内无人机位置信息的问题。

3.本发明通过将无人机之间传输的数据发送到基站，可使观测者在电脑终端掌握实时数据，及时了解实时情况。

附图说明

图1是本发明的应用场景俯视图；

图2是本发明的实现总流程图；

图3是本发明中坐标变换得到室内第j个无人机GPS坐标发送到地面基站子流程图。

具体实施方式

以下参照附图对本发明的实施例作进一步详细描述。

参照图1，本实施例的实现场景包括：室内环境和室外环境。室内环境包括两个房间和两个房间外同层走廊，其中在第一个房间中悬停有一架无人机，在走廊内悬停有一架无人机。室外环境包括第一个房间窗外悬停的中心无人机以及在地面架设的基站。中心无人机在第一个房间1的窗外工作，并与该房间内的无人机保持近似高度。对每个机翼间的距离进行测试并将该信息发送到基站，基站再将该信息数据传输给电脑终端，供观测者实时掌握数据和实时了解情况。

参照图2，本实施例的实现步骤如下：

步骤1，室外中心无人机获得中心GPS坐标，计算机翼GPS坐标。

室外中心无人机通过机载的GPS全球定位系统，获取自身的实时位置坐标X；

获取无人机中心与第k个机翼之间的相位θ_k、距离r_k、高度h这三个关系参数，并根据这三个关系参数和无人机中心GPS的坐标X，计算出中心无人机机翼的GPS坐标P：

P＝{p_k}，

其中，p_k为室外中心无人机第k个机翼的GPS坐标，k＝1,2,...,L，L为室外无人机机翼的数量，

为室外中心无人机第k个机翼的GPS坐标p_k的横坐标，/>

为室外中心无人机第k个机翼的GPS坐标p_k的纵坐标，/>

为室外中心无人机第k个机翼的GPS坐标p_k的高度坐标，X_x为无人机中心GPS的坐标X的横坐标，X_y为无人机中心GPS的坐标X的纵坐标，X_z为无人机中心GPS的坐标X的高度坐标。

步骤2，测试每个机翼之间的距离，并发送给中心无人机和地面基站。

2.1)利用UWB测距模块测试每个机翼之间的距离：

选择超宽带UWB测距模块安装在室外中心无人机与室内无人机每个机翼上，本实例选用但不限于UWB Mini3sPlus模块。该模块具有发送、接收无线电信号及通过无线电信号的传播时间或接收信号的强度计算每个机翼之间的距离的功能，其中：

通过无线电信号的传播时间的方法计算每个机翼之间的距离实现为：

令无线电信号传播所用的时间为τ，则通过该模块计算第i个机翼与第j个机翼之间的距离d_ij为：d_ij＝cτ，其中，c为光速；i,j＝1,2,…,N，N无人机机翼总数量；

通过接收信号的强度的方法计算每个机翼之间的距离实现如下：

令第j个机翼接收到的第i个机翼发送的无线电信号的信号强度为P_r(d_ij)，则第i个机翼与第j个机翼之间的距离d_ij可以通过下式计算：

其中，d₀为参考距离，取值为0.5米，P_r(d₀)为在参考距离d₀处获得的接收信号的强度，ζ_σ表示阴影效应带来的偏差，n是衰减因子，衰减因子的大小与环境有关，即在空旷的室外环境，n比较小，取值为3，而在室内环境，n比较大，取值为6；

2.2)将距离信息发送到中心无人机和地面基站：

给室内室外的每个无人机搭载通信模块，用于传输信息。本实例使用但不限于现有商用模块3DR数传电台模块，由该通信模块将通过无线电信号的传播时间或接收信号的强度计算得到的距离信息d_ij发送给中心无人机和地面基站。基站将接收到的数据传输到电脑终端，以供观测者掌握实时数据，及时了解实时情况。

步骤3，室外中心无人机通过每个机翼之间的距离关系计算室内外每个机翼的相对坐标。

现有的计算方法可采用经典的多维尺度分析MDS算法，但该MDS算法需要所有机翼两两之间存在链路连接，由于实际情况下很难满足，则需要采用现有经典MDS算法的改进算法，其包括MDS-MAP算法，MDS-MAP(P)算法，MDS-MAP(P,R)算法和S-MDS算法，本实例选用但不限于MDS-MAP(P)算法，具体实现如下：

3.1)构建距离矩阵D：

其中，d_ii＝0，d_ij＝d_ji，i,j＝1,2,…,N；

3.2)计算相对坐标C的内积矩阵：B＝{b_ij}，b_ij为内积矩阵B中第i行第j列的元素，b_ij通过如下公式计算：

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

B＝VΛV^T，

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

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

步骤4，进行坐标变换，获得室内第j个无人机中心GPS坐标并发送给地面基站。

室外中心无人机计算得到所有机翼的相对坐标后，需要进行坐标变换才能得到室内无人机机翼绝对坐标，为了使得到的绝对坐标尽可能准确，本实例采用最小二乘原则将室内无人机机翼相对坐标向绝对坐标作坐标变换，即变换完后的坐标与实际坐标之差的平方和最小。

室外中心无人机删除室内室外无人机机翼相对坐标C中室外无人机机翼相对坐标，得到室内无人机机翼相对坐标C'，采用最小二乘原则将室内无人机机翼的相对坐标C'转化为绝对坐标Z中，得到室内第j个无人机机翼的中心绝对坐标Q_j，并利用高度补偿的方法将该坐标Q_j转化为室内第j个无人机中心GPS坐标Y_j。无人机机翼数量依据无人机结构不同而不同，例如有4个机翼，6个机翼，8个机翼的不同无人机结构，本实例以4个但不限于4个机翼为实例。

参照图3本步骤具体实现如下：

4.1)获取室内无人机机翼相对坐标C'：

提取室内室外无人机机翼相对坐标C矩阵中前4×β行元素，将这些元素构成新的矩阵，该矩阵为室内无人机机翼相对坐标C'，其中β为室内无人机数量；

4.2)对室内无人机机翼相对坐标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坐标；

4.3)将(4.2)中的的两个坐标矩阵

和/>

进行相乘，并对相乘结果/>

进行如下奇异值分解：

其中，U为左奇异矩阵，V为右奇异矩阵，(·)^T表示矩阵的转置，Σ＝diag(σ₁,σ₂,..σ_n.,..σ_M)为

的奇异值构成的对角矩阵，σ₁≥σ₂≥..≥σ_n≥..≥σ_M，n＝1,2,...,M；

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

R＝UV^T，

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

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

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

t＝μ_p-μ_c”；

4.7)根据(4.4)和(4.6)的结果计算室内无人机机翼的绝对坐标：

Z＝RC'+1_M·t，

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

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

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

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

其中，

为室内第j个无人机中心GPS坐标Y_j的水平方向坐标，/>

为室内第j个无人机机翼中心绝对坐标Q_j的水平方向坐标，/>

为室内第j个无人机中心GPS坐标Y_j的高度坐标，/>

为室内第j个无人机机翼中心绝对坐标Q_j的高度坐标，h为机翼与无人机中心的高度差；

4.10)将室内第j个无人机中心的GPS坐标发送到地面基站：

室外中心无人机计算出室内第j个无人机中心的GPS坐标，利用通信模块将该信息发送到地面基站，基站将接收到的信息传输到电脑终端，观测者就得到室内第j个无人机中心的GPS坐标信息，确定室内第j个无人机具体位置。

仿真结果表明，上述实例根据每个机翼之间的距离信息，采用MDS-MAP(P)算法计算所有机翼的相对坐标，并将室内无人机机翼的相对坐标向绝对坐标作坐标变换，计算室内无人机机翼中心的绝对坐标，获得室内无人机中心GPS坐标。室内无人机、室外无人机、地面基站三者之间相互通信，实现室内室外无人机协同工作，可保证观测者在电脑终端记录实时数据，及时掌握实时情况，确定室内无人机的位置。与现有在室内环境安装UWB基站和无人机安装UWB标签实现无人机室内定位的方法相比较，该方法更具有普遍性，适用于更多的室内场景，实现对室内无人机的准确定位。

综上，本发明能够适用于更多更普遍室内场景实现无人机室内定位，通过室外无人机协同室内无人机进行三维定位提供了一种新的无人机室内定位方法。

Claims (4)

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为机翼与无人机中心的高度差。

Images