"Is there an issue with the curvature of the trajectory generated by the MPT algorithm?"

[JokerWhy233]

Hi ,i want to ask a question about How should curvature be calculated appropriately?

in figure1figure2 ,topic /planning/scenario_planning/scenario_selector/trajectory is output by freespace planner or autoware_path_optimizer.

I generate a path via autoware_path_optimizer with a sampling-point interval of 0.2 m. As you can see in the figure1 , the curvature exhibits oscillations on structured roads.

But curvature is normal when i generate a path via hybird A start ，and optimizer by SQP with a sampling-point interval of 0.2 m.

So , i want to know: why does this curvature jitter occur, and how can we handle it to compute curvature correctly?

The code for computing curvature is as follows，num_sampling_points is 1:

def calculate_curvature(pose_l, num_sampling_points):
res = [0.0] * len(pose_l)
num_points = len(pose_l)

max_smoothing_num = int(0.5 * (num_points - 1))
L = min(num_sampling_points, max_smoothing_num)
for i in range(L, num_points - L):
    p1 = np.array(pose_l[i - L])
    p2 = np.array(pose_l[i])
    p3 = np.array(pose_l[i + L])
    
    den = max(
        np.linalg.norm(p2 - p1) * np.linalg.norm(p2 - p3) * np.linalg.norm(p3 - p1),
        0.0001)
    curvature = 2.0 * ((p2[0] - p1[0]) * (p3[1] - p1[1]) - (p2[1] - p1[1]) * (p3[0] - p1[0])) / den
    res[i] = curvature
for i in range(min(L, num_points)):
    res[i] = res[min(L, num_points - 1)]
    res[num_points - i - 1] = res[max(0, num_points - L - 1)]
return res

[maxime-clem]

why does this curvature jitter occur

This is because the "smooth" trajectory calculated internally by the autoware_path_optimizer is discretized. Calculating the curvature of the resulting discreet points gives only an approximation of the original curvatures, and this approximation can be quite noisy.

how can we handle it to compute curvature correctly?

I don't think there is a "correct" way to calculate curvature. It really depends on what you want to do with it.
In Autoware, the MPC lateral controller will resample the trajectory and smooth its curvature.

[JokerWhy233]

great !

the resampling and curvature smoothing code in MPC：

Calculate the curvature by curvature_smoothing_num_traj, and resample the trajectory using path_filter_moving_ave_num

const int mpc_traj_resampled_size = static_cast(mpc_traj_resampled.size());
if (
param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
using MoveAverageFilter::filt_vector;
if (
!filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
!filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
!filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
!filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
RCLCPP_DEBUG(m_logger, "path callback: filtering error. stop filtering.");
mpc_traj_smoothed = mpc_traj_resampled;
}
}

But wouldn't this cause the original planned trajectory to lose its accuracy?

[maxime-clem]

The path smoothing is disabled by default.

This is the code that smooths the curvature: https://github.com/autowarefoundation/autoware_universe/blob/a3c7a70e0c52d643450c033242acde64702622dc/control/autoware_mpc_lateral_controller/src/mpc.cpp#L258-L260

[JokerWhy233]

Thanks .
I mentioned in my question that the visualization code referred to is based on this part of the code, so to address the issue of curvature jittering, one would need to increase the parameter num_sampling_points when calculating the curvature, for example, to 15.

Are there any methods to ensure that the trajectory output by autoware_path_optimizer has a non-fluctuating curvature when num_sampling_points = 1?"

[maxime-clem]

Are there any methods to ensure that the trajectory output by autoware_path_optimizer has a non-fluctuating curvature when num_sampling_points = 1?"

I don't think this is possible.

[JokerWhy233]

i got it ,thanks！