├── README.md ├── UISEE_Baidu_control_Report.pdf └── path-tracking-zsj ├── ALG_LQR_Apollo.m ├── ALG_LQR_model3.m ├── ALG_MPC.m ├── ALG_PP.m ├── angle_normalization.m ├── apply_kinematics.m ├── calc_heading_angle.m ├── calc_lqr_Apollo_feedbackward.m ├── calc_lqr_feedbackward.m ├── calc_mpc.m ├── calc_nearest_point.m ├── calc_preview_dist.m ├── calc_proj_pose.m ├── calc_steer_feedforward.m ├── cvt_global_to_local.m ├── draw_arrow.m ├── draw_path_tracking.m ├── draw_traj_curvature.m ├── draw_veh_pose.m ├── generate_trajref.m ├── limit_radius.m ├── limit_steer_angle.m ├── limit_steer_by_angular_vel.m ├── load_circle_pp_params.m ├── load_lqr_apollo_params.m ├── load_lqr_model3_params.m ├── load_mpc_params.m ├── load_pp_params.m ├── log_init.m ├── main_path_tracking.m ├── plot_result.m ├── preview_point_on_trajref.m ├── pure_pursuit.m ├── reference.mat ├── self_aligning_control.m ├── set_title_name.m ├── set_trajref_params.m ├── set_y_limits.m ├── solve_lqr_problem.m ├── solve_mpc_problem.m ├── sp10lqr.mat ├── sp10lqrap.mat ├── sp10mpc.mat ├── sp10pp.mat ├── sp4lqr.mat ├── sp4lqrap.mat ├── sp4mpc.mat ├── sp4pp.mat ├── sp6lqr.mat ├── sp6lqrap.mat ├── sp6mpc.mat ├── sp6pp.mat ├── sp9lqrdelay.mat ├── sp9mpcdelay.mat ├── sp9ppdelay.mat ├── update_LQR_matrix.m ├── update_angular_velocity.m ├── update_state_matrix.m ├── update_veh_pose.m └── update_veh_pose_mpc.m /README.md: -------------------------------------------------------------------------------- 1 | # Baidu-Apollo-control-algorithm 2 | Analyzed Baidu Apollo LQR and MPC lateral control algorithm based on dynamic vehicle model 3 | -------------------------------------------------------------------------------- /UISEE_Baidu_control_Report.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/UISEE_Baidu_control_Report.pdf -------------------------------------------------------------------------------- /path-tracking-zsj/ALG_LQR_Apollo.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/ALG_LQR_Apollo.m -------------------------------------------------------------------------------- /path-tracking-zsj/ALG_LQR_model3.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/ALG_LQR_model3.m -------------------------------------------------------------------------------- /path-tracking-zsj/ALG_MPC.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/ALG_MPC.m -------------------------------------------------------------------------------- /path-tracking-zsj/ALG_PP.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/ALG_PP.m -------------------------------------------------------------------------------- /path-tracking-zsj/angle_normalization.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/angle_normalization.m -------------------------------------------------------------------------------- /path-tracking-zsj/apply_kinematics.m: -------------------------------------------------------------------------------- 1 | function [vehicle_pose]=apply_kinematics(vehicle_pose,vehicle_m,rad_speed,deltaT) 2 | 3 | vehicle_pose=vehicle_pose+[vehicle_m*cos(vehicle_pose(3))*deltaT; ... 4 | vehicle_m*sin(vehicle_pose(3))*deltaT; rad_speed*deltaT]; 5 | end -------------------------------------------------------------------------------- /path-tracking-zsj/calc_heading_angle.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_heading_angle.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_lqr_Apollo_feedbackward.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_lqr_Apollo_feedbackward.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_lqr_feedbackward.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_lqr_feedbackward.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_mpc.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_mpc.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_nearest_point.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_nearest_point.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_preview_dist.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_preview_dist.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_proj_pose.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_proj_pose.m -------------------------------------------------------------------------------- /path-tracking-zsj/calc_steer_feedforward.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/calc_steer_feedforward.m -------------------------------------------------------------------------------- /path-tracking-zsj/cvt_global_to_local.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/cvt_global_to_local.m -------------------------------------------------------------------------------- /path-tracking-zsj/draw_arrow.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/draw_arrow.m -------------------------------------------------------------------------------- /path-tracking-zsj/draw_path_tracking.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/draw_path_tracking.m -------------------------------------------------------------------------------- /path-tracking-zsj/draw_traj_curvature.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/draw_traj_curvature.m -------------------------------------------------------------------------------- /path-tracking-zsj/draw_veh_pose.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/draw_veh_pose.m -------------------------------------------------------------------------------- /path-tracking-zsj/generate_trajref.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/generate_trajref.m -------------------------------------------------------------------------------- /path-tracking-zsj/limit_radius.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/limit_radius.m -------------------------------------------------------------------------------- /path-tracking-zsj/limit_steer_angle.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/limit_steer_angle.m -------------------------------------------------------------------------------- /path-tracking-zsj/limit_steer_by_angular_vel.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/limit_steer_by_angular_vel.m -------------------------------------------------------------------------------- /path-tracking-zsj/load_circle_pp_params.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/load_circle_pp_params.m -------------------------------------------------------------------------------- /path-tracking-zsj/load_lqr_apollo_params.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/load_lqr_apollo_params.m -------------------------------------------------------------------------------- /path-tracking-zsj/load_lqr_model3_params.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/load_lqr_model3_params.m -------------------------------------------------------------------------------- /path-tracking-zsj/load_mpc_params.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/load_mpc_params.m -------------------------------------------------------------------------------- /path-tracking-zsj/load_pp_params.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/load_pp_params.m -------------------------------------------------------------------------------- /path-tracking-zsj/log_init.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/log_init.m -------------------------------------------------------------------------------- /path-tracking-zsj/main_path_tracking.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/main_path_tracking.m -------------------------------------------------------------------------------- /path-tracking-zsj/plot_result.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/plot_result.m -------------------------------------------------------------------------------- /path-tracking-zsj/preview_point_on_trajref.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/preview_point_on_trajref.m -------------------------------------------------------------------------------- /path-tracking-zsj/pure_pursuit.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/pure_pursuit.m -------------------------------------------------------------------------------- /path-tracking-zsj/reference.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/reference.mat -------------------------------------------------------------------------------- /path-tracking-zsj/self_aligning_control.m: -------------------------------------------------------------------------------- 1 | function steering_cmd_new=self_aligning_control(steering_cmd, steering_collect) 2 | % set the correction control for the PP algorithm oscillation 3 | right_angle_ratio=0.8; 4 | left_angle_ratio=0.8; 5 | adjust_angle=0.06; 6 | delay_size=length(steering_collect); 7 | curve_judge=false; 8 | i=1; 9 | %judge for the right turn condition 10 | if (steering_cmd>0) 11 | while(i<=delay_size) 12 | if (steering_collect(i)-steering_cmd<-0.08) 13 | curve_judge=true; 14 | end 15 | i=i+1; 16 | end 17 | if (curve_judge) 18 | steering_cmd=(steering_cmd-adjust_angle)*right_angle_ratio; 19 | end 20 | %judge for the left turn condition 21 | elseif (steering_cmd<0) 22 | while(i<=delay_size) 23 | if (steering_collect(i)-steering_cmd>0.08) 24 | curve_judge=true; 25 | end 26 | i=i+1; 27 | end 28 | if (curve_judge) 29 | steering_cmd=(steering_cmd+adjust_angle)*left_angle_ratio; 30 | end 31 | end 32 | steering_cmd_new=steering_cmd; 33 | 34 | end 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | -------------------------------------------------------------------------------- /path-tracking-zsj/set_title_name.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/set_title_name.m -------------------------------------------------------------------------------- /path-tracking-zsj/set_trajref_params.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/set_trajref_params.m -------------------------------------------------------------------------------- /path-tracking-zsj/set_y_limits.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/set_y_limits.m -------------------------------------------------------------------------------- /path-tracking-zsj/solve_lqr_problem.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/solve_lqr_problem.m -------------------------------------------------------------------------------- /path-tracking-zsj/solve_mpc_problem.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/solve_mpc_problem.m -------------------------------------------------------------------------------- /path-tracking-zsj/sp10lqr.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp10lqr.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp10lqrap.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp10lqrap.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp10mpc.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp10mpc.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp10pp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp10pp.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp4lqr.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp4lqr.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp4lqrap.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp4lqrap.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp4mpc.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp4mpc.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp4pp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp4pp.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp6lqr.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp6lqr.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp6lqrap.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp6lqrap.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp6mpc.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp6mpc.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp6pp.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp6pp.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp9lqrdelay.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp9lqrdelay.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp9mpcdelay.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp9mpcdelay.mat -------------------------------------------------------------------------------- /path-tracking-zsj/sp9ppdelay.mat: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/sp9ppdelay.mat -------------------------------------------------------------------------------- /path-tracking-zsj/update_LQR_matrix.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/update_LQR_matrix.m -------------------------------------------------------------------------------- /path-tracking-zsj/update_angular_velocity.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/update_angular_velocity.m -------------------------------------------------------------------------------- /path-tracking-zsj/update_state_matrix.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/update_state_matrix.m -------------------------------------------------------------------------------- /path-tracking-zsj/update_veh_pose.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/update_veh_pose.m -------------------------------------------------------------------------------- /path-tracking-zsj/update_veh_pose_mpc.m: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/mubowen/Baidu-Apollo-control-algorithm/3c02470b08b5ad935bd1b3be268a34f82727290e/path-tracking-zsj/update_veh_pose_mpc.m --------------------------------------------------------------------------------