python求函数曲率_【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米远处，曲率半径为：