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

PathPlaner: Decreased prediction horizon

parent 9859a6fd
......@@ -13,7 +13,7 @@ component BicycleMPC<Q dt = 0.1, Q l_f = 1, Q l_r = 1, Z hp = 1>
in Q (0.0 m/s : 0.01 m/s : oo m/s) v,
// reference trajectory
in Q^{4, 10} z_ref,
in Q^{4, 5} z_ref,
// previous control in/output
in Q (-3.0 m/s^2 : 2.5 m/s^2) a_prev,
......@@ -28,7 +28,7 @@ component BicycleMPC<Q dt = 0.1, Q l_f = 1, Q l_r = 1, Z hp = 1>
Q dt = 0.1 s;
Q l_f = 1 m;
Q l_r = 1 m;
Z hp = 1;
Z hp = 2;
// create state vector
Q^{4} z;
......@@ -52,15 +52,29 @@ component BicycleMPC<Q dt = 0.1, Q l_f = 1, Q l_r = 1, Z hp = 1>
Q^{2,2} costR = eye(2,2);
Q^{2,2} costRBar = eye(2,2);
Q^{4,1} z_reft;
z_reft(:, 1) = z_ref(:,1);
// optimization variable
Q^{2, 1} u = zeros(2,1);
Q^{4, 1} zt = [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];
// minimization statement
Q error = minimize(Q^{2, 1} u)
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(:,1)));
Q error = minimize(u)
abs(sum(zt - z_reft));
subject to
u_min(1,1) <= u(1,1) <= u_max(1,1);
u_min(2,1) <= u(2,1) <= u_max(2,1);
u_min(1) <= u(1,1) <= u_max(1);
u_min(2) <= u(2,1) <= u_max(2);
end
// assign output
a = u(1);
steering = u(2);
a = u(1,1);
steering = u(2,1);
}
}
\ No newline at end of file
......@@ -20,7 +20,7 @@ component PathPlaner<Q dt = 0.1>
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_y,
// reference trajectory for prediction horizon
out Q^{4, 10} z_ref;
out Q^{4, 5} z_ref;
// trims planned trajectory to current position
instance TrimPath pathTrimmer;
......
......@@ -17,20 +17,22 @@ component ReferencePathCreator<Q dt = 0.1>
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_y,
// reference trajectory for prediction horizon
out Q^{4, 10} z_ref;
out Q^{4, 5} z_ref;
implementation Math
{
Z hp = 10; // prediction horizon
Z hp = 5; // prediction horizon
Q dt = 0.1 s; // time increment;
Q^{2} segmentStart;
segmentStart(1) = trajectory_x(1);
segmentStart(2) = trajectory_y(1);
Z segmentIdx = 2;
Q^{2} segmentEnd;
segmentEnd(1) = trajectory_x(2);
segmentEnd(2) = trajectory_y(2);
segmentEnd(1) = trajectory_x(segmentIdx);
segmentEnd(2) = trajectory_y(segmentIdx);
// yaw angle heading to the next waypoint
Q^{2} dir;
......@@ -67,9 +69,10 @@ component ReferencePathCreator<Q dt = 0.1>
next(1) = segmentEnd(1);
next(2) = segmentEnd(2);
// assign next segment
segmentIdx = segmentIdx + 1;
segmentStart = segmentEnd;
segmentEnd(1) = trajectory_x(3);
segmentEnd(2) = trajectory_y(3);
segmentEnd(1) = trajectory_x(segmentIdx);
segmentEnd(2) = trajectory_y(segmentIdx);
// update direction
dir(1) = segmentEnd(1) - segmentStart(1);
dir(2) = segmentEnd(2) - segmentStart(2);
......
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