无人驾驶-规划-多项式拟合

一、前言

自动驾驶开发中经常涉及到多项式曲线拟合，本文详细描述了使用最小二乘法进行多项式曲线拟合的数学原理，通过样本集构造范德蒙德矩阵，将一元 N 次多项式非线性回归问题转化为 N 元一次线性回归问题，并基于线性代数 C++ 模板库——Eigen 进行了实现，最后，比较了几种实现方法在求解速度与求解精度上的差异。

二、最小二乘法

最小二乘法（Least Square Method，LSM）通过最小化误差（也叫残差）的平方和寻找数据的最优函数匹配。假设给定一组样本数据集P(x,y)，P点内各数据点Pi(xi, yi)(i = 1,2,3,…m)来自于多项式

的多次采样，其中：m为样本维度，n 为多项式阶数

针对样本数据集P内各数据点的误差平方和为：

最小二乘法：

最优函数的各项系数使得误差平方与平方和取得极小值

三、最小二乘法推导

3.1 代数法

由于最优函数的各项系数需要使得误差平方和S取得极小值，因而，对于最优函数而言，其误差平方和S对各多项式系数的偏导数应满足：

整理上式，j分别取0,1,2…n时，有：

转化为矩阵形式，令：

则有：

使用样本数据集构造出矩阵X和矩阵Y后，便可由上式解得最优函数的系数向量。

3.2 矩阵法

3.2.1 误差平方和S矩阵表示

由于代数法过于繁琐，因此将S拆解为矩阵形式表示：

其中S表示：

3.2.2 最优函数的系数求解

误差平方和S：

Xv是一个范德蒙矩阵（Vandermonde Matrix）, Yr是样本数据集的输出向量。对于最优函数，应满足：

可求得最优函数的多项式系数向量为：

3.2.3 代数法与矩阵法对比

代数法

矩阵法

推导得到：

四、代码实现

4.1 函数声明

#ifndef LEAST_SQUARE_METHOD_H
#define LEAST_SQUARE_METHOD_H

#include <Eigen\Dense>
#include <vector>

using namespace std;

/**
 * @brief 多阶式曲线
 * @param X轴 X-axis 
 * @param Y轴 Y-axis
 * @return Eigen::多阶式拟合曲线
 */
Eigen::VectorXf FitterLeastSquareMethod(vector<float> &X, vector<float> &Y, uint8_t orders);

#endif

4.2 函数实现

#include "LeastSquareMethod.h"

Eigen::VectorXf FitterLeastSquareMethod(vector<float> &X, vector<float> &Y, uint8_t orders)
{
	// 保证数据不为空
    if (X.size() < 2 || Y.size() < 2 || X.size() != Y.size() || orders < 1)
        exit(EXIT_FAILURE);

    // map sample data from STL vector to eigen vector
    Eigen::Map<Eigen::VectorXf> sampleX(X.data(), X.size());
    Eigen::Map<Eigen::VectorXf> sampleY(Y.data(), Y.size());

    Eigen::MatrixXf mtxVandermonde(X.size(), orders + 1);  // Vandermonde matrix of X-axis coordinate vector of sample data
    Eigen::VectorXf colVandermonde = sampleX;              // Vandermonde column

    // construct Vandermonde matrix column by column
    for (size_t i = 0; i < orders + 1; ++i)
    {
        if (0 == i)
        {
            mtxVandermonde.col(0) = Eigen::VectorXf::Constant(X.size(), 1, 1);
            continue;
        }
        if (1 == i)
        {
            mtxVandermonde.col(1) = colVandermonde;
            continue;
        }
        colVandermonde = colVandermonde.array()*sampleX.array();
        mtxVandermonde.col(i) = colVandermonde;
    }

    // calculate coefficients vector of fitted polynomial
    Eigen::VectorXf result = (mtxVandermonde.transpose()*mtxVandermonde).inverse()*(mtxVandermonde.transpose())*sampleY;

    return result;
}

4.3 主函数

// 主函数测试
#include <iostream>
#include "LeastSquareMethod.h"

using namespace std;

int main()
{
    float x[5] = {1, 2, 3, 4, 5};
    float y[5] = {7, 35, 103, 229, 431};

    vector<float> X(x, x + sizeof(x) / sizeof(float));
    vector<float> Y(y, y + sizeof(y) / sizeof(float));

    Eigen::VectorXf result(FitterLeastSquareMethod(X, Y, 3));

    cout << "\nThe coefficients vector is: \n" << endl;
    for (size_t i = 0; i < result.size(); ++i)
        cout << "theta_" << i << ": " << result[i] << endl;

    return 0;
}

五、参考链接

1. https://zhuanlan.zhihu.com/p/268884807