C语言lidar_align雷达里程计校准功能怎么用
更新时间：2023-11-12介绍
lidar_align是一个处理和分析激光雷达数据的常用工具包，包括里程计校准功能。该功能旨在针对自动驾驶过程中，利用车辆的测距传感器，对激光雷达的数据进行位置调整和补偿，实现激光雷达数据与车辆传感器数据的无缝衔接。此功能可帮助用户快速准确地生成3D地图、检测障碍物、定位等。
使用方法
实现里程计校准功能需要完成以下步骤：
首先需要创建一个lidar_llauncher包，该包用于设置节点、定义参数等。
$ catkin_create_pkg lidar_llauncher roscpp sensor_msgs
创建完毕之后，通过sudo apt-get install ros-kinetic-robot-localization下载所需的包。接下来，需设置配置文件路径。
$ mkdir lidar_llauncher/config $ cd lidar_llauncher/config
在config文件夹下创建一个YAML文件，文件名为lidar_llauncher.yaml，文件内容如下：
vehicle_imu:
update_rate: 50
gyro_alpha: 0.1 # gyroscope noise equivalent rate
accel_alpha: 0.1 # accelerometer noise equivalent acceleration
wheel_odom:
update_rate: 100
wheel_radius: 0.4
wheel_base: 1.9
pub_rate: 50
publish_adaptive: false
use_imu_heading: true
imu_filter:
type: madgwick
params:
time_constant: 0.5
beta: 0.4
接下来，创建一个launch文件，命名为lidar_llauncher.launch，文件内容如下：
user
最后，通过以下命令启动lidar_llauncher节点。
$ roslaunch lidar_llauncher lidar_llauncher.launch
功能原理
里程计校准功能的原理是利用车辆传感器数据和激光雷达数据，通过结合EKF滤波器进行数据融合，实现位置和方向的校准。
首先，从车辆传感器中获取车辆的位置、姿态和速度等信息，并将这些信息作为里程计的输入。
void vehicleImuCallback(const sensor_msgs::ImuConstPtr& imu_msg)
{
if (!initialized_)
{
last_imu_time_ = imu_msg->header.stamp;
initialized_ = true;
bool set_topic = false;
if (!nh_.getParam("odometry_topic", odometry_topic_))
{
ROS_ERROR("Set the odometry_topic parameter and try again.");
set_topic = true;
}
if(set_topic)
{
ROS_WARN("Defaulting to /odom");
odometry_topic_ = "odom";
}
// Set up publishers and subscribers
imu_sub_.shutdown();
imu_sub_ = nh_.subscribe("imu/data", 50, &ImuFilter::imuCallback, this);
odom_pub_ = nh_.advertise(odometry_topic_, 50);
reset_pub_ = nh_.advertise("reset", 50);
// Initialize the filter
filter_.initialize(ros::Time::now(), "map");
filter_.setSensorTimeout(nh_.param("sensor_timeout", 0.2));
filter_.useControl(nh_.param("use_control", false));
filter_.setDebug(nh_.param("debug", false));
}
else
{
double dt = (imu_msg->header.stamp - last_imu_time_).toSec();
last_imu_time_ = imu_msg->header.stamp;
filter_.predict(imu_msg, dt);
if(filter_.isInitialized())
{
nav_msgs::Odometry odom = filter_.getOdometry();
odom_pub_.publish(odom);
}
...
}
}
其次，从激光雷达中获取点云信息，并进行处理、变换，最终转换为基于"map"坐标系的点云数据。
void LidarIntegrationPlugin::pointCloudCallback(
const sensor_msgs::PointCloud2ConstPtr& point_cloud_msg,
const Eigen::Matrix4f& vehicle_to_map,
const ros::Time& timestamp) const
{
if(!initialized_)
return;
pcl::PointCloud::Ptr pcl_cloud(new pcl::PointCloud());
pcl::fromROSMsg(*point_cloud_msg, *pcl_cloud);
Eigen::Matrix4f lidar_to_vehicle;
lidar_to_vehicle << 0, 0, 1, 0,
-1, 0, 0, 0,
0, -1, 0, 0,
0, 0, 0, 1;
pcl::transformPointCloud(*pcl_cloud, *pcl_cloud, lidar_to_vehicle);
Eigen::Matrix4f lidar_to_map = vehicle_to_map * lidar_to_vehicle;
pcl::transformPointCloud(*pcl_cloud, *pcl_cloud, lidar_to_map);
sensor_msgs::PointCloud2 transformed_cloud;
pcl::toROSMsg(*pcl_cloud, transformed_cloud);
...
}
最后，通过EKF滤波器将两个数据源进行融合，实现激光雷达数据和车辆传感器数据的位置和方向补偿。
void Ekf::correct(const Measurement& measurement, const Eigen::VectorXd& measurement_vec)
{
if(process_noise_covariance_.isZero())
{
// If the process noise covariance matrix hasn't been set yet, then
// default to the identity matrix (i.e., no process noise).
process_noise_covariance_ = Eigen::MatrixXd::Identity(state_.rows(), state_.rows());
}
// If we've never had a measurement before, create our covariance matrix and state vector here
if(!measurement_initialized_)
{
initializeMeasurements(measurement, measurement_vec, measurement_covariance_.diagonal().asDiagonal());
measurement_initialized_ = true;
return;
}
Eigen::MatrixXd H_jacobian;
// 激光雷达测距数据
if(measurement.type_ == Measurement::LASER)
{
H_jacobian = measurement.jacobian_ = jacobianH(state_);
measurement_covariance_ = measurement.covariance_;
measurement_matrix_ = Eigen::MatrixXd::Identity(measurement_vec.rows(), measurement_vec.rows());
}
else if(measurement.type_ == Measurement::IMU)
{
H_jacobian = measurement.jacobian_ = jacobianG(state_);
measurement_covariance_ = measurement.covariance_;
}
else if(measurement.type_ == Measurement::ODOM)
{
H_jacobian = measurement.jacobian_ = jacobianOdom(state_);
measurement_covariance_ = measurement.covariance_;
}
Eigen::VectorXd y = measurement_vec - measurement.expected_;
y(3) = angles::normalize_angle(y(3));
y(4) = angles::normalize_angle(y(4));
y(5) = angles::normalize_angle(y(5));
Eigen::MatrixXd H_j_t = H_jacobian.transpose();
Eigen::MatrixXd S = (H_jacobian * P_ * H_j_t) + measurement_covariance_.matrix();
Eigen::MatrixXd S_inv = S.inverse();
Eigen::MatrixXd K = P_ * H_j_t * S_inv;
updateState(K * y);
Eigen::MatrixXd I = Eigen::MatrixXd::Identity(state_.rows(), state_.rows());
P_ = (I - K * H_jacobian) * P_ * (I - K * H_jacobian).transpose() + K * measurement_covariance_.matrix() * K.transpose();
measurements_.push_back(measurement);
previous_update_time_ = ros::Time::now();
}
总结
lidar_align的里程计校准功能可以帮助用户快速生成3D地图、检测障碍物、定位等。使用lidar_align进行里程计校准需要通过几个步骤完成：创建一个lidar_llauncher包等；调用车辆传感器数据和激光雷达数据，完成数据变换并把两种功与数据进行融合等。最终，通过里程计校准功能，可以实现激光雷达数据和车辆传感器数据的位置和方向补偿，实现无缝衔接。
里程计校准功能实现的核心是EKF滤波器，对于需要求解非线性问题具有很好的效果。在实际应用中，需要根据具体需求调整滤波器中的参数，以确保算法精度和稳定性。
此外，lidar_align的里程计校准功能，虽然最终生成的精度高、准确性高的3D地图等数据，但需要大量的计算和测量，并且难度较大，适合应用于复杂环境下的自动驾驶领域等。
