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.