前几天分享出我的代码啦，这里开始细表一下导航方面

导航相关的主要参数在gazebo_nav文件夹下面，这里主要内容就在gazebo_nav和ucar_controller两个文件夹里的内容。

首先说一下建图的事情

建图我用的gmapping，都是提前建好一张图。
我前期调车是用的车子跑出来的地图，分辨率是0.05，效果一般，在代码的map文件夹下面。

后来在与鲁东大学的同学交流的时候发现他们用的是仿真建图的，之后我也效仿他们用仿真建图，最终得到的0.01分辨率的地图。

这个地图分辨率最终设置为0.01呢，单纯的三觉得这个分辨率下建出来的地图更加好看一些，其它感觉没什么影响。
还有人比较喜欢P图的，我用仿真建好图的时候，有一些地方老是撞，我当时想着P几个障碍物在要撞上的地方，结果发现没什么卵用。。。。。。

然后是定位相关

定位可能是问题最大的地方，很多同学说定位飘啊之类的。
这里说一下我的理解，官方的定位是通过里程计来的，也就是常说的odom，它是根据车子轮子转速积分来的，我看了一下车上的电机是比较常用的直流减速无刷电机，它的转速不是很快输出的脉冲也十分靠谱，积分效果挺好的。就是在拐弯的时候会偏得十分厉害。

在这个现象出现的很长一段时间里我都非常怀疑是不是轮子打滑之类的原因，我利用RVIZ查看了一下odom、map、base_link的tf坐标系信息，想着如果轮子打滑很严重的话odom坐标系应该会偏离它最开始的位置，跑了几圈发现其实没什么大的漂移。

这让我开始怀疑是否是定位出了问题，我定位用的amcl，我开始怀疑amcl到底行不行，最后调了很长时间的amcl参数，定位效果还是飘，直道上不飘，但是一拐弯就会漂移。
我甚至开始觉得是amcl定位算法的问题，我已经开始准备自己写一个定位了，根据已有的地图，通过odom信息和激光雷达信息来实现定位确实思路挺直接。就在我准备开干的时候，我一直有联系的一个前辈帮我看了一下我车子跑的时候的rviz录屏，他告诉我amcl只是起到一个辅助矫正作用，重点还是odom信息。

于是我暂时放下自己的想法，去研究研究odom话题三怎么来的，我看到了官方代码里的base_driver.cpp，里面关于odom是怎么来的，搜索关键字odom_pub_，可以看到
这里面有两句被我看到了重点，

Vth = (-vw1+vw2+vw3-vw4)/(4*(base_shape_a_+base_shape_b_));
double delta_th = Vth * dt;

这说明车子的篇航信息也是来自于轮子积分，再一对比我之前车子一拐弯就飘得特别厉害，我基本上肯定了锅该由它来背了。
于是我胆子比较大地去修改base_driver.cpp。

首先找到了imu信息，我定义了俩变量从imu获取偏航信息，为什么是两个呢？因为我需要一个初始角度，保证我每次启动的时候odom的偏航角是从0开始的。并且imu信息是0度-360度，odom则是-180度-180度，这个需要转换一下搜索关键字imu_data1和imu_data可以看到，具体操作如下：

首先取出角度信息imu_data1和imu_data，其中imu_data1只取一次作为初始信息

    imu_pub_.publish(imu_data);
    if(imu_flag==0){
        imu_data1=imu_data;
        imu_flag=1;
    }

然后转为角度并且传递给th_，th_就是以后odom用的偏航角。

  double yaw1=tf::getYaw(imu_data1.orientation);
  double yaw2=tf::getYaw(imu_data.orientation);
  double yaw=yaw2 - yaw1;
  if(yaw > 180){
    yaw = yaw - 360;
  }else if(yaw < -180){
    yaw = yaw + 360;
  }
  th_ = yaw;

工作完成！
然后我发现定位问题解决了！
当然也有人发现了偏航的问题，打算用ekf去矫正篇航，但是我考虑了一下发现并不值得尝试。
首先，车子的里程计的x、y方向上的位移计算三这样子的

  double delta_x = (Vx * cos(th_) - Vy * sin(th_)) * dt;
  double delta_y = (Vx * sin(th_) + Vy * cos(th_)) * dt;
  x_  += delta_x;
  y_  += delta_y;

很明显它俩依赖于th_，用ekf可以从odom外部把篇航修改过来，但是对于th_的值它还是通过轮子积分来的，说白了，不靠谱的照样不靠谱。

其次，我觉得在车跑完赛道用时不到一分钟的情况下，imu是不会漂移的，我测了一下，车上千元级的imu在一分钟里没飘过0.01度，说明imu信息是完全可靠的啊，有完全可靠的信息，为什么还要去融合信息呢。这么短的时间里，把轮子积分的角度丢掉直接换成imu信息我感觉是最可靠的。可能我胆子比较大，所以我直接没有去尝试ekf。

当然如果说通过ekf可以帮我补偿x、y方向上的位移，我肯定会尝试，但是据我所知，貌似x、y方向上的位移找不到其它可以观测的信息，激光雷达算一个，但是激光雷达已经有amcl用上了啊，说白了激光雷达还是是在给x、y方向上的位移做补偿的。

over，关于定位方向上的问题我就是简单粗暴地改了，没有其它特殊操作。
最高端的食材往往采用最朴素的烹饪方式～

然后就是导航参数啦

导航首先说一下我的movebase方案，我的全局路径规划用的是dijkstra，本地规划器用的是teb，定位用的amcl。

具体参数可以看我的代码。
这里我赶重要的说。

先说一下代价地图和全局规划
我是先调的代价地图的，因为jetsonnano计算量有限，我发现代价地图的膨胀半径和代价比例系数对这个影响很大，重要参数如下

#代价比例系数，越大则代价值越小
cost_scaling_factor: 25

#将机器人的几何参数告诉导航功能包集，footprint表示多边形面积，robot_radius表示圆形
#footprint: [[0.1, 0.15], [-0.1, 0.15], [-0.1, -0.15], [0.1, -0.15]]
robot_radius: 0.01
#膨胀半径，扩展在碰撞区域以外的代价区域，给定机器人与障碍物之间必须要保持的最小距离0.20
inflation_radius: 0.25

在代码里我都有注释的。cost_scaling_factor和inflation_radius一起调，因为赛道是50cm宽，为了让全局路径始终保持在中央，我把膨胀半径设置为了2.5，同时cost_scaling_factor越大占用的计算量越小，这个具体原因我就不知道了，总之就是在inflation_radius为2.5的时候cost_scaling_factor尽量大，但是如果cost_scaling_factor太大会导致实际的膨胀半径没有inflation_radius设置的那么大。

robot_radius是机器人体型相关参数，我这里设置的非常小，具体原因三因为赛道弯道比较多，而且挡板之间有比较大的缝隙，在小车走的过程中代价地图有可能会堵住某弯道一侧的路，这个时候会报错无法规划出全局路径车就停了，但是车不过去弯道就没法更新地图，这里意思是先让它过去弯道再说，过去了地图更新了路就通了～
还有一个要点是本地代价地图大小我设置的长宽为2，这个会影响teb的前向模拟距离，而且是取整数的，在设置为2*2时可能车子不能动什么的，这是jetsonnano计算能力不够，因为我teb前向距离是1，所以我这里要比1大，关于计算量就得慢慢省出来了。

全局路径规划相关的参数在base_global_planner_params.yaml，其中比较重要的有

  lethal_cost: 253
  neutral_cost: 1
  cost_factor: 2.0

这三个参数关乎全局路径的顺化程度，可以通过动态态调参工具来调这仨，调到满意为止。

最重要的当然是本地规划器
这里我所说的都是关于TEB的经验。
关于速度和加速度相关参数

  acc_lim_x: 0.15
  acc_lim_y: 0.15
  acc_lim_theta: 2.5
  max_vel_x: 16.25
  max_vel_x_backwards: 16.45
  max_vel_y: 16.25
  max_vel_theta: 16.55

方针十分明确，可能我胆子比较大，我不给它限速，速度限制都随便给，加速度限制上细调。
然后关于转向半径min_turning_radius设置为0.0，因为我用的是麦克纳姆轮嘛，全向车。

关于车子模型我设置为线型，这样会减少许多计算量，不过在拐弯时会有撞的风险，没办法，为了提速我给的线型

  footprint_model:
    #type:"polygon"
    #vertices:[[0.16, 0.11], [-0.16, 0.11], [-0.16, -0.11], [0.16, -0.11]]
    type: "line"
    line_start: [-0.16, 0.0]
    line_end: [0.16, 0.0]

再就是前向距离max_global_plan_lookahead_dist，这个当然是越大越好，前向距离大，走的就越顺畅，在里我设置的是1
然后要关闭并行规划！！！

  enable_homotopy_class_planning: false
  enable_multithreading: false

之前说代价地图设置为2后车子不能动，在关闭这个后问题会得到缓解，它会节省出很大的计算能力。
然后是weight_optimaltime这个参数，这个参数是最优时间权重，如果大了，那么车会在直道上快速加速，并且路径规划的也会切内道，说白了，在不撞的情况下越大越好。
再然后是膨胀半径min_obstacle_dist，这里我给的0.25，teb默认不会采用代价地图的膨胀半径什么的，这也就是我在代价地图里面的那个模型敢给1cm大小的原因。

再然后是weight_kinematics_nh，这个对应的是Y方向运动的权重，在wiki上有说，这个不设置车子是不会横着走的，越小越倾向与横着走，这个我给的200。没有给太小，试了一下，设置了这个以后会顺滑不少，同时amcl也会变准一些（我在amcl里给的是全向模型）。
还有一个重要参数feasibility_check_no_poses，这个是检查距离，这个设置得小一点好，竞速嘛，要啥子安全检测，冲过去就完事儿，之前说的赛道弯道多有缝隙，会堵住路，如果这个值按照默认，会经常报错teb路径不合适导致车子卡在那里不能动。

最后俩参数是global_plan_viapoint_sep和weight_viapoint，官网上有介绍，这个是关于teb路径对全局路径的追踪权重。在国赛之前我没有调这俩参数，weight_optimaltime就只能给25左右否则就会撞上障碍物，我胆子比较大把这俩参数改得比较大，避障效果和顺滑程度都好很多，而且会节省一点计算量。

其它参数

其它参数比如更新频率啊什么的也需要调，不过太细节了，具体可以去看我的代码，上一篇博客https://www.guyuehome.com/35175是我代码的说明。

以上是一些经验，对于算法本身的东西我了解的确实不多，也有人挖苦过我就一调包调参的，确实惭愧，不过我觉得我以后会慢慢学习这些的，毕竟做比赛嘛，还是以实践经验为主

over，希望能得到你们的小星星