发明内容
本公开要解决的一个技术问题是提供一种交通工具导航数据融合方法、装置和***,能够提高导航***的稳定性。
根据本公开一方面,提出一种交通工具导航数据融合方法,包括:获取惯性传感器采集的交通工具的导航状态数据;基于导航状态数据中的惯性速度数据建立观测方程;基于观测方程对导航状态数据求偏导获取观测雅可比矩阵;基于视觉传感器采集的图像信息获取交通工具的光流速度数据;基于光流速度数据确定光流角速度数据;基于观测雅可比矩阵,利用光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据。
在一个实施例中,基于导航状态数据中的惯性速度数据建立观测方程包括:基于载体系到传感器系的姿态矩阵和载体系到惯性系的姿态矩阵的转置矩阵,以及导航状态数据中的惯性速度数据得到交通工具在载体系的相对速度数据,其中,导航状态数据为惯性导航状态数据;确定视觉传感器与地面的距离信息;基于交通工具在载体系的相对速度数据与距离信息的比值,确定观测方程,其中,观测方程包括交通工具在载体系的第一坐标轴角速度数据和第二坐标轴角速度数据。
在一个实施例中,基于观测方程对导航状态数据求偏导获取观测雅可比矩阵包括:基于第一坐标轴角速度数据对导航状态数据中的状态量求偏导获得第一坐标轴观测雅可比矩阵;基于第二坐标轴角速度数据对导航状态数据中的状态量求偏导获得第二坐标轴观测雅可比矩阵。
在一个实施例中,根据视觉传感器采集图像信息的时间、交通工具的倾斜角以及视觉传感器与地面的距离信息确定根据光流角速度数据是否可用;若光流角速度数据可用,则利用光流角速度数据对导航状态数据进行融合。
在一个实施例中,若惯性传感器采集状态导航数据的时间与视觉传感器采集图像信息的时间之差小于时间阈值,交通工具的倾斜角小于倾斜角阈值,并且视觉传感器与地面的距离信息大于距离阈值,则确定光流角速度数据可用。
在一个实施例中,基于观测雅可比矩阵,利用光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据包括:基于观测雅可比矩阵和观测方程构建非线性滤波模型;将光流角速度数据和导航状态数据输入至非线性滤波模型,得到交通工具的融合导航数据。
在一个实施例中,非线性滤波模型为卡尔曼滤波模型。
根据本公开的另一方面,还提出一种交通工具导航数据融合装置,包括:导航数据获取单元,被配置为获取惯性传感器采集的交通工具的导航状态数据;观测方程建立单元,被配置为基于导航状态数据中的惯性速度数据建立观测方程;观测矩阵获取单元,被配置为基于观测方程对导航状态数据求偏导获取观测雅可比矩阵;光流速度获取单元,被配置为基于视觉传感器采集的图像信息获取交通工具的光流速度数据;光流角速度确定单元,被配置为基于光流速度数据确定光流角速度数据;导航数据融合单元,被配置为基于观测雅可比矩阵,利用光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据。
在一个实施例中,光流角速度可用确定单元,被配置为根据视觉传感器采集图像信息的时间、交通工具的倾斜角以及视觉传感器与地面的距离信息确定根据光流角速度数据是否可用;导航数据融合单元被配置为若光流角速度数据可用,则利用光流角速度数据对导航状态数据进行融合。
根据本公开的另一方面,还提出一种交通工具导航数据融合装置,包括:存储器;以及耦接至存储器的处理器,处理器被配置为基于存储在存储器的指令执行如上述的交通工具导航数据融合方法。
根据本公开的另一方面,还提出一种交通工具导航数据融合***,包括:惯性传感器,被配置为采集交通工具的导航状态数据;视觉传感器,被配置为采集图像信息,其中,根据图像信息获取交通工具的光流速度数据;以及上述交通工具导航数据融合装置。
根据本公开的另一方面,还提出一种计算机可读存储介质,其上存储有计算机程序指令,该指令被处理器执行时实现上述的交通工具导航数据融合方法的步骤。
与相关技术相比,本公开实施例基于惯性传感器采集的导航状态数据中的惯性速度数据建立观测方程,基于观测方程对导航状态数据求偏导获取观测雅可比矩阵,然后基于观测雅可比矩阵,利用视觉传感器得到的光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据,由于将视觉传感器的数据与惯性传感器的数据进行融合,因此能够提高导航***的稳定性。
通过以下参照附图对本公开的示例性实施例的详细描述,本公开的其它特征及其优点将会变得清楚。
具体实施方式
现在将参照附图来详细描述本公开的各种示例性实施例。应注意到:除非另外具体说明,否则在这些实施例中阐述的部件和步骤的相对布置、数字表达式和数值不限制本公开的范围。
同时,应当明白,为了便于描述,附图中所示出的各个部分的尺寸并不是按照实际的比例关系绘制的。
以下对至少一个示例性实施例的描述实际上仅仅是说明性的,决不作为对本公开及其应用或使用的任何限制。
对于相关领域普通技术人员已知的技术、方法和设备可能不作详细讨论,但在适当情况下,所述技术、方法和设备应当被视为授权说明书的一部分。
在这里示出和讨论的所有示例中,任何具体值应被解释为仅仅是示例性的,而不是作为限制。因此,示例性实施例的其它示例可以具有不同的值。
应注意到:相似的标号和字母在下面的附图中表示类似项,因此,一旦某一项在一个附图中被定义,则在随后的附图中不需要对其进行进一步讨论。
为使本公开的目的、技术方案和优点更加清楚明白,以下结合具体实施例,并参照附图,对本公开进一步详细说明。
图1为本公开交通工具导航数据融合方法的一个实施例的流程示意图。
在步骤110,获取惯性传感器采集的交通工具的导航状态数据。交通工具例如为无人机、无人车等无人设备,导航状态数据可以包括多维状态量。
在步骤120,基于导航状态数据中的惯性速度数据建立观测方程。惯性速度数据例如包括北向速度vn,东向速度ve和地向速度vd,观测方程包括交通工具在载体系的第一坐标轴角速度数据和第二坐标轴角速度数据,第一坐标轴角速度数据例如为X轴角速度数据flowrateX和第二坐标轴角速度数据例如为Y轴角速度数据flowrateY。
在步骤130,基于观测方程对导航状态数据求偏导获取观测雅可比矩阵。例如,基于X轴角速度数据flowrateX对导航状态数据中的状态量求偏导获得X轴观测雅可比矩阵H_flowX;以及基于Y轴角速度数据flowrateY对导航状态数据中的状态量求偏导获得Y轴观测雅可比矩阵H_flowY,H_flowX和H_flowY构成观测雅可比矩阵H。
在步骤140,基于视觉传感器采集的图像信息获取交通工具的光流速度数据。视觉传感器例如为光流传感器,利用光流传感器获得的图像信息可以转换为载体系的速度,其中,将图像信息转换为载体系的速度的方法有LK光流法、块匹配法、Horm-Schunck法等。
步骤110和步骤140可以同时执行也可以不分先后执行。
在步骤150,基于光流速度数据确定光流角速度数据。
在步骤160,基于观测雅可比矩阵,利用光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据。例如,基于观测雅可比矩阵和观测方程构建非线性滤波模型,将光流角速度数据和导航状态数据输入至非线性滤波模型,得到交通工具的融合导航数据。
在该实施例中,基于惯性传感器采集的导航状态数据中的惯性速度数据建立观测方程,基于观测方程对导航状态数据求偏导获取观测雅可比矩阵,然后基于观测雅可比矩阵,利用视觉传感器得到的光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据,因此,即使在没有GPS的情况下,将视觉传感器的数据与惯性传感器的数据进行融合,能够提高导航***的稳定性。
图2为本公开交通工具导航数据融合方法的另一个实施例的流程示意图。
在步骤210,获取惯性传感器采集的无人机惯性导航状态数据。惯性状态导航数据中的状态量如表1所示,其中,表1中示出22维状态量。本领域的技术人员应当理解,可以根据实际情况设置更多维度或更少维度的状态量。
表1
在步骤220,基于载体系到传感器系的姿态矩阵和载体系到惯性系的姿态矩阵的转置矩阵,以及惯性导航状态数据中的惯性速度数据得到无人机在载体系的相对速度数据。
例如,利用公式确定无人机在载体系的相对速度数据relvel。其中,/>为载体系到传感器系的姿态矩阵,/>为载体系到惯性系的姿态矩阵。载体系是指无人机的坐标系,传感器系指光流传感器的坐标系。
在步骤230,确定光流传感器与地面的距离信息。
在步骤240,基于无人机在载体系的相对速度数据与距离信息的比值,确定观测方程。其中,观测方程包括无人机在载体系的X轴角速度数据flowrateX和无人机在载体系的Y轴角速度数据flowrateY。
例如,光流传感器与地面的距离为range,X轴角速度数据flowrateX=relvel(2)/range,Y轴角速度数据flowrateY=-relvel(1)/range,relvel(2)为无人机在载体系的X轴的相对速度,relvel(1)为无人机在载体系的Y轴的相对速度。由flowrateX和flowrateY构成观测方程。
在步骤250,基于X轴角速度数据对导航状态数据中的状态量求偏导获得X轴观测雅可比矩阵H_flowX;基于Y轴角速度数据对导航状态数据中的状态量求偏导获得Y轴观测雅可比矩阵H_flowY。
例如,利用flowrateX对表1中的所有状态量求偏导,得到X轴观测雅可比矩阵H_flowX为:
H_flowx(8)=H_flowx(9)=...=H_flowx(24)=0
例如,利用flowrateY对表1中的所有状态量求偏导,得到Y轴观测雅可比矩阵H_flowY为:
H_flowy(8)=H_flowy(9)=...=H_flowy(24)=0
在步骤260,基于光流传感器采集的图像信息获取无人机在载体系的光流速度数据。
在步骤270,基于光流速度数据确定光流角速度数据Z(k)。
在一个实施例中,需要先判断光流传感器采集的数据是否可用。例如,根据光流传感器采集图像信息的时间、无人机的倾斜角以及光流传感器与地面的距离信息确定根据光流角速度数据是否可用。
若光流传感器采集数据的时间为time_flow,惯性传感器采集数据的时间为time_imu,以惯性传感器采集数据的时间为主,检测时间间隔,判断惯性传感器采集数据的时间与光流传感器采集数据的时间之差是否小于时间阈值,例如,time_imu-time_flow是否小于1s。
根据状态导航数据中的四元数状态量可以得到无人机的姿态矩阵,根据姿态矩阵的第三行第三列可以得到无人机的俯仰角cosθ和横滚角cosγ,根据俯仰角和横滚角得到无人机的倾斜角。当cosθcosγ>δ,则说明倾斜角合格,其中,δ可以根据实际情况进行设定,例如,若δ设置为0.71,则说明倾斜角小于40.7°。
计算光流传感器到地面的高度差high_flow-position_z,若high_flow-position_z小于0.5米,则说明无人机可能还没有起飞,光流传感器还不能聚焦,此时,可以设置光流传感器的输出值为0,用于约束误差。
当惯性传感器采集数据的时间与光流传感器采集数据的时间之差小于时间阈值,无人机的倾斜角小于倾斜角阈值,光流传感器到地面的距离大于距离阈值,则确定光流角速度数据可用。
在步骤280,基于观测雅可比矩阵和观测方程构建非线性滤波模型。其中,非线性滤波模型例如为卡尔曼滤波模型。
在步骤290,将光流角速度数据和导航状态数据输入至非线性滤波模型,得到无人机的融合导航数据。
在一个实施例中,可以先设定光流传感器的测量噪声为R,例如R为0.05弧度/s,则可以基于测量噪声、观测雅可比矩阵和预测误差方差阵确定增益调节参数K(k),例如,可以利用公式K(k)=P-(k)HT(HP-(k)HT+R)-1确定增益调节参数K(k),其中,P-(k)为预测误差方差阵。
在一个实施例中,可以判断P-(k)是否可用,例如,利用公式确定革新方差/>若/>大于测量噪声R,则说明P-(k)可用。
在具体数据融合时,可以利用以下公式:
其中,利用光流传感器的实际输出值Z(k)对导航状态数据进行状态修正。是上一次的导航状态数据的状态值,/>为最终修正后的导航状态数据的状态值。在完成本次修正的同时,对本次的协方差矩阵依据公式进行更新,其中,/>为滤波误差方差阵。更新之后,再次进行预测,然后再用观测雅可比矩阵进行修正,这样循环处理,达到数据融合目的。
在该实施例中,可以在传统IMU/GPS融合的前提下,将光流传感器采集的数据与惯性传感器采集的数据进行融合,增强了导航***的稳定性和适应性,减少因GPS数据丢失导致的人力、财产损失。
图3为本公开交通工具导航数据融合装置的一个实施例的结构示意图。该装置包括导航数据获取单元310、观测方程建立单元320、观测矩阵获取单元330、光流速度获取单元340、光流角速度确定单元350和导航数据融合单元360。
导航数据获取单元310被配置为获取惯性传感器采集的交通工具的导航状态数据。交通工具例如为无人机、无人车等,导航状态数据可以包括多维状态量。
观测方程建立单元320被配置为基于导航状态数据中的惯性速度数据建立观测方程。惯性速度数据例如包括北向速度vn,东向速度ve和地向速度vd,观测方程包括交通工具在载体系的X轴角速度数据flowrateX和在载体系的Y轴角速度数据flowrateY。
观测矩阵获取单元330被配置为基于观测方程对导航状态数据求偏导获取观测雅可比矩阵。例如,基于X轴角速度数据flowrateX对导航状态数据中的状态量求偏导获得X轴观测雅可比矩阵H_flowX;以及基于Y轴角速度数据flowrateY对导航状态数据中的状态量求偏导获得Y轴观测雅可比矩阵H_flowY,H_flowX和H_flowY构成观测雅可比矩阵H。
光流速度获取单元340被配置为基于视觉传感器采集的图像信息获取交通工具的光流速度数据。视觉传感器例如为光流传感器,利用光流传感器获得的图像信息可以转换为载体系的速度,其中,将图像信息转换为载体系的速度的方法有LK光流法、块匹配法、Horm-Schunck法等。
光流角速度确定单元350被配置为基于光流速度数据确定光流角速度数据。
导航数据融合单元360被配置为基于观测雅可比矩阵,利用光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据。
例如,基于观测雅可比矩阵和观测方程构建非线性滤波模型,将光流角速度数据和导航状态数据输入至非线性滤波模型,得到交通工具的融合导航数据。
在该实施例中,基于惯性传感器采集的导航状态数据中的惯性速度数据建立观测方程,基于观测方程对导航状态数据求偏导获取观测雅可比矩阵,然后基于观测雅可比矩阵,利用视觉传感器得到的光流角速度数据对导航状态数据进行融合,确定交通工具的融合导航数据,因此,即使在没有GPS的情况下,将视觉传感器的数据与惯性传感器的数据进行融合,能够提高导航***的稳定性。
图4为本公开无人机导航数据融合装置的另一个实施例的结构示意图。该装置包括导航数据获取单元410、观测方程建立单元420、观测矩阵获取单元430、光流速度获取单元440、光流角速度确定单元450、光流角速度可用确定单元460和导航数据融合单元470。
导航数据获取单元410被配置为获取惯性传感器采集的无人机的惯性导航状态数据。惯性状态导航数据中的状态量如表1所示,其中,表1中示出22维状态量。
观测方程建立单元420被配置为基于载体系到传感器系的姿态矩阵和载体系到惯性系的姿态矩阵的转置矩阵,以及导航状态数据中的惯性速度数据得到无人机在载体系的相对速度数据;确定光流传感器与地面的距离信息;基于无人机在载体系的相对速度数据与距离信息的比值,确定观测方程。其中,观测方程包括无人机在载体系的X轴角速度数据和无人机在载体系的Y轴角速度数据。
观测矩阵获取单元430被配置为基于观测方程对导航状态数据求偏导获取观测雅可比矩阵。例如,基于X轴角速度数据对导航状态数据中的状态量求偏导获得X轴观测雅可比矩阵H_flowX;基于Y轴角速度数据对导航状态数据中的状态量求偏导获得Y轴观测雅可比矩阵H_flowY。
光流速度获取单元440被配置为基于光流传感器采集的图像信息获取无人机的光流速度数据。
光流角速度确定单元450被配置为基于光流速度数据确定光流角速度数据。
光流角速度可用确定单元460被配置为根据光流传感器采集图像信息的时间、无人机的倾斜角以及光流传感器与地面的距离信息确定根据光流角速度数据是否可用。例如,若惯性传感器采集状态导航数据的时间与光流传感器采集图像信息的时间之差小于时间阈值,无人机的倾斜角小于倾斜角阈值,并且光流传感器与地面的距离信息大于距离阈值,则确定光流角速度数据可用。
导航数据融合单元470被配置为若光流角速度数据可用,则基于观测雅可比矩阵和观测方程构建非线性滤波模型,将光流角速度数据和导航状态数据输入至非线性滤波模型,得到无人机的融合导航数据。其中,非线性滤波模型例如为卡尔曼滤波模型。
在该实施例中,可以在传统IMU/GPS融合的前提下,将光流传感器采集的数据与惯性传感器采集的数据进行融合,增强了导航***的稳定性和适应性,减少因GPS数据丢失导致的人力、财产损失。
图5为本公开交通工具导航数据融合装置的再一个实施例的结构示意图。该装置包括:存储器510和处理器520,其中,存储器510可以是磁盘、闪存或其它任何非易失性存储介质。存储器510被配置为存储图1、2所对应实施例中的指令。处理器520耦接至存储器510,可以作为一个或多个集成电路来实施,例如微处理器或微控制器。该处理器520被配置为执行存储器中存储的指令。
在一个实施例中,还可以如图6所示,该装置600包括存储器610和处理器620。处理器620通过BUS总线630耦合至存储器610。该装置600还可以通过存储接口640连接至外部存储装置650以便调用外部数据,还可以通过网络接口660连接至网络或者另外一台计算机***(未标出),此处不再进行详细介绍。
在该实施例中,通过存储器存储数据指令,再通过处理器处理上述指令,将视觉传感器的数据与惯性传感器的数据进行融合,能够提高导航***的稳定性。
图7为本公开交通工具导航数据融合***的一个实施例的结构示意图。该***包括惯性传感器710、视觉传感器720和交通工具导航数据融合装置730,其中,交通工具导航数据融合装置730已在上述实施例中进行了详细介绍,此处不再进一步阐述。
惯性传感器710和视觉传感器720设置在交通工具上,该交通工具例如为无人机、无人车等,视觉传感器720例如为光流传感器。
惯性传感器710被配置为采集交通工具的导航状态数据。
视觉传感器720被配置为采集图像信息,其中,根据图像信息获取交通工具的光流速度数据。
惯性传感器710采集的数据和视觉传感器720采集的数据可以通过交通工具导航数据融合装置730进行数据融合,从而提高交通工具的导航***的稳定性。
在另一个实施例中,一种计算机可读存储介质,其上存储有计算机程序指令,该指令被处理器执行时实现图1、2所对应实施例中的方法的步骤。本领域内的技术人员应明白,本公开的实施例可提供为方法、装置、或计算机程序产品。因此,本公开可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且,本公开可采用在一个或多个其中包含有计算机可用程序代码的计算机可用非瞬时性存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本公开是参照根据本公开实施例的方法、设备(***)和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器,使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中,使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品,该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上,使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理,从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
至此,已经详细描述了本公开。为了避免遮蔽本公开的构思,没有描述本领域所公知的一些细节。本领域技术人员根据上面的描述,完全可以明白如何实施这里公开的技术方案。
虽然已经通过示例对本公开的一些特定实施例进行了详细说明,但是本领域的技术人员应该理解,以上示例仅是为了进行说明,而不是为了限制本公开的范围。本领域的技术人员应该理解,可在不脱离本公开的范围和精神的情况下,对以上实施例进行修改。本公开的范围由所附权利要求来限定。