Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

feat(multi_object_tracker): change vehicle model from constant velocity and turn rate model to bicycle model #2679

Merged
merged 2 commits into from
Jan 19, 2023
Merged

feat(multi_object_tracker): change vehicle model from constant velocity and turn rate model to bicycle model #2679

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
943e409
try bicycle model
YoshiRi Jan 13, 2023
893f056
apply bicycle model to bicycle tracker
YoshiRi Jan 18, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 6 additions & 10 deletions ...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,23 +32,17 @@ class BicycleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX {
X = 0,
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
};
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
struct EkfParams
{
char dim_x = 5;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_slip;
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand All @@ -58,7 +52,9 @@ class BicycleTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_wz_;
double max_slip_;
double lf_;
double lr_;
float z_;

private:
Expand Down
16 changes: 6 additions & 10 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -32,23 +32,17 @@ class BigVehicleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX {
X = 0,
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
};
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
struct EkfParams
{
char dim_x = 5;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_slip;
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand All @@ -58,7 +52,9 @@ class BigVehicleTracker : public Tracker
float p0_cov_yaw;
} ekf_params_;
double max_vx_;
double max_wz_;
double max_slip_;
double lf_;
double lr_;
float z_;
double velocity_deviation_threshold_;

Expand Down
16 changes: 6 additions & 10 deletions ...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -33,24 +33,18 @@ class NormalVehicleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX {
X = 0,
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
};
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };

struct EkfParams
{
char dim_x = 5;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_slip;
float q_cov_vx;
float p0_cov_vx;
float p0_cov_wz;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
float r_cov_yaw;
Expand All @@ -61,7 +55,9 @@ class NormalVehicleTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_wz_;
double max_slip_;
double lf_;
double lr_;
float z_;
double velocity_deviation_threshold_;

Expand Down
85 changes: 47 additions & 38 deletions perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -55,30 +55,30 @@ BicycleTracker::BicycleTracker(
float q_stddev_y = 0.6; // [m/s]
float q_stddev_yaw = tier4_autoware_utils::deg2rad(10); // [rad/s]
float q_stddev_vx = tier4_autoware_utils::kmph2mps(10); // [m/(s*s)]
float q_stddev_wz = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)]
float q_stddev_slip = tier4_autoware_utils::deg2rad(25); // [rad/(s*s)]
float r_stddev_x = 0.6; // [m]
float r_stddev_y = 0.4; // [m]
float r_stddev_yaw = tier4_autoware_utils::deg2rad(30); // [rad]
float p0_stddev_x = 0.8; // [m/s]
float p0_stddev_y = 0.5; // [m/s]
float p0_stddev_yaw = tier4_autoware_utils::deg2rad(1000); // [rad/s]
float p0_stddev_vx = tier4_autoware_utils::kmph2mps(1000); // [m/(s*s)]
float p0_stddev_wz = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)]
float p0_stddev_slip = tier4_autoware_utils::deg2rad(10); // [rad/(s*s)]
ekf_params_.q_cov_x = std::pow(q_stddev_x, 2.0);
ekf_params_.q_cov_y = std::pow(q_stddev_y, 2.0);
ekf_params_.q_cov_yaw = std::pow(q_stddev_yaw, 2.0);
ekf_params_.q_cov_vx = std::pow(q_stddev_vx, 2.0);
ekf_params_.q_cov_wz = std::pow(q_stddev_wz, 2.0);
ekf_params_.q_cov_slip = std::pow(q_stddev_slip, 2.0);
ekf_params_.r_cov_x = std::pow(r_stddev_x, 2.0);
ekf_params_.r_cov_y = std::pow(r_stddev_y, 2.0);
ekf_params_.r_cov_yaw = std::pow(r_stddev_yaw, 2.0);
ekf_params_.p0_cov_x = std::pow(p0_stddev_x, 2.0);
ekf_params_.p0_cov_y = std::pow(p0_stddev_y, 2.0);
ekf_params_.p0_cov_yaw = std::pow(p0_stddev_yaw, 2.0);
ekf_params_.p0_cov_vx = std::pow(p0_stddev_vx, 2.0);
ekf_params_.p0_cov_wz = std::pow(p0_stddev_wz, 2.0);
ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0);
max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
max_wz_ = tier4_autoware_utils::deg2rad(30); // [rad/s]
max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s]

// initialize X matrix
Eigen::MatrixXd X(ekf_params_.dim_x, 1);
Expand All @@ -87,11 +87,10 @@ BicycleTracker::BicycleTracker(
X(IDX::YAW) = tf2::getYaw(object.kinematics.pose_with_covariance.pose.orientation);
if (object.kinematics.has_twist) {
X(IDX::VX) = object.kinematics.twist_with_covariance.twist.linear.x;
X(IDX::WZ) = object.kinematics.twist_with_covariance.twist.angular.z;
} else {
X(IDX::VX) = 0.0;
X(IDX::WZ) = 0.0;
}
X(IDX::SLIP) = 0.0;

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -110,7 +109,7 @@ BicycleTracker::BicycleTracker(
P(IDX::Y, IDX::X) = P(IDX::X, IDX::Y);
P(IDX::YAW, IDX::YAW) = ekf_params_.p0_cov_yaw;
P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz;
P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip;
} else {
P(IDX::X, IDX::X) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
P(IDX::X, IDX::Y) = object.kinematics.pose_with_covariance.covariance[utils::MSG_COV_IDX::X_Y];
Expand All @@ -121,12 +120,10 @@ BicycleTracker::BicycleTracker(
if (object.kinematics.has_twist_covariance) {
P(IDX::VX, IDX::VX) =
object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::X_X];
P(IDX::WZ, IDX::WZ) =
object.kinematics.twist_with_covariance.covariance[utils::MSG_COV_IDX::YAW_YAW];
} else {
P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
P(IDX::WZ, IDX::WZ) = ekf_params_.p0_cov_wz;
}
P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip;
}

if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand All @@ -136,6 +133,11 @@ BicycleTracker::BicycleTracker(
bounding_box_ = {1.0, 0.5, 1.7};
}
ekf_.init(X, P);

// Set lf, lr
double point_ratio = 0.5; // comes to front if larger
lf_ = bounding_box_.length * point_ratio;
lr_ = bounding_box_.length * (1.0 - point_ratio);
}

bool BicycleTracker::predict(const rclcpp::Time & time)
Expand All @@ -150,47 +152,53 @@ bool BicycleTracker::predict(const rclcpp::Time & time)

bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
{
/* == Nonlinear model ==
/* == Nonlinear model == static bicycle model
*
* x_{k+1} = x_k + vx_k * cos(yaw_k) * dt
* y_{k+1} = y_k + vx_k * sin(yaw_k) * dt
* yaw_{k+1} = yaw_k + (wz_k) * dt
* x_{k+1} = x_k + vx_k * cos(yaw_k + slip_k) * dt
* y_{k+1} = y_k + vx_k * sin(yaw_k + slip_k) * dt
* yaw_{k+1} = yaw_k + vx_k / l_r * sin(slip_k) * dt
* vx_{k+1} = vx_k
* wz_{k+1} = wz_k
* slip_{k+1} = slip_k
*
*/

/* == Linearized model ==
*
* A = [ 1, 0, -vx*sin(yaw)*dt, cos(yaw)*dt, 0]
* [ 0, 1, vx*cos(yaw)*dt, sin(yaw)*dt, 0]
* [ 0, 0, 1, 0, dt]
* [ 0, 0, 0, 1, 0]
* [ 0, 0, 0, 0, 1]
* A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, -vx*sin(yaw+slip)*dt]
* [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, vx*cos(yaw+slip)*dt]
* [ 0, 0, 1, 1/l_r*sin(slip)*dt, vx/l_r*cos(slip)*dt]
* [ 0, 0, 0, 1, 0]
* [ 0, 0, 0, 0, 1]
*/

// X t
Eigen::MatrixXd X_t(ekf_params_.dim_x, 1); // predicted state
ekf.getX(X_t);
const double cos_yaw = std::cos(X_t(IDX::YAW));
const double sin_yaw = std::sin(X_t(IDX::YAW));
const double cos_yaw = std::cos(X_t(IDX::YAW) + X_t(IDX::SLIP));
const double sin_yaw = std::sin(X_t(IDX::YAW) + X_t(IDX::SLIP));
const double cos_slip = std::cos(X_t(IDX::SLIP));
const double sin_slip = std::sin(X_t(IDX::SLIP));
const double vx = X_t(IDX::VX);
const double sin_2yaw = std::sin(2.0f * X_t(IDX::YAW));

// X t+1
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
X_next_t(IDX::X) = X_t(IDX::X) + X_t(IDX::VX) * cos_yaw * dt; // dx = v * cos(yaw)
X_next_t(IDX::Y) = X_t(IDX::Y) + X_t(IDX::VX) * sin_yaw * dt; // dy = v * sin(yaw)
X_next_t(IDX::YAW) = X_t(IDX::YAW) + (X_t(IDX::WZ)) * dt; // dyaw = omega
Eigen::MatrixXd X_next_t(ekf_params_.dim_x, 1); // predicted state
X_next_t(IDX::X) = X_t(IDX::X) + vx * cos_yaw * dt; // dx = v * cos(yaw)
X_next_t(IDX::Y) = X_t(IDX::Y) + vx * sin_yaw * dt; // dy = v * sin(yaw)
X_next_t(IDX::YAW) = X_t(IDX::YAW) + vx / lr_ * sin_slip * dt; // dyaw = omega
X_next_t(IDX::VX) = X_t(IDX::VX);
X_next_t(IDX::WZ) = X_t(IDX::WZ);
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);

// A
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
A(IDX::X, IDX::YAW) = -X_t(IDX::VX) * sin_yaw * dt;
A(IDX::X, IDX::YAW) = -vx * sin_yaw * dt;
A(IDX::X, IDX::VX) = cos_yaw * dt;
A(IDX::Y, IDX::YAW) = X_t(IDX::VX) * cos_yaw * dt;
A(IDX::X, IDX::SLIP) = -vx * sin_yaw * dt;
A(IDX::Y, IDX::YAW) = vx * cos_yaw * dt;
A(IDX::Y, IDX::VX) = sin_yaw * dt;
A(IDX::YAW, IDX::WZ) = dt;
A(IDX::Y, IDX::SLIP) = vx * cos_yaw * dt;
A(IDX::YAW, IDX::VX) = 1.0 / lr_ * sin_slip * dt;
A(IDX::YAW, IDX::SLIP) = vx / lr_ * cos_slip * dt;

// Q
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -204,7 +212,7 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
Q(IDX::Y, IDX::X) = Q(IDX::X, IDX::Y);
Q(IDX::YAW, IDX::YAW) = ekf_params_.q_cov_yaw * dt * dt;
Q(IDX::VX, IDX::VX) = ekf_params_.q_cov_vx * dt * dt;
Q(IDX::WZ, IDX::WZ) = ekf_params_.q_cov_wz * dt * dt;
Q(IDX::SLIP, IDX::SLIP) = ekf_params_.q_cov_slip * dt * dt;
Eigen::MatrixXd B = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Eigen::MatrixXd u = Eigen::MatrixXd::Zero(ekf_params_.dim_x, 1);

Expand Down Expand Up @@ -315,8 +323,8 @@ bool BicycleTracker::measureWithPose(
if (!(-max_vx_ <= X_t(IDX::VX) && X_t(IDX::VX) <= max_vx_)) {
X_t(IDX::VX) = X_t(IDX::VX) < 0 ? -max_vx_ : max_vx_;
}
if (!(-max_wz_ <= X_t(IDX::WZ) && X_t(IDX::WZ) <= max_wz_)) {
X_t(IDX::WZ) = X_t(IDX::WZ) < 0 ? -max_wz_ : max_wz_;
if (!(-max_slip_ <= X_t(IDX::SLIP) && X_t(IDX::SLIP) <= max_slip_)) {
X_t(IDX::SLIP) = X_t(IDX::SLIP) < 0 ? -max_slip_ : max_slip_;
}
ekf_.init(X_t, P_t);
}
Expand Down Expand Up @@ -420,7 +428,8 @@ bool BicycleTracker::getTrackedObject(

// twist
twist_with_cov.twist.linear.x = X_t(IDX::VX);
twist_with_cov.twist.angular.z = X_t(IDX::WZ);
twist_with_cov.twist.angular.z =
X_t(IDX::VX) / lr_ * std::sin(X_t(IDX::SLIP)); // yaw_rate = vx_k / l_r * sin(slip_k)
// twist covariance
constexpr double vy_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
constexpr double vz_cov = 0.1 * 0.1; // TODO(yukkysaito) Currently tentative
Expand All @@ -429,11 +438,11 @@ bool BicycleTracker::getTrackedObject(
twist_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::VX, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = vy_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::Z_Z] = vz_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::WZ);
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::WZ, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::X_YAW] = P(IDX::VX, IDX::SLIP);
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_X] = P(IDX::SLIP, IDX::VX);
twist_with_cov.covariance[utils::MSG_COV_IDX::ROLL_ROLL] = wx_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::WZ, IDX::WZ);
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP);

// set shape
object.shape.dimensions.x = bounding_box_.length;
Expand Down
Loading