CN111578947A - 具有深度恢复能力的无人机单目slam可扩展框架 - Google Patents

具有深度恢复能力的无人机单目slam可扩展框架 Download PDF

Info

Publication number
CN111578947A
CN111578947A CN202010471313.0A CN202010471313A CN111578947A CN 111578947 A CN111578947 A CN 111578947A CN 202010471313 A CN202010471313 A CN 202010471313A CN 111578947 A CN111578947 A CN 111578947A
Authority
CN
China
Prior art keywords
coordinate system
orb
camera
drone
module
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
Application number
CN202010471313.0A
Other languages
English (en)
Other versions
CN111578947B (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.)
Shenzhen Hongyue Information Technology Co ltd
State Grid Zhejiang Electric Power Co Ltd Taizhou Jiaojiang District Power Supply Co
Original Assignee
Tianjin Polytechnic 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 Tianjin Polytechnic University filed Critical Tianjin Polytechnic University
Priority to CN202010471313.0A priority Critical patent/CN111578947B/zh
Publication of CN111578947A publication Critical patent/CN111578947A/zh
Application granted granted Critical
Publication of CN111578947B publication Critical patent/CN111578947B/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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

仅使用单目传感器，就为DJI自主无人机设计了具有深度恢复功能的可扩展的同时定位与建图(SLAM)框架，该无人机利用机器人操作系统(ROS)集成在机载计算机中。具体地，基于知识库ORB‑SLAM2，获得了车载单目视觉的具有比例的姿态信息，并且利用AprilTag2进行融合操作以恢复场景深度信息。另外，基于单目传感器，姿态转换模块和姿态释放模块被设计为允许无人机直接使用的相应的姿态信息。利用所提供的姿态信息，用于无人机的外环几何跟踪控制器被设计出来，通过计算期望的推力和姿态来完成从轨迹规划模块生成的飞行任务。由于在ROS中明确定义了通信接口，因此可以轻松实现所提出的框架，从而使无人飞行器仅凭单目视觉即可稳定，准确地自主执行轨迹跟踪和着陆任务。进行实际实验以验证所提出框架的有效性。

Description

具有深度恢复能力的无人机单目SLAM可扩展框架
技术领域
本发明属于计算机视觉与无人机的技术领域，特别是涉及一种GPS导航信号不可用的场景中无人机的自主恢复深度及轨迹跟踪控制方法。
背景技术
近年来，无人驾驶飞机在视觉伺服，非线性控制，SLAM等许多研究领域受到了广泛的关注。DJI Matrice100由于其出色的稳定性，对开发的再次支持和强大的耐力而被广泛用于无人机研究领域。例如轨迹跟踪控制，目标检测和跟踪，路径规划，基于视觉的导航等。在室外环境中，DJI自动驾驶仪可以稳定多旋翼的位置，并通过全球定位系统(GPS)提供正确的姿态和速度信号估计。但是，在室内环境下使用DJI Guidance时，它无法直接融合来自外部传感器的任何数据，因为缺少GPS信号。因此，设计无人机的框架以提供状态和控制信息非常重要，该信息可以获取无法访问GPS的速度和姿态。
单目相机具有结构简单，成本低廉，采样数据充足等优点，在SLAM研究领域引起了广泛的关注。利用单目视觉，已经有许多关于状态估计，里程计和SLAM的完整著作，包括PTAM，SVO，LSD-SLAM和VINS-Mono。对于单目视觉，对于无人机上的SLAM应用而言，快速完成初始化并准确恢复尺度信息至关重要。
由于负载限制和平台规模小，单目配置是无人机状态估计，映射和非线性控制的合理选择。使用ORB-SLAM2，仅通过使用单目传感器就可以快速完成初始化过程，这意味着可以满足无人机的状态估计和制图需求。但是，由于无法从单目相机恢复深度信息，因此地图的比例和预测的轨迹是未知的。因此，对于无人机上的单目SLAM应用，应该准确的完成有关单目相机比例尺信息的过程。此外，AprilTag2最近因其稳定性和准确性而备受关注，可用于单目SLAM的尺度恢复。
为了准确地执行轨迹跟踪任务，对于无人机来说，处理复杂的动力学和不确定的扰动需要适当的控制策略。线性控制方案，例如比例微分控制器和线性二次调节器，被广泛用于增强无人机的稳定性。由于很多控制器基于欧拉角，因此当表示跟踪不规则轨迹的四旋翼无人机的复杂旋转时，存在奇异点。为了解决这个问题，使用了一种用于轨迹跟踪任务的四旋翼无人机的几何控制器，它可以控制欠驱动的四旋翼无人机使用四个推力输入来稳定六个平移和旋转自由度，同时渐近地跟踪四个由其位置和航向组成的输出。
发明内容
本发明的目的是提供一个框架解决无人机缺失GPS信号而无法获取位置和姿态信息的情况，该框架能自主恢复单目相机的深度信息并使用单目SLAM为无人机提供位姿信息。这将会大大提高无人机的适应能力。
本发明提出一种GPS信号丢失的场景中无人机的自主恢复深度信息并完成轨迹跟踪任务的设计。在本发明中，通过利用具有深度恢复的单目状态估计器，为开源无人机设计了可扩展框架。在初始化步骤中估计单目深度信息，然后由单目稀疏SLAM完成轨迹跟踪任务。拟议的框架包含五个模块，包括状态估计，深度恢复，轨迹规划，跟踪控制器和硬件接口。框架分为三个阶段，首先无人机开始检测AprilTag2标签，同时运行ORB-SLAM2的单目模式，然后在初始化ORB-SLAM2存储库后同步融合深度信息。在单目状态估计中，第二阶段采用运动控制器驱动无人飞行器完成复杂的轨迹跟踪任务，状态估计也由深度融合获得ORB-SLAM2。然后在第三阶段中，无人飞行器降落到原点位置。收集实验结果以验证所提出框架的可行性。本文的主要贡献是，在简单的单目机载传感器配置的基础上设计了可扩展的框架，用于无人机自主的完成状态估计和轨迹跟踪控制。
本发明提供的具有深度恢复能力的无人机单目SLAM可扩展框架包括：
一种具有深度恢复能力的无人机单目SLAM可扩展框架，其特征包括以下步骤：
第1，框架的概述
框架中五个模块的关系如图1所示。深度求解模块首先被设计为利用AprilTag2算法来计算比例尺信息。初始化单目模式的ORB-SLAM2，然后将其与相机深度融合以提供状态估计结果。引入了轨迹规划模块以生成所需轨迹，并且控制器模块通过合并实时姿态信息来计算用于安全，准确的无人机运动的输入命令。最后，DJI自动驾驶仪通过硬件接口接收控制器发送的输入命令。框架中所有模块之间的通信都是通过ROS接口进行的。
第2，坐标系转换和深度求解
该框架的第一个模块是通过合并AprilTag2和ORB-SLAM2获得有关单目相机的深度信息。位姿发布和转换模块是在ROS环境下设计的。无人机上安装了英特尔实感D435i传感器，并且所提的框架中仅使用单目相机，相机坐标系表示为
Figure BSA0000210024130000021
在初始时刻t＝0时表示为
Figure BSA0000210024130000022
表示无人机机体坐标系，
Figure BSA0000210024130000023
在初始时刻t＝0时表示为
Figure BSA0000210024130000024
参考坐标系定义在AprilTag2的标签上，用
Figure BSA0000210024130000025
表示。利用标签检测算法，在
Figure BSA0000210024130000026
下表示的坐标系
Figure BSA0000210024130000027
的旋转矩阵
Figure BSA0000210024130000028
和平移向量
Figure BSA0000210024130000029
都是通过AprilTag2姿态计算算法获得的。
Figure BSA00002100241300000210
表示坐标系
Figure BSA00002100241300000211
在
Figure BSA00002100241300000212
下的转换关系，同样的
Figure BSA00002100241300000213
表示坐标系
Figure BSA00002100241300000214
在
Figure BSA00002100241300000215
下的转换关系，
Figure BSA00002100241300000216
表示预校准的相机和无人机之间的转换关系。根据坐标系转换原理，可以通过计算
Figure BSA00002100241300000217
和
Figure BSA00002100241300000218
得到
Figure BSA00002100241300000219
计算过程如下所示：
Figure BSA00002100241300000220
然后，旋转矩阵
Figure BSA00002100241300000221
和平移向量
Figure BSA00002100241300000222
可以通过以下获得：
Figure BSA00002100241300000223
系统启动时，AprilTag2和ORB-SLAM2都会启动，t′表示ORB-SLAM2初始化完成的瞬间。我们假设启动系统时标签位于相机视场中间。通过对AprilTag2进行的姿态测量，可以得到从时间ti到
Figure BSA00002100241300000224
的摄像机坐标系的变化量
Figure BSA00002100241300000225
如下所示：
Figure BSA00002100241300000226
Figure BSA00002100241300000227
的平移向量
Figure BSA00002100241300000228
已经获得。
此外，在时间t′处，可以在ORB-SLAM2的输出下获得从
Figure BSA00002100241300000229
到时间t′的摄像机坐标系的缩放平移矢量
Figure BSA00002100241300000230
因此，通过计算相同相机平移矢量的上述两个不同表达式之间的比率，可以恢复复杂的ORB-SLAM2的比例因子s：
Figure BSA0000210024130000031
第3，状态估计模块
结合已建立的单目SLAM(例如ORB-SLAM2)，将其用于无人机平台，它可以提供可靠、准确的无人机状态估计和工作区映射结果。
在ORB-SLAM2中，仅运动包调整(BA)可以将摄像机坐标系的方向
Figure BSA0000210024130000032
和位置
Figure BSA0000210024130000033
从
Figure BSA0000210024130000034
最佳化为当前时间t，从而使匹配的3-D点
Figure BSA0000210024130000035
在
Figure BSA0000210024130000036
以下和图像关键点
Figure BSA0000210024130000037
与i∈χ之间的重投影误差最小。所有匹配项的集合：
Figure BSA0000210024130000038
其中ρ(·)是鲁棒的Huber成本函数，而∑是与关键点规模相关的协方差矩阵。πm(·)的定义如下：
Figure BSA0000210024130000039
其中校准的相机参数(fx，fy)是缩放的焦距，而(cx，cy)是图像上的主要特征点。
原始的ORB-SLAM2主要具有三个并行线程：跟踪，局部映射和循环关闭，但是，生成的姿态信息不会实时发布。为了解决无人机无法直接获得深度融合ORB-SLAM2提供的状态估计的问题，设计了一种姿态发布转换模块，如图2所示。
在图2中，姿态信息是通过利用ROS发布者/订阅者工具进行传输的。利用转换节点，可以根据摄像机/无人机坐标系的关系从深度融合摄像机的姿态中获得无人机姿态，如图3所示。
如坐标系转换和深度求解章节所述，无人机和摄像机坐标系的变换是已知的且恒定不变，而如状态估计模块所述，通过融合的ORB-SLAM2姿态计算可获得
Figure BSA00002100241300000310
下
Figure BSA00002100241300000311
的变换
Figure BSA00002100241300000312
在此基础上，根据坐标系变换原理，可以通过以下关系得到坐标系
Figure BSA00002100241300000313
在
Figure BSA00002100241300000314
下的变换
Figure BSA00002100241300000315
Figure BSA00002100241300000316
然后，使用ROS发布者功能实时发布
Figure BSA00002100241300000317
在这一部分中，将计算无人机的实时位置和姿态，并通过ROS接口将其发送到控制器模块。
第4，轨迹规划模块
为了通过无人机完成复杂的任务，通常会结合平台大小，目标跟踪，能耗等因素，完成适当的运动轨迹规划。
在本框架中，以伯努利曲线为例，将伯努利曲线作为期望轨迹，因为其难以被跟踪的特性，可以很好的评估后续跟踪控制模块的性能。伯努利曲线的参数方程如下所示：
Figure BSA0000210024130000041
在(12)中，引入参数ω来设置所需的线性和角速度，r与曲线的大小有关，并且参数π/2涉及与无人机的起降过程的合理连接。
第5，跟踪控制器
对于大疆无人机而言，不支持期望总时刻的输入，推力fu(t)，俯仰角θu(t)，横摆角φu(t)，偏航率
Figure BSA0000210024130000042
是唯一可用的输入。为了解决这种情况，在框架中采用了基于SE(3)的几何控制器来实现复杂的轨迹跟踪任务。
控制过程如图4所示。位置和速度误差都可以通过计划模块和当前状态估计模块中的所需轨迹来计算。ep(t)表示期望位置和当前位置之间的误差向量，ev(t)表示期望速度和当前速度之间的误差向量，m是整个系统的总重量。对角增益矩阵表示为Kp和Kv。然后，所需推力fu(t)计算为：
Figure BSA0000210024130000043
如下提供了计算控制器的期望姿态输入Rd(t)的方法：
Figure BSA0000210024130000044
n2d＝[-sinψd cosψd 0]T， (11)
Figure BSA0000210024130000045
r2d＝r3d×r1d， (13)
Figure BSA0000210024130000046
然后分别计算输入的俯仰角和侧倾角为
θu＝atan(-r3，r1cosψd+r2sinψd)， (15)
φu＝atan(r7sinψd-r8cosψd，r5cosψd-r4sinψd). (16)
为了稳定偏航角跟踪误差的零平衡，应用比例控制器来计算所需的偏航率输入
Figure BSA0000210024130000047
Figure BSA0000210024130000048
第6，硬件接口
框架中的最后一个模块是硬件接口，如图5所示。在收到控制输入后，接口模块将通过硬件连接将控制命令发送到大疆无人机。
DJI经纬100(M100)无人机用作运动平台，Intel D435i设备用作单目相机以捕获图像信息，并且在飞机上安装了具有Ubuntu 16.04的Intel NUC(8i7BEH)以发送控制指令。作为地面站，个人计算机还配备有Ubuntu 16.04，可通过安全外壳(SSH)远程终端实时掌握车载计算机的状态。USB-TTL和UART电缆都是M100和Intel NUC之间的通信通道，但是远程终端相对独立于板载控制。在基于ROS(Kinetic)的Ubuntu 16.04上运行的DJI SDK(OSDK3.9)是用于M100开发的必不可少的工具包。
框架中的所有模块都以ROS节点的形式执行通信，这些节点分别是AprilTag节点，ORB-SLAM2节点，姿态转换节点，深度节点和DJI SDK节点。节点图如图6所示。
启动系统后，AprilTag节点和ORB-SLAM节点均立即开始工作。通过深度节点，利用接收到的上述两个节点的信息输入，获得单目相机的深度信息。对于比例恢复的姿态，UAV的状态信息是通过姿态转换节点获得的。DJI SDK节点一旦接收到相应的姿态信息，便向UAV发送控制信号。
在DJI SDK节点中，操作过程如图7所示。在初始化过程中，需要启用四个电机并掌握控制权限，以便机载计算机可以操纵四旋翼。电动机启动后，需要以50Hz的频率向无人机发送控制命令，以进行稳定的飞行控制。在无人机着陆后释放控制权之前，应先禁用电动机。
附图说明：
图1为所提框架概述框图
图2为位姿发布和转换模块图
图3为无人机与机载摄像头的坐标系关系
图4为控制量流程图
图5为框架中的硬件接口图
图6为ROS中的节点图
图7为DJI SDK节点的操作流程
图8为实验设备图
图9为实验图：无人机在跟踪过程中的运动轨迹(实线：当前轨迹；虚线：期望轨迹。)
图10为实验图：无人机在跟踪过程中的位置演变(实线：当前轨迹；虚线：所需轨迹)。
图11为实验图：无人机在整个过程中的姿态演变(实线：当前轨迹；虚线：所需轨迹)。
图12为实验图：无人机推力在整个过程中的演变
具体实施方式：
一种具有深度恢复能力的无人机单目SLAM可扩展框架，其特征包括以下步骤：
第1，框架的概述
框架中五个模块的关系如图1所示。深度求解模块首先被设计为利用AprilTag2算法来计算比例尺信息。初始化单目模式的ORB-SLAM2，然后将其与相机深度融合以提供状态估计结果。引入了轨迹计划模块以生成所需轨迹，并且控制器模块通过整合实时姿态信息来计算用于安全，准确的无人机运动的输入命令。最后，DJI自动驾驶仪通过硬件接口接收控制器发送的输入命令。框架中所有模块之间的通信都是通过ROS接口进行的。
第2，坐标系转换和深度求解
该框架的第一个模块是通过合并AprilTag2和ORB-SLAM2获得有关单目相机的深度信息。位姿发布和转换模块实在ROS环境设计的。无人机上安装了英特尔实感D435i传感器，并且所提的框架中仅使用单目相机，相机坐标系表示为
Figure BSA0000210024130000061
在初始时刻t＝0时表示为
Figure BSA0000210024130000062
表示无人机机体坐标系，
Figure BSA0000210024130000063
在初始时刻t＝0时表示为
Figure BSA0000210024130000064
参考坐标系定义在AprilTag2的标签上，用
Figure BSA0000210024130000065
表示。利用标签检测算法，在
Figure BSA0000210024130000066
下表示的坐标系
Figure BSA0000210024130000067
的旋转矩阵
Figure BSA0000210024130000068
和平移向量
Figure BSA0000210024130000069
都是通过AprilTag2姿态计算算法获得的。
Figure BSA00002100241300000610
表示坐标系
Figure BSA00002100241300000611
在
Figure BSA00002100241300000612
下的转换关系，同样的
Figure BSA00002100241300000613
表示坐标系
Figure BSA00002100241300000614
在
Figure BSA00002100241300000615
下的转换关系，
Figure BSA00002100241300000616
表示预校准的相机和无人机之间的转换关系。根据坐标系转换原理，可以通过计算
Figure BSA00002100241300000617
和
Figure BSA00002100241300000618
得到
Figure BSA00002100241300000619
计算过程如下所示：
Figure BSA00002100241300000620
然后，旋转矩阵
Figure BSA00002100241300000621
和平移向量
Figure BSA00002100241300000622
可以通过以下获得：
Figure BSA00002100241300000623
系统启动时，AprilTag2和ORB-SLAM2都会启动，t′表示ORB-SLAM2初始化完成的瞬间。我们假设启动系统时标签位于相机视场中。通过对AprilTag2进行的姿态测量，可以得到从时间ti到
Figure BSA00002100241300000624
的摄像机坐标系的变化量
Figure BSA00002100241300000625
如下所示：
Figure BSA00002100241300000626
Figure BSA00002100241300000627
的平移向量
Figure BSA00002100241300000628
已经获得。
此外，在时间t′处，可以在ORB-SLAM2的输出下获得从
Figure BSA00002100241300000629
到时间t′的摄像机坐标系的缩放平移矢量
Figure BSA00002100241300000630
因此，通过计算相同相机平移矢量的上述两个不同表达式之间的比率，可以恢复复杂的ORB-SLAM2的比例因子s：
Figure BSA00002100241300000631
第3，状态估计模块
结合已建立的单目SLAM(例如ORB-SLAM2)，将其用于无人机平台，它可以提供可靠、准确的无人机状态估计和工作区映射结果。
在ORB-SLAM2中，仅运动包调整(BA)可以将摄像机坐标系的方向
Figure BSA0000210024130000071
和位置
Figure BSA0000210024130000072
从
Figure BSA0000210024130000073
最佳化为当前时间t，从而使匹配的3-D点
Figure BSA0000210024130000074
在
Figure BSA0000210024130000075
以下和图像关键点
Figure BSA0000210024130000076
与i∈χ之间的重投影误差最小。所有匹配项的集合：
Figure BSA0000210024130000077
其中ρ(·)是鲁棒的Huber成本函数，而∑是与关键点规模相关的协方差矩阵。πm(·)的定义如下：
Figure BSA0000210024130000078
其中校准的相机参数(fx，fy)是缩放的焦距，而(cx，cy)是图像上的主要特征点。
原始的ORB-SLAM2主要具有三个并行线程：跟踪，局部映射和循环关闭，但是，生成的姿态信息不会实时发布。为了解决无人机无法直接获得深度融合ORB-SLAM2提供的状态估计的问题，设计了一种姿态发布转换模块，如图2所示。
在图2中，姿态信息是通过利用ROS发布者/订阅者工具进行传输的。利用转换节点，可以根据摄像机/无人机坐标系的关系从深度融合摄像机的姿态中获得无人机姿态，如图3所示。
如坐标系转换和深度求解章节所述，无人机和摄像机坐标系的变换是已知的且恒定不变，而如状态估计模块所述，通过融合的ORB-SLAM2姿态计算可获得
Figure BSA0000210024130000079
下
Figure BSA00002100241300000710
的变换
Figure BSA00002100241300000711
在此基础上，根据坐标系变换原理，可以通过以下关系得到坐标系
Figure BSA00002100241300000712
在
Figure BSA00002100241300000713
下的变换
Figure BSA00002100241300000714
Figure BSA00002100241300000715
然后，使用ROS发布者功能实时发布
Figure BSA00002100241300000716
在这一部分中，将计算无人机的实时位置和姿态，并通过ROS接口将其发送到控制器模块。
第4，轨迹规划模块
为了通过无人机完成复杂的任务，通常会结合平台大小，目标跟踪，能耗等因素，制定适当的运动轨迹规划。
在本框架中，以伯努利曲线为例，将伯努利曲线作为期望轨迹，因为其难以被跟踪的特性，可以很好的评估后续跟踪控制模块的性能。伯努利曲线的参数方程如下所示：
Figure BSA00002100241300000717
在(12)中，引入参数ω来设置所需的线性和角速度，r与曲线的大小有关，并且参数π/2涉及与无人机的起降过程的合理连接。
第5，跟踪控制器
对于大疆无人机而言，不支持期望总时刻的输入，推力fu(t)，俯仰角θu(t)，横摆角φu(t)，偏航率
Figure BSA0000210024130000081
是唯一可用的输入。为了解决这种情况，在框架中采用了基于SE(3)的几何控制器来实现复杂的轨迹跟踪任务。
控制过程如图4所示。位置和速度误差都可以通过轨迹规划模块和当前状态估计模块中的所需轨迹来计算。ep(t)表示期望位置和当前位置之间的误差向量，ev(t)表示期望速度和当前速度之间的误差向量，m是整个系统的总重量。对角增益矩阵表示为Kp和Kv。然后，所需推力fu(t)计算为：
Figure BSA0000210024130000082
如下提供了计算控制器的期望姿态输入Rd(t)的方法：
Figure BSA0000210024130000083
n2d＝[-sinψd cosψd 0]T， (28)
Figure BSA0000210024130000084
r2d＝r3d×r1d， (30)
Figure BSA0000210024130000085
然后分别计算输入的俯仰角和侧倾角为
θu＝atan(-r3，r1cosψd+r2sinψd)， (32)
φu＝atan(r7sinψd-r8cosψd，r5cosψd-r4sinψd). (33)
为了稳定偏航角跟踪误差的零平衡，应用比例控制器来计算所需的偏航率输入
Figure BSA0000210024130000086
Figure BSA0000210024130000087
第6，硬件接口
框架中的最后一个模块是硬件接口，如图5所示。在收到控制输入后，接口模块将通过硬件连接将控制命令发送到大疆无人机。
DJI经纬100(M100)无人机用作运动平台，Intel D435i设备用作单目相机以捕获图像信息，并且在飞机上安装了具有Ubuntu 16.04的Intel NUC(8i7BEH)以发送控制指令。作为地面站，个人计算机还配备有Ubuntu 16.04，可通过安全外壳(SSH)远程终端实时掌握车载计算机的状态。USB-TTL和UART电缆都是M100和Intel NUC之间的通信通道，但是远程终端相对独立于板载控制。在基于ROS(Kinetic)的Ubuntu 16.04上运行的DJI SDK(OSDK3.9)是用于M100开发的必不可少的工具包。
框架中的所有模块都以ROS节点的形式执行通信，这些节点分别是AprilTag节点，ORB-SLAM2节点，姿态转换节点，深度节点和DJI SDK节点。节点图如图6所示。
启动系统后，AprilTag节点和ORB-SLAM节点均立即开始工作。通过深度节点，利用接收到的上述两个节点的信息输入，获得单目相机的深度信息。对于比例恢复的姿态，无人机的状态信息是通过姿态转换节点获得的。DJI SDK节点一旦接收到相应的姿态信息，便向无人机发送控制信号。
在DJI SDK节点中，操作过程如图7所示。在初始化过程中，需要启用四个电机并掌握控制权限，以便机载计算机可以操纵四旋翼。电动机启动后，需要以50Hz的频率向无人机发送控制命令，以进行稳定的飞行控制。在无人机着陆后释放控制权之前，应先禁用电动机。
第7，实验和结论
第7.1，实验
通过实验来证明该框架的实用性。
在本节中，将提供实验结果，以基于具有深度恢复能力的单目SLAM来验证所提出框架的有效性。实验场景如图8(a)所示。如图8(b)所示，实验平台为DJI M100自动驾驶仪，配备了Intel NUC车载计算机和Intel RealSense单目相机，硬件连接如图5所示。图8(b)是在执行轨迹跟踪任务期间捕获。
D435i摄像机的校准参数为fx：615.9，fy：616.1，cx：322.4，cy：240.4。摄像机内参数为：
Figure BSA0000210024130000091
此外，Kp＝diag([0.09，0.09，0.1])，kψ＝2.8，r＝1.0，ω＝0.0042，Kv＝diag([0.09，0.09，0.12])。标签的规格和位置为{id：1，size：0.143，x：1.8100，y：0.0000，z：0.0，qw：0.707，qx：0.0，qy：0.0，qz：-0.707}。
在实验中，无人机分别在执行轨迹跟踪任务之前和之后起飞和降落。在轨迹跟踪任务中，图9显示了无人机的运动轨迹，其中虚线是期望的伯努利双按钮线。姿态变化如图10所示，我们看到当前位置和期望位置具有很好的一致性。
在
Figure BSA0000210024130000092
坐标系下表示的三个横摆角，俯仰角和偏航角的演变，而推力b的输出如图12所示。
实验结果表明，无人机在深度恢复后可以通过单目ORB-SLAM2算法成功完成轨迹跟踪任务，所提出的框架可以实时执行复杂任务。
第7.2，结论
通过利用具有深度恢复的单目状态估计，为开源无人机设计了可扩展框架。在计算单目SLAM的深度信息后，无人机使用状态估计提供的姿态信息成功完成轨迹跟踪和自主着陆任务。在提出的框架中，深度恢复和轨迹跟踪任务在无人机飞行过程中能够自主并精准的完成。

Claims (1)

1.一种具有深度恢复能力的无人机单目SLAM可扩展框架，其特征包括以下步骤：
第1，框架的概述
框架中五个模块的关系如图1所示。深度求解模块首先被设计为利用AprilTag2算法来计算比例尺信息。初始化单目模式的ORB-SLAM2，然后将其与相机深度融合以提供状态估计结果。引入了轨迹设计模块以生成所需轨迹，并且控制器模块通过合并实时姿态信息来计算用于安全，准确的无人机运动的输入命令。最后，DJI自动驾驶仪通过硬件接口接收控制器发送的输入命令。框架中所有模块之间的通信都是通过ROS接口进行的。
第2，坐标系转换和深度求解
该框架的第一个模块是通过合并AprilTag2和ORB-SLAM2获得有关单目相机的深度信息。位姿发布和转换模块是在ROS环境下设计的。无人机上安装了英特尔实感D435i传感器，并且所提的框架中仅使用单目相机，相机坐标系表示为
Figure FSA0000210024120000011
在初始时刻t＝0时表示为
Figure FSA0000210024120000012
表示无人机机体坐标系，
Figure FSA0000210024120000013
在初始时刻t＝0时表示为
Figure FSA0000210024120000014
参考坐标系定义在AprilTag2的标签上，用
Figure FSA0000210024120000015
表示。利用标签检测算法，在
Figure FSA0000210024120000016
下表示的坐标系
Figure FSA0000210024120000017
的旋转矩阵
Figure FSA0000210024120000018
和平移向量
Figure FSA0000210024120000019
都是通过AprilTag2姿态计算算法获得的。
Figure FSA00002100241200000110
表示坐标系
Figure FSA00002100241200000111
在
Figure FSA00002100241200000112
下的转换关系，同样的
Figure FSA00002100241200000113
表示坐标系
Figure FSA00002100241200000114
在
Figure FSA00002100241200000115
下的转换关系，
Figure FSA00002100241200000116
表示预校准的相机和无人机之间的转换关系。根据坐标系转换原理，可以通过计算
Figure FSA00002100241200000117
和
Figure FSA00002100241200000118
得到
Figure FSA00002100241200000119
计算过程如下所示：
Figure FSA00002100241200000120
然后，旋转矩阵
Figure FSA00002100241200000121
和平移向量
Figure FSA00002100241200000122
可以通过以下获得：
Figure FSA00002100241200000123
系统启动时，AprilTag2和ORB-SLAM2都会启动，t′表示ORB-SLAM2初始化完成的瞬间。我们假设启动系统时标签位于相机视场中。通过对AprilTag2进行的姿态测量，可以得到从时间ti到
Figure FSA00002100241200000124
的摄像机坐标系的变化量
Figure FSA00002100241200000125
如下所示：
Figure FSA00002100241200000126
Figure FSA00002100241200000127
的平移向量
Figure FSA00002100241200000128
已经获得。
此外，在时间t′处，可以在ORB-SLAM2的输出下获得从
Figure FSA00002100241200000129
到时间t′的摄像机坐标系的缩放平移矢量
Figure FSA00002100241200000130
因此，通过计算相同相机平移矢量的上述两个不同表达式之间的比率，可以恢复复杂的ORB-SLAM2的比例因子s：
Figure FSA00002100241200000131
第3，状态估计模块
结合已建立的单目SLAM(例如ORB-SLAM2)，将其用于无人机平台，它可以提供可靠、准确的无人机状态估计和工作区映射结果。
在ORB-SLAM2中，仅运动包调整(BA)可以将摄像机坐标系的方向
Figure FSA0000210024120000021
和位置
Figure FSA0000210024120000022
从
Figure FSA0000210024120000023
最佳化为当前时间t，从而使匹配的3-D点
Figure FSA0000210024120000024
在
Figure FSA0000210024120000025
以下和图像关键点
Figure FSA0000210024120000026
与i∈χ之间的重投影误差最小。所有匹配项的集合：
Figure FSA0000210024120000027
其中ρ(·)是鲁棒的Huber成本函数，而∑是与关键点规模相关的协方差矩阵。πm(·)的定义如下：
Figure FSA0000210024120000028
其中校准的相机参数(fx，fy)是缩放的焦距，而(cx，cy)是图像上的主要特征点。
原始的ORB-SLAM2主要具有三个并行线程：跟踪，局部映射和循环关闭，但是，生成的姿态信息不会实时发布。为了解决无人机无法直接获得深度融合ORB-SLAM2提供的状态估计的问题，设计了一种姿态发布转换模块，如图2所示。
在图2中，姿态信息是通过利用ROS发布者/订阅者工具进行传输的。利用转换节点，可以根据摄像机/无人机坐标系的关系从深度融合摄像机的姿态中获得无人机姿态，如图3所示。
如坐标系转换和深度求解章节所述，无人机和摄像机坐标系的变换是已知的且恒定不变，而如状态估计模块所述，通过融合的ORB-SLAM2姿态计算可获得
Figure FSA0000210024120000029
下
Figure FSA00002100241200000210
的变换
Figure FSA00002100241200000211
在此基础上，根据坐标系变换原理，可以通过以下关系得到坐标系
Figure FSA00002100241200000212
在
Figure FSA00002100241200000213
下的变换
Figure FSA00002100241200000214
Figure FSA00002100241200000215
然后，使用ROS发布者功能实时发布
Figure FSA00002100241200000216
在这一部分中，将计算无人机的实时位置和姿态，并通过ROS接口将其发送到控制器模块。
第4，轨迹规划模块
为了通过无人机完成复杂的任务，通常会结合平台大小，目标跟踪，能耗等因素，设计适当的运动轨迹规划。
在本框架中，以伯努利曲线为例，将伯努利曲线作为期望轨迹，因为其难以被跟踪的特性，可以很好的评估后续跟踪控制模块的性能。伯努利曲线的参数方程如下所示：
Figure FSA0000210024120000031
在(12)中，引入参数ω来设置所需的线性和角速度，r与曲线的大小有关，并且参数π/2涉及与无人机的起降过程的合理连接。
第5，跟踪控制器
对于大疆无人机而言，不支持期望总时刻的输入，推力fu(t)，俯仰角θu(t)，横摆角φu(t)，偏航率
Figure FSA0000210024120000032
是唯一可用的输入。为了解决这种情况，在框架中采用了基于SE(3)的几何控制器来实现复杂的轨迹跟踪任务。
控制过程如图4所示。位置和速度误差都可以通过计划模块和当前状态估计模块中的所需轨迹来计算。ep(t)表示期望位置和当前位置之间的误差向量，ev(t)表示期望速度和当前速度之间的误差向量，m是整个系统的总重量。对角增益矩阵表示为Kp和Kv。然后，所需推力fu(t)计算为：
Figure FSA0000210024120000033
如下提供了计算控制器的期望姿态输入Rd(t)的方法：
Figure FSA0000210024120000034
n2d＝[-sinψd cosψd 0]T， (11)
Figure FSA0000210024120000035
r2d＝r3d×r1d， (13)
Figure FSA0000210024120000036
然后分别计算输入的俯仰角和侧倾角为
θu＝atan(-r3，r1cosψd+r2sinψd)， (15)
φu＝atan(r7sinψd-r8cosψd，r5cosψd-r4sinψd). (16)
为了稳定偏航角跟踪误差的零平衡，应用比例控制器来计算所需的偏航率输入
Figure FSA0000210024120000037
Figure FSA0000210024120000038
第6，硬件接口
框架中的最后一个模块是硬件接口，如图5所示。在收到控制输入后，接口模块将通过硬件连接将控制命令发送到大疆无人机。
DJI经纬100(M100)无人机用作运动平台，Intel D435i设备用作单目相机以捕获图像信息，并且在飞机上安装了具有Ubuntu 16.04的Intel NUC(8i7BEH)以发送控制指令。作为地面站，个人计算机还配备有Ubuntu 16.04，可通过安全外壳(SSH)远程终端实时掌握车载计算机的状态。USB-TTL和UART电缆都是M100和Intel NUC之间的通信通道，但是远程终端相对独立于板载控制。在基于ROS(Kinetic)的Ubuntu 16.04上运行的DJI SDK(OSDK 3.9)是用于M100开发的必不可少的工具包。
框架中的所有模块都以ROS节点的形式执行通信，这些节点分别是AprilTag节点，ORB-SLAM2节点，姿态转换节点，深度节点和DJI SDK节点。节点图如图6所示。
启动系统后，AprilTag节点和ORB-SLAM节点均立即开始工作。通过深度节点，利用接收到的上述两个节点的信息输入，获得单目相机的深度信息。对于比例恢复的姿态，UAV的状态信息是通过姿态转换节点获得的。DJI SDK节点一旦接收到相应的姿态信息，便向UAV发送控制信号。
在DJI SDK节点中，操作过程如图7所示。在初始化过程中，需要启用四个电机并掌握控制权限，以便机载计算机可以操纵四旋翼。电动机启动后，需要以50Hz的频率向无人机发送控制命令，以进行稳定的飞行控制。在无人机着陆后释放控制权之前，应先禁用电动机。
CN202010471313.0A 2020-05-29 2020-05-29 具有深度恢复能力的无人机单目slam可扩展框架 Active CN111578947B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010471313.0A CN111578947B (zh) 2020-05-29 2020-05-29 具有深度恢复能力的无人机单目slam可扩展框架

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010471313.0A CN111578947B (zh) 2020-05-29 2020-05-29 具有深度恢复能力的无人机单目slam可扩展框架

Publications (2)

Publication Number Publication Date
CN111578947A true CN111578947A (zh) 2020-08-25
CN111578947B CN111578947B (zh) 2023-12-22

Family

ID=72125665

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010471313.0A Active CN111578947B (zh) 2020-05-29 2020-05-29 具有深度恢复能力的无人机单目slam可扩展框架

Country Status (1)

Country Link
CN (1) CN111578947B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104808685A (zh) * 2015-04-27 2015-07-29 中国科学院长春光学精密机械与物理研究所 用于无人机自主降落的视觉辅助装置及视觉辅助方法
CN106885574A (zh) * 2017-02-15 2017-06-23 北京大学深圳研究生院 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法
CN108303099A (zh) * 2018-06-14 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维视觉slam的无人机室内自主导航方法
CN108827306A (zh) * 2018-05-31 2018-11-16 北京林业大学 一种基于多传感器融合的无人机slam导航方法及系统
CN109029417A (zh) * 2018-05-21 2018-12-18 南京航空航天大学 基于混合视觉里程计和多尺度地图的无人机slam方法
US20190114777A1 (en) * 2017-10-18 2019-04-18 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN109816687A (zh) * 2017-11-20 2019-05-28 天津工业大学 轮式移动机器人视觉伺服轨迹跟踪并发深度辨识
CN109960145A (zh) * 2017-12-22 2019-07-02 天津工业大学 移动机器人混合视觉轨迹跟踪策略

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104808685A (zh) * 2015-04-27 2015-07-29 中国科学院长春光学精密机械与物理研究所 用于无人机自主降落的视觉辅助装置及视觉辅助方法
CN106885574A (zh) * 2017-02-15 2017-06-23 北京大学深圳研究生院 一种基于重跟踪策略的单目视觉机器人同步定位与地图构建方法
US20190114777A1 (en) * 2017-10-18 2019-04-18 Tata Consultancy Services Limited Systems and methods for edge points based monocular visual slam
CN109816687A (zh) * 2017-11-20 2019-05-28 天津工业大学 轮式移动机器人视觉伺服轨迹跟踪并发深度辨识
CN109960145A (zh) * 2017-12-22 2019-07-02 天津工业大学 移动机器人混合视觉轨迹跟踪策略
CN109029417A (zh) * 2018-05-21 2018-12-18 南京航空航天大学 基于混合视觉里程计和多尺度地图的无人机slam方法
CN108827306A (zh) * 2018-05-31 2018-11-16 北京林业大学 一种基于多传感器融合的无人机slam导航方法及系统
CN108303099A (zh) * 2018-06-14 2018-07-20 江苏中科院智能科学技术应用研究院 基于三维视觉slam的无人机室内自主导航方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
张剑华;王燕燕;王曾媛;陈胜勇;管秋;: "单目同时定位与建图中的地图恢复融合技术" *
张阳;李海森;马礼;王健;郭企嘉;祁晶;闫铭;: "基于ORB-SLAM2算法的水下机器人实时定位研究" *
陈国军;陈巍;郁汉琪;王涵立;: "基于语义ORB-SLAM2算法的移动机器人自主导航方法研究" *

Also Published As

Publication number Publication date
CN111578947B (zh) 2023-12-22

Similar Documents

Publication Publication Date Title
García Carrillo et al. Combining stereo vision and inertial navigation system for a quad-rotor UAV
García Carrillo et al. Stabilization and trajectory tracking of a quad-rotor using vision
Lee et al. Adaptive image-based visual servoing for an underactuated quadrotor system
Deng et al. Indoor multi-camera-based testbed for 3-D tracking and control of UAVs
Mahboubi et al. Camera based localization for autonomous UAV formation flight
Rady et al. A hybrid localization approach for UAV in GPS denied areas
Xie Dynamic visual servoing of rotary wing unmanned aerial vehicles
Lin et al. Development of an unmanned coaxial rotorcraft for the DARPA UAVForge challenge
Liu et al. Vision aided unmanned aerial vehicle autonomy: An overview
Agarwal et al. Evaluation of a commercially available autonomous visual inertial odometry solution for indoor navigation
Fink et al. Dynamic visual servoing for a quadrotor using a virtual camera
Hosen et al. Vision-aided nonlinear observer for fixed-wing unmanned aerial vehicle navigation
Guo et al. Nonlinear vision-based observer for visual servo control of an aerial robot in global positioning system denied environments
Andersen et al. Improving MAV pose estimation using visual information
Wang et al. Vision-aided tracking of a moving ground vehicle with a hybrid uav
Fusini et al. Experimental validation of a uniformly semi-globally exponentially stable non-linear observer for gnss-and camera-aided inertial navigation for fixed-wing uavs
Lee et al. Landing Site Inspection and Autonomous Pose Correction for Unmanned Aerial Vehicles
Gonçalves et al. Vision-based automatic approach and landing of fixed-wing aircraft using a dense visual tracking
CN107161350B (zh) 事故处置飞行器自主续航方法及其系统
Leishman et al. Utilizing an improved rotorcraft dynamic model in state estimation
CN111578947B (zh) 具有深度恢复能力的无人机单目slam可扩展框架
Zhao et al. Vision-based robust position control for ground target tracking and hovering of quadrotors
Corke et al. Sensing and control on the sphere
CN114089779A (zh) Gps拒止环境下空中机器人视觉自主控制方法及系统
Hosen et al. A vision-aided nonlinear observer for fixed-wing UAV navigation

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
TA01 Transfer of patent application right

Effective date of registration: 20231124

Address after: 318001 No. 380, Gongren East Road, Baiyun Street, Jiaojiang District, Taizhou City, Zhejiang Province

Applicant after: State Grid Zhejiang Electric Power Co., Ltd. Taizhou Jiaojiang District Power Supply Co.

Address before: 518000 1104, Building A, Zhiyun Industrial Park, No. 13, Huaxing Road, Henglang Community, Longhua District, Shenzhen, Guangdong Province

Applicant before: Shenzhen Hongyue Information Technology Co.,Ltd.

Effective date of registration: 20231124

Address after: 518000 1104, Building A, Zhiyun Industrial Park, No. 13, Huaxing Road, Henglang Community, Longhua District, Shenzhen, Guangdong Province

Applicant after: Shenzhen Hongyue Information Technology Co.,Ltd.

Address before: No. 399 Bingshui Road, Xiqing District, Tianjin, Tianjin

Applicant before: TIANJIN POLYTECHNIC University

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant