CN117804442A - 飞行器的位姿确定方法、装置、电子设备及存储介质 - Google Patents
飞行器的位姿确定方法、装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN117804442A CN117804442A CN202311851248.4A CN202311851248A CN117804442A CN 117804442 A CN117804442 A CN 117804442A CN 202311851248 A CN202311851248 A CN 202311851248A CN 117804442 A CN117804442 A CN 117804442A
- Authority
- CN
- China
- Prior art keywords
- dimensional
- determining
- feature points
- image
- aircraft
- 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
- 238000000034 method Methods 0.000 title claims abstract description 72
- 239000011159 matrix material Substances 0.000 claims abstract description 69
- 238000005259 measurement Methods 0.000 claims abstract description 68
- 238000006243 chemical reaction Methods 0.000 claims description 29
- 230000006870 function Effects 0.000 claims description 27
- 230000003287 optical effect Effects 0.000 claims description 17
- 238000001514 detection method Methods 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 12
- 230000015654 memory Effects 0.000 claims description 11
- 238000001914 filtration Methods 0.000 claims description 10
- 230000010354 integration Effects 0.000 claims description 9
- 230000001131 transforming effect Effects 0.000 claims 1
- 238000005457 optimization Methods 0.000 description 14
- 230000001133 acceleration Effects 0.000 description 7
- 230000007704 transition Effects 0.000 description 6
- 238000010276 construction Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 4
- 230000008569 process Effects 0.000 description 4
- 230000009466 transformation Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 3
- 238000012937 correction Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 238000013519 translation Methods 0.000 description 2
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 230000005764 inhibitory process Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000009877 rendering Methods 0.000 description 1
- 238000012216 screening Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
Landscapes
- Image Analysis (AREA)
Abstract
本申请提供了一种飞行器的位姿确定方法、装置、电子设备及存储介质,包括:在图像数据中确定多个二维特征点;确定多个二维特征点各自对应的二维坐标,并根据多个二维特征点各自对应的二维坐标确定多个二维特征点各自对应的三维坐标;根据多个二维特征点各自对应的三维坐标确定多个二维特征点各自对应的重投影误差;根据惯性测量数据确定飞行器的状态数据和飞行器的噪声数据以及协方差矩阵;根据噪声数据、协方差矩阵和状态数据确定卡尔曼增益系数;根据卡尔曼增益系数对状态数据进行修正,得到目标状态数据,并根据目标状态数据确定飞行器的位姿。本申请能够实时且准确的确定飞行器的位姿。
Description
技术领域
本申请涉及飞行器技术领域,更具体地,涉及一种飞行器的位姿确定方法、装置、电子设备及存储介质。
背景技术
随着科技的快速发展,飞行器在日产生活中的使用范围也越来越广。目前飞行器的传统导航技术一般依赖于机外传感器(例如全球导航卫星系统)和机载传感器(例如视觉传感器和惯性传感器)来估计飞行器的位姿。但是,由于机外传感器进行定位容易受外部环境影响,而机载传感器可通过滤波方法和图优化方法来进行定位。但是图优化方法计算量大,定位过程可能需要迭代多次,无法满足实时性的要求,滤波方法容易造成误差累积。因此,如何能够实时且准确的确定飞行器的位姿成为亟待解决的问题。
发明内容
有鉴于此,本申请实施例提出了一种飞行器的位姿确定方法、装置、电子设备及存储介质,以改善上述问题。
根据本申请实施例的一个方面,提供了一种飞行器的位姿确定方法,所述方法包括:获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点;确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标;根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差;获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据、所述飞行器的噪声数据以及协方差矩阵;根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数;根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
根据本申请实施例的一个方面,提供了一种飞行器的位姿确定装置,所述装置包括:二维特征点确定模块,用于获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点;三维转换模块,用于确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标;重投影误差确定模块,用于根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差;数据确定模块,用于获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵;增益系数确定模块,用于根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数;位姿确定模块,用于根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
根据本申请实施例的一个方面,提供了一种电子设备,包括:处理器;存储器,所述存储器上存储有计算机可读指令,所述计算机可读指令被所述处理器执行时,实现如上所述飞行器的位姿确定方法。
根据本申请实施例的一个方面,提供了一种计算机可读存储介质,其上存储有计算机可读指令,当所述计算机可读指令被处理器执行时,实现如上所述飞行器的位姿确定方法。
在本申请的方案中,先根据图像数据中对应的多个二维特征点各自对应的二维坐标来进行三维转换,得到多个二维特征点各自对应的三维坐标,进而根据多个二维特征点各自对应的三维坐标进行重投影,得到多个二维特征点各自对应的重投影二维坐标,以此根据多个二维特征点各自对应的重投影二维坐标和多个二维特点各自对应的二维坐标确定多个二维特征点各自对应的重投影误差,然后根据惯性测量数据确定飞行器的状态数据和飞行器的噪声数据和协方差矩阵,以此能够根据多个二维特征点各自对应的重投影误差、飞行器的状态数据、协方差矩阵和飞行器的噪声数据来确定卡尔曼增益系数,最后能够根据卡尔曼增益系数来进行卡尔曼滤波,来对状态数据进行修正,得到目标状态数据,以此根据目标状态数据来确定飞行器的位姿。本申请通过对图像数据中的每一图像帧通过滤波融合视觉和IMU来估计飞行器的位姿,能够实时确定飞行器的位姿,保证了飞行器的位姿的确定的实时性高同时提高位姿的准确性。
应当理解的是,以上的一般描述和后文细节描述仅是示例性和解释性的,并不能限制本发明。
附图说明
此处的附图被并入说明书中并构成本说明书的一部分,示出了符合本申请的实施例,并与说明书一起用于解释本申请的原理。显而易见地,下面描述中的附图仅仅是本申请的一些实施例,对于本领域普通技术人员来讲,在不付出创造性劳动的前提下,还可以根据这些附图获得其他的附图。
图1是根据本申请一实施例示出的飞行器的位姿确定方法的流程图。
图2是根据本申请另一实施例示出的飞行器的位姿确定方法的流程图。
图3是根据本申请一实施例示出的步骤207的具体步骤的流程图。
图4是根据本申请一实施例示出的步骤209的具体步骤的流程图。
图5是根据本申请再一实施例示出的飞行器的位姿确定方法的流程图。
图6是根据本申请又一实施例示出的飞行器的位姿确定方法的流程图。
图7是根据本申请还一实施例示出的飞行器的位姿确定方法的流程图。
图8是根据本申请一实施例示出的飞行器的位姿确定方法的流程示意图。
图9是根据本申请一实施例示出的飞行器的位姿确定装置的框图。
图10是根据本申请一实施例示出的电子设备的硬件结构图。
通过上述附图,已示出本发明明确的实施例,后文中将有更详细的描述,这些附图和文字描述并不是为了通过任何方式限值本发明构思的范围,而是通过特定实施例为本领域计算书人员说明本发明的概念。
具体实施方式
现在将参考附图更全面地描述示例实施方式。然而,示例实施方式能够以多种形式实施,且不应被理解为限于在此阐述的范例;相反,提供这些实施方式使得本申请将更加全面和完整,并将示例实施方式的构思全面地传达给本领域的技术人员。
需要说明的是,本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象,而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换,以便这里描述的本发明的实施例能够以除了在这里图示或描述的那些以外的顺序实施。此外,术语“包括”和“具有”以及他们的任何变形,意图在于覆盖不排他的包含,例如,包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元,而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。
请参阅图1,图1示出了本申请一实施例提供的飞行器的位姿确定方法,在具体的实施例中,该飞行器的位姿确定方法可以应用于如图9所示的飞行器的位姿确定装置800以及配置有飞行器的位姿确定装置800的电子设备900(图10)。下面将说明本实施例的具体流程,当然,可以理解的,该方法可以由具备计算处理能力的云服务器执行,也可以由车载服务器执行。下面将针对图1所示的流程进行详细的阐述,所述飞行器的位姿确定方法具体可以包括以下步骤:
步骤110,获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点。
作为一种方式,可通过飞行器的图像采集装置来采集图像数据。可选的,该图像数据可以是飞行器在飞行过程中所采集的图像帧流,也可以是在一时间周期内所采集的多帧连续图像。
作为一种方式,可通过对采集到的图像数据进行角点检测,以此来在图像数据确定多个二维特征点。可选的,可通过FAST(Features from Accelerated Segment Test)算法来进行角点检测。可选的,可先将图像数据进行灰度处理,得到灰度图像,然后在灰度图像中随机选取一像素点,然后基于该像素点和预设半径来对像素点所在位置为原点,预设半径所在圆的所有与像素进行分割测试,以此进行角点初筛,得到多个候选角点,然后在再基于分类的角点特征检测,以此来确定多个候选角点对应的特征是否为角点特征,以此确定参考角点,最后在利用非大值抑制来进行角点特征的验证,以此排除特征不稳点的参考角点,得到目标角点,进而将目标角点确定为图像数据中作为特征点的二维特征点。
作为另一种方式,图像数据还可以是连续的视频帧序列,以此,还可以是通过光流跟踪法对图像数据进行检测来确定二维特征点。可选的,在通过光流跟踪法来确定图像数据中的二维特征点之前,可先对图像数据进行灰度处理,以此得到灰度图像,然后在连续帧灰度图像中确定像素值变化小于或等于变化阈值的像素点,以此将该像素点确定为图像数据中作为特征点的二维特征点。可选的,还可根据其他算法来确定图像数据中的二维特征点,在此不做具体限定。
步骤120,确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
作为一种方式,在确定了图像数据中的多个二维特征点后,可确定多个特征点在对应的图像坐标下的图像坐标,以此确定多个二维特征点各自对应的二维坐标。
可选的,在进行三维转换之前,可先获取飞行器的图像采集装置的标定系数,该标定系数包括图像采集装置对应的内参,以此可根据图像采集装置对应的内参来进行三维转换。可选的,可根据图像坐标系、相机坐标系、世界坐标系之间的转换关系和多个二维特征点各自对应的二维坐标来确定多个二维特征点各自对应的三维坐标。可选的,可通过下述公式来进行三维转换:
其中,d为一二维特征点到图像采集装置之间的距离,即该图像采集装置的物距,其中(fx,fy)是图像采集装置的焦距,(cx,cy)是图像采集装置的光心坐标,即为图像采集装置的内参参数,K为图像采集装置的内参矩阵,二维特征点的二维坐标为(u,v),二维特征点的三维坐标为(XC,YC,ZC)。其中,d可以是在深度图像中特征点的深度值,还可以根据三角化算法所确定的,基于在不同图像帧中所匹配的特征点的二维坐标计算得到。
步骤130,根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差。
作为一种方式,在确定了多个二维特征点各自对应的三维坐标后,为了准确确定飞行器的位姿信息可通过将推算的二维特征点的三维坐标再次投影至二维坐标系下,得到二维坐标下的重投影二维坐标,以此来确定二维特征点各自对应的重投影误差,以此便于在确定飞行器的位姿时,加入重投影误差使得确定的飞行器的位姿更加准确。
可选的,可根据上述公式来根据确定的多个二维特征点各自对应的二维坐标和重投影二维坐标,来计算二维坐标与对应的重投影二维坐标之间的欧式距离,以此将该欧式距离确定为多个二维特征点各自对应的重投影误差。
步骤140,获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据和协方差矩阵。
作为一种方式,飞行器上设置有惯性测量单元(inertial measurement unit,IMU),IMU可以包括加速度传感器和陀螺仪传感器,可以用于检测飞行器的加速度、角速度、偏航角、俯仰角、翻滚角等,可基于所采集的飞行器的惯性测量数据来预测飞行器的状态数据,即可预测飞行器的当前位姿。
可选的,由于传感器的检测精度的影响,导致IMU所采集的惯性测量数据存在一定的噪声,为了减小IMU噪声对确定飞行器的位姿的影响,可根据惯性测量数据来确定飞行器的噪声数据。可选的,噪声数据还可以是预先对飞行器的IMU进行标定后所确定固定噪声值,可直接获取得到。
作为一种方式,在确定了飞行器的状态数据和噪声数据后,可先根据飞行器的状态数据确定状态转移矩阵以及根据噪声数据确定噪声矩阵,然后根据状态转移矩阵和噪声矩阵,然后获取上一时刻协方差矩阵,初始协方差矩阵可根据实际需要来进行设置,然后通过状态转移矩阵和噪声矩阵对协方差矩阵进行更新,以此确定下一时刻的协方差矩阵。可选的,可通过下述公式来确定状态转移矩阵:
其中,F为状态转移矩阵,I为单位矩阵,矩阵的行从上至下依次为旋转角度第一噪声bg,速度/>第二噪声ba,位移/>矩阵的列从左至右依次代表/>bg,/>ba,/>其中,旋转角度/>可根据惯性测量数据中的飞行器的角速度确定,速度/>可根据惯性测量数据中的飞行器的线性加速度确定,位移/>可根据惯性测量数据中的飞行器的速度确定。
步骤150,根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定协方差矩阵和卡尔曼增益系数。
作为一种方式,噪声矩阵可根据公式Q=ΦGQcGTΦTΔt来确定,其中,Qc为IMU的噪声矩阵,可设定为常值,G可通过下述公式计算得到:
其中,矩阵的行从上至下依次为旋转角度第一噪声bg,速度/>第二噪声ba,位移/>矩阵列从左至右依次为(ng,nwg,na,nwa),分别表示角速度噪声,IMU的第一噪声,加速度噪声IMU的第二噪声。可选的,IMU的第一噪声可以是IMU中的加速度传感器偏差,IMU的第二噪声可以是IMU中的陀螺仪的偏差。
可选的,在确定了噪声矩阵后,可根据公式Pt=ΦPt-1ΦT+Q来确定协方差矩阵,其中,Q为噪声矩阵,Pt-1为上一时刻的协方差矩阵,Φ为状态转移矩阵在离散时间下的积分。
作为一种方式,可根据公式来确定卡尔曼增益系数,其中,为协方差矩阵,H为二维特征点对应的雅克比矩阵,R为噪声矩阵。其中,雅克比矩阵是根据二维特征点的重投影误差确定的。
步骤160,根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
作为一种方式,在确定了卡尔曼增益系数后,可根据公式来对状态数据进行修正,其中,/>为修正前的状态系数,K为卡尔曼增益系数,r为二维特征点各自对应的重投影误差,/>为目标状态数据。可选的,在目标状态数据中包括飞行器的当前三维坐标,偏航角、俯仰角、翻滚角以及速度等,进而,可根据目标状态数据来确定飞行器的位姿。
可选的,还可以根据卡尔曼增益系数对协方差矩阵进行修正,以此便于在下一时刻确定飞行器的位姿时基于更新后的协方差矩阵来进行确定。可选的,可通过公式来对协方差矩阵进行修正,其中,/>为当前协方差矩阵,H为二维特征点的雅克比矩阵,I为单位矩阵,K为卡尔曼增益系数,/>为修正后的协方差矩阵。
在本申请的实施例中,先根据图像数据中对应的多个二维特征点各自对应的二维坐标来进行三维转换,得到多个二维特征点各自对应的三维坐标,进而根据多个二维特征点各自对应的三维坐标进行重投影,得到多个二维特征点各自对应的重投影二维坐标,以此根据多个二维特征点各自对应的重投影二维坐标和多个二维特点各自对应的二维坐标确定多个二维特征点各自对应的重投影误差,然后根据惯性测量数据确定飞行器的状态数据和飞行器的噪声数据和协方差矩阵,以此能够根据多个二维特征点各自对应的重投影误差、飞行器的状态数据、协方差矩阵和飞行器的噪声数据来确定卡尔曼增益系数,最后能够根据卡尔曼增益系数来进行卡尔曼滤波,来对状态数据进行修正,得到目标状态数据,以此根据目标状态数据来确定飞行器的位姿。本申请的方案,通过对图像数据中的每一图像帧通过滤波融合视觉和IMU来估计飞行器的位姿,能够实时确定飞行器的位姿,保证了飞行器的位姿的确定的实时性高同时提高位姿的准确性。
请参阅图2,图2示出了本申请一实施例提供的飞行器的位姿确定方法。下面将针对图2所示的流程进行详细的阐述,所述图像数据包括多帧图像,所述飞行器的位姿确定方法具体可以包括以下步骤:
步骤201,获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点。
步骤202,确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
步骤203,根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差。
步骤204,获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵。
步骤205,根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数。
步骤206,根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
其中,步骤201-步骤206的具体步骤描述可参阅步骤110-步骤160,在此不再进行赘述。
步骤207,在所述多帧图像中确定关键帧图像,并确定所述关键帧图像的数量。
作为一种方式,图像数据中包括多帧图像,可在多帧图像中确定满足遇事儿条件的图像作为关键帧图像,进而可根据关键帧图像来对关键帧中的二维特征点对应的三维坐标进行优化,以此增加二维特征点的三维坐标的准确性,进而提高飞行器的位姿的准确性,进一步提高状态估计的精度。可选的,预设条件可以是特征点数大于点数阈值的图像帧,也可以是其他条件,可根据实际需要来设置预设条件。
在一些实施例中,如图3所示,所述步骤207包括:
步骤310,在所述多帧图像中确定每一帧图像对应的特征点数。
作为一种方式,可通过角点检测算法对每一帧图像中的特征点进行检测,以此确定每一帧图像对应的特征点数。
步骤320,根据所述每一帧图像对应的特征点数,在所述图像数据中确定是否存在当前帧图像与上一帧图像的匹配特征点数是否小于点数阈值。
作为一种方式,可通过光流跟踪方法将当前帧图像中的特征点与上一帧图像中的特征点进行匹配,以此确定当前帧图像与上一帧图像匹配的特征点,进而确定当前帧图像与上一帧图像的匹配特征点数。可选的,点数阈值可以是根据当前帧图像的特征点数和与上一帧图像的匹配特征点数所确定,也可以是根据实际需要设置的,在此不做具体限定。
作为另一种方式,可通过将连续帧图像中特征点数进行比对以此来确定图像数据中满足预设条件的关键帧图像。可选的,还可以通过光流跟踪法来确定当前帧图像与上一帧图像对应的特征点之间的关系,以此来确定当前帧图像对应的特征点数是否小于上一帧图像对应的特征点数。其中,光流跟踪法可以在确定上一帧图像中的特征点后,在当前帧图像中对在上一帧图像中所确定的特征点进行匹配,以此来确定当前帧图像对应发的特征点数是否小于上一帧图像对应的特征点数。
可选的,可确定当前帧图像对应的特征点数与上一帧图像的特征点数之间的点数差,当点数差大于点数差阈值时,可确定当前帧图像对应的特征点数小于上证图像对应的特征点数。
步骤330,若小于所述点数阈值,则将所述当前帧图像确定为所述关键帧图像。
作为一种方式,当确定当前帧图像与上一帧图像所匹配的特征点的特征点数小于点数阈值,以此可确定当前帧图像为关键帧图像,进而可根据关键帧图像来进行优化。
作为另一种方式,当图像数据中存在当前帧图像对应的特征点数小于上一帧图像对应的特征点数的图像帧时,将当前帧图像确定为关键帧图像,进而可根据关键帧图像来进行优化。
在一些实施例中,所述步骤207包括:获取历史关键帧,并确定所述历史关键帧对应的位姿信息;确定所述多帧图像中的每一帧图像对应的位姿信息;根据所述历史关键帧对应的位姿信息和所述多帧图像中的每一帧图像对应的位姿信息,确定历史关键帧与所述每一图像之间的相对距离;若存在相对距离大于距离阈值的图像,则将所述相对距离大于所述距离阈值的图像确定为所述关键帧图像。
作为一种方式,在获取到历史关键帧图像后,可根据历史关键帧图像中的特征点的二维坐标以及采集所述历史关键帧的图像采集装置的位姿来确定该历史关键帧对应的位姿,同理,可根据多张图像中每一帧图像中的特征点的二维坐标以及采集该图像帧的图像采集装置的位姿来确定每一帧图像各自对应的位姿。可选的,可根据历史关键帧对应的位姿和每一帧图像各自对应的位姿来确定历史关键帧与每一帧图像之间的欧式距离,以此将历史关键帧与每一帧图像之间的欧式距离确定为历史关键帧与每一帧图像之间的相对距离。
在另一些实施例中,所述步骤207还包括:获取历史关键帧,并确定所述历史关键帧中的特征点对应的参考二维坐标;确定所述多帧图像中的每一帧图像对应的特征点的二维坐标;分别确定参考二维坐标与所述每一图像对应的特征点的二维坐标之间的坐标差值;若存在坐标差值大于差值阈值的图像,则将所述坐标差值大于差值阈值的图像确定为所述关键帧图像。作为一种方式,当存在历史关键帧图像时,可获取历史关键帧的图像对应的特征点的参考二维坐标,以此可基于参考二维坐标来确定当前图像帧是否为关键帧图像。
可选的,针对每一帧图像,在确定了历史关键帧对应的参考二维坐标后,可确定当前帧图像中与历史关键帧具有相同特征的二维特征点的二维坐标,然后计算参考二维坐标与当前帧图像中与历史关键帧具有相同特征的二维特征点的二维坐标之间的欧式距离,将该欧式距离确定为参考二维坐标与当前帧图像中与历史关键帧具有相同特征的二维特征点之间的坐标差值,以此确定参考二维坐标与每一图像对应的特征点的二维坐标之间的坐标差值。
作为另一种方式,在确定了历史关键帧图像后,可先确定历史关键帧图像对应的历史关键时间戳,然后在历史惯性测量数据中确定对应的历史关键惯性测量数据,并且确定图像数据中每一帧图像各自对应的时间戳,然后确定与每一帧图像各自对应的时间戳对应的当前惯性测量数据,然后根据历史惯性测量数据和每一帧图像对应的当前惯性测量数据来确定目标相对位置信息,当确定目标相对位置信息大于某设定阈值,可确定对应的图像帧为关键帧图像。
请继续参阅图2,步骤208,若所述关键帧图像的数量大于数量阈值,则从所述惯性测量数据中确定与所述关键帧图像具有相同时间戳的关键惯性测量数据。
作为一种方式,当确定关键帧图像的数量大于数量阈值时,可确定当前具有足量的关键帧图像,可通过足量的关键帧图像来进行低头构建或局部地图优化,进而,可先确定关键帧图像对应的关键时间戳,然后在惯性测量数据中确定与关键时间戳对应的关键惯性测量数据,进而,可根据关键惯性测量数据来确定关键帧图像对应的较为精准的飞行器的位姿信息,以此来进行地图构建或局部地图优化。
步骤209,根据所述关键惯性测量数据和所述关键帧图像对应的特征点,确定目标函数。
作为一种方式,在确定关键帧图像和关键帧图像对应的关键惯性测量数据后,可根据关键帧图像和关键帧惯性测量数据来构建一约束函数,然后求得该约束函数对应的最优解,进而将该最优解确定为关键帧图像对应的飞行器的位姿信息,该位姿信息相较于步骤160所得到的飞行器位姿更为准确。可选的,目标函数可以包括三部分,具体可以是连续两个关键帧图像之间对应的IMU数据预积分与两个关键帧图像相对位姿的残差、关键帧图像对应的三维特征点重投影回二维图像后得到的二维坐标与关键帧图像中的关键特征点的二维坐标的残差和由边缘化的关键帧图像中的所有特征点所构成的约束。
在一些实施例中,如图4所示,所述步骤209包括:
步骤410,根据所述关键惯性测量数据确定所述关键帧图像对应的惯性数据预积分,并根据所述惯性数据预积分和所述关键帧位姿确定所述关键帧的第一残差。
作为一种方式,预积分是指在IMU不断的测量IMU数据所累计的观测数据,可通过下述公式确定关键帧图像对应的惯性数据:
其中,第i帧关键帧图像对应的关键惯性测量数据,第j帧关键帧图像对应的关键惯性测量数据,R为旋转参数,p为平移参数,ω为IMU所测量的角速度测量值,v为IMU所测量的线速度测量值,a为IMU所测量的加速度测量值,即k时刻的第一噪声,/>为k时刻的第二噪声,g为重力加速度,η为随机游走噪声,Δtij为第i帧关键帧图像到第j帧关键帧图像的累计时长。
可选的,在确定了的关键惯性测量数据对应的预积分后,将确定的预积分与连续帧之间相对平移、相对旋转、相对速度相减,以此得到第一残差。
步骤420,确定所述关键帧图像中的关键特征点的关键二维坐标和所述关键特征点的关键三维坐标,并基于所述关键三维坐标和所述关键二维坐标确定所述关键特征点的第二残差,其中,所述第二残差为所述关键特征点的重投影误差。
作为一种方式,在确定了关键帧图像后,可在图像数据中确定关键帧图像中的关键特征点对应的关键二维坐标和关键三维坐标,然后根据关键二维坐标进行重投影操作,以此投影至二维坐标系下(即关键帧图像所在的图像坐标系下),以此得到到关键特征点的重投影坐标,然后计算重投影坐标与关键二维坐标之间的差值,以此确定关键特征点对应的重投影误差,以此得到关键特征点的第二残差。
可选的,在根据关键特征点对应的关键三维坐标进行重投影时,可先对关键三维坐标进行优化,以此根据优化后的关键三维坐标来进行重投影,保证了第二残差的准确性。
步骤430,根据滑动窗口的数量确定边缘帧图像,并确定所述边缘帧图像中的边缘二维特征点,其中,边缘帧图像为所述滑动窗口最前端的图像。
作为一种方式,可通过滑动窗口来构造维护一个轻量化的因子图,通过图优化的方法来进行优化,因此,可通过滑动窗口来确定当前关键帧图像,关键帧图像可按照时间戳的大小顺序依次被选入滑动窗口,在滑动窗口中即将选入其他关键帧图像时,可确定滑动窗口最前端的图此为边缘帧图像,即即将被滑动窗口丢弃的图像为边缘帧图像。
可选的,在确定了边缘帧图像后,可通过在步骤120中所确定的图像数据中对应的多个二维特征点中确定边缘帧图像中对应的二维特征点,以此确定边缘二维特征点。可选的,可在图像数据中对每一帧图像设置对应的标志,在确定了边缘帧图像后,可根据边缘帧图像对应的标志在图像数据中确定对应的图像,以此可获取边缘二维特征点。
步骤440,根据所述边缘二维特征点、所述第一残差和所述第二残差确定所述目标函数。
作为一种方式,在确定了边缘二维特征点、关键帧图像对应的第一残差和第二残差后,可将边缘二维特征点的二维坐标、第一残差和第二残差相加,以此确定目标函数,将该目标函数确定为约束条件,进而可根据该目标函数来进行优化求解。
请继续参阅图2,步骤210,基于最小二乘法确定所述目标函数的最优解,并根据所述最优解对所述位姿进行优化,得到目标三维坐标,以使基于所述目标三维坐标进行地图构建。
作为一种方式,在确定了目标函数后,可通过最小化误差的平方和来寻找数据的最佳函数匹配(即最小二乘法),以此得到目标函数的最优解。可选的,最优解可以是位姿信息,即包括空间三维坐标、偏航角、俯仰角或翻滚角等。可选的,在得到最优解后,可根据图像帧的位姿信息和图像帧中的特征点的三维坐标来进行局部地图的构建,以此能够根据构建的局部地图来进行地图优化或全局地图的构建。
在本实施例中,通过在图像数据中确定关键帧图像,以此在关键帧图像的数量大于数量阈值,基于关键图像帧对应的时间戳确定具有相同时间戳的关键惯性测量数据,以此便于根据关键惯性测量数据和关键帧图像对应的特征点来确定目标函数,以此根据最小二乘法来确定目标函数的最优解,进而根据最优解对飞行器的位姿进行优化,得到目标三维坐标,便于根据目标三维坐标进行地图构建,减小长时间累计带来的累计误差,进一步提高状态估计的精度,保证了飞行器的位姿的准确性。
请参阅图5,图5示出了本申请一实施例提供的飞行器的位姿确定方法。下面将针对图5所示的流程进行详细的阐述,所述图像数据包括多帧图像,所述飞行器的位姿确定方法具体可以包括以下步骤:
步骤510,获取飞行器所采集的图像数据,并在所述图像数据中确定是否存在首帧图像,其中,所述首帧图像为所述飞行器所采集的第一帧的图像。
作为一种方式,首帧图像帧为图像数据中采集时间最早的图像帧,即图像采集装置开启后所采集的第一帧图像。首帧图像反映了飞行器的初始状况,而后续图像帧都是基于首帧图像的变化得到的,为了避免在确定多个二维特征点时出现错误或者二维特征点的数量不足的情况,可先确定图像数据中是否存在首帧图像。
可选的,可先确定飞行器的图像采集装置开启的初始时间戳,然后分别确定图像数据中每一帧图像对应的时间戳,然后确定是否存在与初始时间戳相同的时间戳对应的图像帧或与初始时间戳之间的时间差小于差值阈值的时间戳对应的图像帧,以此来确定图像数据中是否存在首帧图像。
步骤520,若确定不存在首帧图像,则对所述图像数据进行光流跟踪确定多个二维匹配点对。
作为一种方式,当确定不存在首帧图像时,为了避免连续帧图像中的特征点确定出现较大的偏差,可以先根据光流跟踪法对某一图像帧进行特征点确定,然后基于光流跟踪法在其他图像帧中确定具有相同特征的特征点,以此来确定多个二维匹配点对。
作为另一种方式,若确定存在首帧图像,则可利用FAST算法通过对首帧图像进行角点检测,以此来确定首帧图像的多个二维特征点,进而,可基于首帧图像所确定的多个二维特征点对其他图像进行光流跟踪,以此确定图像数据中的每一图像各自对应的二维特征点。
步骤530,对所述图像数据进行角点检测,得到多个二维角点。
作为一种方式,由于飞行器在运动状态下,连续帧的不同图像中的特征点存在一定差异,例如,前一时刻图像A1的N个特征点难以全部都在当前时刻图像A2中出现,为了保证飞行器的位姿的准确性,可通过对光流跟踪后所确定的匹配点对进行标记,然后在通过FAST算法对图像数据进行角点检测,得到多个二维角点,以此便于将二维角点也作为图像数据的二维特征点,增加二维特征点的丰富性,以此增加飞行器的位姿的准确性。
步骤540,根据所述多个二维匹配点对和所述多个二维角点确定所述多个二维特征点。
作为一种方式,在确定了多个二维匹配点对以及多个二维角点后,确定多个二维匹配点对和多个二维角点各自对应的图像帧标记,以此确定每一帧图像各自对应的二维匹配点和二维角点,进而确定每一帧图像各自对应的多个二维特征点。
步骤550,确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
步骤560,根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差。
步骤570,获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据和协方差矩阵。
步骤580,根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数。
步骤590,根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
其中,步骤550-步骤590的具体步骤描述可参阅步骤120-步骤160,在此不再进行赘述。
在本实施例中,在获取了飞行器所采集的图像数据后,先确定图像数据中的是否存在首帧图像,以此在确定不存在首帧图像时,先通过光流跟踪来确定图像数据中的多个二维匹配点对,然后再对图像数据进行角点检测来确定多个二维角点,以此根据多个二维匹配点对和多个二维角点来确定多个二维特征点,保证了二维特征点的丰富度,以此增加飞行器的位姿的准确性。
请参阅图6,图6示出了本申请一实施例提供的飞行器的位姿确定方法。下面将针对图6所示的流程进行详细的阐述,所述图像数据包.括多帧图像,所述飞行器的位姿确定方法具体可以包括以下步骤:
步骤610,获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点。
步骤620,确定所述图像数据是否来自不同的图像采集装置。
作为一种方式,可通过确定图像数据中每一帧图像的图像信息来确定每一帧图像的来源,以此来确定图像数据中的每一帧图像是否为相同图像采集装置所采集的。
步骤630,若确定所述图像数据来自不同的图像采集装置,则根据双目三角化算法和所述多个二维特征点各自对应的二维坐标进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
作为一种方式,在确定图像数据来自不同的图像采集装置,则先在不同图像采集装置所采集的同一场景下对应的相同图像帧以及相同图像帧对应的特征点对,进而,根据双目三角化算法以及特征点对对应的二维坐标来进行三维转换,以此确定特征点对在世界坐标下的三维坐标,进而,得到多个二维特征点各自对应的三维坐标。
步骤640,若确定所述图像数据来自同一图像采集装置,则根据多帧三角化算法和所述多个二维特征点各自对应的二维坐标进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
作为一种方式,当确定图像数据中的每一帧图像均来自同一图像采集装置,则可根据图像数据中的前后帧图像中具有相同特征的二维特征点对应的二维坐标以及多帧三角化算法来确定前后帧图像中确定的相同特征的二维特征点的三维坐标,以此确定图像数据的每一帧图像的多个二维特征点各自对应的三维坐标。
步骤650,根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差。
步骤660,获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵。
步骤670,根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数。
步骤680,根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
其中,步骤610和步骤650-步骤680的具体步骤描述可参阅步骤110和130-步骤160,在此不再进行赘述。
在本实施例中,通过先确定图像数据是否为不同采集装填所采集的,来确定对多个二维特征点进行三维转换的算法,即在确定为同一图像采集装置所采集时,通过多帧三角化来对多个二维特征点进行三维转换;在确定为不同图像采集装置所采集时,通过双目三角化来对多个二维特征点进行三维转换,以此提高多个二维特征点对各自对应的三维坐标的准确性,进而提高飞行器的位姿的准确性。
请参阅图7,图7示出了本申请一实施例提供的飞行器的位姿确定方法。下面将针对图7所示的流程进行详细的阐述,所述图像数据包括多帧图像,所述飞行器的位姿确定方法具体可以包括以下步骤:
步骤710,获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点。
步骤720,确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
步骤730,在所述多个二维特征点中确定重复次数大于次数阈值的目标特征点,并根据所述目标特征点的目标二维坐标确定所述目标特征点的参考三维坐标。
作为一种方式,在确定了多个二维特征点各自对应的三维坐标后,为了保证飞行器的位姿的确定的准确性,可对多个二维特征点中满足预设条件的特征点的三维坐标进行优化,使得多个二维特征点中满足预设条件的特征点的三维坐标更加准确,进而提高重投影误差的准确性,进而提高飞行的位姿的准确性。
可选的,当同一特征点在图像数据中的图像中多次重复时,可确定该特征点被图像采集装置多次观测到,以此可基于非线性优化的方法来优化特征点的三维坐标,使得三维坐标更加准确。可选的,次数阈值可以是2,也可以是其他值,可根据实际需要来设置,在此不做具体限定。可选的,在确定目标特征点后,可在步骤720中确定目标特征点对应的参考三维坐标。
步骤740,根据所述目标特征点的参考三维坐标进行重投影,得到所述目标特征点的参考二维坐标,并根据所述目标特征点的目标二维坐标和目标特征点的参考二维坐标确定参考重投影误差。
作为一种方法,在确定了目标特征点的参考三维坐标后,可将目标特征点的参考三维坐标投影至图像数据所在的图像坐标系下,以此得到目标特征点的参考二维坐标,进而可通过计算目标特征点的二维坐标和目标特征点的参考二维坐标之间的差值来确定目标特征点对应的参考重投影误差。可选的,该参考重投影误差可以是目标特征点的二维坐标和目标特征点的参考二维坐标之间的欧式距离,也可以是余弦距离,可根据实际需要来设置重投影误差的形式,在此不做具体限定。
步骤750,根据所述参考重投影误差对所述目标特征点的参考三维坐标进行修正,并根据修正后的参考三维坐标确定所述多个二维特征点各自对应的重投影误差。
作为一种方法,可将目标特征点的参考重投影误差作为优化函数,通过对优化函数求得最小值来对目标特征点的参考三维坐标进行修正。可选的,优化函数可以是∑||ui―TiP||2,其中,P为目标特征点的参考三维坐标,i为第i帧图像,ui为目标特征点的目标二维坐标,Ti为图像采集装置的标定参数,该标定参数可包括图像采集装置的内参和外参(即图像采集装置相对于飞行器的位姿),P*为目标特征点修正后的参考三维坐标。
可选的,在确定多个二维坐标点各自对应的重投影误差时,可根据多个二维坐标点各自对应的修正后的参考三维坐标来进行重投影,以此确定多个二维特征点各自对应的重投影误差。
步骤760,获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵。
步骤770,根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数。
步骤780,根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
其中,步骤710-步骤720和步骤760-步骤780的具体步骤描述可参阅步骤110-步骤120和步骤140-步骤160,在此不再进行赘述。
在本实施例中,通过在多个二维特征点中确定重复次数大于次数阈值的目标特征点,并根据目标特征点的目标二维坐标来确定目标特征点对应的重投影误差,以此能够根据目标特征点的重投影误差来对目标特征的参考三维坐标进行修正,以此根据修正后的参考三维坐标确定目标特征点对应的重投影误差,以此基于目标特征点对应的重投影误差来确定飞行器的位姿,保证了飞行器的位姿的准确性。
图8是根据本申请一实施例所示出的飞行器的位姿确定方法的流程图,如图8所示,先获取飞行器的图像采集装置所采集的图像数据和惯性测量单元所采集的惯性测量数据,然后确定图像数据是否为首帧图像,若确定为首帧图像,则先对首帧图像进行角点检测,确定首帧图像的多个二维特征点;若不是首帧图像,则通过光流跟踪法对当前帧图像和前一帧图像进行特征点提取,以此得到多个二维特征点;然后根据多个二维特征点各自对应的二维坐标进行三维转化,以此对多个二维特征点进行特征点参数化,得到多个二维特征点各自对应的三维坐标,以此根据多个二维特征点的三维坐标进行重投影,并基于多个二维特征点各自对应的二维坐标和重投影后的二维坐标来确定多个二维特征点各自对应的重投影误差,然后再根据惯性测量数据预测飞行器的当前状态数据和IMU对应的协方差矩阵,以此根据飞行器的当前状态数据、协方差矩阵和重投影误差确定卡尔曼增益系数,以此通过卡尔曼率滤波方法对飞行器的当前状态数据和协方差矩阵进行更新,以此得到飞行器的位姿,并且,确定图像数据中是否存在关键帧图像,当确定存在关键帧图像时,将关键帧图像添加至对应的局部地图中,以此根据关键帧图像对局部地图进行优化,以此确定关键帧图像对应的位姿和地图点的坐标,并通过关键帧图像对应的位姿和地图点的坐标对多个二维特征点各自对应的三维坐标进行修正。
图9是根据本申请一实施例示出的飞行器的位姿确定装置的框图,如图9所示,该飞行器的位姿确定装置800包括:二维特征点确定模块810、三维转换模块820、重投影误差确定模块830、惯性数据处理模块840、滤波模块850和位姿确定模块860。
二维特征点确定模块810,用于获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点;三维转换模块820,用于确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标;重投影误差确定模块830,用于根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差;惯性数据处理模块840,用于获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵;滤波模块850,用于根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数;位姿确定模块860,用于根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
在一些实施例中,所述图像数据包括多帧图像,所述飞行器的位姿确定装置800还包括:关键帧图像确定模块,用于在所述多帧图像中确定关键帧图像,并确定所述关键帧图像的数量;关键惯性测量数据确定好模块,用于若所述关键帧图像的数量大于数量阈值,则从所述惯性测量数据中确定与所述关键帧图像具有相同时间戳的关键惯性测量数据;目标函数确定模块,用于根据所述关键惯性测量数据和所述关键帧图像对应的特征点,确定目标函数;优化模块,用于基于最小二乘法确定所述目标函数的最优解,并根据所述最优解对所述位姿和所述多个特征点各自对应的三维坐标进行优化,得到目标三维坐标,以使基于所述目标三维坐标进行地图构建。
在一些实施例中,所述关键帧图像确定模块包括:特征点数确定单元,用于在所述多帧图像中确定每一帧图像对应的特征点数;判断单元,用于根据所述每一帧图像对应的特征点数,在所述图像数据中确定是否存在当前帧图像与上一帧图像的匹配特征点数是否小于点数阈值;关键帧图像第一确定单元,用于若小于所述点数阈值,则将所述当前帧图像确定为所述关键帧图像。在另一些实施例中,所述关键帧图像确定模块包括:历史关键帧获取单元,用于获取历史关键帧,并确定所述历史关键帧对应的位姿信息;位姿信息确定单元,用于确定所述多帧图像中的每一帧图像对应的位姿信息;相对距离确定单元,用于根据所述历史关键帧对应的位姿信息和所述多帧图像中的每一帧图像对应的位姿信息,确定历史关键帧与所述每一图像之间的相对距离;关键帧图像第二确定单元,用于若存在相对距离大于距离阈值阈值的图像,则将所述相对距离大于距离阈值的图像确定为所述关键帧图像。在一些实施例中,所述目标函数确定模块包括:第一残差确定单元,用于根据所述关键惯性测量数据确定所述关键帧图像对应的惯性数据预积分,并根据所述惯性数据预积分和所述关键惯性测量数据确定所述关键帧的第一残差;第二残差确定单元,用于确定所述关键帧图像中的关键特征点的关键二维坐标和所述关键特征点的关键三维坐标,并基于所述关键三维坐标和所述关键二维坐标确定所述关键特征点的第二残差,其中,所述第二残差为所述关键特征点的重投影误差;边缘二维特征点确定单元,用于根据滑动窗口的数量确定边缘帧图像,并确定所述边缘帧图像中的边缘二维特征点,其中,边缘帧图像为所述滑动窗口最前端的图像;目标函数确定单元,用于根据所述边缘二维特征点、所述第一残差和所述第二残差确定所述目标函数。
在一些实施例中,所述二维特征点确定模块810,包括:第二确定单元,用于获取飞行器所采集的图像数据,并在所述图像数据中确定是否存在首帧图像,其中,所述首帧图像为所述飞行器所采集的第一帧的图像;匹配单元,用于若确定不存在首帧图像,则对所述图像数据进行光流跟踪确定多个二维匹配点对;角点检测单元,用于对所述图像数据进行角点检测,得到多个二维角点;二维特征点确定单元,用于根据所述多个二维匹配点对和所述多个二维角点确定所述多个二维特征点。
在一些实施例中,所述三维转换模块820包括:第三确定单元,用于确定所述图像数据是否来自不同的图像采集装置;第一转换单元,用于若确定所述图像数据来自不同的图像采集装置,则根据双目三角化算法和所述多个二维特征点各自对应的二维坐标进行三维转换,确定所述多个二维特征点各自对应的三维坐标;或者第二转换单元,用于若确定所述图像数据来自同一图像采集装置,则根据多帧三角化算法和所述多个二维特征点各自对应的二维坐标进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
在一些实施例中,所述飞行器的位姿确定装置800还包括:参考三维坐标确定模块,用于在所述多个二维特征点中确定重复次数大于次数阈值的目标特征点,并根据所述目标特征点的目标二维坐标确定所述目标特征点的参考三维坐标;参考重投影误差确定模块,用于根据所述目标特征点的参考三维坐标进行重投影,得到所述目标特征点的参考二维坐标,并根据所述目标特征点的目标二维坐标和目标特征点的参考二维坐标确定参考重投影误差;坐标修正模块,用于根据所述参考重投影误差对所述目标特征点的参考三维坐标进行修正,并根据修正后的参考三维坐标确定所述多个二维特征点各自对应的重投影误差。
根据本申请实施例的一个方面,还提供了一种电子设备,如图10所示,该电子设备900包括处理器910以及一个或多个存储器920,一个或多个存储器920用于存储被处理器910执行的程序指令,处理器910执行程序指令时实施上述的飞行器的位姿确定方法。
进一步地,处理器910可以包括一个或者多个处理核。处理器910运行或执行存储在存储器920内的指令、程序、代码集或指令集,以及调用存储在存储器920内的数据。可选地,处理器910可以采用数字信号处理(Digital Signal Processing,DSP)、现场可编程门阵列(Field-Programmable Gate Array,FPGA)、可编程逻辑阵列(Programmable LogicArray,PLA)中的至少一种硬件形式来实现。处理器910可集成中央处理器(CentralProcessing Unit,CPU)、图像处理器(Graphics Processing Unit,GPU)和调制解调器等中的一种或几种的组合。其中,CPU主要处理操作系统、用户界面和应用程序等;GPU用于负责显示内容的渲染和绘制;调制解调器用于处理无线通信。可以理解的是,上述调制解调器也可以不集成到处理器中,单独通过一块通信芯片进行实现。
根据本申请的一个方面,本申请还提供了一种计算机可读存储介质,该计算机可读介质可以是上述实施例中描述的电子设备中所包含的;也可以是单独存在,而未装配入该电子设备中。上述计算机可读存储介质承载计算机可读指令,当该计算机可读存储指令被处理器执行时,实现上述任一实施例中的方法。
需要说明的是,本申请实施例所示的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读存储介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件,或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于:具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器(RAM)、只读存储器(ROM)、可擦式可编程只读存储器(Erasable Programmable Read Only Memory,EPROM)、闪存、光纤、便携式紧凑磁盘只读存储器(Compact Disc Read-Only Memory,CD-ROM)、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中,计算机可读存储介质可以是任何包含或存储程序的有形介质,该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。而在本申请中,计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号,其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式,包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质,该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输,包括但不限于:无线、有线等等,或者上述的任意合适的组合。
描述于本申请实施例中所涉及到的单元可以通过软件的方式实现,也可以通过硬件的方式来实现,所描述的单元也可以设置在处理器中。其中,这些单元的名称在某种情况下并不构成对该单元本身的限定。
Claims (11)
1.一种飞行器的位姿确定方法,其特征在于,所述方法包括:
获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点;
确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标;
根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差;
获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵;
根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数;
根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
2.根据权利要求1所述的方法,其特征在于,所述图像数据包括多帧图像,在所述根据所述卡尔曼增益系数、所述协方差矩阵以及所述二维特征点各自对应的重投影误差对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿之后,所述方法还包括:
在所述多帧图像中确定关键帧图像,并确定所述关键帧图像的数量;
若所述关键帧图像的数量大于数量阈值,则从所述惯性测量数据中确定与所述关键帧图像具有相同时间戳的关键惯性测量数据;
根据所述关键惯性测量数据和所述关键帧图像对应的特征点,确定目标函数;
基于最小二乘法确定所述目标函数的最优解,并根据所述最优解对所述位姿和所述多个特征点各自对应的三维坐标进行优化,得到目标三维坐标,以使基于所述目标三维坐标进行地图构建。
3.根据权利要求2所述的方法,其特征在于,所述在所述多帧图像中确定关键帧图像,包括:
在所述多帧图像中确定每一帧图像对应的特征点数;
根据所述每一帧图像对应的特征点数,在所述图像数据中确定是否存在当前帧图像与上一帧图像的匹配特征点数是否小于点数阈值;
若小于所述点数阈值,则将所述当前帧图像确定为所述关键帧图像。
4.根据权利要求2所述的方法,其特征在于,所述在所述多帧图像中确定关键帧图像,包括:
获取历史关键帧,并确定所述历史关键帧对应的位姿信息;
确定所述多帧图像中的每一帧图像对应的位姿信息;
根据所述历史关键帧对应的位姿信息和所述多帧图像中的每一帧图像对应的位姿信息,确定历史关键帧与所述每一图像之间的相对距离;
若存在相对距离大于距离阈值阈值的图像,则将所述相对距离大于距离阈值的图像确定为所述关键帧图像。
5.根据权利要求2所述的方法,其特征在于,所述根据所述关键惯性测量数据和所述关键帧图像对应的特征点,确定目标函数,包括:
根据所述关键惯性测量数据确定所述关键帧图像对应的惯性数据预积分,并根据所述惯性数据预积分和所述关键惯性测量数据确定所述关键帧的第一残差;
确定所述关键帧图像中的关键特征点的关键二维坐标和所述关键特征点的关键三维坐标,并基于所述关键三维坐标和所述关键二维坐标确定所述关键特征点的第二残差,其中,所述第二残差为所述关键特征点的重投影误差;
根据滑动窗口的数量确定边缘帧图像,并确定所述边缘帧图像中的边缘二维特征点,其中,边缘帧图像为所述滑动窗口最前端的图像;
根据所述边缘二维特征点、所述第一残差和所述第二残差确定所述目标函数。
6.根据权利要求1-5任一项所述的方法,其特征在于,所述获取飞行器所采集的图像数据,并根据所述图像数据确定多个二维特征点,包括:
获取飞行器所采集的图像数据,并在所述图像数据中确定是否存在首帧图像,其中,所述首帧图像为所述飞行器所采集的第一帧的图像;
若确定不存在首帧图像,则对所述图像数据进行光流跟踪确定多个二维匹配点对;
对所述图像数据进行角点检测,得到多个二维角点;
根据所述多个二维匹配点对和所述多个二维角点确定所述多个二维特征点。
7.根据权利要求1-5任一项所述的方法,其特征在于,所述根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标,包括:
确定所述图像数据是否来自不同的图像采集装置;
若确定所述图像数据来自不同的图像采集装置,则根据双目三角化算法和所述多个二维特征点各自对应的二维坐标进行三维转换,确定所述多个二维特征点各自对应的三维坐标;或者
若确定所述图像数据来自同一图像采集装置,则根据多帧三角化算法和所述多个二维特征点各自对应的二维坐标进行三维转换,确定所述多个二维特征点各自对应的三维坐标。
8.根据权利要求1所述的方法,其特征在于,在所述根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标之后,所述方法还包括:
在所述多个二维特征点中确定重复次数大于次数阈值的目标特征点,并根据所述目标特征点的目标二维坐标确定所述目标特征点的参考三维坐标;
根据所述目标特征点的参考三维坐标进行重投影,得到所述目标特征点的参考二维坐标,并根据所述目标特征点的目标二维坐标和目标特征点的参考二维坐标确定参考重投影误差;
根据所述参考重投影误差对所述目标特征点的参考三维坐标进行修正,并根据修正后的参考三维坐标确定所述多个二维特征点各自对应的重投影误差。
9.一种飞行器的位姿确定装置,其特征在于,所述装置包括:
二维特征点确定模块,用于获取飞行器所采集的图像数据,并从所述图像数据中确定多个二维特征点;
三维转换模块,用于确定所述多个二维特征点各自对应的二维坐标,并根据所述多个二维特征点各自对应的二维坐标对所述多个二维特征点进行三维转换,确定所述多个二维特征点各自对应的三维坐标;
重投影误差确定模块,用于根据所述多个二维特征点各自对应的三维坐标进行二维投影,得到所述多个二维特征点各自对应的重投影二维坐标,并根据所述多个二维特征点各自对应的重投影二维坐标和所述多个二维特点各自对应的二维坐标确定所述多个二维特征点各自对应的重投影误差;
惯性数据处理模块,用于获取所述飞行器的惯性测量数据,并根据所述惯性测量数据确定所述飞行器的状态数据和所述飞行器的噪声数据以及协方差矩阵;
滤波模块,用于根据所述重投影误差、所述噪声数据、所述协方差矩阵和所述状态数据确定卡尔曼增益系数;
位姿确定模块,用于根据所述卡尔曼增益系数对所述状态数据进行修正,得到目标状态数据,并根据所述目标状态数据确定所述飞行器的位姿。
10.一种电子设备,其特征在于,所述电子设备包括:
处理器;
存储器,所述存储器上存储有计算机可读指令,所述计算机可读指令被所述处理器执行时,实现如权利要求1至8中任一项所述的方法。
11.一种计算机可读取存储介质,其特征在于,所述计算机可读取存储介质中存储有程序代码,所述程序代码可被处理器调用执行如权利要求1至8任一项所述的方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311851248.4A CN117804442A (zh) | 2023-12-28 | 2023-12-28 | 飞行器的位姿确定方法、装置、电子设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202311851248.4A CN117804442A (zh) | 2023-12-28 | 2023-12-28 | 飞行器的位姿确定方法、装置、电子设备及存储介质 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN117804442A true CN117804442A (zh) | 2024-04-02 |
Family
ID=90423351
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202311851248.4A Pending CN117804442A (zh) | 2023-12-28 | 2023-12-28 | 飞行器的位姿确定方法、装置、电子设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN117804442A (zh) |
-
2023
- 2023-12-28 CN CN202311851248.4A patent/CN117804442A/zh active Pending
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP6760114B2 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
JP6812404B2 (ja) | 点群データを融合させるための方法、装置、コンピュータ読み取り可能な記憶媒体、及びコンピュータプログラム | |
CN109059906B (zh) | 车辆定位方法、装置、电子设备、存储介质 | |
US9270891B2 (en) | Estimation of panoramic camera orientation relative to a vehicle coordinate frame | |
CN112734852B (zh) | 一种机器人建图方法、装置及计算设备 | |
EP2572319B1 (fr) | Procede et systeme pour fusionner des donnees issues de capteurs d'images et de capteurs de mouvement ou de position | |
CN112219087A (zh) | 位姿预测方法、地图构建方法、可移动平台及存储介质 | |
JP4984659B2 (ja) | 自車両位置推定装置 | |
JP7131994B2 (ja) | 自己位置推定装置、自己位置推定方法、自己位置推定プログラム、学習装置、学習方法及び学習プログラム | |
EP2724203A1 (fr) | Génération de données de carte | |
US20180075614A1 (en) | Method of Depth Estimation Using a Camera and Inertial Sensor | |
CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
CN111366153B (zh) | 一种激光雷达与imu紧耦合的定位方法 | |
CN111623773B (zh) | 一种基于鱼眼视觉和惯性测量的目标定位方法及装置 | |
CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
CN114013449A (zh) | 针对自动驾驶车辆的数据处理方法、装置和自动驾驶车辆 | |
CN114217665A (zh) | 一种相机和激光雷达时间同步方法、装置及存储介质 | |
CN110989619B (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
KR20200037502A (ko) | 포즈 정보를 출력하는 방법 및 장치 | |
CN113495281B (zh) | 可移动平台的实时定位方法及装置 | |
CN115560744A (zh) | 机器人以及基于多传感器的三维建图方法、存储介质 | |
CN117804442A (zh) | 飞行器的位姿确定方法、装置、电子设备及存储介质 | |
CN113034538B (zh) | 一种视觉惯导设备的位姿跟踪方法、装置及视觉惯导设备 | |
CN113744308A (zh) | 位姿优化方法、装置、电子设备、介质及程序产品 | |
CN115077467B (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 |