CN108663683B

CN108663683B - 一种距离波门算法

Info

Publication number: CN108663683B
Application number: CN201710211115.9A
Authority: CN
Inventors: 赵永计; 皮雳; 傅振宇; 李庆远; 祝开奕; 李海龙
Original assignee: HUBEI HUAZHONG PHOTOELECTRIC SCIENCE AND TECHNOLOGY Ltd
Current assignee: Hubei Huazhong Changjiang Photoelectric Technology Co ltd
Priority date: 2017-04-01
Filing date: 2017-04-01
Publication date: 2020-07-14
Anticipated expiration: 2037-04-01
Also published as: CN108663683A

Abstract

一种距离波门算法，其中该算法主要包括边界条件设置；现在点距离录入是指按算法需要的格式输入数据，保证该算法使用的数据正确性；距离建航包括首次速度匹配建航和剩余点再次建航递推；现在点距离匹配输出包括筛选距离航路匹配点及最优匹配距离、最优航路距离输出及存储、距离航路失效判断和第一不匹配距离输出，使得该算法输出的当前航路距离最优；未来点距离递推计算包括计算距离航路递推速度、计算递推距离并存储和未来点距离波门修正，使得该算法可以更迅速匹配未来点距离；按照本发明实现的距离波门算法，其所实现的循环运算可解析所有距离航路的所有点距离的正确性，提高航路建航的精确度。

Description

一种距离波门算法

技术领域

本发明属于剔除距离杂波算法，具体涉及工程实际嵌入式环境中将距离曲线降阶为速度直线进行距离递推，用于剔除干扰距离、优选测距过程中的输出值。适用于频率不大于200Hz的重频测距机。

背景技术

激光测距技术有很广泛的应用，包括机载、车载、舰载和手持式等多种用途。在工程环境中测距机多使用嵌入式系统作为其控制载体。

测距值的有效性随测距机性能、目标动态位置、测距机载体动态位置、云层或其它飞行物遮挡和其它环境因素等波动，形成了测距回波的错回、漏回和多回等情况。数据处理因嵌入式系统运行环境限制，传统测距结果反馈或以第一回波为输出、或以简单算法结果为输出，从而使得测距机的回波率及准测率受到影响，使测距机的使用范围受到了限制。

发明内容

本发明为了解决现有技术中的测距机的使用范围受限的技术问题，提出了一种距离波门算法，距离波门算法，其特征在于，该算法包括如下步骤：

边界条件设置，初始化设置距离波门算法的边界条件，包括目标最大速度Vmax，目标最大速率变换倍数，航路预失效点数及失效点数，以及自适应距离波门初始值，包括距离波门最大值BMmax及最小值BMmin；

接收测距机的点距离信息Ln，其中n为接收的有效点距离的序号，每个点距离信息中包含N个距离值，记为Ln¹，Ln²，Ln³…Ln^N，其中N可根据算法头文件中的常量进行设置，最大不能大于10；

利用所述Ln进行首次点距离建航，包括如下过程：当连续接收到3个点距离信息L₁、L₂、L₃时执行首次匹配建航，依次计算相邻2点的差分速度记为V₁＝L₂ ^m2-L₁ ^m1和V₂＝L₃ ^m3-L₂ ^m2(其中L₁ ^m1，L₂ ^m2，L₃ ^m3分别为L₁，L₂，L₃中的某个距离值)，如果V₁及V₂满足如下条件时认为当前3个距离值可以建航，存入第一空航路中，若L₁、L₂、L₃中的距离值均不满足建航条件则建航失败，等待下一个点距离信息：

|V₁|<＝Vmax/f and|V₂|<＝Vmax/f and|V₂-V₁|<＝Vmax/f，其中f为所述测距机的重频频率；

建航后将已参与建航的所述有效点距离值L₁ ^m1，L₂ ^m2，L₃ ^m3分别从L₁，L₂，L₃中删除形成{L₁′,L₂′,L₃′}，并计算存储所述第一航路的最小二乘法参数及当前航路信息，具体为：

所述第一航路当前点速度V_d(1)＝V₂及未来点距离L_d(1)＝L₃，航路点数Nd(1)＝3，距离波门BM(1)＝BMmax；

最小二乘法参数V₀(1)＝V₁，T₀(1)＝2，V_T(1)＝2*V₁，T₂(1)＝T₀ ²，其中所述最小二乘法参数用于计算建航方程y＝kx+b的系数，

其中：k(1)＝((Nd(1)-1)*V_T(1)-V₀(1)*T₀(1))/((Nd(1)-1)*T₂(1)-T₀(1)²)

b(1)＝(V₀(1)*T₂(1)-V_T(1)*T₀(1))/((Nd(1)-1)*T₂(1)-T₀(1)²)，由此完成了首次建航，建航方程为y＝k(1)x+b(1)；

完成现在点距离匹配输出，其中包括如下步骤：若首次建航成功则将L_d(1)作为最优匹配距离输出，否则将当前点距离信息Ln中的距离值Ln^m依次代入公式|Ln^m-L_d(1)|<＝BM(1)，将满足公式的距离记为Ln¹，Ln²…Ln^k，其中k为整数，从中找出|Ln^k Ld(1)|最小的距离值Ln^k1，即为航路的最佳匹配距离；然后标记该航路为有效；将Ln中第一个未匹配有效的距离值进行存储，将所述Ln^k1从所述Ln中删除；

判定所述航路有效的标准为：若该航路连续在所述预失效点数内距离点无匹配距离，则判定航路失效，将该航路移至最后序号，若航路连续在所述失效点数内无有效匹配，则该航路失效，清除该航路；

由此计算出航路下一点m的未来点速度V_d(m)、未来点距离L_d(m)及距离波门BM(m)，

其中：V_d(m)＝k(m)*m+b(m)；

L_d(m)＝L_d(m-1)+V_d(m)；

其中k(m)，b(m)为速度直线y＝kx+b的系数

k(m)＝((Nd(m)-1)*V_T(m)-V₀(m)*T₀(m))/((Nd(m)-1)*T₂(m)-T₀(m)²)，

b(m)＝(V₀(m)*T₂(m)-V_T(m)*T₀(m))/((Nd(m)-1)*T₂(m)-T₀(m)²)；

其中Nd(m)为航路点数，V₀(m)，T₀(m)，V_T(m)，T₂(m)为该航路最小二乘法参数

V0(m)＝V0(m-1)+Vd(m-1)，T0(m)＝T0(m-1)+Nd(m-1)，

VT(m)＝V_T(m-1)+V_d(m-1)*Nd(m-1)，T₂(m)＝T₂(m-1)+Nd²(m-1)，

若当前航路最佳距离匹配标志为有效则将航路下一点匹配的波门BM(m)定量减小，减小不能小于BMmin，反之则增大，增大不能大于BMmax；

以上为航路1所需算法的输入数据、匹配及递推公式、输出方法。

航路1匹配后剩余的点距离信息{L1′,L2′,L3′}对其余航路依次匹配或再次建航，方法步骤及公式与航路1相同。其余航路按航路1的方法步骤及公式进行算法运算及输出。

本发明相对于现有技术而言，由于采用了速度最小二乘法建航的技术方案，所以取得了如下有益效果：

(1)按照本发明的点距离录入采用固定需要的格式，保证该算法使用数据的正确性；

(2)利用算法进行了最优航路距离匹配，使得该算法输出的当前航路距离为最优；

(3)执行未来点距离递推步骤，使得该算法可以迅速匹配未来点距离，保证所有距离航路输出的点距离的正确性；

(4)使用降阶的速度直线建航，使得该算法的运行时间满足实际工程的嵌入式系统应用。

附图说明

图1为按照本发明的距离波门算法的执行步骤示意框图；

图2为按照本发明的距离波门算法的详细执行工作流程示意图；

图3为按照本发明的距离波门算法的其中一个实施例中的实际距离与输出距离曲线图；

图4为按照本发明的距离波门算法的其中一个实施例中的实际速度与递推速度曲线图；

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1-2所示，为按照本发明的距离波门算法的详细执行流程示意图，

由边界条件设置、现在点距离录入、距离建航、现在点距离匹配输出和未来点距离递推计算构成，边界条件设置包括最大速度设置、最大速率变换倍数、航路预失效及失效点数和自适应距离波门初始值，使得该算法在合理范围内进行距离航路曲线的递推计算及输出；

现在点距离录入是指按算法需要的格式输入数据，保证该算法使用的数据正确性；距离建航包括首次速度匹配建航和剩余点再次建航递推；现在点距离匹配输出包括筛选距离航路匹配点及最优匹配距离、最优航路距离输出及存储、距离航路失效判断和第一不匹配距离输出，使得该算法输出的当前航路距离最优；

未来点距离递推计算包括计算距离航路递推速度、计算递推距离并存储和未来点距离波门修正，使得该算法可以更迅速匹配未来点距离；

边界条件设置适用于所有距离航路；

现在点距离录入、距离建航、现在点距离匹配输出和未来点距离递推计算循环运算可解析所有距离航路的所有点距离的正确性。

如无有效距离航路，则对现有距离进行速度计算匹配以建航，建航失败则等待新的距离信息以再次建航；建航成功则进行现在点距离匹配输出及存储；

如有有效距离航路，则对当前距离与距离波门进行匹配，在所有匹配结果中选择最优距离作为该航路当前距离，如所有当前距离均不匹配该航路，则以上次递推的距离作为该航路当前距离；将剩余距离匹配到剩余航路中，如有效距离航路条数不足设置值，则使用匹配完成后的剩余距离进行再次距离建航；

对所有航路匹配完成后将当前已有航路的距离和第一不匹配距离进行输出及存储，从现在点距离录入到现在点距离匹配输出在嵌入式系统中的处理时间≤1ms，以保证该数据在后端系统的实时性；

计算当前点速度，进行速度直线递推计算出未来点的速度，进而计算出未来点的距离值，并将该距离进行存储；同时根据当前距离有效性灵活调整距离波门大小，使得距离波门可根据航路距离有效性进行自适应变化；

循环对所有已有距离航路进行匹配及递推，在两条航路情况下从现在点距离录入到递推完所有航路数据在嵌入式系统中的处理时间≤5ms，比相同条件下建立距离2次曲线递推所用时间大大减小；

逐次判断已有距离航路的失效性，剔除已失效航路，将预失效航路优先级降到最低，从而将备用航路提升为正式航路。

以某跟踪系统在外场对飞机的跟踪测距为实例，说明具体实施方式。已知飞机的飞行速度为V＝200m/s～300m/s，激光测距重频为f＝5Hz。

(1)所述边界条件设置

算法中对目标最大速度留取一定裕量，设置最大速度为Vmax＝350m/s；设置最大速率变换倍数为Bmax＝5，超过一定速度后若连续两点间速率变化量大于5倍时认为当前测距值不在航路上；设置建立航路后，若连续10点无距离回波或距离回波在距离波门以外，该航路失效；设置建立航路后，若连续5点无距离回波或距离回波在距离波门以外，该航路即将失效；设置距离波门最大值为BMmax＝50m；设置距离波门最小值为BMmin＝20m。

(2)现在点距离录入

外部输入的现在点距离必须符合算法要求，具体定义为(数据0～N)：数据0—本次测距结果标志；数据1—A采宽度；数据2—前两个A采间隔，如无第二个A采则为0；数据3～N1—测距值，偶数为单个测距值，奇数为此测距值对应的B采宽度；

注：最大波门、最小波门、航路总条数及测距值个数均可通过算法头文件中的常量更改。因嵌入式系统内存限制，建议设置航路总条数不大于5，距离值个数不大于10(即N1不大于22)。

(3)距离建航

目标航路是一条曲线航路，因一次方程建航递推误差较大，故目前多采用高阶方程建航，而在嵌入式系统中因系统平台运算速度及内存等限制，如果采用高阶方程建航会使得系统处理时间大大加长或内存不足，现将距离曲线y＝ax²+bx+c降阶为速度直线y＝kx+b建航，可有效减轻嵌入式系统的压力。

当有连续3个有效距离点时进行建航，在每点中取出一个有效距离值L₁、L₂、L₃计算相邻2点的差分速度，记为V₁＝L₂-L₁和V₂＝L₃-L₂，如果V₁及V₂满足如下条件时认为当前3个距离值可以建航，存入空航路中。

|V₁|<＝Vmax/f and|V₂|<＝Vmax/f and|V₂-V₁|<＝Vmax/f

建航后将已参与建航的距离值从点数据中删除，并计算存储该航路的最小二乘法参数及当前航路信息：

航路当前点速度V_d＝V₂及距离L_d＝L₃，航路点数Nd＝3，距离波门BM＝BMmax；

最小二乘法参数V₀＝V₁，T₀＝2，V_T＝2*V₁，T₂＝T₀ ²。

当剩余点中的距离值仍可以建航时，同样的方法再次建立新的航路。

(4)现在点距离匹配输出

对点数据中的当前距离依次记为Ln(n＝1、2…N)，以已建航航路序号为顺序，依次对当前距离进行匹配，每条航路均进行最优匹配，并将匹配结果及匹配标志进行存储，如果匹配完成仍有为匹配的距离点，则将第一个未匹配的距离点也进行存储，上述工作完成后将存储的匹配结果及标志输出。

以单个航路为例，匹配方法如下：

将Ln(n＝1、2…N)依次代入公式|Ln-Ld|<＝BM(Ld为递推的未来点距离)，将满足公式的距离记为Lk(k＝1、2…K)，从中找出|Lk-Ld|最小的距离值Lp，即为航路的最佳匹配距离，并记匹配标志为有效，此时记录航路当前点速度Vd＝Lp-Lq(Lq为该航路上一点的匹配距离)及距离Ld＝Lp；如果Lk为空，则将递推的当前距离记为航路最佳匹配距离，并记匹配标志为无效。

将当前航路匹配距离Lp从Ln中删除，剩余距离对下一航路进行匹配，依次匹配完所有航路。

如果航路连续5点无有效匹配距离，则航路将失效，将该航路移至最后(已建航航路中航路序号最大)；如果航路连续10点无有效匹配距离，则航路失效，清除该航路。

(5)未来点距离递推计算

未来点距离递推计算由最小二乘法参数计算、速度直线系数计算、未来点速度计算及未来点距离计算共4步组成。具体公式如下。

最小二乘法参数计算：

V₀＝V₀+V_d,T₀＝T₀+N_d,V_T＝V_T+V_d*N_d,T₂＝T₂+Nd₂

速度直线系数计算：

k＝((N_d-1)*V_T-V₀*T₀)/((N_d-1)*T₂-T₀ ²)

b＝(V₀*T₂-V_T*T₀)/((Nd-1)*T₂-T₀ ²)

未来点速度计算：

Vd＝(Nd+1)*k+b

未来点距离计算：

Ld＝Ld+Vd

将递推的距离Ld存储，在下一点信息到来后进行距离匹配。

如果距离匹配标志有效则波门BM定量减小(最小不能小于BMmin)，反之波门定量增大(最大不能大于BMmax)。

同样的方法依次计算所有航路的递推距离及波门大小。

综上可得到本发明的计算公式、原理及方法。

具体为：

初始化本算法参数，设：

1、2、3…………N为接收点距离符号；

BMmax为距离波门最大值，BMmin为距离波门最小值；

L₁、L₂、L₃……………Ln为接收测距机的点距离信息输入值，其中Ln¹，Ln²，Ln³…Ln^N为每个点距离信息包含的距离值；

若有连续接收点3个点L₁、L₂、L₃，计算相邻2点的查分速度V₁＝L₂ ^m2-L₁ ^m1和V₂＝L₃ ^m3-L₂ ^m2(其中L₁ ^m1，L₂ ^m2，L₃ ^m3分别为L₁，L₂，L₃中的某个距离值)；

若满足：|V₁|<＝Vmax/f and|V₂|<＝Vmax/f and|V₂-V1|<＝Vmax/f条件则可以建航，存入k条航路中，第一个航路可记为k1，建航后将已参与建航的所述有效点距离值L₁ ^m1，L₂ ^m2，L₃ ^m3分别从L₁，L₂，L₃中删除形成{L₁′,L₂′,L₃′}。

第1航路初始信息具体为：

V_d(1)＝V₂，L_d(1)＝L₃，Nd(1)＝3，BM(1)＝BMmax

计算存储该该航路的最小二乘法参数：

V₀(1)＝V₁，T₀(1)＝2，V_T(1)＝2*V₁，T₂(1)＝T₀ ²，以此来计算建航方程y＝kx+b的系数，其中：

k(1)＝((Nd(1)-1)*V_T(1)-V₀(1)*T₀(1))/((Nd(1)-1)*T₂(1)-T₀(1)²)

b(1)＝(V₀(1)*T₂(1)-V_T(1)*T₀(1))/((Nd(1)-1)*T₂(1)-T₀(1)²)，由此完成了首次建航；

若首次建航成功则将L_d(1)作为最优匹配距离输出，否则将当前点距离信息Ln中的距离值Ln^m依次代入公式|Ln^m-L_d(1)|<＝BM(1)，将满足公式的距离记为Ln¹，Ln²…Ln^k，其中k为整数，从中找出|Ln^kLd(1)|最小的距离值Ln^k1，即为航路的最佳匹配距离；然后标记该航路为有效；将Ln中第一个未匹配有效的距离值进行存储，将所述Ln^k1从所述Ln中删除；

每个当前点匹配航路完成后按下方公式对最小二乘法参数进行计算

V₀(m)＝V₀(m-1)+V_d(m-1)，T₀(m)＝T₀(m-1)+Nd(m-1)，

V_T(m)＝V_T(m-1)+V_d(m-1)*Nd(m-1)，

T₂(m)＝T₂(m-1)+Nd(m-1)，

并由此算出速度直线y＝kx+b的系数k、b，进而推算出航路未来点的速度Vd、未来点距离Ld及距离波门BM，具体公式如下(以点m为例)

k(m)＝((Nd(m)-1)*V_T(m)-V₀(m)*T₀(m))/((Nd(m)-1)*T₂(m)-T₀(m)²)，

b(m)＝(V₀(m)*T₂(m)-V_T(m)*T₀(m))/((Nd(m)-1)*T₂(m)-T₀(m)²)；

Vd(m+1)＝k(m)*m+b(m)；

Ld(m+1)＝Ld(m)+Vd(m)；

以上为航路1所需算法的输入数据、建航、匹配及递推公式。

航路1匹配后剩余的点距离信息{L₁′,L₂′,L₃′}再次建航，方法步骤及公式与航路1相同。当前点参数及未来点参数计算方法及公式与航路1相同，以此类推可进行更多航路的建航、匹配及递推。

当所有航路建航或匹配循环完毕，计算出所有的最佳匹配距离，Lp1，………Lpm，Lpm+1，并将当前点信息中剩余的第一个距离值做为第一不匹配距离，将所有航路的匹配距离和第一不匹配距离输出。

图3为按照上述实施例执行后的某航路实际测距值与采用本算法剔除错误信息递推距离值。从图中可以看出，实际测距值中含有跳动较大的错误距离信息，经本算法后输出距离信息平稳。

图4为按照上述实施例执行后的实际测距值差分得到的速度信息值与本算法中间过程采用的速度信息对比。由实际测距值差分得到的速度信息波动很大，而本算法中间过程采用的速度信息比较平稳。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims

1.一种距离波门算法，其特征在于，该算法包括如下步骤：

利用所述Ln进行首次点距离建航，包括如下过程：当连续接收到3个点距离信息L₁、L₂、L₃时执行首次匹配建航，依次计算相邻2点的差分速度记为V₁＝L₂ ^m2-L₁ ^m1和V₂＝L₃ ^m3-L₂ ^m2，其中L₁ ^m1，L₂ ^m2，L₃ ^m3分别为L₁，L₂，L₃中的某个距离值，如果V₁及V₂满足如下条件时认为当前3个距离值可以建航，存入第一空航路中，若L₁、L₂、L₃中的距离值均不满足建航条件则建航失败，等待下一个点距离信息：

建航后将已参与建航的所述有效点距离值L₁ ^m1，L₂ ^m2，L₃ ^m3分别从L₁，L₂，L₃中删除形成{L₁′,L₂′,L₃′}，并计算存储第一航路的最小二乘法参数及当前航路信息，具体为：

最小二乘法参数V₀(1)＝V₁，T₀(1)＝2，V_T(1)＝2*V₁，T₂(1)＝T₀(1)²，其中所述最小二乘法参数用于计算建航方程y＝kx+b的系数，

完成现在点距离匹配输出，其中包括如下步骤：若首次建航成功则将L_d(1)作为最优匹配距离输出，否则将当前点距离信息Ln中的距离值Ln^m依次代入公式|Ln^m-L_d(1)|<＝BM(1)，1≤m≤N，其中m为整数，将满足公式的距离记为Ln¹，Ln²…Ln^k，其中k为整数，从中找出|Ln^k-Ld(1)|最小的距离值Ln^k1，即为航路的最佳匹配距离；然后标记该航路为有效；将Ln中第一个未匹配有效的距离值进行存储，将所述Ln^k1从所述Ln中删除；

其中：V_d(m)＝k(m)*m+b(m)；

L_d(m)＝L_d(m-1)+V_d(m)；

其中k(m)，b(m)为速度直线y＝kx+b的系数

k(m)＝((Nd(m)-1)*V_T(m)-V₀(m)*T₀(m))/((Nd(m)-1)*T₂(m)-T₀(m)²)，

b(m)＝(V₀(m)*T₂(m)-V_T(m)*T₀(m))/((Nd(m)-1)*T₂(m)-T₀(m)²)；

其中Nd(m)为航路点数，V₀(m)，T₀(m)，V_T(m)，T₂(m)为该航路最小二乘法参数；

V₀(m)＝V₀(m-1)+V_d(m-1)，T₀(m)＝T₀(m-1)+Nd(m-1)，

V_T(m)＝V_T(m-1)+V_d(m-1)*Nd(m-1)，T₂(m)＝T₂(m-1)+Nd²(m-1)，

以上为所述第一航路所需算法的输入数据、匹配及递推公式、输出方法；

所述第一航路匹配后剩余的点距离信息{L1′,L2′,L3′}，对其余航路依次匹配或再次建航，方法步骤及公式与所述第一航路相同，其余航路按所述第一航路的方法步骤及公式进行算法运算及输出。