Navigation--导航算法（局部视野导航）--DWA、TAB

DWA

        动态窗口法（dynamic window approach，DWA）目前与A*一样都是ROS导航包中提供的基本路径规划算法。DWA是一种贪心的算法，通过可选速度、可选角速度的组合，模拟出很多局部轨迹，然后选择最优的。这种方法时间复杂度很高，而且并不能保证到达目的地，更别说保证最优性了。

        动态窗口法在一定程度上采用了粒子滤波的思想，在速度空间（v,w）中采样多组速度，并模拟出这些速度在一定时间内的运动轨迹，并通过评价函数对这些轨迹进行评价，选取最优轨迹对应的速度驱动机器人运动。

基本思想
1.在机器人的控制空间中离散采样 (dx,dy,dtheta)
2.对于每个采样速度，从机器人的当前状态执行前向模拟，以预测如果采样速度应用于某个（短）时间段会发生什么。
3.使用包含以下特征的度量来评估前向模拟产生的每个轨迹：与障碍物的距离、与目标的距离、与全局路径的距离和速度，排除非法轨迹（与障碍物相撞的轨迹）。
4.选择得分最高的轨迹并将相关的速度发送到移动基地。
5.重复执行上述步骤。

优点
        计算简单
        适用于差分和全向车模

缺点
        前瞻性不足
        动态效果差
        不适用于阿克曼模型车模

DWAPlannerROS:
# Robot Configuration Parameters 
  max_vel_x: 0.45  #x方向最大线速度绝对值，单位:米/秒
  min_vel_x: 0 #x方向最小线速度绝对值，负数代表可后退，单位:米/秒
  max_vel_y: 0.0  # #y方向最大线速度绝对值，单位:米/秒。差分驱动机器人为0    
  min_vel_y: 0.0  # #y方向最小线速度绝对值，单位:米/秒。差分驱动机器人为0   
  max_vel_trans: 0.5 #   #机器人最大平移速度的绝对值，单位为 m/s
  min_vel_trans: 0.1  # #机器人最小平移速度的绝对值，单位为 m/s 不可为零  
  trans_stopped_vel: 0.1  #机器人被认属于“停止”状态时的平移速度。如果机器人的速度低于该值，则认为机器人已停止。单位为 m/s
  max_vel_theta: 0.7  #机器人的最大旋转角速度的绝对值，单位为 rad/s 
  min_vel_theta: 0.3  # 器人的最小旋转角速度的绝对值，单位为 rad/s
  theta_stopped_vel : 0.4 #机器人被认属于“停止”状态时的旋转速度。单位为 rad/s  
  acc_lim_x: 0.5 # 机器人在x方向的极限加速度，单位为 meters/sec^2
  acc_lim_theta: 3.5 #机器人的极限旋转加速度，单位为 rad/sec^2
  acc_lim_y: 0.0      #  机器人在y方向的极限加速度，对于差分机器人来说当然是0
# Goal Tolerance Parameters目标距离公差参数
  yaw_goal_tolerance: 0.15 #到达目标点时，控制器在偏航/旋转时的弧度容差(tolerance)。即：到达目标点时偏行角允许的误差，单位弧度
  xy_goal_tolerance: 0.2  # 到到目标点时，控制器在x和y方向上的容差（tolerence）（米）。即：到达目标点时,在xy平面内与目标点的距离误差
  # latch_xy_goal_tolerance: false #设置为true时表示：如果到达容错距离内,机器人就会原地旋转；即使转动是会跑出容错距离外。

# Forward Simulation Parameters前向模拟参数
  sim_time: 1.8      # 前向模拟轨迹的时间，单位为s(seconds) 
  vx_samples: 6       # x方向速度空间的采样点数
  vy_samples: 0       # y方向速度空间采样点数.。差分驱动机器人y方向永远只有1个值（0.0）
  vtheta_samples: 20  # 旋转方向的速度空间采样点数

# Trajectory Scoring Parameters
  path_distance_bias: 100.0      #控制器与给定路径接近程度的权重
  goal_distance_bias: 24.0      #控制器与局部目标点的接近程度的权重，也用于速度控制
  occdist_scale: 0.5            # 控制器躲避障碍物的程度
  forward_point_distance: 0.325 #以机器人为中心，额外放置一个计分点的距离
  stop_time_buffer: 0.1         #机器人在碰撞发生前必须拥有的最少时间量。该时间内所采用的轨迹仍视为有效。即：为防止碰撞,机器人必须提前停止的时间长度
  scaling_speed: 0.25           #开始缩放机器人足迹时的速度的绝对值，单位为m/s。
  max_scaling_factor: 0.2       #最大缩放因子。max_scaling_factor为上式的值的大小。

# Oscillation Prevention Parameters
  oscillation_reset_dist: 0.05  # 0.05   #机器人必须运动多少米远后才能复位震荡标记(机器人运动多远距离才会重置振荡标记)
  oscillation_reset_angle: 0.05

# Debugging调试参数
  publish_traj_pc : true #将规划的轨迹在RVIZ上进行可视化
  publish_cost_grid_pc: true   #将代价值进行可视化显示
  global_frame_id: map #全局参考坐标系

# Differential-drive robot configuration - necessary?
#  holonomic_robot: false

TEB

        TEB（Time Elastic Band）算法的思想是将路径看作橡皮筋，然后用外力控制形变，其思想类似于样条曲线和Bezier曲线，因此也有不方便考虑避障的特点。

        时间弹性带(TEB)简而言之，就是连接起始、目标点，并让这个路径可以变形，变形的条件就是将所有约束当做橡皮筋的外力。起始点、目标点状态由用户/全局规划器指定，中间插入N个控制橡皮筋形状的控制点（机器人姿态），当然，为了显示轨迹的运动学信息，我们在点与点之间定义运动时间Time。

        虽然待优化的橡皮筋有不少状态点与时间段，目标函数也好像很多。但是，每个目标函数只与橡皮筋中的某几个状态有关，而非整条橡皮筋。

优点
前瞻性好
适用于各种车模

缺点
计算复杂
速度和角速度波动大，控制不稳定

TebLocalPlannerROS:

 odom_topic: /odom
 map_frame: odom_combined

 #Trajectory
 teb_autosize: True #优化期间允许改变轨迹的时域长度 default:True
 dt_ref: 0.45 #局部路径规划的解析度# minimum 0.01 default:0.3  0.01~1.0
 dt_hysteresis: 0.1 #允许改变的时域解析度的浮动范围， 一般为 dt_ref 的 10% 左右 default:0.1  0.002~0.5
 global_plan_overwrite_orientation: False #覆盖全局路径中局部路径点的朝向 default:True
 #allow_init_with_backwards_motion:False #允许在开始时想后退来执行轨迹 default:False
 max_global_plan_lookahead_dist: 3.0 #考虑优化的全局计划子集的最大长度 （累积欧几里得距离）（如果为0或负数：禁用；长度也受本地Costmap大小的限制） default:3.0  0.0~50.0
 feasibility_check_no_poses: 5 #检测位姿可到达的时间间隔 default:5 0~50

 # Robot
 max_vel_x: 0.5 #最大x前向速度
 max_vel_y: 0.3 #最大y前向速度，非全向移动小车需要设置为0
 max_vel_x_backwards: 0.5 #最大后退速度
 max_vel_theta: 1.5  #最大转向角速度
 acc_lim_x: 0.2  #最大x向加速度
 acc_lim_y: 0.2  #最大y向加速度，非全向移动小车需要设置为0
 acc_lim_theta: 0.3  #最大角加速度

 #阿克曼小车参数，非阿克曼小车设置为0
 min_turning_radius: 0.0   #车类机器人的最小转弯半径 default:0.0   0.0~50.0

 wheelbase: 0.0  #驱动轴和转向轴之间的距离（仅适用于启用了“Cmd_angle_而不是_rotvel”的Carlike机器人）；对于后轮式机器人，该值可能为负 default:1.0  -10.0~10.0
 #wheelbase: 0.143 #for mini_akm
 #wheelbase: 0.320 #for senior_akm
 #wheelbase: 0.503 #for top_akm_bs
 #wheelbase: 0.549 #for top_akm_dl

 cmd_angle_instead_rotvel: False #将收到的角速度消息转换为 操作上的角度变化。 default:False
                                 #true则cmd_vel/angular/z内的数据是舵机角度

 footprint_model: # types: "point", "circular", "two_circles", "line", "polygon" 默认“point”类型
  type: "polygon"   #多边形类型for akm/mec，默认”point”。

  #麦轮
  vertices: [[-0.133, -0.125], [-0.133, 0.125],[0.133,0.125],[0.133, -0.125]] #多边形端点坐标 for mini_mec

  #全向轮
  #radius: 0.12 #for mini_omni

 xy_goal_tolerance: 0.2   #目标 xy 偏移容忍度 default:0.2 minimum 0.001 maximum 0.2
 yaw_goal_tolerance: 0.1 #目标 角度 偏移容忍度 default:0.1  minimum 0.001 maximum 0.1
 free_goal_vel: False  #允许机器人以最大速度驶向目的地 default:False
 complete_global_plan: True
 
 # Obstacles
 min_obstacle_dist: 0.1 #和障碍物最小距离 default:0.5  0.0~10.0
 include_costmap_obstacles: True  #是否将动态障碍物预测为速度模型，costmap 中的障碍物是否被直接考虑 default:True  
 costmap_obstacles_behind_robot_dist: 1.5  #限制机器人后方规划时考虑的局部成本地图障碍物 default:1.5  0.0~20.0
 obstacle_poses_affected: 15   #障碍物姿态受影响0~30 default:30  0~200
 costmap_converter_plugin: ""
 costmap_converter_spin_thread: True
 costmap_converter_rate: 5
 include_dynamic_obstacles: True   #是否将动态障碍物预测为速度模型 default False
 dynamic_obstacle_inflation_dist: 0.6 # 动态障碍物的膨胀范围 default:0.6  0.0~15.0

 # Optimization  
 no_inner_iterations: 5  #被外循环调用后内循环执行优化次数 default:5  1~100
 no_outer_iterations: 4  #执行的外循环的优化次数 default:4  1~100
 optimization_activate: True #激活优化 default:True
 optimization_verbose: False # 打印优化过程详情
 penalty_epsilon: 0.1 #对于硬约束近似，在惩罚函数中添加安全范围 default:0.1  0.0~1.0
 obstacle_cost_exponent: 4
 weight_max_vel_x: 2  #最大x速度权重 default:2.0  0.0~1000.0
 weight_max_vel_theta: 1 #最大速度权重 default:1.0  0.0~1000.0
 weight_acc_lim_x: 1  #最大x 加速度权重 default:1.0  0.0~1000.0
 weight_acc_lim_theta: 1  #最大角速度权重 default:1.0  0.0~1000.0
 weight_kinematics_nh: 1000 #满足非完整运动学的最优权 default: 1000.0  0~10000.0
 weight_kinematics_forward_drive: 1   #优化过程中，迫使机器人只选择前进方向，差速轮适用 default:1.0  0~1000.0
 weight_kinematics_turning_radius: 1  #优化过程中，车型机器人的最小转弯半径的权重。 default:1.0   0.0~1000
 weight_optimaltime: 1 # must be > 0  #优化过程中，基于轨迹的时间上的权重 default:1.0  0~1000
 weight_shortest_path: 0 #
 weight_obstacle: 100
 weight_inflation: 0.2   #优化过程中， 膨胀区的权重 default:0.1  0.0~10.0
 weight_dynamic_obstacle: 10 # not in use yet 优化过程中，和动态障碍物最小距离的权重 default:50.0  0.0~1000
 weight_dynamic_obstacle_inflation: 0.2  #优化过程中，和动态障碍物膨胀区的权重  default 1.0  0.0~1000
 weight_viapoint: 1  #优化过程中，和全局路径采样点距离的权重  default:1.0  0.0 1000
 weight_adapt_factor: 2  #default 2.0  1.0~100

 # Homotopy Class Planner
 enable_homotopy_class_planning: False 
 enable_multithreading: True #允许多线程并行处理 default: True
 max_number_classes: 4 # 允许的线程数 default:5 1~100
 selection_cost_hysteresis: 1.0 
 selection_prefer_initial_plan: 0.95
 selection_obst_cost_scale: 1.0
 selection_alternative_time_cost: False

 roadmap_graph_no_samples: 15
 roadmap_graph_area_width: 5
 roadmap_graph_area_length_scale: 1.0
 h_signature_prescaler: 0.5
 h_signature_threshold: 0.1
 obstacle_heading_threshold: 0.45
 switching_blocking_period: 0.0
 viapoints_all_candidates: True
 delete_detours_backwards: True
 max_ratio_detours_duration_best_duration: 3.0
 visualize_hc_graph: False
 visualize_with_time_as_z_axis_scale: False

 # Recovery
 
 shrink_horizon_backup: True #当规划器检测到系统异常，允许缩小时域规划范围。default True
 shrink_horizon_min_duration: 10
 oscillation_recovery: False  #尝试检测和解决振荡 default:True
 oscillation_v_eps: 0.1
 oscillation_omega_eps: 0.1
 oscillation_recovery_min_duration: 10
 oscillation_filter_duration: 10

MPC

        MPC其实是一种基于对受控对象进行预测的控制方法。MPC最大的特点在于，相对于LQR控制而言，MPC可以考虑空间状态变量的各种约束，而LQR，PID等控制只能够考虑输入输出变量的各种约束。

​         MPC的作用机理可以表述为：在每一个采样时刻，根据当前的测量信息，在线求解一个有限时间开环优化问题，并将得到的控制序列的第一个元素用于被控对象；在下一个采样时刻，用新的测量值作为此时预测系统未来动态的初试条件，刷新优化问题求解。应用于机器人的典型的模型预测控制方法：

        MPC(Model Predictive Control)与上文提到的DWA和TEB算法不同，MPC只是一个控制器，在自动驾驶领域，其与PID控制器一样，控制器输入包括车辆下一步的运行轨迹，车辆的当前状态，输出是速度和转角。不同之处在于，PID控制器是实时处理当前车辆与目标轨迹的差距来调整输出，使车辆接近目标轨迹，而MPC控制器将未来一个时间段 t 分成 N 个节点，预测每个节点的车辆状态，再调整控制器的输出使车辆尽可能接近参考轨迹。相比于PID控制器的单输入单输出特性，模型预测控制更加适用于多输入多输出的复杂控制系统，可以通过调参，使得车辆的控制更加平稳、更接近于期望轨迹等。

参考：ROS与navigation教程-目录 - 创客智造 (ncnynl.com)https://www.ncnynl.com/archives/201708/1880.html