CN102564417A - 非接触式动态主动定位方法 - Google Patents

Abstract

本发明公开了一种非接触式动态主动定位方法，涉及卫星导航领域技术领域，根据在至少一个流动点对目标点测得定位数据计算所述目标点的坐标，所述定位数据包括：所述流动点的坐标、所述流动点到目标点的距离、所述流动点相对于目标点的俯仰角和方向角；或所述流动点的坐标、所述流动点到目标点的距离、所述流动点相对于目标点的俯仰角；或所述流动点的坐标、所述流动点到目标点的距离。本发明实现了对不可达的目标点坐标的精确定位。

Description

非接触式动态主动定位方法

技术领域

本发明涉及卫星导航领域技术领域，特别涉及一种非接触式动态主动定位方法。

背景技术

目前，对于非接触式的定位测量一般采用全站仪等在静态条件下定位，而动态条件下一般采用航空航天影像测量的方法。在地面非接触式立体测量一般采用三维扫描仪或者近景摄影测量的方式。对于海岛(礁)定位测量，由于许多海岛的地理条件限制，作业人员无法登岛，使得调绘、像控点布测与特征点定位测量非常困难，且存在很大危险性。

国内外高精度非接触式定位设备主要是无棱镜全站仪、超站仪以及3D激光扫描设备。虽然这些设备均具有远程定位功能，但综合考察结果，尤其是从海岛礁动态定位要求来权衡，发现：1)全站仪不能在动态条件下使用；2)超站仪静态条件下精度较高，使用方便，动态条件下可以作为RTK使用。但超站仪RTK并不支持船载动态条件下定位，无法对远距离目标观测进行非接触式测量；3.)3D激光扫描设备作业距离较短，一般只有500m左右。且不具备在船舶运载条件下动态定位能力。因此，在海岛礁等不易/不宜到达特殊条件下进行远距离动态定位是测绘目前正在努力寻求解决的难题。

发明内容

(一)要解决的技术问题

本发明要解决的技术问题是：如何对不可到达的目标实现精确地非接触式动态主动定位。

(二)技术方案

为解决上述计算问题，本发明提供了一种非接触式动态主动定位方法，根据在至少一个流动点对目标点测得的定位数据计算所述目标点的坐标，所述定位数据包括：

所述流动点的坐标、所述流动点到目标点的距离、所述流动点相对于目标点的俯仰角和方向角；或

所述流动点的坐标、所述流动点到目标点的距离、所述流动点相对于目标点的俯仰角；或

所述流动点的坐标、所述流动点到目标点的距离。

其中，获取流动点A的坐标(x，y，z)、所述流动点A到目标点的距离d、所述流动点A相对于目标点的俯仰角β和方向角α，采用以下公式计算所述目标点的坐标(X，Y，Z)：

$\left\{\begin{array}{c}X=x+\cos \mathrm{\β}\×\cos \mathrm{\α}\×d\\ Y=y+\cos \mathrm{\β}\×\sin \mathrm{\α}\×d\\ Z=z+\sin \mathrm{\β}\×d\end{array}\right.$

其中，分别获取流动点A和流动点B的坐标(X₁，Y₁，Z₁)和(X₂，Y₂，Z₂)、分别测得所述流动点A和流动点B到目标点的距离L₁和L₂、测得所述流动点A和流动点B分别相对于目标点的俯仰角α₁和α₂，采用以下公式计算所述目标点的坐标(X，Y，Z)：

$\left\{\begin{array}{c}\sqrt{{(X_1-X)}^2+{(Y_1-Y)}^2+{(Z_1-Z)}^2}=L_1\\ L_1\sin {\mathrm{\α}}_1={Z-Z}_1\\ \sqrt{{(X_2-X)}^2+{(Y_2-Y)}^2+{(Z_2-Z)}^2}=L_2\\ L_2\sin {\mathrm{\α}}_2={Z-Z}_2\end{array}\right.$

其中，分别获取在流动点A、流动点B和流动点C的坐标(X₁，Y₁，Z₁)、(X₂，Y₂，Z₂)和(X₃，Y₃，Z₃)、分别测得所述流动点A、流动点B和流动点C到目标点的距离L₁、L₂和L₃，根据A、B、C三点和目标点构成的四面体来计算所述目标点的坐标。

其中，所述根据A、B、C三点和目标点构成的四面体来计算所述目标点的坐标的步骤包括：

计算以所述目标点为顶点的四面体的高和平面ABC的法向量；

根据所述高和所述法向量的平行关系计算所述顶点在平面ABC上的投影点的坐标；

根据所述法向量的方向、高及投影点的坐标得到所述目标点的坐标。

其中，获取三个流动点以上的坐标，按以下方式计算目标点的坐标：

平差函数模型和随机模型，平差函数模型为：

$\left[\begin{array}{ccc}\frac{X_1-X_0}{L_1^0}& \frac{Y_1-Y_0}{L_1^0}& \frac{Z_1-Z_0}{L_1^0}\\ .& .& .\\ .& .& .\\ .& .& .\\ \frac{X_n-X_0}{L_n^0}& \frac{Y_n-Y_0}{L_n^0}& \frac{Z_n-Z_0}{L_n^0}\\ 0& 0& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{V}_X\\ V_Y\\ V_Z\end{array}\right]=\left[\begin{array}{c}{L}_1^0-L_1\\ .\\ .\\ .\\ L_n^0-L_n\\ Z_1+L_1\sin {\mathrm{\α}}_n-Z_0\\ .\\ .\\ .\\ Z_n+L_n\sin {\mathrm{\α}}_n-Z_0\end{array}\right]$

其中， $L_i=\sqrt{{(X_i-X_0)}^2+{(Y_i-Y_0)}^2+{(Z_i-Z_0)}^2},$

为流动点到目标点距离理论真值，V_X，V_Y，V_Z分别为X、Y、Z方向误差；

随机模型为：

测距观测配权： $P_{L_i}=L_i^2/{(p+q{L}_i)}^2;$

角度观测配权：

式中p为测距仪标定的加常数，q为测距仪标定的乘常数，ε为罗盘标定的倾角精度；

按以下非线性迭代的方法来进行求解，各流动点坐标分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z₂)，...，(X_n，Y_n，Z_n)，各流动点到目标点距离为L₁，L₂，...，L_n，α_i为第i个流动点到目标点的俯仰角α_i，(X₀，Y₀，Z₀)表示目标点坐标；

对于测距方程，其观测方程可表示为：

$\left\{\begin{array}{c}\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2+{(Z_1-Z_0)}^2}=L_1\\ \sqrt{{(X_2-X_0)}^2+{(Y_2-Y_0)}^2+{(Z_2-Z_0)}^2}=L_2\\ .\\ .\\ .\\ \sqrt{{(X_n-X_0)}^2+{(Y_n-Y_0)}^2+{(Z_n-Z_0)}^2}=L_n\end{array}\right.$

则误差方程为：

$\left\{\begin{array}{c}\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2+{(Z_1-Z_0)}^2}-L_1=V_1\\ \sqrt{{(X_2-X_0)}^2+{(Y_2-Y_0)}^2+{(Z_2-Z_0)}^2}-L_2=V_2\\ .\\ .\\ .\\ \sqrt{{(X_n-X_0)}^2+{(Y_n-Y_0)}^2+{(Z_n-Z_0)}^2}-L_n=V_n\end{array}\right.$

V_i为残差，按照最小二乘准则，则满足：

min表示取最小值，即得到目标函数：

其中，

利用求最值原理，构造迭代关系：

$\left\{\begin{array}{c}{X}^{k+1}=[(X_1-X^k)L_1/L_1^0+(X_2-X^k)L_2/L_2^0+\·\·\·+(X_n-X^k)L_n/L_n^0-(X_1+X_2+\·\·\·+X_n)]/n\\ Y^{k+1}=[(Y_1-Y^k)L_1/L_1^0+(Y_2-Y^k)L_2/L_2^0+\·\·\·+(Y_n-Y^k)L_n/L_n^0-(Y_1+Y_2+\·\·\·+Y_n)]/n\\ Z^{k+1}=[(Z_1-Z^k)L_1/L_1^0+(Z_2-Z^k)L_2/L_2^0+\·\·\·+(Z_n-Z^k)L_n/L_n^0-(Z_1+Z_2+\·\·\·+Z_n)]/n\end{array}\right.$

当点(X^k+1，Y^k+1，Z^k+1)和点(X^k，Y^k，Z^k)的距离满足||(X^k+1-X^k，Y^k+1-Y^k，Z^k+1-Z^k)||＜ε时停止迭代，其中，ε为停止迭代条件，将最终的(X^k+1，Y^k+1，Z^k+1)或(X^k，Y^k，Z^k)作为目标点的坐标。

其中，所述流动点的坐标采用差分定位或RTK技术测量。

(三)有益效果

通过本发明的方法实现了对不可到达的目标实现精确地非接触式动态主动定位。

附图说明

图1是本发明的一种非接触式动态主动定位方法中基于流动点测量的示意图；

图2是本发明的一种非接触式动态主动定位方法中采用三个流动点测量时计算目标点的示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

如图1所示，点P为不可达的待测点，A～E为不同的流动点，可以采用在至少一个流动点测得的定位数据(基线、流动点坐标等)来计算P点的坐标。下面通过不同的实施例进行说明。

实施例1

在流动点A利用GPS接收机/数字罗盘/激光测距单元综合采集器(实用新型专利号：ZL 201120022769.5，主动式动态定位仪)的激光测距仪测量流动点到目标点的距离d。此时采用GPS定位+距离+罗盘进行单点粗定位，具体方法如下：

根据一次测距信息加罗盘的方位角α和俯仰角β解算目标点的坐标(X，Y，Z)位置，先把观测点坐标(x，y，z)转到站心坐标系下，然后根据下式求得目标点的坐标。该方法主要用于定位初期目标点位置以及线性最小二乘解算初值。

$\left\{\begin{array}{c}X=x+\cos \mathrm{\β}\×\cos \mathrm{\α}\×d\\ Y=y+\cos \mathrm{\β}\×\sin \mathrm{\α}\×d\\ Z=z+\sin \mathrm{\β}\×d\end{array}\right.$

该实施例中由于采用一个流动点，测量结果不是很精确。

实施例2

本实施例利用两个流动点来测量P的坐标，可以采用GPS定位+两点距离+俯仰角进行两点定位，具体方法如下：

分别获取在流动点A和流动点B的坐标(X₁，Y₁，Z₁)和(X₂，Y₂，Z₂)、在所述流动点A和流动点B分别测得的目标点的基线长L₁和L₂、所述流动点A和流动点B分别相对于目标点的俯仰角α₁和α₂，采用以下公式计算所述目标点的坐标(X，Y，Z)：

$\left\{\begin{array}{c}\sqrt{{(X_1-X)}^2+{(Y_1-Y)}^2+{(Z_1-Z)}^2}=L_1\\ L_1\sin {\mathrm{\α}}_1={Z-Z}_1\\ \sqrt{{(X_2-X)}^2+{(Y_2-Y)}^2+{(Z_2-Z)}^2}=L_2\\ L_2\sin {\mathrm{\α}}_2={Z-Z}_2\end{array}\right.$

本实施例中，由于采用了两点定位的方式，因此不再需要方位角，而且测量的结果比实施例1更精确。

实施例3

本实施例利用三个流动点来测量P的坐标，有三条基线前提下，可采用三点解析法确定准确初值(因为实际操作时，都是3点以上定位，采用迭代求解的方法计算，先求出一个初值，然后迭代。同时GPS数据还有高精度后处理算法。利用后处理GPS结果和迭代算法得到的才是最终结果)。在流动点A(X₁，Y₁，Z₁)，B(X₂，Y₂，Z₂)，C(X₃，Y₃，Z₃)分别测得到目标点P的距离：PA、PB、PC。△ABC与点P组成四面体，如图2所示，D为P在△ABC上的垂足，PD是四面体的垂距。

此时要求P点坐标，只要求出D点坐标及P相对于D点的坐标增量即可。作DE⊥AB于E，DF⊥BC于F，连接PE，PF，设∠ABP＝∠1，∠PBD＝∠2，∠CBP＝∠3，因为PA，PB，PC已知，AB，BC可求，∠1、∠3也可用余弦定理求出。所以只要求出∠2的正弦值即可求出PD。由此可知：

cos∠1＝cos∠2×cos∠DBE

cos∠3＝cos∠2×cos∠DBF

$\sin \∠\mathrm{DBE}=\sqrt{1-{\cos }^2\∠\mathrm{DBE}}$

$\sin \∠\mathrm{DBF}=\sqrt{1-{\cos }^2\∠\mathrm{DBF}}$

∠DBE+∠DBF＝∠ABC

cos(∠DBE+∠DBF)＝cos∠DBE×cos∠DBF-sin∠DBE×sin∠DBF

将上面5个式子代入最后一个公式得：

$\cos \∠\mathrm{ABC}=\frac{\cos \∠1}{\cos \∠2}\×\frac{\cos \∠3}{\cos \∠2}-\sqrt{1-\frac{{\cos }^2\∠3}{{\cos }^2\∠2}}\×\sqrt{1-\frac{{\cos }^2\∠1}{{\cos }^2\∠2}}$

因为

$\cos \∠\mathrm{ABC}=\frac{{\left|\stackrel{\→}{\mathrm{AB}}\right|}^2+{\left|\stackrel{\→}{\mathrm{BC}}\right|}^2+{\left|\stackrel{\→}{\mathrm{AC}}\right|}^2}{2\×\left|\stackrel{\→}{\mathrm{AB}}\right|\×\left|\stackrel{\→}{\mathrm{BC}}\right|}$

所以

$\cos \∠2=\sqrt{\frac{{\cos }^2\∠1+{\cos }^2\∠3-2\×\cos \∠\mathrm{ABC}\×\cos \∠1\×\cos \∠3}{1-{\cos }^2\∠\mathrm{ABC}}}$

$\sin \∠2=\sqrt{{1-\cos }^2\∠2}=\sqrt{\frac{{1-\cos }^2\∠1-{\cos }^2\∠3-{\cos }^2\∠\mathrm{ABC}+2\×\cos \∠1\×\cos \∠3\×\cos \∠\mathrm{ABC}}{1-{\cos }^2\∠\mathrm{ABC}}}$

因为∠1、∠3和∠ABC均可求，解得所以PD：

h＝PD＝PB×sin∠2

在求取完高PD后，求解D点坐标：由PD与平面ABC的法向量平行，可利用它们的平行关系计算D点坐标。设L为平面ABC的法向量，则有L就等于△ABC中任意两条相交向量的乘积，取 $L=\stackrel{\→}{\mathrm{AB}}\×\stackrel{\→}{\mathrm{BC}}$ 有：

$L=\left|\left(\begin{array}{ccc}i& j& k\\ X_2-X_1& Y_2-Y_1& Z_2-Z_1\\ X_3-X_2& Y_3-Y_2& Z_3-Z_2\end{array}\right)\right|$

a＝(Y₂-Y₁)(Z₃-Z₂)-(Z₂-Z₁)(Y₃-Y₂)

设：b＝(Z₂-Z₁)(X₃-X₂)-(X₂-X₁)(Z₃-Z₂)

c＝(X₂-X₁)(Y₃-Y₂)-(Y₂-Y₁)(X₃-X₂)

则有L＝ai+bj+ck，则法向量L的单位向量为：

${\mathrm{\α}}_x=\frac{a}{\sqrt{a^2+b^2+c^2}}$

${\mathrm{\β}}_y=\frac{b}{\sqrt{a^2+b^2+c^2}}$

${\mathrm{\γ}}_z=\frac{c}{\sqrt{a^2+b^2+c^2}}$

则P相对于D点坐标增量为：

r₁＝h×α_x

r₂＝h×β_y

r₃＝h×γ_z

设D点坐标为x₀，y₀，z₀，根据向量余弦公式与余弦定理可得：

$\cos \∠\mathrm{BAD}=\frac{\stackrel{\→}{\mathrm{AB}}\·\stackrel{\→}{\mathrm{AD}}}{\left|\stackrel{\→}{\mathrm{AB}}\right|\×\left|\stackrel{\→}{\mathrm{AD}}\right|}=\frac{{\left|\stackrel{\→}{\mathrm{AB}}\right|}^2+{\left|\stackrel{\→}{\mathrm{AD}}\right|}^2-{\left|\stackrel{\→}{\mathrm{BD}}\right|}^2}{2\×\left|\stackrel{\→}{\mathrm{AB}}\right|\×\left|\stackrel{\→}{\mathrm{AD}}\right|}---\left(1\right)$

$\cos \∠\mathrm{CBD}=\frac{\stackrel{\→}{\mathrm{BC}}\·\stackrel{\→}{\mathrm{BD}}}{\left|\stackrel{\→}{\mathrm{BC}}\right|\×\left|\stackrel{\→}{\mathrm{BD}}\right|}=\frac{{\left|\stackrel{\→}{\mathrm{BC}}\right|}^2+{\left|\stackrel{\→}{\mathrm{BD}}\right|}^2-{\left|\stackrel{\→}{\mathrm{CD}}\right|}^2}{2\×\left|\stackrel{\→}{\mathrm{BC}}\right|\×\left|\stackrel{\→}{\mathrm{BD}}\right|}---\left(2\right)$

同时，平面上任意一条向量与法向量L点乘积为0，取

$\stackrel{\→}{\mathrm{CD}}\·L=0---\left(3\right)$

联立以上(1)、(2)、(3)式，即可组成关于x₀，y₀，z₀的三元一次方程，矩阵形式为：

EX＝F

其中，E、F分别为矩阵形式的系数，X＝[x₀，y₀，z₀]^T。矩阵求解可得D点坐标(x₀，y₀，z₀)。此时在平面ABC上下个有一个解，若P点坐标在平面ABC上方，则取正解(a)，若P点在面ABC下方，则取负解(b)，如下：

X＝x₀+r₁        X＝x₀-r₁

Y＝y₀+r₂        Y＝y₀-r₂

Z＝z₀+r₃        Z＝z₀-r₃

(a)             (b)

上面的两个解需要根据实际情况进行排除。可通过单个点的距离和角度信息给出目标点的大略坐标，在计算前给出，从而确定二个值中哪一个是正确解。

实施例4

三个流动点以上情况，采用多点距离+罗盘融合解，测得第i个流动点的坐标(X_i，Y_i，Z_i)，第i个流动点到目标点的距离L_i及俯仰角α_i，目标点坐标(X₀，Y₀，Z₀)可采用以下两个模型求解。下面给出平差函数模型和随机模型。平差函数模型为：

$\left[\begin{array}{ccc}\frac{X_1-X_0}{L_1^0}& \frac{Y_1-Y_0}{L_1^0}& \frac{Z_1-Z_0}{L_1^0}\\ .& .& .\\ .& .& .\\ .& .& .\\ \frac{X_n-X_0}{L_n^0}& \frac{Y_n-Y_0}{L_n^0}& \frac{Z_n-Z_0}{L_n^0}\\ 0& 0& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{V}_X\\ V_Y\\ V_Z\end{array}\right]=\left[\begin{array}{c}{L}_1^0-L_1\\ .\\ .\\ .\\ L_n^0-L_n\\ Z_1+L_1\sin {\mathrm{\α}}_n-Z_0\\ .\\ .\\ .\\ Z_n+L_n\sin {\mathrm{\α}}_n-Z_0\end{array}\right]$

其中， $L_i=\sqrt{{(X_i-X_0)}^2+{(Y_i-Y_0)}^2+{(Z_i-Z_0)}^2},$

为流动点到目标点距离理论真值，V_X，V_Y，V_Z分别为X、Y、Z方向误差。

随机模型为：

测距观测配权： $P_{L_i}=L_i^2/{(p+q{L}_i)}^2;$

角度观测配权：

式中p为测距仪标定的加常数(单位：米)，q为测距仪标定的乘常数(无量刚)，ε为罗盘标定的倾角精度(单位：度)。

多流动点距离交会时，线性化最小二乘迭代算法在某些观测结构较差的情况下，若不收敛，则无法求出目标点的坐标，在这种情况下可按以下非线性迭代的方法来进行求解。各流动点坐标分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z₂)，...，(X_n，Y_n，Z_n)，各流动点到目标点距离为L₁，L₂，...，L_n。

对于测距方程，其观测方程可表示为：

$\left\{\begin{array}{c}\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2+{(Z_1-Z_0)}^2}=L_1\\ \sqrt{{(X_2-X_0)}^2+{(Y_2-Y_0)}^2+{(Z_2-Z_0)}^2}=L_2\\ .\\ .\\ .\\ \sqrt{{(X_n-X_0)}^2+{(Y_n-Y_0)}^2+{(Z_n-Z_0)}^2}=L_n\end{array}\right.$

则误差方程为：

$\left\{\begin{array}{c}\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2+{(Z_1-Z_0)}^2}-L_1=V_1\\ \sqrt{{(X_2-X_0)}^2+{(Y_2-Y_0)}^2+{(Z_2-Z_0)}^2}-L_2=V_2\\ .\\ .\\ .\\ \sqrt{{(X_n-X_0)}^2+{(Y_n-Y_0)}^2+{(Z_n-Z_0)}^2}-L_n=V_n\end{array}\right.$

V_i为残差，按照加权最小二乘准则，则满足：

min表示取最小值，即得到目标函数：

其中，

利用求最值原理，构造迭代关系：

$\left\{\begin{array}{c}{X}^{k+1}=[(X_1-X^k)L_1/L_1^0+(X_2-X^k)L_2/L_2^0+\·\·\·+(X_n-X^k)L_n/L_n^0-(X_1+X_2+\·\·\·+X_n)]/n\\ Y^{k+1}=[(Y_1-Y^k)L_1/L_1^0+(Y_2-Y^k)L_2/L_2^0+\·\·\·+(Y_n-Y^k)L_n/L_n^0-(Y_1+Y_2+\·\·\·+Y_n)]/n\\ Z^{k+1}=[(Z_1-Z^k)L_1/L_1^0+(Z_2-Z^k)L_2/L_2^0+\·\·\·+(Z_n-Z^k)L_n/L_n^0-(Z_1+Z_2+\·\·\·+Z_n)]/n\end{array}\right.$

当点(X^k+1，Y^k+1，Z^k+1)和点(X^k，Y^k，Z^k)的距离满足||(X^k+1-X^k，Y^k+1-Y^k，Z^k+1-Z^k)||＜ε时停止迭代。其中，ε为停止迭代条件，通常为一自定义微小量。将最终的(X^k+1，Y^k+1，Z^k+1)或(X^k，Y^k，Z^k)作为目标点的坐标。

利用以上公式，即可求得多流动站(3点以上)目标点远程非接触式主动定位。海岛(礁)现场试验证明，在3条基线以上定位结果精度最高，其N、E、U三个方向定位精度均在0.5m以内。

实施例3和实施例4中，由于采用了三点或三个以上点定位的方式，测量的结果比实施例1和实施例2更精确。

上述实施例中采用差分定位或RTK技术测量流动点的坐标，流动点的卫星定位精度优于10cm，三维数字罗盘姿态测量精度一般为1/100(0.5°)。由误差分析方法可知，当流动点与目标之间的平均距离(下称作用距离)为400m时，仅姿态误差就可导致目标定位误差超过1.0m。此时，即使基线精度优于10cm，目标的定位误差也很难优于1.0m。因此，要提高目标主动式动态定位精度，必须提高姿态测量精度和基线精度。可以通过在30°～120°范围内几条基线前方交会方法，弱化姿态测量精度的影响；采用基线多次重复测量方法提高基线精度，从而在扩大作用距离(最大作用距离6km)的同时，使得非接触式动态主动定位精度达到1.0m以内。

GPS激光测距海岛(礁)非接触动态主动定位时，可在附近40km范围内设GPS基准站(保证卫星动态定位精度优于10cm)。作业过程中，GPS基准站和流动点之间无需通信、相对独立。GPS流动点的激光触发时刻及流动点定位在数据后处理中可用GPS时精密同步。

以上实施方式仅用于说明本发明，而并非对本发明的限制，有关技术领域的普通技术人员，在不脱离本发明的精神和范围的情况下，还可以做出各种变化和变型，因此所有等同的技术方案也属于本发明的范畴，本发明的专利保护范围应由权利要求限定。

Claims (7)

1.一种非接触式动态主动定位方法，其特征在于，根据在至少一个流动点对目标点测得的定位数据计算所述目标点的坐标，所述定位数据包括：

所述流动点的坐标、所述流动点到目标点的距离、所述流动点相对于目标点的俯仰角和方向角；或

所述流动点的坐标、所述流动点到目标点的距离、所述流动点相对于目标点的俯仰角；或

所述流动点的坐标、所述流动点到目标点的距离。

2.如权利要求1所述的非接触式动态主动定位方法，其特征在于，获取流动点A的坐标(x，y，z)、所述流动点A到目标点的距离d、所述流动点A相对于目标点的俯仰角β和方向角α，采用以下公式计算所述目标点的坐标(X，Y，Z)：

$\left\{\begin{array}{c}X=x+\cos \mathrm{\β}\×\cos \mathrm{\α}\×d\\ Y=y+\cos \mathrm{\β}\×\sin \mathrm{\α}\×d\\ Z=z+\sin \mathrm{\β}\×d\end{array}\right.$

3.如权利要求1所述的非接触式动态主动定位方法，其特征在于，分别获取流动点A和流动点B的坐标(X₁，Y₁，Z₁)和(X₂，Y₂，Z₂)、分别测得所述流动点A和流动点B到目标点的距离L₁和L₂、测得所述流动点A和流动点B分别相对于目标点的俯仰角α₁和α₂，采用以下公式计算所述目标点的坐标(X，Y，Z)：

$\left\{\begin{array}{c}\sqrt{{(X_1-X)}^2+{(Y_1-Y)}^2+{(Z_1-Z)}^2}=L_1\\ L_1\sin {\mathrm{\α}}_1={Z-Z}_1\\ \sqrt{{(X_2-X)}^2+{(Y_2-Y)}^2+{(Z_2-Z)}^2}=L_2\\ L_2\sin {\mathrm{\α}}_2={Z-Z}_2\end{array}\right.$

4.如权利要求1所述的非接触式动态主动定位方法，其特征在于，分别获取在流动点A、流动点B和流动点C的坐标(X₁，Y₁，Z₁)、(X₂，Y₂，Z₂)和(X₃，Y₃，Z₃)、分别测得所述流动点A、流动点B和流动点C到目标点的距离L₁、L₂和L₃，根据A、B、C三点和目标点构成的四面体来计算所述目标点的坐标。

5.如权利要求4所述的非接触式动态主动定位方法，其特征在于，所述根据A、B、C三点和目标点构成的四面体来计算所述目标点的坐标的步骤包括：

计算以所述目标点为顶点的四面体的高和平面ABC的法向量；

根据所述高和所述法向量的平行关系计算所述顶点在平面ABC上的投影点的坐标；

根据所述法向量的方向、高及投影点的坐标得到所述目标点的坐标。

6.如权利要求1所述的非接触式动态主动定位方法，其特征在于，获取三个流动点以上的坐标，按以下方式计算目标点的坐标：

平差函数模型和随机模型，平差函数模型为：

$\left[\begin{array}{ccc}\frac{X_1-X_0}{L_1^0}& \frac{Y_1-Y_0}{L_1^0}& \frac{Z_1-Z_0}{L_1^0}\\ .& .& .\\ .& .& .\\ .& .& .\\ \frac{X_n-X_0}{L_n^0}& \frac{Y_n-Y_0}{L_n^0}& \frac{Z_n-Z_0}{L_n^0}\\ 0& 0& 1\\ .& .& .\\ .& .& .\\ .& .& .\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}{V}_X\\ V_Y\\ V_Z\end{array}\right]=\left[\begin{array}{c}{L}_1^0-L_1\\ .\\ .\\ .\\ L_n^0-L_n\\ Z_1+L_1\sin {\mathrm{\α}}_n-Z_0\\ .\\ .\\ .\\ Z_n+L_n\sin {\mathrm{\α}}_n-Z_0\end{array}\right]$

其中， $L_i=\sqrt{{(X_i-X_0)}^2+{(Y_i-Y_0)}^2+{(Z_i-Z_0)}^2},$

为流动点到目标点距离理论真值，V_X，V_Y，V_Z分别为X、Y、Z方向误差；

随机模型为：

测距观测配权： $P_{L_i}=L_i^2/{(p+q{L}_i)}^2;$

角度观测配权：

式中p为测距仪标定的加常数，q为测距仪标定的乘常数，ε为罗盘标定的倾角精度；

按以下非线性迭代的方法来进行求解，各流动点坐标分别为(X₁，Y₁，Z₁)，(X₂，Y₂，Z₂)，...，(X_n，Y_n，Z_n)，各流动点到目标点距离为L₁，L₂，...，L_n，α_i为第i个流动点到目标点的俯仰角α_i，(X₀，Y₀，Z₀)表示目标点坐标；

对于测距方程，其观测方程可表示为：

$\left\{\begin{array}{c}\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2+{(Z_1-Z_0)}^2}=L_1\\ \sqrt{{(X_2-X_0)}^2+{(Y_2-Y_0)}^2+{(Z_2-Z_0)}^2}=L_2\\ .\\ .\\ .\\ \sqrt{{(X_n-X_0)}^2+{(Y_n-Y_0)}^2+{(Z_n-Z_0)}^2}=L_n\end{array}\right.$

则误差方程为：

$\left\{\begin{array}{c}\sqrt{{(X_1-X_0)}^2+{(Y_1-Y_0)}^2+{(Z_1-Z_0)}^2}-L_1=V_1\\ \sqrt{{(X_2-X_0)}^2+{(Y_2-Y_0)}^2+{(Z_2-Z_0)}^2}-L_2=V_2\\ .\\ .\\ .\\ \sqrt{{(X_n-X_0)}^2+{(Y_n-Y_0)}^2+{(Z_n-Z_0)}^2}-L_n=V_n\end{array}\right.$

V_i为残差，按照最小二乘准则，则满足：

min表示取最小值，即得到目标函数：

其中，

利用求最值原理，构造迭代关系：

$\left\{\begin{array}{c}{X}^{k+1}=[(X_1-X^k)L_1/L_1^0+(X_2-X^k)L_2/L_2^0+\·\·\·+(X_n-X^k)L_n/L_n^0-(X_1+X_2+\·\·\·+X_n)]/n\\ Y^{k+1}=[(Y_1-Y^k)L_1/L_1^0+(Y_2-Y^k)L_2/L_2^0+\·\·\·+(Y_n-Y^k)L_n/L_n^0-(Y_1+Y_2+\·\·\·+Y_n)]/n\\ Z^{k+1}=[(Z_1-Z^k)L_1/L_1^0+(Z_2-Z^k)L_2/L_2^0+\·\·\·+(Z_n-Z^k)L_n/L_n^0-(Z_1+Z_2+\·\·\·+Z_n)]/n\end{array}\right.$

当点(X^k+1，Y^k+1，Z^k+1)和点(X^k，Y^k，Z^k)的距离满足||(X^k+1-X^k，Y^k+1-Y^k，Z^k+1-Z^k)||＜ε时停止迭代，其中，ε为停止迭代条件，将最终的(X^k+1，Y^k+1，Z^k+1)或(X^k，Y^k，Z^k)作为目标点的坐标。

7.如权利要求1～6中任一项所述的非接触式动态主动定位方法，其特征在于，所述流动点的坐标采用差分定位或RTK技术测量。

Images