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: falseTEB
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
![[python]初步练习脚本](https://img-blog.csdnimg.cn/268fecf5c30540bf82d96b47e0bfc418.png)
