CN105260610B - 一种多探测器坐标系转化及误差纠正方法 - Google Patents

Abstract

本发明涉及多探测器坐标系转换领域，尤其是一种多探测器坐标系转化及误差纠正方法。本发明针对现有技术存在的问题，提供一种多探测器坐标系转化及误差纠正方法，本发明针对FOD探测***中主要依靠的雷达、图像和激光三种探测技术提出了探测器自身坐标与跑道全局坐标系的相互转换数学模型，并提出了计算转换模型参数计算的方法。在此基础上，针对转换结果的误差，提出了一种误差纠正方法，进一步提高了坐标转换的精度。本发明使用探测器测得测试点的探测器坐标系下对应位置信息；根据对应关系求解坐标转换模型中的参数；并在探测中获取目标坐标后，计算出多种探测器的探测坐标与跑道标准坐标进行相互转换的简化公式。

Description

一种多探测器坐标系转化及误差纠正方法

技术领域

本发明涉及多探测器坐标系转换领域，尤其是一种多探测器坐标系转化及误差纠正方法。

背景技术

固定式FOD探测***的核心设备是在跑道两侧安装的若干探测单元。探测单元可以是雷达、图像和激光中的一种或几种设备。探测单元发现FOD目标后，在缺乏全局信息的情况下只能提供自身坐标系下的目标坐标。但是该坐标难以被直观理解，也难以被其他设备使用，需要转换到可以被直观理解的跑道坐标系。一个准确的坐标系转化方法，是FOD探测整体方法中关键的一步，是多个探测单元坐标相互转换的标准和桥梁，也是多探测器数据融合探测的基础。

一些***直接测量探测单元的位置，简单地对探测器自身坐标进行位移得到跑道坐标。这样的方法易于操作，但是结果往往存在比较大的转化误差，甚至超过探测器的探测误差。科学的做法是，首先建立坐标系转换的数学模型，然后求解数学模型中的参数，最后根据计算所得的参数对坐标进行转换。简单的转换模型易于计算参数，但是转换结果误差较大；反之，复杂的转换模型转换结果误差较小，但是难以计算参数。优秀的转换方法能够用比较方便的途径计算出转换模型的参数，并且转换结果准确度高。

发明内容

本发明所要解决的技术问题是：针对上述存在的问题，提供一种多探测器坐标系转化及误差纠正方法，本发明针对FOD探测***中主要依靠的雷达、图像和激光三种探测技术提出了探测器自身坐标与跑道全局坐标系的相互转换数学模型，并提出了计算转换模型参数计算的方法。在此基础上，针对转换结果的误差，提出了一种误差纠正方法，进一步提高了坐标转换的精度。

本发明采用的技术方案如下：

一种多探测器坐标系转化方法包括：

步骤1：提出了一种通用的探测坐标系与跑道坐标系的转换原理模型，探测目标的在探测器极坐标表达形式为(θ,d,δ)，其中d表示探测器到被探测物体距离，θ表示被探测物体与探测器之间的方位角，δ表示被探测物体与探测器之间的俯仰角；探测器笛卡尔坐标系下表达形式为P_T(x_T,y_T,z_T)：

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

将P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)的坐标转换模型：

P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为探测器坐标系原点在跑道坐标系中的位置；该模型中的未知参数为R₀、P₀，而探测器根据类型的不同获取的坐标信息为三个分量的全部或其中两个。

步骤2：在跑道路面上，均匀设置N₁个被探测物体，即为N₁个测试点，使用测量仪测得其在跑道坐标系下坐标分别为(x_j,y_j,z_j)；同时使用探测器测得测试点的探测器坐标系下对应位置信息；根据对应关系求解步骤1坐标转换模型中的参数；

步骤3：在探测中获取目标坐标后，使用步骤2中坐标转换公式模型中的参数对步骤1坐标转换模型进行化简，计算出多种探测器的探测坐标与跑道标准坐标进行相互转换的简化公式。

进一步的，当所述探测器是雷达时，测试点经雷达探测所得雷达坐标系中坐标分布为(θ_j,d_j),1≤j≤N₁，步骤1中坐标转换模型未知参数为δ、R₀、P₀；即当实际测量中δ_j、R₀、P₀为未知数；其中δ_j为N1个测试点对应的被探测物体与雷达之间的俯仰角；θ_j为N1个测试点对应的被测物体与雷达之间的方位角；d_j为N1个测试点对应的被测物体与雷达之间的距离；步骤2中求解坐标转换模型中的参数具体包括：

步骤21：构造

根据计算得到P₀＝(A^TA)^-1A^TB，得到P₀；

步骤22：因为R₀ ^-1(P_G-P₀)＝P_T，记(x_j-x₀,y_j-y₀,z_j-z₀)为(vx_j,vy_j,vz_j)，记R₀ ^-1为

构造

并对temp^Ttemp进行特征值分解或者奇异值分解，取最小非零特征值对应的特征向量，并将该向量记为(r₁₁',r₁₂',r₁₃',r₂₂',r₂₃')，执行步骤23；

步骤23：计算：

得到R₀ ^-1中所有参数的值，执行步骤24：

步骤24：对R₀ ^-1求逆矩阵，得到R₀的值；

步骤25：对于任意跑道平面上目标均有z_G≈0，且

即r₁₁(x_G-x₀)+r₁₂(y_G-y₀)+r₁₃(-z₀)＝x_T，r₁₂(x_G-x₀)+r₂₂(y_G-y₀)+r₂₃(-z₀)＝y_T；

且有x_G ²+y_G ²＝d²-z₀ ²

根据以上两个限制条件，令：

算出y_G为二元一次方程(1+k₁ ²)y_G ²+2k₁k₂y_G+k₂ ²-d²+z₀ ²＝0的正解，而x_G＝k₁y_G+k₂

从而在无需知道参数δ的情况下实现雷达坐标向跑道坐标的转换。

进一步的，当所述探测器是雷达时，步骤3分别通过步骤31或步骤32具体实现：

步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其雷达探测坐标(θ,d)计算方法为：

θ＝ar tan(y_T/x_T)

步骤32：给定目标雷达探测坐标(θ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)的计算方法为：令

得到y_G为二元一次方程：(1+k₁ ²)y_G ²+2k₁k₂y_G+k₂ ²-d²+z₀ ²＝0的正解，而x_G＝k₁y_G+k₂，z_G＝0。

进一步的，当所述探测器是图像探测器时，步骤1中坐标转换模型未知参数为d、R₀、P₀；图像坐标系下，测试点经过相机探测所得图像坐标系中坐标分布为(θ_j,δ_j),1≤j≤N₁；即实际测量中d_j、R₀、P₀为未知数；其中δ_j为N1个测试点对应的被探测物体与图像探测器之间的俯仰角；θ_j为N1个测试点对应的被测物体与图像探测器之间的方位角；d_j为N1个测试点对应的被测物体与图像探测器之间的距离；步骤2中求解坐标转换模型中的参数具体包括：

经图像探测所得图像坐标系中坐标分布为(θ_j,δ_j),1≤j≤N₁，测量探测器在跑道坐标系下坐标为在P₀’为中心σ为半径，2σ×2σ×2σ的正方体空间内，取均匀分布间距为τ的点P₀'＝(x₀',y₀',z₀')作为P₀估算值， σ的取值为0.1米到5米，τ的取值为到对所有的P₀’执行以下计算：

步骤21：对每一个测试点计算V_j＝(x_j-x₀',y_j-y₀',z_j-z₀')，并将向量V_j缩放为单位向量(除以自身的模)

步骤22：对每一个测试点计算W_j＝(cosδ_jcosθ_j,cosδ_jsinθ_j,sinδ_j)；

步骤23：计算向量K_j为Wj和Vj的叉乘K_j＝W_j×V_j，记为K_j＝(kx_j,ky_j,kz_j)，并计算ψ_j＝ar sin(kz)，ζ_j＝ar tan(ky_j/kx_j)；

步骤24：计算向量Wj和Vj之间的夹角

步骤25：对所有的测试点计算ψ_j，ζ_j，数值分布的标准样本差的和ε，表示计算从x₁到x_N1共N1个数值分布的标准样本差。

在所有的P₀’中，选取ε值最小的一组，令

P₀＝P₀'

得到R₀、P₀，其中表示计算从x₁到x_N1共N1个数值的平均值；

步骤26：令由P_G＝R₀P_T+P₀可知跑道面上目标满足关系t₁₃dcosδcosθ+t₂₃dcosδsinθ+t₃₃dsinδ＝-z₀；

反向计算出参数d为：

进一步的，当所述探测单元是图像探测器时，步骤3具体包括：

步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其图像探测坐标(θ,δ)计算方法为

θ＝ar tan(y_T/x_T)

给定图像探测目标坐标(θ,δ)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

根据得到P_G。

进一步的，当所述探测单元是激光扫描器时，步骤1中坐标转换模型未知参数为R₀、P₀；经激光探测被探测物体，测试点在激光坐标系中坐标分布为(θ_j,δ_j,d_j),1≤j≤N₁；其中δ_j为N1个测试点对应的被探测物体与探测器之间的俯仰角；θ_j为N1个测试点对应的被测物体与探测器之间的方位角；d_j为N1个测试点对应的被测物体与探测器之间的距离；则步骤2中求解坐标转换模型中参数具体包括：

步骤21：构造

根据计算得到P₀＝(A^TA)^-1A^TB，得到P₀；

步骤22：对每一个测试点计算V_j＝(x_j-x₀',y_j-y₀',z_j-z₀')，并将向量V_j缩放为单位向量(除以自身的模)

步骤23：对每一个测试点计算W_j＝(cosδ_jcosθ_j,cosδ_jsinθ_j,sinδ_j)；

步骤24：对所有的V_j和W_j计算平均向量

步骤23：计算向量K为和的叉乘并将K缩放为单位向量

记为K＝(kx,ky,kz)，并计算向量和之间的夹角

步骤24：构造

计算

得到模型参数中的R₀。

进一步的，当所述探测单元是激光时，步骤3具体为：

步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其激光探测坐标(θ,δ,d)计算方法为：

θ＝ar tan(y_T/x_T)

步骤32:给定激光探测目标坐标(θ,δ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

一种多探测器坐标系转化的误差纠正方法，当计算出测试点在跑道坐标系的坐标为P_c(cx,cy,cz)，而实际跑道坐标系的坐标为P_g(gx,gy,gz)，在X轴Y轴方向上的误差分别为Δx＝gx-cx，Δy＝gy-cy，时；则误差与坐标的关系为Δx＝f₁(cx,cy)，Δy＝f₂(cx,cy)，f₁、f₂、f₃、f₄均为平滑曲面函数，包含二元一次曲面、二元二次曲面、二元三次曲面、三角函数、高斯曲面。

当所述Δx＝f₁(cx,cy)函数为二元二次曲面时，记N₁个测试点计算出跑道坐标系的坐标分别为P_c-j(cx_j,cy_j,cz_j),1≤j≤N₁，而实际坐标为P_g-j(gx_j,gy_j,gz_j),1≤j≤N₁，该点X轴上的误差为Δx_j＝gx_j-cx_j。构造矩阵:

令E_X＝(A^TA)^-1A^TB_X，E_Y＝(A^TA)^-1A^TB_Y；

并记E＝(e₁,e₂,e₃,e₄,e₅,e₆)；H＝(h₁,h₂,h₃,h₄,h₅,h₆)

则对于计算得点(cx,cy)的经过误差纠正后的值应为：

cx＝cx+e₁cx²+e₂cxcy+e₃cy³+e₄cx+e₅cy+e₆；

cy＝cy+h₁cx²+h₂cxcy+h₃cy³+h₄cx+h₅cy+h₆。

综上所述，由于采用了上述技术方案，本发明的有益效果是：

1、提出了一种多探测器坐标与跑道坐标互相转换的方法，方法使用了复杂的三维模型，并提出了在已知信息不同情况下该三维模型的不同参数求解与化简方法，相比普通近似模型能够更小误差地实现探测器与跑道坐标相互转换；

2、以跑道坐标为桥梁，以简化的坐标转换公式为方法，可以快速实现多个探测器间坐标的转换；

3、基于测试点的误差分析及纠正方法能够定量计算坐标转换误差，并在此基础上进一步减小转换误差。

具体实施方式

本说明书中公开的所有特征，或公开的所有方法或过程中的步骤，除了互相排斥的特征和/或步骤以外，均可以以任何方式组合。

本说明书(包括任何附加权利要求、摘要)中公开的任一特征，除非特别叙述，均可被其他等效或具有类似目的的替代特征加以替换。即，除非特别叙述，每个特征只是一系列等效或类似特征中的一个例子而已。

本发明相关说明：

1、跑道坐标系指的是以根据机场标准以跑道布局结构定义的坐标系，与探测器的安装位置和朝向无关。

2、探测器坐标系指的是雷达坐标系、图像坐标系或者激光坐标系，其中雷达坐标系指的是以雷达为探测器，标记被探测物***置信息的坐标系；图像坐标系指的是以图像探测器为探测器，标记被探测物***置信息的坐标系；激光坐标系指的是以激光扫描器为探测器，标记被探测物***置信息的坐标系。其坐标系原点及坐标轴因安装位置和朝向不同而发生变化，且无法依靠测量工具进行高精度的测量。

3、图像探测器指的是固定在转台上的相机。

工作原理：本发明针对含有多个探测器的探测***存在坐标系不一致的问题，尤其针对FOD探测***中主要依靠的雷达、图像和激光三种探测技术，提出了一种将各自坐标系与跑道坐标系的进行相互转换数学模型，并提出了计算转换模型参数计算的方法，以及得出模型参数后简化转换模型的坐标转换公式。在此基础上，针对转换结果的误差，提出了一种误差纠正方法，进一步提高了坐标转换的精度，也间接地提高了探测结果的精度。

一、当探测器是雷达时：

1、雷达坐标系转换模型

雷达坐标系下，探测目标的坐标表达形式为(θ,d)。其中d表示距离，θ表示方位角。

转换模型设雷达坐标系中点P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)公式为P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为雷达坐标系原点在跑道坐标系中的位置。而P_T(x_T,y_T,z_T)是通过

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

计算得到的，δ为雷达坐标系下目标俯仰角。该模型中的未知参数为δ、R₀、P₀。

2、雷达坐标系转换模型参数求解方法

在探测器监视范围内均匀分布摆放N₁个测试点(N₁取值一般在9到85之间)，其在跑道坐标系下坐标分别为(x_j,y_j,z_j),1≤j≤N₁，经雷达探测所得雷达坐标系中坐标分布为(θ_j,d_j),1≤j≤N₁，未知目标俯仰角为δ_j。x_j为测试点在跑道坐标系下x轴的坐标；y_j为测试点在跑道坐标系下y轴的坐标；z_j为测试点在跑道坐标系下z轴的坐标；x、y轴与跑道平行，z轴与跑道垂直，x轴与y轴相互垂直；

通过以下计算步骤计算模型参数：

步骤1：构造

计算P₀＝(A^TA)^-1A^TB，得到模型参数中的P₀；

步骤2：因为R₀ ^-1(P_G-P₀)＝P_T，记(x_j-x₀,y_j-y₀,z_j-z₀)为(vx_j,vy_j,vz_j)，记R₀ ^-1为

构造

并对temp^Ttemp进行特征值分解或者奇异值分解，取最小非零特征值对应的特征向量，并将该向量记为(r₁₁',r₁₂',r₁₃',r₂₂',r₂₃')；

步骤3：计算

得到R₀ ^-1中所有参数的值。

步骤4：对R₀ ^-1求逆矩阵，得到R₀的值。

步骤5：对于任意跑道平面上目标均有z_G≈0，以下关系近似成立

即r₁₁(x_G-x₀)+r₁₂(y_G-y₀)+r₁₃(-z₀)＝x_T，r₁₂(x_G-x₀)+r₂₂(y_G-y₀)+r₂₃(-z₀)＝y_T

且有x_G ²+y_G ²＝d²-z₀ ²

根据以上两个限制条件，令

算出y_G为二元一次方程(1+k₁ ²)y_G ²+2k₁k₂y_G+k₂ ²-d²+z₀ ²＝0的正解，而x_G＝k₁y_G+k₂

从而在无需知道参数δ的情况下实现雷达坐标向跑道坐标的转换。

3、雷达坐标系转换公式

给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其雷达探测坐标(θ,d)计算方法为

θ＝ar tan(y_T/x_T)

给定目标雷达探测坐标(θ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)的计算方法为：令

可以算出y_G为二元一次方程(1+k₁ ²)y_G ²+2k₁k₂y_G+k₂ ²-d²+z₀ ²＝0的正解，而x_G＝k₁y_G+k₂；z_G＝0

二、当探测器是图像探测器时：

1、图像坐标系转换模型

图像坐标系下，探测目标的坐标表达形式为(θ,δ)。其中δ表示俯仰角，θ表示方位角。

转换模型设图像坐标系中点P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)公式为

P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为图像坐标系原点在跑道坐标系中的位置。而P_T(x_T,y_T,z_T)是通过

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

计算得到的，d为探测器到目标的距离。该模型中的未知参数为d、R₀、P₀。

2、图像坐标系转换模型参数求解方法

在探测器监视范围内均匀分布摆放N₁个测试点(N₁取值一般在9到85之间)，其在跑道坐标系下坐标分别为(x_j,y_j,z_j),1≤j≤N₁，经图像探测所得图像坐标系中坐标分布为(θ_j,δ_j),1≤j≤N₁。粗略测量设备安装位置在跑道坐标系下坐标为

在P₀’为中心σ为半径，2σ×2σ×2σ的正方体空间内，取均匀分布间距为τ的点P₀'＝(x₀',y₀',z₀')作为P₀估算值， σ的取值一般在0.1米到5米之间，τ的取值一般是σ的1/100到1/10。对所有的P₀‘执行以下计算：

步骤1：对每一个测试点计算V_j＝(x_j-x₀',y_j-y₀',z_j-z₀')，并将向量V_j缩放为单位向量(除以自身的模)

步骤2：对每一个测试点计算W_j＝(cosδ_jcosθ_j,cosδ_jsinθ_j,sinδ_j)；

步骤3：计算向量K_j为Wj和Vj的叉乘K_j＝W_j×V_j，记为K_j＝(kx_j,ky_j,kz_j)，并计算ψ_j＝ar sin(kz)，ζ_j＝ar tan(ky_j/kx_j)；

步骤4：计算向量Wj和Vj之间的夹角

步骤5：对所有的测试点计算ψ_j，ζ_j(ψ_j，ζ_j为向量Kj的俯仰角和水平角)，数值分布的标准样本差的和ε，表示计算从x₁到x_N1共N1个数值分布的标准样本差。

在所有的P₀‘中，选取ε值最小的一组，令

P₀＝P₀'

得到模型参数中的R₀、P₀，其中表示计算从x₁到x_N1共N1个数值的平均值。

步骤6：令由P_G＝R₀P_T+P₀可知跑道面上目标满足以下关系

t₁₃d cosδcosθ+t₂₃d cosδsinθ+t₃₃d sinδ＝-z₀

反向计算出参数d为

3、图像坐标系转换公式

给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其图像探测坐标(θ,δ)计算方法为

θ＝ar tan(y_T/x_T)

给定图像探测目标坐标(θ,δ)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

三、当探测器是激光扫描器时：

1、激光坐标系转换模型

激光坐标系下，探测目标的坐标表达形式为(θ,δ,d)。其中d表示距离，θ表示方位角，δ表示俯仰角。

转换模型设激光坐标系中点P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)公式为

P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为激光坐标系原点在跑道坐标系中的位置。而P_T(x_T,y_T,z_T)是通过

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

计算得到的。该模型中的未知参数为R₀、P₀。

2、激光坐标系转换模型参数求解方法

在探测器监视范围内均匀分布摆放N₁个测试点(N₁取值一般在9到85之间)，其在跑道坐标系下坐标分别为(x_j,y_j,z_j),1≤j≤N₁，经激光探测所得激光坐标系中坐标分布为(θ_j,δ_j,d_j),1≤j≤N₁。

通过以下计算步骤计算模型参数：

步骤1：构造

计算P₀＝(A^TA)^-1A^TB，得到模型参数中的P₀；

步骤2：对每一个测试点计算V_j＝(x_j-x₀',y_j-y₀',z_j-z₀')，并将向量V_j缩放为单位向量(除以自身的模)

步骤3：对每一个测试点计算W_j＝(cosδ_jcosθ_j,cosδ_jsinθ_j,sinδ_j)；

步骤4：对所有的V_j和W_j计算平均向量

步骤5：计算向量K为和的叉乘并将K缩放为单位向量

记为K＝(kx,ky,kz)，并计算向量和之间的夹角

步骤6：构造

计算

得到模型参数中的R₀。

3、激光坐标系转换公式

给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其激光探测坐标(θ,δ,d)计算方法为

θ＝ar tan(y_T/x_T)

给定激光探测目标坐标(θ,δ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

四、误差纠正方法

由于多种原因，比如探测器本身的测量误差、探测器实际构造比本发明中提出的模型更为复杂、跑道道面与理想平面有误差，使用前述转换方法生成的坐标与实际坐标仍存在一定误差。

设计算出坐标为P_c(cx,cy,cz)，而实际坐标为P_g(gx,gy,gz)。在X轴Y轴方向上的误差分别为Δx＝gx-cx，Δy＝gy-cy，认为误差与坐标的关系为Δx＝f₁(cx,cy)，Δy＝f₂(cx,cy)，f₁、f₂、f₃、f₄均为平滑曲面函数，包含但不限于二元一次曲面、二元二次曲面、二元三次曲面、三角函数、高斯曲面。通过求解平滑曲面参数可以得到误差计算方法。

以Δx＝f₁(cx,cy)函数为二元二次曲面为例，记N₁个测试点计算出的坐标分别为P_c-j(cx_j,cy_j,cz_j),1≤j≤N₁，而实际坐标为P_g-j(gx_j,gy_j,gz_j),1≤j≤N₁，该点X轴上的误差为Δx_j＝gx_j-cx_j。构造矩阵

令E_X＝(A^TA)^-1A^TB_X，E_Y＝(A^TA)^-1A^TB_Y

并记E＝(e₁,e₂,e₃,e₄,e₅,e₆)，H＝(h₁,h₂,h₃,h₄,h₅,h₆)

则对于计算得点(cx,cy)的经过误差纠正后的值应为：

cx＝cx+e₁cx²+e₂cxcy+e₃cy³+e₄cx+e₅cy+e₆；

cy＝cy+h₁cx²+h₂cxcy+h₃cy³+h₄cx+h₅cy+h₆。

本发明并不局限于前述的具体实施方式。本发明扩展到任何在本说明书中披露的新特征或任何新的组合，以及披露的任一新的方法或过程的步骤或任何新的组合。

Claims (7)

1.一种多探测器坐标系转化方法，其特征在于包括：

步骤1：提出了一种通用的探测坐标系与跑道坐标系的转换原理模型，当所述探测器是雷达时，测试点经雷达探测所得雷达坐标系中坐标分布为(θ_j,d_j),1≤j≤N₁；探测器笛卡尔坐标系下表达形式为P_T(x_T,y_T,z_T)：

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

将P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)的坐标转换模型：

P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为探测器坐标系原点在跑道坐标系中的位置；该坐标转换模型未知参数为δ、R₀、P₀；即当实际测量中δ_j、R₀、P₀为未知数；其中δ_j为N1个测试点对应的被探测物体与雷达之间的俯仰角；θ_j为N1个测试点对应的被测物体与雷达之间的方位角；d_j为N1个测试点对应的被测物体与雷达之间的距离；

步骤2：在跑道路面上，均匀设置N1个被探测物体，即为N1个测试点，使用测量仪测得其在跑道坐标系下测试点坐标分别为(x_j,y_j,z_j)；同时使用探测器测得测试点在探测器坐标系下对应位置信息；根据对应关系求解步骤1坐标转换模型中的参数；

步骤3：在探测中获取目标坐标后，使用步骤2中坐标转换公式模型中的参数对步骤1坐标转换模型进行化简，计算出多种探测器的探测坐标与跑道标准坐标进行相互转换的公式；

其中，步骤2中求解坐标转换模型中的参数具体包括：

步骤21：构造

根据计算得到P₀＝(A^TA)^-1A^TB，得到P₀；

步骤22：因为R₀ ^-1(P_G-P₀)＝P_T，记(x_j-x₀,y_j-y₀,z_j-z₀)为(vx_j,vy_j,vz_j)，记R₀ ^-1为

构造

并对temp^Ttemp进行特征值分解或者奇异值分解，取最小非零特征值对应的特征向量，并将该向量记为(r₁₁',r₁₂',r₁₃',r₂₂',r₂₃')，执行步骤23；

步骤23：计算：

得到R₀ ^-1中所有参数的值，执行步骤24：

步骤24：对R₀ ^-1求逆矩阵，得到R₀的值；

步骤25：对于任意跑道平面上目标均有z_G≈0，且

即r₁₁(x_G-x₀)+r₁₂(y_G-y₀)+r₁₃(-z₀)＝x_T；

r₁₂(x_G-x₀)+r₂₂(y_G-y₀)+r₂₃(-z₀)＝y_T

且有

x_G ²+y_G ²＝d²-z₀ ²

根据以上两个限制条件，令：

算出y_G为一元二次方程(1+k₁ ²)y_G ²+2k₁k₂y_G+k₂ ²-d²+z₀ ²＝0的正解，而x_G＝k₁y_G+k₂从而在无需知道参数δ的情况下实现雷达坐标向跑道坐标的转换。

2.权利要求1所述的一种多探测器坐标系转化方法，其特征在于当所述探测器是雷达时，步骤3分别通过步骤31或步骤32具体实现：

步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其雷达探测坐标(θ,d)计算方法为：

θ＝artan(y_T/x_T)

步骤32：给定目标雷达探测坐标(θ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)的计算方法为：令

得到y_G为一元二次方程：(1+k₁ ²)y_G ²+2k₁k₂y_G+k₂ ²-d²+z₀ ²＝0的正解，而x_G＝k₁y_G+k₂；z_G＝0。

3.一种多探测器坐标系转化方法，其特征在于包括：

步骤1：提出了一种通用的探测坐标系与跑道坐标系的转换原理模型，当探测器是图像探测器时，测试点经过图像探测器探测所得图像坐标系中坐标分布为(θ_j,δ_j)，即探测目标在图像探测器极坐标表达形式为(θ_j,δ_j),1≤j≤N₁；图像探测器笛卡尔坐标系下表达形式为P_T(x_T,y_T,z_T)：

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

将P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)的坐标转换模型：

P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为探测器坐标系原点在跑道坐标系中的位置；该坐标转换模型未知参数为d、R₀、P₀；实际测量中d_j、R₀、P₀为未知数；其中δ_j为N1个测试点对应的被探测物体与图像探测器之间的俯仰角；θ_j为N1个测试点对应的被测物体与图像探测器之间的方位角；d_j为N1个测试点对应的被测物体与图像探测器之间的距离；

步骤2：在跑道路面上，均匀设置N1个被探测物体，即为N1个测试点，使用测量仪测得其在跑道坐标系下测试点坐标分别为(x_j,y_j,z_j)；同时使用探测器测得测试点在探测器坐标系下对应位置信息；根据对应关系求解步骤1坐标转换模型中的参数；

步骤3：在探测中获取目标坐标后，使用步骤2中坐标转换公式模型中的参数对步骤1坐标转换模型进行化简，计算出多种探测器的探测坐标与跑道标准坐标进行相互转换的公式；

步骤2中求解坐标转换模型中的参数具体包括：经图像探测器所得图像坐标系中坐标分布为(θ_j,δ_j),1≤j≤N₁，测量探测器在跑道坐标系下坐标为在P₀ ^’ 为中心σ为半径，2σ×2σ×2σ的正方体空间内，取均匀分布间距为τ的点P₀′＝(x₀′，y₀′，z₀′)作为P₀估算值， σ的取值为0.1米到5米，τ的取值为到对所有的P₀ ^’ 执行以下计算：

步骤21：对每一个测试点计算V_j＝(x_j-x₀',y_j-y₀',z_j-z₀')，并将向量V_j缩放为单位向量，即除以自身的模；

步骤22：对每一个测试点计算W_j＝(cosδ_jcosθ_j,cosδ_jsinθ_j,sinδ_j)；

步骤23：计算向量K_j为W_j和V_j的叉乘K_j＝W_j×V_j，记为K_j＝(kx_j，ky_j，kz_j)，并计算ψ_j＝arsin(kz_j)，ζ_j＝artan(ky_j/kx_j)；

步骤24：计算向量W_j和V_j之间的夹角

步骤25：对所有的测试点计算ψ_j，ζ_j，数值分布的标准样本差的和ε，表示计算从x₁到x_N1共N1个数值分布的标准样本差；ψ_j，ζ_j为向量K_j的俯仰角和水平角；

在所有的P₀ ^’ 中，选取ε值最小的一组，令

P₀＝P₀'

得到R₀、P₀，其中表示计算从x₁到x_N1共N1个数值的平均值；

步骤26：令由P_G＝R₀P_T+P₀可知跑道面上目标满足关系：t₁₃d cosδcosθ+t₂₃d cosδsinθ+t₃₃d sinδ＝-z₀；

反向计算出参数d为：

4.根据权利要求3所述的一种多探测器坐标系转化方法，其特征在于当所述探测器是图像探测器时，步骤3具体包括：

步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其图像探测坐标(θ,δ)计算方法为

θ＝ar tan(y_T/x_T)；

给定图像探测目标坐标(θ,δ)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

根据得到P_G。

5.一种多探测器坐标系转化方法，其特征在于包括：

步骤1：提出了一种通用的探测坐标系与跑道坐标系的转换原理模型，当所述探测器是激光扫描器时，测试点在激光坐标系中坐标分布为(θ_j,δ_j,d_j)；即探测目标的在激光器极坐标表达形式为(θ_j,δ_j,d_j),1≤j≤N₁；探测器笛卡尔坐标系下表达形式为P_T(x_T,y_T,z_T)：

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

将P_T(x_T,y_T,z_T)转换到跑道坐标系的坐标P_G(x_G,y_G,z_G)的坐标转换模型：

P_G＝R₀P_T+P₀

其中，R₀为旋转矩阵，P₀(x₀,y₀,z₀)为探测器坐标系原点在跑道坐标系中的位置；该坐标转换模型未知参数为R₀、P₀；经激光探测被探测物体，其中δ_j为N1个测试点对应的被探测物体与激光扫描器之间的俯仰角；θ_j为N1个测试点对应的被测物体与激光扫描器之间的方位角；d_j为N1个测试点对应的被测物体与激光扫描器之间的距离；

步骤2：在跑道路面上，均匀设置N1个被探测物体，即为N1个测试点，使用测量仪测得其在跑道坐标系下测试点坐标分别为(x_j,y_j,z_j)；同时使用探测器测得测试点在探测器坐标系下对应位置信息；根据对应关系求解步骤1坐标转换模型中的参数；

步骤3：在探测中获取目标坐标后，使用步骤2中坐标转换公式模型中的参数对步骤1坐标转换模型进行化简，计算出多种探测器的探测坐标与跑道标准坐标进行相互转换的公式；

则步骤2中求解坐标转换模型中参数具体包括：

步骤21：构造

根据计算得到P₀＝(A^TA)^-1A^TB，得到P₀；

步骤22：对每一个测试点计算V_j＝(x_j-x₀',y_j-y₀',z_j-z₀')，并将向量V_j缩放为单位向量，即除以自身的模：其中(x₀'，y₀'，z₀')作为P₀估算值；

步骤23：对每一个测试点计算W_j＝(cosδ_jcosθ_j,cosδ_jsinθ_j,sinδ_j)；

步骤24：对所有的V_j和W_j计算平均向量

步骤25：计算向量K为和的叉乘并将K缩放为单位向量

记为K＝(kx,ky,kz)，并计算向量和之间的夹角

步骤26：构造

计算

得到模型参数中的R₀。

6.根据权利要求5所述的一种多探测器坐标系转化方法，其特征在于当所述探测单元是激光扫描器时，步骤3具体为：

步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其激光探测坐标(θ,δ,d) 计算方法为：

θ＝ar tan(y_T/x_T)

步骤32:给定激光探测目标坐标(θ,δ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为：

x_T＝d cosδcosθ

y_T＝d cosδsinθ

z_T＝d sinδ

7.基于权利要求1至6之一所述多探测器坐标系转化方法的误差纠正方法，其特征在于当计算出测试点在跑道坐标系的坐标为P_c(cx,cy,cz)，而实际跑道坐标系的坐标为P_g(gx,gy,gz)，在X轴Y轴方向上的误差分别为Δx＝gx-cx，Δy＝gy-cy，时；则误差与坐标的关系为Δx＝f₁(cx,cy)，Δy＝f₂(cx,cy)，f₁、f₂、f₃、f₄均为平滑曲面函数，包含二元一次曲面、二元二次曲面、二元三次曲面、三角函数、高斯曲面；

Δx＝f₁(cx,cy)函数为二元二次曲面时，记N1个测试点计算出跑道坐标系的坐标分别为P_c-j(cx_j,cy_j,cz_j),1≤j≤N₁，而实际坐标为P_g-j(gx_j,gy_j,gz_j),1≤j≤N₁，该点X轴上的误差为Δx_j＝gx_j-cx_j；构造矩阵:

令E_X＝(A^TA)^-1A^TB_X；E_Y＝(A^TA)^-1A^TB_Y

并记E_x＝(e₁,e₂,e₃,e₄,e₅,e₆)，E_y＝(h₁,h₂,h₃,h₄,h₅,h₆)；

则对于计算得点(cx,cy)的经过误差纠正后的值应为：

cx＝cx+e₁cx²+e₂cxcy+e₃cy³+e₄cx+e₅cy+e₆；

cy＝cy+h₁cx²+h₂cxcy+h₃cy³+h₄cx+h₅cy+h₆。