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

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

1.一种多探测器坐标系转化方法，其特征在于包括：
步骤1：提出了一种通用的探测坐标系与跑道坐标系的转换原理模型，探
测目标的在探测器极坐标表达形式为(θ,d,δ)，其中d表示探测器到被探测物体
距离，θ表示被探测物体与探测器之间的方位角，δ表示被探测物体与探测器
之间的俯仰角；探测器笛卡尔坐标系下表达形式为P_T(x_T,y_T,z_T)：
x_T＝dcosδcosθ
y_T＝dcosδsinθ
z_T＝dsinδ
将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坐标转换模型进行化简，计算出多种探测器的探测坐标与跑道标
准坐标进行相互转换的公式。
2.根据权利要求1所述的一种多探测器坐标系转化方法，其特征在于当所
述探测器是雷达时，测试点经雷达探测所得雷达坐标系中坐标分布为
(θ_j,d_j),1≤j≤N₁，步骤1中坐标转换模型未知参数为δ、R₀、P₀；即当实际测量
中δ_j、R₀、P₀为未知数；其中δ_j为N1个测试点对应的被探测物体与雷达之间
的俯仰角；θ_j为N1个测试点对应的被测物体与雷达之间的方位角；d_j为N1个
测试点对应的被测物体与之间的距离；步骤2中求解坐标转换模型中的参数具
体包括：
步骤21：构造 A = 2 ( x 2 - x 1 ) 2 ( y 2 - y 1 ) 2 ( z 2 - z 1 ) 2 ( x 3 - x 2 ) 2 ( y 3 - y 2 ) 2 ( z 3 - z 2 ) ... ... ... 2 ( x N 1 - x N 1 - 1 ) 2 ( y N 1 - y N 1 - 1 ) 2 ( z N 1 - z N 1 - 1 ) ; ]]>
B = d 1 2 + x 2 2 + y 2 2 + z 2 2 - d 2 2 - x 1 2 - y 1 2 - z 1 2 d 2 2 + x 3 2 + y 3 2 + z 3 2 - d 3 2 - x 2 2 - y 2 2 - z 2 2 ... d N 1 - 1 2 + x N 1 2 + y N 1 2 + z N 1 2 - d N 1 2 - x N 1 - 1 2 - y N 1 - 1 2 - z N 1 - 1 2 ]]>
根据计算得到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
为
R 0 - 1 = r 11 r 12 r 13 r 12 r 22 r 23 r 13 r 23 r 33 ; ]]>
构造 t e m p = vx 1 vy 1 - vx 1 tanθ 1 vz 1 - vy 1 tanθ 1 - vz 1 tanθ 1 vx 2 vy 2 - vx 2 tanθ 2 vz 2 - vy 2 tanθ 2 - vz 2 tanθ 2 ... ... ... ... ... vx N 1 vy N 1 - vx N 1 tanθ N 1 vz N 1 - vy N 1 tanθ N 1 - vz N 1 tanθ N 1 ]]>
并对temp^Ttemp进行特征值分解或者奇异值分解，取最小非零特征值对应的特征
向量，并将该向量记为(r₁₁',r₁₂',r₁₃',r₂₂',r₂₃')，执行步骤23；
步骤23：计算：
r 11 = 2 r 11 ′ r 11 ′ 2 + 2 r 1 2 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>
r 12 = 2 r 12 ′ r 11 ′ 2 + 2 r 1 2 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>
r 13 = 2 r 13 ′ r 11 ′ 2 + 2 r 1 2 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>
r 22 = 2 r 22 ′ r 11 ′ 2 + 2 r 1 2 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>
r 23 = 2 r 23 ′ r 11 ′ 2 + 2 r 1 2 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>
r 33 = 1 - r 13 2 - r 23 2 ]]>
得到R₀^-1中所有参数的值，执行步骤24：
步骤24：对R₀^-1求逆矩阵，得到R₀的值；
步骤25：对于任意跑道平面上目标均有z_G≈0，且
R 0 - 1 ( x G y G 0 - P 0 ) = P T ]]>
即r₁₁(x_G-x₀)+r₁₂(y_G-y₀)+r₁₃(-z₀)＝x_T；
r₁₂(x_G-x₀)+r₂₂(y_G-y₀)+r₂₃(-z₀)＝y_T
且有 y T x T = r 12 ( x G - x 0 ) + r 22 ( y G - y 0 ) + r 23 ( - z 0 ) r 11 ( x G - x 0 ) + r 12 ( y G - y 0 ) + r 13 ( - z 0 ) = t a n θ ]]>
x_G²+y_G²＝d²-z₀²
根据以上两个限制条件，令：
k 2 = r 12 x 0 + r 22 y 0 + r 23 z 0 - t a n θ ( r 11 x 0 + r 12 y 0 + r 13 z 0 ) r 12 - r 11 t a n θ ]]>
算出y_G为二元一次方程(1+k₁²)y_G²+2k₁k₂y_G+k₂²-d²+z₀²＝0
的正解，而x_G＝k₁y_G+k₂
从而在无需知道参数δ的情况下实现雷达坐标向跑道坐标的转换。
3.权利要求2所述的一种多探测器坐标系转化方法，其特征在于当所述探
测器是雷达时，步骤3分别通过步骤31或步骤32具体实现：
步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其雷达探测坐标(θ,d)
计算方法为：
P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ]]>
θ＝artan(y_T/x_T)
d = x T 2 + y T 2 + z T 2 ; ]]>
步骤32：给定目标雷达探测坐标(θ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)
的计算方法为：令
k 2 = r 12 x 0 + r 22 y 0 + r 23 z 0 - t a n θ ( r 11 x 0 + r 12 y 0 + r 13 z 0 ) r 12 - r 11 t a n θ ; ]]>
得到y_G为二元一次方程：(1+k₁²)y_G²+2k₁k₂y_G+k₂²-d²+z₀²＝0
的正解，而x_G＝k₁y_G+k₂；z_G＝0。
4.根据权利要求1所述的一种多探测器坐标系转化方法，其特征在于
当所述探测器是图像探测器时，步骤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＝arsin(kz)，ζ_j＝artan(ky_j/kx_j)；
步骤24：计算向量Wj和Vj之间的夹角
步骤25：对所有的测试点计算ψ_j，ζ_j，数值分布的标准样本差的和ε，
表示计算从x₁到x_N1共N1个数值分布的标准样本差；ψ_j，ζ_j为向量Kj
的俯仰角和水平角；

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

k x = c o s ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) c o s ( m e a n 1 ≤ j ≤ N 1 ( ζ j ) ) ]]>
k x = c o s ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) sin ( m e a n 1 ≤ j ≤ N 1 ( ζ j ) ) ]]>
k z = s i n ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) ]]>
R K = 0 - k z k y k z 0 - k x - k y k x 0 ]]>

得到R₀、P₀，其中表示计算从x₁到x_N1共N1个数值的平均值；
步骤26：令 R 0 = t 11 t 12 t 13 t 12 t 22 t 23 t 13 t 23 t 33 , ]]>由P_G＝R₀P_T+P₀可知跑道面上目标满足关系：
t₁₃dcosδcosθ+t₂₃dcosδsinθ+t₃₃dsinδ＝-z₀；
反向计算出参数d为： d = - z 0 t 13 c o s δ c o s θ + t 23 cos δ s i n θ + t 33 s i n δ . ]]>
5.根据权利要求1所述的一种多探测器坐标系转化方法，其特征在于当所
述探测单元是图像探测器时，步骤3具体包括：
步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其图像探测坐标(θ,δ)
计算方法为
P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ; ]]>
δ = a r t a n ( z T / x T 2 + y T 2 ) ; ]]>
θ＝artan(y_T/x_T)；
给定图像探测目标坐标(θ,δ)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算方法为
d = - z 0 t 13 c o s δ c o s θ + t 23 cos δ s i n θ + t 33 s i n δ ]]>
x_T＝dcosδcosθ
y_T＝dcosδsinθ
z_T＝dsinδ
根据 P G = R 0 x T y T z T + P 0 , ]]>得到P_G。
6.根据权利要求1所述的一种多探测器坐标系转化方法，其特征在于当所
述探测器是激光扫描器时，步骤1中坐标转换模型未知参数为R₀、P₀；经激光
探测被探测物体，测试点在激光坐标系中坐标分布为(θ_j,δ_j,d_j),1≤j≤N₁；其中δ_j
为N1个测试点对应的被探测物体与激光扫描器之间的俯仰角；θ_j为N1个测试
点对应的被测物体与激光扫描器之间的方位角；d_j为N1个测试点对应的被测
物体与激光扫描器之间的距离；则步骤2中求解坐标转换模型中参数具体包
括：
步骤21：构造 A = 2 ( x 2 - x 1 ) 2 ( y 2 - y 1 ) 2 ( z 2 - z 1 ) 2 ( x 3 - x 2 ) 2 ( y 3 - y 2 ) 2 ( z 3 - z 2 ) ... ... ... 2 ( x N 1 - x N 1 - 1 ) 2 ( y N 1 - y N 1 - 1 ) 2 ( z N 1 - z N 1 - 1 ) ; ]]>
B = d 1 2 + x 2 2 + y 2 2 + z 2 2 - d 2 2 - x 1 2 - y 1 2 - z 1 2 d 2 2 + x 3 2 + y 3 2 + z 3 2 - d 3 2 - x 2 2 - y 2 2 - z 2 2 ... d N 1 - 1 2 + x N 1 2 + y N 1 2 + z N 1 2 - d N 1 2 - x N 1 - 1 2 - y N 1 - 1 2 - z N 1 - 1 2 ]]>
根据计算得到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计算平均向量
V ‾ = 1 N 1 Σ j V j , W ‾ = 1 N 1 Σ j W j ]]>
步骤23：计算向量K为和的叉乘并将K缩放为单位向量
K = K | | K | | ]]>
记为K＝(kx,ky,kz)，并计算向量和之间的夹角
步骤24：构造 R K = 0 - k z k y k z 0 - k x - k y k x 0 ]]>
计算
得到模型参数中的R₀。
7.根据权利要求1所述的一种多探测器坐标系转化方法，其特征在于当所
述探测单元是激光扫描器时，步骤3具体为：
步骤31：给定目标跑道坐标系下坐标P_G(x_G,y_G,z_G)，其激光探测坐标(θ,δ,d)
计算方法为：
P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ]]>
δ = a r t a n ( z T / x T 2 + y T 2 ) ]]>
θ＝artan(y_T/x_T)
d = x T 2 + y T 2 + z T 2 ]]>
步骤32:给定激光探测目标坐标(θ,δ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)计算
方法为
x_T＝dcosδcosθ
y_T＝dcosδsinθ
z_T＝dsinδ
P G = R 0 x T y T z T + P 0 . ]]>
8.一种多探测器坐标系转化的误差纠正方法，其特征在于当计算出测试点
在跑道坐标系的坐标为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₄均为平滑曲面函数，包含二元一次
曲面、二元二次曲面、二元三次曲面、三角函数、高斯曲面。
9.根据权利要求8所述的一种多探测器坐标系转化的误差纠正方法，其特
征在于Δ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；构造矩阵:
A = cx 1 2 cx 1 cy 1 cy 1 2 cx 1 cy 1 1 cx 2 2 cx 2 cy 2 cy 2 2 cx 2 cy 2 1 ... ... ... ... ... ... cx N 1 2 cx N 1 cy N 1 cy N 1 2 cx N 1 cy N 1 1 ]]>
B X = g x 1 - c x 1 gx 2 - cx 2 ... g x N 1 - c x N 1 B Y = g y 1 - c y 1 gy 2 - cy 2 ... g y N 1 - c y N 1 ]]>
令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₆。

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

x_T＝dcosδcosθ

y_T＝dcosδsinθ

z_T＝dsinδ

将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：构造 A = 2 ( x 2 - x 1 ) 2 ( y 2 - y 1 ) 2 ( z 2 - z 1 ) 2 ( x 3 - x 2 ) 2 ( y 3 - y 2 ) 2 ( z 3 - z 2 ) ... ... ... 2 ( x N 1 - x N 1 - 1 ) 2 ( y N 1 - y N 1 - 1 ) 2 ( z N 1 - z N 1 - 1 ) ; ]]>

B = d 1 2 + x 2 2 + y 2 2 + z 2 2 - d 2 2 - x 1 2 - y 1 2 - z 1 2 d 2 2 + x 3 2 + y 3 2 + z 3 2 - d 3 2 - x 2 2 - y 2 2 - z 2 2 ... d N 1 - 1 2 + x N 1 2 + y N 1 2 + z N 1 2 - d N 1 2 - x N 1 - 1 2 - y N 1 - 1 2 - z N 1 - 1 2 ]]>

根据计算得到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
为 R 0 - 1 = r 11 r 12 r 13 r 12 r 22 r 23 r 13 r 23 r 33 ; ]]>

构造 t e m p = vx 1 vy 1 - vx 1 tanθ 1 vz 1 - vy 1 tanθ 1 - vz 1 tanθ 1 vx 2 vy 2 - vx 2 tanθ 2 vz 2 - vy 2 tanθ 2 - vz 2 tanθ 2 ... ... ... ... ... vx N 1 vy N 1 - vx N 1 tanθ N 1 vz N 1 - vy N 1 tanθ N 1 - vz N 1 tanθ N 1 ]]>

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

步骤23：计算：

r 11 = 2 r 11 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 12 = 2 r 12 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 13 = 2 r 13 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 22 = 2 r 22 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 23 = 2 r 23 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 33 = 1 - r 13 2 - r 23 2 ]]>

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

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

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

R 0 - 1 ( x G y G 0 - P 0 ) = P T ]]>

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

且有 y T x T = r 12 ( x G - x 0 ) + r 22 ( y G - y 0 ) + r 23 ( - z 0 ) r 11 ( x G - x 0 ) + r 12 ( y G - y 0 ) + r 13 ( - z 0 ) = t a n θ ; ]]>x_G²+y_G²＝d²-z₀²

根据以上两个限制条件，令： k 1 = r 12 t a n θ - r 22 r 12 - r 11 t a n θ ]]>

k 2 = r 12 x 0 + r 22 y 0 + r 23 z 0 - tan θ ( r 11 x 0 + r 12 y 0 + r 13 z 0 ) r 12 - r 11 tan θ ]]>

算出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)
计算方法为：

P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ]]>

θ＝artan(y_T/x_T)

d = x T 2 + y T 2 + z T 2 ; ]]>

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

k 2 = r 12 x 0 + r 22 y 0 + r 23 z 0 - t a n θ ( r 11 x 0 + r 12 y 0 + r 13 z 0 ) r 12 - r 11 t a n θ ; ]]>

得到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₀估算值，
x ~ 0 - σ ≤ x 0 ′ ≤ x ~ 0 + σ , y ~ 0 - σ ≤ y 0 ′ ≤ y ~ 0 + σ , z ~ 0 - σ ≤ z 0 ′ ≤ z ~ 0 + σ , ]]>σ的取值为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＝arsin(kz)，ζ_j＝artan(ky_j/kx_j)；

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

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

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

P₀＝P₀'

k x = c o s ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) c o s ( m e a n 1 ≤ j ≤ N 1 ( ζ j ) ) ]]>

k x = c o s ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) sin ( m e a n 1 ≤ j ≤ N 1 ( ζ j ) ) ]]>

k z = s i n ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) ]]>

R K = 0 - k z k y k z 0 - k x - k y k x 0 ]]>

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

步骤26：令 R 0 = t 11 t 12 t 13 t 12 t 22 t 23 t 13 t 23 t 33 , ]]>由P_G＝R₀P_T+P₀可知跑道面上目标满足关系
t₁₃dcosδcosθ+t₂₃dcosδsinθ+t₃₃dsinδ＝-z₀；

反向计算出参数d为： d = - z 0 t 13 c o s δ c o s θ + t 23 cos δ s i n θ + t 33 s i n δ . ]]>

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

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

P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ]]>

δ = a r t a n ( z T / x T 2 + y T 2 ) ]]>

θ＝artan(y_T/x_T)

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

d = - z 0 t 13 c o s δ c o s θ + t 23 cos δ s i n θ + t 33 s i n δ ]]>

x_T＝dcosδcosθ

y_T＝dcosδsinθ

z_T＝dsinδ

根据 P G = R 0 x T y T z T + P 0 , ]]>得到P_G。

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

步骤21：构造 A = 2 ( x 2 - x 1 ) 2 ( y 2 - y 1 ) 2 ( z 2 - z 1 ) 2 ( x 3 - x 2 ) 2 ( y 3 - y 2 ) 2 ( z 3 - z 2 ) ... ... ... 2 ( x N 1 - x N 1 - 1 ) 2 ( y N 1 - y N 1 - 1 ) 2 ( z N 1 - z N 1 - 1 ) ; ]]>

B = d 1 2 + x 2 2 + y 2 2 + z 2 2 - d 2 2 - x 1 2 - y 1 2 - z 1 2 d 2 2 + x 3 2 + y 3 2 + z 3 2 - d 3 2 - x 2 2 - y 2 2 - z 2 2 ... d N 1 - 1 2 + x N 1 2 + y N 1 2 + z N 1 2 - d N 1 2 - x N 1 - 1 2 - y N 1 - 1 2 - z N 1 - 1 2 ]]>

根据计算得到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计算平均向量

V ‾ = 1 N 1 Σ j V j , W ‾ = 1 N 1 Σ j W j ]]>

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

K = K | | K | | ]]>

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

步骤24：构造

R K = 0 - k z k y k z 0 - k x - k y k x 0 ]]>

计算

得到模型参数中的R₀。

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

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

P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ]]>

δ = a r t a n ( z T / x T 2 + y T 2 ) ]]>

θ＝artan(y_T/x_T)

d = x T 2 + y T 2 + z T 2 ]]>

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

x_T＝dcosδcosθ

y_T＝dcosδsinθ

z_T＝dsinδ

P G = R 0 x T y T z T + P 0 . ]]>

一种多探测器坐标系转化的误差纠正方法，当计算出测试点在跑道坐标系
的坐标为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。构造矩阵:

A = cx 1 2 cx 1 cy 1 cy 1 2 cx 1 cy 1 1 cx 2 2 cx 2 cy 2 cy 2 2 cx 2 cy 2 1 ... ... ... ... ... ... cx N 1 2 cx N 1 cy N 1 cy N 1 2 cx N 1 cy N 1 1 ]]>

B X = gx 1 - cx 1 gx 2 - cx 2 ... gx N 1 - cx N 1 B Y = gy 1 - cy 1 gy 2 - cy 2 ... gy N 1 - cy N 1 ]]>

令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＝dcosδcosθ

y_T＝dcosδsinθ

z_T＝dsinδ

计算得到的，δ为雷达坐标系下目标俯仰角。该模型中的未知参数为δ、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：构造

A = 2 ( x 2 - x 1 ) 2 ( y 2 - y 1 ) 2 ( z 2 - z 1 ) 2 ( x 3 - x 2 ) 2 ( y 3 - y 2 ) 2 ( z 3 - z 2 ) ... ... ... 2 ( x N 1 - x N 1 - 1 ) 2 ( y N 1 - y N 1 - 1 ) 2 ( z N 1 - z N 1 - 1 ) ]]>

B = d 1 2 + x 2 2 + y 2 2 + z 2 2 - d 2 2 - x 1 2 - y 1 2 - z 1 2 d 2 2 + x 3 2 + y 3 2 + z 3 2 - d 3 2 - x 2 2 - y 2 2 - z 2 2 ... d N 1 - 1 2 + x N 1 2 + y N 1 2 + z N 1 2 - d N 1 2 - x N 1 - 1 2 - y N 1 - 1 2 - z N 1 - 1 2 ]]>

计算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
为 R 0 - 1 = r 11 r 12 r 13 r 12 r 22 r 23 r 13 r 23 r 33 ]]>

构造 t e m p = vx 1 vy 1 - vx 1 tanθ 1 vz 1 - vy 1 tanθ 1 - vz 1 tanθ 1 vx 2 vy 2 - vx 2 tanθ 2 vz 2 - vy 2 tanθ 2 - vz 2 tanθ 2 ... ... ... ... ... vx N 1 vy N 1 - vx N 1 tanθ N 1 vz N 1 - vy N 1 tanθ N 1 - vz N 1 tanθ N 1 ]]>

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

步骤3：计算

r 11 = 2 r 11 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 12 = 2 r 12 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 13 = 2 r 13 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 22 = 2 r 22 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 23 = 2 r 23 ′ r 11 ′ 2 + 2 r 12 ′ 2 + r 13 ′ 2 + r 22 ′ 2 + r 23 ′ 2 ]]>

r 33 = 1 - r 13 2 - r 23 2 ]]>

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

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

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

R 0 - 1 ( x G y G 0 - P 0 ) = P T ]]>

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

且有 y T x T = r 12 ( x G - x 0 ) + r 22 ( y G - y 0 ) + r 23 ( - z 0 ) r 11 ( x G - x 0 ) + r 12 ( y G - y 0 ) + r 13 ( - z 0 ) = t a n θ , ]]>x_G²+y_G²＝d²-z₀²

根据以上两个限制条件，令 k 1 = r 12 t a n θ - r 22 r 12 - r 11 tan θ ]]>

k 2 = r 12 x 0 + r 22 y 0 + r 23 z 0 - t a n θ ( r 11 x 0 + r 12 y 0 + r 13 z 0 ) r 12 - r 11 t a n θ ]]>

算出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)计算方法为

P T = x T y T z T = R 0 - 1 ( P G - P 0 ) ]]>

θ＝artan(y_T/x_T)

d = x T 2 + y T 2 + z T 2 ]]>

给定目标雷达探测坐标(θ,d)，其跑道坐标系下坐标P_G(x_G,y_G,z_G)的计算方法
为：令 k 1 = r 12 t a n θ - r 22 r 12 - r 11 tan θ ]]>

k 2 = r 12 x 0 + r 22 y 0 + r 23 z 0 - t a n θ ( r 11 x 0 + r 12 y 0 + r 13 z 0 ) r 12 - r 11 t a n θ ]]>

可以算出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＝dcosδcosθ

y_T＝dcosδsinθ

z_T＝dsinδ

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

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

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

在P₀’为中心σ为半径，2σ×2σ×2σ的正方体空间内，取均匀分布间距
为τ的点P₀'＝(x₀',y₀',z₀')作为P₀估算值， x ~ 0 - σ ≤ x 0 ′ ≤ x ~ 0 + σ , y ~ 0 - σ ≤ y 0 ′ ≤ y ~ 0 + σ , ]]>
σ的取值一般在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＝arsin(kz)，ζ_j＝artan(ky_j/kx_j)；

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

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

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

P₀＝P₀'

k x = c o s ( m e a n 1 ≤ j ≤ N 1 ( ψ j ) ) c o s ( m e a n 1 ≤ j ≤ N 1 ( ζ j ) ) ]]>