Skip to content

Commit

Permalink
feature(multi_object_tracker): add acc estimation model (#5282)
Browse files Browse the repository at this point in the history 
* add acc estimator to bicycle tracker

Signed-off-by: yoshiri <[email protected]>

* add attenuate model

Signed-off-by: yoshiri <[email protected]>

* add acceleration model into trackings

Signed-off-by: yoshiri <[email protected]>

* add acc estimation for pedestrian tracker

Signed-off-by: yoshiri <[email protected]>

* fix vector size error in big vehicle tracker

Signed-off-by: yoshiri <[email protected]>

* fix acc coordinate and tune bicycle tracker parameter

Signed-off-by: yoshiri <[email protected]>

---------

Signed-off-by: yoshiri <[email protected]>
  • Loading branch information
@YoshiRi
YoshiRi authored Oct 15, 2023
1 parent bc60bbf commit 65a41a5
Show file tree
Hide file tree
Showing 8 changed files with 153 additions and 37 deletions.
7 changes: 5 additions & 2 deletions ...ption/multi_object_tracker/include/multi_object_tracker/tracker/model/bicycle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,16 +32,18 @@ class BicycleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, AX = 4, SLIP = 5 };
struct EkfParams
{
char dim_x = 5;
char dim_x = 6;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_slip;
float q_cov_vx;
float q_cov_ax;
float p0_cov_vx;
float p0_cov_ax;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
Expand All @@ -52,6 +54,7 @@ class BicycleTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_ax_;
double max_slip_;
double lf_;
double lr_;
Expand Down
7 changes: 5 additions & 2 deletions ...n/multi_object_tracker/include/multi_object_tracker/tracker/model/big_vehicle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,16 +32,18 @@ class BigVehicleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, AX = 4, SLIP = 5 };
struct EkfParams
{
char dim_x = 5;
char dim_x = 6;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_slip;
float q_cov_vx;
float q_cov_ax;
float p0_cov_vx;
float p0_cov_ax;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
Expand All @@ -52,6 +54,7 @@ class BigVehicleTracker : public Tracker
float p0_cov_yaw;
} ekf_params_;
double max_vx_;
double max_ax_;
double max_slip_;
double lf_;
double lr_;
Expand Down
7 changes: 5 additions & 2 deletions ...ulti_object_tracker/include/multi_object_tracker/tracker/model/normal_vehicle_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -33,17 +33,19 @@ class NormalVehicleTracker : public Tracker
private:
KalmanFilter ekf_;
rclcpp::Time last_update_time_;
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, SLIP = 4 };
enum IDX { X = 0, Y = 1, YAW = 2, VX = 3, AX = 4, SLIP = 5 };

struct EkfParams
{
char dim_x = 5;
char dim_x = 6;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_slip;
float q_cov_vx;
float q_cov_ax;
float p0_cov_vx;
float p0_cov_ax;
float p0_cov_slip;
float r_cov_x;
float r_cov_y;
Expand All @@ -55,6 +57,7 @@ class NormalVehicleTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_ax_;
double max_slip_;
double lf_;
double lr_;
Expand Down
8 changes: 6 additions & 2 deletions ...on/multi_object_tracker/include/multi_object_tracker/tracker/model/pedestrian_tracker.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -37,17 +37,20 @@ class PedestrianTracker : public Tracker
Y = 1,
YAW = 2,
VX = 3,
WZ = 4,
AX = 4,
WZ = 5,
};
struct EkfParams
{
char dim_x = 5;
char dim_x = 6;
float q_cov_x;
float q_cov_y;
float q_cov_yaw;
float q_cov_wz;
float q_cov_vx;
float q_cov_ax;
float p0_cov_vx;
float p0_cov_ax;
float p0_cov_wz;
float r_cov_x;
float r_cov_y;
Expand All @@ -58,6 +61,7 @@ class PedestrianTracker : public Tracker
} ekf_params_;

double max_vx_;
double max_ax_;
double max_wz_;
float z_;

Expand Down
49 changes: 42 additions & 7 deletions perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -57,6 +57,7 @@ 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_ax = 9.8 / 2; // [m/(s*s)]
float q_stddev_slip = tier4_autoware_utils::deg2rad(15); // [rad/(s*s)]
float r_stddev_x = 0.6; // [m]
float r_stddev_y = 0.4; // [m]
Expand All @@ -65,11 +66,13 @@ BicycleTracker::BicycleTracker(
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_ax = tier4_autoware_utils::kmph2mps(1000); // [m/(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_ax = std::pow(q_stddev_ax, 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);
Expand All @@ -78,7 +81,9 @@ BicycleTracker::BicycleTracker(
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_ax = std::pow(p0_stddev_ax, 2.0);
ekf_params_.p0_cov_slip = std::pow(p0_stddev_slip, 2.0);
max_ax_ = 9.8 * 0.2;
max_vx_ = tier4_autoware_utils::kmph2mps(100); // [m/s]
max_slip_ = tier4_autoware_utils::deg2rad(30); // [rad/s]

Expand All @@ -93,6 +98,7 @@ BicycleTracker::BicycleTracker(
X(IDX::VX) = 0.0;
}
X(IDX::SLIP) = 0.0;
X(IDX::AX) = 0.0;

// initialize P matrix
Eigen::MatrixXd P = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -111,6 +117,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::AX, IDX::AX) = ekf_params_.p0_cov_ax;
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];
Expand All @@ -126,6 +133,7 @@ BicycleTracker::BicycleTracker(
P(IDX::VX, IDX::VX) = ekf_params_.p0_cov_vx;
}
P(IDX::SLIP, IDX::SLIP) = ekf_params_.p0_cov_slip;
P(IDX::AX, IDX::AX) = ekf_params_.p0_cov_ax;
}

if (object.shape.type == autoware_auto_perception_msgs::msg::Shape::BOUNDING_BOX) {
Expand Down Expand Up @@ -159,18 +167,20 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
* 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
* vx_{k+1} = vx_k + ax_k * dt
* ax_{k+1} = choose_abs_min(ea * ax_k, -vx_k/dt)
* slip_{k+1} = slip_k
*
*/

/* == Linearized model ==
*
* 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]
* A = [ 1, 0, -vx*sin(yaw+slip)*dt, cos(yaw+slip)*dt, 0, -vx*sin(yaw+slip)*dt]
* [ 0, 1, vx*cos(yaw+slip)*dt, sin(yaw+slip)*dt, 0, vx*cos(yaw+slip)*dt]
* [ 0, 0, 1, 1/l_r*sin(slip)*dt, 0, vx/l_r*cos(slip)*dt]
* [ 0, 0, 0, 1, dt, 0]
* [ 0, 0, 0, 0, 1, 0]
* [ 0, 0, 0, 0, 0, 1]
*/

// X t
Expand All @@ -182,14 +192,22 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
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));
const double acc_attenuate_rate = 0.8;

// X t+1
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::VX) = X_t(IDX::VX) + X_t(IDX::AX) * dt;
X_next_t(IDX::SLIP) = X_t(IDX::SLIP);
const double a1 = acc_attenuate_rate * X_t(IDX::AX);
const double a2 = -X_t(IDX::VX) / dt;
if (std::fabs(a1) > std::fabs(a2)) {
X_next_t(IDX::AX) = a2;
} else {
X_next_t(IDX::AX) = a1;
}

// A
Eigen::MatrixXd A = Eigen::MatrixXd::Identity(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -201,6 +219,13 @@ bool BicycleTracker::predict(const double dt, KalmanFilter & ekf) const
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;
A(IDX::VX, IDX::AX) = dt;
if (std::fabs(a1) > std::fabs(a2)) {
A(IDX::AX, IDX::AX) = 0;
A(IDX::AX, IDX::VX) = -1.0 / dt;
} else {
A(IDX::AX, IDX::AX) = acc_attenuate_rate;
}

// Q
Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(ekf_params_.dim_x, ekf_params_.dim_x);
Expand All @@ -214,6 +239,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::AX, IDX::AX) = ekf_params_.q_cov_ax * 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 @@ -325,6 +351,9 @@ 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_ax_ <= X_t(IDX::AX) && X_t(IDX::AX) <= max_ax_)) {
X_t(IDX::AX) = X_t(IDX::AX) < 0 ? -max_ax_ : max_ax_;
}
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_;
}
Expand Down Expand Up @@ -395,6 +424,7 @@ bool BicycleTracker::getTrackedObject(

auto & pose_with_cov = object.kinematics.pose_with_covariance;
auto & twist_with_cov = object.kinematics.twist_with_covariance;
auto & acc_with_cov = object.kinematics.acceleration_with_covariance;

// position
pose_with_cov.pose.position.x = X_t(IDX::X);
Expand Down Expand Up @@ -447,6 +477,11 @@ bool BicycleTracker::getTrackedObject(
twist_with_cov.covariance[utils::MSG_COV_IDX::PITCH_PITCH] = wy_cov;
twist_with_cov.covariance[utils::MSG_COV_IDX::YAW_YAW] = P(IDX::SLIP, IDX::SLIP);

// acc
acc_with_cov.accel.linear.x = X_t(IDX::AX);
acc_with_cov.covariance[utils::MSG_COV_IDX::X_X] = P(IDX::AX, IDX::AX) * std::cos(X_t(IDX::SLIP));
acc_with_cov.covariance[utils::MSG_COV_IDX::Y_Y] = P(IDX::AX, IDX::AX) * std::sin(X_t(IDX::SLIP));

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

0 comments on commit 65a41a5

Please sign in to comment.