openpilot v0.3.7 release

old-commit-hash: daf54ad54d
2026-08-23 17:23:44 +08:00 · 2017-09-30 03:07:27 -07:00
parent 7a6d152ae0
commit 2c0cc6103a
130 changed files with 3557 additions and 1717 deletions
@@ -0,0 +1,137 @@
+#include <acado_code_generation.hpp>
+
+#define PI 3.1415926536
+#define deg2rad(d) (d/180.0*PI)
+
+const int controlHorizon = 50;
+const double samplingTime = 0.05; // 20 Hz
+
+using namespace std;
+
+int main( )
+{
+  USING_NAMESPACE_ACADO
+
+
+  DifferentialEquation f;
+
+  DifferentialState xx; // x position
+  DifferentialState yy; // y position
+  DifferentialState psi; // vehicle heading
+  DifferentialState delta;
+
+  OnlineData curvature_factor;
+  OnlineData v_ref; // m/s
+  OnlineData l_poly_r0, l_poly_r1, l_poly_r2, l_poly_r3;
+  OnlineData r_poly_r0, r_poly_r1, r_poly_r2, r_poly_r3;
+  OnlineData p_poly_r0, p_poly_r1, p_poly_r2, p_poly_r3;
+  OnlineData l_prob, r_prob, p_prob;
+  OnlineData lane_width;
+
+  Control t;
+
+  // Equations of motion
+  f << dot(xx) == v_ref * cos(psi);
+  f << dot(yy) == v_ref * sin(psi);
+  f << dot(psi) == v_ref * delta * curvature_factor;
+  f << dot(delta) == t;
+
+  auto lr_prob = l_prob + r_prob - l_prob * r_prob;
+
+  auto poly_l = l_poly_r0*(xx*xx*xx) + l_poly_r1*(xx*xx) + l_poly_r2*xx + l_poly_r3;
+  auto poly_r = r_poly_r0*(xx*xx*xx) + r_poly_r1*(xx*xx) + r_poly_r2*xx + r_poly_r3;
+  auto poly_p = p_poly_r0*(xx*xx*xx) + p_poly_r1*(xx*xx) + p_poly_r2*xx + p_poly_r3;
+
+  auto angle_l = atan(3*l_poly_r0*xx*xx + 2*l_poly_r1*xx + l_poly_r2);
+  auto angle_r = atan(3*r_poly_r0*xx*xx + 2*r_poly_r1*xx + r_poly_r2);
+  auto angle_p = atan(3*p_poly_r0*xx*xx + 2*p_poly_r1*xx + p_poly_r2);
+
+  auto c_left_lane = exp(-(poly_l - yy));
+  auto c_right_lane = exp(poly_r - yy);
+
+  auto r_phantom = poly_l - lane_width/2.0;
+  auto l_phantom = poly_r + lane_width/2.0;
+
+  auto path = lr_prob       * (l_prob * r_phantom + r_prob * l_phantom) / (l_prob + r_prob + 0.0001)
+              + (1-lr_prob) * poly_p;
+
+  auto angle = lr_prob      * (l_prob * angle_l + r_prob * angle_r) / (l_prob + r_prob + 0.0001)
+               + (1-lr_prob) * angle_p;
+
+  // Running cost
+  Function h;
+
+  // Distance errors
+  h << path - yy;
+  h << l_prob * c_left_lane;
+  h << r_prob * c_right_lane;
+
+  // Heading error
+  h << (v_ref + 1.0 ) * (angle - psi);
+
+  // Angular rate error
+  h << (v_ref + 1.0 ) * t;
+
+  DMatrix Q(5,5);
+  Q(0,0) = 1.0;
+  Q(1,1) = 1.0;
+  Q(2,2) = 1.0;
+
+  Q(3,3) = 1.0;
+
+  Q(4,4) = 1.0;
+
+  // Terminal cost
+  Function hN;
+
+  // Distance errors
+  hN << path - yy;
+  hN << l_prob * c_left_lane;
+  hN << r_prob * c_right_lane;
+
+  // Heading errors
+  hN << (2.0 * v_ref + 1.0 ) * (angle - psi);
+
+  DMatrix QN(4,4);
+  QN(0,0) = 1.0;
+  QN(1,1) = 1.0;
+  QN(2,2) = 1.0;
+
+  QN(3,3) = 1.0;
+
+  // Setup Optimal Control Problem
+  const double tStart = 0.0;
+  const double tEnd   = samplingTime * controlHorizon;
+
+  OCP ocp( tStart, tEnd, controlHorizon );
+  ocp.subjectTo(f);
+
+  ocp.minimizeLSQ(Q, h);
+  ocp.minimizeLSQEndTerm(QN, hN);
+
+  ocp.subjectTo( deg2rad(-90) <= psi <= deg2rad(90));
+  ocp.subjectTo( deg2rad(-25) <= delta <= deg2rad(25));
+  ocp.subjectTo( -0.1 <= t <= 0.1);
+  ocp.setNOD(18);
+
+  OCPexport mpc(ocp);
+  mpc.set( HESSIAN_APPROXIMATION, GAUSS_NEWTON );
+  mpc.set( DISCRETIZATION_TYPE, MULTIPLE_SHOOTING );
+  mpc.set( INTEGRATOR_TYPE, INT_RK4 );
+  mpc.set( NUM_INTEGRATOR_STEPS, 250 );
+
+  mpc.set( SPARSE_QP_SOLUTION, CONDENSING );
+  mpc.set( QP_SOLVER, QP_QPOASES );
+  mpc.set( HOTSTART_QP, YES );
+  mpc.set( GENERATE_TEST_FILE, NO);
+  mpc.set( GENERATE_MAKE_FILE, NO );
+  mpc.set( GENERATE_MATLAB_INTERFACE, NO );
+  mpc.set( GENERATE_SIMULINK_INTERFACE, NO );
+
+  if (mpc.exportCode( "mpc_export" ) != SUCCESSFUL_RETURN)
+    exit( EXIT_FAILURE );
+
+  mpc.printDimensionsQP( );
+
+  return EXIT_SUCCESS;
+}