Commit 2bd2a967 authored by Christoph Richter's avatar Christoph Richter
Browse files

AutopilotMPC enhancements

parent c593684d
......@@ -73,6 +73,9 @@ component AutopilotMPC {
connect currentVelocity -> mpcTrajectoryController.currentVelocity;
connect compass -> mpcTrajectoryController.currentYawAngle;
connect trajectory_x -> mpcTrajectoryController.plannedTrajectoryX;
connect trajectory_y -> mpcTrajectoryController.plannedTrajectoryY;
connect behaviorGeneration.desiredDirectionX -> mpcTrajectoryController.desiredDirectionX;
connect behaviorGeneration.desiredDirectionY -> mpcTrajectoryController.desiredDirectionY;
connect behaviorGeneration.desiredVelocity -> mpcTrajectoryController.desiredVelocity;
......
......@@ -21,10 +21,10 @@ component KinematicBicycleMPC
Q l_r = 1; // length center to back
// static bounds on u
Q^{2} u_min = [-1.5 m/s^2; -37];
Q^{2} u_max = [1 m/s^2; 37];
Q^{2} du_min = [-3 m/s^3; -10];
Q^{2} du_max = [1.5 m/s^3; 10];
Q^{2} u_min = [-3 m/s^2; -0.785];
Q^{2} u_max = [2.5 m/s^2; -0.785];
Q^{2} du_min = [-3 m/s^3; -0.5];
Q^{2} du_max = [2.5 m/s^3; -0.5];
// define cost matrices
Q^{4,4} costQ = eye(4,4);
......@@ -42,5 +42,7 @@ component KinematicBicycleMPC
end
// assign value
u = u_i(:,1);
// u(1) = 0.5;
// u(2) = 0.2;
}
}
\ No newline at end of file
......@@ -12,14 +12,18 @@ component KinematicBicycleMPCInAdapter
in Q (-oo : 0.001 : oo) currentYawAngle,
in Q (0.0 m/s : 0.01 m/s : oo m/s) currentVelocity,
// desired trajectory
// desired direction
in Q desiredDirectionX,
in Q desiredDirectionY,
in Q (0.0 m/s : 0.01 m/s : oo m/s) desiredVelocity,
// trajectory
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryY,
// output
out Q^{4} z_t,
out Q^{4} z_ref;
out Q^{4} z_ref;
implementation Math
{
......@@ -30,6 +34,15 @@ component KinematicBicycleMPCInAdapter
z_t(4) = currentVelocity;
// create reference
z_ref = zeros(4);
// position -> next waypoints
z_ref(1) = plannedTrajectoryX(1);
z_ref(2) = plannedTrajectoryX(2);
// direction -> to next waypoint
Q^{2} curPos = [z_t(1); z_t(2)];
Q^{2} nextPos = [z_ref(1); z_ref(2)];
Q^{2} dir = nextPos - curPos;
z_ref(3) = atan2(dir(2), dir(1));
// velocity -> constant 14 m/s -+50km/h
z_ref(4) = 14 m/s;
}
}
\ No newline at end of file
......@@ -17,6 +17,10 @@ component KinematicBicycleMPCWrapper
in Q desiredDirectionY,
in Q (0.0 m/s : 0.01 m/s : oo m/s) desiredVelocity,
// trajectory
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryY,
// output
out Q (0.0 : 0.001 : 2.5) engine,
out Q (-0.785 : 0.001 : 0.785) steering,
......@@ -36,6 +40,8 @@ component KinematicBicycleMPCWrapper
connect desiredDirectionX -> mpcInAdapter.desiredDirectionX;
connect desiredDirectionY -> mpcInAdapter.desiredDirectionY;
connect desiredVelocity -> mpcInAdapter.desiredVelocity;
connect plannedTrajectoryX -> mpcInAdapter.plannedTrajectoryX;
connect plannedTrajectoryY -> mpcInAdapter.plannedTrajectoryY;
// mpc in
connect mpcInAdapter.z_ref -> mpcTrajectoryController.z_ref;
connect mpcInAdapter.z_t -> mpcTrajectoryController.z_t;
......
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