|
|
@@ -0,0 +1,561 @@
|
|
|
+#include "OsqpEigen/OsqpEigen.h"
|
|
|
+#include "dcs/inc/mpc.h"
|
|
|
+#include <Eigen/Dense>
|
|
|
+#include <iostream>
|
|
|
+#include <sstream>
|
|
|
+#include <cstdlib>
|
|
|
+#include <string>
|
|
|
+#include <vector>
|
|
|
+#include <fstream>
|
|
|
+#include <cmath>
|
|
|
+#include <QVector>
|
|
|
+#include <qdebug.h>
|
|
|
+#include <iomanip>
|
|
|
+using namespace std;
|
|
|
+
|
|
|
+//==========================================================
|
|
|
+void DynamicModel(Eigen::Matrix<double, nx, nx>& a, Eigen::Matrix<double, nx, nu>& b, double vel, double delta, double phi, double T,double L)
|
|
|
+{
|
|
|
+
|
|
|
+ a <<1., 0., cos(phi) * T, -sin(phi)* T*vel,
|
|
|
+ 0., 1., sin(phi)* T, cos(phi)* T*vel,
|
|
|
+ 0., 0., 1., 0,
|
|
|
+ 0., 0., (T*tan(delta))/L, 1;
|
|
|
+ b <<0, 0.,
|
|
|
+ 0, 0.,
|
|
|
+ T, 0.,
|
|
|
+ 0., T*vel/(L*cos(delta)*cos(delta));
|
|
|
+}
|
|
|
+
|
|
|
+void InequalityConstraints(Eigen::Matrix<double, nx, 1>& xMax, Eigen::Matrix<double, nx, 1>& xMin,
|
|
|
+ Eigen::Matrix<double, nu, 1>& uMax, Eigen::Matrix<double, nu, 1>& uMin)
|
|
|
+{
|
|
|
+ double u0 = 0;
|
|
|
+
|
|
|
+ uMin << -5 - u0,
|
|
|
+ -0.2 - u0;
|
|
|
+ uMax << 5- u0,
|
|
|
+ 0.2 - u0;
|
|
|
+
|
|
|
+ xMin << -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY, -OsqpEigen::INFTY;
|
|
|
+ xMax << OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY, OsqpEigen::INFTY;
|
|
|
+}
|
|
|
+
|
|
|
+int Roadmode(vector<vector<double>>&ref_traj,int8_t gear_flag,double speed)
|
|
|
+{ int roadmode,roadmode_pre;
|
|
|
+ double maxdelta=0;
|
|
|
+ static int last_mode=3;
|
|
|
+ if(gear_flag==-1)
|
|
|
+ {
|
|
|
+ return 4;
|
|
|
+ }
|
|
|
+
|
|
|
+ if(ref_traj.size()>6)
|
|
|
+ {
|
|
|
+ for(int i=0;i<6;i++)
|
|
|
+ {
|
|
|
+ if((fabs(ref_traj[i][3])*180/PI) > maxdelta)
|
|
|
+ {maxdelta=fabs(ref_traj[i][3])*180/PI;}
|
|
|
+ }
|
|
|
+ }else
|
|
|
+ {
|
|
|
+ maxdelta=0;
|
|
|
+ }
|
|
|
+
|
|
|
+ if (speed<=18/3.6)
|
|
|
+ { //低速情形
|
|
|
+ if(maxdelta>0.8)
|
|
|
+ {roadmode=5;}//低速弯道
|
|
|
+ else
|
|
|
+ {roadmode=6;}//低速直道
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ if(maxdelta>4.1)
|
|
|
+ {roadmode=1;}
|
|
|
+ else if((maxdelta<=4.1)&&(maxdelta>0.25))
|
|
|
+ {roadmode=2;}
|
|
|
+ else
|
|
|
+ {roadmode=3;}
|
|
|
+
|
|
|
+ }
|
|
|
+
|
|
|
+ if(roadmode==3)
|
|
|
+ {
|
|
|
+ if((last_mode==1)||(last_mode==2))
|
|
|
+ {
|
|
|
+ maxdelta=0;
|
|
|
+ for(int i=ref_traj.size()-6;i<ref_traj.size();i++)
|
|
|
+ {
|
|
|
+ if((fabs(ref_traj[i][3])*180/PI) > maxdelta)
|
|
|
+ {maxdelta=fabs(ref_traj[i][3])*180/PI;}
|
|
|
+ }
|
|
|
+ if(maxdelta>4.1)
|
|
|
+ {roadmode_pre=1;}
|
|
|
+ else if((maxdelta<=4.1)&&(maxdelta>0.25))
|
|
|
+ {roadmode_pre=2;}
|
|
|
+ else
|
|
|
+ {roadmode_pre=3;}
|
|
|
+
|
|
|
+ if(roadmode_pre==3)
|
|
|
+ roadmode=3;
|
|
|
+ else
|
|
|
+ roadmode=last_mode;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ if(roadmode==6)
|
|
|
+ {
|
|
|
+ if(last_mode==5)
|
|
|
+ {
|
|
|
+ maxdelta=0;
|
|
|
+ for(int i=ref_traj.size()-6;i<ref_traj.size();i++)
|
|
|
+ {
|
|
|
+ if((fabs(ref_traj[i][3])*180/PI) > maxdelta)
|
|
|
+ {maxdelta=fabs(ref_traj[i][3])*180/PI;}
|
|
|
+ }
|
|
|
+ if(maxdelta>0.8)
|
|
|
+ {roadmode_pre=5;}//低速弯道
|
|
|
+ else
|
|
|
+ {roadmode_pre=6;}//低速直道
|
|
|
+
|
|
|
+ if(roadmode_pre==6)
|
|
|
+ roadmode=6;
|
|
|
+ else
|
|
|
+ roadmode=last_mode;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ last_mode=roadmode;
|
|
|
+ return roadmode;
|
|
|
+
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void WeightMatrices(Eigen::DiagonalMatrix<double, nx>& Q, Eigen::DiagonalMatrix<double, nu>& R,int roadmode,double *_mpc_param,double speed)
|
|
|
+{
|
|
|
+ double speedswitch;
|
|
|
+ switch(roadmode)
|
|
|
+ {
|
|
|
+ case 1:
|
|
|
+ if (speed>30/3.6)
|
|
|
+ {Q.diagonal() << _mpc_param[6], _mpc_param[7], _mpc_param[8], _mpc_param[9];
|
|
|
+ R.diagonal() << _mpc_param[10], _mpc_param[11]; } //a delta}//x y v psi
|
|
|
+ else
|
|
|
+ {speedswitch=_mpc_param[31]*0.5-(speed*3.6-18)/(30-18)*(_mpc_param[31]*0.5-_mpc_param[7]); //20-(35-28)/12*(20-3.35)
|
|
|
+ Q.diagonal() << speedswitch, speedswitch, _mpc_param[8], _mpc_param[9]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[10], _mpc_param[11]; }//a delta
|
|
|
+ break;
|
|
|
+ case 2:
|
|
|
+ if (speed>30/3.6)
|
|
|
+ {Q.diagonal() << _mpc_param[12], _mpc_param[13], _mpc_param[14], _mpc_param[15];
|
|
|
+ R.diagonal() << _mpc_param[16], _mpc_param[17]; } //a delta}//x y v psi
|
|
|
+ else
|
|
|
+ {speedswitch=_mpc_param[31]*0.5-(speed*3.6-18)/(30-18)*(_mpc_param[31]*0.5-_mpc_param[13]); //20-(40-28)/12*(20-2.9)
|
|
|
+ Q.diagonal() << speedswitch, speedswitch, _mpc_param[14], _mpc_param[15]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[16], _mpc_param[17]; }//a delta
|
|
|
+ break;
|
|
|
+ case 3:
|
|
|
+ if (speed>30/3.6)
|
|
|
+ {Q.diagonal() << _mpc_param[0], _mpc_param[1], _mpc_param[2], _mpc_param[3];
|
|
|
+ R.diagonal() << _mpc_param[4], _mpc_param[5]; } //a delta}//x y v psi
|
|
|
+ else
|
|
|
+ {speedswitch=_mpc_param[25]-(speed*3.6-18)/(30-18)*(_mpc_param[25]-_mpc_param[1]); //15-(40-28)/12*13
|
|
|
+ Q.diagonal() << speedswitch, speedswitch, _mpc_param[2], _mpc_param[3]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[4], _mpc_param[5]; }//a delta
|
|
|
+ break;
|
|
|
+ case 4:
|
|
|
+ Q.diagonal() << _mpc_param[18], _mpc_param[19], _mpc_param[20], _mpc_param[21]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[22], _mpc_param[23]; //a delta
|
|
|
+ break;
|
|
|
+ case 5:
|
|
|
+ Q.diagonal() << _mpc_param[30], _mpc_param[31], _mpc_param[32], _mpc_param[33]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[34], _mpc_param[35]; //a delta
|
|
|
+ break;
|
|
|
+ case 6:
|
|
|
+ Q.diagonal() << _mpc_param[24], _mpc_param[25], _mpc_param[26], _mpc_param[27]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[28], _mpc_param[29]; //a delta
|
|
|
+ break;
|
|
|
+ default:
|
|
|
+ Q.diagonal() << _mpc_param[0], _mpc_param[1],_mpc_param[2], _mpc_param[3]; //x y v psi
|
|
|
+ R.diagonal() << _mpc_param[4], _mpc_param[5]; //a delta
|
|
|
+ break;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MPC2QPHessian(const Eigen::DiagonalMatrix<double, nx>& Q, const Eigen::DiagonalMatrix<double, nu>& R,
|
|
|
+ Eigen::SparseMatrix<double>& hessianMatrix)
|
|
|
+{
|
|
|
+ hessianMatrix.resize(nx * (mpcWindow + 1.) + nu * mpcWindow, nx * (mpcWindow + 1.) + nu * mpcWindow);
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 0; i < nx * (mpcWindow + 1) + nu * mpcWindow; i++) {
|
|
|
+ if (i < nx * (mpcWindow + 1)) {
|
|
|
+ int posQ = i % nx;
|
|
|
+ float value = Q.diagonal()[posQ];
|
|
|
+ if (value != 0)
|
|
|
+ hessianMatrix.insert(i, i) = value;
|
|
|
+ }
|
|
|
+ else {
|
|
|
+ int posR = i % nu;
|
|
|
+ float value = R.diagonal()[posR];
|
|
|
+ if (value != 0)
|
|
|
+ hessianMatrix.insert(i, i) = value;
|
|
|
+ }
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MPC2QPGradient(const Eigen::DiagonalMatrix<double, nx> &Q, const Eigen::Matrix<double, nx, mpcWindow+1> &xRef, Eigen::VectorXd &gradient)
|
|
|
+{
|
|
|
+ gradient = Eigen::VectorXd::Zero(nx * (mpcWindow + 1) + nu * mpcWindow, 1);
|
|
|
+
|
|
|
+}
|
|
|
+void MPC2QPConstraintMatrix(const Eigen::Matrix<double, nx, nx>& dynamicMatrix, const Eigen::Matrix<double, nx, nu>& controlMatrix,
|
|
|
+ Eigen::SparseMatrix<double>& constraintMatrix)
|
|
|
+{
|
|
|
+ constraintMatrix.resize(nx * (mpcWindow + 1.) + nx * (mpcWindow + 1) + nu * mpcWindow, nx * (mpcWindow + 1.) + nu * mpcWindow);
|
|
|
+
|
|
|
+
|
|
|
+ for (int i = 0; i < nx * (mpcWindow + 1); i++) {
|
|
|
+ constraintMatrix.insert(i, i) = -1;
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = 0; i < mpcWindow; i++)
|
|
|
+ for (int j = 0; j < nx; j++)
|
|
|
+ for (int k = 0; k < nx; k++) {
|
|
|
+ float value = dynamicMatrix(j, k);
|
|
|
+ if (value != 0) {
|
|
|
+ constraintMatrix.insert(nx * (i + 1) + j, nx * i + k) = value;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = 0; i < mpcWindow; i++)
|
|
|
+ for (int j = 0; j < nx; j++)
|
|
|
+ for (int k = 0; k < nu; k++) {
|
|
|
+ float value = controlMatrix(j, k);
|
|
|
+ if (value != 0) {
|
|
|
+ constraintMatrix.insert(nx * (i + 1) + j, nu * i + k + nx * (mpcWindow + 1)) = value;
|
|
|
+ }
|
|
|
+ }
|
|
|
+
|
|
|
+ for (int i = 0; i < nx * (mpcWindow + 1) + nu * mpcWindow; i++) {
|
|
|
+ constraintMatrix.insert(i + (mpcWindow + 1) * nx, i) = 1;
|
|
|
+ }
|
|
|
+}
|
|
|
+
|
|
|
+void MPC2QPConstraintVectors(const Eigen::Matrix<double, nx, 1>& xMax, const Eigen::Matrix<double, nx, 1>& xMin,
|
|
|
+ const Eigen::Matrix<double, nu, 1>& uMax, const Eigen::Matrix<double, nu, 1>& uMin,
|
|
|
+ const Eigen::Matrix<double, nx, 1>& x0,
|
|
|
+ Eigen::VectorXd& lowerBound, Eigen::VectorXd& upperBound)
|
|
|
+{
|
|
|
+
|
|
|
+ Eigen::VectorXd lowerInequality = Eigen::MatrixXd::Zero(nx * (mpcWindow + 1.) + nu * mpcWindow, 1);
|
|
|
+ Eigen::VectorXd upperInequality = Eigen::MatrixXd::Zero(nx * (mpcWindow + 1.) + nu * mpcWindow, 1);
|
|
|
+ for (int i = 0; i < mpcWindow + 1; i++) {
|
|
|
+ lowerInequality.block(nx * i, 0, nx, 1) = xMin;
|
|
|
+ upperInequality.block(nx * i, 0, nx, 1) = xMax;
|
|
|
+ }
|
|
|
+ for (int i = 0; i < mpcWindow; i++) {
|
|
|
+ lowerInequality.block(nu * i + nx * (mpcWindow + 1), 0, nu, 1) = uMin;
|
|
|
+ upperInequality.block(nu * i + nx * (mpcWindow + 1), 0, nu, 1) = uMax;
|
|
|
+ }
|
|
|
+
|
|
|
+
|
|
|
+ Eigen::VectorXd lowerEquality = Eigen::MatrixXd::Zero(nx * (mpcWindow + 1), 1);
|
|
|
+ Eigen::VectorXd upperEquality;
|
|
|
+ lowerEquality.block(0, 0, nx, 1) = -x0;
|
|
|
+ upperEquality = lowerEquality;
|
|
|
+ lowerEquality = lowerEquality;
|
|
|
+
|
|
|
+
|
|
|
+ lowerBound = Eigen::MatrixXd::Zero(2 * nx * (mpcWindow + 1) + nu * mpcWindow, 1);
|
|
|
+ lowerBound << lowerEquality,
|
|
|
+ lowerInequality;
|
|
|
+
|
|
|
+ upperBound = Eigen::MatrixXd::Zero(2 * nx * (mpcWindow + 1) + nu * mpcWindow, 1);
|
|
|
+ upperBound << upperEquality,
|
|
|
+ upperInequality;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+void UpdateConstraintVectors(const Eigen::Matrix<double, nx, 1>& x0,
|
|
|
+ Eigen::VectorXd& lowerBound, Eigen::VectorXd& upperBound)
|
|
|
+{
|
|
|
+ lowerBound.block(0, 0, nx, 1) = -x0;
|
|
|
+ upperBound.block(0, 0, nx, 1) = -x0;
|
|
|
+}
|
|
|
+
|
|
|
+
|
|
|
+double getErrorNorm(const Eigen::Matrix<double, nx, 1>& x,
|
|
|
+ const Eigen::Matrix<double, nx, 1>& xRef)
|
|
|
+{
|
|
|
+
|
|
|
+ Eigen::Matrix<double, nx, 1> error = x - xRef;
|
|
|
+
|
|
|
+
|
|
|
+ return error.norm();
|
|
|
+}
|
|
|
+/***************************************************************************************************
|
|
|
+***Function Name: mpc_solve
|
|
|
+***Function description: mpc求解器
|
|
|
+***Inputs:
|
|
|
+***
|
|
|
+*** vector<double> &states: 输入变量，states为车体坐标系下车的x(单位m),y(单位m),v(单位m/s),phi(单位rad),其中x,y,phi都为0
|
|
|
+*** vector<vector<double>> &ref_traj: 输入变量，存的为车体坐标系下，参考轨迹每个点的x(ref_traj[i][0]，单位m),y(ref_traj[i][1],单位m),phi(ref_traj[i][2],单位rad),delta(参考的前轮转角phi(ref_traj[i][3]),单位rad)
|
|
|
+*** int8_t gear_flag: 档位标志，D:1,R:-1
|
|
|
+*** double *_mpc_param: mpc的参数，分别为高速直道参数，高速小曲率弯道参数，高速大曲率弯道参数，倒车参数，低速弯道参数，低速直道参数，轴距。详见WeightMatrices函数
|
|
|
+***Outputs:
|
|
|
+*** vector<double> &control_cmd: 输出的控制指令，control_cmd[1]为车前轮转角，单位rad
|
|
|
+*** vector<double> &mpc_xy: 输出的用于显示的预测轨迹，可以不用为其赋值，目的为了观测
|
|
|
+*****************************************************************************************************/
|
|
|
+__attribute__((force_align_arg_pointer)) bool mpc_solve(vector<double> &control_cmd, vector<double> &states, vector<vector<double>> &ref_traj,vector<double> &mpc_xy,int8_t gear_flag,double *_mpc_param){
|
|
|
+ //状态量是x，y，v，phi
|
|
|
+ static int outinit=0;
|
|
|
+ double vel = 0.01;
|
|
|
+ double delta = 0.01;
|
|
|
+ double psi;
|
|
|
+ double T = 0.02;
|
|
|
+ double L = _mpc_param[36];
|
|
|
+
|
|
|
+ Eigen::Matrix<double, nx, nx> a;
|
|
|
+ Eigen::Matrix<double, nx, nu> b;
|
|
|
+
|
|
|
+ Eigen::Matrix<double, nx, 1> xMax;
|
|
|
+ Eigen::Matrix<double, nx, 1> xMin;
|
|
|
+
|
|
|
+ Eigen::Matrix<double, nu, 1> uMax;
|
|
|
+ Eigen::Matrix<double, nu, 1> uMin;
|
|
|
+
|
|
|
+ Eigen::DiagonalMatrix<double, nx> Q;
|
|
|
+ Eigen::DiagonalMatrix<double, nu> R;
|
|
|
+
|
|
|
+
|
|
|
+ Eigen::Matrix<double, nx, 1> x0;
|
|
|
+ Eigen::Matrix<double, nx, mpcWindow+1> xRef; //构造4列的阵
|
|
|
+ Eigen::SparseMatrix<double> hessian;
|
|
|
+ Eigen::VectorXd gradient;
|
|
|
+ Eigen::SparseMatrix<double> linearMatrix;
|
|
|
+ Eigen::VectorXd lowerBound;
|
|
|
+ Eigen::VectorXd upperBound;
|
|
|
+
|
|
|
+
|
|
|
+ int Roadmodecurrent=Roadmode(ref_traj,gear_flag,states[2]);
|
|
|
+ for (int i = 0; i < mpcWindow+1; i++)
|
|
|
+ {
|
|
|
+ xRef(0,i)=0;
|
|
|
+ xRef(1,i)=0;
|
|
|
+ xRef(2,i)=0;
|
|
|
+ xRef(3,i)=0;
|
|
|
+ }
|
|
|
+ vel=gear_flag*states[2];
|
|
|
+ psi=0;
|
|
|
+ delta=gear_flag*ref_traj[0][3];
|
|
|
+
|
|
|
+ x0 << 0-ref_traj[0][0], 0-ref_traj[0][1], 0, 0-ref_traj[0][2];//这里改过了，states[0/1]是大地坐标系减过初始值的
|
|
|
+
|
|
|
+ DynamicModel(a, b, vel, delta, psi, T,L);
|
|
|
+ InequalityConstraints(xMax, xMin, uMax, uMin);
|
|
|
+ WeightMatrices(Q, R, Roadmodecurrent,_mpc_param,states[2]);//采用参考轨迹的下2个点来决定模式
|
|
|
+
|
|
|
+
|
|
|
+ MPC2QPHessian(Q, R, hessian);
|
|
|
+ MPC2QPGradient(Q, xRef, gradient);
|
|
|
+ MPC2QPConstraintMatrix(a, b, linearMatrix);
|
|
|
+ MPC2QPConstraintVectors(xMax, xMin, uMax, uMin, x0, lowerBound, upperBound);
|
|
|
+
|
|
|
+ OsqpEigen::Solver solver;
|
|
|
+ solver.settings()->setWarmStart(true);
|
|
|
+
|
|
|
+
|
|
|
+ solver.data()->setNumberOfVariables(nx * (mpcWindow + 1) + nu * mpcWindow);
|
|
|
+ solver.data()->setNumberOfConstraints(2 * nx * (mpcWindow + 1) + nu * mpcWindow);
|
|
|
+
|
|
|
+ if (!solver.data()->setGradient(gradient)) return 1;
|
|
|
+ if (!solver.data()->setHessianMatrix(hessian)) return 1;
|
|
|
+ if (!solver.data()->setLinearConstraintsMatrix(linearMatrix)) return 1;
|
|
|
+ if (!solver.data()->setLowerBound(lowerBound)) return 1;
|
|
|
+ if (!solver.data()->setUpperBound(upperBound)) return 1;
|
|
|
+
|
|
|
+
|
|
|
+ if (!solver.initSolver()) return 1;
|
|
|
+
|
|
|
+ Eigen::VectorXd ctr;
|
|
|
+ Eigen::VectorXd QPSolution;
|
|
|
+ Eigen::VectorXd state_osqp;
|
|
|
+ if (!solver.solve()) return 1;
|
|
|
+ QPSolution = solver.getSolution();
|
|
|
+ ctr = QPSolution.block(nx * (mpcWindow + 1), 0, nu*mpcWindow, 1);
|
|
|
+ state_osqp = QPSolution.block(0, 0, nx * (mpcWindow + 1), 1);
|
|
|
+
|
|
|
+ //输出处理方式1，最终输出进行拟合
|
|
|
+ int fitting_num=20;//存储前十次的输出
|
|
|
+ int fitting_numgs=10;//存储前十次的输出
|
|
|
+ int fitting_order=3;//拟合曲线的项数（三阶曲线值是四，二阶是三）
|
|
|
+ static double delta_y[20];//存储前num次的输出值，这里是存计算值还是输出值还有待考虑
|
|
|
+ static double delta_x[20];//x轴值（数字编号）
|
|
|
+ static double delta_ygs[10];//存储前num次的输出值，这里是存计算值还是输出值还有待考虑
|
|
|
+ static double delta_xgs[10];//x轴值（数字编号）
|
|
|
+ double poly_result[10];//返回拟合多项式系数，由低到高，poly_result[0]常数项
|
|
|
+ double fitting_effect[3];//dt[0]返回拟合多项式与各数据点误差的平方和；dt[1]返回拟合多
|
|
|
+ //项式与各数据点的误差绝对值之和；dt[2]返回拟合多项式与各数据
|
|
|
+ //点误差绝对值的最大值
|
|
|
+ outinit=outinit+1;
|
|
|
+
|
|
|
+ delta_x[0]=1; //给x,y循环赋值
|
|
|
+ for (int i=1; i<fitting_num; ++i)
|
|
|
+ { delta_x[i]=i+1; //x赋值自然数1,2,3....
|
|
|
+ delta_y[i-1]=delta_y[i];//依次更新之前的y值
|
|
|
+ }
|
|
|
+ delta_y[fitting_num-1]=ctr[1]+gear_flag*ref_traj[0][3];//更新最后的y值（本次的计算值）
|
|
|
+ for (int i=0;i<fitting_numgs;++i)
|
|
|
+ { delta_xgs[i]=delta_x[i+fitting_num-fitting_numgs] ;
|
|
|
+ delta_ygs[i]=delta_y[i+fitting_num-fitting_numgs] ;
|
|
|
+ }
|
|
|
+ if((Roadmodecurrent==5)||(Roadmodecurrent==6))
|
|
|
+ {
|
|
|
+ PolyFit(delta_x, delta_y, fitting_num, poly_result, fitting_order, fitting_effect);//拟合函数得到结果
|
|
|
+ control_cmd[0] = ctr[1]/PI*180;//显示mpc计算的delta
|
|
|
+ control_cmd[1] = poly_result[0]+poly_result[1]*(20-10.5)
|
|
|
+ +poly_result[2]*(20-10.5)*(20-10.5);
|
|
|
+ }
|
|
|
+ else{
|
|
|
+ PolyFit(delta_xgs,delta_ygs, fitting_numgs, poly_result, fitting_order, fitting_effect);//拟合函数得到结果
|
|
|
+ control_cmd[0] = ctr[1]/PI*180;//显示mpc计算的delta
|
|
|
+ control_cmd[1] = poly_result[0]+poly_result[1]*(10-5.5)
|
|
|
+ +poly_result[2]*(10-5.5)*(10-5.5);//实际控制量输出(这里是二次多项式,(X-Xavr))
|
|
|
+ }
|
|
|
+ if (outinit<20)
|
|
|
+ {control_cmd[0]=0;
|
|
|
+ control_cmd[1]=0;}
|
|
|
+ else {outinit=50;}
|
|
|
+
|
|
|
+ // 将数据写入文件开始
|
|
|
+ ofstream outfile;
|
|
|
+ outfile.open("ctr0427xiashe.txt", ostream::app); /*以添加模式打开文件*/
|
|
|
+ outfile<< setprecision(10)<<"MODE"<< "," <<Roadmodecurrent<< "," << "x=" << "," <<x0[0] << ","<< "y=" << ","<<x0[1] << ","<< "phi="<< ","<< x0[3] << "," << "delta" << "," << delta*180/PI
|
|
|
+ << "," << "mpc"<< ","<<ctr[0]<<","<<ctr[1]*180/PI<<","<<"拟合"<< ","<<control_cmd[1]
|
|
|
+ << "," << "方向盘输出" << "," << 15.97*control_cmd[1]*180/PI
|
|
|
+ << ","<< "gpsnow"<<","<<states[0]<<","<<states[1]<<","<<states[3]<<','<<"速度"<<','<<states[2]*3.6<<','<<endl;
|
|
|
+ outfile.close(); /*关闭文件*/
|
|
|
+ // 将数据写入文件结束
|
|
|
+ return true;
|
|
|
+}
|
|
|
+
|
|
|
+/*****************************************************************************
|
|
|
+***Function Name: PolyFit
|
|
|
+***Function description: 最小二乘法曲线拟合
|
|
|
+***Inputs:
|
|
|
+*** double *x: 存放n个数据点的X坐标
|
|
|
+*** double *y: 存放n个数据点的Y坐标
|
|
|
+*** int n: 给定数据点个数
|
|
|
+*** int m: 拟合多项式的项数，即拟合多项式的最高次为m-1。要求m<=n,且m<=20。若m>n或m>20，则本函数自动按m=min{n,20}处理
|
|
|
+***Outputs:
|
|
|
+*** double *a: 返回m-1次拟合多项式的m个系数
|
|
|
+*** double *dt: dt[0]返回拟合多项式与各数据点误差的平方和；dt[1]返回拟合多项式与各数据点的误差绝对值之和；dt[2]返回拟合多项式与各数据点误差绝对值的最大值
|
|
|
+***注意事项：拟合多项式的形式为 y = a0 + a1*(x-Xavr)...
|
|
|
+*****************************************************************************/
|
|
|
+void PolyFit(double* x, double* y, int n, double* a, int m, double* dt)
|
|
|
+{
|
|
|
+ int i, j, k;
|
|
|
+ double z, p, c, g, q, d1, d2, s[20], t[20], b[20];
|
|
|
+ for (i = 0; i <= m - 1; i++)
|
|
|
+ { a[i] = 0.0;}
|
|
|
+ if (m > n)
|
|
|
+ { m = n; }
|
|
|
+ if (m > 20)
|
|
|
+ { m = 20; }
|
|
|
+ z = 0.0;
|
|
|
+ for (i = 0; i <= n - 1; i++)
|
|
|
+ {
|
|
|
+ z = z + x[i] / (1.0 * n);
|
|
|
+ }
|
|
|
+ b[0] = 1.0;
|
|
|
+ d1 = 1.0 * n;
|
|
|
+ p = 0.0;
|
|
|
+ c = 0.0;
|
|
|
+ for (i = 0; i <= n - 1; i++)
|
|
|
+ {
|
|
|
+ p = p + (x[i] - z);
|
|
|
+ c = c + y[i];
|
|
|
+ }
|
|
|
+ c = c / d1;
|
|
|
+ p = p / d1;
|
|
|
+ a[0] = c * b[0];
|
|
|
+ if (m > 1)
|
|
|
+ {
|
|
|
+ t[1] = 1.0;
|
|
|
+ t[0] = -p;
|
|
|
+ d2 = 0.0;
|
|
|
+ c = 0.0;
|
|
|
+ g = 0.0;
|
|
|
+ for (i = 0; i <= n - 1; i++)
|
|
|
+ {
|
|
|
+ q = x[i] - z - p;
|
|
|
+ d2 = d2 + q * q;
|
|
|
+ c = c + y[i] * q;
|
|
|
+ g = g + (x[i] - z) * q * q;
|
|
|
+ }
|
|
|
+ c = c / d2;
|
|
|
+ p = g / d2;
|
|
|
+ q = d2 / d1;
|
|
|
+ d1 = d2;
|
|
|
+ a[1] = c * t[1];
|
|
|
+ a[0] = c * t[0] + a[0];
|
|
|
+ }
|
|
|
+ for (j = 2; j <= m - 1; j++)
|
|
|
+ {
|
|
|
+ s[j] = t[j - 1];
|
|
|
+ s[j - 1] = -p * t[j - 1] + t[j - 2];
|
|
|
+ if (j >= 3)
|
|
|
+ for (k = j - 2; k >= 1; k--)
|
|
|
+ {
|
|
|
+ s[k] = -p * t[k] + t[k - 1] - q * b[k];
|
|
|
+ }
|
|
|
+ s[0] = -p * t[0] - q * b[0];
|
|
|
+ d2 = 0.0;
|
|
|
+ c = 0.0;
|
|
|
+ g = 0.0;
|
|
|
+ for (i = 0; i <= n - 1; i++)
|
|
|
+ {
|
|
|
+ q = s[j];
|
|
|
+ for (k = j - 1; k >= 0; k--)
|
|
|
+ {
|
|
|
+ q = q * (x[i] - z) + s[k];
|
|
|
+ }
|
|
|
+ d2 = d2 + q * q;
|
|
|
+ c = c + y[i] * q;
|
|
|
+ g = g + (x[i] - z) * q * q;
|
|
|
+ }
|
|
|
+ c = c / d2;
|
|
|
+ p = g / d2;
|
|
|
+ q = d2 / d1;
|
|
|
+ d1 = d2;
|
|
|
+ a[j] = c * s[j];
|
|
|
+ t[j] = s[j];
|
|
|
+ for (k = j - 1; k >= 0; k--)
|
|
|
+ {
|
|
|
+ a[k] = c * s[k] + a[k];
|
|
|
+ b[k] = t[k];
|
|
|
+ t[k] = s[k];
|
|
|
+ }
|
|
|
+ }
|
|
|
+ dt[0] = 0.0;
|
|
|
+ dt[1] = 0.0;
|
|
|
+ dt[2] = 0.0;
|
|
|
+ for (i = 0; i <= n - 1; i++)
|
|
|
+ {
|
|
|
+ q = a[m - 1];
|
|
|
+ for (k = m - 2; k >= 0; k--)
|
|
|
+ {
|
|
|
+ q = a[k] + q * (x[i] - z);
|
|
|
+ }
|
|
|
+ p = q - y[i];
|
|
|
+ if (fabs(p) > dt[2])
|
|
|
+ {
|
|
|
+ dt[2] = fabs(p);
|
|
|
+ }
|
|
|
+ dt[0] = dt[0] + p * p;
|
|
|
+ dt[1] = dt[1] + fabs(p);
|
|
|
+ }
|
|
|
+ return;
|
|
|
+}
|
|
|
+
|
zhangjia