athena/Blimp-Autopilot-Ascend/README.md

383 lines
20 KiB
Markdown
Raw Permalink Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

# 代码
## 结构
在Atlas 200I DK A2开发板上的代码结构如下
- blimp-autopilot
- src
- ego-planner
- mavros
- vins-fusion
- shfiles
- devel编译时自动生成
- build编译时自动生成
- README.md
其中blimp-autopilot建议放在home目录下如使用root登录则可以放在root下
由于实际运行的时候飞艇在空中不方便接HDMI线无法直接运行开发板上的可视化组建因此可以通过配置多机版ROS通过局域网连接在在本地计算机上可视化显示里程计、建图、规划效果具体操作为
将/src/vins-fusion/utils/l路径下的odom_visualization、pose_utils、quadrotor_msgs这三个目录拷贝至本地进行编译然后在配置好多机ROS相关IP的情况下在本地启动`roslaunch odom_visualization vins_vis.launch`
在本地运行可视化的工程目录结构:
- odom_vis_ws
- src
- odom_visualization
- pose_utils
- quadrotor_msgs
然后在odom_vins_ws路径下执行`catkin_make`即可。编译成功后将`source`此工程`build/setup.bash`的语句加入`.bashrc`即可。
## 依赖
### Mavlink
此工程中包括mavros但对其中一个文件vision_pose_estimate.cpp做了修改编译此工程即可提供mavros相关功能。
mavros依赖mavlink二进制安装mavlink的方式为
```
sudo apt-get install ros-noetic-mavlink
```
安装完成之后使用`rospack list`查看是否已经有mavlink
### Ceres
参见:
https://zhuanlan.zhihu.com/p/460685629
### Boost_python3
参见:
https://blog.csdn.net/zsf10220208/article/details/102571611#:~:text=%281%29%20%E5%8E%BBboost%E5%AE%98%E7%BD%91%20https%3A%2F%2Fwww.boost.org%2F%20%E4%B8%8B%E8%BD%BD%E8%BE%83%E6%96%B0%E7%89%88%E6%9C%AC%E7%9A%84boost%2C%E5%A6%82boost_1_71_0.zip.%20%282%29%20%E8%A7%A3%E5%8E%8B%3A%20unzip%20boost_1_71_0.zip,sudo.%2Fb2%20install%20%28d%29%20sudo%20ldconfig%20%284%29%20%E7%BC%96%E8%AF%91%E6%88%90%E5%8A%9F%E5%90%8E%E5%8F%AF%E4%BB%A5%E5%9C%A8%E9%BB%98%E8%AE%A4%E7%BC%96%E8%AF%91%E8%B7%AF%E5%BE%84%2Fusr%2Flocal%2Flib%E4%B8%8B%E6%89%BE%E5%88%B0%E7%9B%B8%E5%85%B3%E7%9A%84libboost%2A.so%2C%20boost%E5%9C%A8%2Fusr%2Flocal%2Finclude%2Fboost.
https://boostorg.jfrog.io/artifactory/main/release/1.71.0/source/
### OpenCV 3.4
https://github.com/opencv/opencv/tree/3.4
下载源码解压之后进入目录新建build目录在build目录下执行
```
cmake .. -DCMAKE_BUILD_TYPE=RELEASE -DCMAKE_INSTALL_PREFIX=/usr/local/opencv34 -DWITH_LIBV4L=ON -DWITH_CUDA=OFF -DWITH_TBB=ON -DWITH_OPENMP=ON -DWITH_OPENGL=ON
make -j8
sudo make install
```
### RealsenseSDK
https://github.com/IntelRealSense/librealsense/releases
下载源码解压之后进入目录新建build目录在build目录下执行
```
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8
sudo make install
```
## 编译
在blimp-autopilot下执行`catkin_make -DCMAKE_BUILD_TYPE=Release`即可执行编译
- 不加`-DCMAKE_BUILD_TYPE=Release`也可以执行编译,但运行时性能较差
- 根据实际情况,添加`-j2`或`-j8`等选项提升编译速度若不指定默认等于CPU核心数目
# 硬件连接
首先使用串口连接昇腾底板波特率115200使用MobaXterm等工具进行连接。开机过程中需要等待一段时间待出现登录提示时输入账户和密码登录Ubuntu 22.04系统。
登录之后,运行`./wifi_XXX.sh`以连接无线网络。若连接成功将显示其IP地址等信息若连接失败可以重新运行`./wifi_XXX.sh`;若连续失败,可以拔插无线网卡模块并重新运行`./wifi_XXX.sh`,直至连接成功为止。
在无线网络连接成功后确保自己的计算机与昇腾连接在同一网络内使用Ping等方式确认连通性没有问题后使用MobaXterm或VSCode等工具登录昇腾连接没有问题后即可断开串口连接。
# 环境变量及配置
## 环境变量
* roscore的环境变量
* mavros的环境变量
* realsense的环境变量
* VINS的环境变量
* ego_planner的环境变量
* odometry_visualization的环境变量在本地
可以通过在新启动的终端中source相关的devel/setup.bash来实现也可以写入.bashrc来实现
## 多机版ROS需要配置IP
主机侧与从机侧均需要在.bashrc里配置`ROS_MASTER_URI`与`ROS_IP`两个变量,其中
1. 所有的`ROS_MASTER_URI`均须与主机的IP保持一致
2. 所有的`ROS_IP`均须与自身的IP保持一致
# 使用ROSBAG进行离线验证
## 准备工作
如果手头没有完善的硬件设备可以使用ROSBAG在本地计算机上运行VINS-Fusion与Ego-planner验证代码编译是否有问题。
如果需要运行VINS-Fusion需要的rosbag中至少要包括以双目相机+IMU的配置为例
- 左目相机的topic用于vins定位在我们的例子中其名称为/camera/infra1/image_rect_raw
- 右目相机的topic用于vins定位在我们的例子中其名称为/camera/infra1/image_rect_raw
- IMU的topic用于vins定位在我们的例子中其名称为/mavros/imu/data或/mavros/imu/data_raw
而如果需要运行Ego-planner需要的rosbag中至少包括
- 里程计的topic用于轨迹规划在我们的例子中其名称为/loop_fusion/odometry_rect
- 深度相机的topic用于建图与避障在我们的例子中其名称为/camera/depth/image_rect_raw
如果同时运行Vins-Fusion与Ego-planner则可以省去里程计的topic因为VINS-Fusion会输出这个topic
- 左目相机的topic用于vins定位在我们的例子中其名称为/camera/infra1/image_rect_raw
- 右目相机的topic用于vins定位在我们的例子中其名称为/camera/infra1/image_rect_raw
- IMU的topic用于vins定位在我们的例子中其名称为/mavros/imu/data或/mavros/imu/data_raw
- 深度相机的topic用于建图与避障在我们的例子中其名称为/camera/depth/image_rect_raw
运行之前需要确保stereo_imu_config_d435f.yaml中标注的各项topic与rosbag中各个topic能对应上。
## 离线运行
首先应保证所有的环境变量已经配置正确可以在每个新开的终端中source相关的setup.bash也可以写入~/.bashrc中
1. 开启一个终端启动roscore
2. 开启一个终端进入blimp-autopilot/shfiles将vins_run.sh中关于启动realsense的语句注释或删去`roslaunch realsense2_camera rs_camera.launch & sleep 6;`,并在这个终端中运行`./vins_run.sh`并等待至终端输出“no previous graph”
3. 开启一个终端播放rosbag`rosbag play rosbag_demo.bag`,这时将可以看到启动`vins_run.sh`的终端中会有新的输出
4. 开启新的终端,可以使用`rostopic echo /loop_fusion/odometry_rect`等指令观察里程计输出,也可以`roslaunch odom_visualization vins_vis.launch`来进行可视化的观察
5. 开启新终端在确保里程计的topic已经发布之后在blimp-autopilot/shfiles中执行`./ego_plan.sh`,等待出现绿色文字提示规划成功且不断刷新,也可以在可视化窗口中观察到规划的轨迹
# 运行VINS
## 理论上的执行顺序
1. 配置mavros环境变量并启动mavros节点
2. 配置realsense环境变量并启动realsense节点
3. 配置vins相关的环境变量并依次启动vins_node与loop_fusion_node
## 实际上的运行方式
首先进入/root/blimp-autopilot-master/shfiles/ascend/路径下,然后开两个终端分别执行以下操作
1. ./mavros.sh
2. ./vins_run.sh
然后即可使用`rostopic echo /loop_fusion/odometry_rect`来查看里程计数据。
具体来看,这两个脚本分别实现了以下功能:
mavros.sh需要管理员权限
```
source ../devel/setup.bash
sudo chmod 666 /dev/ttyACM0
roslaunch mavros px4.launch
```
vins_run.sh
```
source ../devel/setup.bash
roslaunch realsense2_camera rs_camera.launch
rosrun vins vins_node /root/blimp-autopilot-master/src/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml & sleep 5;
rosrun loop_fusion loop_fusion_node /root/blimp-autopilot-master/src/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml & sleep 1;
```
在启动了VINS之后得到了视觉里程计输出即可使用遥控器实现定点飞行等功能。
# 启动规划
先向下位机发送解锁以及定高指令
take_off.sh
```
source ../devel/setup.bash
rosservice call /mavros/set_mode "base_mode: 0
custom_mode: 'OFFBOARD'"
sleep 5
rosservice call /mavros/set_mode "base_mode: 0
custom_mode: 'AUTO.TAKEOFF'"
```
然后须使用指令`roslaunch ego_planner run_in_exp_435f.launch`来启动ego_planner节点
待其启动之后,发送一条`/traj_start_trigger`消息来触发规划的进行这可以通过ros pub指令来实现
```
rostopic pub -1 /traj_start_trigger geometry_msgs/PoseStamped "header:
seq: 0
stamp:
secs: 0
nsecs: 0
frame_id: ''
pose:
position:
x: 0.0
y: 0.0
z: 0.0
orientation:
x: 0.0
y: 0.0
z: 0.0
w: 0.0"
```
这两步可以合并至一个脚本中实现
ego_plan.sh
```
source ../devel/setup.bash;
roslaunch ego_planner run_in_exp_435f.launch & sleep 8;
./trigger.sh & sleep 1;
```
# 标定外参
修改stereo_imu_config_d435f.yaml中`estimate_extrinsic`为1即“在线标定外参”并将`print_extrinsic`设为1即“在线输出外参”与此同时手动给出粗略的外参至少保定平移变量的符号正确、绝对值大致接近真值。
启动vins并观察其输出`/loop_fusion/odometry_rect`或`/vins_estimator/odometry`确认进程正常启动。拿起机体进行平缓的、充分的运动持续观察里程计输出以及外参输出。当观察到外参达到收敛变化不大且里程计较为准确至少没有发散即可停止vins的运行并将最新的外参值写入stereo_imu_config_d435f.yaml的对应位置
```
body_T_cam0: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [-0.019802,0.096046,0.995180,0.138254,
-0.998751,0.043769,-0.024097,0.033161,
-0.045873,-0.994414,0.095059,-0.040689,
0., 0., 0., 1. ]
body_T_cam1: !!opencv-matrix
rows: 4
cols: 4
dt: d
data: [-0.021758,0.096712,0.995075,0.138524,
-0.998741,0.042888,-0.026006,-0.011314,
-0.045192,-0.994388,0.095657,-0.042557,
0., 0., 0., 1. ]
```
最后将`estimate_extrinsic`和`print_extrinsic`均改为0须注意外参矩阵的格式每一行的开头均需对齐。
# 常见的可调节参数及其效果
## VINS相关视觉自主定位
### 相机帧率
改变相机的原始输出帧率需要通过修改realsense包的launch文件来实现。
/src/vins-fusion/realsense-ros-ros1-legacy/realsense2_camera/launch/rs_camera.launch中的depth_fps与infra_fps分别对应了深度图与灰度图的帧率。
- 当帧率过低VINS的输出帧率也会跟着降低且不够平滑
- 当帧率过高VINS的计算负担过重可能导致实时性变差、待求解数据在队列里堆积内存耗尽、求解失败里程计发散等问题。
### 外参
外参写在src/vins-fusion/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml中是左右目相机在IMU坐标系下的齐次变换矩阵。外参的准确与否很大程度上决定了里程计的准确程度甚至会影响里程计是否发散。参考*外参标定*。
### mod_num
此参数同样写在src/vins-fusion/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml中是一个用于对相机原始图像帧率进行降采样的参数里程计输出的帧率等于相机原始帧率处以mod_num可以在不改变相机原始帧率的情况下对里程计帧率进行修改。
### 多线程
在src/vins-fusion/realflight_modules/VINS-Fusion/config/px4/stereo_imu_config_d435f.yaml中的multiple_thread为0时为单线程模式特征点的提取与基于特征的相对位姿计算在同一线程内完成当此值不为0时特征点的提取与相对位姿解算则分成两个线程进行。
- 当未开启多线程时,前端的耗时为特征提取与位姿解算两者之和;
- 当开启多线程时, 前端的耗时为特征提取与位姿解算两者中较长的那个。
## ego-planner相关自主避障规划
### 膨胀半径
src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/obstacles_inflation
进行轨迹规划时,为了避免机体与障碍物的碰撞,需要对建立好的占据地图进行膨胀,膨胀半径不小于机体最小外接圆的半径。
- 此膨胀半径如果设置过小,则规划出的轨迹会与障碍物相距过近,机体飞行过程中可能与障碍物发生碰撞;
- 此半径如果设置过大,则留给机体的可行空间过小,有可能无法搜索到可行解,或通行效率降低。
### 球心高度
这个高度出现在以下几个位置:
- 在src/ego-planner/planner/plan_manage/src下traj_server.cpp中
- assignment_cmd中对cmd_.position.z减去的高度
- assignment_cmd_mavros中对cmd_mavros_.position.z减去的高度
- 在src/ego-planner/planner/plan_manage/src下ego_replan_fsm.cpp中
- EGOReplanFSM::assignment_cmd中对msg.position.z减去的高度
- EGOReplanFSM::assignment_cmd_mavros中对cmd_mavros.position.z减去的高度
球心高度是指IMU至气球机器人外接球球心的垂直高度它应当与实际相符。
- 当球心高度比实际低,气球机器人可能碰到高处的障碍物(如天花板等);
- 当球心高度比实际高,气球机器人可能碰到低处的障碍物(如地面或气球机器人试图越过的障碍物等)。
*若需要修改此参数修改完需要重新编译ego-planner*
### 虚拟天花板与地板
src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/virtual_ceil为虚拟天花板grid_map/virtual_ground为虚拟地板这两个参数主要用于应对相机视野有限、向前拍摄时无法观察到真实的天花板与地板的问题。通过设置此虚拟天花板在ego-planner建立的局部栅格占据地图中加入虚拟的天花板与地面人为限制轨迹规划的可行空间避免规划器向它看不到但实际有障碍的区域规划。
以虚拟天花板为例:
- 当天花板设置过高,比真实场景中的天花板更高,飞艇如果发现前方被障碍物占据,在看不到上方真实天花板的情况下,可能规划穿入天花板的轨迹来从上方越过障碍物,直到气球已经撞到天花板,由于其视野受限依然无法发现上方的天花板,会继续增大油门向上飞,可能发生危险;
- 当天花板设置过低会严重限制规划器的可行空间导致飞艇前方明明有宽阔的可行空间但在ego-planner看来前方已经被障碍物完全遮挡而无法规划出可行轨迹。
虚拟地面的情况与之类似。
### 速度、加速度、角速度限制
在src/ego-planner/src/planner/plan_manage/launch/run_in_exp_435f.launch中max_acc与max_vel是规划轨迹时的线加速度与线速度上限而src/ego-planner/src/planner/plan_manage/src/ego_replan_fsm.cpp中的max_yaw_dot是气球机器人转动时的角速度上限。
### 目标点设定
在在src/ego-planner/planner/plan_manage/launch/run_in_exp_435f.launch中point_num代表了一共有几个导航点规划开始前气球机器人所处的当前位置不计入总导航点数内然后在下方由0开始编号的即为规划要考虑的导航点若point_num为3则只有point0、point1与point2起作用。目前最多支持5个导航点要添加更多导航点须对源码中其他部分进行对应修改可以参见ego-planner-v2开源代码仓中的说明。
# 常见错误与排除方式
## VINS启动了但没有里程计输出
* 没有IMU数据
观察启动VINS的终端里是否持续输出`wait for imu...`,如果是,则使用`rostopic hz /mavros/imu/data`判断是否有IMU数据以及其帧率是否为200Hz
1. 若发现没有输出则须排查mavros节点是否成功启动了
2. 若有输出但帧率不对如帧率为10Hz等须断电重启听电调声音判断飞控是否正常开机如果不起作用则需要使用QGround地面站软件进一步排除是否为sysconfig.toml文件配置问题
* 没有图像数据
如果IMU数据正常启动VINS的终端也没有持续输出`wait for imu...`检查realsense是否正常输出图像使用`rostopic hz /mavros/camera/infra1/image_rect_raw`来检查图像帧率是否为15Hz
1. 如果帧率为0说明没有读取到图像数据检查realsense节点是否启动成功重启此节点并再次尝试`roslaunch realsense2_camera rs_camera.launch`
2. 如果帧率错误则检查realsense节点的launch文件是否有问题具体路径为`VINS_SRC_PATH/realsense-ros-ros1-legacy/realsense2_camera/launch/rs_camera.launch`
## 无法启动mavros或realsense甚至roscore都无法启动
* 检查多机版ROS配置是否正确
1. 在昇腾端使用`ifconfig`查看IP地址查看.bashrc中的`ROS_MASTER_URI`与`ROS_IP`是否与之一致。
* 检查ROS相关环境变量是否正确
1. 使用`echo $ROS_PACKAGE_PATH`检查ROS路径查看所要运行的包的路径是否被包含在其中
* 检查conda配置中是否启用了base环境
1. 若运行roscore时提示`rosversion roslaunch`相关的错误,观察一下终端每一行命令的最前面是否有`(base)`,若没有,则输入`conda activate base`进行尝试
## VINS发散
造成VINS发散的原因有很多有些是使用不当或程序配置不当导致的属于工程问题可以通过工程手段解决而另一些是使用场景及VINS本身性能极限所导致的这是原理方面的一般只能尽量规避。
* 如果VINS一启动立刻就发散检查其帧率降低帧率进行尝试可以降低realsense的原始图像帧率也可以修改`stereo_imu_config_d435f.yaml`中的`mod_num`
* 如果原地不动VINS不发散但一开始运动就发散检查IMU是否装反或相机朝向是否反了
* 如果平移不发散,而一旋转就发散,或旋转不发散,而一平移就发散,则使用`rostopic echo /mavros/imu/data`观察IMU数据是否正常是否包含线加速度、姿态、角速度完整的部分其符号是否正确、读数范围是否大致正确
* 保持`estimate_extrinsic`为1的情况下VINS会持续对外参进行估计但相当于在一个优化问题中引入了更多的自由变量在不良工况下发散的可能性会更大一些
* 机体转动过快时更容易发散,因为快速的转动比快速平移时画面中特征点的位移更大、画面的动态模糊更严重,此时光流追踪更容易失败
* 当摄像头朝向缺乏纹理的白墙、容易反光的玻璃/镜子、具有大量重复纹理的墙纸/地砖或场景中存在大量运动物体时VINS较容易发散
## Ego-Planner自主运动中可能出现的问题
### 无法规划可行轨迹
* 启动ego-planner后在可视化界面中发现前方几乎全是障碍物将可行空间堵死而无法规划出可行轨迹
1. 检查膨胀半径是否过大
2. 检查虚拟天花板设置是否过低
* 前方有可行空间ego-planner一直在尝试求解但始终无法得到可行解
1. 检查所设置的目标点是否位于障碍物区域内,如果目标点被障碍物(膨胀之后的)包裹,则无论如何也无法规划出不碰撞的可行轨迹
### 飞行轨迹不合理
* 左右侧有可行空间,但飞艇直接向上飞,甚至撞到天花板
1. 检查虚拟天花板是否过高,无法起到限制作用
* 飞艇在飞行过程中,轨迹超调严重,轨迹跟踪精度不足
1. 检查速度、加速度上限是否设置过高;
2. 检查飞艇重量,“飞艇自身+气囊升力+配重”全部抵消之后的净重力维持在1020g为最佳状态净重过大会严重影响控制性能可以通过称重判断是否存在此问题也可以通过飞艇原地悬停时螺旋桨转速来判断。