2006681a5a
|
||
|---|---|---|
| .. | ||
| shfiles | ||
| src | ||
| .catkin_workspace | ||
| README.md | ||
README.md
代码
结构
在Atlas 200I DK A2开发板上的代码结构如下：
- blimp-autopilot
- src
- ego-planner
- mavros
- vins-fusion
- shfiles
- devel（编译时自动生成）
- build（编译时自动生成）
- README.md
- src
其中，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
- src
然后在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://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两个变量，其中
- 所有的
ROS_MASTER_URI均须与主机的IP保持一致 - 所有的
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中）
- 开启一个终端，启动roscore
- 开启一个终端，进入blimp-autopilot/shfiles，将vins_run.sh中关于启动realsense的语句注释或删去
roslaunch realsense2_camera rs_camera.launch & sleep 6;，并在这个终端中运行./vins_run.sh，并等待至终端输出“no previous graph” - 开启一个终端，播放rosbag：
rosbag play rosbag_demo.bag，这时将可以看到启动vins_run.sh的终端中会有新的输出 - 开启新的终端，可以使用
rostopic echo /loop_fusion/odometry_rect等指令观察里程计输出，也可以roslaunch odom_visualization vins_vis.launch来进行可视化的观察 - 开启新终端，在确保里程计的topic已经发布之后，在blimp-autopilot/shfiles中执行
./ego_plan.sh，等待出现绿色文字提示规划成功且不断刷新，也可以在可视化窗口中观察到规划的轨迹
运行VINS
理论上的执行顺序
- 配置mavros环境变量，并启动mavros节点
- 配置realsense环境变量，并启动realsense节点
- 配置vins相关的环境变量，并依次启动vins_node与loop_fusion_node
实际上的运行方式
首先进入/root/blimp-autopilot-master/shfiles/ascend/路径下，然后开两个终端分别执行以下操作
- ./mavros.sh
- ./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
- 若发现没有输出，则须排查mavros节点是否成功启动了
- 若有输出，但帧率不对，如帧率为10Hz等，须断电重启，听电调声音判断飞控是否正常开机，如果不起作用，则需要使用QGround地面站软件进一步排除是否为sysconfig.toml文件配置问题
- 没有图像数据
如果IMU数据正常，启动VINS的终端也没有持续输出
wait for imu...，检查realsense是否正常输出图像，使用rostopic hz /mavros/camera/infra1/image_rect_raw来检查图像帧率是否为15Hz
- 如果帧率为0，说明没有读取到图像数据，检查realsense节点是否启动成功，重启此节点并再次尝试，
roslaunch realsense2_camera rs_camera.launch - 如果帧率错误，则检查realsense节点的launch文件是否有问题，具体路径为
VINS_SRC_PATH/realsense-ros-ros1-legacy/realsense2_camera/launch/rs_camera.launch
无法启动mavros或realsense，甚至roscore都无法启动
- 检查多机版ROS配置是否正确
- 在昇腾端使用
ifconfig查看IP地址，查看.bashrc中的ROS_MASTER_URI与ROS_IP是否与之一致。
- 检查ROS相关环境变量是否正确
- 使用
echo $ROS_PACKAGE_PATH检查ROS路径，查看所要运行的包的路径是否被包含在其中
- 检查conda配置中是否启用了base环境
- 若运行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后，在可视化界面中发现前方几乎全是障碍物，将可行空间堵死而无法规划出可行轨迹
- 检查膨胀半径是否过大
- 检查虚拟天花板设置是否过低
- 前方有可行空间，ego-planner一直在尝试求解但始终无法得到可行解
- 检查所设置的目标点是否位于障碍物区域内，如果目标点被障碍物（膨胀之后的）包裹，则无论如何也无法规划出不碰撞的可行轨迹
飞行轨迹不合理
- 左右侧有可行空间，但飞艇直接向上飞，甚至撞到天花板
- 检查虚拟天花板是否过高，无法起到限制作用
- 飞艇在飞行过程中，轨迹超调严重，轨迹跟踪精度不足
- 检查速度、加速度上限是否设置过高；
- 检查飞艇重量，“飞艇自身+气囊升力+配重”全部抵消之后的净重力维持在10～20g为最佳状态，净重过大会严重影响控制性能，可以通过称重判断是否存在此问题，也可以通过飞艇原地悬停时螺旋桨转速来判断。