Skip to content

Commit

Permalink
feat(imu_corrector): changed GyroBiasEstimator to use ndt pose instea…
Browse files Browse the repository at this point in the history
…d of twist (#5259)

* Implemented ndt pose based GyroBiasEstimator

Signed-off-by: Shintaro Sakoda <[email protected]>

* style(pre-commit): autofix

* Added missing includes

Signed-off-by: Shintaro Sakoda <[email protected]>

* FIxed default gyro_bias_threshold

Signed-off-by: Shintaro Sakoda <[email protected]>

* Restored gyro_bias_pub_, which had been deleted due to a mistake

Signed-off-by: Shintaro Sakoda <[email protected]>

* removed get_bias_std and anything else that is no longer needed as a result of remove

Signed-off-by: Shintaro Sakoda <[email protected]>

* Updated README.md

Signed-off-by: Shintaro Sakoda <[email protected]>

* Revert "Updated README.md"

This reverts commit f669c20.

* Updated README.md

Signed-off-by: Shintaro Sakoda <[email protected]>

* Added notes to README.md

Signed-off-by: Shintaro Sakoda <[email protected]>

---------

Signed-off-by: Shintaro Sakoda <[email protected]>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
  • Loading branch information
SakodaShintaro and pre-commit-ci[bot] authored Oct 17, 2023
1 parent 6825200 commit 3a1d480
Show file tree
Hide file tree
Showing 8 changed files with 310 additions and 125 deletions.
31 changes: 18 additions & 13 deletions sensing/imu_corrector/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -51,10 +51,16 @@ Note that the node calculates bias from the gyroscope data by averaging the data

### Input

| Name | Type | Description |
| ----------------- | ------------------------------------------------ | ---------------- |
| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data |
| `~/input/twist` | `geometry_msgs::msg::TwistWithCovarianceStamped` | vehicle velocity |
| Name | Type | Description |
| ----------------- | ----------------------------------------------- | ---------------- |
| `~/input/imu_raw` | `sensor_msgs::msg::Imu` | **raw** imu data |
| `~/input/pose` | `geometry_msgs::msg::PoseWithCovarianceStamped` | ndt pose |

Note that the input pose is assumed to be accurate enough. For example when using NDT, we assume that the NDT is appropriately converged.

Currently, it is possible to use methods other than NDT as a `pose_source` for Autoware, but less accurate methods are not suitable for IMU bias estimation.

In the future, with careful implementation for pose errors, the IMU bias estimated by NDT could potentially be used not only for validation but also for online calibration.

### Output

Expand All @@ -64,12 +70,11 @@ Note that the node calculates bias from the gyroscope data by averaging the data

### Parameters

| Name | Type | Description |
| --------------------------- | ------ | ----------------------------------------------------------------------------- |
| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] |
| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] |
| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] |
| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] |
| `velocity_threshold` | double | threshold of the vehicle velocity to determine if the vehicle is stopped[m/s] |
| `timestamp_threshold` | double | threshold of the timestamp diff between IMU and twist [s] |
| `data_num_threshold` | int | number of data used to calculate bias |
| Name | Type | Description |
| ------------------------------------- | ------ | ------------------------------------------------------------------------------------------- |
| `angular_velocity_offset_x` | double | roll rate offset in imu_link [rad/s] |
| `angular_velocity_offset_y` | double | pitch rate offset imu_link [rad/s] |
| `angular_velocity_offset_z` | double | yaw rate offset imu_link [rad/s] |
| `gyro_bias_threshold` | double | threshold of the bias of the gyroscope [rad/s] |
| `timer_callback_interval_sec` | double | seconds about the timer callback function [sec] |
| `straight_motion_ang_vel_upper_limit` | double | upper limit of yaw angular velocity, beyond which motion is not considered straight [rad/s] |
5 changes: 2 additions & 3 deletions sensing/imu_corrector/config/gyro_bias_estimator.param.yaml
Original file line number Diff line number Diff line change
@@ -1,6 +1,5 @@
/**:
ros__parameters:
gyro_bias_threshold: 0.0015 # [rad/s]
velocity_threshold: 0.0 # [m/s]
timestamp_threshold: 0.1 # [s]
data_num_threshold: 200 # [num]
timer_callback_interval_sec: 0.5 # [sec]
straight_motion_ang_vel_upper_limit: 0.015 # [rad/s]
4 changes: 2 additions & 2 deletions sensing/imu_corrector/launch/gyro_bias_estimator.launch.xml
Original file line number Diff line number Diff line change
@@ -1,14 +1,14 @@
<?xml version="1.0" encoding="UTF-8"?>
<launch>
<arg name="input_imu_raw" default="imu_raw"/>
<arg name="input_twist" default="twist"/>
<arg name="input_pose" default="pose"/>
<arg name="output_gyro_bias" default="gyro_bias"/>
<arg name="gyro_bias_estimator_param_file" default="$(find-pkg-share imu_corrector)/config/gyro_bias_estimator.param.yaml"/>
<arg name="imu_corrector_param_file" default="$(find-pkg-share imu_corrector)/config/imu_corrector.param.yaml"/>

<node pkg="imu_corrector" exec="gyro_bias_estimator" name="gyro_bias_estimator" output="screen">
<remap from="~/input/imu_raw" to="$(var input_imu_raw)"/>
<remap from="~/input/twist" to="$(var input_twist)"/>
<remap from="~/input/pose" to="$(var input_pose)"/>
<remap from="~/output/gyro_bias" to="$(var output_gyro_bias)"/>
<param from="$(var gyro_bias_estimator_param_file)"/>
<param from="$(var imu_corrector_param_file)"/>
Expand Down
124 changes: 86 additions & 38 deletions sensing/imu_corrector/src/gyro_bias_estimation_module.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -14,58 +14,106 @@

#include "gyro_bias_estimation_module.hpp"

#include "tier4_autoware_utils/geometry/geometry.hpp"

#include <rclcpp/rclcpp.hpp>

namespace imu_corrector
{
GyroBiasEstimationModule::GyroBiasEstimationModule(
const double velocity_threshold, const double timestamp_threshold,
const size_t data_num_threshold)
: velocity_threshold_(velocity_threshold),
timestamp_threshold_(timestamp_threshold),
data_num_threshold_(data_num_threshold),
is_stopped_(false)
{
}

void GyroBiasEstimationModule::update_gyro(
const double time, const geometry_msgs::msg::Vector3 & gyro)
/**
* @brief perform dead reckoning based on "gyro_list" and return a relative pose (in RPY)
*/
geometry_msgs::msg::Vector3 integrate_orientation(
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list,
const geometry_msgs::msg::Vector3 & gyro_bias)
{
if (time - last_velocity_time_ > timestamp_threshold_) {
return;
}
if (!is_stopped_) {
return;
}
gyro_buffer_.push_back(gyro);
if (gyro_buffer_.size() > data_num_threshold_) {
gyro_buffer_.pop_front();
geometry_msgs::msg::Vector3 d_rpy{};
double t_prev = rclcpp::Time(gyro_list.front().header.stamp).seconds();
for (std::size_t i = 0; i < gyro_list.size() - 1; ++i) {
double t_cur = rclcpp::Time(gyro_list[i + 1].header.stamp).seconds();

d_rpy.x += (t_cur - t_prev) * (gyro_list[i].vector.x - gyro_bias.x);
d_rpy.y += (t_cur - t_prev) * (gyro_list[i].vector.y - gyro_bias.y);
d_rpy.z += (t_cur - t_prev) * (gyro_list[i].vector.z - gyro_bias.z);

t_prev = t_cur;
}
return d_rpy;
}

void GyroBiasEstimationModule::update_velocity(const double time, const double velocity)
/**
* @brief calculate RPY error on dead-reckoning (calculated from "gyro_list") compared to the
* ground-truth pose from "pose_list".
*/
geometry_msgs::msg::Vector3 calculate_error_rpy(
const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list,
const geometry_msgs::msg::Vector3 & gyro_bias)
{
is_stopped_ = velocity <= velocity_threshold_;
last_velocity_time_ = time;
const geometry_msgs::msg::Vector3 rpy_0 =
tier4_autoware_utils::getRPY(pose_list.front().pose.orientation);
const geometry_msgs::msg::Vector3 rpy_1 =
tier4_autoware_utils::getRPY(pose_list.back().pose.orientation);
const geometry_msgs::msg::Vector3 d_rpy = integrate_orientation(gyro_list, gyro_bias);

geometry_msgs::msg::Vector3 error_rpy;
error_rpy.x = tier4_autoware_utils::normalizeRadian(-rpy_1.x + rpy_0.x + d_rpy.x);
error_rpy.y = tier4_autoware_utils::normalizeRadian(-rpy_1.y + rpy_0.y + d_rpy.y);
error_rpy.z = tier4_autoware_utils::normalizeRadian(-rpy_1.z + rpy_0.z + d_rpy.z);
return error_rpy;
}

geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias() const
/**
* @brief update gyroscope bias based on a given trajectory data
*/
void GyroBiasEstimationModule::update_bias(
const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list)
{
if (gyro_buffer_.size() < data_num_threshold_) {
throw std::runtime_error("Bias estimation is not yet ready because of insufficient data.");
const double dt_pose =
(rclcpp::Time(pose_list.back().header.stamp) - rclcpp::Time(pose_list.front().header.stamp))
.seconds();
const double dt_gyro =
(rclcpp::Time(gyro_list.back().header.stamp) - rclcpp::Time(gyro_list.front().header.stamp))
.seconds();
if (dt_pose == 0 || dt_gyro == 0) {
throw std::runtime_error("dt_pose or dt_gyro is zero");
}

geometry_msgs::msg::Vector3 bias;
bias.x = 0.0;
bias.y = 0.0;
bias.z = 0.0;
for (const auto & gyro : gyro_buffer_) {
bias.x += gyro.x;
bias.y += gyro.y;
bias.z += gyro.z;
auto error_rpy = calculate_error_rpy(pose_list, gyro_list, geometry_msgs::msg::Vector3{});
error_rpy.x *= dt_pose / dt_gyro;
error_rpy.y *= dt_pose / dt_gyro;
error_rpy.z *= dt_pose / dt_gyro;

gyro_bias_pair_.first.x += dt_pose * error_rpy.x;
gyro_bias_pair_.first.y += dt_pose * error_rpy.y;
gyro_bias_pair_.first.z += dt_pose * error_rpy.z;
gyro_bias_pair_.second.x += dt_pose * dt_pose;
gyro_bias_pair_.second.y += dt_pose * dt_pose;
gyro_bias_pair_.second.z += dt_pose * dt_pose;

geometry_msgs::msg::Vector3 gyro_bias;
gyro_bias.x = error_rpy.x / dt_pose;
gyro_bias.y = error_rpy.y / dt_pose;
gyro_bias.z = error_rpy.z / dt_pose;
}

/**
* @brief getter function for current estimated bias
*/
geometry_msgs::msg::Vector3 GyroBiasEstimationModule::get_bias_base_link() const
{
geometry_msgs::msg::Vector3 gyro_bias_base;
if (
gyro_bias_pair_.second.x == 0 || gyro_bias_pair_.second.y == 0 ||
gyro_bias_pair_.second.z == 0) {
throw std::runtime_error("gyro_bias_pair_.second is zero");
}
bias.x /= gyro_buffer_.size();
bias.y /= gyro_buffer_.size();
bias.z /= gyro_buffer_.size();
return bias;
gyro_bias_base.x = gyro_bias_pair_.first.x / gyro_bias_pair_.second.x;
gyro_bias_base.y = gyro_bias_pair_.first.y / gyro_bias_pair_.second.y;
gyro_bias_base.z = gyro_bias_pair_.first.z / gyro_bias_pair_.second.z;
return gyro_bias_base;
}

} // namespace imu_corrector
25 changes: 11 additions & 14 deletions sensing/imu_corrector/src/gyro_bias_estimation_module.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -11,33 +11,30 @@
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef GYRO_BIAS_ESTIMATION_MODULE_HPP_
#define GYRO_BIAS_ESTIMATION_MODULE_HPP_

#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <geometry_msgs/msg/vector3_stamped.hpp>

#include <deque>
#include <utility>
#include <vector>

namespace imu_corrector
{
class GyroBiasEstimationModule
{
public:
GyroBiasEstimationModule(
const double velocity_threshold, const double timestamp_threshold,
const size_t data_num_threshold);
geometry_msgs::msg::Vector3 get_bias() const;
void update_gyro(const double time, const geometry_msgs::msg::Vector3 & gyro);
void update_velocity(const double time, const double velocity);
GyroBiasEstimationModule() = default;
void update_bias(
const std::vector<geometry_msgs::msg::PoseStamped> & pose_list,
const std::vector<geometry_msgs::msg::Vector3Stamped> & gyro_list);
geometry_msgs::msg::Vector3 get_bias_base_link() const;

private:
const double velocity_threshold_;
const double timestamp_threshold_;
const size_t data_num_threshold_;
bool is_stopped_;
std::deque<geometry_msgs::msg::Vector3> gyro_buffer_;

double last_velocity_time_;
std::pair<geometry_msgs::msg::Vector3, geometry_msgs::msg::Vector3> gyro_bias_pair_;
};
} // namespace imu_corrector

Expand Down
Loading

0 comments on commit 3a1d480

Please sign in to comment.