自动驾驶车道线拟合

jrrz0828

已于 2024-09-05 11:31:37 修改

阅读量409

点赞数 7

文章标签：自动驾驶人工智能机器学习

于 2024-09-05 11:25:44 首次发布

本文链接：https://blog.csdn.net/jrrz0828/article/details/141923960

版权

1.问题定义

假设采用n次多项式对m个采样点进行最小二乘法拟合， $({x_1},{y_1}),({x_2},{y_2}), \ldots ,({x_m},{y_m})$ 分别为m个采样点的坐标，根据这m个采样点拟合的n次多项式为

$y(x,A) = {a_0} + {a_1}x + {a_2}{x^2} + {a_3}{x^3} + \ldots + {a_n}{x^n} = \sum\limits_{i = 0}^n {{a_i}{x^i}}$

我们的目的是通过m个采样点的坐标数据，求得n次多项式中的系数，即 ${a_0},{a_1},{a_2}, \ldots ,{a_n}$

2.求解方法

令

$A = \left[ {\begin{array}{*{20}{c}} {{a_0}}\\ {{a_1}}\\ \vdots \\ {{a_n}} \end{array}} \right],X = \left[ {\begin{array}{*{20}{c}} 1&{{x_1}}&{x_1^2}& \cdots &{x_1^n}\\ 1&{{x_2}}&{x_2^2}& \cdots &{x_2^n}\\ \vdots & \vdots & \vdots & \ddots & \vdots \\ 1&{{x_m}}&{x_m^2}& \cdots &{x_m^n} \end{array}} \right],Y = \left[ {\begin{array}{*{20}{c}} {{y_1}}\\ {{y_2}}\\ \vdots \\ {{y_m}} \end{array}} \right]$

在 ${x_1},{x_2}, \ldots ,{x_m}$ 处采样的y值分别为 ${y_1},{y_2}, \ldots ,{y_m}$ ，而经过n次多项式计算得到的y值为 $y({x_1},A),y({x_2},A), \ldots ,y({x_m},A)$ ，定义误差函数为y值之间差值的平方和（即下式中E(A)）

$E(A) = \frac{1}{2}\sum\limits_{i = 1}^m {{{(y({x_i},A) - {y_i})}^2}} = \frac{1}{2}{(XA - Y)^T}(XA - Y)$

求n次多项式的系数（即A）为何值时，误差函数E(A)最小，根据如下所示矩阵求导公式

可得

$\frac{{\partial E(A)}}{{\partial A}} = {X^T}XA - {X^T}Y = 0$

$A = {({X^T}X)^{ - 1}}{X^T}Y$

3.举例说明

在自动驾驶中通常使用三次多项式进行车道线拟合

$y = {C_0} + {C_1}x + {C_2}{x^2} + {C_3}{x^3}$

%5个采样点，x1到x5
x=[1;2;3;4;5];
y=zeros(5,1);
for i=1:5
    %生成y1到y5,加入随机数
    y(i)=1+0.5*x(i)+0.25*x(i)*x(i)+rand(1,1)*0.5;
end
%X矩阵
X=[1 x(1) x(1)^2 x(1)^3;
   1 x(2) x(2)^2 x(3)^3;
   1 x(3) x(3)^2 x(3)^3;
   1 x(4) x(4)^2 x(4)^3;
   1 x(5) x(5)^2 x(5)^3;];
%A矩阵，即为所求的C0到C3，对应A矩阵中的A(1)到A(4)
A=(X'*X)\(X'*y);
%由于rand(1,1)*0.5所带来的干扰，A(1)到A(4)分别为1.7507,0.0876，03524，-0.0068


%画图
xA=0.1:0.1:6;
yA=zeros(60,1);
for i=1:60
    yA(i)=A(1)+A(2)*xA(i)+A(3)*xA(i)^2+A(4)*xA(i)^3;
end
%三次多项式拟合曲线
plot(xA,yA);
hold on;
%5个采样点的散点图
scatter(x,y);