基于长短焦双目视觉与IMU融合的无人机自主导航方法与流程

摘要: 本发明涉及无人机视觉惯性融合导航,具体为基于长短焦双目视觉与imu融合的无人机自主导航方法。背景技术:1、在卫星导航信号受遮挡或干扰的环境中,无人机需要依靠机载...
基于长短焦双目视觉与IMU融合的无人机自主导航方法与流程

本发明涉及无人机视觉惯性融合导航,具体为基于长短焦双目视觉与imu融合的无人机自主导航方法。

背景技术:

1、在卫星导航信号受遮挡或干扰的环境中,无人机需要依靠机载传感器完成定位与航路跟踪。imu为惯性测量单元(inertial measurement unit)的英文缩写,包含陀螺仪与加速度计,分别提供角速度与比力测量数据。视觉惯性导航通过融合图像观测和惯性测量数据估计机体运动状态,图像观测提供环境地标约束,惯性测量单元提供相邻图像时刻之间的运动信息。视觉观测减少或持续缺失时,惯性递推误差可能累积,影响后续航段的定位可靠性。

2、长短焦双目采用不同焦距的相机获取环境图像。短焦相机通常具有较大的视场,便于维持地标跟踪;长焦相机能够提供远处目标的图像细节,但视场相对狭窄。无人机调整速度与航向时,同一地标在两类图像中的可见范围和停留时间存在差异,可能出现短焦仍能持续跟踪、长焦观测已经中断的情况。此时,当前定位结果仍可能满足要求,但继续沿原航路飞行可能使后续定位不确定性超过航路允许范围。

3、已有研究将特征可见性纳入无人机轨迹规划,也有研究在轨迹重规划过程中保留安全备用轨迹。然而,在长焦观测接续场景下,地标进入长焦视场不代表已经形成可参与融合的有效观测。曝光、特征匹配和融合更新均需要时间,视场内停留不足或观测处理延迟可能导致接续观测尚未可用,定位不确定性已经越限。仅依据当前图像质量或目标可见性调整航向,难以兼顾观测形成时间与定位约束持续时间。

4、此外,接续观测具有采集时刻与融合完成时刻不一致的特点,处理不当可能造成状态时序不一致或观测重复参与更新。若仅因获得新图像便恢复原航路,也难以判断后续航段是否满足定位要求。因此,需要协调长焦观测接续、定位不确定性预测与制动轨迹规划,明确观测等待期间的飞行条件和融合更新后的航路恢复条件。

技术实现思路

1、基于以上所述现有技术的缺点,本发明的目的是提供基于长短焦双目视觉与imu融合的无人机自主导航方法,以解决上述技术问题。

2、为实现上述目的,本发明提供如下技术方案:基于长短焦双目视觉与imu融合的无人机自主导航方法,包括:

3、对长焦图像、短焦图像和imu数据进行视觉惯性融合,生成机体状态、地标三维位置、联合误差协方差和定位不确定性;

4、以机体状态和联合误差协方差为初值,保留短焦跟踪与惯性递推,通过误差传播预测无新增长焦观测时预设航路的定位不确定性;

5、预计超过航路定位容限时启动长焦观测接续,调整速度与航向生成候选轨迹,将短焦持续跟踪且缺少当前长焦观测的地标三维位置投影至长焦视场,筛选覆盖曝光过程的长焦采集时段,与候选轨迹关联形成候选执行记录;

6、根据记录中的采集结束时刻与实测处理耗时预估观测可用时刻,通过误差传播预测无新增长焦观测时的候选轨迹定位不确定性,将首次越限的预测时刻作为定位越限时刻;

7、从当前机体状态与候选轨迹沿途状态滚动规划预设安全走廊内的制动轨迹,以当前状态存在制动轨迹作为接续执行条件;

8、逐控制周期选取预测区间覆盖观测可用时刻、截至该时刻未越限且接续期间可转入制动轨迹的记录,无合格记录时结束接续后执行当前状态的制动轨迹;

9、将采集时段内取得且与短焦跟踪地标匹配的新增长焦观测纳入视觉惯性融合;

10、在候选指令执行前,以最新实际融合的机体状态和联合误差协方差,通过误差传播复核后续航段的定位不确定性,未越限时结束接续后执行预设航路,仍越限时,当前时刻早于选定记录的观测可用时刻则跟踪该记录的候选轨迹,否则结束接续后执行当前状态的制动轨迹。

11、本发明进一步设置为,所述对长焦图像、短焦图像和imu数据进行视觉惯性融合,生成机体状态、地标三维位置、联合误差协方差和定位不确定性包括:

12、对同步采集的长焦图像与短焦图像执行极线约束下的特征匹配和短焦帧间跟踪,生成按地标标识关联的图像观测;

13、以前一图像时刻的机体状态为起点,对相邻图像时刻之间的imu数据执行惯性递推,生成机体状态预测值;

14、根据图像观测中的长短焦匹配观测,按照相机成像参数、相机相对机体的标定位姿和机体状态预测值执行双目三角化,生成新增地标在导航坐标系下的三维位置初值;

15、以机体状态预测值为惯性先验,以新增地标三维位置初值和已有地标三维位置为地标初始状态,根据图像观测位置与重投影位置的偏差执行联合更新,生成机体状态和地标三维位置;

16、以前一图像时刻的联合误差协方差为起点,结合imu噪声与图像观测噪声,随惯性递推和联合更新执行协方差预测与校正,生成更新后的联合误差协方差,根据其中的机体位置协方差生成预设置信水平下的位置误差上限作为定位不确定性。

17、本发明进一步设置为,所述以机体状态和联合误差协方差为初值,保留短焦跟踪与惯性递推,通过误差传播预测无新增长焦观测时预设航路的定位不确定性包括:

18、以当前机体状态为起点,按照预设航路的飞行速度与航向执行状态外推,生成按短焦采集时刻排列的预测机体状态序列;

19、沿预测机体状态序列,按照短焦相机成像参数和相对机体的标定位姿,将当前短焦跟踪地标的三维位置投影至短焦成像平面,按地标标识生成预测短焦观测序列,每个序列终止于该地标投影位置首次越出短焦视场的预测时刻之前;

20、以当前融合更新后的联合误差协方差为初值,根据预测机体状态序列和imu噪声执行相邻预测时刻之间的惯性误差传播,生成联合误差协方差预测值;

21、根据预测短焦观测的投影关系和图像观测噪声,对联合误差协方差预测值执行观测校正,无预测短焦观测时保留预测值,将校正后或保留的联合误差协方差作为下一预测时刻的传播初值;

22、根据逐预测时刻的机体位置协方差,按照与当前定位不确定性相同的置信水平生成位置误差上限,形成预设航路的定位不确定性时间序列。

23、本发明进一步设置为,所述预计超过航路定位容限时启动长焦观测接续,调整速度与航向生成候选轨迹包括:

24、对预设航路的预测定位不确定性与航路定位容限执行逐时刻比较,存在预计越限时刻时启动长焦观测接续;

25、在飞控设定的加速度和转向角速度限值内,对预设航路的速度与航向执行分段调整,生成候选速度与航向序列;

26、以当前机体状态为初值,在从当前控制时刻起至预设预测终点止的候选轨迹预测区间内,按照无人机飞行动力学模型,对飞控跟踪候选速度与航向序列的过程执行逐控制周期前向仿真,生成包含预测机体位置、速度与姿态的候选轨迹。

27、本发明进一步设置为,所述将短焦持续跟踪且缺少当前长焦观测的地标三维位置投影至长焦视场,筛选覆盖曝光过程的长焦采集时段,与候选轨迹关联形成候选执行记录包括:

28、按照短焦跟踪结果和当前长焦匹配结果,筛选短焦持续跟踪且当前未获得长焦匹配观测的地标作为待接续地标;

29、根据候选轨迹中的预测机体位置与姿态,按照长焦相机成像参数和相对机体的标定位姿,对待接续地标三维位置执行逐时刻投影,生成带有时间标记的长焦图像投影轨迹;

30、沿长焦图像投影轨迹,按照投影位置进入与离开长焦视场的时刻和候选轨迹预测区间端点,划分投影位置位于长焦视场内的连续停留时段;

31、按照长焦相机的采集时序与曝光时长,筛选曝光起止时刻均位于同一连续停留时段内的采集时段作为长焦采集时段;

32、将长焦采集时段与产生该时段的候选轨迹关联,按候选轨迹与长焦采集时段的组合形成候选执行记录。

33、本发明进一步设置为,所述根据记录中的采集结束时刻与实测处理耗时预估观测可用时刻,通过误差传播预测无新增长焦观测时的候选轨迹定位不确定性,将首次越限的预测时刻作为定位越限时刻包括:

34、对本次飞行中已完成的长焦观测处理过程,按照采集结束至视觉惯性融合更新完成的起止时刻形成实测耗时序列,选取实测耗时最大值作为预估处理时长;

35、以候选执行记录中长焦采集时段的结束时刻为起点,按照预估处理时长限定观测处理时段,将观测处理时段的结束时刻作为该记录的观测可用时刻;

36、以当前视觉惯性融合中机体状态与地标位置的联合误差协方差为初值,沿候选执行记录中的候选轨迹执行惯性误差传播,仅依据短焦跟踪地标在短焦视场内的预测投影关系执行观测校正,生成不含新增长焦观测的预测联合误差协方差序列;

37、根据预测联合误差协方差序列中的机体位置协方差,按照生成当前定位不确定性时采用的置信水平生成位置误差上限,形成候选轨迹的定位不确定性时间序列;

38、按照预测时刻的先后顺序,对候选轨迹的定位不确定性时间序列执行航路定位容限比较,将首次超过航路定位容限的预测时刻作为定位越限时刻,未出现越限时保留候选轨迹预测区间内未越限的结果,将定位越限时刻或未越限结果关联至该候选轨迹的候选执行记录。

39、本发明进一步设置为,所述从当前机体状态与候选轨迹沿途状态滚动规划预设安全走廊内的制动轨迹,以当前状态存在制动轨迹作为接续执行条件包括:

40、将当前机体状态和候选轨迹按控制周期排列的预测机体状态分别作为制动起始状态;

41、以制动起始状态为初值,将飞控的目标速度设为零,按照飞行动力学模型对受飞控加速度限值约束的减速过程执行前向仿真,生成以零速度为终端目标的待核验制动轨迹;

42、根据无人机外形尺寸和沿途预测机体姿态,沿待核验制动轨迹展开机体扫掠范围,保留机体扫掠范围完全位于预设安全走廊内的制动轨迹;

43、在下一控制周期,以融合更新的机体状态和更新后的候选轨迹替换制动起始状态,重复制动轨迹生成与机体扫掠范围核验,更新当前机体状态和候选轨迹沿途的制动轨迹,仅在当前机体状态存在通过核验的制动轨迹时执行观测接续。

44、本发明进一步设置为,所述逐控制周期选取预测区间覆盖观测可用时刻、截至该时刻未越限且接续期间可转入制动轨迹的记录,无合格记录时结束接续后执行当前状态的制动轨迹包括:

45、在观测接续未结束时,逐控制周期以融合更新的机体状态重新生成候选轨迹,按候选轨迹与长焦采集时段的组合更新候选执行记录中的观测可用时刻和定位越限结果,长焦图像采集后,按照实际采集结束时刻修正本次接续的观测可用时刻,此后更新的候选执行记录关联该已采集图像,保持该观测可用时刻不变;

46、对候选执行记录逐项核验,剔除候选轨迹预测区间未覆盖观测可用时刻的记录,在余下记录中保留观测可用时刻早于定位越限时刻或候选轨迹预测区间内未出现越限的记录;

47、沿保留记录中的候选轨迹,自当前时刻至观测可用时刻逐控制周期核验是否存在以预测机体状态为起始状态的制动轨迹,剔除任一预测时刻不存在制动轨迹的候选执行记录;

48、存在通过核验的候选执行记录时,选取观测可用时刻最早的记录作为当前控制周期的选定记录;

49、不存在通过核验的候选执行记录时,在结束观测接续后按照以当前机体状态为起始状态的制动轨迹执行减速。

50、本发明进一步设置为,所述将采集时段内取得且与短焦跟踪地标匹配的新增长焦观测纳入视觉惯性融合包括:

51、对选定记录的长焦采集时段内取得的新增长焦图像与同步短焦图像执行极线约束下的特征匹配,保留短焦特征属于持续跟踪地标的长短焦匹配关系;

52、按照长短焦匹配关系和短焦跨帧跟踪关系,将新增长焦观测关联至原地标标识下的地标三维位置;

53、以新增长焦图像采集时刻保留、尚未纳入该新增长焦观测的机体状态、地标三维位置和联合误差协方差为起点,根据新增长焦观测位置与原地标重投影位置之间的偏差执行联合状态更新与协方差校正;

54、以采集时刻更新后的机体状态、地标三维位置和联合误差协方差为起点,按照采集时序对晚于该采集时刻的imu数据和已融合图像观测重新执行视觉惯性融合,同一图像观测在本次重处理过程中仅参加一次更新,生成当前机体状态、地标三维位置、联合误差协方差和定位不确定性。

55、本发明进一步设置为,所述在候选指令执行前,以最新实际融合的机体状态和联合误差协方差,通过误差传播复核后续航段的定位不确定性,未越限时结束接续后执行预设航路,仍越限时,当前时刻早于选定记录的观测可用时刻则跟踪该记录的候选轨迹,否则结束接续后执行当前状态的制动轨迹包括:

56、逐控制周期在候选指令执行前,以最新实际融合的机体状态为初值,对飞控跟踪预设航路后续航段的过程执行前向仿真,生成后续航段的预测机体状态序列;

57、以同次融合更新的联合误差协方差为初值,沿预测机体状态序列执行惯性误差传播,仅依据短焦跟踪地标在短焦视场内的预测投影关系执行观测校正,生成后续航段的预测联合误差协方差序列;

58、根据预测联合误差协方差序列中的机体位置协方差,按照与当前定位不确定性相同的置信水平生成位置误差上限,形成后续航段的定位不确定性时间序列;

59、后续航段的定位不确定性时间序列中不存在超过航路定位容限的预测值时,结束观测接续后按照预设航路执行当前控制周期的速度与航向跟踪;

60、后续航段仍存在超过航路定位容限的预测值且当前时刻早于选定记录的观测可用时刻时,按照选定记录的候选轨迹执行当前控制周期的速度与航向跟踪,当前时刻不早于该观测可用时刻且后续航段仍存在超过航路定位容限的预测值时,在结束观测接续后按照当前机体状态下的制动轨迹执行减速。

61、本发明提供基于长短焦双目视觉与imu融合的无人机自主导航方法,所述方法通过对长焦图像、短焦图像和imu数据进行视觉惯性融合,生成机体状态、地标三维位置、联合误差协方差和定位不确定性;以机体状态和联合误差协方差为初值,保留短焦跟踪与惯性递推,通过误差传播预测无新增长焦观测时预设航路的定位不确定性;预计超过航路定位容限时启动长焦观测接续,调整速度与航向生成候选轨迹,将短焦持续跟踪且缺少当前长焦观测的地标三维位置投影至长焦视场,筛选覆盖曝光过程的长焦采集时段,与候选轨迹关联形成候选执行记录;根据记录中的采集结束时刻与实测处理耗时预估观测可用时刻,通过误差传播预测无新增长焦观测时的候选轨迹定位不确定性,将首次越限的预测时刻作为定位越限时刻;从当前机体状态与候选轨迹沿途状态滚动规划预设安全走廊内的制动轨迹,以当前状态存在制动轨迹作为接续执行条件;逐控制周期选取预测区间覆盖观测可用时刻、截至该时刻未越限且接续期间可转入制动轨迹的记录,无合格记录时结束接续后执行当前状态的制动轨迹;将采集时段内取得且与短焦跟踪地标匹配的新增长焦观测纳入视觉惯性融合;在候选指令执行前,以最新实际融合的机体状态和联合误差协方差,通过误差传播复核后续航段的定位不确定性,未越限时结束接续后执行预设航路,仍越限时,当前时刻早于选定记录的观测可用时刻则跟踪该记录的候选轨迹,否则结束接续后执行当前状态的制动轨迹,产生的有益效果包括:

62、1、定位退化预测与接续触发:在保留短焦跟踪约束和惯性递推的条件下,预测缺少新增长焦观测时的定位不确定性变化,提前识别航路越限风险,为长焦观测接续提供触发依据,减少仅依据当前定位状态作出调整的滞后;

63、2、观测时序与制动条件协同:将地标在长焦视场内的停留时段、曝光时长和实测处理耗时纳入候选轨迹筛选,结合接续期间的制动可行性调整速度与航向,降低观测尚未可用而定位已越限的风险;

64、3、延迟观测校正与航路复核:将新增长焦观测关联至原地标,在采集时刻校正后重处理后续数据,减少延迟观测的时序偏差与重复更新;根据实际融合结果复核后续航段,为航路恢复或制动切换提供依据。

65、上述说明仅是本技术技术方案的概述,为了能够更清楚了解本技术的技术手段,而可依照说明书的内容予以实施,并且为了让本技术的上述和其它目的、特征和优点能够更明显易懂,以下特举本技术的具体实施方式。