(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 202011008679.0
(22)申请日 2020.09.23
(71)申请人 广东海洋大学
地址 524000 广东省湛江市麻章区海大路1
号
(72)发明人 陈亮 彭小红 闫秀英 余应淮
王骥 邓锐 刘桃丽 谢水镔
李登印 谢宝达 叶友强 苏泽宇
(74)专利代理机构 广州润禾知识产权代理事务
所(普通合伙) 44446
代理人 林伟斌
(51)Int.Cl.
G06T 7/73(2017.01)
G06T 7/80(2017.01)
(54)发明名称
(57)摘要
本发明提供了一种提升无人机相机对
AprilTag识别定位精度方法，
包括对相机进行标定；将多个AprilTag空间点信息进行融合；使用 PnP后再增加一次高斯牛顿迭代求解相机姿态；
使用位姿图优化理论对AprilTag之间的变换矩
阵进行优化。本发明还提供了一种无人机定位系
统及方法，定位过程简单易于操作，合理运用了
上述提升精度方法。本发明可在室内gps信号较
弱的情况下实现无人机的高精度定位，且只需要
通过二维码阵列进行定位，定位精确，成本低，性
能可靠、
受环境干扰小。权利要求书3页 说明书11页 附图15页CN 112184812 A 2021.01.05
C N 112184812
A
1.一种提升无人机相机对AprilTag识别定位精度方法，其特征在于，包括以下步骤：
S1、对无人机上的相机进行标定，计算得出相机内参及畸变系数；
S2、在得到相机内参及畸变参数的基础上，相机获取AprilTag图像，然后从得到的AprilTag图像中识别出空间点信息，将多个AprilTag空间点信息进行融合；
S3、在得到多个空间点信息后，采用OpenCV中PnP求解相机在世界坐标系下的姿态，在通过PnP求解姿态后，再增加一次高斯牛顿法迭代优化姿态；
S4、使用位姿图优化理论对AprilTag之间的变换矩阵进行优化。
2.根据权利要求1所述的一种提升无人机相机对AprilTag定位精度方法，其特征在于，所述步骤S1中求解相机内参过程为：
根据针孔相机模型成像原理，一个在相机坐标系下的3D点P(X,Y,Z)，它在像素坐标系投影点P1坐标为(u,v)，二者对应关系有
其中，K为相机内参矩阵，f x为x轴焦距，f y为y轴焦距，c x为x轴平移量，c y为y轴平移量；
对于畸变系数求解可分为径向畸变和切向畸变，求解公式数学模型如下:
x distorted＝x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)
y distorted＝y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
其中(x,y)为P点在归一化平面坐标，x distorted和y distored为畸变后坐标，r为(x,y)到原点距离，k1、k2、k3为径向畸变系数，p1、p2为切向畸变系数。
3.根据权利要求1所述的一种提升无人机相机对AprilTag定位精度方法，其特征在于，步骤S2中融合多个AprilTag空间点信息，具体过程为采用AprilTag的中心与图像中心的欧氏距离为可信度权重，将两个AprilTag空间点信息融合，使用公式：
其中Q为融合后的空间点，q1、q2分别为两个AprilTag对应的空间点，r1、r2为两个不同AprilTag的中心像素与图像中心的欧氏距离。
4.根据权利要求1所述的一种提升无人机相机对AprilTag定位精度方法，其特征在于，所述步骤S3具体过程为采用非线性优化算法中的Levenberg-Marquardt算法求解BA (Bundle Adjustment)问题，公式如下：
(J T J+μI)Δx＝-J T e，
公式中J为2*6雅克比矩阵，e为误差函数，I为单位矩阵，J T为J矩阵的转置，形式如下:
其中(X′,Y′,Z′)为相机坐标系下的空间点坐标，δξ为变换矩阵T在特殊欧式SE(3)下的扰动，(u,v)为像素坐标；k1、k2为畸变系数，f x为x轴焦距，f y为y轴焦距，c x为x轴平移量，c y 为y轴平移量，d为像素坐标在归一化坐标系与原点的距离；
Levenberg-Marquardt算法步骤如下：
S31、给定初始变换矩阵T0，阻尼因子μ；
S32、对于第n次迭代，求误差函数e在x处雅克比矩阵，根据公式得出增量Δx，令x new＝x ⊕Δx；
S33、计算阻尼因子更新比例因子ρ；
S34、ρ>0.75，μ＝μ*2，接受本次更新x new；
S35、ρ<0.25，μ＝μ/3，拒绝本次更新x new；
S36、判断算法是否收敛，如果收敛则结束迭代，否则回到S32；
其中ρ为阻尼因子更新比例因子，表达式为
5.根据权利要求1所述的一种提升无人机相机对AprilTag定位精度方法，其特征在于，所述步骤S4中，位姿图优化理论对AprilTag之间的变换矩阵进行优化具体过程为：每个AprilTag的姿态为T1，…，T n，T i和T j之间变换矩阵为T ij，可得如下等式
T ij＝T i-1T j
每个AprilTag的姿态之间的变换存在误差，则等式不会完全成立，设误差为e ij，则有
所有边的集合为ε，则总体目标函数如下：
6.一种无人机定位方法，其特征在于，包括以下步骤：
B1、在场景内放置AprilTag阵列，使用权利要求1至5任一项所述步骤S1中计算得出相机内参及畸变系数，对无人机进行配置校正；
B2、无人机收到遥控器起飞指令，自动飞到指定高度进行悬停；
B3、无人机摄像头识别AprilTag信息，并运用权利要求1-5任一项中所述步骤S2、S3对AprilTag定位信息进行优化，向上位机发送优化后的AprilTag定位信息；
B4、上位机接收无人机摄像头传输的AprilTag定位信息，并在指定消息框显示信息并给每个AprilTag定位信息区域命名一个ID号，以及在可视化界面进行二维轨迹绘制；
B5、上位机指定其中一个ID号发送至无人机，无人机接收指定ID号后飞行至指定ID的AprilTag上，该过程中持续使用上述权利要求1中所述步骤S4对AprilTag之间的变换矩阵进行优化。
7.根据权利要求6所述的一种无人机定位方法，其特征在于，所述步骤B5中，上位机指定ID号发送至无人机，还包括指定飞行轨迹发送至无人机。
8.根据权利要求6所述的一种无人机定位方法，其特征在于，所述步骤B5中无人机接收指定ID号后飞行至指定ID的AprilTag上，具体包括：
B51、上位机指定其中一个ID号发送至无人机，无人机将指定ID转换为目标AprilTag的像素坐标；
B52、无人机将得到的像素坐标与roll、pitch解耦；
B53、无人机融合高度信息h；
B54、使用PID控制器控制无人机飞行。
9.一种运用上述权利要求6-8任一项所述定位方法的无人机定位系统，其特征在于，包括室内空间、二维码、无人机以及上位机，所述二维码放置于所述室内空间地面，所述无人机与所述上位机蓝牙或WIFI连接，所述上位机控制所述无人机。
10.根据权利要求9所述的一种无人机定位系统，其特征在于，所述无人机包括无人机主体、免驱摄像头、光流传感器以及OpenMV，所述免驱摄像头、光流传感器以及OpenMV均与所述无人机主体相连；所述上位机为PC端。
提升无人机相机对AprilTag识别定位精度方法及定位方法和
系统
技术领域
[0001]本发明涉及无人机定位领域，更具体地，涉及一种提升无人机相机对AprilTag识别定位精度方法及定位方法和系统。
背景技术
[0002]室内定位在AR、VR以及机器人等领域是一个重点问题，并且是室内自主导航的前提。虽然GPS能解决定位问题，但是在室内定位中，其信号较弱导致精度无法满足要求，甚至无法进行定位，所以室内定位需要采用其他定位技术。现在为解决室内定位的问题，已经出现了UWB定位、WIFI定位、蓝牙定位、以及SLAM定位等方案。
[0003]但是上述方案均存在较大的技术局限或者缺乏经济实用性：
[0004]1、基于蓝牙的定位受环境干扰大，且不能遮挡设备；
[0005]2、基于UWB的定位方案在无人机上有所应用，但是最少需要3个，且信号传播会受到墙壁、天花板、门、人等障碍物的阻挡，导致信号发生反射、折射、衍射现象，发射信号经过不同的时间、不同路径到达UWB接收端，最终造成UWB精度降低，需要较复杂的算法进行滤波以及优化；
[0006]3、基于视觉SLAM的定位方案在运行的过程中占用CPU大量资源，需要比较强悍的CPU才能实时运行，设备成本高，技术难度大，且需要环境纹理特征丰富，而室内无人机的俯视视角的纹理特征并不是特别丰富，所以一般的SLAM算法并不完全适用于无人机俯视视角，且定位轨迹容易漂移；
[0007]4、采用三维激光雷达来进行建图与定位，其成本昂贵，且重量大，对无人机要求极高。
发明内容
[0008]本发明旨在克服上述现有技术的至少一种缺陷，提供一种提升无人机相机对AprilTag识别定位精度方法及定位方法和系统，用于解决室内无人机定位受环境干扰大、定位精度低、CPU占用资源大以及成本高的问题。
[0009]本发明采取的技术方案是，一种提升无人机相机对AprilTag识别定位精度方法，包括以下步骤：
[0010]S1、对无人机上的相机进行标定，计算得出相机内参及畸变系数；
[0011]S2、在得到相机内参及畸变参数的基础上，相机获取AprilTag图像，然后从得到的AprilTag图像中识别出空间点信息，将多个AprilTag空间点信息进行融合；
[0012]S3、在得到多个空间点信息后，采用OpenCV中PnP求解相机在世界坐标系下的姿态，在通过PnP求解姿态后，再增加一次高斯牛顿法迭代优化姿态；
[0013]S4、使用优姿图优化理论对AprilTag之间的变换矩阵进行优化。
[0014]所述步骤S1先对无人机相机进行标定，在机器视觉领域，相机的标定是一个关键
说 明 书1/11页CN 112184812 A
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议