CN111811501B - 一种基于树干特征的无人机定位方法、无人机及存储介质 - Google Patents

一种基于树干特征的无人机定位方法、无人机及存储介质 Download PDF

Info

Publication number
CN111811501B
CN111811501B CN202010597975.2A CN202010597975A CN111811501B CN 111811501 B CN111811501 B CN 111811501B CN 202010597975 A CN202010597975 A CN 202010597975A CN 111811501 B CN111811501 B CN 111811501B
Authority
CN
China
Prior art keywords
trunk
frame
measurement unit
unmanned aerial
aerial vehicle
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.)
Active
Application number
CN202010597975.2A
Other languages
English (en)
Other versions
CN111811501A (zh
Inventor
王星博
崔金强
奚乐乐
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Peng Cheng Laboratory
Original Assignee
Peng Cheng Laboratory
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Peng Cheng Laboratory filed Critical Peng Cheng Laboratory
Priority to CN202010597975.2A priority Critical patent/CN111811501B/zh
Publication of CN111811501A publication Critical patent/CN111811501A/zh
Application granted granted Critical
Publication of CN111811501B publication Critical patent/CN111811501B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes

Abstract

本发明公开了一种基于树干特征的无人机定位方法、无人机及存储介质，所述方法包括：根据无人机的惯性测量单元的测量数据获取所述无人机的定位参数在预设时段内各个时刻分别与惯性测量单元参数的第一对应关系；获取所述惯性测量单元在所述预设时段内的数据残差与所述惯性测量单元参数的第二对应关系；根据树干特征获取所述预设时段内的树干特征投影残差与所述惯性测量单元参数的第三对应关系；根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取目标惯性测量单元参数。所述基于树干特征的无人机定位方法基于树干特征以及无人机的惯性测量单元来对无人机进行定位，能够在森林环境下不依靠GPS信号实现定位。

Description

一种基于树干特征的无人机定位方法、无人机及存储介质
技术领域
本发明涉及无人机技术领域，特别涉及一种基于树干特征的无人机定位方法、无人机及存储介质。
背景技术
在野外森林环境中，人工及地面车辆的正常工作与活动受到极大的限制，难以保证作业的高效开展，无人机由于灵活方便的特性，是执行森林地带等复杂环境下作业任务的理想工具。目前无人机的定位是基于GPS信号来进行，但由于野外森林环境存在大量严重遮挡，导致GPS信号丢失而无法进行定位。
因此，现有技术还有待改进和提高。
发明内容
针对现有技术的上述缺陷，本发明提供一种基于树干特征的无人机定位方法、无人机及存储介质，旨在解决现有技术中依靠GPS信号定位在野外森林环境下无人机无法定位的问题。
为了解决上述技术问题，本发明所采用的技术方案如下：
本发明的第一方面，提供一种基于树干特征的无人机定位方法，所述方法包括：
获取无人机的惯性测量单元的测量数据，根据所述测量数据获取所述无人机的定位参数在预设时段内各个时刻分别与惯性测量单元参数的第一对应关系，其中，所述定位参数包括无人机的位置、速度和姿态，所述预设时段为图像序列中n个连续图像帧对应的时间段，所述图像序列通过所述无人机的机载相机获取；
根据所述第一对应关系获取所述惯性测量单元在所述预设时段内的数据残差与所述惯性测量单元参数的第二对应关系；
根据所述图像序列获取树干特征，根据所述树干特征获取所述预设时段内的树干特征投影残差与所述惯性测量单元参数的第三对应关系；
根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取所述预设时段内各个图像帧分别对应的目标惯性测量单元参数，并根据所述目标惯性测量单元参数对所述无人机进行定位。
所述的基于树干特征的无人机定位方法，其中，所述惯性测量单元包括加速度计和陀螺仪，所述测量数据包括加速度计测量值和陀螺仪测量值，所述惯性测量单元参数包括加速度计偏置、加速度计噪声、陀螺仪偏置和陀螺仪噪声；所述定位参数与惯性测量单元参数的第一对应关系通过第一预设公式表示，所述第一预设公式为：
Figure BDA0002558040190000021
Figure BDA0002558040190000022
Figure BDA0002558040190000023
其中，l+1时刻为l时刻的后一时刻，Δt为l+1时刻和l时刻之间的间隔，
Figure BDA0002558040190000024
分别为l,l+1时刻无人机的位置，
Figure BDA00025580401900000213
为三维数组，
Figure BDA0002558040190000025
分别为l,l+1时刻无人机的速度，
Figure BDA0002558040190000026
分别为l,l+1时刻无人机的姿态的四元数表示，gw为世界坐标系中的重力加速度，
Figure BDA0002558040190000027
为τ时刻的无人机的姿态的旋转矩阵表示，
Figure BDA0002558040190000028
为τ时刻的加速度计测量值，
Figure BDA0002558040190000029
为τ时刻的加速度计偏置，na为加速度计噪声，s为虚拟积分符号，
Figure BDA00025580401900000210
为τ时刻的陀螺仪测量值，wm为三维数组，
Figure BDA00025580401900000211
为为τ时刻的陀螺仪偏置，ng为陀螺仪噪声，
Figure BDA00025580401900000212
为无人机τ时刻相对于l时刻的姿态变化。
所述的基于树干特征的无人机定位方法，其中，所述数据残差与所述惯性测量单元参数的第二对应关系通过第二预设公式被表示，所述第二预设公式为：
Figure BDA0002558040190000031
Figure BDA0002558040190000032
Figure BDA0002558040190000033
Figure BDA0002558040190000034
Figure BDA0002558040190000035
Figure BDA0002558040190000036
其中，ei-1,i为所述图像序列中第i-1帧和第i帧之间的所述测量数据的数据残差，
Figure BDA0002558040190000037
δba T,δbg T分别为无人机的位置残差、姿态对应的旋转角残差、速度残差、加速度计偏置残差、陀螺仪偏置残差，
Figure BDA0002558040190000038
为第i帧对应的无人机姿态，
Figure BDA0002558040190000039
为第j帧对应的无人机位置，
Figure BDA00025580401900000310
为第i帧对应的无人机速度，
Figure BDA00025580401900000311
为第j帧相对于第i帧的无人机位置变化，
Figure BDA00025580401900000312
为第j帧相对于第i帧的无人机姿态变化，
Figure BDA00025580401900000313
为第j帧对应的无人机姿态，
Figure BDA00025580401900000314
为第j帧相对于第i帧的无人机速度变化，
Figure BDA00025580401900000315
为第j帧对应的加速度计偏置，
Figure BDA00025580401900000316
为第j帧对应的陀螺仪偏置，第i帧为第j帧的前一帧。
所述的基于树干特征的无人机定位方法，其中，所述根据所述图像序列获取树干特征包括：
对于每一个图像帧对，获取所述图像帧对对应的光流，根据所述光流获取所述图像帧对对应的特征点对，其中，所述图像帧对包括第一图像帧和与所述第一图像帧相邻的第二图像帧，每个特征点对包括所述第一图像帧上的第一特征点和所述第二图像帧上与所述第一特征点对应的第二特征点；
根据各个图像帧对分别对应的所述特征点对获取所述树干特征。
所述的基于树干特征的无人机定位方法，其中，所述根据各个图像帧对分别对应的所述特征点对获取所述树干特征包括：
对于新获取的每一帧图像帧，检测所述图像帧上的目标树干矩形区域，获取所述目标树干矩形区域中包括的目标特征点的目标识别信息，根据所述目标识别信息以及已存储的树干矩形区域的特征点识别信息确定所述目标树干矩形区域是否为新的树干矩形区域；
如是，则建立所述目标树干矩形区域对应的树干特征；
其中，每个特征点对中包括的所述第一特征点和所述第二特征点的识别信息相同。
所述的基于树干特征的无人机定位方法，其中，所述树干特征投影残差与所述惯性测量单元参数的第三对应关系通过第三预设公式被表示，所述第三预设公式为：
Figure BDA0002558040190000041
其中，ec,k,m为第k帧中树干特征mc的树干区域中的点云m对应的所述树干特征投影残差，
Figure BDA0002558040190000042
为点云m在世界坐标系中的坐标
Figure BDA0002558040190000043
到树干特征mc表面的距离。
所述的基于树干特征的无人机定位方法，其中，所述根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取目标惯性测量单元参数包括：
求解预设的优化函数，获取使得所述优化函数达到最小值的惯性测量单元参数序列，其中所述关系测量单元参数序列中包括所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数；
所述优化函数为：
Figure BDA0002558040190000044
其中，ep为先验残差，Vp -1为ep的权重，ef,k为第k帧中的特征点f对应的重投影残差，F为第k帧中的特征点的个数，Qf,k -1为ef,k的权重，ek-1,k为k-1,k两帧间的所述数据残差，Rk-1,k -1为ek-1,k的权重，ec,k,m为第k帧中树干特征mc区域中点云m对应的所述树干特征投影残差，Uc,k,m -1为ec,k,m的权重，PCk为第k帧中
Figure BDA0002558040190000045
的树干区域的点云，v为第k帧中树干特征的个数，n为所述预设时段内的视频帧的数量。
所述的基于树干特征的无人机定位方法，其中，所述根据所述目标惯性测量单元参数对所述无人机进行定位包括：
根据所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数以及所述第一对应关系获取所述预设时段内各个图像帧分别对应的所述定位参数。
本发明的第二方面，提供一种无人机，所述无人机包括处理器、与处理器通信连接的存储介质，所述存储介质适于存储多条指令，所述处理器适于调用所述存储介质中的指令，以执行实现上述任一项所述的基于树干特征的无人机定位方法的步骤。
本发明的第三方面，提供一种存储介质，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现上述任一项所述的基于树干特征的无人机定位方法的步骤。
与现有技术相比，本发明提供了一种基于树干特征的无人机定位方法、无人机及存储介质，所述基于树干特征的无人机定位方法基于树干特征以及无人机的惯性测量单元来对无人机进行定位，能够在森林环境下不依靠GPS信号实现定位。
附图说明
图1为本发明提供的基于树干特征的无人机定位方法的实施例的流程图；
图2为本发明提供的基于树干特征的无人机定位方法的实施例中子步骤的流程图；
图3为本发明提供的无人机的实施例的原理示意图。
具体实施方式
为使本发明的目的、技术方案及效果更加清楚、明确，以下参照附图并举实施例对本发明进一步详细说明。应当理解，此处所表示的具体实施例仅用以解释本发明，并不用于限定本发明。
实施例一
本发明提供的基于树干特征的无人机定位方法，可以是应用在无人机中，所述无人机可以通过本发明提供的基于树干特征的无人机定位方法进行定位，也可以是应用在安装在无人机上的装置中，所述装置可以与无人机进行通信，并通过本发明提供的基于树干特征的无人机定位方法对无人机进行定位。如图1所示，所述基于树干特征的无人机定位方法的一个实施例中，包括步骤：
S100、获取无人机的惯性测量单元的测量数据，根据所述测量数据获取所述无人机的定位参数在预设时段内各个时刻分别与惯性测量单元参数的第一对应关系。
所述定位参数包括无人机的位置、速度和姿态，所述预设时段为图像序列中n个连续图像帧对应的时间段，所述图像序列对所述无人机的记载相机拍摄的图像序列。
在现有技术中，能够通过对惯性测量单元采集的测量数据进行积分的方式来获取无人机的位置，实现无人机的定位，但是由于惯性测量单元的测量数据受到噪声和偏置等参数的影响，定位精度低，无法满足长期导航对定位精度的需求，在本实施例中，是通过测量数据的数据残差以及树干特征投影残差来对惯性测量单元参数进行优化，获取使得残差更小的关系测量单元参数，实现更准确的定位。具体地，在本实施例中，所述惯性测量单元(IMU)包括加速度计和陀螺仪，所述关系测量单元参数包括加速度计偏置、加速度计噪声、陀螺仪偏置和陀螺仪噪声，所述根据所述测量数据获取所述无人机的定位参数在预设时段内各个时刻分别与惯性测量单元参数的第一对应关系包括：
所述定位参数与惯性测量单元参数的第一对应关系通过第一预设公式表示，所述第一预设公式为：
Figure BDA0002558040190000071
Figure BDA0002558040190000072
Figure BDA0002558040190000073
其中，l+1时刻为l时刻的后一时刻，Δt为l+1时刻和l时刻之间的间隔，
Figure BDA0002558040190000074
分别为l,l+1时刻无人机的位置，
Figure BDA0002558040190000075
为三维数组，
Figure BDA0002558040190000076
分别为l,l+1时刻无人机的速度，
Figure BDA0002558040190000077
分别为l,l+1时刻无人机的姿态的四元数表示，gw为世界坐标系中的重力加速度，
Figure BDA0002558040190000078
为τ时刻的无人机的姿态的旋转矩阵表示，
Figure BDA0002558040190000079
为τ时刻的加速度计测量值，
Figure BDA00025580401900000710
为τ时刻的加速度计偏置，na为加速度计噪声，s为虚拟积分符号，
Figure BDA00025580401900000711
为τ时刻的陀螺仪测量值，wm为三维数组，
Figure BDA00025580401900000712
为为τ时刻的陀螺仪偏置，ng为陀螺仪噪声，
Figure BDA00025580401900000713
为无人机τ时刻相对于l时刻的姿态变化。
值得说明的是，无人机的姿态的旋转矩阵表示和四元数表示可以互相转化。
其中，在所述基于树干特征的无人机定位方法开始的第一时刻，所述无人机的位置、速度和姿态是可以确切获取的，不难看出，根据所述第一预设公式可以将所述预设时段内各个时刻的无人机的位置、速度和姿态用所述惯性测量单元参数表示出来。
在获取到各个时刻的无人机位置、速度和姿态的表示后可以获取相邻两帧图像之间的无人机相对运动，并获取相邻两帧图像之间的所述测量数据协方差矩阵。
具体地，在本实施例中，首先是积分计算相邻两帧之间的相对运动：
相邻的第i帧和第j帧图像帧之间的相对姿态变化、速度变化以及位置变化分别为：
Figure BDA0002558040190000081
Figure BDA0002558040190000082
Figure BDA0002558040190000083
Figure BDA0002558040190000084
其中，相邻的第i帧和第j帧之间存在多个时刻，l时刻为第i帧和第j帧之间的一个时刻，
Figure BDA0002558040190000085
分别表示无人机在第帧的时刻相对于第i帧的时刻的相对位置，度和姿态，
Figure BDA0002558040190000086
为第i帧对应的陀螺仪偏置，
Figure BDA0002558040190000087
为l时刻相对于第i帧的时刻的姿态变化的旋转矩阵表示，
Figure BDA0002558040190000088
为l-1时刻相对于第i帧的时刻的速度变化，
Figure BDA0002558040190000089
为第i帧对应的加速度计偏置，
Figure BDA00025580401900000810
为l时刻的加速度计测量值，
Figure BDA00025580401900000811
为l时刻的陀螺仪测量值。
在i,j两帧之间的l+1时刻，根据扩展卡尔曼滤波(EKF)中的预测步骤，更新上述公式的预积分误差协方差矩阵Pi,l+1为：
Pi,l+1＝Φ(l+1,l)Pi,lΦ(l+1,l)T+Qd
Figure BDA00025580401900000812
其中，
Figure BDA00025580401900000813
Figure BDA0002558040190000091
Figure BDA0002558040190000092
I为单位矩阵，
Figure BDA0002558040190000093
可通过
Figure BDA0002558040190000094
转换求解，
Figure BDA0002558040190000095
分别为加速度计噪声的方差，陀螺仪噪声的方差、加速度计偏置的方差以及陀螺仪偏置的方差。
对i,j两帧图像之间所有时刻的数据进行上述的递归计算，即可得到两帧间误差状态的协方差矩阵Pi,j，Pi,j可用于求解目标惯性测量单元参数的过程中，具体将在后文被详细说明。
请再次参阅图1，所述基于树干特征的无人机定位方法还包括：
S200、根据所述第一对应关系获取所述所述惯性测量单元在所述预设时段内的数据残差与所述惯性测量单元参数的第二对应关系。
具体地，所述根据所述第一对应关系获取所述测量数据在所述预设时段内的数据残差与所述惯性测量单元参数的第二对应关系包括：
所述数据残差与所述惯性测量单元参数的第二对应关系通过第二预设公式被表示，所述第二预设公式为：
Figure BDA0002558040190000096
Figure BDA0002558040190000101
Figure BDA0002558040190000102
Figure BDA0002558040190000103
Figure BDA0002558040190000104
Figure BDA0002558040190000105
其中，ei-1,i为所述图像序列中第i-1帧和第i帧之间的所述测量数据的数据残差，
Figure BDA0002558040190000106
δba T,δbg T分别为无人机的位置残差、姿态对应的旋转角残差、速度残差、加速度计偏置残差、陀螺仪偏置残差，
Figure BDA0002558040190000107
为第i帧对应的无人机姿态，
Figure BDA0002558040190000108
为第j帧对应的无人机位置，
Figure BDA0002558040190000109
为第i帧对应的无人机速度，
Figure BDA00025580401900001010
为第j帧相对于第i帧的无人机位置变化，
Figure BDA00025580401900001011
为第j帧相对于第i帧的无人机姿态变化，
Figure BDA00025580401900001012
为第j帧对应的无人机姿态，
Figure BDA00025580401900001013
为第j帧相对于第i帧的无人机速度变化，
Figure BDA00025580401900001014
为第j帧对应的加速度计偏置，
Figure BDA00025580401900001015
为第j帧对应的陀螺仪偏置，第i帧为第j帧的前一帧。
下面对各个项的具体计算方式进行说明：
Figure BDA00025580401900001016
Figure BDA00025580401900001017
Figure BDA00025580401900001018
Δba,Δbg分别为第j帧相对于第i帧的bg,ba变化，由于所述数据残差向中用到了步骤S100中计算的预积分值
Figure BDA00025580401900001019
计算时用到了
Figure BDA00025580401900001020
在优化过程中当bg,ba状态发生改变时，使用原始预积分值
Figure BDA00025580401900001021
对原始偏置
Figure BDA00025580401900001022
的一阶近似修正预积分，从而避免重新计算
Figure BDA00025580401900001023
造成的计算资源浪费。
Figure BDA00025580401900001024
对偏置bg的一阶雅可比矩阵分别为：
Figure BDA0002558040190000111
Figure BDA0002558040190000112
可以用递推的方式计算，在i,j两帧之间的l时刻，每收到一组IMU数据，
Figure BDA0002558040190000113
更新如下：
Figure BDA0002558040190000114
其中，
Figure BDA0002558040190000115
Figure BDA0002558040190000116
φ＝(φ1 φ2 φ3)T
Figure BDA0002558040190000117
Figure BDA0002558040190000118
有同样的性质，其递推公式分别为：
Figure BDA0002558040190000119
Figure BDA00025580401900001110
Figure BDA00025580401900001111
Figure BDA00025580401900001112
Figure BDA00025580401900001113
其中，，
Figure BDA00025580401900001114
可通过
Figure BDA00025580401900001115
转换求解，
Figure BDA00025580401900001117
为第i帧对应的陀螺仪偏置，
Figure BDA00025580401900001118
为第i帧对应的加速度计偏置。
请再次参阅图1，所述基于树干特征的无人机定位方法还包括步骤：
S300、根据所述图像序列获取树干特征，根据所述树干特征获取所述预设时段内的树干特征投影残差与所述惯性测量单元参数的第三对应关系。
具体地，如图2所示，所述根据所述图像序列获取树干特征包括：
S310、对于每一个图像帧对，获取所述图像帧对对应的光流，根据所述光流获取所述图像帧对对应的特征点对；
其中，所述图像帧对包括第一图像帧和与所述第一图像帧相邻的第二图像帧，每个特征点对包括所述第一图像帧上的第一特征点和所述第二图像帧上与所述第一特征点对应的第二特征点。所述特征点对中的第一特征点和所述第二特征点是实际环境中的同一特征点，也就是说，二者是同一物体上的同一点在不同帧上的显示，所述获取所述图像帧对对应的光流可以是根据现有的光流算法得到，在本实施例中，是通过LK光流法(Lucas-Kanade光流法)来获取所述图像帧对对应的光流，根据所述光流可以获取所述特征点对，此为现有技术，在此不再赘述。对于所述图像序列中的第一个图像帧，先进行特征点提取，在本实施例中，是通过FAST(Features From Accelerated SegmentTest)算法来提取特征点，当然，也可以采用其他的特征点提取算法来提取特征点，本发明对此不做具体的限制，此后对于新获取的每一帧，都与前一帧构成所述图像帧对，从而可以获取到所述特征点对，即可以获取到前一帧的特征点在当前帧中的位置。特别地，对于当前帧图像中没有特征点的区域，提取新的特征点。
对于每一个特征点，都进行存储，具体的存储方式为：每一个特征点对应有特征点数据，所述特征点数据中包括了特征点的识别信息以及观测到该特征点的图像帧ID以及该特征点在该图像帧上的2D位置，所述识别信息可以设置为特征点的ID，所述识别信息用于识别特征点，不难看出，对于每个特征点对中的第一特征点和第二特征点，应具有相同的识别信息。
S320、根据各个图像帧对分别对应的所述特征点对获取所述树干特征。
在获取到所述特征点对后，可以根据所述特征点对来获取各个图像帧中的树干特征。具体地，所述根据各个图像帧对分别对应的所述特征点对获取所述树干特征包括：
S321、对于新获取的每一帧图像帧，检测所述图像帧上的目标树干矩形区域，获取所述目标树干矩形区域中包括的目标特征点的目标识别信息，根据所述目标识别信息以及已存储的树干矩形区域的特征点识别信息确定所述目标树干矩形区域是否为新的树干矩形区域；
S322、如是，则建立所述目标树干矩形区域对应的树干特征。
在本实施例中，根据所述无人机的记载相机拍摄的所述图像序列构建树干地图，所述树干地图中存储有所述无人机所在环境中的各个树干，以及各个树干分别对应的树干数据，所述树干数据包括树干的特征(在本实施例中，为树干的圆柱模型)、观测到该树干的图像帧ID、该图像帧上检测的该树干的树干矩形区域以及该树干矩形区域中包含的特征点。
具体地，每次获取到新的图像帧时，首先对该图像帧进行内容识别，检测所述图像帧上的树干矩形区域，对于每一个树干矩形区域进行处理时，将该树干矩形区域作为所述目标树干矩形区域，将所述目标树干矩形区域中包括的特征点作为目标特征点，首先遍历所述树干地图中所有的树干，获取每个树干对应的最新的所述特征点数据，确定每个树干对应的最新的特征点的识别信息，当所述树干地图中的某个树干具有的与所述目标树干矩形区域中的所述目标特征点的目标识别信息相同的特征点的数量超过预设值时，则确定该树干与所述目标树干矩形区域对应的树干为同一树干，如果遍历所述树干地图中的所有树干后，都不存在与所述目标树干矩形区域的特征点识别信息相同的特征点数量超过预设值的树干，那么，确定所述目标树干矩形区域对应的为一个新的树干，将其加入到所述树干地图中去。
在确定所述目标树干矩形对应的树干为新的树干后，建立所述目标树干矩形区域对应的树干特征。
在本实施例中，树干特征是树干对应的圆柱模型，所述建立所述目标树干矩形区域对应的树干特征是根据深度图提取所述目标树干矩形区域中所有像素点的深度数据，再通过相机内参反算出所述目标树干矩形区域中所有像素点在相机坐标系下的三维点云，利用这些属于树干的三维点云估计树干的圆柱参数。
在本实施例中，对于树干c，其对应的圆柱模型可以表示为
Figure BDA0002558040190000141
其中，ρ为圆柱模型上的点到世界坐标系原点的距离，n为该点的单位法向量(指向圆柱内部)，单位法向量可以用球坐标系中的两个角度
Figure BDA0002558040190000142
θ表示：
Figure BDA0002558040190000143
Figure BDA0002558040190000144
为n在X-Y平面的投影与X轴正向的夹角，θ∈[0,π]为n与Z轴正向的夹角。已知
Figure BDA0002558040190000145
θ，该点在球坐标系中的另两个单位矢量为：
Figure BDA0002558040190000146
α为圆柱旋转轴与nθ之间的夹角。κ为圆柱模型的曲率半径。
根据以下相机模型：
Figure BDA0002558040190000147
其中，(Xc Yc Zc)T为某点在相机坐标系中的坐标，(u v)T为该点在图像坐标系中的坐标，fx,fy,cx,cy为相机模型的内参数。利用深度图测量值(u v d)T可以算出该点在相机坐标系中的坐标：
Figure BDA0002558040190000148
在获得树干上的所有三维点云
Figure BDA0002558040190000151
后，计算所有三维点云的法向量，求解点云
Figure BDA0002558040190000152
的法向量
Figure BDA0002558040190000153
问题，可以通过解决一个最小二乘平面拟合问题得到：对每个点云在KD树中查找它的K邻域，K邻域中所有点云的位置均值为
Figure BDA0002558040190000154
K邻域中第i个点云到拟合平面的距离为
Figure BDA0002558040190000155
令所有的di＝0，得到一个齐次最小二乘问题，可以通过PCA(Principal Component Analysis，主成分分析)求解，法向量的解为协方差矩阵C的最小特征值λ2对应的特征向量v2：
Figure BDA0002558040190000156
在获得所有三维点云
Figure BDA0002558040190000157
及对应的法向量
Figure BDA0002558040190000158
后，估计圆柱参数的初值。具体地，圆柱参数的初值是通过树干的三维点云中距离世界坐标系远点最近的点云p0来获取，在查找到距离世界坐标系原点最近的点云p0后，则得到ρ0和该点的单位法向量n0，
Figure BDA0002558040190000159
θ0可由n0分解得到：
Figure BDA00025580401900001510
若
Figure BDA00025580401900001511
则
Figure BDA00025580401900001512
Figure BDA00025580401900001513
Figure BDA00025580401900001514
分别为n0在X和Y平面上的分量。
计算p0在球坐标系中的另外两个单位矢量：
Figure BDA00025580401900001515
Figure BDA00025580401900001516
估计圆柱模型的旋转轴：任意选取法向量非平行的4个点p0,p1,p2,p3，对应的单位法向量分别为：n0、n1、n2、n3，设轴线上的两点为p0+t0n0，p1+t1n1。则向量组p0+t0n0-p2，p1+t1n1-p0-t0n0，n2与向量组p0+t0n0-p3，p1+t1n1-p0-t0n0，n3均满足共面约束，三个共面向量的三重积等于0，即：
((p0+t0n0-p2)(p1+t1n1-p0-t0n0)n2)＝0
((p0+t0n0-p3)(p1+t1n1-p0-t0n0)n3)＝0
展开可以得到：
(n0n1n2)t0t1+((p2-p1)n0n2)t0+((p0-p2)n1n2)t1+((p0-p2)(p1-p0)n2)＝0
(n0n1n3)t0t1+((p3-p1)n0n3)t0+((p0-p3)n1n3)t1+((p0-p3)(p1-p0)n3)＝0
消去t0t1，可以分别得到关于t0,t1的二次方程，理想情况下可以得到{t0,t1}的四组解{t0(0),t1(0)},{t0(0),t1(1)},{t0(1),t1(0)},{t0(1),t1(1)}，因此旋转轴的初值存在4种可能：
a0(0)＝(p1+t1n1(0))-(p0+t0n0(0))
a0(1)＝(p1+t1n1(1))-(p0+t0n0(0))
a0(2)＝(p1+t1n1(0))-(p0+t0n0(1))
a0(3)＝(p1+t1n1(1))-(p0+t0n0(1))
对每个可能的旋转轴a0(i)，计算其与
Figure BDA0002558040190000161
的夹角得到α0。
估计该点的曲率：κ0＝λ2(λ0+λ1+λ2)，λi为计算该点法向量时的分解协方差矩阵得到的特征值，λ2＜λ1＜λ0。
对圆柱c，在得到圆柱参数的初值
Figure BDA0002558040190000162
后，建立代价函数优化圆柱参数，对每一个点云
Figure BDA0002558040190000163
有代价函数：
Figure BDA0002558040190000164
代价函数对各参数的导数为：
Figure BDA0002558040190000171
Figure BDA0002558040190000172
Figure BDA0002558040190000173
Figure BDA0002558040190000174
Figure BDA0002558040190000175
根据所述代价函数对所述圆柱参数进行优化，得到优化后的圆柱参数mc。
在获取到所述树干特征后，根据所述树干特征获取所述预设时段内的树干特征投影残差与所述惯性测量单元参数的第三对应关系。
具体地，所述树干特征投影残差与所述惯性测量单元参数的第三对应关系通过第三预设公式被表示，所述第三预设公式为：
Figure BDA0002558040190000176
ec,k,m为第k帧中树干
Figure BDA0002558040190000177
的树干区域中的点云m对应的所述树干特征投影残差，
Figure BDA0002558040190000178
的物理意义为点云m在世界坐标系中的坐标
Figure BDA0002558040190000179
到树干特征(圆柱模型)
Figure BDA00025580401900001710
表面的距离，
Figure BDA00025580401900001711
为第k帧对应的无人机姿态，
Figure BDA00025580401900001712
为第k帧对应的无人机的位置，n0为树干
Figure BDA00025580401900001713
的树干区域中距离世界坐标系的远点最近的点云的单位法向量，a为树干
Figure BDA00025580401900001714
的旋转轴。
请再次参阅图1，所述基于树干特征的无人机定位方法还包括步骤：
S400、根据所述第一对应关系、所述第二对应关系和所述第三对应关系获取所述预设时段内各个图像帧分别对应的目标惯性查了单元参数，并根据所述目标惯性测量单元参数对所述无人机进行定位。
具体地，在步骤S300中，是求解预设的优化函数，获取使得所述优化函数达到最小值的惯性测量单元参数序列，其中，所述惯性测量单元参数序列中包括所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数。
所述优化函数为：
Figure BDA0002558040190000181
其中，ek-1,k为k-1,k两帧间的所述数据残差，Rk-1,k -1为ek-1,k的权重，ec,k,m为第k帧中树干特征mc区域中点云m对应的所述树干特征投影残差，Uc,k,m -1为ec,k,m的权重，PCk为第k帧中
Figure BDA0002558040190000182
的树干区域的点云，v为第k帧中树干特征的个数，n为所述预设时段内的视频帧的数量，结合前面的说明不难看出，所述惯性测量单元参数为所述优化函数中的变量，在本实施例中，是建立大小为n的滑动窗口，优化滑动窗口中的变量，这样，可以在无人机的飞行过程中实现对所述惯性测量单元参数的持续优化。根据所述优化函数求解使得J最小的每个图像帧分别对应的所述目标惯性测量单元参数。
Rk-1,k -1可以设置为步骤S100中获取的惯性测量单元数据的误差状态协方差矩阵Pi,j的逆矩阵，Uc,k,m -1可以设置为单位阵。
所述优化函数的求解，使用ceres优化库，梯度下降策略使用LevenbergMarquardt方法。
在另一种可能的实现方式中，为了提升定位精度，还结合特征点重投影残差和先验残差获取所述目标惯性测量单元参数，在这种实现方式中，所述优化函数为：
Figure BDA0002558040190000183
其中，ep为先验残差，Vp -1为ep的权重，ef,k为第k帧中的特征点f对应的重投影残差，F为第k帧中的特征点的个数，Qf,k -1为ef,k的权重，ek-1,k为k-1,k两帧间的所述数据残差，Rk-1,k -1为ek-1,k的权重，ec,k,m为第k帧中树干特征mc区域中点云m对应的所述树干特征投影残差，Uc,k,m -1为ec,k,m的权重，PCk为第k帧中
Figure BDA0002558040190000191
的树干区域的点云，v为第k帧中树干特征的个数，n为所述预设时段内的视频帧的数量。
Vp -1设置为单位阵，Qf,k -1为提取图像帧中的特征点时特征尺度(大小)的倒数，FAST算法的特征尺度为1，因此，当采用FAST算法提取特征点时，Qf,k -1为单位阵。
下面对先验残差以及特征点重投影残差的获取方式进行说明：
特征点重投影残差ef,k：
特征点重投影残差采用AHP(层次分析法)参数化表示，特征点f属于第k-1帧，其参数表示为：
Figure BDA0002558040190000192
其在第k帧图像上的重投影残差为：
Figure BDA0002558040190000193
Figure BDA0002558040190000194
h()是点特征路标的投影函数，zf,k为特征点f在第k帧上的投影观测值，
Figure BDA0002558040190000195
与TBC＝[RBC,pBC]分别表示第k帧时刻的无人机位姿和无人机机载相机到惯性测量单元的外参变换。
先验残差ep：
已知先验变量xp及其协方差矩阵Hpp，对Hpp分解有Hpp＝Jpp TJpp，先验残差的更新为：
ep＝ep0+Jppδxp
ep0为先验残差初值，每次优化前设为0。
完成优化之后，计算下一次优化中要用到的先验变量xp：
先验变量的分布(协方差)可以通过求解此增量方程得到：
Figure BDA0002558040190000201
xm是将要被边缘化(从滑动窗口中移除)的变量，在滑动窗口中所有包含xm的残差记为em，xp为em中与xm组成残差项的其它优化变量，xp将作为下一次优化中要用到的先验变量。
em对xm,xp的雅可比矩阵分别记为Jmm,Jpp，则增量方程中的各项为：
Hmm＝Jmm TJmm,Hmp＝Jpp TJmm
Hpm＝Jmm TJpp,Hpp＝Jpp TJpp
bm＝-Jmm Tem,bp＝-Jpp Tep
边缘化xm后，先验变量xp的协方差矩阵可以通过舒尔消元法算得：
Figure BDA0002558040190000202
至此，求得下一次优化时先验变量的协方差矩阵Hpp *。
在根据所述预设函数获取到所述惯性测量单元参数序列后，根据所述惯性测量单元参数序列中包括的所述预设时段内各个帧分别对应的所述目标惯性测量单元参数以及定位参数与惯性测量单元参数的所述第一对应关系，即可获取所述预设时段内各个图像帧分别对应的所述定位参数，即可以获取所述预设时段内各个图像帧分别对应的无人机位置、速度以及姿态，实现对所述无人机的定位。
综上所述，本实施例提供一种基于树干特征的无人机定位方法，所述基于树干特征的无人机定位方法所述基于树干特征的无人机定位方法基于树干特征以及无人机的惯性测量单元来对无人机进行定位，能够在森林环境下不依靠GPS信号实现定位。
应该理解的是，虽然本发明说明书附图中给出的的流程图中的各个步骤按照箭头的指示依次显示，但是这些步骤并不是必然按照箭头指示的顺序依次执行。除非本文中有明确的说明，这些步骤的执行并没有严格的顺序限制，这些步骤可以以其它的顺序执行。而且，流程图中的至少一部分步骤可以包括多个子步骤或者多个阶段，这些子步骤或者阶段并不必然是在同一时刻执行完成，而是可以在不同的时刻执行，这些子步骤或者阶段的执行顺序也不必然是依次进行，而是可以与其它步骤或者其它步骤的子步骤或者阶段的至少一部分轮流或者交替地执行。
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的计算机程序可存储于一非易失性计算机可读取存储介质中，该计算机程序在执行时，可包括如上述各方法的实施例的流程。其中，本发明所提供的各实施例中所使用的对存储器、存储、数据库或其它介质的任何引用，均可包括非易失性和/或易失性存储器。非易失性存储器可包括只读存储器(ROM)、可编程ROM(PROM)、电可编程ROM(EPROM)、电可擦除可编程ROM(EEPROM)或闪存。易失性存储器可包括随机存取存储器(RAM)或者外部高速缓冲存储器。作为说明而非局限，RAM以多种形式可得，诸如静态RAM(SRAM)、动态RAM(DRAM)、同步DRAM(SDRAM)、双数据率SDRAM(DDRSDRAM)、增强型SDRAM(ESDRAM)、同步链路(Synchlink)DRAM(SLDRAM)、存储器总线(Rambus)直接RAM(RDRAM)、直接存储器总线动态RAM(DRDRAM)、以及存储器总线动态RAM(RDRAM)等。
实施例二
基于上述实施例，本发明还相应提供了一种无人机，如图3所示，所述无人机包括处理器10以及存储器20。图3仅示出了无人机的部分组件，但是应理解的是，并不要求实施所有示出的组件，可以替代的实施更多或者更少的组件。
所述存储器20在一些实施例中可以是所述无人机的内部存储单元，例如无人机的硬盘或内存。所述存储器20在另一些实施例中也可以是所述无人机的外部存储设备，例如所述无人机上配备的插接式硬盘，智能存储卡(Smart Media Card,SMC)，安全数字(SecureDigital,SD)卡，闪存卡(Flash Card)等。进一步地，所述存储器20还可以既包括所述无人机的内部存储单元也包括外部存储设备。所述存储器20用于存储安装于所述无人机的应用软件及各类数据。所述存储器20还可以用于暂时地存储已经输出或者将要输出的数据。在一实施例中，存储器20上存储有基于树干特征的无人机定位程序30，该基于树干特征的无人机定位程序30可被处理器10所执行，从而实现本申请中基于树干特征的无人机定位方法。
所述处理器10在一些实施例中可以是一中央处理器(Central Processing Unit,CPU)，微处理器或其他芯片，用于运行所述存储器20中存储的程序代码或处理数据，例如执行所述基于树干特征的无人机定位方法等。
在一实施例中，当处理器10执行所述存储器20中基于树干特征的无人机定位程序30时实现以下步骤：
获取无人机的惯性测量单元的测量数据，根据所述测量数据获取所述无人机的定位参数在预设时段内各个时刻分别与惯性测量单元参数的第一对应关系，其中，所述定位参数包括无人机的位置、速度和姿态，所述预设时段为图像序列中n个连续图像帧对应的时间段，所述图像序列通过所述无人机的机载相机获取；
根据所述第一对应关系获取所述惯性测量单元在所述预设时段内的数据残差与所述惯性测量单元参数的第二对应关系；
根据所述图像序列获取树干特征，根据所述树干特征获取所述预设时段内的树干特征投影残差与所述惯性测量单元参数的第三对应关系；
根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取所述预设时段内各个图像帧分别对应的目标惯性测量单元参数，并根据所述目标惯性测量单元参数对所述无人机进行定位。
其中，所述惯性测量单元包括加速度计和陀螺仪，所述测量数据包括加速度计测量值和陀螺仪测量值，所述惯性测量单元参数包括加速度计偏置、加速度计噪声、陀螺仪偏置和陀螺仪噪声；所述定位参数与惯性测量单元参数的第一对应关系通过第一预设公式表示，所述第一预设公式为：
Figure BDA0002558040190000231
Figure BDA0002558040190000232
Figure BDA0002558040190000233
其中，l+1时刻为l时刻的后一时刻，Δt为l+1时刻和l时刻之间的间隔，
Figure BDA0002558040190000234
分别为l,l+1时刻无人机的位置，
Figure BDA0002558040190000235
为三维数组，
Figure BDA0002558040190000236
分别为l,l+1时刻无人机的速度，
Figure BDA0002558040190000237
分别为l,l+1时刻无人机的姿态的四元数表示，gw为世界坐标系中的重力加速度，
Figure BDA0002558040190000238
为τ时刻的无人机的姿态的旋转矩阵表示，
Figure BDA0002558040190000239
为τ时刻的加速度计测量值，
Figure BDA00025580401900002310
为τ时刻的加速度计偏置，na为加速度计噪声，s为虚拟积分符号，
Figure BDA00025580401900002311
为τ时刻的陀螺仪测量值，wm为三维数组，
Figure BDA00025580401900002312
为为τ时刻的陀螺仪偏置，ng为陀螺仪噪声，
Figure BDA00025580401900002313
为无人机τ时刻相对于l时刻的姿态变化。
其中，所述数据残差与所述惯性测量单元参数的第二对应关系通过第二预设公式被表示，所述第二预设公式为：
Figure BDA00025580401900002314
Figure BDA00025580401900002315
Figure BDA00025580401900002316
Figure BDA00025580401900002317
Figure BDA00025580401900002318
Figure BDA00025580401900002319
其中，ei-1,i为所述图像序列中第i-1帧和第i帧之间的所述测量数据的数据残差，
Figure BDA00025580401900002320
δba T,δbg T分别为无人机的位置残差、姿态对应的旋转角残差、速度残差、加速度计偏置残差、陀螺仪偏置残差，
Figure BDA00025580401900002321
为第i帧对应的无人机姿态，
Figure BDA0002558040190000241
为第j帧对应的无人机位置，
Figure BDA0002558040190000242
为第i帧对应的无人机速度，
Figure BDA0002558040190000243
为第j帧相对于第i帧的无人机位置变化，
Figure BDA0002558040190000244
为第j帧相对于第i帧的无人机姿态变化，
Figure BDA0002558040190000245
为第j帧对应的无人机姿态，
Figure BDA0002558040190000246
为第j帧相对于第i帧的无人机速度变化，
Figure BDA0002558040190000247
为第j帧对应的加速度计偏置，
Figure BDA0002558040190000248
为第j帧对应的陀螺仪偏置，第i帧为第j帧的前一帧。
其中，所述根据所述图像序列获取树干特征包括：
对于每一个图像帧对，获取所述图像帧对对应的光流，根据所述光流获取所述图像帧对对应的特征点对，其中，所述图像帧对包括第一图像帧和与所述第一图像帧相邻的第二图像帧，每个特征点对包括所述第一图像帧上的第一特征点和所述第二图像帧上与所述第一特征点对应的第二特征点；
根据各个图像帧对分别对应的所述特征点对获取所述树干特征。
其中，所述根据各个图像帧对分别对应的所述特征点对获取所述树干特征包括：
对于新获取的每一帧图像帧，检测所述图像帧上的目标树干矩形区域，获取所述目标树干矩形区域中包括的目标特征点的目标识别信息，根据所述目标识别信息以及已存储的树干矩形区域的特征点识别信息确定所述目标树干矩形区域是否为新的树干矩形区域；
如是，则建立所述目标树干矩形区域对应的树干特征；
其中，每个特征点对中包括的所述第一特征点和所述第二特征点的识别信息相同。
其中，所述树干特征投影残差与所述惯性测量单元参数的第三对应关系通过第三预设公式被表示，所述第三预设公式为：
Figure BDA0002558040190000249
其中，ec,k,m为第k帧中树干特征mc的树干区域中的点云m对应的所述树干特征投影残差，
Figure BDA00025580401900002410
为点云m在世界坐标系中的坐标
Figure BDA00025580401900002411
到树干特征mc表面的距离。
其中，所述根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取目标惯性测量单元参数包括：
求解预设的优化函数，获取使得所述优化函数达到最小值的惯性测量单元参数序列，其中所述关系测量单元参数序列中包括所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数；
所述优化函数为：
Figure BDA0002558040190000251
其中，ep为先验残差，Vp -1为ep的权重，ef,k为第k帧中的特征点f对应的重投影残差，F为第k帧中的特征点的个数，Qf,k -1为ef,k的权重，ek-1,k为k-1,k两帧间的所述数据残差，Rk-1,k -1为ek-1,k的权重，ec,k,m为第k帧中树干特征mc区域中点云m对应的所述树干特征投影残差，Uc,k,m -1为ec,k,m的权重，PCk为第k帧中
Figure BDA0002558040190000252
的树干区域的点云，v为第k帧中树干特征的个数，n为所述预设时段内的视频帧的数量。
其中，所述根据所述目标惯性测量单元参数对所述无人机进行定位包括：
根据所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数以及所述第一对应关系获取所述预设时段内各个图像帧分别对应的所述定位参数。
实施例三
本发明还提供一种存储介质，其中，存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如上所述的基于树干特征的无人机定位方法的步骤。
最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (7)

1.一种基于树干特征的无人机定位方法，其特征在于，所述方法包括：
获取无人机的惯性测量单元的测量数据，根据所述测量数据获取所述无人机的定位参数在预设时段内各个时刻分别与惯性测量单元参数的第一对应关系，其中，所述定位参数包括无人机的位置、速度和姿态，所述预设时段为图像序列中n个连续图像帧对应的时间段，所述图像序列通过所述无人机的机载相机获取；
根据所述第一对应关系获取所述惯性测量单元在所述预设时段内的数据残差与所述惯性测量单元参数的第二对应关系；
根据所述图像序列获取树干特征，根据所述树干特征获取所述预设时段内的树干特征投影残差与所述惯性测量单元参数的第三对应关系；
根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取所述预设时段内各个图像帧分别对应的目标惯性测量单元参数，并根据所述目标惯性测量单元参数对所述无人机进行定位；
所述根据所述图像序列获取树干特征包括：
对于每一个图像帧对，获取所述图像帧对对应的光流，根据所述光流获取所述图像帧对对应的特征点对，其中，所述图像帧对包括第一图像帧和与所述第一图像帧相邻的第二图像帧，每个特征点对包括所述第一图像帧上的第一特征点和所述第二图像帧上与所述第一特征点对应的第二特征点；
根据各个图像帧对分别对应的所述特征点对获取所述树干特征；
所述根据各个图像帧对分别对应的所述特征点对获取所述树干特征包括：
对于新获取的每一帧图像帧，检测所述图像帧上的目标树干矩形区域，获取所述目标树干矩形区域中包括的目标特征点的目标识别信息，根据所述目标识别信息以及已存储的树干矩形区域的特征点识别信息确定所述目标树干矩形区域是否为新的树干矩形区域；
如是，则建立所述目标树干矩形区域对应的树干特征；
其中，每个特征点对中包括的所述第一特征点和所述第二特征点的识别信息相同；
所述树干特征投影残差与所述惯性测量单元参数的第三对应关系通过第三预设公式被表示，所述第三预设公式为：
Figure FDA0003412119220000021
ec,k,m为第k帧中树干
Figure FDA0003412119220000022
的树干区域中的点云m对应的所述树干特征投影残差，
Figure FDA0003412119220000023
的物理意义为点云m在世界坐标系中的坐标
Figure FDA0003412119220000024
到树干圆柱模型
Figure FDA0003412119220000025
表面的距离，
Figure FDA0003412119220000026
为第k帧对应的无人机姿态，
Figure FDA0003412119220000027
为第k帧对应的无人机的位置，n0为树干
Figure FDA0003412119220000028
的树干区域中距离世界坐标系的远点最近的点云的单位法向量，a为树干
Figure FDA0003412119220000029
的旋转轴，ρ为树干圆柱模型上的点到世界坐标系原点的距离，
Figure FDA00034121192200000210
为该点的单位法向量在X-Y平面的投影与X轴正向的夹角，θ∈[0,π]为该点的单位法向量与Z轴正向的夹角，
Figure FDA00034121192200000211
α为圆柱旋转轴与nθ之间的夹角，κ为圆柱模型的曲率半径。
2.根据权利要求1所述的基于树干特征的无人机定位方法，其特征在于，所述惯性测量单元包括加速度计和陀螺仪，所述测量数据包括加速度计测量值和陀螺仪测量值，所述惯性测量单元参数包括加速度计偏置、加速度计噪声、陀螺仪偏置和陀螺仪噪声；所述定位参数与惯性测量单元参数的第一对应关系通过第一预设公式表示，所述第一预设公式为：
Figure FDA0003412119220000031
Figure FDA0003412119220000032
Figure FDA0003412119220000033
其中，l+1时刻为l时刻的后一时刻，Δt为l+1时刻和l时刻之间的间隔，
Figure FDA0003412119220000034
分别为l,l+1时刻无人机的位置，
Figure FDA0003412119220000035
为三维数组，
Figure FDA0003412119220000036
分别为l,l+1时刻无人机的速度，
Figure FDA0003412119220000037
分别为l,l+1时刻无人机的姿态的四元数表示，gw为世界坐标系中的重力加速度，
Figure FDA0003412119220000038
为τ时刻的无人机的姿态的旋转矩阵表示，
Figure FDA0003412119220000039
为τ时刻的加速度计测量值，
Figure FDA00034121192200000310
为τ时刻的加速度计偏置，na为加速度计噪声，s为虚拟积分符号，
Figure FDA00034121192200000311
为τ时刻的陀螺仪测量值，wm为三维数组，
Figure FDA00034121192200000312
为为τ时刻的陀螺仪偏置，ng为陀螺仪噪声，
Figure FDA00034121192200000313
为无人机τ时刻相对于l时刻的姿态变化。
3.根据权利要求2所述的基于树干特征的无人机定位方法，其特征在于，所述数据残差与所述惯性测量单元参数的第二对应关系通过第二预设公式被表示，所述第二预设公式为：
Figure FDA00034121192200000314
Figure FDA00034121192200000315
Figure FDA00034121192200000316
Figure FDA00034121192200000317
Figure FDA00034121192200000318
Figure FDA00034121192200000319
其中，ei-1,i为所述图像序列中第i-1帧和第i帧之间的所述测量数据的数据残差，
Figure FDA00034121192200000320
δba T,δbg T分别为无人机的位置残差、姿态对应的旋转角残差、速度残差、加速度计偏置残差、陀螺仪偏置残差，
Figure FDA00034121192200000321
为第i帧对应的无人机姿态，
Figure FDA00034121192200000322
为第j帧对应的无人机位置，
Figure FDA00034121192200000323
为第i帧对应的无人机速度，
Figure FDA00034121192200000324
为第j帧相对于第i帧的无人机位置变化，
Figure FDA00034121192200000325
为第j帧相对于第i帧的无人机姿态变化，
Figure FDA0003412119220000041
为第j帧对应的无人机姿态，
Figure FDA0003412119220000042
为第j帧相对于第i帧的无人机速度变化，
Figure FDA0003412119220000043
为第j帧对应的加速度计偏置，
Figure FDA0003412119220000044
为第j帧对应的陀螺仪偏置，第i帧为第j帧的前一帧。
4.根据权利要求1所述的基于树干特征的无人机定位方法，其特征在于，所述根据所述第一对应关系、所述第二对应关系以及所述第三对应关系获取目标惯性测量单元参数包括：
求解预设的优化函数，获取使得所述优化函数达到最小值的惯性测量单元参数序列，其中所述惯性测量单元参数序列中包括所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数；
所述优化函数为：
Figure FDA0003412119220000045
其中，ep为先验残差，Vp -1为ep的权重，ef,k为第k帧中的特征点f对应的重投影残差，F为第k帧中的特征点的个数，Qf,k -1为ef,k的权重，ek-1,k为k-1,k两帧间的所述数据残差，Rk-1,k -1为ek-1,k的权重，ec,k,m为第k帧中树干特征mc区域中点云m对应的所述树干特征投影残差，Uc,k,m -1为ec,k,m的权重，PCk为第k帧中
Figure FDA0003412119220000046
的树干区域的点云，v为第k帧中树干特征的个数，n为所述预设时段内的视频帧的数量。
5.根据权利要求1所述的基于树干特征的无人机定位方法，其特征在于，所述根据所述目标惯性测量单元参数对所述无人机进行定位包括：
根据所述预设时段内各个图像帧分别对应的所述目标惯性测量单元参数以及所述第一对应关系获取所述预设时段内各个图像帧分别对应的所述定位参数。
6.一种无人机，其特征在于，所述无人机包括：处理器、与处理器通信连接的存储介质，所述存储介质适于存储多条指令，所述处理器适于调用所述存储介质中的指令，以执行实现上述权利要求1-5任一项所述的基于树干特征的无人机定位方法的步骤。
7.一种存储介质，其特征在于，所述存储介质存储有一个或者多个程序，所述一个或者多个程序可被一个或者多个处理器执行，以实现如权利要求1-5任一项所述的基于树干特征的无人机定位方法的步骤。
CN202010597975.2A 2020-06-28 2020-06-28 一种基于树干特征的无人机定位方法、无人机及存储介质 Active CN111811501B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010597975.2A CN111811501B (zh) 2020-06-28 2020-06-28 一种基于树干特征的无人机定位方法、无人机及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010597975.2A CN111811501B (zh) 2020-06-28 2020-06-28 一种基于树干特征的无人机定位方法、无人机及存储介质

Publications (2)

Publication Number Publication Date
CN111811501A CN111811501A (zh) 2020-10-23
CN111811501B true CN111811501B (zh) 2022-03-08

Family

ID=72855631

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010597975.2A Active CN111811501B (zh) 2020-06-28 2020-06-28 一种基于树干特征的无人机定位方法、无人机及存储介质

Country Status (1)

Country Link
CN (1) CN111811501B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113066103B (zh) * 2021-03-18 2023-02-21 鹏城实验室 一种相机帧间运动确定方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102006062061A1 (de) * 2006-12-29 2008-07-03 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. Vorrichtung, Verfahren und Computerprogramm zum Bestimmen einer Position basierend auf einem Kamerabild von einer Kamera
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN109558879A (zh) * 2017-09-22 2019-04-02 华为技术有限公司 一种基于点线特征的视觉slam方法和装置
CN109631894A (zh) * 2018-12-11 2019-04-16 智灵飞（北京）科技有限公司 一种基于滑动窗口的单目视觉惯性紧耦合方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9031809B1 (en) * 2010-07-14 2015-05-12 Sri International Method and apparatus for generating three-dimensional pose using multi-modal sensor fusion
US8676498B2 (en) * 2010-09-24 2014-03-18 Honeywell International Inc. Camera and inertial measurement unit integration with navigation data feedback for feature tracking
WO2015143615A1 (zh) * 2014-03-24 2015-10-01 深圳市大疆创新科技有限公司 飞行器状态实时修正的方法和装置
US10267924B2 (en) * 2017-01-04 2019-04-23 Qualcomm Incorporated Systems and methods for using a sliding window of global positioning epochs in visual-inertial odometry
SG11201908974XA (en) * 2017-03-29 2019-10-30 Agency Science Tech & Res Real time robust localization via visual inertial odometry
US10884433B2 (en) * 2017-08-28 2021-01-05 Nec Corporation Aerial drone utilizing pose estimation
CN107747941B (zh) * 2017-09-29 2020-05-15 歌尔股份有限公司 一种双目视觉定位方法、装置及系统
US10726264B2 (en) * 2018-06-25 2020-07-28 Microsoft Technology Licensing, Llc Object-based localization
CN109974693B (zh) * 2019-01-31 2020-12-11 中国科学院深圳先进技术研究院 无人机定位方法、装置、计算机设备及存储介质

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
DE102006062061A1 (de) * 2006-12-29 2008-07-03 Fraunhofer-Gesellschaft zur Förderung der angewandten Forschung e.V. Vorrichtung, Verfahren und Computerprogramm zum Bestimmen einer Position basierend auf einem Kamerabild von einer Kamera
CN109558879A (zh) * 2017-09-22 2019-04-02 华为技术有限公司 一种基于点线特征的视觉slam方法和装置
CN107869989A (zh) * 2017-11-06 2018-04-03 东北大学 一种基于视觉惯导信息融合的定位方法及系统
CN109631894A (zh) * 2018-12-11 2019-04-16 智灵飞（北京）科技有限公司 一种基于滑动窗口的单目视觉惯性紧耦合方法
CN109993113A (zh) * 2019-03-29 2019-07-09 东北大学 一种基于rgb-d和imu信息融合的位姿估计方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
Multi-Unmanned Aerial Vehicle (UAV) Cooperative Fault Detection Employing Differential Global Positioning (DGPS), Inertial and Vision Sensors;Guillermo Heredia等;《Sensors》;20090924;第9卷(第9期);第7566-7579页 *
坐式垂直起降无人机的一种姿态解算算法的设计;刘东辉等;《河北科技大学学报》;20160115;第37卷(第1期);第47-51页 *
多旋翼无人机光流/惯性组合导航技术研究;邓一民;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;20170315(第3期);C031-909 *

Also Published As

Publication number Publication date
CN111811501A (zh) 2020-10-23

Similar Documents

Publication Publication Date Title
CN111811506B (zh) 视觉/惯性里程计组合导航方法、电子设备及存储介质
CN110146909B (zh) 一种定位数据处理方法
US9243916B2 (en) Observability-constrained vision-aided inertial navigation
Panahandeh et al. Vision-aided inertial navigation based on ground plane feature detection
CN112734852B (zh) 一种机器人建图方法、装置及计算设备
JP5987823B2 (ja) 画像センサおよび運動または位置センサから生じたデータを融合するための方法およびシステム
CN112837352B (zh) 基于图像的数据处理方法、装置及设备、汽车、存储介质
US20220051031A1 (en) Moving object tracking method and apparatus
US20180150974A1 (en) Image processing apparatus, method, and program
CN111583369A (zh) 一种基于面线角点特征提取的激光slam方法
CN110047108B (zh) 无人机位姿确定方法、装置、计算机设备及存储介质
CN110726406A (zh) 一种改进的非线性优化单目惯导slam的方法
CN112815939B (zh) 移动机器人的位姿估计方法及计算机可读存储介质
CN111598946B (zh) 一种物体位姿测量方法、装置及存储介质
CN112116651B (zh) 一种基于无人机单目视觉的地面目标定位方法和系统
CN112183171A (zh) 一种基于视觉信标建立信标地图方法、装置
Yu et al. An improved hector SLAM algorithm based on information fusion for mobile robot
CN112200869A (zh) 一种基于点线特征的机器人全局最优视觉定位方法及装置
CN115371665A (zh) 一种基于深度相机和惯性融合的移动机器人定位方法
CN116642482A (zh) 基于固态激光雷达和惯性导航的定位方法、设备和介质
CN111811501B (zh) 一种基于树干特征的无人机定位方法、无人机及存储介质
CN108827287B (zh) 一种复杂环境下的鲁棒视觉slam系统
CN109785428A (zh) 一种基于多态约束卡尔曼滤波的手持式三维重建方法
CN115235454B (zh) 行人运动约束的视觉惯性融合定位与建图方法和装置
CN113538699A (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
GR01 Patent grant
GR01 Patent grant