CN109186593A - 一种水下无人机的运动轨迹记录绘制方法 - Google Patents
一种水下无人机的运动轨迹记录绘制方法 Download PDFInfo
- Publication number
- CN109186593A CN109186593A CN201811065514.XA CN201811065514A CN109186593A CN 109186593 A CN109186593 A CN 109186593A CN 201811065514 A CN201811065514 A CN 201811065514A CN 109186593 A CN109186593 A CN 109186593A
- Authority
- CN
- China
- Prior art keywords
- unmanned plane
- underwater unmanned
- underwater
- current
- motion profile
- 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.)
- Pending
Links
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及水下无人机技术领域,特别涉及一种水下无人机的运动轨迹记录绘制方法;本发明是基于加速度传感器的加速度数据,通过在时间方向的积分,将加速度数据积分为速度,然后再积分为位移,最终获取到水下无人机在各个时间点的位移坐标,由此记录并绘制出水下无人机的运动轨迹;本发明可以在因具有无法长距离传递无线信号的特性的水下环境下,能够准确并以能够接受的成本完成水下无人机的运动轨迹绘制。
Description
技术领域
本发明涉及水下无人机技术领域,特别涉及一种水下无人机的运动轨迹记录绘制方法。
背景技术
目前由于水这种介质无法有效长距离的传输无线电信号,该信号在水中的衰减非常快,因此无法采用GPS或北斗卫星定位系统等类似无线电信号定位模式。
目前水下定位方案基本采用多普勒仪和超短基线声学定位系统这两种比较成熟的技术。水下多普勒仪能够比较精确的测量水下运动机构的速度,通过速度转换为位移,所求得的位移比较精确,进而得到水下位置坐标,但是多普勒仪非常昂贵,体积庞大,并不适用于小型的水下无人机设备使用。水下超短基线声学定位系统主要以水下声波作为主要测量媒介,通过声学定位的方式为水下运动机构进行准确定位,其原理是基于水声在水中的反射时间计算运动机构与声学定位点之间的距离和方向性,并且利用多个声学定位点的数据信息计算获得准确的机构水下定位坐标,该方法水下定位精度很高,但是需要预先在目标水域内投放安装多个声学定位点,投入非常巨大且只能在安装有声学定位点的有限水域内使用,有较大的局限性。目前的水下定位方案要么成本高昂同时仪器庞大难以维护,要么就需要预先在目标水域内大量布放水下基础设施,对于一般小型的水下无人机并不适用,且限制了其使用水域和环境,造成了较大的局限性。
发明内容
本发明主要解决的技术问题是提供一种水下无人机的运动轨迹记录绘制方法,是基于加速度传感器的加速度数据,通过在时间方向的积分,将加速度数据积分为速度,然后再积分为位移,最终获取到水下无人机在各个时间点的位移坐标,由此记录并绘制出水下无人机的运动轨迹。
为解决上述技术问题,本发明采用的一个技术方案是:提供一种水下无人机的运动轨迹记录绘制方法,其中,包括如下步骤:
步骤S1、根据水下无人机上的加速度传感器测出水下无人机当前的加速度值;
步骤S2、对水下无人机当前的加速度值按时间积分为水下无人机当前的速度值;
步骤S3、对水下无人机当前的速度值积分为水下无人机当前的位移值;
步骤S4、采用龙格库塔法对水下无人机的加速度和速度分别求解积分,从而分别得到水下无人机在本体坐标系上的速度量和位移量;步骤S5、根据水下无人机在本体坐标系上的速度量和位移量,得到水下无人机在本体坐标系上的坐标;
步骤S6、将水下无人机在本体坐标系上的坐标转换成水下无人机在大地坐标系上的坐标;
步骤S7、将水下无人机各个时刻在大地坐标系上的坐标进行记录再绘制出水下无人机的运动轨迹。
作为本发明的一种改进,步骤S1还包括:对水下无人机当前的加速度值进行滤波处理。
作为本发明的进一步改进,在步骤S2内,对水下无人机当前的加速度值采用一阶的龙格库塔法积分为水下无人机当前的速度值。
作为本发明的更进一步改进,在步骤S3内,对水下无人机当前的速度值采用一阶的龙格库塔法积分为水下无人机当前的位移值。
作为本发明的更进一步改进,在步骤S4内,对水下无人机当前的速度值和位移值采用龙格库塔法二次积分从而分别得到水下无人机在本体坐标系上的速度量和位移量。
作为本发明的更进一步改进,在步骤S6内,将水下无人机在本体坐标系上的坐标通过欧拉角旋转矩阵转换成水下无人机在大地坐标系上的坐标。
作为本发明的更进一步改进,在步骤S2内,将水下无人机当前的加速度值采用公式v=∫a dt得到水下无人机当前的速度值。
作为本发明的更进一步改进,在步骤S3内,将水下无人机当前的速度值采用公式s=∫v dt得到水下无人机当前的位移值。
作为本发明的更进一步改进,在步骤S4内,采用离散化的计算速度量积分方式:将水下无人机前一个时刻的速度值采用公式得到水下无人机在本体坐标系上的当前的速度量。
作为本发明的更进一步改进,在步骤S4内,采用离散化的计算位移量积分方式:将水下无人机前一个时刻的位移值采用公式得到水下无人机在本体坐标系上的当前的位移量。
本发明的有益效果是:与现有技术相比,本发明是基于加速度传感器的加速度数据,通过在时间方向的积分,将加速度数据积分为速度,然后再积分为位移,最终获取到水下无人机在各个时间点的位移坐标,由此记录并绘制出水下无人机的运动轨迹;本发明可以在因具有无法长距离传递无线信号的特性的水下环境下,能够准确并以能够接受的成本完成水下无人机的运动轨迹绘制。
附图说明
图1是本发明的运动轨迹记录绘制方法的步骤框图;
图2是本发明的水下无人机的本体坐标系的示意图;
图3是本发明的水下无人机的本体坐标系和大地坐标系重合的示意图;
图4是本发明的水下无人机的本体坐标系和大地坐标系不重合的示意图;
图5是本发明的水下无人机应用到的欧拉角变换的原理示意图。
具体实施方式
在本发明中,水下无人机是在自身本体坐标系上进行多个自由度的运动,但是由于运动姿态的自由偏转,水下无人机的本体坐标系和固定的大地坐标系之间实时存在偏转角度;水下无人机的本体坐标系是为以水下无人机本身为平面而成的坐标系,大地坐标系是以地球为平面而成的坐标系,该大地坐标系也就是我们通常应用到的坐标系。
在记录和绘制水下无人机的位移轨迹时,坐标点一般是基于大地坐标系中的坐标,因此需要通过坐标转换将水下无人机在本体坐标系内的运动转换到大地坐标系上并计算出轨迹坐标点。
如图3所示,为了简化本体坐标系和大地坐标系之间的转换流程,这里将水下无人机的本体坐标系的原点和大地坐标系的原点都设置在水下无人机的重心O点上。
请参照图1至图5,本发明的一种水下无人机的运动轨迹记录绘制方法,包括如下步骤:
步骤S1、根据水下无人机上的加速度传感器测出水下无人机当前的加速度值;
步骤S2、对水下无人机当前的加速度值按时间积分为水下无人机当前的速度值;
步骤S3、对水下无人机当前的速度值积分为水下无人机当前的位移值;
步骤S4、采用龙格库塔法对水下无人机的加速度和速度分别求解积分,从而分别得到水下无人机在本体坐标系上的速度量和位移量;步骤S5、根据水下无人机在本体坐标系上的速度量和位移量,得到水下无人机在本体坐标系上的坐标;
步骤S6、将水下无人机在本体坐标系上的坐标转换成水下无人机在大地坐标系上的坐标;
步骤S7、将水下无人机各个时刻在大地坐标系上的坐标进行记录再绘制出水下无人机的运动轨迹。
与现有技术相比,本发明是基于加速度传感器的加速度数据,通过在时间方向的积分,将加速度数据积分为速度,然后再积分为位移,最终获取到水下无人机在各个时间点的位移坐标,由此记录并绘制出水下无人机的运动轨迹;本发明可以在因具有无法长距离传递无线信号的特性的水下环境下,能够准确并以能够接受的成本完成水下无人机的运动轨迹绘制。
其中,步骤S1还包括:对水下无人机当前的加速度值进行滤波处理。
在步骤S2内,对水下无人机当前的加速度值采用一阶的龙格库塔法积分为水下无人机当前的速度值;具体地讲,在步骤S2内,将水下无人机当前的加速度值采用公式v=∫adt得到水下无人机当前的速度值。
在步骤S3内,对水下无人机当前的速度值采用一阶的龙格库塔法积分为水下无人机当前的位移值;具体地讲,在步骤S3内,将水下无人机当前的速度值采用公式s=∫v dt得到水下无人机当前的位移值。
在步骤S4内,采用龙格库塔法对水下无人机的加速度求解积分,从而得到水下无人机在本体坐标系上的速度量,具体地讲,采用龙格库塔法对水下无人机的步骤S2的速度值积分,也可以说是,采用龙格库塔法对水下无人机的步骤S1的加速度值求解积分,得到水下无人机在本体坐标系上的速度量。
在步骤S4内,采用龙格库塔法对水下无人机的速度求解积分,从而得到水下无人机在本体坐标系上的位移量,具体地讲,采用龙格库塔法对水下无人机的步骤S3的位移值积分,也可以说是,采用龙格库塔法对水下无人机的步骤S2的速度值求解积分,得到水下无人机在本体坐标系上的位移量。
在步骤S4内,采用龙格库塔法对水下无人机的加速度和速度分别求解积分,从而分别得到水下无人机在本体坐标系上的速度量和位移量;更加具体地细化解释为,在步骤S4内,采用离散化的计算速度量积分方式:将水下无人机前一个时刻的速度值采用公式得到水下无人机在本体坐标系上的当前的速度量;在该公式计算是当前的速度,但需要通过预估前一个时刻的速度vt来计算出当前的速度,比如公式中的t时刻表示就是当前的时刻,需要预估前一个时刻的预估速度vt,再采用公式从而通过预估速度vt计算出比较真实的当前速度量vt+△t。
在步骤S4内,采用离散化的计算位移量积分方式:将水下无人机前一个时刻的位移值采用公式得到水下无人机在本体坐标系上的当前的位移量;位移量的计算与速度量的计算的原理是一样的,需要预估前一个时刻的预估位移st,再采用从而通过预估位移st计算出比较真实的当前位移量st+△t。
在步骤S6内,将水下无人机在本体坐标系上的坐标通过欧拉角旋转矩阵转换成水下无人机在大地坐标系上的坐标;如图5所示,循笛卡尔(右手)坐标系原则,设置水下无人机本体坐标系中俯视视角下x轴为正前方向,y轴正右方向,z轴为正下方向。本体坐标系O-xyz相对于水下无人机机体保持相对静止,但是由于自身姿态和航向的偏移,其本体坐标系O-xyz同大地坐标系O-XYZ存在绕不同方向的角度转动。水下无人机本体坐标系旋转根据其九轴姿态角传感器测得数据进行坐标变换,按照欧拉角变换的方式将本体坐标系O-xyz上的坐标点分量投影到大地坐标系O-XYZ上。图5上显示的欧拉角是用三个旋转角度α、β、γ来标示旋转的,图5中Oxyz是起始的坐标系,OXYZ坐标系是最后旋转完成的坐标系;整个旋转分为三个步骤:将坐标系绕z轴旋转α角,将旋转后坐标系绕自己本身的x轴(也就是图中的N轴)旋转β角,将旋转后坐标系绕自己本身的y轴旋转γ角;由于绕不同的轴旋转所得到的欧拉角是不同的,所以欧拉角在使用的时候必须要先指明旋转的顺序,这里旋转的顺序采用“zxy”的顺序;如图4所示,水下无人机本体坐标系Oxyz按照上述欧拉角模式转换到大地坐标系OXYZ上的主要过程为:遵循依次按照航向角α、横摇角β和纵倾角γ旋转局部坐标系,直到局部坐标系旋转到与大地坐标系重合,如图3所示;经过依次坐标系的旋转后,可得欧拉角旋转矩阵表达式C:
通过欧拉角旋转矩阵C,可以将水下无人机的本体坐标系O-xyz上的点(x,y,z)与原点O连接的矢量转换投影到大地坐标系O-XYZ上:
本发明提供一个实施例,水下无人机在水下的位移运动轨迹绘制主要采用对加速度二次积分的方式计算:首先在水下无人机的本体坐标系内,加速度计算出的3个方向的线加速度经过滤波后,按时间积分为速度,然后以速度跟随时间积分得到位移量;对于离散数据做积分可以采用一阶的龙格库塔法;设a表示加速度矢量,v表示速度矢量,s表示位移矢量;采用最基本的积分原则,由加速度a积分为速度v,再由速度v积分出位移s:v=∫a dt,s=∫v dt。
采用龙格库塔法根据离散的加速度量计算速度和位移,即分别为:其中为t时刻的加速度,为t时刻的速度。
经过龙格库塔法二次积分运算后,可以将离散的加速度计测得的加速度量积分成为速度和位移量;然后通过欧拉角旋转矩阵C,将水下无人机的本体局部坐标系O-xyz上的点(x,y,z)与转换投影到大地坐标系O-XYZ。
在本发明中,可以通过对加速度的龙格库塔二次积分运算和欧拉角矩阵转换实时计算更新出各个时刻的水下无人机坐标点,从而记录并绘制其运动位移轨迹。本发明的技术方案,主要基于九轴加速度传感器采集的离散加速度数据,不需要依赖昂贵笨重的仪器或在目标水域内投放声学定位点,在满足定位精度要求的前提下大大降低了水下无人机的定位技术成本,使得小型/微型水下无人机的运动轨迹记录和绘制在较低成本的基础上得到实现。
以上所述仅为本发明的实施方式,并非因此限制本发明的专利范围,凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换,或直接或间接运用在其他相关的技术领域,均同理包括在本发明的专利保护范围内。
Claims (10)
1.一种水下无人机的运动轨迹记录绘制方法,其特征在于,包括如下步骤:
步骤S1、根据水下无人机上的加速度传感器测出水下无人机当前的加速度值;
步骤S2、对水下无人机当前的加速度值按时间积分为水下无人机当前的速度值;
步骤S3、对水下无人机当前的速度值积分为水下无人机当前的位移值;
步骤S4、采用龙格库塔法对水下无人机的加速度和速度分别求解积分,从而分别得到水下无人机在本体坐标系上的速度量和位移量;
步骤S5、根据水下无人机在本体坐标系上的速度量和位移量,得到水下无人机在本体坐标系上的坐标;
步骤S6、将水下无人机在本体坐标系上的坐标转换成水下无人机在大地坐标系上的坐标;
步骤S7、将水下无人机各个时刻在大地坐标系上的坐标进行记录再绘制出水下无人机的运动轨迹。
2.根据权利要求1所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,步骤S1还包括:对水下无人机当前的加速度值进行滤波处理。
3.根据权利要求2所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S2内,对水下无人机当前的加速度值采用一阶的龙格库塔法积分为水下无人机当前的速度值。
4.根据权利要求3所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S3内,对水下无人机当前的速度值采用一阶的龙格库塔法积分为水下无人机当前的位移值。
5.根据权利要求4所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S4内,对水下无人机当前的速度值和位移值采用龙格库塔法二次积分从而分别得到水下无人机在本体坐标系上的速度量和位移量。
6.根据权利要求5所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S6内,将水下无人机在本体坐标系上的坐标通过欧拉角旋转矩阵转换成水下无人机在大地坐标系上的坐标。
7.根据权利要求3所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S2内,将水下无人机当前的加速度值采用公式v=∫adt得到水下无人机当前的速度值,其中,a表示加速度矢量,v表示速度矢量。
8.根据权利要求4所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S3内,将水下无人机当前的速度值采用公式s=∫vdt得到水下无人机当前的位移值,其中,s表示位移矢量,v表示速度矢量。
9.根据权利要求5所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S4内,采用离散化的计算速度量积分方式:将水下无人机前一个时刻的速度值采用公式得到水下无人机在本体坐标系上的当前的速度量。
10.根据权利要求5所述的一种水下无人机的运动轨迹记录绘制方法,其特征在于,在步骤S4内,采用离散化的计算位移量积分方式:将水下无人机前一个时刻的位移值采用公式得到水下无人机在本体坐标系上的当前的位移量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811065514.XA CN109186593A (zh) | 2018-09-13 | 2018-09-13 | 一种水下无人机的运动轨迹记录绘制方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201811065514.XA CN109186593A (zh) | 2018-09-13 | 2018-09-13 | 一种水下无人机的运动轨迹记录绘制方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN109186593A true CN109186593A (zh) | 2019-01-11 |
Family
ID=64910844
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201811065514.XA Pending CN109186593A (zh) | 2018-09-13 | 2018-09-13 | 一种水下无人机的运动轨迹记录绘制方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109186593A (zh) |
Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120022820A1 (en) * | 2008-04-17 | 2012-01-26 | Guenter Schmitz | Method for inertial navigation under water |
CN103792595A (zh) * | 2014-02-27 | 2014-05-14 | 中国电子科技集团公司第四十一研究所 | 用于定位水下运动磁目标的磁传感器阵列校准方法 |
CN104457754A (zh) * | 2014-12-19 | 2015-03-25 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
CN104515522A (zh) * | 2013-09-28 | 2015-04-15 | 南京专创知识产权服务有限公司 | 一种水下磁场及六轴惯性联合定位系统 |
CN106708088A (zh) * | 2016-09-26 | 2017-05-24 | 重庆零度智控智能科技有限公司 | 坐标计算方法及装置、飞行控制方法及系统、无人机 |
CN107084723A (zh) * | 2017-05-12 | 2017-08-22 | 中国人民解放军91550部队 | 一种海洋环境下的水下航行体运动轨迹估计方法 |
-
2018
- 2018-09-13 CN CN201811065514.XA patent/CN109186593A/zh active Pending
Patent Citations (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US20120022820A1 (en) * | 2008-04-17 | 2012-01-26 | Guenter Schmitz | Method for inertial navigation under water |
CN104515522A (zh) * | 2013-09-28 | 2015-04-15 | 南京专创知识产权服务有限公司 | 一种水下磁场及六轴惯性联合定位系统 |
CN103792595A (zh) * | 2014-02-27 | 2014-05-14 | 中国电子科技集团公司第四十一研究所 | 用于定位水下运动磁目标的磁传感器阵列校准方法 |
CN104457754A (zh) * | 2014-12-19 | 2015-03-25 | 东南大学 | 一种基于sins/lbl紧组合的auv水下导航定位方法 |
CN106708088A (zh) * | 2016-09-26 | 2017-05-24 | 重庆零度智控智能科技有限公司 | 坐标计算方法及装置、飞行控制方法及系统、无人机 |
CN107084723A (zh) * | 2017-05-12 | 2017-08-22 | 中国人民解放军91550部队 | 一种海洋环境下的水下航行体运动轨迹估计方法 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104121905B (zh) | 一种基于惯性传感器的航向角获取方法 | |
CN105723240B (zh) | 用加速度/减速度确定设备和船只间的错位的方法和装置 | |
CN108303043B (zh) | 多传感器信息融合的植物叶面积指数检测方法及系统 | |
CN106842271B (zh) | 导航定位方法及装置 | |
CN103941223A (zh) | 声源定位系统及其方法 | |
CN105547290B (zh) | 一种基于超短基线定位系统的从潜器导航方法 | |
CN108845345B (zh) | 利用gnss测速原理的双天线定向测姿的方法 | |
CN107909614A (zh) | 一种gps失效环境下巡检机器人定位方法 | |
CN107490378A (zh) | 一种基于mpu6050与智能手机的室内定位与导航的方法 | |
CN113048983B (zh) | 一种异时序贯量测的改进分层式auv协同导航定位方法 | |
CN104316058B (zh) | 一种采用交互多模型的移动机器人wsn/ins组合导航方法 | |
CN106093992A (zh) | 一种基于cors的亚米级组合定位导航系统及导航方法 | |
CN106643670A (zh) | 一种无人机航摄站点坐标求解装置及方法 | |
CN110646783A (zh) | 一种水下航行器的水下信标定位方法 | |
WO2017203209A1 (en) | Sports officiating system | |
CN107990901A (zh) | 一种基于传感器的用户方向定位方法 | |
CN103076023A (zh) | 一种步长计算方法和装置 | |
CN103823209B (zh) | 用于轻小型合成孔径雷达系统中低成本运动误差测量装置 | |
CN108332756A (zh) | 一种基于拓扑信息的水下航行器协同定位方法 | |
CN113933526B (zh) | 一种体目标高精度三维运动速度测量方法 | |
CN104964689A (zh) | 基于声源定位、ins的爬壁机器人导航定位系统及方法 | |
CN204788411U (zh) | 基于声源定位、ins的爬壁机器人导航定位系统 | |
CN111708008B (zh) | 一种基于imu和tof的水下机器人单信标导航方法 | |
CN110658515A (zh) | 一种基于uwb单基站的多用户imu定位对齐方法 | |
CN205506069U (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 | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20190111 |
|
RJ01 | Rejection of invention patent application after publication |