carbib_odom test¶

激光里程计标定 参考项目：OdomLaserCalibraTool

In [ ]:

from diffDriver import DiffDriver
import pandas as pd
import numpy as np
from pandas import DataFrame

driver = DiffDriver(rL=0.3, rR=0.4, L=1)

# 轮式里程计 内参
diff_driver_inner_params = np.array([0.4, 0.4, 1.0]) # [rL,rR,L]


driver.set_calibrated_params(diff_driver_inner_params)

#  [x,y,theta,t]

poses_real = [np.append(driver.POS.to_array(), 0.0)]
pose_odom = [np.append(driver.POS_odom.to_array(), 0.0)]


dt = 0.01
# update
for i in range(1, 100):
    driver.update(v=1.0, w=0.0, dt=dt)
    poses_real.append(np.append(driver.POS.to_array(), dt * (i + 1)))
    pose_odom.append(np.append(driver.POS_odom.to_array(), dt * (i + 1)))

from diffDriver import DiffDriver import pandas as pd import numpy as np from pandas import DataFrame driver = DiffDriver(rL=0.3, rR=0.4, L=1) # 轮式里程计 内参 diff_driver_inner_params = np.array([0.4, 0.4, 1.0]) # [rL,rR,L] driver.set_calibrated_params(diff_driver_inner_params) # [x,y,theta,t] poses_real = [np.append(driver.POS.to_array(), 0.0)] pose_odom = [np.append(driver.POS_odom.to_array(), 0.0)] dt = 0.01 # update for i in range(1, 100): driver.update(v=1.0, w=0.0, dt=dt) poses_real.append(np.append(driver.POS.to_array(), dt * (i + 1))) pose_odom.append(np.append(driver.POS_odom.to_array(), dt * (i + 1)))

In [2]:

# convert to DataFrame

poses_real_df : DataFrame = pd.DataFrame(poses_real, columns=['x', 'y', 'theta', 't'])
poses_odom_df : DataFrame = pd.DataFrame(pose_odom, columns=['x', 'y', 'theta', 't'])

# convert to DataFrame poses_real_df : DataFrame = pd.DataFrame(poses_real, columns=['x', 'y', 'theta', 't']) poses_odom_df : DataFrame = pd.DataFrame(pose_odom, columns=['x', 'y', 'theta', 't'])

In [3]:

def annoate_start_end(ax,start=[0.0,0.0],end=[0.0,0.0]):
    start_x, start_y = start[0], start[1]
    end_x, end_y = end[0], end[1]
#     ax.annotate(f'start\n({start_x:.3f}, {start_y:.3f})',
#             xy=(start_x, start_y),
#             arrowprops=dict(arrowstyle='->', color='red', lw=1.5),
#             bbox=dict(boxstyle='round,pad=0.3', facecolor='yellow', alpha=0.8),
#             fontsize=10, fontweight='bold',
#             ha='center')

    # 标注终点
    ax.annotate(f'end\n({end_x:.3f}, {end_y:.3f})',
                xy=(end_x, end_y),
                arrowprops=dict(arrowstyle='->', color='green', lw=1.5),
                bbox=dict(boxstyle='round,pad=0.3', facecolor='lightgreen', alpha=0.8),
                fontsize=10, fontweight='bold',
                ha='center')

    # 标记点
    ax.scatter([start_x, end_x], [start_y, end_y], 
            c=['red', 'green'], s=100, zorder=5, 
            edgecolors='black', linewidths=2)
 

def annoate_start_end(ax,start=[0.0,0.0],end=[0.0,0.0]): start_x, start_y = start[0], start[1] end_x, end_y = end[0], end[1] # ax.annotate(f'start\n({start_x:.3f}, {start_y:.3f})', # xy=(start_x, start_y), # arrowprops=dict(arrowstyle='->', color='red', lw=1.5), # bbox=dict(boxstyle='round,pad=0.3', facecolor='yellow', alpha=0.8), # fontsize=10, fontweight='bold', # ha='center') # 标注终点 ax.annotate(f'end\n({end_x:.3f}, {end_y:.3f})', xy=(end_x, end_y), arrowprops=dict(arrowstyle='->', color='green', lw=1.5), bbox=dict(boxstyle='round,pad=0.3', facecolor='lightgreen', alpha=0.8), fontsize=10, fontweight='bold', ha='center') # 标记点 ax.scatter([start_x, end_x], [start_y, end_y], c=['red', 'green'], s=100, zorder=5, edgecolors='black', linewidths=2)

In [4]:

# draw 
ax=None
colors = ['red','blue','green','yellow']
ax = poses_real_df.plot.scatter('x','y',ax=ax,c=colors[0],label="real")
ax.set_xlim(-1,1)
ax.set_xlabel("m")
ax.set_ylabel("m")
end = [poses_real_df['x'].iloc[-1],poses_real_df['y'].iloc[-1]]
annoate_start_end(ax,start=[0,0],end=end)
print("read end: ",end)

# odom
ax = poses_odom_df.plot.scatter('x','y',ax=ax,c=colors[1],label="odom")
end = [poses_odom_df['x'].iloc[-1],poses_odom_df['y'].iloc[-1]]
annoate_start_end(ax,start=[0,0],end=end)
print("odom end: ",end)

# draw ax=None colors = ['red','blue','green','yellow'] ax = poses_real_df.plot.scatter('x','y',ax=ax,c=colors[0],label="real") ax.set_xlim(-1,1) ax.set_xlabel("m") ax.set_ylabel("m") end = [poses_real_df['x'].iloc[-1],poses_real_df['y'].iloc[-1]] annoate_start_end(ax,start=[0,0],end=end) print("read end: ",end) # odom ax = poses_odom_df.plot.scatter('x','y',ax=ax,c=colors[1],label="odom") end = [poses_odom_df['x'].iloc[-1],poses_odom_df['y'].iloc[-1]] annoate_start_end(ax,start=[0,0],end=end) print("odom end: ",end)

read end:  [np.float64(-0.10643797747364456), np.float64(0.8574598128423966)]
odom end:  [np.float64(3.448532298269697e-06), np.float64(0.9900515796973096)]

carlibration¶

In [5]:

# 按 等时间间隔 取出数据 作为 雷达 icp 结果 laser_frame (加上 error)
sync_laser_poses_df = poses_real_df[poses_real_df.index % 10 == 0].copy()
sync_laser_poses_df['x'] = sync_laser_poses_df['x'] + np.random.uniform(0, 0.005, len(sync_laser_poses_df))
sync_laser_poses_df['y'] = sync_laser_poses_df['y'] + np.random.uniform(0, 0.005, len(sync_laser_poses_df))
sync_laser_poses_df['theta'] = sync_laser_poses_df['theta'] + np.random.uniform(0, 0.005, len(sync_laser_poses_df))

sync_laser_poses_df['dx'] = sync_laser_poses_df['x'].diff()
sync_laser_poses_df['dy'] = sync_laser_poses_df['y'].diff()
sync_laser_poses_df['dtheta'] = sync_laser_poses_df['theta'].diff()

# odom
sync_odom_poses_df = poses_odom_df[poses_odom_df.index % 10 == 0].copy()

sync_odom_poses_df['dt'] = sync_odom_poses_df['t'].diff()
sync_odom_poses_df['dx'] = sync_odom_poses_df['x'].diff()
sync_odom_poses_df['dy'] = sync_odom_poses_df['y'].diff()
sync_odom_poses_df['dtheta'] = sync_odom_poses_df['theta'].diff()

# 计算线速度v和角速度w
# 线速度 = 位置变化量 / 时间
sync_odom_poses_df['v'] = np.sqrt(sync_odom_poses_df['dx'] ** 2 + sync_odom_poses_df['dy'] ** 2) / sync_odom_poses_df['dt']
# 角速度 = 角度变化量 / 时间
sync_odom_poses_df['w'] = sync_odom_poses_df['dtheta'] / sync_odom_poses_df['dt']

# plot 
ax=None
colors = ['red','blue','green','yellow']
ax = sync_odom_poses_df.plot.scatter('t','v',ax=ax,c=colors[0],label="v")
ax = sync_odom_poses_df.plot.scatter('t','w',ax=ax,c=colors[1],label="w")

# 按 等时间间隔 取出数据 作为 雷达 icp 结果 laser_frame (加上 error) sync_laser_poses_df = poses_real_df[poses_real_df.index % 10 == 0].copy() sync_laser_poses_df['x'] = sync_laser_poses_df['x'] + np.random.uniform(0, 0.005, len(sync_laser_poses_df)) sync_laser_poses_df['y'] = sync_laser_poses_df['y'] + np.random.uniform(0, 0.005, len(sync_laser_poses_df)) sync_laser_poses_df['theta'] = sync_laser_poses_df['theta'] + np.random.uniform(0, 0.005, len(sync_laser_poses_df)) sync_laser_poses_df['dx'] = sync_laser_poses_df['x'].diff() sync_laser_poses_df['dy'] = sync_laser_poses_df['y'].diff() sync_laser_poses_df['dtheta'] = sync_laser_poses_df['theta'].diff() # odom sync_odom_poses_df = poses_odom_df[poses_odom_df.index % 10 == 0].copy() sync_odom_poses_df['dt'] = sync_odom_poses_df['t'].diff() sync_odom_poses_df['dx'] = sync_odom_poses_df['x'].diff() sync_odom_poses_df['dy'] = sync_odom_poses_df['y'].diff() sync_odom_poses_df['dtheta'] = sync_odom_poses_df['theta'].diff() # 计算线速度v和角速度w # 线速度 = 位置变化量 / 时间 sync_odom_poses_df['v'] = np.sqrt(sync_odom_poses_df['dx'] ** 2 + sync_odom_poses_df['dy'] ** 2) / sync_odom_poses_df['dt'] # 角速度 = 角度变化量 / 时间 sync_odom_poses_df['w'] = sync_odom_poses_df['dtheta'] / sync_odom_poses_df['dt'] # plot ax=None colors = ['red','blue','green','yellow'] ax = sync_odom_poses_df.plot.scatter('t','v',ax=ax,c=colors[0],label="v") ax = sync_odom_poses_df.plot.scatter('t','w',ax=ax,c=colors[1],label="w")

calc [wL,wR]¶

In [6]:

# 轮式里程计 内参（用于从位姿差推算轮速的初始内参）
print('initial inner params (used to infer wheel speeds):', diff_driver_inner_params)
rL_init, rR_init, L_init = diff_driver_inner_params.ravel()

# 激光里程计 外参 （占位，后续会估计）
lasar_outer_params = np.array([0.0, 0.0, 0.0])  # [x,y,yaw]
# 手眼标定与非线性最小二乘优化：
# 同时估计激光外参 (tx,ty,dtheta) 与 轮式里程计的尺度/角度校正因子 (proxy for 内参)：
# params = [tx, ty, dtheta, s_trans, s_rot]
# 我们假设来自里程计的增量在里程计本体坐标系上可用尺度因子放缩，
# 并对角增量应用角度缩放；然后积分得到校正后的 odom 轨迹与激光对齐。


# 
sync_odom_poses_df['wL'] = np.nan
sync_odom_poses_df['wR'] = np.nan

# Precompute inverse transform once for efficiency and clearer error reporting
try:
    T_inv = np.linalg.inv(driver.transform_matrix_cali())
except Exception as e:
    print('failed to invert transform matrix:', e)
    T_inv = None

for idx in sync_odom_poses_df.index:
    v = sync_odom_poses_df.at[idx, 'v']
    w = sync_odom_poses_df.at[idx, 'w']

    # skip rows with invalid/missing speed measurements or if we couldn't invert the matrix
    if T_inv is None or (not np.isfinite(v)) or (not np.isfinite(w)):
        sync_odom_poses_df.at[idx, 'wL'] = np.nan
        sync_odom_poses_df.at[idx, 'wR'] = np.nan
        continue

    try:
        W = np.array([v, w])
        wheel_speeds = T_inv.dot(W)
        sync_odom_poses_df.at[idx, 'wL'] = wheel_speeds[0]
        sync_odom_poses_df.at[idx, 'wR'] = wheel_speeds[1]
    except Exception as e:
        print(f'failed at idx {idx}:', e)
        sync_odom_poses_df.at[idx, 'wL'] = np.nan
        sync_odom_poses_df.at[idx, 'wR'] = np.nan

# 轮式里程计 内参（用于从位姿差推算轮速的初始内参） print('initial inner params (used to infer wheel speeds):', diff_driver_inner_params) rL_init, rR_init, L_init = diff_driver_inner_params.ravel() # 激光里程计 外参 （占位，后续会估计） lasar_outer_params = np.array([0.0, 0.0, 0.0]) # [x,y,yaw] # 手眼标定与非线性最小二乘优化： # 同时估计激光外参 (tx,ty,dtheta) 与 轮式里程计的尺度/角度校正因子 (proxy for 内参)： # params = [tx, ty, dtheta, s_trans, s_rot] # 我们假设来自里程计的增量在里程计本体坐标系上可用尺度因子放缩， # 并对角增量应用角度缩放；然后积分得到校正后的 odom 轨迹与激光对齐。 # sync_odom_poses_df['wL'] = np.nan sync_odom_poses_df['wR'] = np.nan # Precompute inverse transform once for efficiency and clearer error reporting try: T_inv = np.linalg.inv(driver.transform_matrix_cali()) except Exception as e: print('failed to invert transform matrix:', e) T_inv = None for idx in sync_odom_poses_df.index: v = sync_odom_poses_df.at[idx, 'v'] w = sync_odom_poses_df.at[idx, 'w'] # skip rows with invalid/missing speed measurements or if we couldn't invert the matrix if T_inv is None or (not np.isfinite(v)) or (not np.isfinite(w)): sync_odom_poses_df.at[idx, 'wL'] = np.nan sync_odom_poses_df.at[idx, 'wR'] = np.nan continue try: W = np.array([v, w]) wheel_speeds = T_inv.dot(W) sync_odom_poses_df.at[idx, 'wL'] = wheel_speeds[0] sync_odom_poses_df.at[idx, 'wR'] = wheel_speeds[1] except Exception as e: print(f'failed at idx {idx}:', e) sync_odom_poses_df.at[idx, 'wL'] = np.nan sync_odom_poses_df.at[idx, 'wR'] = np.nan

initial inner params (used to infer wheel speeds): [0.4 0.4 1. ]

prepare cleaned data¶

In [7]:

# 初始猜测： 外参为零，内参使用初始值
x0 = np.array([0.0, 0.0, 0.0, rL_init, rR_init, L_init])

# remove null
sync_odom_poses_df_cleaned = sync_odom_poses_df.dropna()
sync_laser_poses_df_cleaned = sync_laser_poses_df.dropna()


# Prepare observation vectors and inputs for optimization
wL_obs = sync_odom_poses_df_cleaned['wL'].to_numpy()
wR_obs = sync_odom_poses_df_cleaned['wR'].to_numpy()

dt_vec = sync_odom_poses_df_cleaned['dt'].to_numpy()

odom_dxdy = sync_odom_poses_df_cleaned[['dx', 'dy']].to_numpy()
odom_dtheta = sync_odom_poses_df_cleaned['dtheta'].to_numpy()

# Absolute laser poses for plotting/analysis
laser_dxdy_abs = sync_laser_poses_df_cleaned[['dx', 'dy']].to_numpy()
laser_dtheta_abs = sync_laser_poses_df_cleaned['dtheta'].to_numpy()

# Basic sanity checks
N = len(wL_obs)
assert len(wR_obs) == N == len(dt_vec) == len(odom_dxdy) == len(laser_dxdy_abs), "Length mismatch among observation vectors"
print(f'Prepared {N} synchronized samples for optimization (using deltas)')

# 初始猜测： 外参为零，内参使用初始值 x0 = np.array([0.0, 0.0, 0.0, rL_init, rR_init, L_init]) # remove null sync_odom_poses_df_cleaned = sync_odom_poses_df.dropna() sync_laser_poses_df_cleaned = sync_laser_poses_df.dropna() # Prepare observation vectors and inputs for optimization wL_obs = sync_odom_poses_df_cleaned['wL'].to_numpy() wR_obs = sync_odom_poses_df_cleaned['wR'].to_numpy() dt_vec = sync_odom_poses_df_cleaned['dt'].to_numpy() odom_dxdy = sync_odom_poses_df_cleaned[['dx', 'dy']].to_numpy() odom_dtheta = sync_odom_poses_df_cleaned['dtheta'].to_numpy() # Absolute laser poses for plotting/analysis laser_dxdy_abs = sync_laser_poses_df_cleaned[['dx', 'dy']].to_numpy() laser_dtheta_abs = sync_laser_poses_df_cleaned['dtheta'].to_numpy() # Basic sanity checks N = len(wL_obs) assert len(wR_obs) == N == len(dt_vec) == len(odom_dxdy) == len(laser_dxdy_abs), "Length mismatch among observation vectors" print(f'Prepared {N} synchronized samples for optimization (using deltas)')

Prepared 9 synchronized samples for optimization (using deltas)

simulate_from_wheels¶

In [8]:

# helpers
def angle_diff(a, b):
    """Return angular difference a-b in [-pi, pi]"""
    d = (a - b + np.pi) % (2.0 * np.pi) - np.pi
    return d


def simulate_from_wheels(wL, wR, rL, rR, L, dt_vec, init_xy, init_theta):
    """Simulate odom poses from wheel angular speeds."""
    sim = DiffDriver(rL=rL, rR=rR, L=L)
    sim.set_calibrated_params([rL,rR,L])
    # keep the existing Pose2D object and set its fields (avoid replacing with list)
    sim.POS.x = init_xy[0]
    sim.POS.y = init_xy[1]
    sim.POS.theta = init_theta
    
    wL = np.asarray(wL)
    wR = np.asarray(wR)
    dt_vec = np.asarray(dt_vec)
    N = len(wL)
    xs = np.zeros((N, 2))
    thetas = np.zeros(N)
    for i in range(N):
        sim.update_by_wheel_speeds([wL[i],wR[i]],dt_vec[i])
        xs[i, 0] = sim.POS.x; xs[i, 1] = sim.POS.y
        thetas[i] = sim.POS.theta
    return xs, thetas


def residuals_params(params, wL_obs, wR_obs, dt_vec, odom_xy0, odom_theta0, laser_xy_abs, laser_theta_abs):
    tx, ty, dtheta, rL, rR, L = params
    # simulate odom using candidate wheel params
    odom_sim_xy, odom_sim_theta = simulate_from_wheels(wL_obs, wR_obs, rL, rR, L, dt_vec, odom_xy0, odom_theta0)
    # apply estimated extrinsic (rotation + translation)
    c = np.cos(dtheta); s = np.sin(dtheta)
    R = np.array([[c, -s], [s, c]])
    odom_trans = (R @ odom_sim_xy.T).T + np.array([tx, ty])
    # position residuals (x,y stacked)
    pos_res = (odom_trans - laser_xy_abs).ravel()
    # angular residuals (wrapped)
    ang_res = angle_diff(odom_sim_theta + dtheta, laser_theta_abs)
    return np.hstack([pos_res, ang_res])

# prepare absolute pose arrays (used by optimizer)
odom_xy_abs = sync_odom_poses_df_cleaned[['x','y']].to_numpy()
odom_theta_abs = sync_odom_poses_df_cleaned['theta'].to_numpy()
laser_xy_abs = sync_laser_poses_df_cleaned[['x','y']].to_numpy()
laser_theta_abs = sync_laser_poses_df_cleaned['theta'].to_numpy()
odom_xy0 = sync_odom_poses_df[['x','y']].to_numpy()[0]
odom_theta0 = sync_odom_poses_df['theta'].to_numpy()[0]
print('Prepared odom_xy_abs:', odom_xy_abs.shape, 'laser_xy_abs:', laser_xy_abs.shape)

# helpers def angle_diff(a, b): """Return angular difference a-b in [-pi, pi]""" d = (a - b + np.pi) % (2.0 * np.pi) - np.pi return d def simulate_from_wheels(wL, wR, rL, rR, L, dt_vec, init_xy, init_theta): """Simulate odom poses from wheel angular speeds.""" sim = DiffDriver(rL=rL, rR=rR, L=L) sim.set_calibrated_params([rL,rR,L]) # keep the existing Pose2D object and set its fields (avoid replacing with list) sim.POS.x = init_xy[0] sim.POS.y = init_xy[1] sim.POS.theta = init_theta wL = np.asarray(wL) wR = np.asarray(wR) dt_vec = np.asarray(dt_vec) N = len(wL) xs = np.zeros((N, 2)) thetas = np.zeros(N) for i in range(N): sim.update_by_wheel_speeds([wL[i],wR[i]],dt_vec[i]) xs[i, 0] = sim.POS.x; xs[i, 1] = sim.POS.y thetas[i] = sim.POS.theta return xs, thetas def residuals_params(params, wL_obs, wR_obs, dt_vec, odom_xy0, odom_theta0, laser_xy_abs, laser_theta_abs): tx, ty, dtheta, rL, rR, L = params # simulate odom using candidate wheel params odom_sim_xy, odom_sim_theta = simulate_from_wheels(wL_obs, wR_obs, rL, rR, L, dt_vec, odom_xy0, odom_theta0) # apply estimated extrinsic (rotation + translation) c = np.cos(dtheta); s = np.sin(dtheta) R = np.array([[c, -s], [s, c]]) odom_trans = (R @ odom_sim_xy.T).T + np.array([tx, ty]) # position residuals (x,y stacked) pos_res = (odom_trans - laser_xy_abs).ravel() # angular residuals (wrapped) ang_res = angle_diff(odom_sim_theta + dtheta, laser_theta_abs) return np.hstack([pos_res, ang_res]) # prepare absolute pose arrays (used by optimizer) odom_xy_abs = sync_odom_poses_df_cleaned[['x','y']].to_numpy() odom_theta_abs = sync_odom_poses_df_cleaned['theta'].to_numpy() laser_xy_abs = sync_laser_poses_df_cleaned[['x','y']].to_numpy() laser_theta_abs = sync_laser_poses_df_cleaned['theta'].to_numpy() odom_xy0 = sync_odom_poses_df[['x','y']].to_numpy()[0] odom_theta0 = sync_odom_poses_df['theta'].to_numpy()[0] print('Prepared odom_xy_abs:', odom_xy_abs.shape, 'laser_xy_abs:', laser_xy_abs.shape)

Prepared odom_xy_abs: (9, 2) laser_xy_abs: (9, 2)

In [9]:

# 使用 scipy 的 least_squares 进行优化（如果没有 scipy，可改用闭式解）

from scipy.optimize import least_squares

bounds_lower = np.array([-1.0, -1.0, -np.pi, 0.01, 0.01, 0.8])
bounds_upper = np.array([1.0, 1.0, np.pi, 1.0, 1.0, 1.2]) 
res = least_squares(residuals_params, x0, bounds=(bounds_lower, bounds_upper), args=(wL_obs, wR_obs, dt_vec, odom_xy0, odom_theta0, laser_xy_abs, laser_theta_abs), verbose=2)
print('res =', res)
tx, ty, dtheta, rL_est, rR_est, L_est = res.x
print('Estimated params: tx,ty,dtheta,rL,rR,L =', res.x)

# simulate with estimated params and compare to absolute laser poses for plotting
odom_sim_xy, odom_sim_theta = simulate_from_wheels(wL_obs, wR_obs, rL_est, rR_est, L_est, dt_vec, odom_xy0, odom_theta0)
c = np.cos(dtheta)
s = np.sin(dtheta)
R = np.array([[c, -s], [s, c]])
odom_trans = (R @ odom_sim_xy.T).T + np.array([tx, ty])

pos_errors = np.linalg.norm(odom_trans - laser_xy_abs, axis=1)
print('Position RMSE:', np.sqrt(np.mean(pos_errors**2)))
ang_errors = np.abs(angle_diff(odom_sim_theta + dtheta, laser_theta_abs))
print('Median orientation error (deg):', np.median(ang_errors) * 180.0 / np.pi)

# 绘图比较
import matplotlib.pyplot as plt

fig, ax = plt.subplots()
ax.scatter(laser_xy_abs[:, 0], laser_xy_abs[:, 1], c='r', label='laser')
ax.scatter(odom_trans[:, 0], odom_trans[:, 1], c='b', marker='x', label='odom_sim')
for i in range(len(laser_xy_abs)):
    ax.plot([laser_xy_abs[i, 0], odom_trans[i, 0]], [laser_xy_abs[i, 1], odom_trans[i, 1]], c='gray', linestyle='--', linewidth=0.8)
ax.legend()
ax.set_aspect('equal', 'box')
ax.set_title(f'tx={tx:.3f}, ty={ty:.3f}, dtheta={dtheta:.3f}, rL={rL_est:.4f}, rR={rR_est:.4f}, L={L_est:.4f}')
plt.show()
lasar_outer_params = np.array([tx, ty, dtheta])

# 使用 scipy 的 least_squares 进行优化（如果没有 scipy，可改用闭式解） from scipy.optimize import least_squares bounds_lower = np.array([-1.0, -1.0, -np.pi, 0.01, 0.01, 0.8]) bounds_upper = np.array([1.0, 1.0, np.pi, 1.0, 1.0, 1.2]) res = least_squares(residuals_params, x0, bounds=(bounds_lower, bounds_upper), args=(wL_obs, wR_obs, dt_vec, odom_xy0, odom_theta0, laser_xy_abs, laser_theta_abs), verbose=2) print('res =', res) tx, ty, dtheta, rL_est, rR_est, L_est = res.x print('Estimated params: tx,ty,dtheta,rL,rR,L =', res.x) # simulate with estimated params and compare to absolute laser poses for plotting odom_sim_xy, odom_sim_theta = simulate_from_wheels(wL_obs, wR_obs, rL_est, rR_est, L_est, dt_vec, odom_xy0, odom_theta0) c = np.cos(dtheta) s = np.sin(dtheta) R = np.array([[c, -s], [s, c]]) odom_trans = (R @ odom_sim_xy.T).T + np.array([tx, ty]) pos_errors = np.linalg.norm(odom_trans - laser_xy_abs, axis=1) print('Position RMSE:', np.sqrt(np.mean(pos_errors**2))) ang_errors = np.abs(angle_diff(odom_sim_theta + dtheta, laser_theta_abs)) print('Median orientation error (deg):', np.median(ang_errors) * 180.0 / np.pi) # 绘图比较 import matplotlib.pyplot as plt fig, ax = plt.subplots() ax.scatter(laser_xy_abs[:, 0], laser_xy_abs[:, 1], c='r', label='laser') ax.scatter(odom_trans[:, 0], odom_trans[:, 1], c='b', marker='x', label='odom_sim') for i in range(len(laser_xy_abs)): ax.plot([laser_xy_abs[i, 0], odom_trans[i, 0]], [laser_xy_abs[i, 1], odom_trans[i, 1]], c='gray', linestyle='--', linewidth=0.8) ax.legend() ax.set_aspect('equal', 'box') ax.set_title(f'tx={tx:.3f}, ty={ty:.3f}, dtheta={dtheta:.3f}, rL={rL_est:.4f}, rR={rR_est:.4f}, L={L_est:.4f}') plt.show() lasar_outer_params = np.array([tx, ty, dtheta])

   Iteration     Total nfev        Cost      Cost reduction    Step norm     Optimality   
       0              1         1.2219e-01                                    4.25e+00    
       1              2         5.7435e-03      1.16e-01       8.33e-02       1.48e-01    
       2              3         2.6225e-04      5.48e-03       7.32e-02       2.96e-02    
       3              4         2.8489e-05      2.34e-04       2.33e-02       2.31e-03    
       4              5         2.8268e-05      2.21e-07       6.43e-04       4.45e-06    
       5             10         2.8268e-05      1.05e-13       1.52e-05       8.65e-09    
`gtol` termination condition is satisfied.
Function evaluations 10, initial cost 1.2219e-01, final cost 2.8268e-05, first-order optimality 8.65e-09.
res =      message: `gtol` termination condition is satisfied.
     success: True
      status: 1
         fun: [ 8.351e-04  1.244e-03 ... -1.105e-03 -9.942e-05]
           x: [ 4.085e-03  2.171e-03  3.854e-03  3.015e-01  3.988e-01
                9.854e-01]
        cost: 2.8268109623960948e-05
         jac: [[ 1.000e+00  0.000e+00 ... -1.288e-02  1.074e-03]
               [ 0.000e+00  1.000e+00 ...  1.247e-01  2.164e-05]
               ...
               [ 0.000e+00  0.000e+00 ...  2.029e+00 -2.004e-01]
               [ 0.000e+00  0.000e+00 ...  2.283e+00 -2.254e-01]]
        grad: [-3.300e-10 -5.431e-11 -7.972e-10  6.698e-09 -1.439e-08
               -5.072e-09]
  optimality: 8.648813917904593e-09
 active_mask: [0 0 0 0 0 0]
        nfev: 10
        njev: 6
Estimated params: tx,ty,dtheta,rL,rR,L = [0.00408528 0.00217073 0.00385428 0.30152311 0.39881291 0.98535826]
Position RMSE: 0.0022874735077996434
Median orientation error (deg): 0.0601782467566816

多解原因¶

未约束 L 变量时，求解为：[rL,rR,L] = [0.25,0.45,2]

参数耦合（scale/coupling ambiguity）¶

差分驱动的小增量模型（近似）： v ≈ (rR·ωR + rL·ωL)/2 ω ≈ (rR·ωR - rL·ωL)/L 这里 rL, rR, L 同时出现在公式内，部分组合（或缩放）可以产生相似的 (v, ω) 输出，从而让多组 (rL,rR,L) 产生相近的轨迹残差。

观测信息不足 / 退化运动¶

如果数据主要是直线运动（ω ≈ 0），则第二个方程对 L 几乎不给信息 — L 在这种情况下不可观测，优化就会沿着某个自由方向（null space）漂移（例如把 L 变大，r 变小来折中）

实用使用建议¶

• 约束或固定 L 到合理范围（你已经尝试过 0.8–1.2 的 bound）：这是最直接有效的方式
• 数据包含多种运动轨迹 (直线，圆弧, ...) ， 提高数据信息量