一、问题分析
机器人可以基于码盘数据和底盘运动学模型进行航迹推演，得到机器人的轨迹，但实际轨迹与推演轨迹存在误差，分析原因如下：
底盘实际尺寸与理论模型存在偏差，如轮子半径，两轮间距等；
底盘在运动过程中打滑或路面不平整导致偏差；
注意：
方向上的偏差较平移的偏差影响更大，会随着移动扩大误差。
码盘里程计受电控，底盘机械结构影响，在路况复杂和室外场景下，误差会较大。
解决方案
直接线性（此处使用该方法）
基于模型（可参考2d Laser 和 Odomter 内外参数标定工具原理及使用方法）
二、航迹推算
欧拉积分
该积分误差主要来源于平移积分过程中认为角度恒定，这在直线运动时不受影响，以及△ t 很小时误差影响也较小。
二阶Runge-Kutta积分
该积分过程中认为角度为运动前后的均值，误差比欧拉积分小一点。
三、激光匹配
在移动机器人中，里程计一般由底盘编码器提供，但其存在误差，一般可将其相对坐标变换[ R , t ] [R,t][R,t]（当前与前一状态）作为激光匹配算法的first guess，然后通过迭代匹配优化作为真值用于里程计的标定（实际上并不是真值）。
此处使用srf_laser_odometry替代ICP实现激光里程计。
四、算法实现
4.1 原理分析
u ( x , y , θ )为增加值，非累加值，每一小段构成一组方程。
航迹推算在odom坐标系下进行，每个步距的移动在机器人base_link坐标系下，需要转换到odom坐标系下，再进行累加。
4.2 工程实现
激光匹配
通过第一帧初始化基本参数并作为参考帧，之后不断读入当前帧与参考帧做计算获取坐标变换。
注意
雷达安装位置不在小车中心，则需要将变换结果转换到小车坐标系上。
高频雷达可两到三帧匹配一次，降低计算量提高稳定性，但会降低匹配结果频率。
旋转匹配因运动畸变的原因会造成一定偏差。
class SRFScanMatch
{
public:
SRFScanMatch();
/**
* @brief 初始化srf
*
* @param last_scan 第一帧雷达信息
*/
void init(sensor_msgs::LaserScan scan_msg);
/**
* @brief 激光匹配
*
* @param current_scan
* @return Eigen::Vector3d
*/
Eigen::Vector3d scanMatch(sensor_msgs::LaserScan current_scan);
private:
// Core class of SRF
SRF_RefS srf_obj;
// 坐标变换
mrpt::poses::CPose3D laser_pose_on_robot_inv_;
mrpt::poses::CPose3D robot_pose_;
nav_msgs::Odometry initial_robot_pose_;
tf2_ros::Buffer tf_Buffer_;
std::string base_frame_id_ = "base_link";
bool first_laser_scan_;
double laser_min_range_, laser_max_range_;
};
SRFScanMatch::SRFScanMatch()
{
// tf2_ros::TransformListener tfListener(tf_Buffer_);
initial_robot_pose_.pose.pose.position.x = 0;
initial_robot_pose_.pose.pose.position.y = 0;
initial_robot_pose_.pose.pose.position.z = 0;
initial_robot_pose_.pose.pose.orientation.w = 1;
initial_robot_pose_.pose.pose.orientation.x = 0;
initial_robot_pose_.pose.pose.orientation.y = 0;
initial_robot_pose_.pose.pose.orientation.z = 0;
first_laser_scan_ = true;
}
void SRFScanMatch::init(sensor_msgs::LaserScan scan_msg)
{
tf2_ros::TransformListener tfListener(tf_Buffer_);
const unsigned int scan_size = scan_msg.ranges.size(); // Num of samples (size) of the scan laser
const float fov = fabs(scan_msg.angle_max - scan_msg.angle_min); // Horizontal Laser's FOV
// 初始化，雷达数量，视场角，HYBRID模式
srf_obj.initialize(scan_size, fov, 2);
// 获取雷达坐标系
geometry_msgs::TransformStamped laser_to_base;
mrpt::poses::CPose3D LaserPoseOnTheRobot;
try
{
laser_to_base = tf_Buffer_.lookupTransform(base_frame_id_, scan_msg.header.frame_id, ros::Time(0));
// transform -> mrpt::CPose3D
geometry_msgs::Pose laser_transform;
laser_transform.orientation = laser_to_base.transform.rotation;
laser_transform.position.x = laser_to_base.transform.translation.x;
laser_transform.position.y = laser_to_base.transform.translation.y;
laser_transform.position.z = laser_to_base.transform.translation.z;
mrpt_bridge::convert(laser_transform, LaserPoseOnTheRobot);
ROS_INFO_STREAM(laser_transform);
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
}
// 获取雷达坐标系
geometry_msgs::TransformStamped base_to_laser;
try
{
base_to_laser = tf_Buffer_.lookupTransform(scan_msg.header.frame_id, base_frame_id_, ros::Time(0));
}
catch (tf2::TransformException& ex)
{
ROS_WARN("%s", ex.what());
}
// transform -> mrpt::CPose3D
geometry_msgs::Pose base_transform;
base_transform.orientation = base_to_laser.transform.rotation;
base_transform.position.x = base_to_laser.transform.translation.x;
base_transform.position.y = base_to_laser.transform.translation.y;
base_transform.position.z = base_to_laser.transform.translation.z;
mrpt_bridge::convert(base_transform, laser_pose_on_robot_inv_);
ROS_INFO_STREAM(base_transform);
// Robot initial pose
mrpt::poses::CPose3D robotInitialPose;
geometry_msgs::Pose _src = initial_robot_pose_.pose.pose;
mrpt_bridge::convert(_src, robotInitialPose);
// Set the Laser initial pose = Robot_initial_pose + LaserPoseOnTheRobot
srf_obj.laser_pose = mrpt::poses::CPose2D(robotInitialPose + LaserPoseOnTheRobot);
srf_obj.laser_oldpose = mrpt::poses::CPose2D(robotInitialPose + LaserPoseOnTheRobot);
ROS_INFO("[SRF] Configuration Done.");
}
Eigen::Vector3d SRFScanMatch::scanMatch(sensor_msgs::LaserScan last_scan)
{
// Initialize module on first scan
if (first_laser_scan_)
{
init(last_scan);
first_laser_scan_ = false;
// Load first scan and build pyramid
for (unsigned int i = 0; i < last_scan.ranges.size(); i++)
srf_obj.range_wf(i) = last_scan.ranges[i];
srf_obj.createScanPyramid();
// Set laser min-max distances (if not set as parameters)
laser_min_range_ = 0.0;
laser_max_range_ = 0.99f * last_scan.range_max;
return Eigen::Vector3d{ 0, 0, 0 };
}
else
{
// copy laser scan to internal srf variable
for (unsigned int i = 0; i < last_scan.ranges.size(); i++)
{
// Check min-max distances, e.g. to avoid including points of the own robot
if ((last_scan.ranges[i] > laser_max_range_) || (last_scan.ranges[i] < laser_min_range_))
{
srf_obj.range_wf(i) = 0.f; // invalid measurement
last_scan.ranges[i] = 0.0;
}
else
srf_obj.range_wf(i) = last_scan.ranges[i];
}
// Process odometry estimation
srf_obj.odometryCalculation();
robot_pose_ = srf_obj.laser_pose + laser_pose_on_robot_inv_;
return Eigen::Vector3d{ robot_pose_.x(), robot_pose_.y(), robot_pose_.yaw() };
//return Eigen::Vector3d{ srf_obj.laser_pose.x(), srf_obj.laser_pose.y(), srf_obj.laser_pose.phi() };
}
return Eigen::Vector3d{ 0, 0, 0 };
}
里程计标定
读入真值和测量值，并设置最大数据防止过大，再通过Eigen实现QR分解计算最小二乘解。
class OdomCalib
{
private:
Eigen::MatrixXd A_; // 里程计测量值
Eigen::VectorXd b_; // 移动机器人真值
int data_len_, now_len_;
public:
OdomCalib();
~OdomCalib();
/**
* @brief 设置数据长度
*
* @param len
*/
void setDataLen(int len);
/**
* @brief 构建最小二乘需要的超定方程组,Ax = b
*
* @param odom_value 里程计值
* @param true_value 移动真值
* @return true
* @return false
*/
bool addData(Eigen::Vector3d odom_value, Eigen::Vector3d true_value);
/**
* @brief 解最小二乘
*
* @return Eigen::Matrix3d
*/
Eigen::Matrix3d solveFun();
/**
* @brief 用于判断数据是否满，满即进行最小二乘计算
*
* @return true
* @return false
*/
bool isFull();
/**
* @brief 数据清零
*
*/
void cleanData();
};
OdomCalib::OdomCalib()
{
data_len_ = 0;
now_len_ = 0;
}
OdomCalib::~OdomCalib(){}
void OdomCalib::setDataLen(int len)
{
data_len_ = len;
A_.conservativeResize(len * 3, 9);
b_.conservativeResize(len * 3);
A_.setZero();
b_.setZero();
}
bool OdomCalib::addData(Eigen::Vector3d odom_value, Eigen::Vector3d true_value)
{
if (now_len_ < INT_MAX)
{
// 构建超定方程组
A_(now_len_ % data_len_ * 3, 0) = odom_value(0);
A_(now_len_ % data_len_ * 3, 1) = odom_value(1);
A_(now_len_ % data_len_ * 3, 2) = odom_value(2);
A_(now_len_ % data_len_ * 3 + 1, 3) = odom_value(0);
A_(now_len_ % data_len_ * 3 + 1, 4) = odom_value(1);
A_(now_len_ % data_len_ * 3 + 1, 5) = odom_value(2);
A_(now_len_ % data_len_ * 3 + 2, 6) = odom_value(0);
A_(now_len_ % data_len_ * 3 + 2, 7) = odom_value(1);
A_(now_len_ % data_len_ * 3 + 2, 8) = odom_value(2);
b_(now_len_ % data_len_ * 3) = true_value(0);
b_(now_len_ % data_len_ * 3 + 1) = true_value(1);
b_(now_len_ % data_len_ * 3 + 2) = true_value(2);
now_len_++;
return true;
}
else
{
return false;
}
}
bool OdomCalib::isFull()
{
if (now_len_ % data_len_ == 0 && now_len_ >= 1)
{
now_len_ = data_len_;
return true;
}
else
return false;
}
Eigen::Matrix3d OdomCalib::solveFun()
{
Eigen::Matrix3d correct_matrix;
// 求解线性最小二乘
Eigen::VectorXd correct_vector = A_.colPivHouseholderQr().solve(b_);
ROS_INFO_STREAM("correct_vector:" << correct_vector);
correct_matrix << correct_vector(0), correct_vector(1), correct_vector(2), correct_vector(3), correct_vector(4),
correct_vector(5), correct_vector(6), correct_vector(7), correct_vector(8);
return correct_matrix;
}
void OdomCalib::cleanData()
{
A_.setZero();
b_.setZero();
}
校准节点
订阅里程计和雷达消息，然后通过tf2_ros::MessageFilter实现消息的时间对齐，再计算两帧之间的步距并添加到里程计标定中，最后进行计算矫正矩阵并对里程计进行矫正处理获得矫正路径。
class OdomCalibNode
{
private:
// ros
ros::NodeHandle* nh_;
nav_msgs::Path path_odom_, path_scan_;
ros::Time current_time_;
std::string odom_frame_;
std::string base_frame_;
// odom & scan进行位姿积分
Eigen::Vector3d scan_pos_cal_;
Eigen::Vector3d odom_pos_cal_;
//用来储存两帧之间的里程计的增量
std::vector<Eigen::Vector3d> odom_increments_;
Eigen::Vector3d odom_last_pos_, scan_last_pos_;
//设置接收器
tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;
//初始化接发器
ros::Subscriber calib_flag_sub_, odom_sub_;
ros::Publisher odom_path_pub_, scan_path_pub_, calib_path_pub_;
//时间同步
message_filters::Subscriber<sensor_msgs::LaserScan>* scan_filter_sub_;
tf2_ros::MessageFilter<sensor_msgs::LaserScan>* scan_filter_;
//初始化激光匹配和里程计标定
ScanMatch scan_match;
SRFScanMatch srf_scan_match;
OdomCalib odom_calib;
bool first_laser_scan_;
//坐标变换
tf2_ros::TransformBroadcaster broadcaster;
geometry_msgs::TransformStamped transformStamped;
public:
OdomCalibNode(ros::NodeHandle* nh);
~OdomCalibNode();
/**
* @brief 标定回调
*
* @param msg
*/
void calibFlagCallBack(const std_msgs::Empty& msg);
/**
* @brief 雷达回调
*
* @param msg
*/
void scanCallBack(const sensor_msgs::LaserScan::ConstPtr& msg);
/**
* @brief 读取里程计信息并发布坐标变换
*
* @param msg
*/
void odomCallBack(const nav_msgs::Odometry::ConstPtr& msg);
/**
* @brief 发布odom & laser path
*
* @param pose
* @param path
* @param mcu_path_pub_
*/
void pubMsg(Eigen::Vector3d& pose, nav_msgs::Path& path, ros::Publisher& mcu_path_pub);
/**
* @brief 发布correct path
*
* @param path_eigen
* @param path_pub_
*/
void publishPathEigen(std::vector<Eigen::Vector3d>& path_eigen, ros::Publisher& path_pub);
/**
* @brief 获取当前机器人在odom位姿
*
* @param pose 返回位姿
* @param t 时间点
* @return true
* @return false
*/
bool getOdomPose(Eigen::Vector3d& pose, const ros::Time& t);
/**
* @brief 计算当前位姿与前一位姿的距离
*
* @param last_pose 前一位姿
* @param now_pose 当前位姿
* @return Eigen::Vector3d
*/
Eigen::Vector3d calDeltaDistence(Eigen::Vector3d last_pose, Eigen::Vector3d now_pose);
/**
* @brief 欧垃角转旋转变换矩阵
*
* @param yaw
* @param pitch
* @param roll
* @return Eigen::Matrix3d
*/
Eigen::Matrix3d eul2rotm(double yaw, double pitch, double roll);
};
OdomCalibNode::OdomCalibNode(ros::NodeHandle* nh) : nh_(nh), tf_listener_(tf_buffer_)
{
// 设置坐标系
if (!nh_->getParam("odom_frame", odom_frame_))
odom_frame_ = "odom";
if (!nh_->getParam("base_frame", base_frame_))
base_frame_ = "base_link";
// 初始化变量
scan_pos_cal_.setZero();
odom_pos_cal_.setZero();
odom_increments_.clear();
odom_last_pos_.setZero();
scan_last_pos_.setZero();
first_laser_scan_ = true;
// 初始化里程计标定
odom_calib.setDataLen(150);
odom_calib.cleanData();
//最小二乘的解算
calib_flag_sub_ = nh_->subscribe("calib_flag", 5, &OdomCalibNode::calibFlagCallBack, this);
//发布路径
odom_path_pub_ = nh_->advertise<nav_msgs::Path>("odom_path_pub_", 1, true);
scan_path_pub_ = nh_->advertise<nav_msgs::Path>("scan_path_pub_", 1, true);
calib_path_pub_ = nh_->advertise<nav_msgs::Path>("calib_path_pub_", 1, true);
current_time_ = ros::Time::now();
path_odom_.header.stamp = current_time_;
path_scan_.header.stamp = current_time_;
path_odom_.header.frame_id = "odom";
path_scan_.header.frame_id = "odom";
// odom_sub_ = nh_->subscribe<nav_msgs::Odometry>("/odom", 10, &OdomCalibNode::odomCallBack, this);
//进行里程计和激光雷达数据的同步
scan_filter_sub_ = new message_filters::Subscriber<sensor_msgs::LaserScan>(*nh_, "/scan", 10);
scan_filter_ =
new tf2_ros::MessageFilter<sensor_msgs::LaserScan>(*scan_filter_sub_, tf_buffer_, odom_frame_, 10, *nh_);
scan_filter_->registerCallback(boost::bind(&OdomCalibNode::scanCallBack, this, _1));
ROS_INFO_STREAM("Calibration Online,Wait for Data");
}
OdomCalibNode::~OdomCalibNode()
{
}
void OdomCalibNode::calibFlagCallBack(const std_msgs::Empty& msg)
{
Eigen::Matrix3d correct_matrix = odom_calib.solveFun();
Eigen::Matrix3d tmp_transform_matrix;
ROS_INFO_STREAM("correct_matrix:" << correct_matrix);
//计算矫正之后的路径
Eigen::Vector3d calib_pos(0, 0, 0); //矫正之后的位姿
std::vector<Eigen::Vector3d> calib_path_eigen; //矫正之后的路径
for (int i = 0; i < odom_increments_.size(); i++)
{
Eigen::Vector3d odom_inc = odom_increments_[i];
Eigen::Vector3d correct_inc = correct_matrix * odom_inc;
double c = cos(calib_pos(2) + correct_inc(2) / 2);
double s = sin(calib_pos(2) + correct_inc(2) / 2);
tmp_transform_matrix << c, -s, 0, s, c, 0, 0, 0, 1;
calib_pos += tmp_transform_matrix * correct_inc;
calib_path_eigen.push_back(calib_pos);
}
//发布矫正之后的路径
publishPathEigen(calib_path_eigen, calib_path_pub_);
// 写入参数
std::vector<double> correct_vector{ correct_matrix(0, 0), correct_matrix(0, 1), correct_matrix(0, 2),
correct_matrix(1, 0), correct_matrix(1, 1), correct_matrix(1, 2),
correct_matrix(2, 0), correct_matrix(2, 1), correct_matrix(2, 2) };
ros::param::set("/odom_calib", correct_vector);
// 矫正完毕，退出订阅
scan_filter_sub_->unsubscribe();
ROS_INFO_STREAM("calibration over");
std::string pkg_path = ros::package::getPath("robot_driver");
std::string param_file = pkg_path + "/param/odom_calib.yaml";
YAML::Node yamlConfig = YAML::LoadFile(param_file);
//修改参数
yamlConfig["odom_calib"] = correct_vector;
//写
std::ofstream file;
file.open(param_file);
file.flush();
file << yamlConfig;
file.close();
}
void OdomCalibNode::scanCallBack(const sensor_msgs::LaserScan::ConstPtr& msg)
{
static long int dataCnt = 0;
sensor_msgs::LaserScan scan;
Eigen::Vector3d odom_pose; //底盘编码器里程计位姿变换
Eigen::Vector3d scan_pose; //激光里程计位姿变换
Eigen::Vector3d d_point_odom; //里程计计算的dpose
Eigen::Vector3d d_point_scan; //激光的scanmatch计算的dpose
Eigen::MatrixXd transform_matrix(3, 3); //临时的变量
d_point_odom << 0, 0, 0;
double c, s;
scan = *msg;
//得到对应的里程计数据
if (!getOdomPose(odom_pose, msg->header.stamp))
return;
//初始化srf雷达信息和里程计当前信息
if (first_laser_scan_)
{
scan_pose = srf_scan_match.scanMatch(scan);
first_laser_scan_ = false;
odom_last_pos_ = odom_pose;
return;
}
//若静止则不计算
if (abs(odom_pose(0) - odom_last_pos_(0)) < 0.0001 && abs(odom_pose(1) - odom_last_pos_(1)) < 0.0001 &&
abs(odom_pose(2) - odom_last_pos_(2)) < tf2Radians(0.01))
{
return;
}
//前后两帧里程计的位姿差
d_point_odom = calDeltaDistence(odom_last_pos_, odom_pose);
//如果运动的距离太短，则不进行处理．
if (std::isnan(d_point_odom(0)) || std::isnan(d_point_odom(1)) || std::isnan(d_point_odom(2)))
{
return;
}
if (abs(d_point_odom(0)) < 0.05 && abs(d_point_odom(1)) < 0.05 && abs(d_point_odom(2)) < tf2Radians(3))
{
return;
}
// srf scan match计算
scan_pose = srf_scan_match.scanMatch(scan);
d_point_scan = calDeltaDistence(scan_last_pos_, scan_pose);
// 去除跳变
// if (abs(d_point_scan(0)) > 0.1 || abs(d_point_scan(1)) > 0.1 || abs(d_point_scan(2)) > tf2Radians(10))
//{
// return;
//}
// 记录当前位姿
scan_last_pos_ = scan_pose;
odom_last_pos_ = odom_pose;
// 里程计累计的位姿,for odom_path visualization
c = cos(odom_pos_cal_(2) + d_point_odom(2) / 2);
s = sin(odom_pos_cal_(2) + d_point_odom(2) / 2);
transform_matrix << c, -s, 0, s, c, 0, 0, 0, 1;
odom_pos_cal_ += transform_matrix * d_point_odom;
//记录下里程计的增量数据
odom_increments_.push_back(d_point_odom);
// 两针scan计算本身累计的位姿 for laser_path visualization
c = cos(scan_pos_cal_(2) + d_point_scan(2) / 2);
s = sin(scan_pos_cal_(2) + d_point_scan(2) / 2);
transform_matrix << c, -s, 0, s, c, 0, 0, 0, 1;
scan_pos_cal_ += transform_matrix * d_point_scan;
// 显示路径
current_time_ = ros::Time::now();
pubMsg(odom_pos_cal_, path_odom_, odom_path_pub_);
pubMsg(scan_pos_cal_, path_scan_, scan_path_pub_);
//构造超定方程组
odom_calib.addData(d_point_odom, d_point_scan);
dataCnt++;
if (odom_calib.isFull())
ROS_INFO_STREAM("Get enough data:" << dataCnt);
else
ROS_INFO_STREAM("Data Cnt:" << dataCnt);
}
void OdomCalibNode::odomCallBack(const nav_msgs::Odometry::ConstPtr& msg)
{
transformStamped.header.stamp = msg->header.stamp;
transformStamped.header.frame_id = "odom";
transformStamped.child_frame_id = "base_link";
transformStamped.transform.translation.x = msg->pose.pose.position.x;
transformStamped.transform.translation.y = msg->pose.pose.position.y;
transformStamped.transform.translation.z = msg->pose.pose.position.z;
transformStamped.transform.rotation = msg->pose.pose.orientation;
broadcaster.sendTransform(transformStamped);
}
void OdomCalibNode::pubMsg(Eigen::Vector3d& pose, nav_msgs::Path& path, ros::Publisher& mcu_path_pub)
{
// current_time_ = ros::Time::now();
geometry_msgs::PoseStamped cur_pose_stamped;
cur_pose_stamped.header.stamp = current_time_;
cur_pose_stamped.header.frame_id = "odom";
cur_pose_stamped.pose.position.x = pose(0);
cur_pose_stamped.pose.position.y = pose(1);
cur_pose_stamped.pose.position.z = 0.0;
tf2::Quaternion goal_quat;
goal_quat.setRPY(0, 0, pose(2));
cur_pose_stamped.pose.orientation.x = goal_quat.x();
cur_pose_stamped.pose.orientation.y = goal_quat.y();
cur_pose_stamped.pose.orientation.z = goal_quat.z();
cur_pose_stamped.pose.orientation.w = goal_quat.w();
path.poses.push_back(cur_pose_stamped);
mcu_path_pub.publish(path);
}
void OdomCalibNode::publishPathEigen(std::vector<Eigen::Vector3d>& path_eigen, ros::Publisher& path_pub)
{
nav_msgs::Path visual_path;
current_time_ = ros::Time::now();
visual_path.header.stamp = current_time_;
visual_path.header.frame_id = "odom";
geometry_msgs::PoseStamped tmpPose;
tmpPose.header.stamp = current_time_;
tmpPose.header.frame_id = "odom";
for (int i = 0; i < path_eigen.size(); i++)
{
Eigen::Vector3d poseEigen = path_eigen[i];
tmpPose.pose.position.x = poseEigen(0);
tmpPose.pose.position.y = poseEigen(1);
tf2::Quaternion goal_quat;
goal_quat.setRPY(0, 0, poseEigen(2));
tmpPose.pose.orientation.x = goal_quat.x();
tmpPose.pose.orientation.y = goal_quat.y();
tmpPose.pose.orientation.z = goal_quat.z();
tmpPose.pose.orientation.w = goal_quat.w();
visual_path.poses.push_back(tmpPose);
}
path_pub.publish(visual_path);
}
bool OdomCalibNode::getOdomPose(Eigen::Vector3d& pose, const ros::Time& t)
{
geometry_msgs::PoseStamped base_pose;
base_pose.header.frame_id = base_frame_;
base_pose.header.stamp = t;
base_pose.pose.position.x = 0.0;
base_pose.pose.position.y = 0.0;
base_pose.pose.position.z = 0.0;
base_pose.pose.orientation.w = 1;
base_pose.pose.orientation.x = 0;
base_pose.pose.orientation.y = 0;
base_pose.pose.orientation.z = 0;
// 获得t时间点下，base_link坐标系相对odom的坐标变换
try
{
tf_buffer_.transform(base_pose, base_pose, "odom");
// 变换数据类型
tf2::Stamped<tf2::Transform> odom_pose;
tf2::fromMsg(base_pose, odom_pose);
pose << odom_pose.getOrigin().x(), odom_pose.getOrigin().y(), tf2::getYaw(odom_pose.getRotation());
}
catch (tf2::TransformException& ex)
{
ROS_WARN("can't transform: %s", ex.what());
return false;
}
return true;
}
Eigen::Vector3d OdomCalibNode::calDeltaDistence(Eigen::Vector3d last_pose, Eigen::Vector3d now_pose)
{
double theta, x, y;
Eigen::Vector3d d_pos;
//前一次的位姿
Eigen::Isometry3d T_prev = Eigen::Isometry3d::Identity();
T_prev.rotate(eul2rotm(last_pose(2), 0, 0));
T_prev.pretranslate(Eigen::Vector3d(last_pose(0), last_pose(1), 0));
// 当前位姿
Eigen::Isometry3d T_now = Eigen::Isometry3d::Identity();
T_now.rotate(eul2rotm(now_pose(2), 0, 0));
T_now.pretranslate(Eigen::Vector3d(now_pose(0), now_pose(1), 0));
// 相对位姿
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T = T_prev.inverse() * T_now;
d_pos(0) = T.translation()(0);
d_pos(1) = T.translation()(1);
d_pos(2) = atan2(T.matrix()(1, 0), T.matrix()(0, 0));
return d_pos;
}
Eigen::Matrix3d OdomCalibNode::eul2rotm(double yaw, double pitch, double roll)
{
Eigen::AngleAxisd yawAngle(yaw, Eigen::Vector3d::UnitZ());
Eigen::AngleAxisd pitchAngle(pitch, Eigen::Vector3d::UnitY());
Eigen::AngleAxisd rollAngle(roll, Eigen::Vector3d::UnitX());
Eigen::Quaterniond q = yawAngle * pitchAngle * rollAngle;
Eigen::Matrix3d R = q.matrix();
return R;
}
double ds = linear_speed_ * dt;
double d_theta = angular_speed_ * dt;
// 割线模型
Eigen::Vector3d odom_inc{ ds, 0, d_theta };
Eigen::Vector3d correct_inc = correct_matrix_ * odom_inc;
transform_matrix_ << cos(theta_ + correct_inc[2] / 2), -sin(theta_ + correct_inc[2] / 2), 0, sin(theta_ + correct_inc[2] / 2),
cos(theta_ + correct_inc[2] / 2), 0, 0, 0, 1;
Eigen::Vector3d calib_pos = transform_matrix_ * correct_inc;
//累积
position_x_ += calib_pos[0];
position_y_ += calib_pos[1];
theta_ += calib_pos[2];
评论（0）
您还未登录，请登录后发表或查看评论