Commit 9859a6fd authored by Christoph Richter's avatar Christoph Richter
Browse files

Implemented low level ReferencePathCreator

parent d78ad76a
Pipeline #65078 failed with stage
in 19 seconds
package de.rwth.monticar.mpc;
import de.rwth.monticar.mpc.bicycle.*;
import de.rwth.monticar.mpc.trajectory.*;
component TrajectoryControllerMPC {
port
......
......@@ -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} z_ref,
in Q^{4, 10} z_ref,
// previous control in/output
in Q (-3.0 m/s^2 : 2.5 m/s^2) a_prev,
......@@ -37,13 +37,15 @@ component BicycleMPC<Q dt = 0.1, Q l_f = 1, Q l_r = 1, Z hp = 1>
z(3) = yaw;
z(4) = v;
// create previous input vector
Q^{2} u_prev = [a_prev; steering_prev];
Q^{2} u_prev;
u_prev(1) = a_prev;
u_prev(2) = steering_prev;
// static bounds on u
Q^{2} u_min = [0.01 m/s^2; -0.785];
Q^{2} u_max = [2.5 m/s^2; -0.785];
Q^{2} u_min = [0.0 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];
Q^{2} du_max = [2.5 m/s^3; 0.5];
// define cost matrices
Q^{4,4} costQ = eye(4,4);
......@@ -52,7 +54,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(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));
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)));
subject to
u_min(1,1) <= u(1,1) <= u_max(1,1);
u_min(2,1) <= u(2,1) <= u_max(2,1);
......
// PathPlaner
// PathPlaner:
// - First trims path to current progress
// - Then creates low level reference state vector for mpc controller
// @author Christoph Richter
package de.rwth.monticar.mpc;
package de.rwth.monticar.mpc.trajectory;
component PathPlaner<Q dt = 0.1>
{
......@@ -18,37 +20,33 @@ 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} z_ref;
implementation Math
{
// for now stupid solution just next reference point
static Z currIdx = 1;
// TODO: project first point onto planned trajectory
Q^{2} firstPoint;
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(currIdx + 1);
secondPoint(2) = trajectory_y(currIdx + 1);
// TODO: create next points for prediction horizon dependent on velocity
// yaw angle heading to the next waypoint
Q dirX = secondPoint(1) - firstPoint(1);
Q dirY = secondPoint(2) - firstPoint(2);
z_ref(3) = atan2(dirY, dirX);
// constant target velocity +-50km/h
z_ref(4) = 14 m/s;
//drive to next point if control point is reached
if (abs(firstPoint(1) - x + firstPoint(2) - y) < 50) && (currIdx <= trajectory_length)
currIdx = currIdx + 1;
end
}
out Q^{4, 10} z_ref;
// trims planned trajectory to current position
instance TrimPath pathTrimmer;
// creates a low lever reference path for the prediction horizon
instance ReferencePathCreator referencePathCreator;
// velocity controller
instance VelocityController velocityController;
// connect trim path
connect x -> pathTrimmer.currentPositionX;
connect y -> pathTrimmer.currentPositionY;
connect trajectory_length -> pathTrimmer.plannedTrajectoryLength;
connect trajectory_x -> pathTrimmer.plannedTrajectoryX;
connect trajectory_y -> pathTrimmer.plannedTrajectoryY;
// connect referencePathCreator
connect x -> referencePathCreator.x;
connect y -> referencePathCreator.y;
connect velocityController.desiredVelocity -> referencePathCreator.desiredVelocity;
connect pathTrimmer.trimmedTrajectoryLength -> referencePathCreator.trajectory_length;
connect pathTrimmer.trimmedTrajectoryX -> referencePathCreator.trajectory_x;
connect pathTrimmer.trimmedTrajectoryY -> referencePathCreator.trajectory_y;
// connect output
connect referencePathCreator.z_ref -> z_ref;
}
\ No newline at end of file
// ReferencePath: Created a reference path for the prediction horizon of the mpc controller
// @author Christoph Richter
package de.rwth.monticar.mpc.trajectory;
component ReferencePathCreator<Q dt = 0.1>
{
ports
// state vector z
in Q x,
in Q y,
in Q (0.0 m/s : 0.01 m/s : oo m/s) desiredVelocity,
// trajectory trimmed to current position
in Z (0 : 100) trajectory_length,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_x,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} trajectory_y,
// reference trajectory for prediction horizon
out Q^{4, 10} z_ref;
implementation Math
{
Z hp = 10; // prediction horizon
Q dt = 0.1 s; // time increment;
Q^{2} segmentStart;
segmentStart(1) = trajectory_x(1);
segmentStart(2) = trajectory_y(1);
Q^{2} segmentEnd;
segmentEnd(1) = trajectory_x(2);
segmentEnd(2) = trajectory_y(2);
// yaw angle heading to the next waypoint
Q^{2} dir;
dir(1) = segmentStart(1) - x;
dir(2) = segmentStart(2) - y;
// assign first state
z_ref(1,1) = segmentStart(1);
z_ref(2,1) = segmentStart(2);
z_ref(3,1) = atan2(dir(2), dir(1));
z_ref(4,1) = desiredVelocity;
for i = 2:hp
// calculate desired waypoint distance dependent on desired velocity and time increment
Q waypointDistance = desiredVelocity * dt;
// calculate possible direction vector
dir(1) = cos(z_ref(3, i - 1));
dir(2) = sin(z_ref(3, i - 1));
// calculate possible next state
Q^{2} next;
Q^{2} possibleNext;
possibleNext(1) = z_ref(1, i - 1) + dir(1) * waypointDistance;
possibleNext(2) = z_ref(2, i - 1) + dir(2) * waypointDistance;
// check if trajectory point is reached
if sum(segmentEnd - possibleNext) > 0
// possible next is fine
next = possibleNext;
else
// use segment end
next(1) = segmentEnd(1);
next(2) = segmentEnd(2);
// assign next segment
segmentStart = segmentEnd;
segmentEnd(1) = trajectory_x(3);
segmentEnd(2) = trajectory_y(3);
// update direction
dir(1) = segmentEnd(1) - segmentStart(1);
dir(2) = segmentEnd(2) - segmentStart(2);
end
// assign next
z_ref(1,i) = next(1);
z_ref(2,i) = next(2);
z_ref(3,i) = atan2(dir(2), dir(1));
z_ref(4,i) = desiredVelocity;
end
}
}
\ No newline at end of file
package de.rwth.monticar.mpc.trajectory;
// trims the planned path
// such that the first segment
// is the closest to the current vehicle's position
// and the first point is the projection
// of the current position onto the planned path
component TrimPath {
port
in Q (-oo m : 0.01 m : oo m) currentPositionX,
in Q (-oo m : 0.01 m : oo m) currentPositionY,
in Z (0 : 100) plannedTrajectoryLength,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryX,
in Q (-oo m : 0.01 m : oo m) ^ {1,100} plannedTrajectoryY,
out Z (0 : 100) trimmedTrajectoryLength,
out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryX,
out Q (-oo m : 0.01 m : oo m) ^ {1,100} trimmedTrajectoryY;
implementation Math {
trimmedTrajectoryLength = 0;
if plannedTrajectoryLength == 1
trimmedTrajectoryLength = 1;
trimmedTrajectoryX(1,1) = plannedTrajectoryX(1,1);
trimmedTrajectoryY(1,1) = plannedTrajectoryY(1,1);
elseif plannedTrajectoryLength > 1
Z closestSegmentIndex = -1;
Q closestSegmentDistance = -1.0;
Q lastSegmentIndex = plannedTrajectoryLength - 1;
for i = 1:lastSegmentIndex
Q p1x = plannedTrajectoryX(1,i);
Q p1y = plannedTrajectoryY(1,i);
Q p2x = plannedTrajectoryX(1,i+1);
Q p2y = plannedTrajectoryY(1,i+1);
// projection
Q vx = currentPositionX - p1x;
Q vy = currentPositionY - p1y;
Q v12x = p2x - p1x;
Q v12y = p2y - p1y;
Q k = ( vx*v12x + vy*v12y ) / ( v12x*v12x + v12y*v12y );
Q projection_x = p1x + k * v12x;
Q projection_y = p1y + k * v12y;
Q is_projection_on_segment = ((p1x-projection_x) * (p2x-projection_x) <= 0) && ((p1y-projection_y) * (p2y-projection_y) <= 0);
if (is_projection_on_segment)
Q d_proj_sqr = (currentPositionX-projection_x)*(currentPositionX-projection_x) + (currentPositionY-projection_y)*(currentPositionY-projection_y);
Q d_proj = sqrt( d_proj_sqr );
if (closestSegmentDistance < 0) || (d_proj < closestSegmentDistance)
closestSegmentIndex = i;
closestSegmentDistance = d_proj;
trimmedTrajectoryX(1,1) = projection_x;
trimmedTrajectoryY(1,1) = projection_y;
end
else
Q d1_sqr = (currentPositionX-p1x)*(currentPositionX-p1x) + (currentPositionY-p1y)*(currentPositionY-p1y);
Q d1 = sqrt( d1_sqr );
Q d2_sqr = (currentPositionX-p2x)*(currentPositionX-p2x) + (currentPositionY-p2y)*(currentPositionY-p2y);
Q d2 = sqrt( d2_sqr );
Q d_min = min(d1,d2);
if (closestSegmentDistance < 0) || (d_min < closestSegmentDistance)
closestSegmentIndex = i;
closestSegmentDistance = d_min;
trimmedTrajectoryX(1,1) = projection_x;
trimmedTrajectoryY(1,1) = projection_y;
end
end
end
if closestSegmentIndex > -1
Z currentFree = 2; // 1st point is always the projection
Z start = closestSegmentIndex + 1;
for i = start:plannedTrajectoryLength
trimmedTrajectoryX(1,currentFree) = plannedTrajectoryX(1,i);
trimmedTrajectoryY(1,currentFree) = plannedTrajectoryY(1,i);
currentFree += 1;
end
trimmedTrajectoryLength = currentFree - 1;
end
end
}
}
// Provides a desired velocity
// @author Christoph Richter
package de.rwth.monticar.mpc.trajectory;
component VelocityController
{
ports
out Q (-oo m/s : oo m/s) desiredVelocity;
implementation Math
{
desiredVelocity = 10 m/s;
}
}
\ No newline at end of file
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