|
@@ -56,20 +56,20 @@ public:
|
|
|
{
|
|
|
|
|
|
// 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 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 dx = m_data[6];
|
|
|
double dy = m_data[7];
|
|
|
double dth = m_data[8]; ///两坐标系角度
|
|
|
|
|
|
///前车 cost weight
|
|
|
- const int y_cost_weight = 8000;
|
|
|
+ const int y_cost_weight = 2000;
|
|
|
const int th_cost_weight = 1000;
|
|
|
const int v_cost_weight = 20;
|
|
|
|
|
@@ -79,7 +79,7 @@ public:
|
|
|
|
|
|
fg[0] = 0;
|
|
|
///后车 cost weight
|
|
|
- const int back_y_cost_weight = 8000;
|
|
|
+ const int back_y_cost_weight = 2000;
|
|
|
const int back_th_cost_weight = 1000;
|
|
|
const int back_v_cost_weight = 20;
|
|
|
|
|
@@ -100,12 +100,12 @@ public:
|
|
|
fg[0] += v_cost_weight * CppAD::pow(vars[nv1 + t] - ref_v, 2);
|
|
|
fg[0] += vth_cost_weight * CppAD::pow(vars[nvth1 + t], 2);
|
|
|
|
|
|
- // 在前车坐标系下,后车y坐标差
|
|
|
+ // 在后车坐标系下,后车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
|
|
|
+ // 在后车坐标系下,后车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);
|
|
@@ -115,7 +115,7 @@ public:
|
|
|
// Costs for steering (delta) and acceleration (a)
|
|
|
for (int t = 0; t < K - 1; t++)
|
|
|
{
|
|
|
- fg[0] += a_cost_weight * CppAD::pow(vars[nacc1+ t], 2);
|
|
|
+ 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_a_cost_weight * CppAD::pow(vars[nacc2 + t], 2);
|
|
@@ -123,9 +123,9 @@ public:
|
|
|
}
|
|
|
|
|
|
///添加两车距离损失
|
|
|
- const int distance_weight = 800;
|
|
|
- const int delta_th_weight = 10;
|
|
|
- char buf[255]={0};
|
|
|
+ const int distance_weight = 600;
|
|
|
+ const int delta_th_weight = 50;
|
|
|
+ // char buf[255]={0};
|
|
|
AD<double> dtheta = vars[nth1] - vars[nth2] + dth;
|
|
|
for (int t = 0; t < K; ++t)
|
|
|
{
|
|
@@ -140,24 +140,24 @@ public:
|
|
|
|
|
|
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)));
|
|
|
+ // 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;
|
|
|
+ // std::cout<<buf<<std::endl;
|
|
|
|
|
|
// x, y, theta 路径约束
|
|
|
- fg[1 + nx1] = vars[nx1];// - vars[nv1] * dt;
|
|
|
+ fg[1 + nx1] = vars[nx1]; // - vars[nv1] * dt;
|
|
|
fg[1 + ny1] = vars[ny1];
|
|
|
- fg[1 + nth1] = vars[nth1];// - vars[nvth1] * dt;
|
|
|
- fg[1 + nv1] = vars[nv1];
|
|
|
- fg[1 + nvth1] = vars[nvth1];
|
|
|
+ fg[1 + nth1] = vars[nth1]; // - vars[nvth1] * dt;
|
|
|
+ fg[1 + nv1] = vars[nv1];// + vars[nacc1] * dt;
|
|
|
+ fg[1 + nvth1] = vars[nvth1];// + vars[naccth1] * 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];
|
|
|
+ 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];// + vars[nacc2] * dt;
|
|
|
+ fg[1 + nvth2 - 2 * (K - 1)] = vars[nvth2];// + vars[naccth2] * dt;
|
|
|
|
|
|
// The rest of the constraints
|
|
|
for (int t = 1; t < K; t++)
|
|
@@ -178,9 +178,9 @@ public:
|
|
|
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 + nx1 + t] = fx1 - (fx0 + (fv0 + 0.5 * facc0 * dt) * CppAD::cos(fth0 + 0.5 * fvth0 * dt) * dt);
|
|
|
+ fg[1 + ny1 + t] = fy1 - (fy0 + (fv0 + 0.5 * facc0 * dt) * CppAD::sin(fth0 + 0.5 * fvth0 * dt) * dt);
|
|
|
+ fg[1 + nth1 + t] = fth1 - (fth0 + (fvth0 + 0.5 * faccth0 * dt) * dt);
|
|
|
fg[1 + nv1 + t] = fv1 - (fv0 + facc0 * dt);
|
|
|
fg[1 + nvth1 + t] = fvth1 - (fvth0 + faccth0 * dt);
|
|
|
|
|
@@ -200,11 +200,11 @@ public:
|
|
|
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 + nx2 + t - 2 * (K - 1)] = bx1 - (bx0 + (bv0 + 0.5 * bacc0 * dt) * CppAD::cos(bth0 + 0.5 * bvth0 * dt) * dt);
|
|
|
+ fg[1 + ny2 + t - 2 * (K - 1)] = by1 - (by0 + (bv0 + 0.5 * bacc0 * dt) * CppAD::sin(bth0 + 0.5 * bvth0 * dt) * dt);
|
|
|
+ fg[1 + nth2 + t - 2 * (K - 1)] = bth1 - (bth0 + (bvth0 + 0.5 * baccth0 * dt) * dt);
|
|
|
+ fg[1 + nv2 + t - 2 * (K - 1)] = bv1 - (bv0 + bacc0 * dt);
|
|
|
+ fg[1 + nvth2 + t - 2 * (K - 1)] = bvth1 - (bvth0 + baccth0 * dt);
|
|
|
}
|
|
|
}
|
|
|
};
|
|
@@ -216,13 +216,13 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
|
|
|
// State vector holds all current values neede for vars below
|
|
|
|
|
|
// 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 ref_v = state[4];
|
|
|
double dt = state[5];
|
|
|
-
|
|
|
// double dx = state[6]; /// 两车初始位姿差 前车相对后车坐标系
|
|
|
// double dy = state[7];
|
|
|
// double dth = state[8]; ///两坐标系角度
|
|
@@ -235,14 +235,14 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
|
|
|
{
|
|
|
vars[i] = 0.0;
|
|
|
}
|
|
|
- for (int i = 0; i < K; ++i)
|
|
|
- {
|
|
|
- vars[nv1 + i] = v1;
|
|
|
- vars[nv2 + i] = v2;
|
|
|
+ // for (int i = 0; i < 1; ++i)
|
|
|
+ // {
|
|
|
+ // 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);
|
|
@@ -273,13 +273,16 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
|
|
|
{
|
|
|
//// limit acc1 acc2
|
|
|
vars_lowerbound[nacc1 + i] = -5.0;
|
|
|
+ vars_upperbound[nacc1 + i] = 5.0;
|
|
|
+ vars_lowerbound[nacc2 + 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;
|
|
|
+ vars_lowerbound[naccth1 + i] = -0.05; //3度
|
|
|
+ vars_upperbound[naccth1 + i] = 0.05; //3度
|
|
|
+ vars_lowerbound[naccth2 + i] = -0.05;
|
|
|
+ vars_upperbound[naccth2 + i] = 0.05; //3度
|
|
|
}
|
|
|
-
|
|
|
|
|
|
//// constraints
|
|
|
Dvector constraints_lowerbound(n_constraints);
|
|
@@ -293,20 +296,32 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
|
|
|
for (int i = 0; i < K; i++)
|
|
|
{
|
|
|
////acc v1 v2
|
|
|
- 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;
|
|
|
+ // constraints_lowerbound[nv1 + i] = -0.5;
|
|
|
+ // constraints_upperbound[nv1 + i] = 0.5;
|
|
|
+ // constraints_lowerbound[nv2 + i - 2 * (K - 1)] = -0.5;
|
|
|
+ // constraints_upperbound[nv2 + i - 2 * (K - 1)] = 0.5;
|
|
|
|
|
|
- //acc th 1 2
|
|
|
- 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;
|
|
|
+ // //acc th 1 2
|
|
|
+ // constraints_lowerbound[nvth1 + i] = -0.05;
|
|
|
+ // constraints_upperbound[nvth1 + i] = 0.05;
|
|
|
+ // constraints_lowerbound[nvth2 + i - 2 * (K - 1)] = -0.05;
|
|
|
+ // constraints_upperbound[nvth2 + i - 2 * (K - 1)] = 0.05;
|
|
|
|
|
|
// constraints_lowerbound[relative_pose_contrains + i - 4*(K-1)] = min_dist;
|
|
|
// constraints_upperbound[relative_pose_contrains + i - 4*(K-1)] = max_dist;
|
|
|
}
|
|
|
+ //acc v1 v2
|
|
|
+ constraints_lowerbound[nv1] = v1;// + vars_lowerbound[nacc1] * dt;
|
|
|
+ constraints_upperbound[nv1] = v1;// + vars_upperbound[nacc1] * dt;
|
|
|
+ constraints_lowerbound[nv2 - 2 * (K - 1)] = v2;// + vars_lowerbound[nacc2] * dt;
|
|
|
+ constraints_upperbound[nv2 - 2 * (K - 1)] = v2;// + vars_upperbound[nacc2] * dt;
|
|
|
+
|
|
|
+ //acc th 1 2
|
|
|
+ constraints_lowerbound[nvth1] = vth1;// + vars_lowerbound[naccth1] * dt;
|
|
|
+ constraints_upperbound[nvth1] = vth1;// + vars_upperbound[naccth1] * dt;
|
|
|
+ constraints_lowerbound[nvth2 - 2 * (K - 1)] = vth2;// + vars_lowerbound[naccth2] * dt;
|
|
|
+ constraints_upperbound[nvth2 - 2 * (K - 1)] = vth2;// + vars_upperbound[naccth2] * dt;
|
|
|
+ // std::cout<<"state:"<<state<<std::endl;
|
|
|
|
|
|
// object that computes objective and constraintsv
|
|
|
FG_eval_2 fg_eval(coeffs, coeffs_b, state);
|
|
@@ -333,10 +348,10 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
|
|
|
out.clear();
|
|
|
m_trajectory_f.clear();
|
|
|
m_trajectory_b.clear();
|
|
|
- out.push_back(solution.x[nv1]);
|
|
|
- out.push_back(solution.x[nvth1]);
|
|
|
- out.push_back(solution.x[nv2]);
|
|
|
- out.push_back(solution.x[nvth2]);
|
|
|
+ out.push_back(solution.x[nv1+1]);
|
|
|
+ out.push_back(solution.x[nvth1+1]);
|
|
|
+ out.push_back(solution.x[nv2+1]);
|
|
|
+ out.push_back(solution.x[nvth2+1]);
|
|
|
for (int i = 0; i < K; ++i)
|
|
|
{
|
|
|
tf::Pose pose1, pose2;
|
|
@@ -349,6 +364,9 @@ bool TowBotMPC::Solve(Eigen::VectorXd state, double max_v, Eigen::VectorXd coeff
|
|
|
// 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("(参考速度: %.3f) 前车速度: %.3f 后车速度: %.3f \n",
|
|
|
+ state[4], solution.x[nv1], solution.x[nv2]);
|
|
|
+
|
|
|
return ok;
|
|
|
}
|
|
|
|