CN113790711B - 一种无人机低空飞行位姿无控多视测量方法及存储介质 - Google Patents

一种无人机低空飞行位姿无控多视测量方法及存储介质 Download PDF

Info

Publication number
CN113790711B
CN113790711B CN202111060978.3A CN202111060978A CN113790711B CN 113790711 B CN113790711 B CN 113790711B CN 202111060978 A CN202111060978 A CN 202111060978A CN 113790711 B CN113790711 B CN 113790711B
Authority
CN
China
Prior art keywords
target point
camera
matrix
representing
orientation
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
CN202111060978.3A
Other languages
English (en)
Other versions
CN113790711A (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.)
Tongji University
Original Assignee
Tongji University
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 Tongji University filed Critical Tongji University
Priority to CN202111060978.3A priority Critical patent/CN113790711B/zh
Publication of CN113790711A publication Critical patent/CN113790711A/zh
Application granted granted Critical
Publication of CN113790711B publication Critical patent/CN113790711B/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
    • G01C11/00—Photogrammetry or videogrammetry, e.g. stereogrammetry; Photographic surveying
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C1/00—Measuring angles
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Multimedia (AREA)
  • Image Analysis (AREA)

Abstract

本发明涉及一种无人机低空飞行位姿无控多视测量方法，该方法包括以下步骤：步骤S1：基于相机标定获取相机的内方位参数；同时采用五点相对定向解析算法求解本质矩阵，获取相机的外方位参数初值；步骤S2：采用相对定向参数优化算法进行位姿参数估算，包括通过共面条件方程确定的误差方程和约束条件方程，采用间接平差方法求解未知参数，进行位姿参数估算；步骤S3：采用绝对定向算法将模型坐标系转换为自定义局部物方坐标系；步骤S4：无人机飞行参数解算。与现有技术相比，本发明可应用于无控制点的位姿测量场景，适用性更强，且测量精度达到毫米级。

Description

一种无人机低空飞行位姿无控多视测量方法及存储介质
技术领域
本发明涉及无人机位姿测量领域，尤其是涉及一种无人机低空飞行位姿无控多视测量方法及存储介质。
背景技术
无人飞行器在空中飞行时的位置和飞行姿态是表现其航行状况及飞行控制的重要参数，对于无人飞行器系统的研发、控制飞行姿态、实时规划和调整航迹、事故分析等具有非常重要的意义。在采用视频测量方式获取无人飞行器的位姿等空中飞行参数时面临的困难是空中背景内缺乏可用的控制点。当联测的高速相机公共视野中没有足够的控制点位时，如何估计相机位姿并以此计算目标的高精度三维坐标是一个难题。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种无需控制点、精度高的无人机低空飞行位姿无控多视测量方法及存储介质。
本发明的目的可以通过以下技术方案来实现：
根据本发明的第一方面，一种无人机低空飞行位姿无控多视测量方法，该方法包括以下步骤：
步骤S1：基于相机标定获取相机的内方位参数；同时采用五点相对定向解析算法求解本质矩阵，获取相机的外方位参数初值；
步骤S2：采用相对定向参数优化算法进行位姿参数估算，包括通过共面条件方程确定的误差方程和约束条件方程，采用间接平差方法求解未知参数，进行位姿参数估算；
步骤S3：采用绝对定向算法将模型坐标系转换为自定义局部物方坐标系；
步骤S4：进行无人机飞行参数解算。
优选地，所述步骤S1包括以下步骤：
步骤S11：基于相机标定估算相机的内方位参数；
步骤S12：获取每对同名点对应的本质矩阵约束方程：
Figure BDA0003256545880000021
其中，Pr与Pl分别表示某一物方点的左相机像空间坐标和右相机像空间坐标，qr和ql分别表示某一物方点的左影像坐标和右影像坐标；E为本质矩阵，F为基础矩阵；
步骤S13：采用五点相对定向解析算法求解本质矩阵约束方程，获得本质矩阵E中的各个参数，从而确定相机外方位参数的初值。
优选地，所述步骤S12中的本质矩阵E表达式为：
Figure BDA0003256545880000022
其中，R为相机三个外方位角元素
Figure BDA0003256545880000023
组成的旋转矩阵，(Tx Ty Tz)为相机的三个外方位线元素；
所述基础矩阵F表达式为：
Figure BDA0003256545880000024
其中，E为本质矩阵，Mr与Ml分别为左、右相机内方位元素所组成的矩阵，其表达式为
Figure BDA0003256545880000025
其中cx、cy为主点坐标，fx、fy为相机焦距。
优选地，所述步骤S2中的相对定向参数优化算法为连续像对相对定向参数优化算法，具体指以左影像为基准，求出右影像相对于左影像的外方位元素的过程。
优选地，所述步骤S2的未知参数包括：旋转矩阵R中的九个方向余弦ai,bi,ci,i＝1,2,3，以及三个基线分量(Bx,By,Bz)。
优选地，所述步骤S2具体包括以下步骤：
步骤S21：以左影像为基准，确定共面条件方程：
Figure BDA0003256545880000031
其中，(x0，y0，f1)和(x'0，y'0，f2)分别为左、右相机的内方位元素，ai,bi,ci(i＝1,2,3)为由旋转角
Figure BDA0003256545880000032
计算获得的九个方向余弦，(Bx,By,Bz)表示平方和为定值的三个基线分量，满足
Figure BDA0003256545880000033
(X1,Y1,Z1)、(X2,Y2,Z2)分别为左、右影像的物方坐标，(x1,y1,z1)、(x2,y2,z2)分别为左、右影像的像方坐标，R为旋转矩阵，满足RRT＝RTR＝I；
步骤S22：由共面条件方程确定误差方程：
v＝Ax-l (4)
满足，
xT＝[dBx dBy dBz da1 da2 da3 db1 db2 db3 dc1 dc2 dc3] (5)
l＝-F0＝BzX2Y1+ByX1Z2+BxY2Z1-BxY1Z2-BzX1Y2-ByX2Z1 (6)
Figure BDA0003256545880000034
其中，系数矩阵A中的各个元素分别为：
Figure BDA0003256545880000035
步骤S23：获取约束条件方程，该方程包含一个基线约束条件方程和六个旋转矩阵元素约束方程，其表达式为：
Figure BDA0003256545880000041
其中，ai,bi,ci,i＝1,2,3为由旋转角
Figure BDA0003256545880000042
计算获得的九个方向余弦，(Bx,By,Bz)表示平方和为定值的三个基线分量；
整理式(8)为法方程形式：
Cx-W＝0 (9)
其中，
Figure BDA0003256545880000043
Figure BDA0003256545880000044
通过间接平差方法解算12个未知参数的最优估值，其函数模型如下：
Figure BDA0003256545880000045
Nbb＝ATA，U＝ATl (11)
法方程表示为：
Figure BDA0003256545880000046
则待估计值求解公式为：
Figure BDA0003256545880000047
Figure BDA0003256545880000048
其中，KS为拉格朗日乘子，X0为近似值，x为改正数；
步骤S24：求得相对定向算法的参数之后，通过点投影系数法或基于共线方程的前方交会算法来求解每帧影像中所有目标点在模型坐标系下的三维坐标。
优选地，所述步骤S24中的点投影系数法的计算过程具体为：
步骤S241：基于式(13)，计算左、右影像的物方坐标(X1,Y1,Z1)、(X2,Y2,Z2)：
Figure BDA0003256545880000051
其中，(x1,y1,z1)、(x2,y2,z2)分别为左、右影像的像方坐标；(x0，y0，f1)和(x'0，y'0，f2)分别为左、右相机的内方位元素；R1、R2分别为左、右相机的旋转矩阵；
步骤S242：分别计算左、右相机的点投影系数N1、N2：
Figure BDA0003256545880000052
其中，(Bx,By,Bz)为三个基线分量，(X1,Y1,Z1)、(X2,Y2,Z2)分别为左、右影像的物方坐标；
步骤S243：计算目标点p在模型坐标系下的点位坐标(Xp,Yp,Zp)：
Figure BDA0003256545880000053
优选地，所述步骤S3为：通过参数坐标转换模型将模型坐标系转换至自定义局部物方坐标系下，所述参数坐标转换模型表达式为：
Figure BDA0003256545880000054
其中，H表示姿态矩阵，(X,Y,Z)表示目标点在自定义局部物方坐标系下的三维坐标，(Xp,Yp,Zp)表示目标点p在模型坐标系下的三维坐标，(ΔX,ΔY,ΔZ)表示目标在X、Y和Z方向的偏移量，M为坐标旋转矩阵，λ表示尺度参数；
计算求得目标点在自定义局部物方坐标系下的三维坐标(X,Y,Z)。
优选地，所述步骤S4具体包括以下步骤：
步骤S41：位移解算；
定义目标点在初始时刻的位移值为0mm，则该目标点在n时刻的X，Y和Z方向的位移值为：
Figure BDA0003256545880000061
其中，
Figure BDA0003256545880000062
和
Figure BDA0003256545880000063
分别表示目标点在n时刻X、Y和Z方向的位移值；X0、Y0和Z0分别表示目标点在初始时刻X、Y和Z方向的坐标值；Xn、Yn和Zn分别表示目标点在n时刻X、Y和Z方向的坐标值；
步骤S42：速度解算；
定义目标点在n时刻的速度为目标点在n-1时刻和n+1时刻间的平均速度，表达式为：
Figure BDA0003256545880000064
其中，
Figure BDA0003256545880000065
和
Figure BDA0003256545880000066
表示目标点在n时刻X，Y和Z方向的速度，Xn+1、Yn+1和Zn+1表示目标点在n+1时刻X、Y和Z方向三维的空间坐标，Xn-1、Yn-1和Zn-1表示目标点在n-1时刻X，Y和Z方向三维的空间坐标；ΔT表示相邻时刻的时间间隔；
步骤S43：加速度解算；
定义目标点在n时刻的加速度为目标点在n-1时刻和n+1时刻间的平均加速度，表达式为：
Figure BDA0003256545880000067
其中，
Figure BDA0003256545880000068
和
Figure BDA0003256545880000069
表示目标点在时刻n的加速度，
Figure BDA00032565458800000610
和
Figure BDA00032565458800000611
表示目标点在n+1时刻X，Y和Z方向的速度；
Figure BDA00032565458800000612
和
Figure BDA00032565458800000613
表示目标点在n-1时刻X、Y和Z方向的速度；ΔT表示相邻影像的时间间隔；
步骤S44：姿态解算；
通过目标点时序坐标变换求取姿态矩阵H：
Figure BDA00032565458800000614
其中，Xn+1、Yn+1和Zn+1表示目标点在n+1时刻X、Y和Z方向的空间坐标，Xn、Yn和Zn表示目标点在n刻X、Y和Z方向的空间坐标,ΔXn+1、ΔYn+1和ΔZn+1表示n+1时刻目标在X、Y和Z方向的偏移量；
通过坐标旋转，获得旋转矩阵M：
Figure BDA0003256545880000071
求解后续状态相对于初始状态空间姿态变化的外方位角元素：
Figure BDA0003256545880000072
根据本发明的第二方面，提供了一种计算机可读存储介质，其上存储有计算机程序，所述程序被处理器执行时实现上述的方法。
与现有技术相比，本发明具有以下优点：
1)本发明解决了无控制点情况下的无人机低空飞行位姿视频测量难题。首先通过本质矩阵解析方法获取相机在模型坐标系下的外方位元素，为后续的相对定向参数优化算法提供了位姿参数初值；然后通过绝对定向算法将模型坐标系转换至自定义的局部物方坐标系，求解无人机目标在局部物方坐标系下的三维空间坐标，进而解析获得飞行的速度、加速度和姿态角；
2)本发明能够在无控条件下的实际场景中在不增加无人机自重的情况下实现无人飞行轨迹和姿态的快速矫正；
3)本发明针对空中无人机点位测量结果的高程精度达到毫米级，平面精度为厘米级，总定位精度约为10cm～20cm。
附图说明
图1为本发明的一种无人机低空飞行位姿无控多视测量方法的流程图；
图2为共面条件示意图；
图3为X方向坐标变换示意图；
图4为Y方向坐标变换示意图；
图5为Z方向坐标变换示意图；
图6为实验对象和飞行区域示意图；
图7为高速相机摆放示意图；
图8为无人机实验控制场的控制点分布与编号示意图；
图9为X方向位移测量结果；
图10为Y方向位移测量结果；
图11为Z方向位移测量结果；
图12为X方向速度测量结果；
图13为Y方向速度测量结果；
图14为Z方向速度测量结果；
图15为X方向加速度图；
图16为Y方向加速度图；
图17为Z方向加速度图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明的一部分实施例，而不是全部实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动的前提下所获得的所有其他实施例，都应属于本发明保护的范围。
对于高速视频测量来说，准确的几何方位元素是保证三维重建精度的关键。常规方法是在联测高速相机的公共视野设置一定数量的控制点位，通过这些具有三维坐标的控制点及其在影像上对应像点的坐标反算出相机的位置与姿态。而在一些特定的测量场景中存在缺乏足够控制点的困难，为此，本专利提出了一种用于无控制点的高速相机位姿估计方法，利用相对定向算法解决相机位姿估计问题。首先通过本质矩阵解析方法获取相机在模型坐标系下的外方位元素，为后续的相对定向算法提供了位姿参数初值。然后通过绝对定向算法能够将模型坐标系转换至自定义的局部物方坐标系，进而求解目标点在局部物方坐标系下的三维空间坐标，完成目标三维重建任务。
下面结合具体实施例详细介绍本发明的方法。
1、实验场景
本实验的实验对象为无人机，试验的飞行高度约为40m。如图6所示，为了测试高速相机无控测量精度，无人机在40m×40m的区域反复飞行，以垂直高度5m的间隔来回飞行8次(单程)，即分别在40m、35m、30m、25m、20m、15m、10m和5m的高度在指定的测量区域中横向飞行。
2、高速视频测量方案
在摄影测量中，需要每两台相机组成一个立体测量组合进行测量。本实验采用两台400万像素的高速相机同步拍摄无人机整个运动过程。在实验过程中，高速相机应保持稳定不动。高速相机配备20mm或者35mm的定焦镜头。
如图7所示，在实际的测试实验中，高速相机将以中心对称、交向摄影的方式拍摄目标物。鉴于同步控制线的长度约束，两台高速相机的基线长度设为7m。两台高速相机应水平距离目标区域约40m处，两台相机的俯仰角约22.5°。
在摄影测量三维计算过程中，由于高速相机在实验过程中保持不变，因而无人机在测量区域中每一时刻的飞行位置即增加一对同名点。其处理步骤如下：1)人工筛选同名点位(无人机或人工标志特征点位)；2)利用标定方法估算相机内方位参数；3)利用本质矩阵求解相机的外方位参数；4)对无人机飞行的未知点位进行三维重建，进而获得其三维运动状态。
如图1所示，该方法具体方法包括以下步骤：
2.1相对定向算法中位姿初值的确定方法
在计算机立体视觉领域，本质矩阵E与基础矩阵F确定了立体相机间的对极(核线)几何关系。本质矩阵E是由立体相机间的旋转和平移参数(外方位参数)所组成，而基础矩阵F中除了上述的外方位参数，还包含了立体相机的内方位参数。本质矩阵条件方程与基础矩阵条件方程分别表示如下：
Figure BDA0003256545880000091
其中：
Figure BDA0003256545880000092
在公式(1)中，Pr与Pl分别表示某一物方点的左相机像空间坐标和右相机像空间坐标，qr和ql分别表示某一物方点的左影像坐标和右影像坐标；R是由相机间三个外方位角元素组成的旋转矩阵；[Tx Ty Tz]则由相机间的三个外方位线元素组成，M是由相机内方位元素所组成的矩阵。
当完成相机标定后，每一对同名点可获得一个本质矩阵约束方程。因此，可将解析立体相机外方位元素问题转换为数学上求解多元多次方程问题。
为了求解本质矩阵中的各个参数，Nister(2004)提出的五点相对定向解析算法能够鲁棒地求解本质矩阵中的各个参数，进而从本质矩阵中恢复出立体相机的外方位角元素与线元素。该算法已经被集成在开源计算机视觉库OpenCV中，能够快速且有效地完成相机定向任务(Kaehler和Bradski，2018)。
在本发明中，该算法被用来求解相机外方位参数的初始值，为后续相对定向参数优化算法提供初始条件。
2.2高速视频测量相对定向-绝对定向解析
相对定向是指恢复或确定同名影像摄影光束的相对关系，即解算立体像对的相对方位元素。本节将以连续像对相对定向方法为例阐述整个定向过程。图2展示了立体模型中严格的相对定向示意图，O1p1与O2p2是一对同名光线，这对同名光线与相机间的基线O1O2共面，即这三个矢量的混合积为0，满足共面方程。若以左影像为基准，共面条件方程可表示为：
Figure BDA0003256545880000101
在公式(3)中，(x0，y0，f1)和(x'0，y'0，f2)分别为两张像片的内方位元素，ai,bi,ci(i＝1,2,3)为九个方向余弦，可由旋转角
Figure BDA0003256545880000102
计算获得，(Bx,By,Bz)为三个基线分量。
连续像对相对定向是以左影像为基准，求出右影像相对于左影像的外方位元素(即相对定向元素)。在解算过程中，将旋转矩阵R中的九个方向余弦(ai,bi,ci)作为未知数，并且将三个基线分量(Bx,By,Bz)作为未知数。因此，相对定向算法共需要求解12个未知数。
然而，由于旋转矩阵R中只有三个独立参数，且三个基线分量中只有两个独立参数，因而可加入7个条件方程。旋转矩阵R为正交矩阵，即RRT＝RTR＝I，可列出6个条件方程(Jue，2008)；三个基线分量的平方和为定值，即
Figure BDA0003256545880000103
同样可列出1个条件方程。综上，连续像对相对定向算法的求解过程如下：
由共面条件方程列出的误差方程式如公式(4)所示。
v＝Ax-l (4)
在公式(4)中，
xT＝[dBx dBy dBz da1 da2 da3 db1 db2 db3 dc1 dc2 dc3] (5)
l＝-F0＝BzX2Y1+ByX1Z2+BxY2Z1-BxY1Z2-BzX1Y2-ByX2Z1 (6)
Figure BDA0003256545880000111
在公式(7)中，误差方程的系数矩阵如下所示：
Figure BDA0003256545880000112
在上述解析模型中，七个约束条件方程包含一个基线约束条件方程和六个旋转矩阵元素约束方程，如公式(8)所示，
Figure BDA0003256545880000113
公式(8)可整理为以下法方程形式：
Cx-W＝0 (9)
在公式(9)中，
Figure BDA0003256545880000114
Figure BDA0003256545880000121
通过附有约束条件的间接平差能够解算未知数最优估值，其函数模型如下：
Figure BDA0003256545880000122
Nbb＝ATA，U＝ATl (11)
法方程公式可表示为：
Figure BDA0003256545880000123
则待估计值求解公式为：
Figure BDA0003256545880000124
Figure BDA0003256545880000125
上述的求解过程是一个迭代优化过程，因此需要较好的参数初始值以达到待估参数快速收敛的目的。相对定向算法中的相机外方位元素初值可通过本质矩阵解析来获取。在求得相对定向参数之后，可通过点投影系数方法或者基于共线方程的前方交会算法来求解每帧影像中所有目标点(跟踪点)在模型坐标系下的三维坐标。点投影系数法的计算过程如下所示：
从公式(13)可知，
Figure BDA0003256545880000126
点投影系数可通过公式(16)获得。
Figure BDA0003256545880000127
进而通过公式(17)计算出目标点在模型坐标系下的点位坐标(Xp,Yp,Zp)。
Figure BDA0003256545880000128
在绝对定向解析过程中，可通过七参数坐标转换方法将模型坐标系转换至自定义坐标系。
Figure BDA0003256545880000131
公式(18)是七参数坐标转换模型，其中(X,Y,Z)表示目标点在自定义坐标系下的三维坐标，(ΔX,ΔY,ΔZ)表示目标在X、Y和Z方向的偏移量，(Xp,Yp,Zp)表示目标点在模型坐标系下的三维坐标，M表示坐标旋转矩阵，λ表示尺度参数。
2.3无人机飞行参数解算
2.3.1位移解算；
定义目标点在初始时刻的位移值为0mm，则该目标点在n时刻的X，Y和Z方向的位移值为：
Figure BDA0003256545880000132
其中，
Figure BDA0003256545880000133
和
Figure BDA0003256545880000134
分别表示目标点在n时刻X、Y和Z方向的位移值；X0、Y0和Z0分别表示目标点在初始时刻X、Y和Z方向的坐标值；Xn、Yn和Zn分别表示目标点在n时刻X、Y和Z方向的坐标值；
2.3.2速度解算；
定义目标点在n时刻的速度为目标点在n-1时刻和n+1时刻间的平均速度，表达式为：
Figure BDA0003256545880000135
其中，
Figure BDA0003256545880000136
和
Figure BDA0003256545880000137
表示目标点在n时刻X，Y和Z方向的速度，Xn+1、Yn+1和Zn+1表示目标点在n+1时刻X、Y和Z方向三维的空间坐标，Xn-1、Yn-1和Zn-1表示目标点在n-1时刻X，Y和Z方向三维的空间坐标；ΔT表示相邻时刻的时间间隔；
2.3.3加速度解算；
定义目标点在n时刻的加速度为目标点在n-1时刻和n+1时刻间的平均加速度，表达式为：
Figure BDA0003256545880000138
其中，
Figure BDA0003256545880000141
和
Figure BDA0003256545880000142
表示目标点在时刻n的加速度，
Figure BDA0003256545880000143
和
Figure BDA0003256545880000144
表示目标点在n+1时刻X，Y和Z方向的速度；
Figure BDA0003256545880000145
和
Figure BDA0003256545880000146
表示目标点在n-1时刻X、Y和Z方向的速度；ΔT表示相邻影像的时间间隔；
2.3.4姿态解算，如图3～5所示；
通过目标点时序坐标变换求取姿态矩阵H：
Figure BDA0003256545880000147
其中，Xn+1、Yn+1和Zn+1表示目标点在n+1时刻X、Y和Z方向的空间坐标，Xn、Yn和Zn表示目标点在n刻X、Y和Z方向的空间坐标,ΔXn+1、ΔYn+1和ΔZn+1表示n+1时刻X、Y和Z方向的偏移量；
通过坐标旋转，获得旋转矩阵M：
Figure BDA0003256545880000148
求解后续状态相对于初始状态空间姿态变化的外方位角元素：
Figure BDA0003256545880000149
3、实验结果与精度
图8为本发明无人机实验控制场的控制点分布与编号示意图，下表1为相机定向结果。
表1相机定向结果
Figure BDA00032565458800001410
图9-图11分别为X,Y,Y方向上的位移测量结果；图12-与14分别为X,Y,Y方向上的速度测量结果；图15-与17分别为X,Y,Y方向上的加速度测量结果。
对于均匀且随机地抽取的50个无人机点位，通过相对定向获取相机在模型坐标系下的姿态，再利用地面上控制点进行空间中三个方向和实际尺度的确定。从实验结果来看，空中无人机点位测量结果的高程精度达到毫米级，平面精度为厘米级，总定位精度约为10cm～20cm。
总结：针对无控制点情况下的无人机低空飞行位姿视频测量难题，本发明提出了一种无人机低空飞行位姿无控多视测量方法，该方法利用相对定向算法解决相机位姿估计问题的方法。首先通过本质矩阵解析方法获取相机在模型坐标系下的外方位元素，为后续的相对定向算法提供了位姿参数初值；然后通过绝对定向算法将模型坐标系转换至自定义的局部物方坐标系，求解无人机目标在局部物方坐标系下的三维空间坐标，进而解析获得飞行的速度、加速度和姿态角。本发明成果在无人机系统的研发等方面具有潜在的应用价值。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，所述描述的模块的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
本发明电子设备包括中央处理单元(CPU)，其可以根据存储在只读存储器(ROM)中的计算机程序指令或者从存储单元加载到随机访问存储器(RAM)中的计算机程序指令，来执行各种适当的动作和处理。在RAM中，还可以存储设备操作所需的各种程序和数据。CPU、ROM以及RAM通过总线彼此相连。输入/输出(I/O)接口也连接至总线。
设备中的多个部件连接至I/O接口，包括：输入单元，例如键盘、鼠标等；输出单元，例如各种类型的显示器、扬声器等；存储单元，例如磁盘、光盘等；以及通信单元，例如网卡、调制解调器、无线通信收发机等。通信单元允许设备通过诸如因特网的计算机网络和/或各种电信网络与其他设备交换信息/数据。
处理单元执行上文所描述的各个方法和处理，例如方法S1～S4。例如，在一些实施例中，方法S1～S4可被实现为计算机软件程序，其被有形地包含于机器可读介质，例如存储单元。在一些实施例中，计算机程序的部分或者全部可以经由ROM和/或通信单元而被载入和/或安装到设备上。当计算机程序加载到RAM并由CPU执行时，可以执行上文描述的方法S1～S4的一个或多个步骤。备选地，在其他实施例中，CPU可以通过其他任何适当的方式(例如，借助于固件)而被配置为执行方法S1～S4。
本文中以上描述的功能可以至少部分地由一个或多个硬件逻辑部件来执行。例如，非限制性地，可以使用的示范类型的硬件逻辑部件包括：场可编程门阵列(FPGA)、专用集成电路(ASIC)、专用标准产品(ASSP)、芯片上系统的系统(SOC)、负载可编程逻辑设备(CPLD)等等。
用于实施本发明的方法的程序代码可以采用一个或多个编程语言的任何组合来编写。这些程序代码可以提供给通用计算机、专用计算机或其他可编程数据处理装置的处理器或控制器，使得程序代码当由处理器或控制器执行时使流程图和/或框图中所规定的功能/操作被实施。程序代码可以完全在机器上执行、部分地在机器上执行，作为独立软件包部分地在机器上执行且部分地在远程机器上执行或完全在远程机器或服务器上执行。
在本发明的上下文中，机器可读介质可以是有形的介质，其可以包含或存储以供指令执行系统、装置或设备使用或与指令执行系统、装置或设备结合地使用的程序。机器可读介质可以是机器可读信号介质或机器可读储存介质。机器可读介质可以包括但不限于电子的、磁性的、光学的、电磁的、红外的、或半导体系统、装置或设备，或者上述内容的任何合适组合。机器可读存储介质的更具体示例会包括基于一个或多个线的电气连接、便携式计算机盘、硬盘、随机存取存储器(RAM)、只读存储器(ROM)、可擦除可编程只读存储器(EPROM或快闪存储器)、光纤、便捷式紧凑盘只读存储器(CD-ROM)、光学储存设备、磁储存设备、或上述内容的任何合适组合。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (7)

1.一种无人机低空飞行位姿无控多视测量方法，其特征在于，该方法包括以下步骤：
步骤S1：基于相机标定获取相机的内方位参数；同时采用五点相对定向解析算法求解本质矩阵，获取相机的外方位参数初值；
步骤S2：采用相对定向参数优化算法进行位姿参数估算，包括通过共面条件方程确定的误差方程和约束条件方程，采用间接平差方法求解未知参数，进行位姿参数估算；
步骤S3：采用绝对定向算法将模型坐标系转换为自定义局部物方坐标系；
步骤S4：进行无人机飞行参数解算；
所述步骤S2具体包括以下步骤：
步骤S21：以左影像为基准，确定共面条件方程：
Figure FDA0003791553270000011
其中，(x0，y0，f1)和(x'0，y'0，f2)分别为左、右相机的内方位元素，ai,bi,ci(i＝1,2,3)为由旋转角
Figure FDA0003791553270000012
计算获得的九个方向余弦，(Bx,By,Bz)表示平方和为定值的三个基线分量，满足
Figure FDA0003791553270000013
(X1,Y1,Z1)、(X2,Y2,Z2)分别为左、右影像的物方坐标，(x1,y1,z1)、(x2,y2,z2)分别为左、右影像的像方坐标，R为旋转矩阵，满足RRT＝RTR＝I；
步骤S22：由共面条件方程确定误差方程：
v＝Ax-l (4)
满足，
xT＝[dBx dBy dBz da1 da2 da3 db1 db2 db3 dc1 dc2 dc3] (5)
l＝-F0＝BzX2Y1+ByX1Z2+BxY2Z1-BxY1Z2-BzX1Y2-ByX2Z1 (6)
Figure FDA0003791553270000021
其中，系数矩阵A中的各个元素分别为：
Figure FDA0003791553270000022
步骤S23：获取约束条件方程，该方程包含一个基线约束条件方程和六个旋转矩阵元素约束方程，其表达式为：
Figure FDA0003791553270000023
其中，ai,bi,ci,i＝1,2,3为由旋转角
Figure FDA0003791553270000024
计算获得的九个方向余弦，(Bx,By,Bz)表示平方和为定值的三个基线分量；
整理式(8)为法方程形式：
Cx-W＝0 (9)
其中，
Figure FDA0003791553270000025
Figure FDA0003791553270000031
通过间接平差方法解算12个未知参数的最优估值，其函数模型如下：
Figure FDA0003791553270000032
Nbb＝ATA，U＝ATl (11)
法方程表示为：
Figure FDA0003791553270000033
则待估计值求解公式为：
Figure FDA0003791553270000034
Figure FDA0003791553270000035
其中，KS为拉格朗日乘子，X0为近似值，x为改正数；
步骤S24：求得相对定向算法的参数之后，通过点投影系数法或基于共线方程的前方交会算法来求解每帧影像中所有目标点在模型坐标系下的三维坐标；
所述步骤S24中的点投影系数法的计算过程具体为：
步骤S241：基于式(13)，计算左、右影像的物方坐标(X1,Y1,Z1)、(X2,Y2,Z2)：
Figure FDA0003791553270000036
其中，(x1,y1,z1)、(x2,y2,z2)分别为左、右影像的像方坐标；(x0，y0，f1)和(x'0，y'0，f2)分别为左、右相机的内方位元素；R1、R2分别为左、右相机的旋转矩阵；
步骤S242：分别计算左、右相机的点投影系数N1、N2：
Figure FDA0003791553270000037
其中，(Bx,By,Bz)为三个基线分量，(X1,Y1,Z1)、(X2,Y2,Z2)分别为左、右影像的物方坐标；
步骤S243：计算目标点p在模型坐标系下的点位坐标(Xp,Yp,Zp)：
Figure FDA0003791553270000041
所述步骤S3为：通过参数坐标转换模型将模型坐标系转换至自定义局部物方坐标系下，所述参数坐标转换模型表达式为：
Figure FDA0003791553270000042
其中，H表示姿态矩阵，(X,Y,Z)表示目标点在自定义局部物方坐标系下的三维坐标，(Xp,Yp,Zp)表示目标点p在模型坐标系下的三维坐标，(ΔX,ΔY,ΔZ)表示目标在X、Y和Z方向的偏移量，M为坐标旋转矩阵，λ表示尺度参数；
计算求得目标点在自定义局部物方坐标系下的三维坐标(X,Y,Z)。
2.根据权利要求1所述的一种无人机低空飞行位姿无控多视测量方法，其特征在于，所述步骤S1包括以下步骤：
步骤S11：基于相机标定估算相机的内方位参数；
步骤S12：获取每对同名点对应的本质矩阵约束方程：
Figure FDA0003791553270000043
其中，Pr与Pl分别表示某一物方点的左相机像空间坐标和右相机像空间坐标，qr和ql分别表示某一物方点的左影像坐标和右影像坐标；E为本质矩阵，F为基础矩阵；
步骤S13：采用五点相对定向解析算法求解本质矩阵约束方程，获得本质矩阵E中的各个参数，从而确定相机外方位参数的初值。
3.根据权利要求2所述的一种无人机低空飞行位姿无控多视测量方法，其特征在于，所述步骤S12中的本质矩阵E表达式为：
Figure FDA0003791553270000044
其中，R为相机三个外方位角元素
Figure FDA0003791553270000045
组成的旋转矩阵，(Tx Ty Tz)为相机的三个外方位线元素；
所述基础矩阵F表达式为：
Figure FDA0003791553270000046
其中，E为本质矩阵，Mr与Ml分别为左、右相机内方位元素所组成的矩阵，其表达式为
Figure FDA0003791553270000051
其中cx、cy为主点坐标，fx、fy为相机焦距。
4.根据权利要求1所述的一种无人机低空飞行位姿无控多视测量方法，其特征在于，所述步骤S2中的相对定向参数优化算法为连续像对相对定向参数优化算法，具体指以左影像为基准，求出右影像相对于左影像的外方位元素的过程。
5.根据权利要求1所述的一种无人机低空飞行位姿无控多视测量方法，其特征在于，所述步骤S2的未知参数包括：旋转矩阵R中的九个方向余弦ai,bi,ci,i＝1,2,3，以及三个基线分量(Bx,By,Bz)。
6.根据权利要求1所述的一种无人机低空飞行位姿无控多视测量方法，其特征在于，所述步骤S4具体包括以下步骤：
步骤S41：位移解算；
定义目标点在初始时刻的位移值为0mm，则该目标点在n时刻的X，Y和Z方向的位移值为：
Figure FDA0003791553270000052
其中，
Figure FDA0003791553270000053
和
Figure FDA0003791553270000054
分别表示目标点在n时刻X、Y和Z方向的位移值；X0、Y0和Z0分别表示目标点在初始时刻X、Y和Z方向的坐标值；Xn、Yn和Zn分别表示目标点在n时刻X、Y和Z方向的坐标值；
步骤S42：速度解算；
定义目标点在n时刻的速度为目标点在n-1时刻和n+1时刻间的平均速度，表达式为：
Figure FDA0003791553270000055
其中，
Figure FDA0003791553270000056
和
Figure FDA0003791553270000057
表示目标点在n时刻X，Y和Z方向的速度，Xn+1、Yn+1和Zn+1表示目标点在n+1时刻X、Y和Z方向三维的空间坐标，Xn-1、Yn-1和Zn-1表示目标点在n-1时刻X，Y和Z方向三维的空间坐标；ΔT表示相邻时刻的时间间隔；
步骤S43：加速度解算；
定义目标点在n时刻的加速度为目标点在n-1时刻和n+1时刻间的平均加速度，表达式为：
Figure FDA0003791553270000061
其中，
Figure FDA0003791553270000062
和
Figure FDA0003791553270000063
表示目标点在时刻n的加速度，
Figure FDA0003791553270000064
和
Figure FDA0003791553270000065
表示目标点在n+1时刻X，Y和Z方向的速度；
Figure FDA0003791553270000066
和
Figure FDA0003791553270000067
表示目标点在n-1时刻X、Y和Z方向的速度；ΔT表示相邻影像的时间间隔；
步骤S44：姿态解算；
通过目标点时序坐标变换求取姿态矩阵H：
Figure FDA0003791553270000068
其中，Xn+1、Yn+1和Zn+1表示目标点在n+1时刻X、Y和Z方向的空间坐标，Xn、Yn和Zn表示目标点在n刻X、Y和Z方向的空间坐标,ΔXn+1、ΔYn+1和ΔZn+1表示n+1时刻目标在X、Y和Z方向的偏移量；
通过坐标旋转，获得旋转矩阵M：
Figure FDA0003791553270000069
求解后续状态相对于初始状态空间姿态变化的外方位角元素：
Figure FDA00037915532700000610
7.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，所述程序被处理器执行时实现如权利要求1～6中任一项所述的方法。
CN202111060978.3A 2021-09-10 2021-09-10 一种无人机低空飞行位姿无控多视测量方法及存储介质 Active CN113790711B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111060978.3A CN113790711B (zh) 2021-09-10 2021-09-10 一种无人机低空飞行位姿无控多视测量方法及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111060978.3A CN113790711B (zh) 2021-09-10 2021-09-10 一种无人机低空飞行位姿无控多视测量方法及存储介质

Publications (2)

Publication Number Publication Date
CN113790711A CN113790711A (zh) 2021-12-14
CN113790711B true CN113790711B (zh) 2022-10-25

Family

ID=79182948

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111060978.3A Active CN113790711B (zh) 2021-09-10 2021-09-10 一种无人机低空飞行位姿无控多视测量方法及存储介质

Country Status (1)

Country Link
CN (1) CN113790711B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117724141B (zh) * 2024-02-18 2024-04-16 中国民航大学 一种面向飞行器的北斗正交基线测向的角度优化方法

Family Cites Families (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103759716B (zh) * 2014-01-14 2016-08-17 清华大学 基于机械臂末端单目视觉的动态目标位置和姿态测量方法
CN108759788B (zh) * 2018-03-19 2020-11-24 深圳飞马机器人科技有限公司 无人机影像定位定姿方法及无人机
CN110285827B (zh) * 2019-04-28 2023-04-07 武汉大学 一种距离约束的摄影测量高精度目标定位方法

Also Published As

Publication number Publication date
CN113790711A (zh) 2021-12-14

Similar Documents

Publication Publication Date Title
CN106529495B (zh) 一种飞行器的障碍物检测方法和装置
CN112444242B (zh) 一种位姿优化方法及装置
CN112037260B (zh) 一种跟踪目标的位置估计方法、装置及无人飞行器
JP5992184B2 (ja) 画像データ処理装置、画像データ処理方法および画像データ処理用のプログラム
US8437501B1 (en) Using image and laser constraints to obtain consistent and improved pose estimates in vehicle pose databases
CN112184824B (zh) 一种相机外参标定方法、装置
CN111415387A (zh) 相机位姿确定方法、装置、电子设备及存储介质
CN110084785B (zh) 一种基于航拍图像的输电线垂弧测量方法及系统
CN113850126A (zh) 一种基于无人机的目标检测和三维定位方法和系统
US20220180560A1 (en) Camera calibration apparatus, camera calibration method, and nontransitory computer readable medium storing program
CN111915685B (zh) 一种变焦摄像机标定方法
WO2023134745A1 (en) Methods and systems for correcting positioning deviation
Liu et al. A novel adjustment model for mosaicking low-overlap sweeping images
CN113168716A (zh) 对象解算、绕点飞行方法及设备
CN113790711B (zh) 一种无人机低空飞行位姿无控多视测量方法及存储介质
CN114926316A (zh) 距离测量方法、装置、电子设备及存储介质
CN115222905A (zh) 基于视觉特征的空地多机器人地图融合方法
CN113436267B (zh) 视觉惯导标定方法、装置、计算机设备和存储介质
CN114777768A (zh) 一种卫星拒止环境高精度定位方法、系统及电子设备
CN114529585A (zh) 基于深度视觉和惯性测量的移动设备自主定位方法
CN114049401A (zh) 双目相机标定方法、装置、设备及介质
CN111553954B (zh) 一种基于直接法单目slam的在线光度标定方法
CN114419259A (zh) 一种基于物理模型成像仿真的视觉定位方法及系统
CN114608522A (zh) 一种基于视觉的障碍物识别与测距方法
CN112652018B (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