mpc_controller one vehicle control and model research. modify two vehicle model in MPC and it works fine.

MPC/launch/MPC_2.launch

@@ -10,5 +10,6 @@
 
   <node name="map_server" pkg="map_server" type="map_server" args="$(find rbx1_nav)/maps/blank_map.yaml"/>
   <node pkg="MPC" type="MPC2_node" name="MPC2_node" output="screen"/>
+  <node name="rviz" pkg="rviz" type="rviz" args="-d $(find MPC)/launch/default.rviz" />
 
 </launch>

MPC/src/mpc/TowBotMPC.cpp

@@ -10,156 +10,179 @@
 
 using CppAD::AD;
 
-size_t K=10;
+size_t K = 10;
 size_t nx1 = 0;
 size_t ny1 = nx1 + K;
 size_t nth1 = ny1 + K;
 size_t nv1 = nth1 + K;
 size_t nvth1 = nv1 + K;
+size_t nacc1 = nvth1 + K;
+size_t naccth1 = nacc1 + K - 1;
 
-
-size_t nx2 = nvth1 + K;
+size_t nx2 = naccth1 + K - 1;
 size_t ny2 = nx2 + K;
 size_t nth2 = ny2 + K;
 size_t nv2 = nth2 + K;
 size_t nvth2 = nv2 + K;
+size_t nacc2 = nvth2 + K;
+size_t naccth2 = nacc2 + K - 1;
+
+size_t relative_pose_contrains = naccth2 + K - 1;
+
+double Lx1 = -1.3;
+double Ly1 = 0;
+double Lx2 = 1.3;
+double Ly2 = 0;
 
-double Lx1=-1.3;
-double Ly1=0;
-double Lx2=1.3;
-double Ly2=0;
+double ref_dist = 2.0;
+// double min_dist = 1.5;
+// double max_dist = 3.0;
 
-class FG_eval_2 {
+class FG_eval_2
+{
 public:
     // Fitted polynomial coefficients
-    Eigen::VectorXd coeffs,coeffs_b;
+    Eigen::VectorXd coeffs, coeffs_b;
     Eigen::VectorXd m_data;
-    FG_eval_2(Eigen::VectorXd coeffs,Eigen::VectorXd coeffs_b,Eigen::VectorXd P) {
+    FG_eval_2(Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b, Eigen::VectorXd P)
+    {
         this->coeffs = coeffs;
-        this->coeffs_b=coeffs_b;
-        m_data=P;
+        this->coeffs_b = coeffs_b;
+        m_data = P;
     }
 
     typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
-    void operator()(ADvector& fg, const ADvector& vars) {
-
-        double v1=m_data[0];
-        double v2=m_data[1];
-        double vth1=m_data[2];
-        double vth2=m_data[3];
-        double ref_v=m_data[4];
-        double dt=m_data[5];
-
-        double dx=m_data[6];  /// 两车初始位姿差   前车相对后车坐标系
-        double dy=m_data[7];
-        double dth=m_data[8];   ///两坐标系角度
-
+    void operator()(ADvector &fg, const ADvector &vars)
+    {
 
+        // std::cout << "max size: " << naccth2 + K - 1 << std::endl;
+        double v1 = m_data[0];
+        double v2 = m_data[1];
+        double vth1 = m_data[2];
+        double vth2 = m_data[3];
+        double ref_v = m_data[4];
+        double dt = m_data[5];
+        
+        /// 两车初始位姿差   前车相对后车坐标系
+        double dx = m_data[6]; 
+        double dy = m_data[7];
+        double dth = m_data[8]; ///两坐标系角度
 
         ///前车 cost weight
-        const int y_cost_weight = 1500;
+        const int y_cost_weight = 8000;
         const int th_cost_weight = 1000;
         const int v_cost_weight = 20;
 
         const int vth_cost_weight = 100;
         const int a_cost_weight = 1;
-        const int ath_cost_weight=100;
-
-        ///将坐标旋转到后车坐标系下
-        double new_xt=cos(dth)*Lx1-sin(dth)*Ly1;
-        double new_yt=sin(dth)*Lx1+cos(dth)*Ly1;
-
-        double distance_x=new_xt-Lx2+dx;
-        double distance_y=new_yt-Ly2+dy;
-        printf(" Key Point distance xy: %.3f  %.3f\n",fabs(distance_x),fabs(distance_y));
+        const int ath_cost_weight = 10;
 
         fg[0] = 0;
         ///后车 cost weight
-        const int back_y_cost_weight = 500;
-        const int back_th_cost_weight = 300;
-        const int back_v_cost_weight = 1;
+        const int back_y_cost_weight = 8000;
+        const int back_th_cost_weight = 1000;
+        const int back_v_cost_weight = 20;
 
-        const int back_vth_cost_weight = 3;
+        const int back_vth_cost_weight = 100;
         const int back_a_cost_weight = 1;
-        const int back_ath_cost_weight=5;
+        const int back_ath_cost_weight = 10;
 
-        for (int t = 0; t < K; t++) {
-            AD<double> xt=vars[nx1+t];
+        for (int t = 0; t < K; t++)
+        {
+            // 前车坐标系下，y坐标差
+            AD<double> xt = vars[nx1 + t];
             AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
-            fg[0] += y_cost_weight * CppAD::pow(vars[ny1 + t]-fx, 2);
+            fg[0] += y_cost_weight * CppAD::pow(vars[ny1 + t] - fx, 2);
 
-            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
-            fg[0] += th_cost_weight * CppAD::pow(vars[nth1 + t]-fth, 2);
-            fg[0]+=v_cost_weight*CppAD::pow(vars[nv1+t]-ref_v,2);
-
-            ////   loss back
-            AD<double> xt2=vars[nx2+t];
-            AD<double> fx2 = coeffs[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
-            fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t]-fx2, 2);
+            // 前车坐标系下，theta cost & v cost
+            AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * pow(xt, 2));
+            fg[0] += th_cost_weight * CppAD::pow(vars[nth1 + t] - fth, 2);
+            fg[0] += v_cost_weight * CppAD::pow(vars[nv1 + t] - ref_v, 2);
+            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
 
-            AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2*coeffs_b[2]*xt2 + 3*coeffs_b[3]*pow(xt2,2));
-            fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t]-fth2, 2);
-            fg[0] +=back_v_cost_weight*CppAD::pow(vars[nv2+t]-ref_v,2);
+            // 在前车坐标系下，后车y坐标差
+            AD<double> xt2 = vars[nx2 + t];
+            AD<double> fx2 = coeffs_b[0] + coeffs_b[1] * xt2 + coeffs_b[2] * pow(xt2, 2) + coeffs_b[3] * pow(xt2, 3);
+            fg[0] += back_y_cost_weight * CppAD::pow(vars[ny2 + t] - fx2, 2);
 
+            // 在前车坐标系下，后车theta cost & v cost
+            AD<double> fth2 = CppAD::atan(coeffs_b[1] + 2 * coeffs_b[2] * xt2 + 3 * coeffs_b[3] * pow(xt2, 2));
+            fg[0] += back_th_cost_weight * CppAD::pow(vars[nth2 + t] - fth2, 2);
+            fg[0] += back_v_cost_weight * CppAD::pow(vars[nv2 + t] - ref_v, 2);
+            fg[0] += back_vth_cost_weight * CppAD::pow(vars[nvth2 + t], 2);
         }
         //加速度 cost
         // Costs for steering (delta) and acceleration (a)
-        for (int t = 0; t < K-1; t++) {
-            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
-            fg[0] += a_cost_weight * CppAD::pow(vars[nv1 + t+1]-vars[nv1+t], 2);
-            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth1 + t+1]-vars[nvth1+t], 2);
+        for (int t = 0; t < K - 1; t++)
+        {
+            fg[0] += a_cost_weight * CppAD::pow(vars[nacc1+ t], 2);
+            fg[0] += ath_cost_weight * CppAD::pow(vars[naccth1 + t], 2);
 
-            fg[0] += back_vth_cost_weight * CppAD::pow(vars[nvth2 + t], 2);
-            fg[0] += back_a_cost_weight * CppAD::pow(vars[nv2 + t+1]-vars[nv2+t], 2);
-            fg[0] += back_ath_cost_weight * CppAD::pow(vars[nvth2 + t+1]-vars[nvth2+t], 2);
+            fg[0] += back_a_cost_weight * CppAD::pow(vars[nacc2 + t], 2);
+            fg[0] += back_ath_cost_weight * CppAD::pow(vars[naccth2 + t], 2);
         }
+
         ///添加两车距离损失
-        const int distance_weight_x=10;
-        const int distance_weight_y=50;
-        const int delta_th_weight=1;
-        for(int t=0;t<K;++t)
+        const int distance_weight = 800;
+        const int delta_th_weight = 10;
+        char buf[255]={0};
+        AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
+        for (int t = 0; t < K; ++t)
         {
-            AD<double> xt=vars[nx1+t]+Lx1;
-            AD<double> yt=vars[ny1+t]+Ly1;
+            AD<double> x1_in_front = vars[nx1 + t];
+            AD<double> y1_in_front = vars[ny1 + t];
             ///将坐标旋转到后车坐标系下
-            AD<double> new_xt=CppAD::cos(dth)*xt-CppAD::sin(dth)*yt;
-            AD<double> new_yt=CppAD::sin(dth)*xt+CppAD::cos(dth)*yt;
-
-            AD<double> distance_x=new_xt-(vars[nx2+t]+Lx2)+dx;
-            AD<double> distance_y=new_yt-(vars[ny2+t]+Ly2)+dy;
+            AD<double> x1_in_back = CppAD::cos(dtheta) * x1_in_front - CppAD::sin(dtheta) * y1_in_front + dx;
+            AD<double> y1_in_back = CppAD::sin(dtheta) * x1_in_front + CppAD::cos(dtheta) * y1_in_front + dy;
 
-            fg[0]+=distance_weight_x*CppAD::pow(distance_x,2);//+distance_weight_y*CppAD::pow(distance_y,2);
-            fg[0]+=delta_th_weight*CppAD::pow(vars[nth1+t]-vars[nth2+t]+dth,2);
+            AD<double> distance_x = x1_in_back - vars[nx2 + t];
+            AD<double> distance_y = y1_in_back - vars[ny2 + t];
 
+            fg[0] += distance_weight * CppAD::pow(CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2)) - ref_dist, 2);
+            fg[0] += delta_th_weight * CppAD::pow(CppAD::atan(y1_in_back / x1_in_back), 2);
+            sprintf(buf+strlen(buf), " %.4f ", sqrt(pow(distance_x,2)+pow(distance_y,2)));
+            // // 两车最短距离约束
+            // fg[1 + relative_pose_contrains + t - 4*(K-1)] = CppAD::sqrt(CppAD::pow(distance_x, 2) + CppAD::pow(distance_y, 2));
         }
+        std::cout<<buf<<std::endl;
 
-        /////////////////////
-        fg[1 + nx1] = vars[nx1]-vars[nv1]*dt;
+        // x, y, theta 路径约束
+        fg[1 + nx1] = vars[nx1];// - vars[nv1] * dt;
         fg[1 + ny1] = vars[ny1];
-        fg[1 + nth1] = vars[nth1]-vars[nvth1]*dt;
+        fg[1 + nth1] = vars[nth1];// - vars[nvth1] * dt;
+        fg[1 + nv1] = vars[nv1];
+        fg[1 + nvth1] = vars[nvth1];
 
-        fg[1 + nx2] = vars[nx2]-vars[nv2]*dt;
-        fg[1 + ny2] = vars[ny2];
-        fg[1 + nth2] = vars[nth2]-vars[nvth2]*dt;
+        fg[1 + nx2 - 2*(K-1)] = vars[nx2];// - vars[nv2] * dt;
+        fg[1 + ny2 - 2*(K-1)] = vars[ny2];
+        fg[1 + nth2 - 2*(K-1)] = vars[nth2];// - vars[nvth2] * dt;
+        fg[1 + nv2 - 2*(K-1)] = vars[nv2];
+        fg[1 + nvth2 - 2*(K-1)] = vars[nvth2];
 
         // The rest of the constraints
-        for (int t = 1; t < K; t++) {
+        for (int t = 1; t < K; t++)
+        {
             /// 位置约束 1
-            AD<double> x1 = vars[nx1 + t];
-            AD<double> y1 = vars[ny1 + t];
-            AD<double> th1 = vars[nth1 + t];
-            AD<double> v1 = vars[nv1 + t];
-            AD<double> vth1 = vars[nvth1 + t];
-
-            AD<double> x0 = vars[nx1 + t -1];
-            AD<double> y0 = vars[ny1 + t -1];
-            AD<double> th0 = vars[nth1 + t-1];
-            AD<double> v0 = vars[nv1 + t-1];
-            AD<double> vth0 = vars[nvth1 + t-1];
-
-            fg[1 + nx1 + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
-            fg[1 + ny1 + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
-            fg[1 + nth1 + t] = th1 - (th0 + vth0 * dt);
+            AD<double> fx1 = vars[nx1 + t];
+            AD<double> fy1 = vars[ny1 + t];
+            AD<double> fth1 = vars[nth1 + t];
+            AD<double> fv1 = vars[nv1 + t];
+            AD<double> fvth1 = vars[nvth1 + t];
+
+            AD<double> fx0 = vars[nx1 + t - 1];
+            AD<double> fy0 = vars[ny1 + t - 1];
+            AD<double> fth0 = vars[nth1 + t - 1];
+            AD<double> fv0 = vars[nv1 + t - 1];
+            AD<double> fvth0 = vars[nvth1 + t - 1];
+
+            AD<double> facc0 = vars[nacc1 + t - 1];
+            AD<double> faccth0 = vars[naccth1 + t - 1];
+
+            fg[1 + nx1 + t] = fx1 - (fx0 + fv0 * CppAD::cos(fth0) * dt);
+            fg[1 + ny1 + t] = fy1 - (fy0 + fv0 * CppAD::sin(fth0) * dt);
+            fg[1 + nth1 + t] = fth1 - (fth0 + fvth0 * dt);
+            fg[1 + nv1 + t] = fv1 - (fv0 + facc0 * dt);
+            fg[1 + nvth1 + t] = fvth1 - (fvth0 + faccth0 * dt);
 
             //位置约束2
             AD<double> bx1 = vars[nx2 + t];
@@ -168,115 +191,125 @@ public:
             AD<double> bv1 = vars[nv2 + t];
             AD<double> bvth1 = vars[nvth2 + t];
 
-            AD<double> bx0 = vars[nx2 + t -1];
-            AD<double> by0 = vars[ny2 + t -1];
-            AD<double> bth0 = vars[nth2 + t-1];
-            AD<double> bv0 = vars[nv2 + t-1];
-            AD<double> bvth0 = vars[nvth2 + t-1];
+            AD<double> bx0 = vars[nx2 + t - 1];
+            AD<double> by0 = vars[ny2 + t - 1];
+            AD<double> bth0 = vars[nth2 + t - 1];
+            AD<double> bv0 = vars[nv2 + t - 1];
+            AD<double> bvth0 = vars[nvth2 + t - 1];
 
-            fg[1 + nx2 + t] = bx1 - (bx0 + bv0 * CppAD::cos(bth0) * dt);
-            fg[1 + ny2 + t] = by1 - (by0 + bv0 * CppAD::sin(bth0) * dt);
-            fg[1 + nth2 + t] = bth1 - (bth0 + bvth0 * dt);
+            AD<double> bacc0 = vars[nacc2 + t - 1];
+            AD<double> baccth0 = vars[naccth2 + t - 1];
 
+            fg[1 + nx2 + t - 2*(K-1)] = bx1 - (bx0 + bv0 * CppAD::cos(bth0) * dt);
+            fg[1 + ny2 + t - 2*(K-1)] = by1 - (by0 + bv0 * CppAD::sin(bth0) * dt);
+            fg[1 + nth2 + t - 2*(K-1)] = bth1 - (bth0 + bvth0 * dt);
+            fg[1 + nv2 + t - 2*(K-1)] = bv1 - (bv0 + bacc0 * dt);
+            fg[1 + nvth2 + t - 2*(K-1)] = bvth1 - (bvth0 + baccth0 * dt);
         }
-
-        fg[1 + nv1]=(vars[nv1]-v1)/dt;
-        fg[1+nvth1]=(vars[nvth1]-vth1)/dt;
-
-        fg[1 + nv2]=(vars[nv2]-v2)/dt;
-        fg[1+nvth2]=(vars[nvth2]-vth2)/dt;
-        for(int t=1;t<K;++t)
-        {
-            fg[1+nv1+t]=(vars[nv1+t]-vars[nv1+t-1])/dt;
-            fg[1+nvth1+t]=(vars[nvth1+t]-vars[nvth1+t-1])/dt;
-            fg[1+nv2+t]=(vars[nv2+t]-vars[nv2+t-1])/dt;
-            fg[1+nvth2+t]=(vars[nvth2+t]-vars[nvth2+t-1])/dt;
-        }
-
     }
 };
 
-bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v,Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b,std::vector<double> &out)
+bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeffs, Eigen::VectorXd coeffs_b, std::vector<double> &out)
 {
     bool ok = true;
     typedef CPPAD_TESTVECTOR(double) Dvector;
     // State vector holds all current values neede for vars below
 
-    double v1=state[0];
-    double v2=state[1];
-    double vth1=state[2];
-    double vth2=state[3];
-    double ref_v=state[4];
-    double dt=state[5];
+    // std::cout<<"state: "<<state<<std::endl;
+    double v1 = state[0];
+    double v2 = state[1];
+    double vth1 = state[2];
+    double vth2 = state[3];
+    double ref_v = state[4];
+    double dt = state[5];
 
-    double dx=state[6];  /// 两车初始位姿差   前车相对后车坐标系
-    double dy=state[7];
-    double dth=state[8];   ///两坐标系角度
+    // double dx = state[6]; /// 两车初始位姿差   前车相对后车坐标系
+    // double dy = state[7];
+    // double dth = state[8]; ///两坐标系角度
 
-    size_t n_vars = K * 5 *2;
-    size_t n_constraints = K * 5 *2;
+    size_t n_vars = 2 * (K * 5 + (K - 1) * 2);
+    size_t n_constraints = K * (5 * 2 + 0);
 
     Dvector vars(n_vars);
-    for (int i = 0; i < n_vars; i++) {
+    for (int i = 0; i < n_vars; i++)
+    {
         vars[i] = 0.0;
     }
-    for(int i=0;i<K;++i)
+    for (int i = 0; i < K; ++i)
     {
-        vars[nv1+i]=v1;
-        vars[nv2+i]=v2;
+        vars[nv1 + i] = v1;
+        vars[nv2 + i] = v2;
 
-        vars[nvth1+i]=vth1;
-        vars[nvth2+i]=vth2;
+        vars[nvth1 + i] = vth1;
+        vars[nvth2 + i] = vth2;
     }
 
     Dvector vars_lowerbound(n_vars);
     Dvector vars_upperbound(n_vars);
 
-    for (int i = 0; i < n_vars; i++) {
+    for (int i = 0; i < n_vars; i++)
+    {
         vars_lowerbound[i] = -1.0e19;
         vars_upperbound[i] = 1.0e19;
     }
 
-    for (int i = 0; i < K; i++) {
+    for (int i = 0; i < K; i++)
+    {
         //// limit  v1 v2
-        vars_lowerbound[nv1+i] = -max_v;
-        vars_upperbound[nv1+i] = max_v;
+        vars_lowerbound[nv1 + i] = -max_v;
+        vars_upperbound[nv1 + i] = max_v;
 
-        vars_lowerbound[nv2+i] = -max_v;
-        vars_upperbound[nv2+i] = max_v;
+        vars_lowerbound[nv2 + i] = -max_v;
+        vars_upperbound[nv2 + i] = max_v;
 
         //// limit  vth1 2
-        vars_lowerbound[nvth1+i] = -0.5;
-        vars_upperbound[nvth1+i] = 0.5;
+        vars_lowerbound[nvth1 + i] = -0.5;
+        vars_upperbound[nvth1 + i] = 0.5;
 
-        vars_lowerbound[nvth2+i] = -0.5;
-        vars_upperbound[nvth2+i] = 0.5;
+        vars_lowerbound[nvth2 + i] = -0.5;
+        vars_upperbound[nvth2 + i] = 0.5;
     }
+    for (int i = 0; i < K - 1; i++)
+    {
+        //// limit acc1 acc2
+        vars_lowerbound[nacc1 + i] = -5.0;
+        vars_upperbound[nacc2 + i] = 5.0;
+
+        //// limit accth1 accth2
+        vars_lowerbound[naccth1 + i] = -0.05;//3度
+        vars_upperbound[naccth2 + i] = 0.05;
+    }
+    
+
     ////   constraints
     Dvector constraints_lowerbound(n_constraints);
     Dvector constraints_upperbound(n_constraints);
-    for (int i = 0; i < n_constraints; i++) {
+    for (int i = 0; i < n_constraints; i++)
+    {
         constraints_lowerbound[i] = 0;
         constraints_upperbound[i] = 0;
     }
 
-    for (int i = 0; i < K; i++) {
+    for (int i = 0; i < K; i++)
+    {
         ////acc v1 v2
-        constraints_lowerbound[nv1+i] = -5.0;
-        constraints_upperbound[nv1+i] = 5.0;
-        constraints_lowerbound[nv2+i] = -5.0;
-        constraints_upperbound[nv2+i] = 5.0;
+        constraints_lowerbound[nv1 + i] = -3.0;
+        constraints_upperbound[nv1 + i] = 3.0;
+        constraints_lowerbound[nv2 + i - 2*(K-1)] = -3.0;
+        constraints_upperbound[nv2 + i - 2*(K-1)] = 3.0;
 
         //acc th 1 2
-        constraints_lowerbound[nvth1+i] = -2.5;
-        constraints_upperbound[nvth1+i] = 2.5;
-        constraints_lowerbound[nvth2+i] = -2.5;
-        constraints_upperbound[nvth2+i] = 2.5;
+        constraints_lowerbound[nvth1 + i] = -0.1;
+        constraints_upperbound[nvth1 + i] = 0.1;
+        constraints_lowerbound[nvth2 + i - 2*(K-1)] = -0.1;
+        constraints_upperbound[nvth2 + i - 2*(K-1)] = 0.1;
+
+        // constraints_lowerbound[relative_pose_contrains + i - 4*(K-1)] = min_dist;
+        // constraints_upperbound[relative_pose_contrains + i - 4*(K-1)] = max_dist;
     }
 
     // object that computes objective and constraintsv
-    FG_eval_2 fg_eval(coeffs,coeffs_b,state);
-
+    FG_eval_2 fg_eval(coeffs, coeffs_b, state);
 
     std::string options;
     options += "Integer print_level  0\n";
@@ -286,7 +319,6 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v,Eigen::VectorXd coeffs
 
     CppAD::ipopt::solve_result<Dvector> solution;
 
-
     CppAD::ipopt::solve<Dvector, FG_eval_2>(
         options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
         constraints_upperbound, fg_eval, solution);
@@ -305,23 +337,24 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v,Eigen::VectorXd coeffs
     out.push_back(solution.x[nvth1]);
     out.push_back(solution.x[nv2]);
     out.push_back(solution.x[nvth2]);
-    for (int i = 0; i < K; ++i) {
-        tf::Pose pose1,pose2;
-        pose1.setOrigin(tf::Vector3(solution.x[nx1+i],solution.x[ny1+i],0));
-        pose2.setOrigin(tf::Vector3(solution.x[nx2+i],solution.x[ny2+i],0));
+    for (int i = 0; i < K; ++i)
+    {
+        tf::Pose pose1, pose2;
+        pose1.setOrigin(tf::Vector3(solution.x[nx1 + i], solution.x[ny1 + i], 0));
+        pose2.setOrigin(tf::Vector3(solution.x[nx2 + i], solution.x[ny2 + i], 0));
         m_trajectory_f.push_back(pose1);
         m_trajectory_b.push_back(pose2);
     }
 
-    /*printf("Cost : %.3f ref:%.3f  cv : %.3f  cvth : %.3f  ||  v2:%.3f  th2:%.3f\n",solution.obj_value,
-        ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);*/
+    // printf("Cost : %.3f ref:%.3f  cv : %.3f  cvth : %.3f  ||  v2:%.3f  th2:%.3f\n",solution.obj_value,
+    //     ref_v,v2,vth2,solution.x[nv2],solution.x[nvth2]);
 
     return ok;
-
 }
 
 TowBotMPC::TowBotMPC()
-{}
+{
+}
 TowBotMPC::~TowBotMPC()
-{}
-
+{
+}

MPC/src/node2.cpp

@@ -35,6 +35,7 @@ FourthTrajectory maker;
 mpc_tools mpc_tool;
 double g_max_vx;
 
+// 目标设置回调
 void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
 {
     printf("new goal recieve ...  \n");
@@ -55,7 +56,7 @@ void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
     g_direction=distance1>distance2?-1:1;
     ///更新轨迹
 
-    if(maker.Make(start_point,g_goal,50)) {
+    if(maker.Make(start_point,g_goal,30)) {
         nav_msgs::Path gui_path;
         gui_path.poses.resize(maker.Size());
         gui_path.header.frame_id = "map";
@@ -80,6 +81,7 @@ void goalCallback(const geometry_msgs::PoseStamped::ConstPtr& msg)
 
 }
 
+// 一号车位姿回调
 void poseCallback1(const turtlesim::Pose::ConstPtr& msg)
 {
     double theta=msg->theta;
@@ -115,6 +117,7 @@ void poseCallback1(const turtlesim::Pose::ConstPtr& msg)
 
 }
 
+// 二号车回调
 void poseCallback2(const turtlesim::Pose::ConstPtr& msg)
 {
     double theta=msg->theta;
@@ -165,7 +168,7 @@ bool MPC()
 
     std::string tf_error;
 
-    int fitNum = 10;
+    int fitNum = 6;
     if (g_goal_valid == false || maker.Size() < fitNum) return false;
 
 //////判断是否到达目标点
@@ -177,6 +180,7 @@ bool MPC()
     double theta_b;
     g_pose_b.getBasis().getEulerZYX(theta_b, a, b);
 
+    // 设置线速度、角速度，以及终点判定
     double dis_th=0,offsetx = 0, offsety = 0,liner_vx=0,angular_vz=0;
     if (g_direction == 1) {
         dis_th=fabs(yaw - theta);
@@ -204,8 +208,6 @@ bool MPC()
         return true;
     }
 
-
-
     //////////////////////////////////////////////////   导航 部分  ///////////////////////////////////
     double dx=offsetx,dy=offsety;
 
@@ -243,7 +245,6 @@ bool MPC()
         }
         //std::cout<<"publish "<<mpc.m_trajectory.size()<<std::endl;
         g_fit_plan_pub.publish(gui_path);
-
     }
     if (!mpc_tool.filterPath(g_pose_b, maker.trajectory_, poses_b, fitNum, direct_x < 0)) {
         ROS_ERROR(" fit error ...");
@@ -272,11 +273,9 @@ bool MPC()
 
     }
 
-
-    //////   fit   select points
+    // 生成参数
     Eigen::VectorXd ptsx_car(poses.size());
     Eigen::VectorXd ptsy_car(poses.size());
-
     for (int i = 0; i < poses.size(); i++) {
         double x = poses[i].getOrigin().getX() - g_pose_f.getOrigin().getX();
         double y = poses[i].getOrigin().getY() - g_pose_f.getOrigin().getY();
@@ -293,7 +292,6 @@ bool MPC()
         ptsx_car_b[i] = x * cos(-theta_b) - y * sin(-theta_b);
         ptsy_car_b[i] = x * sin(-theta_b) + y * cos(-theta_b);
     }
-
     Eigen::VectorXd coeffs_b = mpc_tool.polyfit(ptsx_car_b, ptsy_car_b, 3);
 
     Eigen::VectorXd state(9);
@@ -301,16 +299,22 @@ bool MPC()
     double vx2 = g_cmd_vel_b.linear.x;
     double vth1 = g_cmd_vel_f.angular.z;
     double vth2 = g_cmd_vel_b.angular.z;
-    double Dx = g_pose_f.getOrigin().getX() - g_pose_b.getOrigin().getX();
-    double Dy = g_pose_f.getOrigin().getY() - g_pose_b.getOrigin().getY();
-    double Dth = theta - theta_b;
-
+    double dx0 = g_pose_f.getOrigin().getX() - g_pose_b.getOrigin().getX();
+    double dy0 = g_pose_f.getOrigin().getY() - g_pose_b.getOrigin().getY();
+    double dth1 = theta - theta_b;
+    // 距离反向旋转到后车坐标系
+    double dx1 = dx0 * cos(-theta_b) - dy0 * sin(-theta_b);
+    double dy1 = dx0 * sin(-theta_b) + dy0 * cos(-theta_b);
+    // double frontx = g_pose_f.getOrigin().getX();
+    // double fronty = g_pose_f.getOrigin().getY();
+    // double backx = g_pose_b.getOrigin().getX();
+    // double backy = g_pose_b.getOrigin().getY();
 
     /*printf(" (%.3f %.3f ) , (%.3f %.3f)  (%.3f %.3f | %.3f %.3f)\n",g_pose_f.getOrigin().getX(),
         g_pose_f.getOrigin().getY(),g_pose_b.getOrigin().getX(),g_pose_b.getOrigin().getY(),
         g_cmd_vel_f.linear.x,g_cmd_vel_f.angular.z,g_cmd_vel_b.linear.x,g_cmd_vel_b.angular.z);*/
 
-    state << vx1, vx2, vth1, vth2, ref_v, dt, Dx, Dy, Dth;
+    state << vx1, vx2, vth1, vth2, ref_v, dt, dx1, dy1, dth1;
     TowBotMPC mpc;
     std::vector<double> out;
     if (mpc.Solve(state, g_max_vx, coeffs, coeffs_b, out)) {
@@ -440,9 +444,10 @@ void updateGoal(const geometry_msgs::PoseStamped::ConstPtr& msg)
 
 int main(int argc ,char* argv[])
 {
-    std::cout<<"  input max vx : ";
-    std::cin>>g_max_vx;
-    std::cout<<std::endl;
+    // std::cout<<"  input max vx : ";
+    // std::cin>>g_max_vx;
+    // std::cout<<std::endl;
+    g_max_vx = 2.0;
 
     ros::init(argc, argv, "MPC_node");
 

mpc_control/CMakeLists.txt

@@ -146,15 +146,19 @@ catkin_package(
 # target_link_libraries(laser_scan_matcher ${catkin_LIBRARIES} ${csm_LIBRARIES} )
 
 #Create node
-add_executable(mpc_controller_node src/mpc_controller_node.cpp src/MpcDiffModel.cpp src/make_trajectory.cpp src/mpc_tools.cpp)
-target_link_libraries(mpc_controller_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
-
-add_executable(mpc_two_vehicle_test src/mpc_two_vehicle/trajectory_tool.cpp src/test/two_vehicle_test.cpp)
-target_link_libraries(mpc_two_vehicle_test ${catkin_LIBRARIES} )
-
-aux_source_directory(src/mpc_two_vehicle two_vehicle)
-add_executable(mpc_two_vehicle_node ${two_vehicle})
-target_link_libraries(mpc_two_vehicle_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
+# add_executable(mpc_controller_node src/mpc_controller_node.cpp src/MpcDiffModel.cpp src/make_trajectory.cpp src/mpc_tools.cpp)
+# target_link_libraries(mpc_controller_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
+
+# eigen 路径测试
+# add_executable(mpc_two_vehicle_test src/mpc_two_vehicle/trajectory_tool.cpp src/test/two_vehicle_test.cpp)
+# target_link_libraries(mpc_two_vehicle_test ${catkin_LIBRARIES} )
+
+# 两车模型
+add_executable(mpc_particle_node 
+                src/mpc_two_vehicle/mpc_particle_controller_node.cpp 
+                src/mpc_two_vehicle/mpc_particle_model.cpp 
+                src/mpc_two_vehicle/trajectory_tool.cpp)
+target_link_libraries(mpc_particle_node ${catkin_LIBRARIES} ${PCL_LIBRARIES} ipopt)
 
 # add_executable(mpc_tools src/mpc_tools.cpp)
 # target_link_libraries(mpc_tools ${catkin_LIBRARIES} ${PCL_LIBRARIES})

mpc_control/src/mpc_two_vehicle/MonitorMPC.cpp

@@ -1,245 +0,0 @@
-#include "MonitorMPC.h"
-#include <cppad/cppad.hpp>
-#include <cppad/ipopt/solve.hpp>
-#include "Eigen/Core"
-#include "Eigen/QR"
-
-using CppAD::AD;
-
-// Set the timestep length and duration
-// Currently tuned to predict 1 second worth
-size_t N = 10;
-
-
-// The solver takes all the state variables and actuator
-// variables in a singular vector. Thus, we should to establish
-// when one variable starts and another ends to make our lifes easier.
-size_t nx = 0;
-size_t ny = nx + N;
-size_t nth = ny + N;
-size_t nv = nth + N;
-size_t nvth = nv + N;
-
-
-class FG_eval {
-public:
-    // Fitted polynomial coefficients
-    Eigen::VectorXd coeffs;
-    double m_ref_v,m_dt;
-    double m_v,m_vth;
-    FG_eval(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt) {
-        this->coeffs = coeffs;
-        m_v=v;
-        m_vth=vth;
-        m_ref_v=ref_v;
-        m_dt=dt;
-    }
-
-    typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
-    void operator()(ADvector& fg, const ADvector& vars) {
-        // Implementing MPC below
-        // `fg` a vector of the cost constraints, `vars` is a vector of variable values (state & actuators)
-        // The cost is stored is the first element of `fg`.
-        // Any additions to the cost should be added to `fg[0]`.
-        fg[0] = 0;
-        double dt=m_dt;
-        // Reference State Cost
-        // Below defines the cost related the reference state and
-        // any anything you think may be beneficial.
-
-        // Weights for how "important" each cost is - can be tuned
-        const int y_cost_weight = 1500;
-        const int th_cost_weight = 1000;
-        const int v_cost_weight = 20;
-        const int vth_cost_weight = 100;
-        const int a_cost_weight = 1;
-        const int ath_cost_weight=100;
-
-        // Cost for CTE, psi error and velocity
-        for (int t = 0; t < N; t++) {
-            AD<double> xt=vars[nx+t];
-            AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * pow(xt, 2) + coeffs[3] * pow(xt, 3);
-            fg[0] += y_cost_weight * CppAD::pow(vars[ny + t]-fx, 2);
-
-            AD<double> fth = CppAD::atan(coeffs[1] + 2*coeffs[2]*xt + 3*coeffs[3]*pow(xt,2));
-            fg[0] += th_cost_weight * CppAD::pow(vars[nth + t]-fth, 2);
-
-            fg[0]+=v_cost_weight*CppAD::pow(vars[nv+t]-m_ref_v,2);
-
-        }
-
-        // Costs for steering (delta) and acceleration (a)
-        for (int t = 0; t < N-1; t++) {
-            fg[0] += vth_cost_weight * CppAD::pow(vars[nvth + t], 2);
-            fg[0] += a_cost_weight * CppAD::pow(vars[nv + t+1]-vars[nv+t], 2);
-            fg[0] += ath_cost_weight * CppAD::pow(vars[nvth + t+1]-vars[nvth+t], 2);
-        }
-
-        /////////////////////
-        fg[1 + nx] = vars[nx]-vars[nv]*dt;
-        fg[1 + ny] = vars[ny];
-        fg[1 + nth] = vars[nth]-vars[nvth]*dt;
-
-        // The rest of the constraints
-        for (int t = 1; t < N; t++) {
-            // State at time t + 1
-            AD<double> x1 = vars[nx + t];
-            AD<double> y1 = vars[ny + t];
-            AD<double> th1 = vars[nth + t];
-            AD<double> v1 = vars[nv + t];
-            AD<double> vth1 = vars[nvth + t];
-
-            // State at time t
-            AD<double> x0 = vars[nx + t -1];
-            AD<double> y0 = vars[ny + t -1];
-            AD<double> th0 = vars[nth + t-1];
-            AD<double> v0 = vars[nv + t-1];
-            AD<double> vth0 = vars[nvth + t-1];
-
-            // Actuator constraints at time t only
-
-            // Setting up the rest of the model constraints
-            fg[1 + nx + t] = x1 - (x0 + v0 * CppAD::cos(th0) * dt);
-            fg[1 + ny + t] = y1 - (y0 + v0 * CppAD::sin(th0) * dt);
-            fg[1 + nth + t] = th1 - (th0 + vth0 * dt);
-
-        }
-
-        fg[1 + nv]=(vars[nv]-m_v)/dt;
-        fg[1+nvth]=(vars[nvth]-m_vth)/dt;
-        for(int t=1;t<N;++t)
-        {
-            fg[1+nv+t]=(vars[nv+t]-vars[nv+t-1])/dt;
-            fg[1+nvth+t]=(vars[nvth+t]-vars[nvth+t-1])/dt;
-        }
-
-        ////  add
-
-    }
-};
-
-//
-// MPC class definition implementation.
-//
-MonitorMPC::MonitorMPC() {}
-MonitorMPC::~MonitorMPC() {}
-
-bool MonitorMPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out) {
-    bool ok = true;
-    typedef CPPAD_TESTVECTOR(double) Dvector;
-
-    // State vector holds all current values neede for vars below
-    double v = state[0];
-    double vth = state[1];
-    double ref_v = state[2];
-    double max_v=state[3];
-    double dt=state[4];
-
-
-    // Setting the number of model variables (includes both states and inputs).
-    // N * state vector size + (N - 1) * 2 actuators (For steering & acceleration)
-    size_t n_vars = N * 5;
-    // Setting the number of constraints
-    size_t n_constraints = N * 5;
-
-    // Initial value of the independent variables.
-    // SHOULD BE 0 besides initial state.
-    Dvector vars(n_vars);
-    for (int i = 0; i < n_vars; i++) {
-        vars[i] = 0.0;
-    }
-
-    Dvector vars_lowerbound(n_vars);
-    Dvector vars_upperbound(n_vars);
-    // Sets lower and upper limits for variables.
-    // Set all non-actuators upper and lowerlimits
-    // to the max negative and positive values.
-    for (int i = 0; i < n_vars; i++) {
-        vars_lowerbound[i] = -1.0e19;
-        vars_upperbound[i] = 1.0e19;
-    }
-
-    //// limit  v
-    for (int i = nv; i < nv+N; i++) {
-        vars_lowerbound[i] = -max_v;
-        vars_upperbound[i] = max_v;
-    }
-
-    ////limint vth
-    for (int i = nvth; i < nvth+N; i++) {
-        vars_lowerbound[i] = -0.5;
-        vars_upperbound[i] = 0.5;
-    }
-
-    // Lower and upper limits for the constraints
-    // Should be 0 besides initial state.
-    Dvector constraints_lowerbound(n_constraints);
-    Dvector constraints_upperbound(n_constraints);
-    for (int i = 0; i < n_constraints; i++) {
-        constraints_lowerbound[i] = 0;
-        constraints_upperbound[i] = 0;
-    }
-    //// acc v
-    for (int i = nv; i < nv+N; i++) {
-        constraints_lowerbound[i] = -5.0;
-        constraints_upperbound[i] = 5.0;
-    }
-    //// acc vth
-    for (int i = nvth; i < nvth+N; i++) {
-        constraints_lowerbound[i] = -2.5;
-        constraints_upperbound[i] = 2.5;
-    }
-
-
-    // object that computes objective and constraints
-    FG_eval fg_eval(coeffs,v,vth,ref_v,dt);
-
-    //
-    // NOTE: You don't have to worry about these options
-    //
-    // options for IPOPT solver
-    std::string options;
-    // Uncomment this if you'd like more print information
-    options += "Integer print_level  0\n";
-    // NOTE: Setting sparse to true allows the solver to take advantage
-    // of sparse routines, this makes the computation MUCH FASTER. If you
-    // can uncomment 1 of these and see if it makes a difference or not but
-    // if you uncomment both the computation time should go up in orders of
-    // magnitude.
-    options += "Sparse  true        forward\n";
-    options += "Sparse  true        reverse\n";
-    // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
-    // Change this as you see fit.
-    options += "Numeric max_cpu_time          0.5\n";
-
-    // place to return solution
-    CppAD::ipopt::solve_result<Dvector> solution;
-
-    // solve the problem
-    CppAD::ipopt::solve<Dvector, FG_eval>(
-        options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
-        constraints_upperbound, fg_eval, solution);
-
-    // Check some of the solution values
-    ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
-
-    // Cost
-    auto cost = solution.obj_value;
-    //std::cout << "Cost " << cost << std::endl;
-
-    // Return the first actuator values, along with predicted x and y values to plot in the simulator.
-    out.clear();
-    m_trajectory.clear();
-    out.push_back(solution.x[nv]);
-    out.push_back(solution.x[nvth]);
-    for (int i = 0; i < N; ++i) {
-        out.push_back(solution.x[nx + i]);
-        out.push_back(solution.x[ny + i]);
-        tf::Pose pose;
-        pose.setOrigin(tf::Vector3(solution.x[nx+i],solution.x[ny+i],0));
-        m_trajectory.push_back(pose);
-    }
-    printf(" cost : %.3f\n",cost);
-    return ok;
-
-}

mpc_control/src/mpc_two_vehicle/MonitorMPC.h

@@ -1,24 +0,0 @@
-//
-// Created by zx on 2019/8/28.
-//
-
-#ifndef MONITORMPC_H
-#define MONITORMPC_H
-
-#include <vector>
-#include <Eigen/Core>
-#include <tf/transform_datatypes.h>
-
-class MonitorMPC
-{
-public:
-    MonitorMPC();
-    virtual ~MonitorMPC();
-
-    bool Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs, std::vector<double>& out);
-
-    std::vector<tf::Pose> m_trajectory;
-};
-
-
-#endif //MONITORMPC_H

mpc_control/src/mpc_two_vehicle/mpc_particle_controller_node.cpp

@@ -0,0 +1,227 @@
+#include "mpc_particle_controller_node.h"
+
+mpc_particle_controller::mpc_particle_controller(ros::NodeHandle nh) : nh_(nh)
+{
+  omega = 0;
+  velocity = 0;
+  m_ref_vel = 2.0;
+  m_path_publish_count = 0;
+  last_pos = pcl::PointXYZ(0, 0, 0);
+  curr_pos = pcl::PointXYZ(0, 0, 0);
+
+  //发布与订阅
+  vel_publisher_ = nh_.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
+  global_path_publisher_ = nh_.advertise<nav_msgs::Path>("global_path", 2);
+  local_path_publisher_ = nh_.advertise<nav_msgs::Path>("local_path", 2);
+  pose_sub_ = nh_.subscribe<turtlesim::Pose>("/turtle1/pose", 1, &mpc_particle_controller::poseHandler, this);
+  target_sub_ = nh_.subscribe<geometry_msgs::PoseStamped>("/move_base_simple/goal", 1, &mpc_particle_controller::targetHandler, this);
+}
+
+mpc_particle_controller::~mpc_particle_controller()
+{
+}
+
+// 目标设置回调
+void mpc_particle_controller::targetHandler(const geometry_msgs::PoseStamped::ConstPtr &target_pose_msg)
+{
+  m_edge_mutex.lock();
+  tf::poseMsgToTF(target_pose_msg->pose, m_target_pose);
+  Eigen::Matrix2d base_rotation;
+  Eigen::Matrix2d target_rotation;
+
+  base_rotation << m_current_pose.getBasis()[0][0], m_current_pose.getBasis()[0][1], m_current_pose.getBasis()[1][0], m_current_pose.getBasis()[1][1];
+  target_rotation << m_target_pose.getBasis()[0][0], m_target_pose.getBasis()[0][1], m_target_pose.getBasis()[1][0], m_target_pose.getBasis()[1][1];
+
+  m_base.setIdentity();
+  m_base.rotate(base_rotation);
+  m_base.pretranslate(Eigen::Vector2d(m_current_pose.getOrigin().getX(), m_current_pose.getOrigin().getY()));
+
+  m_target.setIdentity();
+  m_target.rotate(target_rotation);
+  m_target.pretranslate(Eigen::Vector2d(m_target_pose.getOrigin().getX(), m_target_pose.getOrigin().getY()));
+  bool create_trajectory_result = m_traj_tool.create_trajectory(m_base, m_target, m_trajectory, 25);
+  m_edge_mutex.unlock();
+
+  // 发布全局路径
+  if (create_trajectory_result)
+  {
+    m_global_path.header.frame_id = "map";
+    m_global_path.header.seq = ++m_path_publish_count;
+    m_global_path.header.stamp = ros::Time::now();
+    m_global_path.poses.clear();
+    for (size_t i = 0; i < m_trajectory.size(); i++)
+    {
+      geometry_msgs::PoseStamped t_pose;
+      t_pose.header = m_global_path.header;
+      t_pose.pose.position.x = m_trajectory[i].translation().x();
+      t_pose.pose.position.y = m_trajectory[i].translation().y();
+      double theta = atan2(m_trajectory[i].rotation()(1, 0), m_trajectory[i].rotation()(0, 0));
+      t_pose.pose.orientation.w = cos(theta / 2.0);
+      t_pose.pose.orientation.z = sin(theta / 2.0);
+      m_global_path.poses.push_back(t_pose);
+    }
+    global_path_publisher_.publish(m_global_path);
+  }
+}
+
+// 当前位姿消息回调
+void mpc_particle_controller::poseHandler(const turtlesim::Pose::ConstPtr &pose_msg)
+{
+  std::lock_guard<std::mutex> lck(m_edge_mutex);
+  m_current_pose.setOrigin(tf::Vector3(pose_msg->x, pose_msg->y, 0));
+  m_current_pose.setRotation(tf::Quaternion(0, 0, sin(pose_msg->theta/2.0), cos(pose_msg->theta/2.0)));
+
+  std::chrono::time_point<std::chrono::system_clock> start = std::chrono::system_clock::now();
+
+  //发布tf
+  tf::StampedTransform transform_msg(m_current_pose, ros::Time::now(), "map", "base_link");
+  tf_broadcaster_.sendTransform(transform_msg);
+
+  // 空路径
+  if (m_trajectory.size() < 1){
+    // std::cout<<"空路径"<<std::endl;
+    return;
+  }
+
+  // 在目标点附近
+  if((m_current_pose.getOrigin() - m_target_pose.getOrigin()).length() < 0.04){
+    // std::cout<<"目标点附近"<<std::endl;
+    return;
+  }
+
+  // 筛选前方N个点
+  // 路径转换为tf位姿
+  std::vector<tf::Pose> t_poses, t_partial_poses, t_pred_poses;
+  int count = 0;
+  // std::cout<<"current pose:"<<current_pose.getOrigin().getX()<<", "<<current_pose.getOrigin().getY()<<std::endl;
+  for (int i = 0; i < m_trajectory.size(); i++)
+  {
+    t_poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(m_trajectory[i].translation().x(), m_trajectory[i].translation().y(), 0)));
+  }
+  // 计算朝向
+  tf::Vector3 orient(cos(pose_msg->theta), sin(pose_msg->theta), 0.0);
+  tf::Vector3 target_direction = m_target_pose.getOrigin() - m_current_pose.getOrigin();
+  if (filterPath(m_current_pose, t_poses, t_partial_poses, 5, target_direction.dot(orient) > 0))
+  {
+    // 终点附近限制速度
+    if (target_direction.length() < 1)
+    {
+      m_ref_vel *= target_direction.length();
+      m_ref_vel = fmax(fabs(m_ref_vel), 0.04);
+      if (target_direction.length() < 0.03)
+      {
+        m_ref_vel = 0;
+      }
+    }
+    else
+    {
+      m_ref_vel = 2.0;
+    }
+
+    // 调整速度方向
+    if (target_direction.dot(orient) > 0)
+    {
+      m_ref_vel = m_ref_vel < 0 ? -m_ref_vel : m_ref_vel;
+    }
+    else
+    {
+      m_ref_vel = m_ref_vel > 0 ? -m_ref_vel : m_ref_vel;
+    }
+    std::cout<<"m_ref_vel: "<<m_ref_vel<<std::endl;
+
+    double velo = pose_msg->linear_velocity;
+    double velo_angle = pose_msg->angular_velocity;
+
+    // std::cout << "start solve" << std::endl;
+    m_mpc_model.set_global_target(m_target_pose);
+    if (m_mpc_model.Solve(m_ref_vel, t_partial_poses, m_current_pose, velo, velo_angle, t_pred_poses))
+    {
+      // std::cout << "solve complete" << std::endl;
+
+      geometry_msgs::Twist velocity;
+      velocity.linear.x = velo; //(v_left+v_right)/2.0;
+      velocity.linear.y = 0;
+      velocity.linear.z = 0;
+      velocity.angular.x = 0;
+      velocity.angular.y = 0;
+      velocity.angular.z = velo_angle; //- (v_right-v_left)/Lf;//turtlesim uses left hand angle, if angle less than 0, it means turn left.
+      vel_publisher_.publish(velocity);
+    }
+    std::chrono::time_point<std::chrono::system_clock> end1 = std::chrono::system_clock::now();
+    std::cout << "time consumed: " << std::chrono::duration_cast<std::chrono::milliseconds>(end1 - start).count() << "ms."<< std::endl;
+
+    //发布局部路径
+    nav_msgs::Path m_local_path;
+    m_local_path.header.seq = m_path_publish_count;
+    m_local_path.header.stamp = ros::Time::now();
+    m_local_path.header.frame_id = "map";
+    for (size_t i = 2; i < t_pred_poses.size(); i++)
+    {
+      geometry_msgs::PoseStamped temp;
+      temp.header = m_local_path.header;
+      t_pred_poses[i].setRotation(t_pred_poses[i].getRotation().normalize());
+      tf::poseTFToMsg(t_pred_poses[i], temp.pose);
+      temp.pose.position.x += m_current_pose.getOrigin().getX();
+      temp.pose.position.y += m_current_pose.getOrigin().getY();
+      temp.pose.position.z += m_current_pose.getOrigin().getZ();
+      m_local_path.poses.push_back(temp);
+    }
+    local_path_publisher_.publish(m_local_path);
+
+    m_last_theta = pose_msg->theta;
+    last_pos = curr_pos;
+  }else{
+    std::cout<<"filter path failed."<<std::endl;
+  }
+}
+
+//获取小车坐标系中，x前进方向 num 个点，用于拟合
+bool mpc_particle_controller::filterPath(tf::Pose pose, std::vector<tf::Pose> poses, std::vector<tf::Pose> &out, int numOfPoints, bool forward)
+{
+  if (poses.size() < numOfPoints)
+  {
+    std::cout<<"lack of poses."<<std::endl;
+    return false;
+  }
+
+  out.clear();
+  // 平移加反向旋转到小车坐标系
+  for (int i = 0; i < poses.size(); i++)
+  {
+    double x = poses[i].getOrigin().getX() - pose.getOrigin().getX();
+    double y = poses[i].getOrigin().getY() - pose.getOrigin().getY();
+    double trans_x = x * pose.getBasis()[0][0] + y * pose.getBasis()[1][0];
+    double trans_y = x * pose.getBasis()[0][1] + y * pose.getBasis()[1][1];
+    bool direction_condition = forward ? (trans_x >= 0) : (trans_x < 0);
+    if (direction_condition && out.size() < numOfPoints)
+    {
+      tf::Pose point;
+      float nx = trans_x * pose.getBasis()[0][0] + trans_y * pose.getBasis()[0][1];
+      float ny = trans_x * pose.getBasis()[1][0] + trans_y * pose.getBasis()[1][1];
+      point.setOrigin(tf::Vector3(nx + pose.getOrigin().getX(),
+                                  ny + pose.getOrigin().getY(), 0));
+      out.push_back(point);
+    }
+  }
+  if (out.size() <= 0){
+    std::cout<<"cannot find forward poses."<<std::endl;
+    return false;
+  }
+  return true;
+}
+
+int main(int argc, char **argv)
+{
+  ros::init(argc, argv, "mpc_particle_controller_node");
+  ros::NodeHandle nh;
+  mpc_particle_controller controller(nh);
+  ros::Rate loop_rate(15);
+
+  while (ros::ok())
+  {
+    ros::spinOnce();
+    loop_rate.sleep();
+  }
+  //controller.~mpc_particle_controller();
+  return 0;
+}

mpc_control/src/mpc_two_vehicle/mpc_particle_controller_node.h

@@ -0,0 +1,80 @@
+#ifndef MPC_PARTICLE_CONTROLLER_NODE_HH
+#define MPC_PARTICLE_CONTROLLER_NODE_HH 
+
+#include "mpc_particle_model.h"
+#include "trajectory_tool.h"
+
+#include <ros/ros.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <tf/transform_listener.h>
+#include <tf/transform_broadcaster.h>
+#include <tf/transform_datatypes.h>
+#include <geometry_msgs/Twist.h>
+#include <nav_msgs/Path.h>
+
+#include <turtlesim/Pose.h>
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+#include <pcl/point_types.h>
+#include <chrono>
+#include <mutex>
+
+// For converting back and forth between radians and degrees.
+constexpr double pi() { return M_PI; }
+double deg2rad(double x) { return x * pi() / 180; }
+double rad2deg(double x) { return x * 180 / pi(); }
+
+/**
+ * 双车模型控制器
+ * 
+ * */
+class mpc_particle_controller{
+
+public:
+    mpc_particle_controller(ros::NodeHandle nh);
+    ~mpc_particle_controller();
+    // 小乌龟位姿回调，筛选局部路径，mpc优化生成v与vth，下发到小乌龟
+    void poseHandler(const turtlesim::Pose::ConstPtr& pose_msg);
+    // 目标设置回调，rviz上设置目标后，更新全局路径
+    void targetHandler(const geometry_msgs::PoseStamped::ConstPtr& target_pose_msg);
+    // 筛选局部路径
+    bool filterPath(tf::Pose pose,std::vector<tf::Pose> poses, std::vector<tf::Pose>& out, int numOfPoints, bool forward = true);
+    
+public:
+    size_t N = 10;
+    double dt = 0.1;
+
+private:
+    Trajectory_tool m_traj_tool;
+    Mpc_particle_model m_mpc_model;
+    std::mutex m_edge_mutex; // 读写端点位姿互斥锁
+    Eigen::Isometry2d m_base;
+    Eigen::Isometry2d m_target;
+    trajectory_type m_trajectory;
+
+    nav_msgs::Path m_global_path;
+    nav_msgs::Path m_local_path;
+    int m_path_publish_count;
+
+    ros::NodeHandle nh_;
+    tf::TransformBroadcaster tf_broadcaster_;
+    ros::Publisher vel_publisher_;
+    ros::Publisher global_path_publisher_;
+    ros::Publisher local_path_publisher_;
+    ros::Subscriber pose_sub_;
+    ros::Subscriber target_sub_;
+
+    // 从话题获取到的当前位姿
+    tf::Pose m_current_pose, m_target_pose;
+
+    double velocity, omega, m_ref_vel, m_last_theta;
+    pcl::PointXYZ last_pos, curr_pos;
+    std::vector<double> ptsx;
+    std::vector<double> ptsy;
+};
+
+#endif /* MPC_PARTICLE_CONTROLLER_NODE_HH */

mpc_control/src/mpc_two_vehicle/mpc_particle_model.cpp

@@ -0,0 +1,371 @@
+#include "mpc_particle_model.h"
+
+using CppAD::AD;
+
+// 设置静态变量值
+double Mpc_particle_model::ref_v = 2.0;
+double Mpc_particle_model::max_vel = 3.0;
+double Mpc_particle_model::dt = 0.1;
+
+class FG_eval_model
+{
+public:
+  // Fitted polynomial coefficients
+  Eigen::VectorXd coeffs;
+  double m_ref_v, m_dt;
+  double m_v, m_vth;
+  double m_global_target_x, m_global_target_y;
+  FG_eval_model(Eigen::VectorXd coeffs,double v,double vth,double ref_v,double dt, 
+                double global_target_x, double global_target_y)
+  {
+    this->coeffs = coeffs;
+    m_v = v;
+    m_vth = vth;
+    m_ref_v = ref_v;
+    m_dt = dt;
+    m_global_target_x = global_target_x;
+    m_global_target_y = global_target_y;
+  }
+
+  typedef CPPAD_TESTVECTOR(AD<double>) ADvector;
+  void operator()(ADvector &fg, const ADvector &vars)
+  {
+    fg[0] = 0;
+
+    // Weights for how "important" each cost is - can be tuned
+    const int y_cost_weight = 5000;
+    const int th_cost_weight = 1000;
+    const int v_cost_weight = 20;
+    const int vth_cost_weight = 100;
+    const int a_cost_weight = 1;
+    const int ath_cost_weight = 5;
+
+    const int target_cost_weight = 20;
+
+    // // std::cout << "-------------"<< std::endl;
+    // // 增加对接近目标时的速度限制, 接近1m内才生效
+    // AD<double> x0 = vars[Mpc_particle_model::x_start];
+    // AD<double> y0 = vars[Mpc_particle_model::y_start];
+    // AD<double> th0 = vars[Mpc_particle_model::theta_start + Mpc_particle_model::N - 1];
+    // AD<double> v0 = vars[Mpc_particle_model::v_start + Mpc_particle_model::N - 1];
+    // AD<double> the_last_predict_x = x0 + v0 * CppAD::cos(th0) * m_dt * (Mpc_particle_model::N - 1);
+    // AD<double> the_last_predict_y = y0 + v0 * CppAD::sin(th0) * m_dt * (Mpc_particle_model::N - 1);
+    // AD<double> dist = CppAD::sqrt(CppAD::pow(the_last_predict_x - m_global_target_x, 2) + CppAD::pow(the_last_predict_y - m_global_target_y, 2));
+    // AD<double> inv_sigmoid = 1 + CppAD::exp(10 * dist - 10);
+    // AD<double> v_target = m_ref_v * (1 - 1 / inv_sigmoid);
+    // std::cout << "dist, sigmoid: " << dist << ", " << (1 - 1 / inv_sigmoid) * m_ref_v << std::endl;
+    // // fg[0] += target_cost_weight * dist / inv_sigmoid ;
+
+    // Cost for CTE, psi error and velocity
+    for (int t = 0; t < Mpc_particle_model::N; t++)
+    {
+      AD<double> xt = vars[ Mpc_particle_model::x_start + t];
+      AD<double> fx = coeffs[0] + coeffs[1] * xt + coeffs[2] * CppAD::pow(xt, 2) + coeffs[3] * CppAD::pow(xt, 3);
+      fg[0] += y_cost_weight * CppAD::pow(vars[Mpc_particle_model::y_start + t] - fx, 2);
+
+      AD<double> fth = CppAD::atan(coeffs[1] + 2 * coeffs[2] * xt + 3 * coeffs[3] * CppAD::pow(xt, 2));
+      fg[0] += th_cost_weight * CppAD::pow(vars[Mpc_particle_model::theta_start + t] - fth, 2);
+      fg[0] += v_cost_weight * CppAD::pow(vars[Mpc_particle_model::v_start + t] - m_ref_v, 2);
+      fg[0] += vth_cost_weight * CppAD::pow(vars[Mpc_particle_model::vth_start + t], 2);
+      // std::cout<<"x, y, theta, v_ref: "<< xt << ", "<<fx<<", " << fth << ", " << m_ref_v <<std::endl;
+    }
+
+    // Costs for steering (delta) and acceleration (a)
+    for (int t = 0; t < Mpc_particle_model::N - 1; t++)
+    {
+      fg[0] += a_cost_weight * CppAD::pow(vars[Mpc_particle_model::acc_start + t], 2);
+      fg[0] += ath_cost_weight * CppAD::pow(vars[Mpc_particle_model::acc_vth_start + t], 2);
+    }
+
+    /////////////////////
+    fg[1 + Mpc_particle_model::x_start] = vars[Mpc_particle_model::x_start];
+    fg[1 + Mpc_particle_model::y_start] = vars[Mpc_particle_model::y_start];
+    fg[1 + Mpc_particle_model::theta_start] = vars[Mpc_particle_model::theta_start];
+    fg[1 + Mpc_particle_model::v_start] = vars[Mpc_particle_model::v_start];
+    fg[1 + Mpc_particle_model::vth_start] = vars[Mpc_particle_model::vth_start];
+
+    // The rest of the constraints
+    for (int t = 1; t < Mpc_particle_model::N; t++)
+    {
+      // State at time t + 1
+      AD<double> x1 = vars[Mpc_particle_model::x_start + t];
+      AD<double> y1 = vars[Mpc_particle_model::y_start + t];
+      AD<double> th1 = vars[Mpc_particle_model::theta_start + t];
+      AD<double> v1 = vars[Mpc_particle_model::v_start + t];
+      AD<double> vth1 = vars[Mpc_particle_model::vth_start + t];
+
+      // State at time t
+      AD<double> x0 = vars[Mpc_particle_model::x_start + t - 1];
+      AD<double> y0 = vars[Mpc_particle_model::y_start + t - 1];
+      AD<double> th0 = vars[Mpc_particle_model::theta_start + t - 1];
+      AD<double> v0 = vars[Mpc_particle_model::v_start + t - 1];
+      AD<double> vth0 = vars[Mpc_particle_model::vth_start + t - 1];
+
+      // Actuator constraints at time t only
+      AD<double> acc0 = vars[Mpc_particle_model::acc_start + t - 1];
+      AD<double> acc_vth0 = vars[Mpc_particle_model::acc_vth_start + t - 1];
+
+      // Setting up the rest of the model constraints
+      fg[1 + Mpc_particle_model::x_start + t] = x1 - (x0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::cos(th0 + 0.5 * vth0 * m_dt) * m_dt);
+      fg[1 + Mpc_particle_model::y_start + t] = y1 - (y0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::sin(th0 + 0.5 * vth0 * m_dt) * m_dt);
+      // fg[1 + Mpc_particle_model::x_start + t] = x1 - (x0 + v0 * CppAD::cos(th0) * m_dt);
+      // fg[1 + Mpc_particle_model::y_start + t] = y1 - (y0 + v0 * CppAD::sin(th0) * m_dt);
+      fg[1 + Mpc_particle_model::theta_start + t] = th1 - (th0 + vth0 * m_dt);
+      fg[1 + Mpc_particle_model::v_start + t] = v1 - (v0 + acc0 * m_dt);
+      fg[1 + Mpc_particle_model::vth_start + t] = vth1 - (vth0 + acc_vth0 * m_dt);
+    }
+  }
+};
+
+Mpc_particle_model::Mpc_particle_model()
+{
+  // ref_v = 2.0;
+  last_velocity = 0.0;
+  last_velocity_theta = 0.0;
+  m_last_pose.setIdentity();
+  m_global_target.setIdentity();
+  mb_has_global_target = false;
+}
+
+Mpc_particle_model::~Mpc_particle_model()
+{
+}
+
+void Mpc_particle_model::set_global_target(tf::Pose target){
+  m_global_target = target;
+  mb_has_global_target = true;
+}
+
+// 求y
+double Mpc_particle_model::polyeval(Eigen::VectorXd coeffs, double x)
+{
+  double result = 0.0;
+  for (int i = 0; i < coeffs.size(); i++)
+  {
+    result += coeffs[i] * pow(x, i);
+  }
+  return result;
+}
+
+// 求参数
+Eigen::VectorXd Mpc_particle_model::polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order)
+{
+  assert(xvals.size() == yvals.size());
+  assert(order >= 1 && order <= xvals.size() - 1);
+  Eigen::MatrixXd A(xvals.size(), order + 1);
+
+  for (int i = 0; i < xvals.size(); i++)
+  {
+    A(i, 0) = 1.0;
+  }
+
+  for (int j = 0; j < xvals.size(); j++)
+  {
+    for (int i = 0; i < order; i++)
+    {
+      A(j, i + 1) = A(j, i) * xvals(j);
+    }
+  }
+
+  auto Q = A.householderQr();
+  auto result = Q.solve(yvals);
+  return result;
+}
+
+// 输入局部路径，当前位姿，速度与角速度
+// 输出用于下发的速度与角速度
+bool Mpc_particle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref_poses, tf::Pose curr_pose, double &v, double &vth, std::vector<tf::Pose> &pred_poses)
+{
+  ref_v = ref_velocity;
+  // 转换坐标后的局部路径与当前位姿
+  Eigen::VectorXd ptsx_car(ref_poses.size());
+  Eigen::VectorXd ptsy_car(ref_poses.size());
+  double t_global_target_trans_x, t_global_target_trans_y;
+  // 反向旋转
+  for (int i = 0; i < ref_poses.size(); i++)
+  {
+    double t_transformed_x = ref_poses[i].getOrigin().getX() - curr_pose.getOrigin().getX();
+    double t_transformed_y = ref_poses[i].getOrigin().getY() - curr_pose.getOrigin().getY();
+    ptsx_car[i] = t_transformed_x * curr_pose.getBasis()[0][0] + t_transformed_y * curr_pose.getBasis()[1][0];
+    ptsy_car[i] = t_transformed_x * curr_pose.getBasis()[0][1] + t_transformed_y * curr_pose.getBasis()[1][1];
+    // ptsx_car[i] = ref_poses[i].getOrigin().getX();
+    // ptsy_car[i] = ref_poses[i].getOrigin().getY();
+    // std::cout<<"x,y: "<<ptsx_car[i]<<", "<<ptsy_car[i]<<std::endl;
+  }
+  double trans_x = m_global_target.getOrigin().getX() - curr_pose.getOrigin().getX();
+  double trans_y = m_global_target.getOrigin().getY() - curr_pose.getOrigin().getY();
+  t_global_target_trans_x = trans_x * curr_pose.getBasis()[0][0] + trans_y * curr_pose.getBasis()[1][0];
+  t_global_target_trans_y = trans_x * curr_pose.getBasis()[0][1] + trans_y * curr_pose.getBasis()[1][1];
+
+  //拟合求斜率与y方向误差
+  if(ptsx_car.size() < 3 || ptsy_car.size() < 3){
+    std::cout<<"点数过少，无法拟合"<<std::endl;
+    return false;
+  }
+  auto coeffs = polyfit(ptsx_car, ptsy_car, 3);
+  // std::cout<<"coeffs: "<<coeffs<<std::endl;
+  // if(std::isnan(coeffs[0])||std::isnan(coeffs[1])||std::isnan(coeffs[2])||std::isnan(coeffs[3]))
+  //   return;
+
+  bool ok = true;
+  typedef CPPAD_TESTVECTOR(double) Dvector;
+
+  size_t n_vars = N * 5 + (N - 1) * 2;
+
+  size_t n_constraints = N * 5;
+
+  Dvector vars(n_vars);
+  for (int i = 0; i < n_vars; i++)
+  {
+    vars[i] = 0.0;
+  }
+
+  // 参数约束
+  Dvector vars_lowerbound(n_vars);
+  Dvector vars_upperbound(n_vars);
+  // x, y constrains
+  for (int i = 0; i < theta_start; i++)
+  {
+    vars_lowerbound[i] = -1.0e2;
+    vars_upperbound[i] = 1.0e2;
+  }
+  // theta constrains
+  for (int i = theta_start; i < v_start; i++)
+  {
+    vars_lowerbound[i] = -M_PI;
+    vars_upperbound[i] = M_PI;
+  }
+  // velocity constrains
+  for (int i = v_start; i < vth_start; i++)
+  {
+    vars_lowerbound[i] = -max_vel;
+    vars_upperbound[i] = max_vel;
+  }
+  // velocity theta constrains
+  for (int i = vth_start; i < acc_start; i++)
+  {
+    vars_lowerbound[i] = -M_PI / 16.0;
+    vars_upperbound[i] = M_PI / 16.0;
+  }
+  // acceleration of velocity constrains
+  for (int i = acc_start; i < acc_vth_start; i++)
+  {
+    vars_lowerbound[i] = -2.0;
+    vars_upperbound[i] = 2.0;
+  }
+  // acceleration of velocity theta
+  for (int i = acc_vth_start; i < n_vars; i++)
+  {
+    vars_lowerbound[i] = -M_PI / 16.0;
+    vars_upperbound[i] = M_PI / 16.0;
+  }
+
+  // 函数约束
+  Dvector constraints_lowerbound(n_constraints);
+  Dvector constraints_upperbound(n_constraints);
+  for (int i = 0; i < n_constraints; i++)
+  {
+    constraints_lowerbound[i] = 0;
+    constraints_upperbound[i] = 0;
+  }
+  // v
+  for (int i = v_start; i < v_start + N; i++)
+  {
+    constraints_lowerbound[i] = -max_vel;
+    constraints_upperbound[i] = max_vel;
+  }
+  // vth
+  for (int i = vth_start; i < vth_start + N; i++)
+  {
+    constraints_lowerbound[i] = -M_PI;
+    constraints_upperbound[i] = M_PI;
+  }
+
+  double theta_diff = curr_pose.getRotation().getAngle() - m_last_pose.getRotation().getAngle();
+  // Predict state after latency
+  double pred_v = (curr_pose.getOrigin() - m_last_pose.getOrigin()).length() / dt;
+  double pred_theta = theta_diff / dt;
+
+  // object that computes objective and constraints
+  FG_eval_model fg_eval(coeffs, pred_v, pred_theta, ref_v, dt, t_global_target_trans_x, t_global_target_trans_y);
+
+  std::string options;
+  // Uncomment this if you'd like more print information
+  options += "Integer print_level  0\n";
+  // NOTE: Setting sparse to true allows the solver to take advantage
+  // of sparse routines, this makes the computation MUCH FASTER. If you
+  // can uncomment 1 of these and see if it makes a difference or not but
+  // if you uncomment both the computation time should go up in orders of
+  // magnitude.
+  // options += "String  sb           yes\n";
+  // maximum number of iterations
+  // options += "Integer max_iter     80\n";
+  // approximate accuracy in first order necessary conditions;
+  // see Mathematical Programming, Volume 106, Number 1,
+  // Pages 25-57, Equation (6)
+  // options += "Numeric tol          1e-4\n";
+  options += "Sparse  true        forward\n";
+  options += "Sparse  true        reverse\n";
+  // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
+  // Change this as you see fit.
+  options += "Numeric max_cpu_time          0.1\n";
+
+  // place to return solution
+  CppAD::ipopt::solve_result<Dvector> solution;
+  // solve the problem
+  // std::cout<<vars.size()<<", "<<vars_lowerbound.size()<<", "<<vars_upperbound.size()<<", "<<constraints_lowerbound.size()<<", "<<constraints_upperbound.size()<<", "<<std::endl;
+  CppAD::ipopt::solve<Dvector, FG_eval_model>(
+      options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound,
+      constraints_upperbound, fg_eval, solution);
+
+  // Check some of the solution values
+  ok &= solution.status == CppAD::ipopt::solve_result<Dvector>::success;
+
+  // Cost
+  auto cost = solution.obj_value;
+  // std::cout << "model   status " << ok << ", Cost " << cost << std::endl;
+
+  // 根据优化结果获取需下发 v vth
+  std::vector<double> solved;
+  if (ok)
+  {
+    // // 先存速度、角速度，后存预测路径
+    // solved.push_back(solution.x[acc_start]);
+    // solved.push_back(solution.x[acc_omega_start]);
+    // for (int i = 0; i < N; ++i)
+    // {
+    //   solved.push_back(solution.x[x_start + i]);
+    //   solved.push_back(solution.x[y_start + i]);
+    //   std::cout<<"path: "<<solution.x[x_start + i]<<", "<<solution.x[y_start + i]<<std::endl;
+    // }
+    // // std::cout<<"return solved"<<std::endl;
+
+    //更新 v vth
+    v = solution.x[v_start];// + fabs(solution.x[acc_start]) * dt;
+    vth = solution.x[vth_start];// + fabs(solution.x[acc_omega_start]) * dt;
+    pred_poses.clear();
+    for (size_t i = 0; i < N; i++)
+    {
+      double real_x = solution.x[x_start + i]*curr_pose.getBasis()[0][0] + solution.x[y_start + i]*curr_pose.getBasis()[0][1];
+      double real_y = solution.x[x_start + i]*curr_pose.getBasis()[1][0] + solution.x[y_start + i]*curr_pose.getBasis()[1][1];
+      // pred_poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(solution.x[x_start + i], solution.x[y_start + i], 0.0)));      
+      pred_poses.push_back(tf::Pose(
+        tf::Quaternion(0, 0, sin(solution.x[theta_start]/2.0), cos(solution.x[theta_start]/2.0)),
+        tf::Vector3(real_x, real_y, 0.0)
+      ));
+      // std::cout<<"path: "<<solution.x[x_start + i]<<", "<<solution.x[y_start + i]<<std::endl;
+    }
+    
+    std::cout << std::setprecision(3) << "model   v, vth, acc, acc_vth: " << v << ", " << vth << ", " << solution.x[acc_start] << ", " << solution.x[acc_vth_start]<< std::endl;
+    // std::cout << " ref_v : " << ref_v << std::endl;
+    m_last_pose = curr_pose;
+    last_velocity = v;
+    last_velocity_theta = vth;
+    return true;
+  }
+  else
+  {
+    m_last_pose.setIdentity();
+    return false;
+  }
+}

mpc_control/src/mpc_two_vehicle/mpc_particle_model.h

@@ -0,0 +1,68 @@
+#ifndef MPC_PARTICLE_MODEL_HH
+#define MPC_PARTICLE_MODEL_HH 
+
+#include <tf/transform_datatypes.h>
+
+#include <vector>
+#include <Eigen/Core>
+#include <Eigen/QR>
+#include <cppad/cppad.hpp>
+#include <cppad/ipopt/solve.hpp>
+/**
+ * mpc 两车模型。每车定义为矩形，双前轮主动，双后轮从动。两车头朝外，中间软性连接。
+ * 
+ * */
+class Mpc_particle_model{
+
+public:
+    // 构造函数
+    Mpc_particle_model();
+    // 析构函数
+    ~Mpc_particle_model();
+    // 模型解析
+    // 参数:
+    //      ref_velocity 参考速度，两车具有相同的参考速度
+    //      ref_poses 规划的路径
+    //      curr_pose 当前位姿
+    //      v 下发速度
+    //      vth 下发角速度
+    bool Solve(double ref_velocity, std::vector<tf::Pose> ref_poses,tf::Pose curr_pose, double& v, double& vth, std::vector<tf::Pose> &pred_poses);
+
+    void set_global_target(tf::Pose target);
+
+private:
+
+    double polyeval(Eigen::VectorXd coeffs, double x);
+    Eigen::VectorXd polyfit(Eigen::VectorXd xvals, Eigen::VectorXd yvals, int order);
+
+public:
+    // 假设车辆由两个质点组成
+    static const size_t N = 12;
+    // static constexpr double Lf=1;
+
+    // x，y； theta旋转角； v速度值； distance_error实际路径误差； epsi角度误差； a加速度
+    // N个点，N个旋转角，N个速度值，N个角速度值, N个加速度值
+    static const size_t x_start = 0;
+    static const size_t y_start = x_start + N;
+    static const size_t theta_start = y_start + N;
+    static const size_t v_start = theta_start + N;
+    static const size_t vth_start = v_start + N;
+
+    static const size_t acc_start = vth_start + N - 1;
+    static const size_t acc_vth_start = acc_start + N - 1;
+
+    static double dt;
+    static double ref_v;
+    static double max_vel;
+
+    bool forward = true;
+
+private:
+
+    double last_velocity, last_velocity_theta;
+    tf::Pose m_last_pose;
+    tf::Pose m_global_target;
+    bool mb_has_global_target;
+};
+
+#endif /* MPC_PARTICLE_MODEL_HH */

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_controller_node.cpp

@@ -90,6 +90,7 @@ void mpc_two_vehicle_controller::poseHandler(const turtlesim::Pose::ConstPtr &po
   }
 
   // 筛选前方N个点
+  // 路径转换为tf位姿
   std::vector<tf::Pose> t_poses, t_partial_poses, t_pred_poses;
   int count = 0;
   // std::cout<<"current pose:"<<current_pose.getOrigin().getX()<<", "<<current_pose.getOrigin().getY()<<std::endl;
@@ -97,22 +98,20 @@ void mpc_two_vehicle_controller::poseHandler(const turtlesim::Pose::ConstPtr &po
   {
     t_poses.push_back(tf::Pose(tf::Quaternion(), tf::Vector3(m_trajectory[i].translation().x(), m_trajectory[i].translation().y(), 0)));
   }
-  if (filterPath(m_current_pose, t_poses, t_partial_poses, 10))
+  // 计算朝向
+  tf::Vector3 orient(cos(pose_msg->theta), sin(pose_msg->theta), 0.0);
+  tf::Vector3 target_direction = m_target_pose.getOrigin() - m_current_pose.getOrigin();
+  if (filterPath(m_current_pose, t_poses, t_partial_poses, 5, target_direction.dot(orient) > 0))
   {
-    // std::cout<<"thetas: "<<current_pose.getRotation().getX()<<", "<<current_pose.getRotation().getY()<<", "<<current_pose.getRotation().getZ()<<", "<<current_pose.getRotation().getW()<<std::endl;
-    tf::Vector3 orient(cos(pose_msg->theta), sin(pose_msg->theta), 0.0);
-    tf::Vector3 target_direction = m_target_pose.getOrigin() - m_current_pose.getOrigin();
-
     // 终点附近限制速度
     if (target_direction.length() < 1)
     {
-      if (target_direction.length() < 0.02)
+      m_ref_vel *= 0.9;
+      m_ref_vel = fmax(m_ref_vel, 0.04);
+      if (target_direction.length() < 0.03)
       {
         m_ref_vel = 0;
       }
-      // m_ref_vel *= 0.95;
-      m_ref_vel = fmin(m_ref_vel, 0.2);
-      m_ref_vel = fmax(m_ref_vel, 0.02);
     }
     else
     {
@@ -130,15 +129,14 @@ void mpc_two_vehicle_controller::poseHandler(const turtlesim::Pose::ConstPtr &po
     }
     // std::cout<<"m_ref_vel: "<<m_ref_vel<<std::endl;
 
-    // 筛选局部路径
     double velo = pose_msg->linear_velocity;
     double velo_angle = pose_msg->angular_velocity;
 
-    std::cout << "start solve" << std::endl;
+    // std::cout << "start solve" << std::endl;
     m_mpc_model.set_global_target(m_target_pose);
     if (m_mpc_model.Solve(m_ref_vel, t_partial_poses, m_current_pose, velo, velo_angle, t_pred_poses))
     {
-      std::cout << "solve complete" << std::endl;
+      // std::cout << "solve complete" << std::endl;
 
       geometry_msgs::Twist velocity;
       velocity.linear.x = velo; //(v_left+v_right)/2.0;

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_controller_node.h

@@ -2,6 +2,7 @@
 #define MPC_TWO_VEHICLE_CONTROLLER_NODE_HH 
 
 #include "mpc_two_vehicle_model.h"
+#include "mpc_particle_model.h"
 #include "trajectory_tool.h"
 
 #include <ros/ros.h>
@@ -50,7 +51,8 @@ public:
 
 private:
     Trajectory_tool m_traj_tool;
-    Mpc_two_vehicle_model m_mpc_model;
+    // Mpc_two_vehicle_model m_mpc_model;
+    Mpc_particle_model m_mpc_model;
     std::mutex m_edge_mutex; // 读写端点位姿互斥锁
     Eigen::Isometry2d m_base;
     Eigen::Isometry2d m_target;

mpc_control/src/mpc_two_vehicle/mpc_two_vehicle_model.cpp

@@ -33,16 +33,28 @@ public:
     fg[0] = 0;
 
     // Weights for how "important" each cost is - can be tuned
-    const int y_cost_weight = 2000;
+    const int y_cost_weight = 5000;
     const int th_cost_weight = 1000;
     const int v_cost_weight = 20;
     const int vth_cost_weight = 100;
-    const int a_cost_weight = 5;
-    const int ath_cost_weight = 100;
+    const int a_cost_weight = 1;
+    const int ath_cost_weight = 5;
 
     const int target_cost_weight = 100;
 
-    std::cout << "-------------"<< std::endl;
+    // std::cout << "-------------"<< std::endl;
+    // 增加对接近目标时的速度限制, 接近1m内才生效
+    AD<double> x0 = vars[Mpc_two_vehicle_model::x_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> y0 = vars[Mpc_two_vehicle_model::y_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> th0 = vars[Mpc_two_vehicle_model::theta_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> v0 = vars[Mpc_two_vehicle_model::v_start + Mpc_two_vehicle_model::N - 1];
+    AD<double> the_last_predict_x = x0 + v0 * CppAD::cos(th0) * m_dt;
+    AD<double> the_last_predict_y = y0 + v0 * CppAD::sin(th0) * m_dt;
+    AD<double> dist = CppAD::sqrt(CppAD::pow(the_last_predict_x - m_global_target_x, 2) + CppAD::pow(the_last_predict_y - m_global_target_y, 2));
+    AD<double> inv_sigmoid = 1 + CppAD::exp(2 * dist - 2);
+    // std::cout << "dist, inv sig， calc: " << dist << ", "<< inv_sigmoid << ", " << 1 / (inv_sigmoid * dist) << std::endl;
+    fg[0] += target_cost_weight * dist / inv_sigmoid ;
+
     // Cost for CTE, psi error and velocity
     for (int t = 0; t < Mpc_two_vehicle_model::N; t++)
     {
@@ -57,18 +69,6 @@ public:
       // std::cout<<"x, y, theta, v_ref: "<< xt << ", "<<fx<<", " << fth << ", " << m_ref_v <<std::endl;
     }
 
-    // 增加对接近目标时的速度限制, 接近1m内才生效
-    AD<double> x0 = vars[Mpc_two_vehicle_model::x_start + Mpc_two_vehicle_model::N - 1];
-    AD<double> y0 = vars[Mpc_two_vehicle_model::y_start + Mpc_two_vehicle_model::N - 1];
-    AD<double> th0 = vars[Mpc_two_vehicle_model::theta_start + Mpc_two_vehicle_model::N - 1];
-    AD<double> v0 = vars[Mpc_two_vehicle_model::v_start + Mpc_two_vehicle_model::N - 1];
-    AD<double> the_last_predict_x = x0 + v0 * CppAD::cos(th0) * m_dt;
-    AD<double> the_last_predict_y = y0 + v0 * CppAD::sin(th0) * m_dt;
-    AD<double> dist = CppAD::sqrt(CppAD::pow(the_last_predict_x - m_global_target_x, 2) + CppAD::pow(the_last_predict_y - m_global_target_y, 2));
-    AD<double> inv_sigmoid = 1 + CppAD::exp(5 * dist - 5);
-    std::cout << "dist, inv sig， calc: " << dist << ", "<< inv_sigmoid << ", " << 1 / (inv_sigmoid * dist) << std::endl;
-    fg[0] += target_cost_weight * dist / inv_sigmoid ;
-
     // Costs for steering (delta) and acceleration (a)
     for (int t = 0; t < Mpc_two_vehicle_model::N - 1; t++)
     {
@@ -105,10 +105,10 @@ public:
       AD<double> acc_vth0 = vars[Mpc_two_vehicle_model::acc_vth_start + t - 1];
 
       // Setting up the rest of the model constraints
-      // fg[1 + Mpc_two_vehicle_model::x_start + t] = x1 - (x0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::cos(th0 + 0.5 * vth0 * m_dt) * m_dt);
-      // fg[1 + Mpc_two_vehicle_model::y_start + t] = y1 - (y0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::sin(th0 + 0.5 * vth0 * m_dt) * m_dt);
-      fg[1 + Mpc_two_vehicle_model::x_start + t] = x1 - (x0 + v0 * CppAD::cos(th0) * m_dt);
-      fg[1 + Mpc_two_vehicle_model::y_start + t] = y1 - (y0 + v0 * CppAD::sin(th0) * m_dt);
+      fg[1 + Mpc_two_vehicle_model::x_start + t] = x1 - (x0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::cos(th0 + 0.5 * vth0 * m_dt) * m_dt);
+      fg[1 + Mpc_two_vehicle_model::y_start + t] = y1 - (y0 + (v0 + 0.5 * acc0 * m_dt) * CppAD::sin(th0 + 0.5 * vth0 * m_dt) * m_dt);
+      // fg[1 + Mpc_two_vehicle_model::x_start + t] = x1 - (x0 + v0 * CppAD::cos(th0) * m_dt);
+      // fg[1 + Mpc_two_vehicle_model::y_start + t] = y1 - (y0 + v0 * CppAD::sin(th0) * m_dt);
       fg[1 + Mpc_two_vehicle_model::theta_start + t] = th1 - (th0 + vth0 * m_dt);
       fg[1 + Mpc_two_vehicle_model::v_start + t] = v1 - (v0 + acc0 * m_dt);
       fg[1 + Mpc_two_vehicle_model::vth_start + t] = vth1 - (vth0 + acc_vth0 * m_dt);
@@ -243,8 +243,8 @@ bool Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref
   // velocity theta constrains
   for (int i = vth_start; i < acc_start; i++)
   {
-    vars_lowerbound[i] = -M_PI;
-    vars_upperbound[i] = M_PI;
+    vars_lowerbound[i] = -M_PI / 16.0;
+    vars_upperbound[i] = M_PI / 16.0;
   }
   // acceleration of velocity constrains
   for (int i = acc_start; i < acc_vth_start; i++)
@@ -255,8 +255,8 @@ bool Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref
   // acceleration of velocity theta
   for (int i = acc_vth_start; i < n_vars; i++)
   {
-    vars_lowerbound[i] = -M_PI;
-    vars_upperbound[i] = M_PI;
+    vars_lowerbound[i] = -M_PI / 16.0;
+    vars_upperbound[i] = M_PI / 16.0;
   }
 
   // 函数约束
@@ -290,7 +290,7 @@ bool Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref
 
   std::string options;
   // Uncomment this if you'd like more print information
-  options += "Integer print_level  0\n";
+  // options += "Integer print_level  0\n";
   // NOTE: Setting sparse to true allows the solver to take advantage
   // of sparse routines, this makes the computation MUCH FASTER. If you
   // can uncomment 1 of these and see if it makes a difference or not but
@@ -307,7 +307,7 @@ bool Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref
   options += "Sparse  true        reverse\n";
   // NOTE: Currently the solver has a maximum time limit of 0.5 seconds.
   // Change this as you see fit.
-  options += "Numeric max_cpu_time          0.5\n";
+  options += "Numeric max_cpu_time          0.07\n";
 
   // place to return solution
   CppAD::ipopt::solve_result<Dvector> solution;
@@ -322,7 +322,7 @@ bool Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref
 
   // Cost
   auto cost = solution.obj_value;
-  std::cout << "model   status " << ok << ", Cost " << cost << std::endl;
+  // std::cout << "model   status " << ok << ", Cost " << cost << std::endl;
 
   // 根据优化结果获取需下发 v vth
   std::vector<double> solved;
@@ -354,8 +354,9 @@ bool Mpc_two_vehicle_model::Solve(double ref_velocity, std::vector<tf::Pose> ref
       ));
       // std::cout<<"path: "<<solution.x[x_start + i]<<", "<<solution.x[y_start + i]<<std::endl;
     }
-
-    std::cout << "model   v: " << v << ", " << vth << ", " << ref_v << std::endl;
+    
+    std::cout << std::setprecision(3) << "model   v, vth, acc, acc_vth: " << v << ", " << vth << ", " << solution.x[acc_start] << ", " << solution.x[acc_vth_start]<< std::endl;
+    std::cout << " ref_v : " << ref_v << std::endl;
     m_last_pose = curr_pose;
     last_velocity = v;
     last_velocity_theta = vth;