Commit b304928d by Jordan Ford

Fixed error in offset calculations. LQR sort of working now-ish.

parent 40623ca9
......@@ -2,3 +2,4 @@
cmake-build-debug/
cmake-build*/
*.swp
*.xlsx#
#include <Eigen/Core>
#include <track.h>
#include <Eigen/Core>
#include <iomanip>
#include <iostream>
using namespace track;
using namespace Eigen;
int main() {
Trajectory refTraj;
refTraj.emplace_back(State {0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State {4, 0, 0, 1.0, 4.0});
Matrix3d qLat = Matrix3d::Identity();
Matrix2d qLon = Matrix2d::Identity();
refTraj.emplace_back(State{0, 0, 0, 0, 1.0, 0.0});
refTraj.emplace_back(State{10, 0, 0, 0, 1.0, 10.0});
refTraj.emplace_back(State{20, 0, 0, 0, 1.0, 20.0});
double rLat = 1;
double rLon = 10000;
double epsTime = .02;
double vehicleWheelbase = 3.0;
double initialSteeringAngle = 0.0;
double initialVelocity = 2.0;
double epsTime = 0.1;
State initialState{0, 1, 0, 0, 2.0, 0.0};
Control initialControl{0, 0};
VehicleParams vehicleParams{3.0};
LQRParams lqrParams{100*Matrix3d::Identity(), 100*Matrix2d::Identity(), 1, 1};
Trajectory smoothTraj = trackLQR(refTraj, qLat, rLat, qLon, rLon,
vehicleWheelbase, initialSteeringAngle,
initialVelocity, epsTime);
Trajectory smoothTraj = trackLQR(initialState, initialControl, refTraj,
lqrParams, vehicleParams, epsTime);
for(const auto& s : smoothTraj) {
std::cout << s.x << "," << s.y << "," << s.theta
<< "," << s.velocity << "," << s.time << std::endl;
std::cout << std::fixed << std::setprecision(4);
for (const auto& s : smoothTraj) {
std::cout << s.x << "," << s.y << "," << s.theta << "," << s.delta << ","
<< s.velocity << "," << s.time << std::endl;
}
return 0;
......
No preview for this file type
......@@ -10,18 +10,35 @@ struct State {
double x;
double y;
double theta;
double delta;
double velocity;
double time;
};
struct Control {
double deltaDot;
double vDot;
};
struct VehicleParams {
double wheelbase;
};
struct LQRParams {
Eigen::Matrix3d qLat;
Eigen::Matrix2d qLon;
double rLat;
double rLon;
};
using Trajectory = std::vector<State>;
using ControlSequence = std::vector<Control>;
Trajectory trackLQR(const Trajectory& referenceTrajectory,
const Eigen::Matrix3d& qLat, const double rLat,
const Eigen::Matrix2d& qLon, const double rLon,
const double vehicleWheelbase,
const double initialSteeringAngle = 0.0,
const double initialVelocity = 1.0,
Trajectory trackLQR(const State& initialState,
const Control& initialControl,
const Trajectory& referenceTrajectory,
const LQRParams& lqrParams,
const VehicleParams& vehicleParams,
const double epsTime = 0.01);
} // namespace track
......
......@@ -61,14 +61,14 @@ bool checkTrajectoryValid(const Trajectory& trajectory) {
double computeLateralOffset(const State& actualState,
const State& targetState) {
return std::sin(targetState.theta) * (targetState.x - actualState.x) -
std::cos(targetState.theta) * (targetState.y - actualState.y);
return std::sin(targetState.theta) * (actualState.x - targetState.x) -
std::cos(targetState.theta) * (actualState.y - targetState.y);
}
double computeLongitudinalOffset(const State& actualState,
const State& targetState) {
return -std::cos(targetState.theta) * (targetState.x - actualState.x) -
-std::sin(targetState.theta) * (targetState.y - actualState.y);
return -std::cos(targetState.theta) * (actualState.x - targetState.x) -
std::sin(targetState.theta) * (actualState.y - targetState.y);
}
} // namespace track
#include "care.h"
#include <iostream>
namespace track {
......@@ -25,7 +26,8 @@ pseudoinverse(const MatT& mat,
}
Eigen::MatrixXd care(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R) {
const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R,
double eps) {
// Should check if the problem is solvable. Skip for now.
Eigen::MatrixXd S = B * R.inverse() * B.transpose();
......@@ -46,7 +48,8 @@ Eigen::MatrixXd care(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
for (int c = 0; c < W0.cols(); ++c) {
for (int r = 0; r < W0.rows(); ++r) {
converged &= std::fabs(W0(r, c) - W(r, c)) < .01;
converged &= std::fabs(W0(r, c) - W(r, c)) < eps;
assert(!std::isnan(W(r, c)));
}
}
......@@ -68,7 +71,7 @@ Eigen::MatrixXd care(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
W11 + Eigen::MatrixXd::Identity(W11.rows(), W11.cols());
N.bottomRows(W21.rows()) = W21;
Eigen::MatrixXd P = pseudoinverse(M) * (-N);
Eigen::MatrixXd P = pseudoinverse(M, eps) * (-N);
return P;
}
......
......@@ -8,7 +8,8 @@ namespace track {
/// solves the continuous time, infinite horizon LQR problem.
Eigen::MatrixXd care(const Eigen::MatrixXd& A, const Eigen::MatrixXd& B,
const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R);
const Eigen::MatrixXd& Q, const Eigen::MatrixXd& R,
double eps=1e-3);
} // namespace track
#endif // TRACK_LQR_CARE_H
......@@ -9,19 +9,21 @@
namespace track {
Trajectory trackLQR(const Trajectory& referenceTrajectory,
const Eigen::Matrix3d& qLat, const double rLat,
const Eigen::Matrix2d& qLon, const double rLon,
const double vehicleWheelbase,
const double initialSteeringAngle,
const double initialVelocity, const double epsTime) {
Trajectory trackLQR(const State& initialState,
const Control& initialControl,
const Trajectory& referenceTrajectory,
const LQRParams& lqrParams,
const VehicleParams& vehicleParams,
const double epsTime) {
assert(checkTrajectoryValid(referenceTrajectory));
assert(referenceTrajectory[0].time == initialState.time);
using namespace Eigen;
double initialTime = referenceTrajectory[0].time;
double finalTime = referenceTrajectory[referenceTrajectory.size() - 1].time;
int points = static_cast<int>(finalTime / epsTime);
int points = static_cast<int>((finalTime - initialTime) / epsTime);
Trajectory trackTrajectory;
trackTrajectory.reserve(points);
......@@ -35,18 +37,16 @@ Trajectory trackLQR(const Trajectory& referenceTrajectory,
bLon << 0, -1;
Matrix<double, 1, 1> rLatMatrix;
rLatMatrix << rLat;
rLatMatrix << lqrParams.rLat;
Matrix<double, 1, 1> rLonMatrix;
rLonMatrix << rLon;
rLonMatrix << lqrParams.rLon;
// Precompute LQR gain for longitudinal control.
Matrix2d pLon = track::care(aLon, bLon, qLon, rLonMatrix);
RowVector2d kLon = (1 / rLon) * bLon.transpose() * pLon;
Matrix2d pLon = track::care(aLon, bLon, lqrParams.qLon, rLonMatrix);
RowVector2d kLon = (1 / lqrParams.rLon) * bLon.transpose() * pLon;
// Set initial values for state and controls.
State state = referenceTrajectory[0];
double delta = initialSteeringAngle;
double velocity = initialVelocity;
State state = initialState;
// Iterate to fill in the new trajectory.
for (int i = 0; i <= points; ++i) {
......@@ -62,38 +62,31 @@ Trajectory trackLQR(const Trajectory& referenceTrajectory,
// Compute lateral state vector.
Vector3d xLat;
xLat << d, targetState.theta - state.theta, delta;
xLat << d, targetState.theta - state.theta, state.delta;
// Compute longitudinal state vector.
Vector2d xLon;
xLon << l, targetState.velocity - state.velocity;
// Re-linearize the model for lateral control.
aLat << 0, state.velocity, 0, 0, 0, -state.velocity / vehicleWheelbase, 0,
0, 0;
aLat << 0, state.velocity, 0, 0, 0, -state.velocity / vehicleParams.wheelbase, 0,
0, 0;
// Recompute the lateral control gain.
Matrix3d pLat = track::care(aLat, bLat, qLat, rLatMatrix);
Matrix3d pLat = track::care(aLat, bLat, lqrParams.qLat, rLatMatrix);
RowVector3d kLat = rLatMatrix.inverse() * bLat.transpose() * pLat;
// Calculate controls.
double uLat = -kLat * xLat;
double uLon = -kLon * xLon;
double deltaDot = uLat;
double vDot = uLon;
delta += deltaDot * epsTime;
velocity += vDot * epsTime;
Vector2d controls;
controls << deltaDot, vDot;
Control controls;
controls.deltaDot = -kLat * xLat;
controls.vDot = -kLon * xLon;
// TODO: Use RK4 to forward integrate.
state.x += state.velocity * (std::cos(state.theta)) * epsTime;
state.y += state.velocity * (std::sin(state.theta)) * epsTime;
state.theta += std::tan(delta) / vehicleWheelbase * epsTime;
state.velocity += vDot * epsTime;
state.theta += (std::tan(state.delta) / vehicleParams.wheelbase) * epsTime;
state.delta = controls.deltaDot * epsTime;
state.velocity += controls.vDot * epsTime;
state.time += epsTime;
}
......
Markdown is supported
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