CN108122255A - 一种基于梯形与圆形组合地标的无人机位姿估计方法 - Google Patents
一种基于梯形与圆形组合地标的无人机位姿估计方法 Download PDFInfo
- Publication number
- CN108122255A CN108122255A CN201711388109.7A CN201711388109A CN108122255A CN 108122255 A CN108122255 A CN 108122255A CN 201711388109 A CN201711388109 A CN 201711388109A CN 108122255 A CN108122255 A CN 108122255A
- Authority
- CN
- China
- Prior art keywords
- mrow
- mtd
- msub
- mtr
- mfrac
- 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.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 54
- 238000003384 imaging method Methods 0.000 claims abstract description 27
- 230000009466 transformation Effects 0.000 claims abstract description 26
- 238000012545 processing Methods 0.000 claims abstract description 8
- 238000001914 filtration Methods 0.000 claims abstract description 7
- 230000000877 morphologic effect Effects 0.000 claims abstract description 7
- 238000006243 chemical reaction Methods 0.000 claims abstract description 3
- 239000011159 matrix material Substances 0.000 claims description 31
- 230000003287 optical effect Effects 0.000 claims description 5
- 238000001514 detection method Methods 0.000 claims description 3
- 238000003708 edge detection Methods 0.000 claims description 2
- 238000012546 transfer Methods 0.000 claims description 2
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000013519 translation Methods 0.000 claims description 2
- 230000017105 transposition Effects 0.000 claims description 2
- 239000000284 extract Substances 0.000 abstract description 2
- 238000000605 extraction Methods 0.000 abstract 1
- 238000010586 diagram Methods 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 4
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 241001347978 Major minor Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 230000010006 flight Effects 0.000 description 1
- 230000010365 information processing Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012544 monitoring process Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/70—Determining position or orientation of objects or cameras
- G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/20—Image enhancement or restoration using local operators
- G06T5/30—Erosion or dilatation, e.g. thinning
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T5/00—Image enhancement or restoration
- G06T5/70—Denoising; Smoothing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/10—Segmentation; Edge detection
- G06T7/13—Edge detection
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20036—Morphological image processing
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/20—Special algorithmic details
- G06T2207/20048—Transform domain processing
- G06T2207/20061—Hough transform
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Image Analysis (AREA)
- Secondary Cells (AREA)
Abstract
一种基于梯形与圆形组合地标的无人机位姿估计方法，属于图像处理技术领域。所述方法如下：步骤1、无人机对地标图案成像二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；步骤2、提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；步骤3、根据步骤2解算出的椭圆参数估计无人机姿态参数；步骤4、建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。本发明针对无人机视觉导航中的位姿精确估计和以及自主着陆等问题，基于梯形和圆形组合的地标图案，根据几何成像特性估计姿态参数，简化共线方程求解模型，进而求解位置参数，计算过程简单，更加适合实际工程应用。
Description
技术领域
本发明属于图像处理技术领域，具体涉及一种基于梯形与圆形组合地标的无人机位姿估计方法。
背景技术
随着无人机及其上搭载的信息处理技术的不断发展和军事战略思想的转变，无人机因具备昼夜可用、机动灵活、使用便捷、成本低、效率比高等特点，已广泛应用于军事侦察、区域监视、地理测绘、智慧交通、电力巡线、农业植保等军民领域。与此同时，无人机也存在对军事要地、机场、大型展会等区域造成重大威胁的可能性，如近期发生的印度无人机入侵我国领土和无人机干扰昆明机场多架航班备降等问题，因此准确、实时获取无人机飞行过程中的姿态和位置，对于无人机的自主控制、智能化处理等实际应用和跟踪定位具有重要意义。
目前无人机系统通常携带获取自身位置的GPS数据，但难以保证定位的精确性，无法精确、实时地获取无人机飞行过程中的高程信息以及姿态参数。尽管目前已有基于双目视觉、模板匹配、特征跟踪颜色分割以及地平线检测等姿态估计方法和算法研究，但仍存在计算复杂、处理速度慢等问题，难以支持实际工程中应用。
发明内容
本发明针对无人机飞行过程中位姿精确自主估计的迫切需求，提供一种基于梯形与圆形组合地标的无人机位姿估计方法，该方法开展基于固定图形地标的无人机位姿参数自主估计方法研究，依据对圆形地标成像结果的椭圆长短轴之比仅与俯仰角有关且长轴倾角仅与滚转角有关的成像特性和直角梯形几何形状简单且满足解算无人机位置参数的最小顶点数目的几何特性，设计了梯形和圆形组合的地标图案。基于该地标图案，首先根据圆形投影到像面上的椭圆特征估计无人机飞行过程中的姿态参数，进而求得旋转变换矩阵带入成像共线方程，避免了正弦、余弦方程求解问题，再将梯形顶点代入无人机对地成像模型解算无人机的位置参数，从而提出基于梯形与圆形组合地标的无人机位姿估计方法。该方法能够在保证估计精度的同时，避免直接求解共线方程时的繁杂计算过程，更加适合实际工程应用。
为实现上述目的，本发明采取的技术方案如下：
一种基于梯形与圆形组合地标的无人机位姿估计方法，所述方法步骤如下：
步骤一：无人机对地成像结果二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；
步骤二：提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；
步骤三：根据步骤二计算出的椭圆参数求解无人机姿态参数；
步骤四：利用步骤三的无人机姿态参数建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。
本发明相对于现有技术的有益效果是：
(1)本发明针对无人机视觉导航中的位姿精确估计以及自主着陆等问题，基于梯形和圆形组合的地标图案，根据地标图案几何成像特性估计出无人机姿态参数，简化共线方程求解模型，进而求解无人机位置参数。与现有位姿估计方法相比，该方法具有较高估计精度的同时，计算过程简单、时效性强，更加适合实际工程应用。
(2)本发明针对无人机飞行过程中姿态的高时效、高精度自主估计问题，设计了圆形和梯形组合的简单图形地标，通过提取圆形和梯形地标经无人机成像投影至像面上的轮廓，根据圆形地标几何成像特性，利用像面椭圆长短轴比、长轴倾角等几何特征反演无人机姿态信息，同时引入误差补偿提高解算精度，从而实现无人机姿态角的快速、精确求解。
(3)本发明针对无人机飞行过程中位置的高实时、精确估计问题，将求出的姿态信息带入无人机对地成像模型，避免成像模型中存在正弦和余弦函数项，将无人机位置信息反演问题转化为简单的线性方程求解问题，然后利用梯形顶点的地面与像面坐标得到多组线性方程，最后基于最小二乘法计算位置参数减小误差，从而实现无人机位置信息的快速、精确求解。
(4)本发明能够基于简单地标中的图形特征，实现无人机姿态与位置参数的高实时、高准确性的估计，采用的算法复杂度低、计算效率高，更加适合工程应用。
附图说明
图1为无人机位姿估计方法流程图；
图2为无人机对地标图案成像结果的二值图像；
图3为基于形态学滤波的孤立噪声去除结果图；
图4为不同水平视场角下误差拟合曲线图；
图5为误差曲线拟合结果示意图；
图6为俯仰角估计方法的几何示意图；
图7为偏航角引起梯形底边倾斜示意图；
图8为偏航角估计方法的几何示意图一；
图9为偏航角估计方法的几何示意图二；
图10为地标平面示意图；
图11为地面坐标系示意图；
图12为相机坐标系示意图；
图13为地面坐标系与相机坐标转换示意图；
图14为相机坐标系到像面坐标系中心投影模型图。
具体实施方式
下面结合附图对本发明的技术方案作进一步的说明，但并不局限于此，凡是对本发明技术方案进行修改或者等同替换，而不脱离本发明技术方案的精神和范围，均应涵盖在本发明的保护范围中。
具体实施方式一：本实施方式记载的是一种基于梯形与圆形组合地标的无人机位姿估计方法，可应用于无人机自主姿态控制、自动着陆等方面，由梯形和圆形组合成地标图案，其估算流程图如图1，所述方法步骤如下：
步骤一：无人机对地标图案成像二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；
步骤二：提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；
步骤三：根据步骤二计算出的椭圆参数求解无人机姿态参数；
步骤四：利用步骤三的无人机姿态参数建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。
具体实施方式二：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤一的具体步骤如下：
(1)进行图像二值化
针对无人机对地成像结果设定阈值，图像灰度值大于或者等于该阈值的像素可视为目标图形，其灰度值用1表示，图像灰度值小于该阈值的像素点被判定为背景区域，其灰度值用0表示；
其中，f表示输入图像，Th为二值化阈值；x，y表示像素点坐标；成像结果二值化图像如图2所示；
(2)进行形态学滤波
对经步骤(1)处理后的二值化图像先后进行开、闭运算去除孤立噪声，处理结果如图3所示。
具体实施方式三：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤二的具体步骤如下：
(1)基于Canny算子对步骤一形态学滤波后的二值化图像进行边缘检测，提取出梯形和椭圆轮廓；
(2)设椭圆轮廓坐标点(x1,y1),(x2,y2),…(xn,yn)，利用最小二乘法可得法方程：
其中，A，B，C，D，E为椭圆轮廓曲线方程待定系数，为法方程系数矩阵的矩阵转置，[·]T为矩阵转置；
法方程系数矩阵：
利用法方程求得待定系数，则椭圆轮廓曲线方程为：
Ax2+Bxy+Cy2+Dx+Ey+1＝0
进而得椭圆几何中心坐标(xeo,yeo)：
长轴倾斜角度
长轴a：
短轴b：
具体实施方式四：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤三的具体步骤如下：
(1)滚转角估计
理想情况下，初步假设仅滚转角会引起椭圆长轴倾斜，即滚转角在数值上即等于椭圆长轴倾角，将图像旋转使长轴水平，则此时椭圆中心相对于图像中心O(xo,yo)的水平、竖直方向偏移量Δx',Δy'为：
其中，为长轴倾斜角度；
实际上，圆形地标成像结果往往不是严格的椭圆，导致真实滚转角与像面椭圆倾角之间存在一定差异，该误差与地标图案成像水平方向视场角近似成正比关系，为提高位姿估计精度，引入误差补偿项，将滚转角γ表示为：
式中，第二项即为误差补偿项，水平方向视场角其中dx为探测器像元水平尺寸，F为镜头焦距；
当无人机内部参数确定时，可通过仿真实验拟合得到，具体方法为：仿真一系列不同位姿状态下的成像结果，按照前面步骤一到步骤三的具体步骤(1)分别计算各图像的值和对应的进而拟合出二者关系曲线，如图4、图5所示；
(2)俯仰角估计
首先根据已经求得的滚转角，修正椭圆中心相对于水平和竖直方向的偏移量Δx,Δy：
然后分别修正地标位置水平和竖直方向视场角：
其中，dy为探测器像元竖直方向尺寸，F为镜头焦距；
由图6中给出的地面原点Og的成像光线和光轴位置以及地面坐标系与像极坐标系相对位置空间几何关系可知：椭圆短轴和长轴之比等于α1的正弦值，即：
进而可得俯仰角：α＝α1+ωy
(3)偏航角
基于霍夫变换进行直线检测，并根据梯形的几何特性提取出所需的四条线段L1L2L3和L4的参数信息如图7所示，进而计算梯形顶点坐标为P1(xp1,yp1)、P2(xp2,yp2)、P3(xp3,yp3)和P4(xp4,yp4)，则xg轴在像面上倾角βxg等于向量和向量倾角的平均值：
其中(xp1,yp1)、(xp2,yp2)、(xp3,yp3)和(xp4,yp4)为四边形顶点P1、P2、P3和P4的坐标；
则由偏航角引起的xg轴倾斜角为β1：β1＝-γ-βxg
如图8，9所示，根据空间几何关系知偏航角β：
具体实施方式五：具体实施方式一所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，步骤四的具体步骤如下：
(1)确定地面坐标系和相机坐标系变换矩阵：
本发明地标图案如图10所示，采用直角梯形与圆形的组合，如图11所示，以圆心为原点建立地面坐标系(右手坐标系)，Xg轴平行于梯形底边，Zg轴垂直于地平面向上，设相机与成像平台固连，相机质心与平台质心重合，相机坐标系与无人机平台坐标系重合，建立如图12所示相机坐标系Oc-XcYcZc，Zc轴平行于光轴方向，无人机对地拍摄时，地面坐标系与相机坐标系位置关系如图13所示，定义α、β和γ分别为俯仰角、偏航角和滚转角，设地面原点在相机坐标系下坐标Tgc＝(TXc,TYc,TZc)，则从地面坐标系变换到相机坐标系的过程为：首先将坐标系绕Z轴旋转β(从Z轴正半轴向原点以逆时针方向为正)，得变换矩阵R1：R1＝RZ(β)，其中，Rz(·)表示绕Z轴旋转变换函数，
若旋转角度为ω，则
再绕X轴旋转90-α，得变换矩阵R2：R2＝RX(90°-α)，其中，RX(·)表示绕X轴旋转变换函数，
若旋转角度为ω，则
然后绕Z轴旋转滚转角γ，变换矩阵为R3：R3＝Rz(γ)
最后平移到Tgc处，变换矩阵为STgc：
则从地面坐标系到相机坐标系的变换关系为：
[Xc,Yc,Zc,1]T＝STgcR3R2R1[Xg,Yg,Zg,1]T＝Mgc[Xg,Yg,Zg,1]T
其中，Mgc表示地面坐标系到相机坐标系的变换矩阵，(Xg,Yg,Zg)为点在地面坐标系的坐标。
通常情况下，已知参数为无人机在地面坐标系下的位置坐标Tcg，此时应先求出从相机坐标系到地面坐标系的变换矩阵Mcg：
Mcg＝STcgRZ(-β)RX(α-90°)RZ(-γ)，
其中，STgc表示从相机坐标系到地面坐标系平移变换矩阵。
将求得的姿态角带入上式求出旋转矩阵Rgc：
Rgc＝RZ(γ)RX(90°-α)RZ(β)
则地面坐标系下点Pg(Xpg,Ypg,Zpg)经旋转变换后坐标(X'pg,Yp'g,Z'pg)为：
[X'pg,Y'pg,Z'pg]T＝Rgc[Xpg,Ypg,Zpg]T
(2)确定图像物理坐标系与图像像素坐标系转化关系：
如图14所示，摄像机坐标系中的物点Qc(Xc,Yc,Zc)在图像物理坐标系中的像点Qu(Xu,Yu,Zu)坐标为：
其中，F为相机焦距；
将图像物理坐标系转化为图像像素坐标系：
其中，(x0，y0)是光轴与图像数字平面的交点的坐标，dx和dy分别为一个像素在Xu与Yu方向上的物理尺寸；
(3)利用直线信息计算标识点坐标，反解位置参数
若Pg对应的像素坐标为P(xp，yp)，设地面坐标系原点在相机坐标系下位置坐标为Tgc＝(TXc,TYc,TZc)，则有方程组：
将梯形地标四个顶点的地面坐标以及对应的像面坐标带入方程组，再次利用最小二乘法求出(TXc,TYc,TZc)的值，从地面坐标系到相机坐标系变换矩阵为：
故从相机坐标系到地面坐标系变换矩阵为：
则Tcg即为所求的无人机位置参数，即无人机在地面坐标系下位置坐标。
Claims (5)
1.一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：所述方法步骤如下：
步骤一：无人机对地成像结果二值化处理分割出地标图案，基于形态学滤波去除孤立噪声；
步骤二：提取地标边缘，利用霍夫变换提取地标中梯形轮廓直线信息，利用最小二乘法拟合椭圆方程并计算椭圆参数；
步骤三：根据步骤二计算出的椭圆参数求解无人机姿态参数；
步骤四：利用步骤三的无人机姿态参数建立无人机对地成像模型，利用梯形轮廓直线信息确定梯形四个顶点坐标估计位置参数。
2.根据权利要求1所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：步骤一的具体步骤如下：
(1)进行图像二值化
针对无人机对地成像结果设定阈值，图像灰度值大于或者等于该阈值的像素可视为目标图形，其灰度值用1表示，图像灰度值小于该阈值的像素点被判定为背景区域，其灰度值用0表示，其灰度值用1表示，灰度值小于阈值的像素点的灰度值用0表示，表示图形之外的背景区域；
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mn>0</mn>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mo><</mo>
<mi>T</mi>
<mi>h</mi>
<mo>;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mn>1</mn>
<mo>,</mo>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>,</mo>
<mi>y</mi>
<mo>)</mo>
</mrow>
<mo>&GreaterEqual;</mo>
<mi>T</mi>
<mi>h</mi>
<mo>;</mo>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
其中，f表示输入图像，Th为二值化阈值；x，y表示像素点坐标；
(2)进行形态学滤波
对经步骤(1)处理后的二值化图像先后进行开、闭运算去除孤立噪声。
3.根据权利要求1所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：步骤二的具体步骤如下：
(1)基于Canny算子对步骤一形态学滤波后的二值化图像进行边缘检测，提取出梯形和椭圆轮廓；
(2)设椭圆轮廓坐标点(x1,y1),(x2,y2),…(xn,yn)，利用最小二乘法可得法方程：
<mrow>
<msubsup>
<mi>G</mi>
<mi>E</mi>
<mi>T</mi>
</msubsup>
<msub>
<mi>G</mi>
<mi>E</mi>
</msub>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mi>A</mi>
<mo>,</mo>
<mi>B</mi>
<mo>,</mo>
<mi>C</mi>
<mo>,</mo>
<mi>D</mi>
<mo>,</mo>
<mi>E</mi>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
<mo>=</mo>
<msubsup>
<mi>G</mi>
<mi>E</mi>
<mi>T</mi>
</msubsup>
<msup>
<mrow>
<mo>&lsqb;</mo>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mo>-</mo>
<mn>1</mn>
<mo>,</mo>
<mo>...</mo>
<mo>,</mo>
<mo>-</mo>
<mn>1</mn>
<mo>&rsqb;</mo>
</mrow>
<mi>T</mi>
</msup>
</mrow>
其中，A，B，C，D，E为椭圆轮廓曲线方程待定系数，为法方程系数矩阵的矩阵转置，[·]T为矩阵转置；
法方程系数矩阵：
<mrow>
<msub>
<mi>G</mi>
<mi>E</mi>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msubsup>
<mi>x</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
</mtd>
<mtd>
<mrow>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msubsup>
<mi>y</mi>
<mn>1</mn>
<mn>2</mn>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>x</mi>
<mn>1</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>y</mi>
<mn>1</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>x</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
</mtd>
<mtd>
<mrow>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
<msub>
<mi>y</mi>
<mn>2</mn>
</msub>
</mrow>
</mtd>
<mtd>
<msubsup>
<mi>y</mi>
<mn>2</mn>
<mn>2</mn>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>x</mi>
<mn>2</mn>
</msub>
</mtd>
<mtd>
<msub>
<mi>y</mi>
<mn>2</mn>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mo>...</mo>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
<mtd>
<mrow></mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<msubsup>
<mi>x</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mtd>
<mtd>
<mrow>
<msub>
<mi>x</mi>
<mi>n</mi>
</msub>
<msub>
<mi>y</mi>
<mi>n</mi>
</msub>
</mrow>
</mtd>
<mtd>
<msubsup>
<mi>y</mi>
<mi>n</mi>
<mn>2</mn>
</msubsup>
</mtd>
<mtd>
<msub>
<mi>x</mi>
<mi>n</mi>
</msub>
</mtd>
<mtd>
<msub>
<mi>y</mi>
<mi>n</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
利用法方程求得待定系数，则椭圆轮廓曲线方程为：
Ax2+Bxy+Cy2+Dx+Ey+1＝0
进而得椭圆几何中心坐标(xeo,yeo)：
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mi>B</mi>
<mi>E</mi>
<mo>-</mo>
<mn>2</mn>
<mi>C</mi>
<mi>D</mi>
</mrow>
<mrow>
<mn>4</mn>
<mi>A</mi>
<mi>C</mi>
<mo>-</mo>
<msup>
<mi>B</mi>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
</mrow>
<mrow>
<msub>
<mi>y</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<mi>B</mi>
<mi>D</mi>
<mo>-</mo>
<mn>2</mn>
<mi>A</mi>
<mi>E</mi>
</mrow>
<mrow>
<mn>4</mn>
<mi>A</mi>
<mi>C</mi>
<mo>-</mo>
<msup>
<mi>B</mi>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
</mrow>
长轴倾斜角度
长轴a：
<mrow>
<msup>
<mi>a</mi>
<mn>2</mn>
</msup>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>Ax</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>Cy</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msub>
<mi>Bx</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<msub>
<mi>y</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>A</mi>
<mo>+</mo>
<mi>C</mi>
<mo>-</mo>
<msqrt>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<mi>A</mi>
<mo>-</mo>
<mi>C</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mi>B</mi>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mrow>
</mfrac>
</mrow>
短轴b：
<mrow>
<msup>
<mi>b</mi>
<mn>2</mn>
</msup>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msubsup>
<mi>Ax</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msubsup>
<mi>Cy</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
<mn>2</mn>
</msubsup>
<mo>+</mo>
<msub>
<mi>Bx</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<msub>
<mi>y</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>A</mi>
<mo>+</mo>
<mi>C</mi>
<mo>+</mo>
<msqrt>
<mrow>
<msup>
<mrow>
<mo>(</mo>
<mi>A</mi>
<mo>-</mo>
<mi>C</mi>
<mo>)</mo>
</mrow>
<mn>2</mn>
</msup>
<mo>+</mo>
<msup>
<mi>B</mi>
<mn>2</mn>
</msup>
</mrow>
</msqrt>
</mrow>
</mfrac>
<mo>.</mo>
</mrow>
4.根据权利要求1所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：步骤三的具体步骤如下：
(1)滚转角估计
将图像旋转使长轴水平，则此时椭圆中心相对于图像中心O(xo,yo)的水平、竖直方向偏移量Δx',Δy'为：
其中为长轴倾斜角度；
为提高位姿估计精度，引入误差补偿项，将滚转角γ表示为：
式中，第二项即为误差补偿项，水平方向视场角其中dx为探测器像元水平尺寸，F为镜头焦距；
(2)俯仰角估计
首先根据已经求得的滚转角，修正椭圆中心相对于水平和竖直方向的偏移量Δx,Δy：
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>x</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>y</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>o</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
然后分别修正地标位置水平和竖直方向视场角：
<mrow>
<msub>
<mi>&omega;</mi>
<mi>x</mi>
</msub>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<msub>
<mi>&Delta;xd</mi>
<mi>x</mi>
</msub>
</mrow>
<mi>F</mi>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>&omega;</mi>
<mi>y</mi>
</msub>
<mo>=</mo>
<mi>a</mi>
<mi>r</mi>
<mi>c</mi>
<mi>t</mi>
<mi>a</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<msub>
<mi>&Delta;yd</mi>
<mi>y</mi>
</msub>
</mrow>
<mi>F</mi>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
其中，dy为探测器像元竖直方向尺寸，F为镜头焦距；
由空间几何关系可知，椭圆短轴和长轴之比等于α1的正弦值，即：
进而可得俯仰角：α＝α1+ωy
<mrow>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>x</mi>
</mtd>
</mtr>
<mtr>
<mtd>
<mi>&Delta;</mi>
<mi>y</mi>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>cos</mi>
<mi>&gamma;</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>x</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>o</mi>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<msub>
<mi>y</mi>
<mrow>
<mi>e</mi>
<mi>o</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
(3)偏航角
基于霍夫变换进行直线检测，并根据梯形的几何特性提取出所需的四条线段L1L2L3和L4的参数信息，进而计算梯形顶点坐标P1(xp1,yp1)、P2(xp2,yp2)、P3(xp3,yp3)和P4(xp4,yp4)，则xg轴在像面上倾角βxg等于向量和向量倾角的平均值：
<mrow>
<msub>
<mi>&beta;</mi>
<mrow>
<mi>x</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>4</mn>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>4</mn>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>2</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>3</mn>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>2</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>3</mn>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
</mrow>
其中(xp1,yp1)、(xp2,yp2)、(xp3,yp3)和(xp4,yp4)为四边形顶点P1、P2、P3和P4的坐标；
则由偏航角引起的xg轴倾斜角为β1：β1＝-γ-βxg
根据空间几何关系知偏航角β：
<mrow>
<msub>
<mi>&beta;</mi>
<mrow>
<mi>x</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>=</mo>
<mfrac>
<mn>1</mn>
<mn>2</mn>
</mfrac>
<mrow>
<mo>&lsqb;</mo>
<mrow>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>4</mn>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>1</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>4</mn>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mi>arctan</mi>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>2</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mrow>
<mi>p</mi>
<mn>3</mn>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>2</mn>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mrow>
<mi>p</mi>
<mn>3</mn>
</mrow>
</msub>
</mrow>
</mfrac>
<mo>)</mo>
</mrow>
</mrow>
<mo>&rsqb;</mo>
</mrow>
<mo>.</mo>
</mrow>
5.根据权利要求1所述的一种基于梯形与圆形组合地标的无人机位姿估计方法，其特征在于：步骤四的具体步骤如下：
(1)确定地面坐标系和相机坐标系变换矩阵：
采用直角梯形与圆形的组合，以圆心为原点建立地面坐标系，Xg轴平行于梯形底边，Zg轴垂直于地平面向上，设相机与成像平台固连，相机质心与平台质心重合，相机坐标系与无人机平台坐标系重合，建立相机坐标系Oc-XcYcZc，Zc轴平行于光轴方向，设地面原点在相机坐标系下坐标Tgc＝(TXc,TYc,TZc)，则从地面坐标系变换到相机坐标系的过程为：首先将坐标系绕Z轴旋转β(从Z轴正半轴向原点以逆时针方向为正)，得变换矩阵R1：R1＝Rz(β)，其中，RZ(·)表示绕Z轴旋转变换函数，
若旋转角度为ω，则
<mrow>
<msub>
<mi>R</mi>
<mi>Z</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&omega;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&omega;</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mi>&omega;</mi>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mi>&omega;</mi>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
再绕X轴旋转90-α，得变换矩阵R2：R2＝RX(90°-α)，其中，RX(·)表示绕X轴旋转变换函数，
若旋转角度为ω，则
<mrow>
<msub>
<mi>R</mi>
<mi>X</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mrow>
<mo>-</mo>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&omega;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
然后绕Z轴旋转滚转角γ，变换矩阵为R3：R3＝RZ(γ)
最后平移到Tgc处，变换矩阵为STgc：
<mrow>
<msub>
<mi>S</mi>
<mrow>
<mi>T</mi>
<mi>g</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>T</mi>
<mrow>
<mi>X</mi>
<mi>c</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<msub>
<mi>T</mi>
<mrow>
<mi>Y</mi>
<mi>c</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
<mtd>
<msub>
<mi>T</mi>
<mrow>
<mi>Z</mi>
<mi>c</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
则从地面坐标系到相机坐标系的变换关系为：
[Xc,Yc,Zc,1]T＝STgcR3R2R1[Xg,Yg,Zg,1]T＝Mgc[Xg,Yg,Zg,1]T
其中，Mgc表示地面坐标系到相机坐标系的变换矩阵，(Xg,Yg,Zg)为点在地面坐标系的坐标；
通常情况下，已知参数为无人机在地面坐标系下的位置坐标Tcg，此时应先求出从相机坐标系到地面坐标系的变换矩阵Mcg：
Mcg＝STcgRZ(-β)RX(α-90°)RZ(-γ)，
其中，STgc表示从相机坐标系到地面坐标系平移变换矩阵；
将求得的姿态角带入上式求出旋转矩阵Rgc：
Rgc＝RZ(γ)RX(90°-α)RZ(β)
则地面坐标系下点Pg(Xpg,Ypg,Zpg)经旋转变换后坐标(X'pg,Y′pg,Z'pg)为：
[X'pg,Y'pg,Z'pg]T＝Rgc[Xpg,Ypg,Zpg]T
(2)确定图像物理坐标系与图像像素坐标系转化关系：
摄像机坐标系中的物点Qc(Xc,Yc,Zc)在图像物理坐标系中的像点Qu(Xu,Yu,Zu)坐标为：
<mrow>
<msub>
<mi>X</mi>
<mi>u</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>FX</mi>
<mi>c</mi>
</msub>
</mrow>
<mrow>
<mo>-</mo>
<msub>
<mi>Z</mi>
<mi>c</mi>
</msub>
</mrow>
</mfrac>
<mo>,</mo>
<msub>
<mi>Y</mi>
<mi>u</mi>
</msub>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>FY</mi>
<mi>c</mi>
</msub>
</mrow>
<mrow>
<mo>-</mo>
<msub>
<mi>Z</mi>
<mi>c</mi>
</msub>
</mrow>
</mfrac>
<mo>,</mo>
</mrow>
其中，F为相机焦距；
将图像物理坐标系转化为图像像素坐标系：
<mrow>
<mi>x</mi>
<mo>=</mo>
<mfrac>
<msub>
<mi>X</mi>
<mi>u</mi>
</msub>
<msub>
<mi>d</mi>
<mi>x</mi>
</msub>
</mfrac>
<mo>+</mo>
<msub>
<mi>x</mi>
<mn>0</mn>
</msub>
<mo>,</mo>
<mi>y</mi>
<mo>=</mo>
<mfrac>
<mi>Y</mi>
<msub>
<mi>d</mi>
<mi>y</mi>
</msub>
</mfrac>
<mo>+</mo>
<msub>
<mi>y</mi>
<mn>0</mn>
</msub>
<mo>,</mo>
</mrow>
其中，(x0，y0)是光轴与图像数字平面的交点的坐标，dx和dy分别为一个像素在Xu与Yu方向上的物理尺寸；
(3)利用直线信息计算标识点坐标，反解位置参数
若Pg对应的像素坐标为P(xp，yp)，设地面坐标系原点在相机坐标系下位置坐标为Tgc＝(TXc,TYc,TZc)，则有方程组：
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mfrac>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mn>0</mn>
</msub>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>p</mi>
</msub>
<mo>)</mo>
<msub>
<mi>d</mi>
<mi>x</mi>
</msub>
</mrow>
<mi>F</mi>
</mfrac>
<mo>=</mo>
<mfrac>
<msub>
<mi>X</mi>
<mrow>
<mi>p</mi>
<mi>c</mi>
</mrow>
</msub>
<msub>
<mi>Z</mi>
<mrow>
<mi>p</mi>
<mi>c</mi>
</mrow>
</msub>
</mfrac>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<msup>
<mi>X</mi>
<mo>&prime;</mo>
</msup>
<mrow>
<mi>p</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>X</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<msup>
<mi>Z</mi>
<mo>&prime;</mo>
</msup>
<mrow>
<mi>p</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>Z</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
</mfrac>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mfrac>
<mrow>
<mo>(</mo>
<msub>
<mi>y</mi>
<mn>0</mn>
</msub>
<mo>-</mo>
<msub>
<mi>y</mi>
<mi>p</mi>
</msub>
<mo>)</mo>
<msub>
<mi>d</mi>
<mi>y</mi>
</msub>
</mrow>
<mi>F</mi>
</mfrac>
<mo>=</mo>
<mfrac>
<msub>
<mi>Y</mi>
<mrow>
<mi>p</mi>
<mi>c</mi>
</mrow>
</msub>
<msub>
<mi>Z</mi>
<mrow>
<mi>p</mi>
<mi>c</mi>
</mrow>
</msub>
</mfrac>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<msup>
<mi>Y</mi>
<mo>&prime;</mo>
</msup>
<mrow>
<mi>p</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>Y</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
<mrow>
<msub>
<msup>
<mi>Z</mi>
<mo>&prime;</mo>
</msup>
<mrow>
<mi>p</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>T</mi>
<mrow>
<mi>Z</mi>
<mi>c</mi>
</mrow>
</msub>
</mrow>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
将梯形地标四个顶点的地面坐标以及对应的像面坐标带入方程组，再次利用最小二乘法求出(TXc,TYc,TZc)的值，从地面坐标系到相机坐标系变换矩阵为：
<mrow>
<msub>
<mi>M</mi>
<mrow>
<mi>g</mi>
<mi>c</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>R</mi>
<mrow>
<mi>g</mi>
<mi>c</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>T</mi>
<mrow>
<mi>g</mi>
<mi>c</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
故从相机坐标系到地面坐标系变换矩阵为：
<mrow>
<msub>
<mi>M</mi>
<mrow>
<mi>c</mi>
<mi>g</mi>
</mrow>
</msub>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<msub>
<mi>R</mi>
<mrow>
<mi>c</mi>
<mi>g</mi>
</mrow>
</msub>
</mtd>
<mtd>
<msub>
<mi>T</mi>
<mrow>
<mi>c</mi>
<mi>g</mi>
</mrow>
</msub>
</mtd>
</mtr>
<mtr>
<mtd>
<mn>0</mn>
</mtd>
<mtd>
<mn>1</mn>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>=</mo>
<msubsup>
<mi>M</mi>
<mrow>
<mi>g</mi>
<mi>c</mi>
</mrow>
<mrow>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
</mrow>
则Tcg即为所求的无人机位置参数，即无人机在地面坐标系下位置坐标。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711388109.7A CN108122255B (zh) | 2017-12-20 | 2017-12-20 | 一种基于梯形与圆形组合地标的无人机位姿估计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201711388109.7A CN108122255B (zh) | 2017-12-20 | 2017-12-20 | 一种基于梯形与圆形组合地标的无人机位姿估计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN108122255A true CN108122255A (zh) | 2018-06-05 |
| CN108122255B CN108122255B (zh) | 2021-10-22 |
Family
ID=62230759
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201711388109.7A Active CN108122255B (zh) | 2017-12-20 | 2017-12-20 | 一种基于梯形与圆形组合地标的无人机位姿估计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN108122255B (zh) |
Cited By (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108898628A (zh) * | 2018-06-21 | 2018-11-27 | 北京纵目安驰智能科技有限公司 | 基于单目的车辆三维目标姿态估计方法、系统、终端和存储介质 |
| CN109241658A (zh) * | 2018-09-27 | 2019-01-18 | 中国电子科技集团公司第五十四研究所 | 基于遥感影像的蝶形卫星天线形态解析方法 |
| CN109445432A (zh) * | 2018-10-31 | 2019-03-08 | 中国科学技术大学 | 基于图像的无人机和地面移动机器人编队定位方法 |
| CN109613923A (zh) * | 2018-11-06 | 2019-04-12 | 武汉华中天经通视科技有限公司 | 一种无人直升机的着舰控制方法 |
| CN111857167A (zh) * | 2020-06-30 | 2020-10-30 | 同济大学 | 一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法 |
| CN113819889A (zh) * | 2021-09-09 | 2021-12-21 | 中国电子科技集团公司第五十四研究所 | 一种基于飞行器旋翼光源检测的相对测距测姿方法 |
| CN116661470A (zh) * | 2023-04-14 | 2023-08-29 | 成都飞机工业（集团）有限责任公司 | 一种基于双目视觉导引着陆的无人机位姿估计方法 |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104180808A (zh) * | 2014-08-05 | 2014-12-03 | 南京航空航天大学 | 一种用于自主空中加油的圆形锥套视觉位姿解算方法 |
| CN105021184A (zh) * | 2015-07-08 | 2015-11-04 | 西安电子科技大学 | 一种用于移动平台下视觉着舰导航的位姿估计系统及方法 |
| CN106326892A (zh) * | 2016-08-01 | 2017-01-11 | 西南科技大学 | 一种旋翼式无人机的视觉着陆位姿估计方法 |
| CN107202982A (zh) * | 2017-05-22 | 2017-09-26 | 徐泽宇 | 一种基于无人机位姿计算的信标布置及图像处理方法 |
| US20170308100A1 (en) * | 2016-04-25 | 2017-10-26 | Uvionix Aerospace Corporation | System and method for automated landing of an unmanned aerial vehicle |
-
2017
- 2017-12-20 CN CN201711388109.7A patent/CN108122255B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN104180808A (zh) * | 2014-08-05 | 2014-12-03 | 南京航空航天大学 | 一种用于自主空中加油的圆形锥套视觉位姿解算方法 |
| CN105021184A (zh) * | 2015-07-08 | 2015-11-04 | 西安电子科技大学 | 一种用于移动平台下视觉着舰导航的位姿估计系统及方法 |
| US20170308100A1 (en) * | 2016-04-25 | 2017-10-26 | Uvionix Aerospace Corporation | System and method for automated landing of an unmanned aerial vehicle |
| CN106326892A (zh) * | 2016-08-01 | 2017-01-11 | 西南科技大学 | 一种旋翼式无人机的视觉着陆位姿估计方法 |
| CN107202982A (zh) * | 2017-05-22 | 2017-09-26 | 徐泽宇 | 一种基于无人机位姿计算的信标布置及图像处理方法 |
Non-Patent Citations (3)
| Title |
|---|
| HUANG LAN ET AL.: "《Research of autonomous vision-based absolute navigation for unmanned aerial vehicle》", 《IEEE XPLORE》 * |
| LALINDRA JAYATILLEKE ET AL.: "《Landmark-Based Localization for Unmanned Aerial Vehicles》", 《IEEE XPLORE》 * |
| 龙古灿: "《无人机视觉着陆引导中的位姿估计问题研究》", 《中国优秀硕士学位论文全文数据库 工程科技辑Ⅱ辑》 * |
Cited By (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN108898628A (zh) * | 2018-06-21 | 2018-11-27 | 北京纵目安驰智能科技有限公司 | 基于单目的车辆三维目标姿态估计方法、系统、终端和存储介质 |
| CN109241658A (zh) * | 2018-09-27 | 2019-01-18 | 中国电子科技集团公司第五十四研究所 | 基于遥感影像的蝶形卫星天线形态解析方法 |
| CN109445432A (zh) * | 2018-10-31 | 2019-03-08 | 中国科学技术大学 | 基于图像的无人机和地面移动机器人编队定位方法 |
| CN109613923A (zh) * | 2018-11-06 | 2019-04-12 | 武汉华中天经通视科技有限公司 | 一种无人直升机的着舰控制方法 |
| CN111857167A (zh) * | 2020-06-30 | 2020-10-30 | 同济大学 | 一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法 |
| CN111857167B (zh) * | 2020-06-30 | 2023-08-29 | 同济大学 | 一种基于单线激光数据椭圆拟合的引水涵洞内无人机定位方法 |
| CN113819889A (zh) * | 2021-09-09 | 2021-12-21 | 中国电子科技集团公司第五十四研究所 | 一种基于飞行器旋翼光源检测的相对测距测姿方法 |
| CN113819889B (zh) * | 2021-09-09 | 2024-01-26 | 中国电子科技集团公司第五十四研究所 | 一种基于飞行器旋翼光源检测的相对测距测姿方法 |
| CN116661470A (zh) * | 2023-04-14 | 2023-08-29 | 成都飞机工业（集团）有限责任公司 | 一种基于双目视觉导引着陆的无人机位姿估计方法 |
| CN116661470B (zh) * | 2023-04-14 | 2024-08-13 | 成都飞机工业（集团）有限责任公司 | 一种基于双目视觉导引着陆的无人机位姿估计方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN108122255B (zh) | 2021-10-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108122255B (zh) | 一种基于梯形与圆形组合地标的无人机位姿估计方法 | |
| CN110966991B (zh) | 一种无控制点下的单幅无人机影像定位方法 | |
| Alonso et al. | Accurate global localization using visual odometry and digital maps on urban environments | |
| CN105021184A (zh) | 一种用于移动平台下视觉着舰导航的位姿估计系统及方法 | |
| CN104484668A (zh) | 一种无人机多重叠遥感影像的建筑物轮廓线提取方法 | |
| CN109460046B (zh) | 一种无人机自然地标识别与自主着陆方法 | |
| CN108917753B (zh) | 基于从运动恢复结构的飞行器位置确定方法 | |
| CN109724586B (zh) | 一种融合深度图和点云的航天器相对位姿测量方法 | |
| Dawood et al. | Harris, SIFT and SURF features comparison for vehicle localization based on virtual 3D model and camera | |
| CN109341686A (zh) | 一种基于视觉-惯性紧耦合的飞行器着陆位姿估计方法 | |
| CN105758386A (zh) | 一种激光点云与航空影像集成的建筑物三维建模方法 | |
| CN115423863B (zh) | 相机位姿估计方法、装置及计算机可读存储介质 | |
| CN108225273A (zh) | 一种基于传感器先验知识的实时跑道检测方法 | |
| CN109341685B (zh) | 一种基于单应变换的固定翼飞机视觉辅助着陆导航方法 | |
| CN112446915A (zh) | 一种基于图像组的建图方法及装置 | |
| Mostafa et al. | Optical flow based approach for vision aided inertial navigation using regression trees | |
| CN112577463B (zh) | 姿态参数修正的航天器单目视觉测距方法 | |
| CN109764864A (zh) | 一种基于颜色识别的室内无人机位姿获取方法及系统 | |
| CN109003295B (zh) | 一种无人机航空影像快速匹配方法 | |
| CN102620745A (zh) | 一种机载imu视准轴误差检校方法 | |
| Wang et al. | Automated mosaicking of UAV images based on SFM method | |
| Božić-Štulić et al. | Complete model for automatic object detection and localisation on aerial images using convolutional neural networks | |
| CN116520295A (zh) | 一种基于自动建图的多激光雷达标定方法 | |
| Wang et al. | Fast stitching of DOM based on small UAV | |
| Xu et al. | Uav autonomous landing algorithm based on machine vision |
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 | ||
| CB03 | Change of inventor or designer information | ||
| CB03 | Change of inventor or designer information |
Inventor after: Zhi Xiyang Inventor after: Niu Ruize Inventor after: Jiang Shikai Inventor after: Zhang Wei Inventor after: Guo Weifeng Inventor before: Zhi Xiyang Inventor before: Niu Ruize Inventor before: Zhang Wei Inventor before: Jiang Shikai Inventor before: Guo Weifeng |
|
| GR01 | Patent grant | ||
| GR01 | Patent grant |