CN103884338A - 空间轨迹实时追踪装置及方法 - Google Patents
空间轨迹实时追踪装置及方法 Download PDFInfo
- Publication number
- CN103884338A CN103884338A CN201410060655.8A CN201410060655A CN103884338A CN 103884338 A CN103884338 A CN 103884338A CN 201410060655 A CN201410060655 A CN 201410060655A CN 103884338 A CN103884338 A CN 103884338A
- Authority
- CN
- China
- Prior art keywords
- microcontroller
- sensor
- omega
- coordinate system
- sensing data
- 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
Images
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
- G01C21/165—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 combined with non-inertial navigation instruments
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明提供一种空间轨迹实时追踪装置及方法,涉及三维空间轨迹追踪技术领域。该装置包括:传感器、微控制器、通用异步收发控制器,传感器包括三轴加速度传感器、三轴陀螺仪和三轴电子罗盘,传感器通过IIC接口将传感数据发送给微控制器;微控制器,用于接收所述传感器发送的传感数据并进行处理,并向通过IIC总线向传感器发送控制命令;同时将处理过后的传感数据通过UART外设发送给通用异步收发控制器;通用异步收发控制器,将微控制器发送的处理过后的传感数据进行转换,并通过USB接口发送给上位机;同时通用异步收发控制器对上位机发送的命令进行转换后发送给所述微控制器。本发明成本低廉;具备高稳定性;具备高精确度;且装置尺寸小,携带方便。
Description
技术领域
本发明涉及三维空间轨迹追踪技术领域,具体涉及一种空间轨迹实时追踪装置及方法。
背景技术
随着社会的经济发展,空间轨迹实时追踪技术能够应用的领域越来越广。如各大型购物中心遍布在城市的各个商业区,在大型购物中心购物的时候,消费者会发现琳琅满目的商铺让人眼花缭乱,如对地下施工的工作人员和对地下事故进行救援的救援人员来说,非常需要一个装置能够帮助他们在进行定位和导航;再如说有些矿井和油田的装置需要跟踪装置的移动距离,使用标尺来测量不能迅速的判定装置的工作状态和性能指标;还有大学做的各种物理实验,需要跟踪物体的运动轨迹,从而需要准确快捷地得到相应的实验结果。
现行所采用的方法是通过移动基站和GPS信号的方式来进行空间轨迹实时追踪的。
然而,在某些特殊的情况下,没有移动基站或者GPS信号时,现有的方法就很难对空间轨迹实时追踪,故而急需一种能够对三维空间中的轨迹进行实时追踪的新的装置。
发明内容
(一)解决的技术问题
针对现有技术的不足,本发明提供了一种空间轨迹实时测量装置及方法,能够解决在没有移动基站或者GPS信号时对三维空间的轨迹实时追踪问题。
(二)技术方案
为实现以上目的,本发明通过以下技术方案予以实现:
一种空间轨迹实时追踪装置,其特征在于,该装置包括:传感器、微控制器、通用异步收发控制器,
所述传感器包括三轴加速度传感器、三轴陀螺仪和三轴电子罗盘,所述三轴加速度传感器用于采集三个互相垂直轴向上的加速度,所述三轴陀螺仪用于采集三个互相垂直轴向上的角速度,所述三轴电子罗盘用于采集三个互相垂直轴向上的姿态角,所述传感器通过IIC接口将传感数据发送给微控制器;
所述微控制器,用于接收所述传感器发送的传感数据并进行处理,同时通过IIC总线向所述传感器发送控制命令;然后将处理过后的传感数据通过UART外设发送给通用异步收发控制器;
所述通用异步收发控制器,将所述微控制器发送的处理过后的传感数据进行转换,并通过USB接口发送给上位机;同时把上位机发送的命令进行转换后发送给所述的微控制器。
优选的,所述的微控制器包括精简指令处理器核心、只读存储器、随机存储器、IIC接口、定时器、中断源和USART等外设;
所述只读存储器与所述精简指令处理器核心通过指令总线相连,用于存储该装置的指令信息;
所述随机存储器与所述精简指令处理器核心通过数据总线相连,用于存储该装置的数据信息;
所述精简处理器核心通过APB总线与IIC接口、定时器、中断源和USART等装置连接;IIC接口与所述传感器相连,用于对所述传感器与所述微控制器的数据和命令的交互;
所述定时器,用于所述微控制器对所述传感器施行采样周期时间间隔的采样;
所述中断源,用于在定时器产生定时器中断的情况下,所述中断源将所述精简指令处理器核心的程序指针指向采样程序的第一条指令,使得在采样周期到来的时候所述微控制器对所述传感器进行采样。
优选的,该装置进一步包括:通用下载接口模块,用于将所述微控制器的连接到仿真器,通过仿真器对所述微控制器的程序进行下载和调试。
优选的,该装置进一步包括:电源模块,所述电源模块包括2个LDO,一个LDO负责对所述微控制器供电,另一个LDO负责对所述传感器供电。
优选的,该装置进一步包括:LED指示灯,所述LED指示灯通过IO接口与所述微控制器相连,所述微控制器通过控制LED指示灯的亮灭来指示装置运行状态。
本发明还提供了一种空间轨迹实时追踪方法,该方法包括步骤:
S1、装置初始化,微控制器开始定时,预订延时时间后,微控制器接收三轴加速度传感器、三轴陀螺仪和三轴电子罗盘将采集到的传感器数据;
S2、所述微控制器对所述传感器数据进行处理,得到装置的初始姿态角,并依据所述姿态角确定初始旋转四元数(q0,q1,q2,q3);
S3、依据所述三轴陀螺仪传输的角速度数据更新所述初始旋转四元数,得到旋转矩阵,并通过所述旋转矩阵将所述三轴加速度传感器传输的三轴加速度数据分解到导航坐标系;
S4、对所述导航坐标系下的三轴加速度分别进行一次积分,得到导航坐标系下的三轴速度;
S5、对所述导航坐标系下的三轴速度进行一次积分,得到导航坐标系下的三轴位移;
S6、合成所述导航坐标系下的三轴位移,得到对物体进行实时追踪的空间轨迹。
优选的,步骤S2中的初始姿态角包括:初始俯仰角Pitch、初始横滚角Roll和初始航向角Yaw;
其中,初始俯仰角Pitch的计算表达式为:
Pitch=asin(AccY/(AccX*AccX+AccY*AccY+AccZ*AccZ)2);
其中,初始横滚角Roll的计算表达式为:
Roll=asin(AccX/(AccX*AccX+AccY*AccY+AccZ*AccZ)2);
其中,初始航向角Yaw的计算表达式为:
Yaw=arctan((HycosRoll1+HzsinRoll1)/(HxcosPitch1+HysinPitch1sinRoll1-HzsinPitch1cosRoll1));
式中,AccX、AccY和AccZ为三轴加速度传感器传输的x、y和z轴传感数据;Hx、Hy、Hz为三轴电子罗盘传输的x、y和z轴传感数据。
优选的,步骤S3中更新后的所述旋转四元数为:
式中,wx、wy和wz为三轴陀螺仪传输的x、y和z轴传感数据;
步骤S3中旋转矩阵为:
式中,q0,q1,q2,q3为上一个状态的旋转四元数(q0,q1,q2,q3)的每个分量的值;
步骤S3中将所述三轴加速度传感器传输的三轴加速度数据分解到导航坐标系的表达式为:
优选的,步骤S4中,对所述导航坐标系下的三轴加速度分别进行一次积分的表达式为:
Vt=Vt-T+at-TT
式中,Vt为导航坐标系下的速度矢量,Vt-T为初始速度;
步骤S5中,对所述导航坐标系下的三轴速度进行一次积分的表达式为:
St=St-T+Vt-TT
式中,St为导航坐标系下的位移矢量,St-T为初始位移。
优选的,步骤S6中,合成所述导航坐标系下的三轴位移的表示式为:
S=sxi+syj+szk
式中,sx、sy和sz为物体在空间坐标系的三个轴向上的移动矢量,i、j和k为空间坐标系三个轴向上的单位向量。
(三)有益效果
本发明提供了一种空间轨迹实时测量装置及方法。本发明通过所述微控制器接收三轴加速度传感器、三轴陀螺仪和三轴电子罗盘发送的传感数据并进行处理,并向通过IIC总线向三轴加速度传感器、三轴陀螺仪和三轴电子罗盘发送控制命令;同时将处理过后的传感数据通过UART外设发送给通用异步收发控制器,再通过通用异步收发控制器与上位机进行交互。本发明成本低廉;具备高稳定性;具备高精确度;且装置尺寸小,携带方便。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案,下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍,显而易见地,下面描述中的附图仅仅是本发明的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1为本发明实施例中的空间轨迹实时测量装置的结构示意图;
图2为本发明实施例中的微控制器构示意图;
图3为本发明实施例中的传感器结构示意图;
图4为本发明实施例中的通用异步收发控制器结构示意图;
图5为本发明实施例中的电源模块的结构示意图;
图6为本发明实施例中的微控制器电路图;
图7为本发明实施例中的加速度传感器电路图;
图8为本发明实施例中的陀螺仪电路图;
图9为本发明实施例中的电子罗盘电路图;
图10为本发明实施例中的供电模块电路图;
图11A为本发明实施例中的通用异步收控制器块电路图;
图11B为本发明实施例中的LED指示灯电路图;
图11C为本发明实施例中的调试下载接口电路图;
图12为本发明实施例中的空间轨迹实时测量方法的流程示意图;
图13为本发明实施例中的空间轨迹实时测量工作过程原理图。
具体实施方式
为使本发明实施例的目的、技术方案和优点更加清楚,下面将结合本发明实施例中的附图,对本发明实施例中的技术方案进行清楚、完整地描述,显然,所描述的实施例是本发明一部分实施例,而不是全部的实施例。基于本发明中的实施例,本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例,都属于本发明保护的范围。
实施例1:
如图1所示,本发明实施例提供了一种空间轨迹实时追踪装置,该装置包括:传感器、微控制器、通用异步收发控制器,
如图3所示,所述传感器包括三轴加速度传感器、三轴陀螺仪和三轴电子罗盘,所述三轴加速度传感器用用于采集三个互相垂直轴向上的加速度,所述三轴陀螺仪用于采集三个互相垂直轴向上的角速度,所述三轴电子罗盘用于采集三个互相垂直轴向上的姿态角,所述传感器通过IIC接口将传感数据发送给微控制器;
所述微控制器,用于接收所述传感器发送的传感数据并进行处理,并向通过IIC总线向所述传感器发送控制命令;同时将处理过后的传感数据通过UART外设发送给通用异步收发控制器;
如图4所示,所述通用异步收发控制器,将所述微控制器发送的处理过后的传感数据进行转换,并通过USB接口发送给上位机;并对上位机发送的命令进行转换后发送给所述微控制器。
本发明通过所述微控制器接收三轴加速度传感器、三轴陀螺仪和三轴电子罗盘发送的传感数据并进行处理,并向通过IIC总线向三轴加速度传感器、三轴陀螺仪和三轴电子罗盘发送控制命令;同时将处理过后的传感数据通过UART外设发送给通用异步收发控制器,再通过通用异步收发控制器与上位机进行交互。本发明成本低廉;具备高稳定性;具备高精确度;且装置尺寸小,携带方便。
下面对本发明实施例进行详细的说明:
如图2所示,所述的微控制器包括精简指令处理器核心、只读存储器、随机存储器、IIC接口、定时器、中断源和USART等外设;
所述只读存储器与所述精简指令处理器核心通过指令总线相连,用于存储该装置的指令信息;
所述随机存储器与所述精简指令处理器核心通过数据总线相连,用于存储该装置的数据信息;
所述精简处理器核心通过APB总线与IIC接口、定时器、中断源和USART等装置连接;IIC接口与所述传感器相连,用于对所述传感器与所述微控制器的数据和命令的交互;
所述定时器,用于所述微控制器对所述传感器施行采样周期时间间隔的采样;
所述中断源,用于在定时器产生定时器中断的情况下,所述中断源将所述精简指令处理器核心的程序指针指向采样程序的第一条指令,使得在采样周期到来的时候所述微控制器对所述传感器进行采样。
如图6所示,为本发明实施例中的微控制器电路图。
如图11C所示,该装置进一步包括:通用下载接口模块,用于将所述微控制器的连接到仿真器,通过连接JLINK仿真器的Vref,SWCLK,SWDIO和GND接口对设备进行程序的下载和调试。其中的SWCLK和SWDIO别连接着处理器模块的28(PA14),25(PA13)脚相连接。所述的调试下载接口使用的是SWD模式负责对装置的调试和对传感器模块的ROM中写入三维空间实时轨迹追踪的控制程序,并进行调试。
如图5所示,该装置进一步包括:电源模块,所述电源模块包括2个LDO,一个LDO负责对所述微控制器供电,另一个LDO负责对所述传感器供电。如图10所示,为本发明实施例中的供电模块电路图。本发明实施例采用了2个LDO分别为微控制器和传感器供电,其中U2为微控制器单元供电,U3为传感器单元供电。C1,C4分别为U2和U3的输入电源滤波电容,C2,C5分别为U2和U3的旁路电容,C3和C6分别为U2和U3的输出电源滤波电容。
如图11B所示,该装置进一步包括:LED指示灯,所述LED指示灯通过IO接口与所述微控制器相连,所述微控制器通过控制LED指示灯的亮灭来指示装置运行状态。LED模块由2个发光二极管组成并与处理器模块相连。LED1发光二极管,负责显示装置的运行状态是否正常,LED2负责显示装置是否正在进行轨迹测量。
另外:如图7所示,本发明实施例中三轴加速度传感器7脚9脚分别连接到SCL线与SDA线,C16和C17为电源的滤波电容,性能为:内置3个14bit的AD转换器能够测量XYZ三个垂直方向上的加速度值。带有3个14bit的AD转换器,在量程为2g的情况下分辨率可达0.25mg/LSB。在输出频率为1.2Khz的低噪音模式下噪音密度为150ug/sqrt(HZ)。在0g情况下的偏移为±15mg。内置可编程的低通滤波器支持10,20,40,75,150,300,600,1200hz的频率;内置1hz的高通滤波器;内置0.2hz-300hz的带通滤波器。输出频率可达1khz。工作温度在-40℃到85℃。在低噪音模式下的工作电流仅为0.975ma。在25℃的情况下误差为2.0%。并带有自我检测功能。支持400khz的IIC总线。
如图8所示,本发明实施例中三轴陀螺仪的2脚和3脚分别和SCL线与SDA线相连,C21和C22为电源滤波电容,C23,C24,R8组成了PLL的滤波电路,其性能为:内置3个16bit的AD转换器能够测量XYZ三个垂直方向上的角速度值。在量程为-250dps~250dps的情况下分辨率可达8.75dps/LSB。非线性程度为0.2%。在带宽为50hz情况下的噪音密度为0.03dps/sqrt(hz)。内置可编程的高通滤波器输出频率可达800hz。工作温度为-40℃到85℃。并带有自我检测功能。支持400khz的IIC总线。
如图9所示,本发明实施例中所述的三轴电子罗盘其性能为:内置3个12bit的AD转换器能够测量XYZ三个垂直方向上的地磁分量。在量程为-1.3Ga~1.3Ga的情况下分辨率为0.92mGa/LSB。在单一测量模式下数据输出频率为160hz。并且带有自我检测功能。
如图11A所示,所述的通用异步收发控制器的第25脚和第26脚连接着微控制器的第21脚(txd)和第22脚(rxd),通用异步收发控制器的第4脚连接着MINI USB接口的D+脚,通用异步收发控制器的第5脚连接着MINI USB接口的D-脚。C14和C14为供电滤波电容,其性能为:支持全速USB2.0规范,传输速度可达12Mbps,支持USB的暂停状态。对UART的规范:支持5,6,7,8的数据位;支持1,1.5,2的停止位,支持300bps到1Mbps的波特率,带有576字节的接收缓存;带有640字节的发送缓存;硬件支持的握手协议;并且支持键盘的事件字符的传输。该模块支持目前市面上主流的操作系统可以在Windows Vista/XP/Server2003/2000,Windows CE5.0以上版本和MacOS上使用。仅仅通过USB的5V电源就可对其供电。最大工作电流仅为500ma。
实施例2:
如图12所示,本发明实施例提供了一种空间轨迹实时追踪方法,该方法包括步骤:
S1、装置初始化,微控制器开始定时,预订延时时间后,微控制器接收三轴加速度传感器、三轴陀螺仪和三轴电子罗盘将采集到的传感器数据;
S2、所述微控制器对所述传感器数据进行处理,得到装置的初始姿态角,并依据所述姿态角确定初始旋转四元数(q0,q1,q2,q3);
S3、依据所述三轴陀螺仪传输的角速度数据更新所述初始旋转四元数,得到旋转矩阵,并通过所述旋转矩阵将所述三轴加速度传感器传输的三轴加速度数据分解到导航坐标系;
S4、对所述导航坐标系下的三轴加速度分别进行一次积分,得到导航坐标系下的三轴速度;
S5、对所述导航坐标系下的三轴速度进行一次积分,得到导航坐标系下的三轴位移;
S6、合成所述导航坐标系下的三轴位移,得到对物体进行实时追踪的空间轨迹。
优选的,步骤S2中的姿态角包括:俯仰角Pitch、横滚角Roll和航向角Yaw;
其中,俯仰角Pitch的计算表达式为:
Pitch=asin(AccY/(AccX*AccX+AccY*AccY+AccZ*AccZ)2);
其中,横滚角Roll的计算表达式为:
Roll=asin(AccX/(AccX*AccX+AccY*AccY+AccZ*AccZ)2);
其中,航向角Yaw的计算表达式为:
Yaw=arctan((HycosRoll1+HzsinRoll1)/(HxcosPitch1+HysinPitch1sinRoll1-HzsinPitch1cosRoll1));
式中,AccX、AccY和AccZ为三轴加速度传感器传输的x、y和z轴传感数据;Hx、Hy、Hz为三轴电子罗盘传输的x、y和z轴传感数据。
优选的,步骤S3中更新后的所述旋转四元数为:
式中,wx、wy和wz为三轴陀螺仪传输的x、y和z轴传感数据;
步骤S3中旋转矩阵为:
式中,q0,q1,q2,q3为上一个状态的旋转四元数(q0,q1,q2,q3)的每个分量的值;
步骤S3中将所述三轴加速度传感器传输的三轴加速度数据分解到导航坐标系的表达式为:
优选的,步骤S4中,对所述导航坐标系下的三轴加速度分别进行一次积分的表达式为:
Vt=Vt-T+at-TT
式中,Vt为导航坐标系下的速度矢量,Vt-T为初始速度;
步骤S5中,对所述导航坐标系下的三轴速度进行一次积分的表达式为:
St=St-T+Vt-TT
式中,St为导航坐标系下的位移矢量,St-T为初始位移。
优选的,步骤S6中,合成所述导航坐标系下的三轴位移的表示式为:
S=sxi+syj+szk
式中,sx、sy和sz为物体在空间坐标系的三个轴向上的移动矢量,i、j和k为空间坐标系三个轴向上的单位向量。
如图13所示,为本发明实施例中的空间轨迹实时测量工作过程原理图。
综上,本发明能够解决在没有移动基站或者GPS信号时对人们在室内无法进行定位导航;救援人员在地下无法进行位置确定;油井缺少对采油装置位移实时测量的装置;在物理实验缺少对轨迹实时追踪的装置,等等需要进行空间轨迹实时追踪问题。
需要说明的是,在本文中术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者装置不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者装置所固有的要素。在没有更多限制的情况下,由语句“包括一个……”限定的要素,并不排除在包括所述要素的过程、方法、物品或者装置中还存在另外的相同要素。
以上实施例仅用以说明本发明的技术方案,而非对其限制;尽管参照前述实施例对本发明进行了详细的说明,本领域的普通技术人员应当理解:其依然可以对前述各实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换;而这些修改或者替换,并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。
Claims (10)
1.一种空间轨迹实时追踪装置,其特征在于,该装置包括:传感器、微控制器、通用异步收发控制器,
所述传感器包括三轴加速度传感器、三轴陀螺仪和三轴电子罗盘,所述三轴加速度传感器用于采集三个互相垂直轴向上的加速度,所述三轴陀螺仪用于采集三个互相垂直轴向上的角速度,所述三轴电子罗盘用于采集三个互相垂直轴向上的姿态角,所述传感器通过IIC接口将传感数据发送给微控制器;
所述微控制器,用于接收所述传感器发送的传感数据并进行处理,同时通过IIC总线向所述传感器发送控制命令;然后将处理过后的传感数据通过UART外设发送给通用异步收发控制器;
所述通用异步收发控制器,将所述微控制器发送的处理过后的传感数据进行转换,并通过USB接口发送给上位机;同时把上位机发送的命令进行转换后发送给所述的微控制器。
2.如权利要求1所述的空间轨迹实时追踪装置,其特征在于,所述的微控制器包括精简指令处理器核心、只读存储器、随机存储器、IIC接口、定时器、中断源和USART等外设;
所述只读存储器与所述精简指令处理器核心通过指令总线相连,用于存储该装置的指令信息;
所述随机存储器与所述精简指令处理器核心通过数据总线相连,用于存储该装置的数据信息;
所述精简处理器核心通过APB总线与IIC接口、定时器、中断源和USART等装置连接;IIC接口与所述传感器相连,用于对所述传感器与所述微控制器的数据和命令的交互;
所述定时器,用于所述微控制器对所述传感器施行采样周期时间间隔的采样;
所述中断源,用于在定时器产生定时器中断的情况下,所述中断源将所述精简指令处理器核心的程序指针指向采样程序的第一条指令,使得在采样周期到来的时候所述微控制器对所述传感器进行采样。
3.如权利要求1所述的空间轨迹实时追踪装置,其特征在于,该装置进一步包括:通用下载接口模块,用于将所述微控制器的连接到仿真器,通过仿真器对所述微控制器的程序进行下载和调试。
4.如权利要求1所述的空间轨迹实时追踪装置,其特征在于,该装置进一步包括:电源模块,所述电源模块包括2个LDO,一个LDO负责对所述微控制器供电,另一个LDO负责对所述传感器供电。
5.如权利要求1所述的空间轨迹实时追踪装置,其特征在于,该装置进一步包括:LED指示灯,所述LED指示灯通过IO接口与所述微控制器相连,所述微控制器通过控制LED指示灯的亮灭来指示装置运行状态。
6.一种空间轨迹实时追踪方法,其特征在于,该方法包括步骤:
S1、装置初始化,微控制器开始定时,预订延时时间后,微控制器接收三轴加速度传感器、三轴陀螺仪和三轴电子罗盘将采集到的传感器数据;
S2、所述微控制器对所述传感器数据进行处理,得到装置的初始姿态角,并依据所述姿态角确定初始旋转四元数(q0,q1,q2,q3);
S3、依据所述三轴陀螺仪传输的角速度数据更新所述初始旋转四元数,得到旋转矩阵,并通过所述旋转矩阵将所述三轴加速度传感器传输的三轴加速度数据分解到导航坐标系;
S4、对所述导航坐标系下的三轴加速度分别进行一次积分,得到导航坐标系下的三轴速度;
S5、对所述导航坐标系下的三轴速度进行一次积分,得到导航坐标系下的三轴位移;
S6、合成所述导航坐标系下的三轴位移,得到对物体进行实时追踪的空间轨迹。
7.如权利要求6所述的空间轨迹实时追踪方法,其特征在于,步骤S2中的初始姿态角包括:初始俯仰角Pitch、初始横滚角Roll和初始航向角Yaw;
其中,初始俯仰角Pitch的计算表达式为:
Pitch=asin(AccY/(AccX*AccX+AccY*AccY+AccZ*AccZ)2);
其中,初始横滚角Roll的计算表达式为:
Roll=asin(AccX/(AccX*AccX+AccY*AccY+AccZ*AccZ)2);
其中,初始航向角Yaw的计算表达式为:
Yaw=arctan((HycosRoll1+HzsinRoll1)/(HxcosPitch1+HysinPitch1sinRoll1-HzsinPitch1cosRoll1));
式中,AccX、AccY和AccZ为三轴加速度传感器传输的x、y和z轴传感数据;Hx、Hy、Hz为三轴电子罗盘传输的x、y和z轴传感数据。
8.如权利要求6所述的空间轨迹实时追踪方法,其特征在于,步骤S3中更新后的所述旋转四元数为:
式中,ωx、ωy和ωz为三轴陀螺仪传输的x、y和z轴传感数据;
步骤S3中旋转矩阵为:
式中,q0,q1,q2,q3为上一个状态的旋转四元数(q0,q1,q2,q3)的每个分量的值;
步骤S3中将所述三轴加速度传感器传输的三轴加速度数据分解到导航坐标系的表达式为:
式中,T是旋转矩阵,为设备坐标系下的加速度矢量,(ax,ay,az)为导航坐标系下加速度矢量。
9.如权利要求6所述的空间轨迹实时追踪方法,其特征在于,步骤S4中,对所述导航坐标系下的三轴加速度分别进行一次积分的表达
Vt=Vt-T+at-TT
式中,Vt为导航坐标系下的速度矢量,Vt-T为初始速度;
步骤S5中,对所述导航坐标系下的三轴速度进行一次积分的表达式为:
St=St-T+Vt-TT
式中,St为导航坐标系下的位移矢量,St-T为初始位移。
10.如权利要求6所述的空间轨迹实时追踪方法,其特征在于,步骤S6中,合成所述导航坐标系下的三轴位移的表示式为:
S=sxi+syj+szk
式中,sx、sy和sz为物体在空间坐标系的三个轴向上的移动矢量,i、j和k为空间坐标系三个轴向上的单位向量。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410060655.8A CN103884338A (zh) | 2014-02-21 | 2014-02-21 | 空间轨迹实时追踪装置及方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201410060655.8A CN103884338A (zh) | 2014-02-21 | 2014-02-21 | 空间轨迹实时追踪装置及方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN103884338A true CN103884338A (zh) | 2014-06-25 |
Family
ID=50953362
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201410060655.8A Pending CN103884338A (zh) | 2014-02-21 | 2014-02-21 | 空间轨迹实时追踪装置及方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN103884338A (zh) |
Cited By (13)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104765476A (zh) * | 2015-04-13 | 2015-07-08 | 深圳市飞马与星月科技研究有限公司 | 手写轨迹生成方法和装置 |
CN105056503A (zh) * | 2015-07-28 | 2015-11-18 | 湖南人文科技学院 | 一种训练用的标枪测绘、标定系统及测绘方法 |
CN105115500A (zh) * | 2015-07-08 | 2015-12-02 | 成都西可科技有限公司 | 一种实现空间轨迹追踪的方法 |
CN106097843A (zh) * | 2016-08-30 | 2016-11-09 | 广东工业大学 | 一种运动学实验系统及方法 |
CN106289247A (zh) * | 2016-07-26 | 2017-01-04 | 北京长城电子装备有限责任公司 | 基于惯性传感器的室内定位装置 |
CN106323225A (zh) * | 2016-08-31 | 2017-01-11 | 潘卫东 | 基于mems传感技术的空间位移测量传感器 |
CN106683037A (zh) * | 2015-11-06 | 2017-05-17 | 阿里巴巴集团控股有限公司 | 三维可视化移动轨迹数据的方法和设备 |
CN104501808B (zh) * | 2014-11-19 | 2017-06-30 | 广东欧珀移动通信有限公司 | 路径追踪方法及装置 |
CN107036568A (zh) * | 2017-06-01 | 2017-08-11 | 中国计量大学 | 空间大尺寸轨迹检测装置及方法 |
CN108534775A (zh) * | 2018-06-25 | 2018-09-14 | 中国铁道科学研究院集团有限公司 | 基于捷联式惯性导航系统的空间轨迹重建方法及装置 |
CN108955672A (zh) * | 2018-06-07 | 2018-12-07 | 重庆交通大学 | 一种隧道注浆浆液轨迹监测系统及方法 |
CN109874045A (zh) * | 2019-01-30 | 2019-06-11 | 深圳岚锋创视网络科技有限公司 | 一种便携式终端及其获取陀螺仪数据进行视频处理的方法 |
CN110579786A (zh) * | 2019-07-19 | 2019-12-17 | 北京理工新源信息科技有限公司 | 定位方法及系统、导航方法及系统和一种车辆管理终端 |
-
2014
- 2014-02-21 CN CN201410060655.8A patent/CN103884338A/zh active Pending
Cited By (18)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN104501808B (zh) * | 2014-11-19 | 2017-06-30 | 广东欧珀移动通信有限公司 | 路径追踪方法及装置 |
CN104765476A (zh) * | 2015-04-13 | 2015-07-08 | 深圳市飞马与星月科技研究有限公司 | 手写轨迹生成方法和装置 |
CN104765476B (zh) * | 2015-04-13 | 2017-11-10 | 深圳市飞马与星月科技研究有限公司 | 手写轨迹生成方法和装置 |
CN105115500B (zh) * | 2015-07-08 | 2018-09-18 | 成都西可科技有限公司 | 一种实现空间轨迹追踪的方法 |
CN105115500A (zh) * | 2015-07-08 | 2015-12-02 | 成都西可科技有限公司 | 一种实现空间轨迹追踪的方法 |
CN105056503A (zh) * | 2015-07-28 | 2015-11-18 | 湖南人文科技学院 | 一种训练用的标枪测绘、标定系统及测绘方法 |
CN106683037A (zh) * | 2015-11-06 | 2017-05-17 | 阿里巴巴集团控股有限公司 | 三维可视化移动轨迹数据的方法和设备 |
CN106289247A (zh) * | 2016-07-26 | 2017-01-04 | 北京长城电子装备有限责任公司 | 基于惯性传感器的室内定位装置 |
CN106097843A (zh) * | 2016-08-30 | 2016-11-09 | 广东工业大学 | 一种运动学实验系统及方法 |
CN106323225A (zh) * | 2016-08-31 | 2017-01-11 | 潘卫东 | 基于mems传感技术的空间位移测量传感器 |
CN107036568A (zh) * | 2017-06-01 | 2017-08-11 | 中国计量大学 | 空间大尺寸轨迹检测装置及方法 |
CN108955672A (zh) * | 2018-06-07 | 2018-12-07 | 重庆交通大学 | 一种隧道注浆浆液轨迹监测系统及方法 |
CN108955672B (zh) * | 2018-06-07 | 2021-02-02 | 重庆交通大学 | 一种隧道注浆浆液轨迹监测系统及方法 |
CN108534775A (zh) * | 2018-06-25 | 2018-09-14 | 中国铁道科学研究院集团有限公司 | 基于捷联式惯性导航系统的空间轨迹重建方法及装置 |
CN108534775B (zh) * | 2018-06-25 | 2020-04-17 | 中国铁道科学研究院集团有限公司 | 基于捷联式惯性导航系统的空间轨迹重建方法及装置 |
CN109874045A (zh) * | 2019-01-30 | 2019-06-11 | 深圳岚锋创视网络科技有限公司 | 一种便携式终端及其获取陀螺仪数据进行视频处理的方法 |
CN109874045B (zh) * | 2019-01-30 | 2021-04-06 | 影石创新科技股份有限公司 | 一种便携式终端及其获取陀螺仪数据进行视频处理的方法 |
CN110579786A (zh) * | 2019-07-19 | 2019-12-17 | 北京理工新源信息科技有限公司 | 定位方法及系统、导航方法及系统和一种车辆管理终端 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN103884338A (zh) | 空间轨迹实时追踪装置及方法 | |
US11327577B2 (en) | Multi-function stylus with sensor controller | |
CN110134143A (zh) | 一种电力巡检方法、系统及电子设备和存储介质 | |
CN103226398B (zh) | 基于微惯性传感器网络技术的数据手套 | |
WO2017148222A1 (zh) | 多传感器信息采集导航系统及方法 | |
CN104567931B (zh) | 一种室内惯性导航定位的航向漂移误差消除方法 | |
CN104407709B (zh) | 可穿戴设备的穿戴状态的处理方法及装置 | |
CN203759869U (zh) | 一种手势感应型飞行器遥控器 | |
CN104223613B (zh) | 智能手环显示控制系统及智能手环 | |
CN103591927B (zh) | 井下钻机姿态测量仪及其测量方法 | |
CN103900571B (zh) | 一种基于惯性坐标系旋转型捷联惯导系统的载体姿态测量方法 | |
US10061400B2 (en) | Input device and control system | |
CN104199459A (zh) | 一种基于手机蓝牙技术的水下机器人控制系统 | |
CN103218065B (zh) | 一种自动变更笔触类型的手写输入装置 | |
CN107449422B (zh) | 一种高动态载体位姿实时测量装置 | |
CN104345904A (zh) | 一种手指式空中鼠标 | |
CN104266648A (zh) | Android平台MARG传感器的室内定位系统 | |
Karpov et al. | Designing the platform for monitoring and visualization orientation in Euler angles | |
US9834316B2 (en) | Portable, externally mounted device for aircraft to provide user selected readings of airspeed, pressure, temperature, orientation, heading, acceleration, and angular rate | |
CN103438883A (zh) | 定位导航及轨迹绘图仪 | |
CN102192739A (zh) | 一种矿井导航仪及导航系统 | |
EP3785049B1 (en) | Method for calibrating sensor or azimuth information obtained through sensor, based on azimuth information obtained using satellite positioning circuit, and electronic device supporting the same | |
CN104655134A (zh) | 一种基于gps时标的多传感器数据采集系统 | |
CN102592161A (zh) | 电子工牌及基于电子工牌的出勤管理系统 | |
CN111147152A (zh) | 一种便携式多目标通讯装置及通讯方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
RJ01 | Rejection of invention patent application after publication | ||
RJ01 | Rejection of invention patent application after publication |
Application publication date: 20140625 |