【Python】车道线拟合曲线的曲率半径计算公式及代码

学习优达学城的Advanced-Lane-Lines课程时，碰到了车道线的曲率半径计算。初见公式略显陌生，直到想起曲率半径的计算公式时才想明白，故记录如下。

def cal_curverature(img_shape, left_fit, right_fit):
    ploty=np.linspace(0, img_shape[0]-1, num=img_shape[0])
    ym_per_pix = 30/720 # meters per pixel in y dimension
    xm_per_pix = 3.7/700 # meters per pixel in x dimension
# Fit new polynomials to x,y in world space
    y_eval = np.max(ploty)
    leftx=left_fit[0]*ploty**2+left_fit[1]*ploty+left_fit[2]
    rightx=right_fit[0]*ploty**2+right_fit[1]*ploty+right_fit[2]
    left_fit_cr = np.polyfit(ploty*ym_per_pix, leftx*xm_per_pix, 2)
    right_fit_cr = np.polyfit(ploty*ym_per_pix, rightx*xm_per_pix, 2)
# Calculate the new radii of curvature
    left_curverad = ((1 + (2*left_fit_cr[0]*y_eval*ym_per_pix + left_fit_cr[1])**2)**1.5) / np.absolute(2*left_fit_cr[0])
    right_curverad = ((1 + (2*right_fit_cr[0]*y_eval*ym_per_pix + right_fit_cr[1])**2)**1.5) / np.absolute(2*right_fit_cr[0])
    xoffset=(left_fit_cr[2]+right_fit_cr[2])/2-img.shape[1]*xm_per_pix/2
    return left_curverad, right_curverad, xoffset

一、曲率半径公式及推导

 

详细推导过程如下：

 

二、利用公式计算拟合车道线的曲率半径

假设车道线方程为二次拟合曲线：

x = a*y*y + b*y +c

则在y米远处，曲率半径为：