CN108061893B - 一种基于目标航迹偏航避碰预警方法及装置 - Google Patents

一种基于目标航迹偏航避碰预警方法及装置 Download PDF

Info

Publication number
CN108061893B
CN108061893B CN201711344822.1A CN201711344822A CN108061893B CN 108061893 B CN108061893 B CN 108061893B CN 201711344822 A CN201711344822 A CN 201711344822A CN 108061893 B CN108061893 B CN 108061893B
Authority
CN
China
Prior art keywords
point
curvature
target
points
real
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201711344822.1A
Other languages
English (en)
Other versions
CN108061893A (zh
Inventor
鲁晓军
黄留进
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing Chaoyan Intelligent Technology Co ltd
Original Assignee
Nanjing Chaoyan Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing Chaoyan Intelligent Technology Co ltd filed Critical Nanjing Chaoyan Intelligent Technology Co ltd
Priority to CN201711344822.1A priority Critical patent/CN108061893B/zh
Publication of CN108061893A publication Critical patent/CN108061893A/zh
Application granted granted Critical
Publication of CN108061893B publication Critical patent/CN108061893B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/933—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of aircraft or spacecraft
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/886—Radar or analogous systems specially adapted for specific applications for alarm systems
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/88—Radar or analogous systems specially adapted for specific applications
    • G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
    • G01S13/937—Radar or analogous systems specially adapted for specific applications for anti-collision purposes of marine craft

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • Electromagnetism (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • General Physics & Mathematics (AREA)
  • Ocean & Marine Engineering (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Traffic Control Systems (AREA)

Abstract

一种基于目标航迹偏航避碰预警方法及装置，检测目标的实时航线，并计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，在计算实时航线某一点的曲率时，通过提取该点的前一坐标和后一坐标计算，通过快速取点的方式计算目标运行轨迹的曲率，避免了复杂的曲线运算，避免使用三角函数计算，无需预测即可实现偏航预警；直接使用Rsqrtps指令进行开方倒数运算，避免的复杂的C语言函数运算；算法中使用矩阵参与运算，数据可以批量处理目标数据，同时充分利用CPU中的SIMD指令功能，在Intel平台架构下使用SSE运算，大大提高了运算效率。

Description

一种基于目标航迹偏航避碰预警方法及装置
技术领域
本发明涉及一种基于目标航迹偏航避碰预警方法及装置。
背景技术
在航空，航海等活动中，雷达系统或监控中心经常要对目标的航向进行预判，进而对目标可能会偏离航向等问题进行预警。对目标的轨迹进行预测并指出其可能发生的偏航碰撞风险进行预警是雷达系统或云平台系统必备的要求。
现有的预警方法中，神经网络、模拟退火的算法具有较高的准确性，但它们需要大量的样本数据进行训练，并且运算时间较长，不适合做实时大量目标的预警，实际操作中经常用到的卡尔曼滤波法，计算简单，实时性较强，但卡尔曼滤波采用的是目标物距离设定航线的长度来判断是否偏航，往往会出现刚开始偏离角度大但偏离长度小不会引起报警，当偏航长度大引起报警时又造成反馈不及时的问题，另外对于大量目标的预测存在数据处理时间长，计算机运算性能利用不充分的问题。
发明内容
为克服现有技术问题，本发明提供一种基于目标航迹偏航避碰预警方法及装置。
一种基于目标航迹偏航避碰预警方法，检测目标的实时航线，并计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，在计算实时航线某一点的曲率时，通过提取该点的前一坐标和后一坐标计算：
在计算Pa+1点的曲率时，
首先提取规定时间内实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pa(xa,ya)，Pa+1(xa+1,ya+1)，Pa+2(xa+2,ya+2)；
然后通过三个点的相对坐标计算Pa+1点的夹角Δα，
Figure BDA0001509105560000011
Figure BDA0001509105560000021
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000022
最后计算Pa+1点的曲率ξ：
Figure BDA0001509105560000023
a为选取的目标点的序号，a＝0，1，2，3……。
优选的是，当目标为多个时，检测各个目标的实时航线，并将各个目标的实时航线的曲率配置到曲率集[ξ]中，将各个实时航线的曲率与相应的设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定该目标偏离航线；
以Pm,a(xm,a,ym,a)为第m个目标的第a个点的相对坐标Pa，每个目标的提取频率相同，在计算时各个点的曲率时，
首先提取规定时间内各个实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pm,a(xm,a,ym,a)，Pm,a+1(xm,a+1,ym,a+1)，Pm,a+2(xm,a+2,ym,a+2)，将各个点配置于矩阵中，
Figure BDA0001509105560000024
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
Figure BDA0001509105560000025
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000026
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：
Figure BDA0001509105560000031
a为选取的目标点的序号，a＝0，1，2，3……；m为目标数量序号，m＝1，2，3……。
优选的是，将检测到的目标相对坐标通过SSE指令集中的Movd指令存储到xmm寄存器中。
优选的是，通过Cvtdq2ps指令将寄存器中的整型数转换为浮点数，然后通过Divps指令进行浮点除法运算得到相应的转角切线，并将转角切线斜率存入数组存储器中。
优选的是，通过Mulps指令进行浮现数同时相乘，然后通过Addps进行浮点数同时相加计算后再通过Rsqrtps指令求解得到开方倒数得到弧长数组，并将弧长数组存入数组存储器中；
最后通过Mulps指令进行浮点乘法运算得到曲率数组。
本发明同时提供一种基于目标航迹偏航避碰预警装置，
包括检测端、计算端和传输端；
所述检测端用于检测目标的实时航线，得到相应的实时相对坐标点，并将坐标点发送给计算端，所述检测端提取规定时间内实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的前一个和后一个点的相对坐标，分别为Pa(xa,ya)Pa+1(xa+1,ya+1)Pa+2(xa+2,ya+2)；
所述计算端用于计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，并将信息发送给传输端；所述计算端首先通过三个点的相对坐标计算Pa+1点的夹角Δα，
Figure BDA0001509105560000032
Figure BDA0001509105560000033
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000034
最后计算Pa+1点的曲率ξ：
Figure BDA0001509105560000035
a为选取的目标点的序号，a＝0，1，2，3……；
所述传输端用于传输警告信息给目标，进行目标航迹偏航避碰预警。
优选的是，当目标为多个时，所述检测端检测各个目标的实时航线，以Pm,a(xm,a,ym,a)为第m个目标的第a个点的相对坐标Pa，每个目标的提取频率相同；
所述计算端用于计算实时各个实时航线的曲率，并将各个目标的实时航线的曲率配置到曲率集[ξ]中，对各个实时航线的曲率与设定航线的曲率进行比较；所述计算端首先提取规定时间内各个实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pm,a(xm,a,ym,a)，Pm,a+1(xm,a+1,ym,a+1)，Pm,a+2(xm,a+2,ym,a+2)，将各个点配置于矩阵中，
Figure BDA0001509105560000041
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
Figure BDA0001509105560000042
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000043
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：
Figure BDA0001509105560000044
a为选取的目标点的序号，a＝0，1，2，3……，m为目标数量序号，m＝1，2，3……。
优选的是，所述计算端将检测到的目标相对坐标通过SSE指令集中的Movd指令存储到xmm寄存器中。
优选的是，所述计算端通过Cvtdq2ps指令将寄存器中的整型数转换为浮点数，然后通过Divps指令进行浮点除法运算得到相应的转角切线，并将转角切线斜率存入数组存储器中。
优选的是，所述计算端通过Mulps指令进行浮现数同时相乘，然后通过Addps进行浮点数同时相加计算后再通过Rsqrtps指令求解得到开方倒数得到弧长数组，并将弧长数组存入数组存储器中；
最后通过Mulps指令进行浮点乘法运算得到曲率数组。
本发明的有益效果：
本发明提供的基于目标航迹偏航避碰预警方法及装置，通过快速取点的方式计算目标运行轨迹的曲率，避免了复杂的曲线运算，避免使用三角函数计算，无需预测即可实现偏航预警；直接使用Rsqrtps指令进行开方倒数运算，避免的复杂的C语言函数运算；算法中使用矩阵参与运算，数据可以批量处理目标数据，同时充分利用CPU中的SIMD指令功能，在Intel平台架构下使用SSE运算，大大提高了运算效率。
附图说明
图1为本发明的基于目标航迹偏航避碰预警方法的流程示意图；
图2为本发明的基于目标航迹偏航避碰预警方法的目标航迹示意图；
图3为本发明的基于目标航迹偏航避碰预警方法的指令集计算示意图。
具体实施方式
如图1和图2所示，一种基于目标航迹偏航避碰预警方法，在实际应用中，目标往往会定时向系统(塔台或水域监控)发送自身的定位信息，系统也会通过雷达实时扫描目标物的位置，每个目标在各个时间点的坐标，这些坐标非常密集，将这些点连起来，便构成了目标的航迹，它由众多弧线构成，利用这些点之间的连线来标识相应的弧线，用相邻的点的连线代替这段弧线，通过目标自身的定位或者雷达扫描检测目标的实时航线，并计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，在计算实时航线某一点的曲率时，通过提取该点的前一坐标和后一坐标计算，同时选取的位置相近保证检测的实时性。
当计算Pa+1点的曲率时，提取该点的前一坐标Pa和后一坐标Pa+2进行计算：
首先提取规定时间内实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pa(xa,ya)，Pa+1(xa+1,ya+1)，Pa+2(xa+2,ya+2)；a为选取的目标点的序号，a＝0，1，2，3……；
然后通过三个点的相对坐标计算Pa+1点的夹角Δα，具体地，
Figure BDA0001509105560000051
Figure BDA0001509105560000061
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；由于选取的三个点位置相近，夹角很小因此Δα≈tanΔα；同时每一次都是选取连续的三个点，而转角切线斜率作为中间变量，因此并未对转角切线斜率进行更多标号区分，仅作为公式的中间变量；转角切线斜率由检测到的目标点的相对坐标而来，两者为差值，因此无需对坐标系进行更复杂的定义，仅需有明确位置数据信息即可，然后计算弧长；
计算Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000062
这里由于选取的点较近，因此可以认为两点之间的直线即为弧线，同时本发明选取了Pa+2点到Pa+1点的弧线，原则上也可以选取Pa+1点到Pa点，或者计算Pa+2点到Pa+1点和Pa+1点到Pa点两者的平均值，最后得到Pa+1点对应的弧线长度；
在得到夹角Δα和弧长Δs的数据后即可计算的到曲率：
最后计算Pa+1点的曲率ξ：
Figure BDA0001509105560000063
在下一个目标点即Pa+3点到来后，即可计算Pa+2点的曲率，如此类推，每次读取三个目标点，待新点读入后，再进行计算，本方法只用到浮点数的四则运算，不涉及三角函数等复杂的计算，故在计算机中较容易实现。
在实际应用中，例如港口监控和空管，需要对目标的出入港航线进行规划，针对每个目标的航迹，管理员会规划一条路线保存在系统中，系统会随时监控目标位置并及时发现偏航等情况，假定有M个目标，每个目标都被规划了各自的航迹，需要系统在极短时间内预测出这N个目标的未来航向并做出偏离判断，当M具备一定规模时依然进行单个计算，如M>1000，将会给系统带来极大的运算量，因此需要将各个数据编入到数组中进行集中运算。
当目标为多个时，检测各个目标的实时航线，并将各个目标的实时航线的曲率配置到曲率集[ξ]中，将各个实时航线的曲率与相应的设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定该目标偏离航线；
以Pm,a(xm,a,ym,a)为第m个目标的第a个点的相对坐标Pa，每个目标的提取频率相同，在计算时各个点的曲率时，
首先提取规定时间内各个实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为P1,a(x1,a,y1,a)P1,a+1(x1,a+1,y1,a+1)P1,a+2(x1,a+2,y1,a+2)，
P2,a(x2,a,y2,a)P2,a+1(x2,a+1,y2,a+1)P2,a+2(x2,a+2,y2,a+2)......Pm,a(xm,a,ym,a)Pm,a+1(xm,a+1,ym,a+1)
Pm,a+2(xm,a+2,ym,a+2)，每个目标选取三个点，并将各个点配置于矩阵中，a为选取的单个目标点的序号，a＝0，1，2，3……；m为目标数量序号，m＝1，2，3……；相同批次采集的序号a相同；
Figure BDA0001509105560000071
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；即将同次时间点取点的各个值x轴坐标和y轴坐标放在同一个矩阵中的标识第a个点的矩阵中，以此类推下一个放在第a+1个点的矩阵中，如此每次至少采用三组矩阵[X]a、[Y]a，[X]a+1、[Y]a+1，[X]a+2、[Y]a+2；每个目标的三个点均分别配置在相应目标的三组相应矩阵中；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
Figure BDA0001509105560000072
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，由于选取的各个目标的三个点位置相近，夹角很小因此Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000073
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
同时本发明选取了Pa+2点到Pa+1点的弧线，原则上也可以选取Pa+1点到Pa点，或者计算Pa+2点到Pa+1点和Pa+1点到Pa点两者的平均值，最后得到Pa+1点对应的弧线长度；
最后计算Pa+1点的曲率集[ξ]：
Figure BDA0001509105560000074
如图3所示，本发明采用的计算工具为：
主机配置：
服务器配置：CPU(AMD Athon 4核)、内存(8G)、操作系统(Ubuntu 12.04)，硬盘(1T)
系统使用Java高并发网络环境，系统模拟1000个目标向服务器发数据，服务器接收到的数据通过接口传给C语言层处理，C语言中将数据存入数组中，当达到一定数目后，统一放入运算区进行计算。各种计算方式时间结果如下表：
表1
Figure BDA0001509105560000081
在众目标的计算中，本案重在利用其矩阵能力和计算机的SIMD指令进行加速计算。
SSE指令集是Intel公司为X86-X64CPU开发的单指令多数据(SIMD)指令集，其特点是一条指令可处理多个浮点数运算，在发明中主要用到的指令如表2：
表2
指令名称 指令用途
Movd 将缓冲区的数据移动到SSE指令的xmm寄存器中
Rsqrtps 计算4个浮点数的开发倒数
Divps 计算4组浮点数除法
Cvtdq2ps 将4个整型数转化成4个浮点数
Mulps 4个浮点数同时进行乘法
AddPs 4个浮点数同时进行加法
Movq 将结果存入结果缓冲区
在实施过程中，设定若干数组，分别存放各目标的坐标和临时变量，每个浮点数为32位，在运算时将数据以矩阵向量形式传送给SSE指令中的Xmm寄存器，每次可以传四组，然后调用SIMD指令进行计算。算法的实现过程和调用SSE指令的使用方式如图3所示，
将检测到的目标相对坐标通过SSE指令集中的Movd指令存储到xmm寄存器中。
通过Cvtdq2ps指令将寄存器中的整型数转换为浮点数，然后通过Divps指令进行浮点除法运算得到相应的转角切线，并将转角切线斜率存入数组存储器中。
通过Mulps指令进行浮现数同时相乘，然后通过Addps进行浮点数同时相加计算后再通过Rsqrtps指令求解得到开方倒数得到弧长数组，并将弧长数组存入数组存储器中；
最后通过Mulps指令进行浮点乘法运算得到曲率数组；
最后将曲率数组存至曲率集中进行比较。
本发明同时提供一种基于目标航迹偏航避碰预警装置，
包括检测端、计算端和传输端；
所述检测端用于检测目标的实时航线，得到相应的实时相对坐标点，并将坐标点发送给计算端，所述检测端提取规定时间内实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的前一个和后一个点的相对坐标，分别为Pa(xa,ya)Pa+1(xa+1,ya+1)Pa+2(xa+2,ya+2)；
所述计算端用于计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，并将信息发送给传输端；所述计算端首先通过三个点的相对坐标计算Pa+1点的夹角Δα，
Figure BDA0001509105560000091
Figure BDA0001509105560000092
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近，夹角很小因此Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000093
最后计算Pa+1点的曲率ξ：
Figure BDA0001509105560000094
a为选取的目标点的序号，a＝0，1，2，3……；
所述传输端用于传输警告信息给目标，进行目标航迹偏航避碰预警。
当目标为多个时，所述检测端检测各个目标的实时航线，以Pm,a(xm,a,ym,a)为第m个目标的第a个点的相对坐标Pa，每个目标的提取频率相同；
所述计算端用于计算实时各个实时航线的曲率，并将各个目标的实时航线的曲率配置到曲率集[ξ]中，对各个实时航线的曲率与设定航线的曲率进行比较；所述计算端首先提取规定时间内各个实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pm,a(xm,a,ym,a)，Pm,a+1(xm,a+1,ym,a+1)，Pm,a+2(xm,a+2,ym,a+2)，将各个点配置于矩阵中，
Figure BDA0001509105560000095
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
Figure BDA0001509105560000101
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近，夹角很小因此Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
Figure BDA0001509105560000102
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：
Figure BDA0001509105560000103
a为选取的目标点的序号，a＝0，1，2，3……，m为目标数量序号，m＝1，2，3……。
所述计算端将检测到的目标相对坐标通过SSE指令集中的Movd指令存储到xmm寄存器中。
所述计算端通过Cvtdq2ps指令将寄存器中的整型数转换为浮点数，然后通过Divps指令进行浮点除法运算得到相应的转角切线，并将转角切线斜率存入数组存储器中。
所述计算端通过Mulps指令进行浮现数同时相乘，然后通过Addps进行浮点数同时相加计算后再通过Rsqrtps指令求解得到开方倒数得到弧长数组，并将弧长数组存入数组存储器中；
最后通过Mulps指令进行浮点乘法运算得到曲率数组。
以上所述的实施例仅仅是对本发明的优选实施方式进行描述，并非对本发明的范围进行限定，在不脱离本发明设计精神的前提下，本领域普通技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明权利要求书确定的保护范围内。

Claims (10)

1.一种基于目标航迹偏航避碰预警方法，其特征在于：
检测目标的实时航线，并计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，在计算实时航线某一点的曲率时，通过提取该点的前一坐标和后一坐标计算：
在计算Pa+1点的曲率时，
首先提取规定时间内实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pa(xa，ya)，Pa+1(xa+1，ya+1)，Pa+2(xa+2，ya+2)；
然后通过三个点的相对坐标计算Pa+1点的夹角Δα，
Figure FDA0002612613890000011
Figure FDA0002612613890000012
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
Figure FDA0002612613890000013
最后计算Pa+1点的曲率ξ：
Figure FDA0002612613890000014
a为选取的目标点的序号，a＝0，1，2，3……。
2.根据权利要求1所述的基于目标航迹偏航避碰预警方法，其特征在于：
当目标为多个时，检测各个目标的实时航线，并将各个目标的实时航线的曲率配置到曲率集[ξ]中，将各个实时航线的曲率与相应的设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定该目标偏离航线；
以Pm，a(xm，a，ym，a)为第m个目标的第a个点的相对坐标Pa，每个目标的提取频率相同，在计算各个点的曲率时，
首先提取规定时间内各个实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pm，a(xm，a，ym，a)，Pm，a+1(xm，a+1，ym，a+1)，Pm，a+2(xm，a+2，ym，a+2)，将各个点配置于矩阵中，
Figure FDA0002612613890000015
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
Figure FDA0002612613890000016
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
Figure FDA0002612613890000017
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：
Figure FDA0002612613890000021
a为选取的目标点的序号，a＝0，1，2，3……；m为目标数量序号，m＝1，2，3……。
3.根据权利要求1或2所述的基于目标航迹偏航避碰预警方法，其特征在于：
将检测到的目标相对坐标通过SSE指令集中的Movd指令存储到xmm寄存器中。
4.根据权利要求3所述的基于目标航迹偏航避碰预警方法，其特征在于：
通过Cvtdq2ps指令将寄存器中的整型数转换为浮点数，然后通过Divps指令进行浮点除法运算得到相应的转角切线，并将转角切线斜率存入数组存储器中。
5.根据权利要求4所述的基于目标航迹偏航避碰预警方法，其特征在于：
通过Mulps指令进行浮现数同时相乘，然后通过Addps进行浮点数同时相加计算后再通过Rsqrtps指令求解得到开方倒数得到弧长数组，并将弧长数组存入数组存储器中；
最后通过Mulps指令进行浮点乘法运算得到曲率数组。
6.一种基于目标航迹偏航避碰预警装置，其特征在于：
包括检测端、计算端和传输端；
所述检测端用于检测目标的实时航线，得到相应的实时相对坐标点，并将坐标点发送给计算端，所述检测端提取规定时间内实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的前一个和后一个点的相对坐标，分别为Pa(xa，ya)，Pa+1(xa+1，ya+1)，Pa+2(xa+2，ya+2)；
所述计算端用于计算实时航线的曲率，将实时航线的曲率与设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定目标偏离航线，并将信息发送给传输端；所述计算端首先通过三个点的相对坐标计算Pa+1点的夹角Δα，
Figure FDA0002612613890000022
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
Figure FDA0002612613890000023
最后计算Pa+1点的曲率ξ：
Figure FDA0002612613890000024
a为选取的目标点的序号，a＝0，1，2，3……；
所述传输端用于传输警告信息给目标，进行目标航迹偏航避碰预警。
7.根据权利要求6所述的基于目标航迹偏航避碰预警装置，其特征在于：
当目标为多个时，检测各个目标的实时航线，并将各个目标的实时航线的曲率配置到曲率集[ξ]中，将各个实时航线的曲率与相应的设定航线的曲率进行比较，当实时航线的曲率偏离设定航线的曲率阈值范围时判定该目标偏离航线；
以Pm，a(xm，a，ym，a)为第m个目标的第a个点的相对坐标Pa，每个目标的提取频率相同，在计算时各个点的曲率时，
首先提取规定时间内各个实时航线上Pa+1点的相对坐标以及与Pa+1点相邻连续的两个点的相对坐标，分别为Pm，a(xm，a，ym，a)，Pm，a+1(xm，a+1，ym，a+1)，Pm，a+2(xm，a+2，ym，a+2)，将各个点配置于矩阵中，
Figure FDA0002612613890000031
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
Figure FDA0002612613890000032
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
Figure FDA0002612613890000033
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：
Figure FDA0002612613890000034
a为选取的目标点的序号，a＝0，1，2，3……；m为目标数量序号，m＝1，2，3……。
8.根据权利要求6或7所述的基于目标航迹偏航避碰预警装置，其特征在于：
所述计算端将检测到的目标相对坐标通过SSE指令集中的Movd指令存储到xmm寄存器中。
9.根据权利要求8所述的基于目标航迹偏航避碰预警装置，其特征在于：
所述计算端通过Cvtdq2ps指令将寄存器中的整型数转换为浮点数，然后通过Divps指令进行浮点除法运算得到相应的转角切线，并将转角切线斜率存入数组存储器中。
10.根据权利要求9所述的基于目标航迹偏航避碰预警装置，其特征在于：
所述计算端通过Mulps指令进行浮现数同时相乘，然后通过Addps进行浮点数同时相加计算后再通过Rsqrtps指令求解得到开方倒数得到弧长数组，并将弧长数组存入数组存储器中；
最后通过Mulps指令进行浮点乘法运算得到曲率数组。
CN201711344822.1A 2017-12-15 2017-12-15 一种基于目标航迹偏航避碰预警方法及装置 Active CN108061893B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201711344822.1A CN108061893B (zh) 2017-12-15 2017-12-15 一种基于目标航迹偏航避碰预警方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201711344822.1A CN108061893B (zh) 2017-12-15 2017-12-15 一种基于目标航迹偏航避碰预警方法及装置

Publications (2)

Publication Number Publication Date
CN108061893A CN108061893A (zh) 2018-05-22
CN108061893B true CN108061893B (zh) 2020-10-27

Family

ID=62138953

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201711344822.1A Active CN108061893B (zh) 2017-12-15 2017-12-15 一种基于目标航迹偏航避碰预警方法及装置

Country Status (1)

Country Link
CN (1) CN108061893B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109960774A (zh) * 2019-02-13 2019-07-02 浩亚信息科技有限公司 一种目视飞行航线实时测算方法、电子设备、存储介质
CN112419787B (zh) * 2020-11-16 2022-04-19 武汉理工大学 一种船舶内河航行预警与辅助避碰方法及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103760908A (zh) * 2014-01-03 2014-04-30 北京控制工程研究所 一种巡视器闭环跟踪控制方法
CN105109491A (zh) * 2015-08-05 2015-12-02 江苏大学 一种基于弯道纵向曲率的车辆行驶航向预测装置及预测方法
CN106909151A (zh) * 2017-01-22 2017-06-30 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的路径规划及其控制方法
CN107037818A (zh) * 2017-06-22 2017-08-11 奇瑞汽车股份有限公司 一种无人驾驶汽车的路径跟踪方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9435653B2 (en) * 2013-09-17 2016-09-06 GM Global Technology Operations LLC Sensor-aided vehicle positioning system

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103760908A (zh) * 2014-01-03 2014-04-30 北京控制工程研究所 一种巡视器闭环跟踪控制方法
CN105109491A (zh) * 2015-08-05 2015-12-02 江苏大学 一种基于弯道纵向曲率的车辆行驶航向预测装置及预测方法
CN106909151A (zh) * 2017-01-22 2017-06-30 无锡卡尔曼导航技术有限公司 用于农机无人驾驶的路径规划及其控制方法
CN107037818A (zh) * 2017-06-22 2017-08-11 奇瑞汽车股份有限公司 一种无人驾驶汽车的路径跟踪方法

Also Published As

Publication number Publication date
CN108061893A (zh) 2018-05-22

Similar Documents

Publication Publication Date Title
CN109374986B (zh) 一种基于聚类分析与网格搜索的雷电定位方法及系统
CN108645415A (zh) 一种船舶航迹预测方法
CN111695299B (zh) 一种中尺度涡轨迹预测方法
CN101482607B (zh) 应用于无线移动传感器网络的目标跟踪方法及装置
CN104199022B (zh) 一种基于目标模态估计的临近空间高超声速目标跟踪方法
CN108061893B (zh) 一种基于目标航迹偏航避碰预警方法及装置
CN103220777A (zh) 一种移动设备定位系统
CN110146087B (zh) 一种基于动态规划思想的船舶路径规划方法
CN101435865A (zh) 一种无需测距的无线传感器网络节点定位方法
CN104519571A (zh) 一种基于rss的室内定位方法
CN109145446B (zh) 一种基于模态应变能和卷积神经网络的结构损伤识别方法
CN111159924A (zh) 用于预测漂移轨迹的方法和装置
CN106896363A (zh) 一种水下目标主动跟踪航迹起始方法
CN104933213A (zh) 基于空间映射的大规模相控天线阵列宽角扫描优化方法
CN104535960A (zh) 一种基于rfid的室内快速定位方法
CN103809161B (zh) 雷达网抗距离欺骗+soj复合干扰方法
CN103885867B (zh) 一种模拟电路性能的在线评价方法
CN102981160B (zh) 一种确定空中目标航迹的方法及装置
KR101970090B1 (ko) 시계열 데이터 검색 장치 및 기록 매체에 저장된 시계열 데이터 검색 프로그램
Sun et al. A novel path planning method for multiple USVs to collect seabed-based data
CN112307685B (zh) 水声传感网中基于高斯混合模型的流体运动边界预测方法
CN109116300A (zh) 一种基于非充分指纹信息的极限学习定位方法
CN107526712A (zh) 基于到达时间差的多目标定位外部逼近近似凸优化算法
CN113836661B (zh) 时间预测方法、模型训练方法、相关装置及电子设备
Ghozali et al. Indoor positioning system using regression-based fingerprint method

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant