Collision of path after optimization

Hi,

Like shown in image below, sometimes the path after final optimization(the green line) will cross the obstacles, even though the KinoSearchTime() alreay found a perfect collision free path (the black dots).

What problem could it be?

Thank you.

Here are the paramters I use:

`planner_node: ros__parameters:

vehicle/cars_num: 1
vehicle/car_id: 0

vehicle/car_width: 0.19
vehicle/car_length: 0.21
vehicle/car_wheelbase: 0.14
vehicle/car_front_suspension: 0.03
vehicle/car_rear_suspension: 0.03
vehicle/car_max_steering_angle: 30.0
vehicle/car_d_cr: 0.07

mapping/resolution: 0.05
mapping/map_size_x: 7.28
mapping/map_size_y: 5.97
mapping/map_size_z: 5.0
mapping/origin_x: -3.64
mapping/origin_y: -2.985
mapping/origin_z: 0.1
mapping/odometry_topic: /car_0/pose
mapping/target_topic: /car_0/target
mapping/lidar_topic: lidar
mapping/map_pub_topic: map
mapping/lidar_height: 0.0
mapping/prob_hit_log: 1.2
mapping/prob_miss_log: -0.3
mapping/clamp_min_log: -2.0
mapping/clamp_max_log: 2.0
mapping/min_occupancy_log: 1.39
mapping/min_ray_length: 0.0
mapping/max_ray_length: 30.0
mapping/frame_id: map

fsm/target_x: 3.0
fsm/target_y: 3.0
fsm/target_yaw: 0.0

planning/car_id: 0
planning/traj_piece_duration: 1.0
planning/traj_res: 8
planning/dense_traj_res: 20

search/horizon: 50.0
search/yaw_resolution: 0.3
search/lambda_heu: 5.0
search/allocate_num: 100000
search/check_num: 7
search/max_search_time: 1000.1
search/traj_forward_penalty: 1.0
search/traj_back_penalty: 3.5
search/traj_gear_switch_penalty: 15.0
search/traj_steer_penalty: 0.5
search/traj_steer_change_penalty: 0.0
search/step_arc: 0.08
search/checkl: 0.05
search/max_vel: 0.8
search/max_acc: 1.0
search/max_cur: 2.0 # 0.5
search/time_resolution: 0.1
search/distance_resolution: 0.01
search/velocity_resolution: 0.01

optimizing/traj_resolution: 8
optimizing/des_traj_resolution: 20
optimizing/wei_sta_obs: 8000.0
optimizing/wei_dyn_obs: 7000.0
optimizing/wei_feas: 1000.0
optimizing/wei_sqrvar: 1000.0
optimizing/wei_time: 500.0
optimizing/dyn_obs_clearance: 0.5
optimizing/max_vel: 0.8
optimizing/min_vel: -0.8
optimizing/max_acc: 1.0
optimizing/min_acc: -1.0
optimizing/max_cur: 1.5
optimizing/traj_piece_duration: 1.0
optimizing/max_low_vel: 16.0
optimizing/max_low_acc: 2.5
optimizing/half_margin: 0.25`

20240322-195355

ZJU-FAST-Lab / Car-like-Robotic-swarm

Collision of path after optimization #15