20 KiB
代码
结构
在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为最佳状态,净重过大会严重影响控制性能,可以通过称重判断是否存在此问题,也可以通过飞艇原地悬停时螺旋桨转速来判断。