CN107024679B - 无人飞行器室内定位计算方法及装置 - Google Patents
无人飞行器室内定位计算方法及装置 Download PDFInfo
- Publication number
- CN107024679B CN107024679B CN201710339762.8A CN201710339762A CN107024679B CN 107024679 B CN107024679 B CN 107024679B CN 201710339762 A CN201710339762 A CN 201710339762A CN 107024679 B CN107024679 B CN 107024679B
- Authority
- CN
- China
- Prior art keywords
- ultrasonic
- receiver
- unmanned aerial
- aerial vehicle
- ultrasonic receiver
- 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
Links
- 238000004364 calculation method Methods 0.000 title claims abstract description 39
- 230000001360 synchronised effect Effects 0.000 claims abstract description 23
- 238000000034 method Methods 0.000 claims abstract description 15
- 230000005540 biological transmission Effects 0.000 claims description 12
- 238000010276 construction Methods 0.000 claims description 3
- 102220079853 rs797045893 Human genes 0.000 abstract 1
- 239000000523 sample Substances 0.000 description 13
- 238000005516 engineering process Methods 0.000 description 6
- 238000005259 measurement Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 238000005457 optimization Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
- 238000000844 transformation Methods 0.000 description 1
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
- G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
- G01S5/18—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using ultrasonic, sonic, or infrasonic waves
- G01S5/22—Position of source determined by co-ordinating a plurality of position lines defined by path-difference measurements
Landscapes
- Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Measurement Of Velocity Or Position Using Acoustic Or Ultrasonic Waves (AREA)
Abstract
本发明提供一种无人飞行器室内定位计算方法及装置。该方法包括以下步骤:S10设置公共接收器至指定位置;S20设置N个超声波接收器在室内不同位置,且N个超声波接收器至公共接收器的距离小于预设距离值,以第一超声波接收器为坐标轴的原点,从N个超声波接收器中另选取第二超声波接收器和第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上;S30公共接收器接收到辅助同步定位信号时,记为开始时刻T1;S40N个超声波接收器接收到超声波信号时,分别记录结束时刻T2、T3…TN+1;S50计算无人飞行器分别至N个超声波接收器的距离;S60建立计算无人飞行器位置坐标的方程组,计算无人飞行器的当前坐标。
Description
技术领域
本发明涉及无人飞行器技术领域,特别是涉及一种无人飞行器室内定位计算方法及装置。
背景技术
无人机是一种有动力、可控制、能携带多种任务设备、执行多种任务并能重复使用的飞行器。能够利用无线遥控设备和自身的控制装置进行控制的不载人飞行器,例如无人直升机、无人固定翼机、无人伞翼机等等。该无人机可以用于挂载拍摄装置,用于航拍、测绘、侦查等等。
目前,随着无人机应用越来越普及,出现了更多航飞速度更快、飞行高度更高、更加操作灵活、体积更小的无人飞行器。而随着飞行速度及飞行高度的不断增加,加上飞行器本体越来越小,对飞行器本身的性能要求也越来越高,具体的,飞行器飞行速度及高度的提升,将导致飞行器根据操控要求实现悬停的难度提高;小型飞行器在室内或空间较为隐蔽的地方飞行时由于无法实现卫星定位或卫星定位精度差,而无法实现定点悬停的问题,上述问题将导致无人飞行器存在一定的安全隐患,同时对操控者而言,也无法体验到良好的飞行操控效果。
全球卫星定位系统(GPS)的应用是基于卫星发射信号给定位端,当定位端同时收到 4 颗以上的卫星信号后,再根据相关的定位算法算出其当前所处位置的三维坐标、速度和时间等。然而,这种定位原理的前提是要能接收到卫星信号,这就将GPS模块的使用限制在室外且能够接收到良好的卫星信号的环境下。在室内等卫星信号不好的一些环境下,我们无法接收到符合要求的卫星信号,或者是完全接收不到信号,并且这个信号产生的GPS位置信息的误差变得非常大,几乎无法使用。
然而,在实际的应用中,无人机通常需要工作在卫星信号不好的环境下,如室内环境等。所以,在正常GPS信号无法满足需求的情况下,有必要研究和开发出一套行之有效的室内定位系统。
对比文件1(CN104932523A)公开一种无人飞行器的定位方法,该方法通过无人飞行器上的摄像头获取视频流图像信息,根据解析视频图像信息得到特征点信息以及高度信息和姿态信息,得到飞行器的漂移方向和漂移距离,融合成视频流定位信息,再通过获取卫星定位信号,将卫星定位信号和视频流定位信号进行融合处理后,得到精度较高的定位信息。
当前,已经有一些关于无人机室内定位技术的专利,如光流定位技术、惯性设备与超生波测距相结合定位技术、摄像头图像结合雷达避障系统定位技术以及摄像头图像对比定位技术等。尽管这些方法在一定程度上可以较好地用于无人机室内定位,但仍存在一些局限性。如使用光流或摄像头图像定位技术,不仅成本较高,而且还需要复杂的图像处理算法;而使用惯性测量器件等,则定位精度会受到一定的限制;同时,采用雷达避障则需要高精度的雷达扫描系统,价格昂贵、结构复杂。
发明内容
本发明正是基于以上一个或多个问题,提供一种无人飞行器室内定位计算方法及装置,用以解决现有技术中存在的定位成本高以及定位不准确等问题。
本发明提供一种无人飞行器室内定位计算方法。所述无人飞行器室内定位计算方法包括以下步骤:
S10 设置一公共接收器至一指定位置;
S20 设置N个超声波接收器在室内不同位置,且所述N个超声波接收器至所述公共接收器的距离小于预设距离值,以所述N个超声波接收器中的一第一超声波接收器为坐标轴的原点,从所述N个超声波接收器中另选取一第二超声波接收器和一第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上,其中N为大于等于3的整数;
S30 所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,记为开始时刻T1;
S40 所述N个超声波接收器接收到所述超声波信号时,分别记录结束时刻T2、T3…TN+1;
S50计算所述无人飞行器分别至所述N个超声波接收器的距离;
S60 建立计算所述无人飞行器位置坐标的方程组,计算所述无人飞行器的当前坐标。
优选地,步骤S20中,所述预设距离值的取值范围通过以下确定:
在所述开始时刻T1前所述辅助同步定位信号从发射到被接收时超声波传输距离对应的传输时间与所述超声波总传输时间的比值小于等于预设的百分比。
优选地,步骤S20中,当N大于3时,首先确定所述N个超声波接收器中第四超声波接收器的位置,所述第四超声波接收器需满足以下条件:设于所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面上,且与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器的连线构成矩形,或者在所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面外,与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器分别位于构建出的一长方体的顶点处。
优选地,步骤S60包括:当N=4时,所述无人飞行器距离各超声波接收器之间的距离方程组为:
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2),其中,S1、S2、S3和S4为所述无人飞行器至各超声波接收器的距离,所述无人飞行器当前的坐标为(x,y,z),4个所述超声波接收器的坐标分别为(xa,ya,za)、(xb,yb,zb)、(xc,yc,zc)、(xd,yd,zd)。
优选地,在四个超声波接收器位于同一个平面时,从下面方程组中选取三个方程:
S1= sqrt((x-xa)2+(y-ya)2+(z-z0)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-z0)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-z0)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-z0)2); z0为固定值,表示距离Z轴原点的距离。
优选地,所述无人飞行器室内定位计算方法还包括以下步骤:步骤S30具体包括:
S31所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,判断所述辅助同步定位信号的有效性,判断方法如下:接收所述无人飞行器发送的高电平脉宽与低电平脉宽,判断所述高电平脉宽与所述低电平脉宽的电平宽度是否与预设的电平宽度一致;
S32若一致,记录为所述开始时刻T1;
S33 若不一致,则重新接收。
本发明还提供一种无人飞行器室内定位计算装置,包括:
公共接收器设置模块,用于设置一公共接收器至一指定位置;
超声波接收器设置模块,用于设置N个超声波接收器在室内不同位置,且所述N个超声波接收器至所述公共接收器的距离小于预设距离值,以所述N个超声波接收器中的一第一超声波接收器为坐标轴的原点,从所述N个超声波接收器中另选取一第二超声波接收器和一第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上,其中N为大于等于3的整数;
开始时刻记录模块,用于所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,记为开始时刻T1;
结束时刻记录模块,用于所述N个超声波接收器接收到所述超声波信号时,分别记录结束时刻T2、T3…TN+1,其中N为大于等于3的整数;
距离计算模块,用于计算所述无人飞行器分别至所述N个超声波接收器的距离;
位置计算模块,用于建立计算所述无人飞行器位置坐标的方程组,计算所述无人飞行器的当前坐标。
优选地,超声波接收器设置模块进一步包括:
几何模型构建单元,用于当N大于3时,首先确定所述N个超声波接收器中第四超声波接收器的位置,所述第四超声波接收器需满足以下条件:设于所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面上,且与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器的连线构成矩形,或者在所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面外,与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器分别位于构建出的一长方体的顶点处。
优选地,所述位置计算模块进一步包括:方程组建立单元,用于当N=4时,建立所述无人飞行器距离各超声波接收器之间的距离方程组:
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2),其中,S1、S2、S3和S4为所述无人飞行器至各超声波接收器的距离,所述无人飞行器当前的坐标为(x,y,z),4个所述超声波接收器的坐标分别为(xa,ya,za)、(xb,yb,zb)、(xc,yc,zc)、(xd,yd,zd)。
优选地,在四个超声波接收器位于同一个平面时,从下面方程组中选取三个方程:
S1= sqrt((x-xa)2+(y-ya)2+(z-z0)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-z0)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-z0)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-z0)2); z0为固定值,表示距离Z轴原点的距离。
本发明所提供的无人飞行器室内定位计算方法及装置,不仅使得无人飞行器的定位成本低下,而且计算量大大减小,定位精度也较高。
附图说明
图1是本发明实施方式一的无人飞行器室内定位计算方法的流程示意图;
图2是本发明实施方式二的无人飞行器室内定位计算装置的结构示意图。
具体实施方式
下面结合附图和实施例对本发明进行详细说明。需要说明的是,如果不冲突,本发明实施例以及实施例中的各个特征可以相互结合,均在本发明的保护范围之内。
实施方式一
如图1所示,本发明提供一种无人飞行器室内定位计算方法,主要包括以下步骤:
S10 设置一公共接收器至一指定位置;
S20 设置N个超声波接收器在室内不同位置,且所述N个超声波接收器至所述公共接收器的距离小于预设距离值,以所述N个超声波接收器中的一第一超声波接收器为坐标轴的原点,从所述N个超声波接收器中另选取一第二超声波接收器和一第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上,其中N为大于等于3的整数;
S30 所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,记为开始时刻T1;
S40 所述N个超声波接收器接收到所述超声波信号时,分别记录结束时刻T2、T3…TN+1;
S50计算所述无人飞行器分别至所述N个超声波接收器的距离;
S60 建立计算所述无人飞行器位置坐标的方程组,计算所述无人飞行器的当前坐标。
本发明所提供的无人飞行器室内定位计算方法,不仅使得无人飞行器的定位成本低下,而且计算量大大减小,定位精度也较高。
在一个具体实施例中,步骤S20中,所述预设距离值的取值范围通过以下确定:
在所述开始时刻T1前所述辅助同步定位信号从发射到被接收时超声波传输距离对应的传输时间与所述超声波总传输时间的比值小于等于预设的百分比。
进一步地,在步骤S20中,当N大于3时,首先确定所述N个超声波接收器中第四超声波接收器的位置,所述第四超声波接收器需满足以下条件:设于所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面上,且与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器的连线构成矩形,或者在所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面外,与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器分别位于构建出的一长方体的顶点处。
进一步地,步骤S60包括:当N=4时,所述无人飞行器距离各超声波接收器之间的距离方程组为:
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2),其中,S1、S2、S3和S4为所述无人飞行器至各超声波接收器的距离,所述无人飞行器当前的坐标为(x,y,z),4个所述超声波接收器的坐标分别为(xa,ya,za)、(xb,yb,zb)、(xc,yc,zc)、(xd,yd,zd)。
在一个优选实施例中,在四个超声波接收器位于同一个平面时,从下面方程组中选取三个方程计算出无人飞行器的当前空间位置:
S1= sqrt((x-xa)2+(y-ya)2+(z-z0)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-z0)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-z0)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-z0)2); z0为固定值,表示距离Z轴原点的距离。
进一步地,所述无人飞行器室内定位计算方法还包括以下步骤:步骤S30具体包括:
S31所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,判断所述辅助同步定位信号的有效性,判断方法如下:接收所述无人飞行器发送的高电平脉宽与低电平脉宽,判断所述高电平脉宽与所述低电平脉宽的电平宽度是否与预设的电平宽度一致;
S32若一致,记录为所述开始时刻T1;
S33 若不一致,则重新接收。
实施例1
在无人飞行器飞行的室内的预定位置上设置四个超声波模块,在测得无人飞行器到各超声波模块的距离后,利用四个超声波模块的相对坐标,计算无人飞行器所处空间的坐标。
其中,四个超声波接收模块设在预定位置上时,这四个超声波接收模块要成矩形排布,以该矩形平面参考水平面构建直角坐标系。
假定以B为坐标系原点,则四个超声波接收模块的坐标为
A(0,e,0);
B(0,0,0);
C(f,0,0);
D(f,e,0);
无人飞行器当前所在空间位置为P(x,y,z),与四个超声波接收模块的距离分别为a,b,c,d。
则有方程组:
x2+(y-e)2+z2 = a2
x2+y2+z2 = b2
(x-f)2+y2+z2= c2
(x-f)2+(y-e)2+z2= d2
解此方程组即可得到无人飞行器当前所在空间位置P(x,y,z)的坐标值。
实施例1是对上述无人飞行器室内定位计算方法的进一步优化,通过上述方式进行定位计算,大大简化了计算复杂度,提高了定位精度。
实施例2
本实施例2所提出的无人飞行器室内定位计算方法在定位计算时,使用了一种空间坐标方程的简化方法。首先将超声波接收探头设置在预定的安装位置,其安装过程如下所述:
a. 设定平面坐标系中的原点,且在原点处设置一第一个超声波接收探头T1;
b. 在X轴、Y轴等距设置第二个超声波接收探头T2和第三个超声波接收探头T3;
c. 在这三个超声波接收探头形成的正方形中的最后一个角上设置第四个超声波接收探头T4;
d. 如果要增加超声波接收探头,就可以将平面原点新设定为T1、T2、T3、T4中的任意一个所在位置;
e. 然后重复上述步骤a、b、c,增加超声波接收探头。
本实施例2所提出的无人飞行器室内定位计算方法以使用四个超声波接收探头为例,但在实际中,在室内空间较大时,要适当增加超声波接收探头数量。本实施例2将四个超声波探头的安装规则设置为:四个超声波接收探头摆放位置关系为位于正方形的四个顶点,这样可以利用特殊数值,进行消元处理,使3元2次方程的求解变得更简易快速。四个超声波接收探头所处位置可以用四个坐标点Pa(xa,ya,za)、Pb(xb,yb,zb)、Pc(xc,yc,zc)和Pd(xd,yd,zd)表示,而无人飞行器当前所处的位置,可用Px(x, y, z)来表示。
根据空间坐标的计算公式S=sqrt((x-x0)2+(y-y0)2+(z-z0)2) 可得在时无人机距各超声波接收模块的距离方程组为:
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2);
由于Pa(xa,ya,za)、Pb(xb,yb,zb)、Pc(xc,yc,zc)和Pd(xd,yd, zd)处于空间内同一平面,所以有za = zb= zc = zd,这里将其假设为0, 即za = zb = zc = zd =0。同时还可以得到几组等量关系如下:
ya = yd,
xa= xb,
yb = yc,
xc= xd;
由于四个点成正交垂直关系,所以只需要有3个测量距离就可以列出一个方程组。从而得到四种情况下的方程组,这里za =zb = zc = zd =0,待测点的Z轴距离默认为正值,下面的方程组可以化简化为:
方程组1(测出了S1,S2,S3):
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
方程组2(测出了S2,S3,S4):
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2);
方程组3(测出了S1,S3,S4):
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2);
方程组4(测出了S1,S2,S4):
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2)。
我们通过关系式:
ya = yd,
xa= xb,
yb = yc,
xc= xd;
对四组方程进行降次消元之后,就可以发现,只有Z是二次元有两个解但是我们已经规定了其为正值,X、Y都是一次元,只有唯一的解。
实施方式二
如图2所示,本发明还提供一种无人飞行器室内定位计算装置,包括:
公共接收器设置模块100,用于设置一公共接收器至一指定位置;
超声波接收器设置模块200,用于设置N个超声波接收器在室内不同位置,且所述N个超声波接收器至所述公共接收器的距离小于预设距离值,以所述N个超声波接收器中的一第一超声波接收器为坐标轴的原点,从所述N个超声波接收器中另选取一第二超声波接收器和一第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上,其中N为大于等于3的整数;
开始时刻记录模块300,用于所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,记为开始时刻T1;
结束时刻记录模块400,用于所述N个超声波接收器接收到所述超声波信号时,分别记录结束时刻T2、T3…TN+1,其中N为大于等于3的整数;
距离计算模块500,用于计算所述无人飞行器分别至所述N个超声波接收器的距离;
位置计算模块600,用于建立计算所述无人飞行器位置坐标的方程组,计算所述无人飞行器的当前坐标。
本发明所提供的无人飞行器室内定位计算装置,只需通过简单的超声波发射器与超声波接收器即可实现定位计算,不仅使得无人飞行器的定位成本低下,而且计算量大大减小,定位精度也较高。
在一个具体的实施例中,超声波接收器设置模块进一步包括:
几何模型构建单元,用于当N大于3时,首先确定所述N个超声波接收器中第四超声波接收器的位置,所述第四超声波接收器需满足以下条件:设于所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面上,且与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器的连线构成矩形,或者在所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面外,与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器分别位于构建出的一长方体的顶点处。
在一个具体的实施例中,所述位置计算模块进一步包括:方程组建立单元,用于当N=4时,建立所述无人飞行器距离各超声波接收器之间的距离方程组:
S1= sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-zd)2),其中,S1、S2、S3和S4为所述无人飞行器至各超声波接收器的距离,所述无人飞行器当前的坐标为(x,y,z),4个所述超声波接收器的坐标分别为(xa,ya,za)、(xb,yb,zb)、(xc,yc,zc)、(xd,yd,zd)。
在一个具体的实施例中,在四个超声波接收器位于同一个平面时,从下面方程组中选取三个方程计算无人飞行器的当前空间位置:
S1= sqrt((x-xa)2+(y-ya)2+(z-z0)2);
S2= sqrt((x-xb)2+(y-yb)2+(z-z0)2);
S3= sqrt((x-xc)2+(y-yc)2+(z-z0)2);
S4= sqrt((x-xd)2+(y-yd)2+(z-z0)2); z0为固定值,表示距离Z轴原点的距离。
以上对本发明所提供的一种无人飞行器室内定位计算方法及装置,进行了详细介绍,本文中应用了具体个例对本发明的原理及实施方式进行了阐述,以上实施例的说明只是用于帮助理解本发明的方法及其核心思想;同时,对于本领域的一般技术人员,依据本发明的思想,在具体实施方式及应用范围上均会有改变之处,综上所述,本说明书内容仅为本发明的实施方式,并非因此限制本发明的专利范围,凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本发明的专利保护范围内。不应理解为对本发明的限制。
Claims (9)
1.一种无人飞行器室内定位计算方法,其特征在于,所述无人飞行器室内定位计算方法包括以下步骤:
S10设置一公共接收器至一指定位置;
S20设置N个超声波接收器在室内不同位置,且所述N个超声波接收器至所述公共接收器的距离小于预设距离值,以所述N个超声波接收器中的一第一超声波接收器为坐标轴的原点,从所述N个超声波接收器中另选取一第二超声波接收器和一第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上,其中N为大于等于3的整数;
S30所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,记为开始时刻T1;
S40所述N个超声波接收器接收到所述超声波信号时,分别记录结束时刻T2、T3…TN+1;
S50计算所述无人飞行器分别至所述N个超声波接收器的距离;
S60建立计算所述无人飞行器位置坐标的方程组,计算所述无人飞行器的当前坐标;
所述步骤S30具体包括:
S31所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,判断所述辅助同步定位信号的有效性,判断方法如下:接收所述无人飞行器发送的高电平脉宽与低电平脉宽,判断所述高电平脉宽与所述低电平脉宽的电平宽度是否与预设的电平宽度一致;
S32若一致,记录为所述开始时刻T1;
S33若不一致,则重新接收。
2.如权利要求1所述的无人飞行器室内定位计算方法,其特征在于,步骤S20中,所述预设距离值的取值范围通过以下确定:
在所述开始时刻T1前所述辅助同步定位信号从发射到被接收时超声波传输距离对应的传输时间与所述超声波总传输时间的比值小于等于预设的百分比。
3.如权利要求1所述的无人飞行器室内定位计算方法,其特征在于,步骤S20中,当N大于3时,首先确定所述N个超声波接收器中第四超声波接收器的位置,所述第四超声波接收器需满足以下条件:设于所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面上,且与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器的连线构成矩形,或者在所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面外,与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器分别位于构建出的一长方体的顶点处。
4.如权利要求3所述的无人飞行器室内定位计算方法,其特征在于,步骤S60包括:当N=4时,所述无人飞行器距离各超声波接收器之间的距离方程组为:
S1=sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2=sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3=sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4=sqrt((x-xd)2+(y-yd)2+(z-zd)2),其中,S1、S2、S3和S4为所述无人飞行器至各超声波接收器的距离,所述无人飞行器当前的坐标为(x,y,z),4个所述超声波接收器的坐标分别为(xa,ya,za)、(xb,yb,zb)、(xc,yc,zc)、(xd,yd,zd)。
5.如权利要求4所述的无人飞行器室内定位计算方法,其特征在于,在四个超声波接收器位于同一个平面时,从下面方程组中选取三个方程:
S1=sqrt((x-xa)2+(y-ya)2+(z-z0)2);
S2=sqrt((x-xb)2+(y-yb)2+(z-z0)2);
S3=sqrt((x-xc)2+(y-yc)2+(z-z0)2);
S4=sqrt((x-xd)2+(y-yd)2+(z-z0)2);z0为固定值,表示距离Z轴原点的距离。
6.一种无人飞行器室内定位计算装置,其特征在于,包括:
公共接收器设置模块,用于设置一公共接收器至一指定位置;
超声波接收器设置模块,用于设置N个超声波接收器在室内不同位置,且所述N个超声波接收器至所述公共接收器的距离小于预设距离值,以所述N个超声波接收器中的一第一超声波接收器为坐标轴的原点,从所述N个超声波接收器中另选取一第二超声波接收器和一第三超声波接收器分别位于X、Y、Z三个坐标轴中任意两个坐标轴上,其中N为大于等于3的整数;
开始时刻记录模块,用于所述公共接收器接收到所述无人飞行器同步发送的超声波信号和辅助同步定位信号中的所述辅助同步定位信号时,记为开始时刻T1;
结束时刻记录模块,用于所述N个超声波接收器接收到所述超声波信号时,分别记录结束时刻T2、T3…TN+1,其中N为大于等于3的整数;
距离计算模块,用于计算所述无人飞行器分别至所述N个超声波接收器的距离;
位置计算模块,用于建立计算所述无人飞行器位置坐标的方程组,计算所述无人飞行器的当前坐标。
7.如权利要求6所述的无人飞行器室内定位计算装置,其特征在于,超声波接收器设置模块进一步包括:
几何模型构建单元,用于当N大于3时,首先确定所述N个超声波接收器中第四超声波接收器的位置,所述第四超声波接收器需满足以下条件:设于所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面上,且与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器的连线构成矩形,或者在所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器确定的平面外,与所述第一超声波接收器、第二超声波接收器与所述第三超声波接收器分别位于构建出的一长方体的顶点处。
8.如权利要求7所述的无人飞行器室内定位计算装置,其特征在于,所述位置计算模块进一步包括:方程组建立单元,用于当N=4时,建立所述无人飞行器距离各超声波接收器之间的距离方程组:
S1=sqrt((x-xa)2+(y-ya)2+(z-za)2);
S2=sqrt((x-xb)2+(y-yb)2+(z-zb)2);
S3=sqrt((x-xc)2+(y-yc)2+(z-zc)2);
S4=sqrt((x-xd)2+(y-yd)2+(z-zd)2),其中,S1、S2、S3和S4为所述无人飞行器至各超声波接收器的距离,所述无人飞行器当前的坐标为(x,y,z),4个所述超声波接收器的坐标分别为(xa,ya,za)、(xb,yb,zb)、(xc,yc,zc)、(xd,yd,zd)。
9.如权利要求8所述的无人飞行器室内定位计算装置,其特征在于,在四个超声波接收器位于同一个平面时,从下面方程组中选取三个方程:
S1=sqrt((x-xa)2+(y-ya)2+(z-z0)2);
S2=sqrt((x-xb)2+(y-yb)2+(z-z0)2);
S3=sqrt((x-xc)2+(y-yc)2+(z-z0)2);
S4=sqrt((x-xd)2+(y-yd)2+(z-z0)2);z0为固定值,表示距离Z轴原点的距离。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710339762.8A CN107024679B (zh) | 2017-05-15 | 2017-05-15 | 无人飞行器室内定位计算方法及装置 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201710339762.8A CN107024679B (zh) | 2017-05-15 | 2017-05-15 | 无人飞行器室内定位计算方法及装置 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN107024679A CN107024679A (zh) | 2017-08-08 |
CN107024679B true CN107024679B (zh) | 2019-12-27 |
Family
ID=59529609
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201710339762.8A Active CN107024679B (zh) | 2017-05-15 | 2017-05-15 | 无人飞行器室内定位计算方法及装置 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN107024679B (zh) |
Families Citing this family (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108267718A (zh) * | 2018-01-12 | 2018-07-10 | 章永强 | 一种超声波近场定位装置、方法及自动跟随体 |
WO2020142036A1 (en) * | 2018-12-31 | 2020-07-09 | Istanbul Sehir University | Indoor drone navigation support system based on machine learning |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104898090A (zh) * | 2014-03-03 | 2015-09-09 | 罗斯蒙特公司 | 室内定位系统 |
CN105203996A (zh) * | 2015-09-16 | 2015-12-30 | 上海智臻智能网络科技股份有限公司 | 移动设备及其室内定位方法、系统及装置、锚节点 |
CN105629201A (zh) * | 2016-01-06 | 2016-06-01 | 南昌大学 | 一种室内超声波定位位置修正方法 |
CN105929368A (zh) * | 2016-04-14 | 2016-09-07 | 南京创维信息技术研究院有限公司 | 基于超声波的室内定位方法、装置及系统 |
KR101705227B1 (ko) * | 2015-11-06 | 2017-02-09 | 주식회사 우리엘테크 | 실내측위방법 |
CN205982632U (zh) * | 2016-08-30 | 2017-02-22 | 广东容祺智能科技有限公司 | 一种无人机室内空间定位系统 |
-
2017
- 2017-05-15 CN CN201710339762.8A patent/CN107024679B/zh active Active
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104898090A (zh) * | 2014-03-03 | 2015-09-09 | 罗斯蒙特公司 | 室内定位系统 |
CN105203996A (zh) * | 2015-09-16 | 2015-12-30 | 上海智臻智能网络科技股份有限公司 | 移动设备及其室内定位方法、系统及装置、锚节点 |
KR101705227B1 (ko) * | 2015-11-06 | 2017-02-09 | 주식회사 우리엘테크 | 실내측위방법 |
CN105629201A (zh) * | 2016-01-06 | 2016-06-01 | 南昌大学 | 一种室内超声波定位位置修正方法 |
CN105929368A (zh) * | 2016-04-14 | 2016-09-07 | 南京创维信息技术研究院有限公司 | 基于超声波的室内定位方法、装置及系统 |
CN205982632U (zh) * | 2016-08-30 | 2017-02-22 | 广东容祺智能科技有限公司 | 一种无人机室内空间定位系统 |
Also Published As
Publication number | Publication date |
---|---|
CN107024679A (zh) | 2017-08-08 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US11822353B2 (en) | Simple multi-sensor calibration | |
US11377211B2 (en) | Flight path generation method, flight path generation system, flight vehicle, program, and storage medium | |
US11218689B2 (en) | Methods and systems for selective sensor fusion | |
US11032527B2 (en) | Unmanned aerial vehicle surface projection | |
CN110455285A (zh) | 一种在卫星导航信号失效时的无人机导航方法及导航装置 | |
US11556681B2 (en) | Method and system for simulating movable object states | |
US20200012756A1 (en) | Vision simulation system for simulating operations of a movable platform | |
JP2014078232A (ja) | 分散された位置の識別 | |
US10527423B1 (en) | Fusion of vision and depth sensors for navigation in complex environments | |
EP3020634A1 (en) | Deployable airborne sensor array system and method of use | |
EP2927771A1 (en) | Flying drone trajectory synchronization | |
JP2018021865A (ja) | 移動体、移動体の制御方法、移動体を制御するプログラム、制御システム、及び情報処理装置 | |
CN107024679B (zh) | 无人飞行器室内定位计算方法及装置 | |
Rhudy et al. | Unmanned aerial vehicle navigation using wide-field optical flow and inertial sensors | |
CN107300704B (zh) | 无人飞行器室内定位方法及装置 | |
CN108508911A (zh) | 无人机编队飞行控制方法及无人机 | |
CN110187695A (zh) | 一种无人机协同控制验证平台 | |
KR101764222B1 (ko) | 고정밀 측위 시스템 및 방법 | |
CN107229034A (zh) | 无人飞行器定位方法及定位系统 | |
CN109521785B (zh) | 一种随身拍智能旋翼飞行器系统 | |
US20220390940A1 (en) | Interfaces And Control Of Aerial Vehicle For Automated Multidimensional Volume Scanning | |
CN116027305A (zh) | 无人机载穿墙雷达探测系统的运动补偿方法、装置及系统 | |
WO2022264413A1 (ja) | 移動体の移動経路生成方法及びプログラム、管理サーバ、管理システム | |
JP6684012B1 (ja) | 情報処理装置および情報処理方法 | |
Troll et al. | Indoor Localization of Quadcopters in Industrial Environment |
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 | ||
TR01 | Transfer of patent right | ||
TR01 | Transfer of patent right |
Effective date of registration: 20231225 Address after: 518000 Room 501, block a, West Industrial Building, Gongyuan Road, zone 22, Xin'an street, Bao'an District, Shenzhen, Guangdong Province Patentee after: SHENZHEN SIMTOO INTELLIGENT TECHNOLOGY Co.,Ltd. Address before: 430070 room 3001, 3rd floor, Nanhu Pioneer Park, 53 Nanhu Avenue, Hongshan District, Wuhan City, Hubei Province Patentee before: WUHAN XINGXUN INTELLIGENT TECHNOLOGY CO.,LTD. |