Update README
This commit is contained in:
parent
e916578014
commit
46c1d143d2
|
@ -245,7 +245,7 @@ body_T_cam1: !!opencv-matrix
|
|||
|
||||
# 常见的可调节参数及其效果
|
||||
|
||||
## VINS相关
|
||||
## VINS相关(视觉自主定位)
|
||||
|
||||
### 相机帧率
|
||||
|
||||
|
@ -271,7 +271,7 @@ body_T_cam1: !!opencv-matrix
|
|||
- 当未开启多线程时,前端的耗时为特征提取与位姿解算两者之和;
|
||||
- 当开启多线程时, 前端的耗时为特征提取与位姿解算两者中较长的那个。
|
||||
|
||||
## ego-planner相关
|
||||
## ego-planner相关(自主避障规划)
|
||||
|
||||
### 膨胀半径
|
||||
|
||||
|
@ -298,6 +298,17 @@ src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/
|
|||
- 当球心高度比实际低,气球机器人可能碰到高处的障碍物(如天花板等);
|
||||
- 当球心高度比实际高,气球机器人可能碰到低处的障碍物(如地面或气球机器人试图越过的障碍物等)。
|
||||
|
||||
*若需要修改此参数,修改完需要重新编译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看来,前方已经被障碍物完全遮挡而无法规划出可行轨迹。
|
||||
|
||||
虚拟地面的情况与之类似。
|
||||
|
||||
### 速度、加速度、角速度限制
|
||||
|
||||
|
@ -342,3 +353,23 @@ src/ego-planner/planner/plan_manage/launch/advanced_param_exp.xml中的grid_map/
|
|||
* 保持`estimate_extrinsic`为1的情况下,VINS会持续对外参进行估计,但相当于在一个优化问题中引入了更多的自由变量,在不良工况下发散的可能性会更大一些
|
||||
* 机体转动过快时更容易发散,因为快速的转动比快速平移时画面中特征点的位移更大、画面的动态模糊更严重,此时光流追踪更容易失败
|
||||
* 当摄像头朝向缺乏纹理的白墙、容易反光的玻璃/镜子、具有大量重复纹理的墙纸/地砖或场景中存在大量运动物体时,VINS较容易发散
|
||||
|
||||
## Ego-Planner自主运动中可能出现的问题
|
||||
|
||||
### 无法规划可行轨迹
|
||||
|
||||
* 启动ego-planner后,在可视化界面中发现前方几乎全是障碍物,将可行空间堵死而无法规划出可行轨迹
|
||||
1. 检查膨胀半径是否过大
|
||||
2. 检查虚拟天花板设置是否过低
|
||||
|
||||
* 前方有可行空间,ego-planner一直在尝试求解但始终无法得到可行解
|
||||
1. 检查所设置的目标点是否位于障碍物区域内,如果目标点被障碍物(膨胀之后的)包裹,则无论如何也无法规划出不碰撞的可行轨迹
|
||||
|
||||
### 飞行轨迹不合理
|
||||
|
||||
* 左右侧有可行空间,但飞艇直接向上飞,甚至撞到天花板
|
||||
1. 检查虚拟天花板是否过高,无法起到限制作用
|
||||
|
||||
* 飞艇在飞行过程中,轨迹超调严重,轨迹跟踪精度不足
|
||||
1. 检查速度、加速度上限是否设置过高;
|
||||
2. 检查飞艇重量,“飞艇自身+气囊升力+配重”全部抵消之后的净重力维持在10~20g为最佳状态,净重过大会严重影响控制性能,可以通过称重判断是否存在此问题,也可以通过飞艇原地悬停时螺旋桨转速来判断。
|
||||
|
|
|
@ -202,17 +202,6 @@ void pose_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
|
|||
*/
|
||||
}
|
||||
|
||||
// void vins_fail_callback(const std_msgs::Bool::ConstPtr &vins_fail)
|
||||
// {
|
||||
// if (vins_fail)
|
||||
// {
|
||||
// geometry_msgs::PoseStamped odometry_mavros_vision_pose;
|
||||
// odometry_mavros_vision_pose.header = pose_msg->header;
|
||||
// odometry_mavros_vision_pose.header.frame_id = "world";
|
||||
// odometry_mavros_vision_pose.covariance
|
||||
// }
|
||||
// }
|
||||
|
||||
void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
|
||||
{
|
||||
//ROS_INFO("vio_callback!");
|
||||
|
@ -256,27 +245,17 @@ void vio_callback(const nav_msgs::Odometry::ConstPtr &pose_msg)
|
|||
init_pos << init_position[0], init_position[1], init_position[2];
|
||||
Quaterniond q_pub = q_yaw * vio_q;
|
||||
pos_pub = q_yaw * vio_t + init_pos;
|
||||
// Quaterniond q_pub = vio_q * q_yaw;
|
||||
// pos_pub = vio_q * init_pos + vio_t;
|
||||
|
||||
nav_msgs::Odometry odometry;
|
||||
odometry.header = pose_msg->header;
|
||||
odometry.header.frame_id = "world";
|
||||
odometry.pose.pose.position.x = vio_t.x();
|
||||
odometry.pose.pose.position.y = vio_t.y();
|
||||
odometry.pose.pose.position.z = vio_t.z();
|
||||
odometry.pose.pose.orientation.x = vio_q.x();
|
||||
odometry.pose.pose.orientation.y = vio_q.y();
|
||||
odometry.pose.pose.orientation.z = vio_q.z();
|
||||
odometry.pose.pose.orientation.w = vio_q.w();
|
||||
|
||||
// odometry.pose.pose.position.x = pos_pub.x();
|
||||
// odometry.pose.pose.position.y = pos_pub.y();
|
||||
// odometry.pose.pose.position.z = pos_pub.z();
|
||||
// odometry.pose.pose.orientation.x = q_pub.x();
|
||||
// odometry.pose.pose.orientation.y = q_pub.y();
|
||||
// odometry.pose.pose.orientation.z = q_pub.z();
|
||||
// odometry.pose.pose.orientation.w = q_pub.w();
|
||||
odometry.pose.pose.position.x = pos_pub.x();
|
||||
odometry.pose.pose.position.y = pos_pub.y();
|
||||
odometry.pose.pose.position.z = pos_pub.z();
|
||||
odometry.pose.pose.orientation.x = q_pub.x();
|
||||
odometry.pose.pose.orientation.y = q_pub.y();
|
||||
odometry.pose.pose.orientation.z = q_pub.z();
|
||||
odometry.pose.pose.orientation.w = q_pub.w();
|
||||
pub_odometry_rect.publish(odometry);
|
||||
|
||||
#ifdef SEND_ODOMETRY_RECT_MAVROS
|
||||
|
@ -560,7 +539,6 @@ int main(int argc, char **argv)
|
|||
ros::Subscriber sub_margin_point = n.subscribe("/vins_estimator/margin_cloud", 2000, margin_point_callback);
|
||||
ros::Subscriber sub_land_trigger = n.subscribe("/land_trigger", 2000, land_trigger_callback);
|
||||
|
||||
// ros::Subscriber sub_vins_failure = n.subscribe("/vins_fail", 2000, vins_fail_callback);
|
||||
|
||||
pub_match_img = n.advertise<sensor_msgs::Image>("match_image", 1000);
|
||||
pub_camera_pose_visual = n.advertise<visualization_msgs::MarkerArray>("camera_pose_visual", 1000);
|
||||
|
|
Loading…
Reference in New Issue