GAAS
Search…
无人机自动驾驶软件系列 E03: 在无GPS环境下通过SLAM实现位置估计
本课中我们将会讲解如何通过SLAM(Simultaneous localization and mapping, 即实时定位与建图)实现无GPS环境下无人机的位置估计。
我们建议开发者使用镜像来安装 GAAS,这样可以节省很多的时间和精力:
注意,在你没有把握的情况下,我们建议你不要轻易进行实机测试。并且测试的时候,请保持距离,并注意安全。本课的内容虽然室内外都可以使用,但我们建议你先在室外空地测试。
GPS 可以为无人机提供较为准确的位置信息,但是某些时候可能会没有 GPS 信号,或者 GPS 信号不够稳定;比如说桥下,室内以及高楼林立的城市内。那么为了能够允许无人机在这些环境中飞行,我们需要提供其它位置估计手段,比如说SLAM。SLAM使用摄像头作为其主要传感器,传感器可以是单目摄像头,双目摄像头以及RGBD深度摄像头。这几种摄像头有着其对应的优缺点;比如说单目摄像头虽然价格低,体积小,但是它需要其它传感器的辅助,否则无法计算环境深度,也就没有办法为SLAM提供尺度信息;双目摄像头可以通过三角测量提供景深,但是计算量较大,并且需要良好标定;RGBD深度相机可以主动测距,但是对于环境较为敏感,测量距离受限,噪声较大,同时对于阳光以及玻璃等环境下无法正常工作。本讲我们将使用基于双目的SLAM为PX4提供位置估计,实现在无GPS环境下的定点飞行。
本讲内容在实机上通过测试,可以实现较为稳定的定点,但是还有很大的优化空间。大家没有把握的情况下我不建议实机测试,因为当SLAM信号中断时会有潜在危险。本讲中的内容会通过Gazebo展示,但是实机上的设置方法也非常类似。
本讲内容可以分为三个部分:
  1. 1.
    环境更新及设置;
  2. 2.
    SLAM及使用;
  3. 3.
    Gazebo验证。

1. 环境更新及设置

如果你完成了前两讲内容,到此你已经设置好了ROS,MAVROS,Gazebo,并且编译好了PX4固件。现在通过如下命令克隆GAAS到本地环境:
1
git clone https://github.com/generalized-intelligence/GAAS
Copied!
或者通过如下命令更新GAAS:
1
cd (GAAS_PATH)
2
git pull origin master
Copied!
将ROS的launch文件拷贝到PX4相应文件夹内:
1
cp -r (GAAS_PATH)/simulator/launch/* (PX4_FIRMWARE_PATH)/Firmware/launch
Copied!
我们最近对Gazebo用到的仿真模型进行了修改,如果你已经完成了前两课,那么你需要将“(PX4_FIRMWARE_PATH)/Firmware/Tools/sitl_gazebo/models" 中的对应模型删除,并重新拷贝:
1
cp -r (GAAS_PATH)/simulator/models/* ~/catkin_ws/src/Firmware/Tools/sitl_gazebo/models/
Copied!

a. 编译SLAM

GAAS使用的SLAM是基于如下项目修改的:
1
https://github.com/gaoxiang12/ygz-stereo-inertial
Copied!
在编译之前,请按照上述项目中的描述安装依赖:
1
Mavros msgs: sudo apt install ros-kinetic-mavros-msgs
2
Pangolin (for visualization): https://github.com/stevenlovegrove/Pangolin
3
Eigen3: sudo apt-get install libeigen3-dev
4
g2o: sudo apt-get install libcxsparse-dev libqt4-dev libcholmod3.0.6 libsuitesparse-dev qt4-qmake
5
glog (for logging): sudo apt-get install libgoogle-glog-dev
6
ROS Octomap: sudo apt-get install ros-kinetic-octomap-*
Copied!
可以采用以下命令安装以上依赖(不包括 Pangolin):
1
sudo apt-get install libeigen3-dev
2
sudo apt-get install ros-kinetic-mavros-msgs
3
sudo apt-get install libgoogle-glog-dev
4
sudo apt-get install ros-kinetic-octomap-*
5
sudo apt-get install libglew-dev
6
sudo apt-get install libxkbcommon-x11-dev
7
sudo apt-get install libqt4-dev libsuitesparse-dev qt4-qmake
8
#以下适用于Ubuntu16.04
9
sudo apt-get install libqglviewer-dev libcholmod3.0.6
10
#以下适用于Ubuntu18.04
11
sudo apt-get install libqglviewer-headers libcholmod3
Copied!
对于Ubuntu 18.04 LTS 系统,将 ros-kinetic- 替换为 ros-melodic- 即可。
注意,你需要单独编译安装Pangolin。
之后,安装DBoW3,PCL, g2o 以及 opencv,项目链接如下:
1
# 记得编译完 make install
2
3
# PCL
4
https://github.com/PointCloudLibrary/pcl
5
6
# DBoW3
7
https://github.com/rmsalinas/DBow3
8
9
# g2o
10
https://github.com/RainerKuemmerle/g2o
11
12
# opencv,建议使用版本高于3.4.5
13
https://github.com/opencv/opencv
Copied!
在编译opencv的过程中,需要确保cmake生成的opencv编译配置中,包括了opencv_cvv组件。为此,你需要首先克隆opencv_contrib:
1
https://github.com/opencv/opencv_contrib
Copied!
进入克隆下来的opencv目录,切换版本到一个较高版本——此处使用版本号3.4.5:
1
git checkout 3.4.5
Copied!
注意,对于opencv和opencv_contrib都需要切换版本,且必须保证两者版本一致。
然后使用cmake进行配置——此处配置较为复杂,若对cmake不熟悉则可以使用cmake-gui:
1
sudo apt-get install cmake-qt-gui
2
cmake-gui
Copied!
首先指定编译目录,点击Configure并选择Unix Makefiles,先进行第一次配置:
指定OPENCV_EXTRA_MODULES_PATH到opencv_contrib,并勾选WITH_QT和ENABLE_CXX11:
再点击一次Configure,即可看到BUILD_opencv_cvv选项,勾选之后点击Generate即可生成目标makefile,进行编译即可。
注意,点击Generate只是在你指定的目录下生成了Makefile,并没有进行编译。你需要进入目录执行make && sudo make install
由于ROS自带了低版本的opencv,为了防止cmake在生成SLAM的makefile的过程中无法找到正确的opencv版本,需要在SLAM的cmake配置文件中指定:
1
cd (GAAS_PATH)/software/SLAM/ygz_slam_ros
2
gedit CMakeLists.txt
Copied!
配置文件的第53行:
find_package(OpenCV REQUIRED)
在OpenCV后面加上版本号(注意不要省略空格),若你安装了其他版本(必须不低于3.4.5),则将3.4.5改为你安装的版本号:
find_package(OpenCV 3.4.5 REQUIRED)
完成后,通过如下命令编译SLAM:
1
cd (GAAS_PATH)/software/SLAM/ygz_slam_ros
2
3
# 编译过程较长,不建议在虚拟机里进行
4
sh generate.sh
Copied!
对于Ubuntu 18.04 LTS ,在编译SLAM时会看到如下警告——这是正常现象,不影响使用:

b. 下载QGroundControl

我们将会使用QGroundControl来设置并查看无人机参数及状态。如果你使用的是UBUNTU 16.04,可以通过如下链接下载:
1
https://s3-us-west-2.amazonaws.com/qgroundcontrol/latest/QGroundControl.AppImage
Copied!
下载后,通过如下命令开启:
1
chmod +x ./QGroundControl.AppImage
2
./QGroundControl.AppImage (或者双击)
Copied!
你也可以通过如下链接查看更多信息:
1
https://docs.qgroundcontrol.com/en/getting_started/download_and_install.html
Copied!
如果你已经成功编译好了ygz_slam_ros并且下载完QGroundControl,你可以继续到下一部分。

2. SLAM及使用

在 ygz_slam_ros/examples文件夹内,你可以看到一个名为simulationCamera.yaml的配置文件,此文件是SLAM的配置文件,文件包含ROS的左右图像topic名,相机内参以及一些其它参数。其中一部分内容如下:
1
%YAML:1.0
2
3
#--------------------------------------------------------------------------------------------
4
# Camera Parameters. Adjust them!
5
#--------------------------------------------------------------------------------------------
6
7
8
# camera and imu topics
9
Left: /gi/simulation/left/image_raw
10
Right: /gi/simulation/right/image_raw
11
12
# not used for now
13
Imu: /mavros/imu/data
14
15
# Camera calibration and distortion parameters (OpenCV)
16
# if running in pure stereo vision mode
17
PureVisionMode: true
18
19
# do we need visualization?
20
UseViewer: false
21
22
# display mappoints?
23
displayMapPoints: false
24
25
Camera.fx: 376.0
26
Camera.fy: 376.0
27
Camera.cx: 376.0
28
Camera.cy: 240.0
29
30
Camera.k1: 0.0
31
Camera.k2: 0.0
32
Camera.p1: 0.0
33
Camera.p2: 0.0
Copied!
如果想测试自己的相机,请更改相机topic,注意此时并没有使用IMU信息,并且此项目还未完成,会有不稳定情况出现。

3. Gazebo验证

现在让我们继续到Gazebo仿真部分。
首先通过如下命令开启仿真环境:
1
roslaunch px4 slam.launch
Copied!
一个Gazebo窗口将会打开,一架无人机会出现在一个空旷环境内,如我在前几课反复强调,一定记得查看MAVROS的连接情况:
1
rostopic echo /mavros/state
Copied!
确保输出的结果为"connected: True"。
若Gazebo此步没有发布图像,请尝试 sudo apt install ros-kinetic-gazebo-control
继续之前,如果你打开slam.launch文件,你会发现如下参数:
1
<arg name="vehicle" default="iris_stereo_gray_no_gps"/>
Copied!
你可以看到我们使用的是"iris_stereo_gray_no_gps" 无人机模型,在此模型中我已经将GPS模块删除了,所以如果你现在通过如下命令打开QGroundControl:
1
./QGroundControl.AppImage # if you downloaded AppImage
Copied!
Fig 1, QGroundControl
QGroundControl 在打开后会自动连接到Gazebo仿真,同时在窗口中你可以看到"No GPS Lock for Vehicle", 这代表无人机中的GPS已被禁用,我们将会使用SLAM作为水平位置估计手段。
运行 Gazebo 时,如果一切正常但出现以上报错信息,该报错信息并不会影响 Gazebo 运行,可以继续进行到下一步。
如果您知道如何解决以上报错,欢迎在 GAAS GitHub 页面提交 PR,帮助我们减少不必要的报错信息。
接下来,开启SLAM:
1
./bin/EurocStereoVIO_ros ./examples/simulationCamera.yaml
Copied!
或者也可以运行:
1
sh run.sh
Copied!
此时会弹出一个窗口,你可以看到机载摄像头中的实时画面如下图所示,注意图中的彩色特征点,此时SLAM已经初始化,否则请确保你已经更新了无人机模型,否则SLAM将不会正常初始化。
Fig 2, SLAM initialized
在QGroundControl中,点击左上角的齿轮按钮,选择最下方的"Parameters", 在搜索栏里输入“vision”,将“EKF2_AID_MASK”的参数改为“vision position fusion",对应值为 “8”。点击“save”保存参数更改。
Fig 3, QGroundContrl, select vision position fusion
我们只会使用SLAM信息来估计水平位置移动,忽略SLAM的YAW信息以及高度信息,并且使用PX4的气压计作为定高手段。
接下来,点击“Clear”,选择“Tools”,选择“reboot vehicle”重启无人机,使参数设置生效。
实际上此时你不需要重启无人机即可使得参数生效,但是为了教程的完整性(尤其是在实机测试的时候),我将重启也作为了这里的一步。
在终端中,输入:
1
rostopic echo /mavros/vision_pose/pose
Copied!
你会看到当前的实时SLAM位置估计信息:
1
---
2
header:
3
seq: 1489
4
stamp:
5
secs: 3453
6
nsecs: 368000000
7
frame_id: ''
8
pose:
9
position:
10
x: 0.000944685346145
11
y: -0.00012923774903
12
z: 0.000286279467091
13
orientation:
14
x: 0.0
15
y: 0.0
16
z: 0.0
17
w: 0.0
18
---
Copied!
接下来,进入到“(GAAS_PATH)/demo/tutorial_3”文件夹,起飞无人机:
1
python px4_mavros_run.py
Copied!
待无人机悬停后,在另外一个终端,通过如下脚本控制无人机飞一个 3x3 米的正方形:
1
python square.py
Copied!
你可以看到无人机能够实现较为理想的位置估计,并且无人机可以依赖SLAM信息实现较为复杂的任务。
Fig 4, SLAM position history
完成预定轨迹后,无人机会降落在地面上,你可以通过SLAM窗口实时观察运行轨迹。
以上内容在实机上进行过测试,但是表现不稳定,为了实现较为理想的效果,我们需要花费一定精力进行调参并改善SLAM效果。在实机测试过程中我们发现了如下问题:
  1. 1.
    SLAM有时会出错并终止运行,此时无人机会跳到“ALTITUDE”模式;
  2. 2.
    有时会出现无人机漂移情况,尽管相关参数已经设定并且SLAM处于运行状态;
  3. 3.
    快速运动情况下定位精度不理想。
如果你想在实机上测试,典型过程如下:
  1. 1.
    进行良好的摄像头标定;
  2. 2.
    开启SLAM;
  3. 3.
    启动MAVROS,确保连接正常;
  4. 4.
    在QGroundControl中设置相关参数;
  5. 5.
    确保 /mavros/local_position/pose 发布的位置信息是有“意义”的,此信息是飞控融合SLAM信息后的位置信息,如果xy方向的值处于 10^-20 的范围,那么此时的位置估计是没有意义的,你需要检查MAVROS连接,QGroundControl参数设定以及SLAM状态;
  6. 6.
    通过遥控器起飞无人机后切换到定点模式,或者通过提供的python脚本起飞无人机。
在Gazebo验证完成前,以及在有合适场地前,不要盲目实机测试。
总而言之,本讲中我们讲述了如何使用SLAM,如何通过QGroundControl设置参数,如何在Gazebo中进行无GPS环境下的无人机飞行控制并完成了一个简单任务。
凡在本教程出现的信息,均仅供参考。本教程将尽力确保所提供信息的准确性及可靠性,但不保证有关资料的准确性及可靠性;任何实际机器测试前请尽可能多地进行测试,并对任何自主决定的行为负责。本教程对有关资料所引致的错误、不确或遗漏,概不负任何法律责任(包括侵权责任、合同责任和其它责任)。
Last modified 1yr ago