Commit b1728418 authored by Christoph Richter's avatar Christoph Richter
Browse files

TrajectoryControllerMPC: Fixed model bugs

parent 8d60bb83
Pipeline #64877 failed with stage
in 21 seconds
......@@ -20,7 +20,7 @@ component ActuatorController
steering = steering_angle;
// if acceleration should be positive => engine
// else break
if u(1) >= 0
if acceleration >= 0
engine = acceleration;
brakes = 0;
else
......
......@@ -27,12 +27,14 @@ component PathPlaner<Q dt = 0.1>
// TODO: project first point onto planned trajectory
Q^{2} firstPoint;
firstPoint(1) = trajectory_x(idx);
firstPoint(2) = trajectory_y(idx);
firstPoint(1) = trajectory_x(currIdx);
firstPoint(2) = trajectory_y(currIdx);
z_ref(1) = firstPoint(1);
z_ref(2) = firstPoint(2);
Q^{2} secondPoint;
secondPoint(1) = trajectory_x(idx + 1);
secondPoint(2) = trajectory_y(idx + 1);
secondPoint(1) = trajectory_x(currIdx + 1);
secondPoint(2) = trajectory_y(currIdx + 1);
// TODO: create next points for prediction horizon dependent on velocity
......@@ -45,8 +47,8 @@ component PathPlaner<Q dt = 0.1>
z_ref(4) = 14 m/s;
//drive to next point if control point is reached
if abs(firstPoint(1) - x + firstPoint(2) - y) < 50
idx = idx + 1;
if (abs(firstPoint(1) - x + firstPoint(2) - y) < 50) && (currIdx <= trajectory_length)
currIdx = currIdx + 1;
end
}
}
\ No newline at end of file
......@@ -63,7 +63,8 @@ component TrajectoryControllerMPC {
connect x -> mpc.x;
connect y -> mpc.y;
connect compass -> mpc.yaw;
connect currentVelocity -> mpc.v;
connect currentVelocity -> mpc.v;
connect pathPlanner.z_ref -> mpc.z_ref;
connect mpc.a -> mpc.a_prev;
connect mpc.steering -> mpc.steering_prev;
......
......@@ -24,9 +24,18 @@ component BicycleMPC<Q dt = 0.1, Q l_f = 1, Q l_r = 1, Z hp = 1>
implementation Math
{
// create state vector
Q^{4} z = [x; y; yaw; v];
// workaround for generator bug -> declare generic type arguments
Q dt = 0.1 s;
Q l_f = 1 m;
Q l_r = 1 m;
Z hp = 1;
// create state vector
Q^{4} z;
z(1) = x;
z(2) = y;
z(3) = yaw;
z(4) = v;
// create previous input vector
Q^{2} u_prev = [a_prev; steering_prev];
......@@ -43,7 +52,7 @@ component BicycleMPC<Q dt = 0.1, Q l_f = 1, Q l_r = 1, Z hp = 1>
// minimization statement
Q error = minimize(Q^{2, 1} u)
abs(sum([z_t(1) + z_t(4) * cos(z_t(3) + atan(l_r / (l_f + l_r) * tan(u(2, 1)))) * dt; z_t(2) + z_t(4) * sin(z_t(3) + atan(l_r / (l_f + l_r) * tan(u(2, 1)))) * dt; z_t(3) + z_t(4) / l_r * sin(atan(l_r / (l_f + l_r) * tan(u(2, 1)))) * dt; z_t(4) + u(1,1) * dt] - z_ref) + sum(u(:,1) - u_prev));
abs(sum([z(1) + z(4) * cos(z(3) + atan(l_r / (l_f + l_r) * tan(u(2, 1)))) * dt; z(2) + z(4) * sin(z(3) + atan(l_r / (l_f + l_r) * tan(u(2, 1)))) * dt; z(3) + z(4) / l_r * sin(atan(l_r / (l_f + l_r) * tan(u(2, 1)))) * dt; z(4) + u(1,1) * dt] - z_ref) + sum(u(:,1) - u_prev));
subject to
u_min(1,1) <= u(1,1) <= u_max(1,1);
u_min(2,1) <= u(2,1) <= u_max(2,1);
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment