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

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

Info

Publication number
CN108061893A
CN108061893A CN201711344822.1A CN201711344822A CN108061893A CN 108061893 A CN108061893 A CN 108061893A CN 201711344822 A CN201711344822 A CN 201711344822A CN 108061893 A CN108061893 A CN 108061893A
Authority
CN
China
Prior art keywords
point
target
curvature
real
curve
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.)
Granted
Application number
CN201711344822.1A
Other languages
English (en)
Other versions
CN108061893B (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 Chao Yan Intelligent Technology Co Ltd
Original Assignee
Nanjing Chao Yan 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 Chao Yan Intelligent Technology Co Ltd filed Critical Nanjing Chao Yan 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

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点的夹角Δα， K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
最后计算Pa+1点的曲率ξ：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)，将各个点配置于矩阵中，
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：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点的夹角Δα，
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
最后计算Pa+1点的曲率ξ：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)，将各个点配置于矩阵中，
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：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点的夹角Δα，具体地，
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；由于选取的三个点位置相近，夹角很小因此Δα≈tanΔα；同时每一次都是选取连续的三个点，而转角切线斜率作为中间变量，因此并未对转角切线斜率进行更多标号区分，仅作为公式的中间变量；转角切线斜率由检测到的目标点的相对坐标而来，两者为差值，因此无需对坐标系进行更复杂的定义，仅需有明确位置数据信息即可，然后计算弧长；
计算Pa+2点到Pa+1点的弧长Δs，这里由于选取的点较近，因此可以认为两点之间的直线即为弧线，同时本发明选取了Pa+2点到Pa+1点的弧线，原则上也可以选取Pa+1点到Pa点，或者计算Pa+2点到Pa+1点和Pa+1点到Pa点两者的平均值，最后得到Pa+1点对应的弧线长度；
在得到夹角Δα和弧长Δs的数据后即可计算的到曲率：
最后计算Pa+1点的曲率ξ：
在下一个目标点即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相同；
[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点的夹角矩阵[Δα]，[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，由于选取的各个目标的三个点位置相近，夹角很小因此Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
同时本发明选取了Pa+2点到Pa+1点的弧线，原则上也可以选取Pa+1点到Pa点，或者计算Pa+2点到Pa+1点和Pa+1点到Pa点两者的平均值，最后得到Pa+1点对应的弧线长度；
最后计算Pa+1点的曲率集[ξ]：
如图3所示，本发明采用的计算工具为：
主机配置：
服务器配置：CPU(AMD Athon 4核)、内存(8G)、操作系统(Ubuntu 12.04)，硬盘(1T)
系统使用Java高并发网络环境，系统模拟1000个目标向服务器发数据，服务器接收到的数据通过接口传给C语言层处理，C语言中将数据存入数组中，当达到一定数目后，统一放入运算区进行计算。各种计算方式时间结果如下表：
表1
在众目标的计算中，本案重在利用其矩阵能力和计算机的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点的夹角Δα，
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近，夹角很小因此Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
最后计算Pa+1点的曲率ξ：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)，将各个点配置于矩阵中，
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近，夹角很小因此Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：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点的夹角Δα， K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
最后计算Pa+1点的曲率ξ：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)，将各个点配置于矩阵中，
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：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点的夹角Δα，
K1为Pa+1点到Pa点的转角切线斜率，K2为Pa+2点到Pa+1点的转角切线斜率；选取的三个点位置相近Δα≈tanΔα；
计算Pa+2点到Pa+1点的弧长Δs，
最后计算Pa+1点的曲率ξ：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)，将各个点配置于矩阵中，
[X]a为m个目标在第a个点的所有x轴相对坐标，[Y]a为m个目标在第a个点的所有y轴相对坐标；
然后通过各个目标的三个点的相对坐标计算Pa+1点的夹角矩阵[Δα]，
[K]1为m个目标的Pa+1点到Pa点的转角切线斜率矩阵，[K]2为Pa+2点到Pa+1点的转角切线斜率矩阵，选取的各个目标的三个点位置相近Δα≈tanΔα；
计算各个目标的Pa+2点到Pa+1点的弧长矩阵[Δs]，首先计算各个目标的Pa+2点到Pa+1点的弧长Δs，
然后m个目标的各个弧长Δs依次放入弧长矩阵中组成所述弧长矩阵[Δs]；
最后计算Pa+1点的曲率集[ξ]：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 true CN108061893A (zh) 2018-05-22
CN108061893B 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)

Cited By (2)

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

Citations (5)

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

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20150081211A1 (en) * 2013-09-17 2015-03-19 GM Global Technologies Operations LLC Sensor-aided vehicle positioning system
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 奇瑞汽车股份有限公司 一种无人驾驶汽车的路径跟踪方法

Cited By (2)

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

Also Published As

Publication number Publication date
CN108061893B (zh) 2020-10-27

Similar Documents

Publication Publication Date Title
CN109933067A (zh) 一种基于遗传算法和粒子群算法的无人艇避碰方法
CN102997911B (zh) 一种被动传感器组网探测多目标方法
Arzamendia et al. Comparison of eulerian and hamiltonian circuits for evolutionary-based path planning of an autonomous surface vehicle for monitoring ypacarai lake
Du et al. Integrating spatial relations into case-based reasoning to solve geographic problems
CN105447574B (zh) 一种辅助截断粒子滤波方法、装置及目标跟踪方法及装置
Ismail et al. Using the Bees Algorithm to solve combinatorial optimisation problems for TSPLIB
Kwok et al. Evolutionary computing based mobile robot localization
Zieliński et al. 3D robotic navigation using a vision-based deep reinforcement learning model
CN108061893A (zh) 一种基于目标航迹偏航避碰预警方法及装置
CN106441279A (zh) 基于自主定位和边缘探索的机器人定位方法、系统
CN104199022A (zh) 一种基于目标模态估计的临近空间高超声速目标跟踪方法
Wang et al. Research on vessel speed heading and collision detection method based on AIS data
CN106546976A (zh) 一种基于长周期非均匀采样目标跟踪处理方法及装置
CN115600733A (zh) 一种船舶航迹预测方法及装置
CN103810382A (zh) 机载分布式多传感器两级数据融合策略选择方法
Lochmatter et al. Theoretical analysis of three bio-inspired plume tracking algorithms
Jang et al. Development of Ship Route-Planning Algorithm Based on Rapidly-Exploring Random Tree (RRT*) Using Designated Space
CN104931932A (zh) 一种改进的去偏坐标转换卡尔曼滤波方法
CN110114195A (zh) 动作转移装置、动作转移方法和存储动作转移程序的非暂时性计算机可读介质
Tarasov et al. A hybrid method for assessment of soil pollutants spatial distribution
Xiong et al. A time domain-based iterative method for helicopter maritime search area planning and construction of the simulation environment
CN113376626A (zh) 基于immpda算法的高机动目标跟踪方法
Yakubovich et al. A methodology for optimizing the objective function in a high-dimensional space using the swarm intelligence algorithm
Xu et al. A cell potential and motion pattern driven multi-robot coverage path planning algorithm
CN102707278B (zh) 奇异值分解的多目标跟踪方法

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