Is the Hybrid A * path optimized by distance approach problem necessarily a collision-free path?

[JackRen88]

Hello,everyone! when I tried to run the trajectory_smoother in the open space planner,I found that sometimes Hybrid A * path optimized by distance approach is the collision path,Is this an inherent flaw in the algorithm? Moreover,What is iterative anchoring smoother designed in the trajectory_smoother used for ？

[JasonZhou404]

If you can provide the collision case for us to check, it would be easier to see what is going on. The possible reasons might include, numeric accuracy issue in Ipopt, slack penalty on collision avoidance in crowded area.

Iterative anchoring is another world frame collision avoidance path planning algorithm we are working on

[JackRen88]

Thanks for your reply! I provide an example, as shown below：

The green line represents Hybrid A* Generated path,the red line represents Optimized path,the Pink line represents obstacle,as you saw,the optimized path overlaps with the obstacle. Here are some of my parameters: vehicle parameters: `vehicle_param { brand: LINCOLN_MKZ

front_edge_to_center: 1.6 back_edge_to_center: 0.3 left_edge_to_center: 0.65 right_edge_to_center: 0.15

length: 1.9 width: 0.8 height: 1.48 min_turn_radius: 0.8 max_acceleration: 2.0 max_deceleration: -6.0 max_steer_angle: 8.20304748437 max_steer_angle_rate: 8.55211 min_steer_angle_rate: 0 steer_ratio: 16 wheel_base: 1.4 wheel_rolling_radius: 0.335 max_abs_speed_when_stopped: 0.2

brake_deadzone: 15.5 throttle_deadzone: 18.0 } Algorithm parameter： warm_start_config { xy_grid_resolution: 0.3 phi_grid_resolution: 0.1 next_node_num: 10 step_size: 0.5 traj_forward_penalty: 1.0 traj_back_penalty: 1.0 traj_gear_switch_penalty: 10.0 traj_steer_penalty: 0.0 traj_steer_change_penalty: 0.0 grid_a_star_xy_resolution: 0.5 node_radius: 0.5 s_curve_config { acc_weight: 1.0 jerk_weight: 0.0 dkappa_penalty_weight: 100.0 ref_s_weight: 0.1 ref_v_weight: 0.0 } } dual_variable_warm_start_config { weight_d: 1.0 ipopt_config { ipopt_print_level: 0 mumps_mem_percent: 6000 mumps_pivtol: 1e-06 ipopt_max_iter: 100 ipopt_tol: 1e-05 ipopt_acceptable_constr_viol_tol: 0.1 ipopt_min_hessian_perturbation: 1e-12 ipopt_jacobian_regularization_value: 1e-07 ipopt_print_timing_statistics: "yes" ipopt_alpha_for_y: "min" ipopt_recalc_y: "yes" } qp_format: OSQP min_safety_distance: 0.0 debug_osqp: false } distance_approach_config { weight_steer: 0.3 weight_a: 1.1 weight_steer_rate: 3.0 weight_a_rate: 2.5 weight_x: 2.3 weight_y: 0.7 weight_phi: 1.5 weight_v: 3.75 weight_steer_stitching: 1.75 weight_a_stitching: 3.25 weight_first_order_time: 4.25 weight_second_order_time: 13.5 weight_end_state: 1.0 min_safety_distance: 0.0 max_speed_forward: 2.0 max_speed_reverse: 1.0 max_acceleration_forward: 2.0 max_acceleration_reverse: 1.0 min_time_sample_scaling: 0.8 max_time_sample_scaling: 1.2 use_fix_time: false ipopt_config { ipopt_print_level: 0 mumps_mem_percent: 6000 mumps_pivtol: 1e-06 ipopt_max_iter: 1000 ipopt_tol: 0.0001 ipopt_acceptable_constr_viol_tol: 0.1 ipopt_min_hessian_perturbation: 1e-12 ipopt_jacobian_regularization_value: 1e-07 ipopt_print_timing_statistics: "yes" ipopt_alpha_for_y: "min" ipopt_recalc_y: "yes" ipopt_mu_init: 0.1 } enable_constraint_check: false enable_hand_derivative: false enable_derivative_check: false enable_initial_final_check: false distance_approach_mode: DISTANCE_APPROACH_IPOPT enable_check_initial_state: false enable_jacobian_ad: true }` Can you find out what is wrong?

[JasonZhou404]

How did you formulate your obstacle constraints? In distance approach. Every obstacle has to be in convex set form (Ax<b). If not, distance approach would fail.

[JackRen88]

Each obstacle region is determined by a line segment in my implemention, so you can find there is two obstacles,o1,o2 in the picture below, obstacle vertices are stored clockwise, as the picture shown below,v1,v2 vertices:

Solve the analytical expression of the line segment consisting of v1, v2 vertices ,y=a*x+b,so I can determine the A and b of obstacle.

[JasonZhou404]

You should formulate o1 and o2 as one convex obstacle, otherwise your implementations make the upper left space wrt v0 the only feasible configuration set for collision avoidance, which will surely fail distance approach. Plz check out open_space_roi for more details

[JackRen88]

Thank you for your patience. According to your instruction,I have the following issue: 1.Can a convex obstacle be determined by a line segment or at least two line segments? 2.Is there a relevant documentation of open_space_roi as mentioned above in apollo?

[JackRen88]

Hello,everyone! The path optimization algorithm in the open space planner will fail for the following scenarios,because Obstacle constraints cannot be represented.

A point to B point path optimization

[JasonZhou404]

The success of distance approach trajectory smoothing relies on the correct convexification of obstacle. There are some ways to do that. Plz check the term “Convex Decomposition” for more detail.

You are free to check our implementation of open_space_roi for your own usage of our distance approach trajectory smoothing algorithm.

To answer your previous question. Yes, an obstacle can be be present as one hyperplane or a polygon bounded by a few hyperplanes.

[Capri2014]

@JackRen88 Also you can modify and verify your algorithm against the open space scenarios we created via http://bce.apollo.auto/, I did not fully understand your drawing in above, but looks like a simple scenario and should be covered in simulation mentioned above.

[JackRen88]

@Capri2014 .thank you for your reply. In the above picture, the green line is the wall.Optimize the car path from point A to point B, the path optimization algorithm (distance approach)will fail,because Obstacle constraints cannot be well represented.

[JasonZhou404]

If you can present the three green line obstacle as three convex sets. Just like how we add perception obstacles in open_space_roi.cc.

[JackRen88]

@JasonZhou404 . Thank you for your reply. In the process of distance approach optimization testing,I found that optimization failure,as follows:

The green line represents hybrid Astar path generation,the red line represents distance approach path optimization,because of yaw angle mutation, distance approach optimization path exist circle path at the start point of path. I don't know if you have encountered such a problem.

[twbabyduck]

@JackRen88 Do you have contact mail or instant message? I would like to discuss the above issue. May I could help on.