基于里程计修复激光雷达数据

[imuncle]

因为商用激光雷达频率多在10~15Hz之间，当机器人快速运动时可能会出现激光雷达数据变形的情况，这会在一定程度上影响机器人的定位和路径规划。因此对激光雷达数据的纠正很关键。

我参考了激光雷达移动状态下的数据矫正这篇文章，其中基本上是一篇论文的中文翻译，论文也在这篇文章中给出了。

我根据论文中的内容使用ROS和OpenCV搭建了一个仿真环境，在这个仿真程序中，雷达数据为10Hz，里程计数据为100Hz，使用键盘控制机器人的前后左右旋转。雷达一圈采集360个点。纠正的大体思路如下：

因为激光雷达数据频率较低，但每一帧中有360个数据，所以激光雷达的每一个数据时间间隔是小于里程计的。首先我们需要构建一个队列存储最近20帧里程计数据，然后在激光雷达数据回调函数中，针对每一个数据的时间戳找到对应的里程计近似数据，最后将这些里程计位姿对齐到最后一帧里程计数据（因为原始数据中只有最后一个雷达数据是准确的。）

程序有四个文件，直接上代码，关键部分有注释：

• keyboard.h

// 作者：谢胜
// 功能：读取键盘输入（无需回车），可用于模拟程序中机器人控制的输入信号

#ifndef KEYBOARD_H
#define KEYBOARD_H

#include <termios.h>
#include <signal.h>
#include <math.h>
#include <stdio.h>
#include <stdlib.h>
#include <sys/poll.h>
#include <std_msgs/UInt8.h>

class Keyboard
{
    public:
    Keyboard()
    {
        kfd = 0;
        tcgetattr(kfd, &cooked);
        memcpy(&raw, &cooked, sizeof(struct termios));
        raw.c_lflag &=~ (ICANON | ECHO);
        raw.c_cc[VEOL] = 1;
        raw.c_cc[VEOF] = 2;
        tcsetattr(kfd, TCSANOW, &raw);
        ufd.fd = kfd;
        ufd.events = POLLIN;
    }
    ~Keyboard()
    {
        tcsetattr(kfd, TCSANOW, &cooked);
    }
    uint8_t Read()
    {
        int num;
        uint8_t c;
        if((num = poll(&ufd, 1, 250)) < 0)
        {
            perror("poll():");
            return 0;
        }
        else if(num > 0)
        {
            if(read(kfd, &c, 1) < 0)
            {
                perror("read():");
                return 0;
            }
        }
        else
        {
            return 0;
        }
        return c;
    }
    private:
    struct pollfd ufd;
    struct termios raw;
    struct termios cooked;
    int kfd;
};

#endif

• car.h

#ifndef CAR_H
#define CAR_H

#include "keyboard.h"
#include <ros/ros.h>
#include <thread>
#include "common_msgs/CarPose.h"

// 存储机器人位置的类（其实没怎么用它，基本多余）
class CarPosition
{
    public:
    CarPosition(){}
    CarPosition(float x, float y, float orientation)
    {
        x_ = x;
        y_ = y;
        orientation_ = orientation;
    }
    float x_;
    float y_;
    float orientation_;
};

// 机器人控制类，实现了键盘控制机器人移动，并以100帧的频率发布里程计数据
class Car
{
    public:
    Car(float x, float y, float orientation, int width, int height)
    {
        car_pose_pub_ = nh.advertise<common_msgs::CarPose>("car_pose", 1);
        position_.x_ = x;
        position_.y_ = y;
        position_.orientation_ = orientation;
        width_ = width;
        height_ = height;
        control_thread_ = std::thread(&Car::CanControl, this);
        keyboard_thread_ = std::thread(&Car::KeyboardRead, this);
    }
    ~Car()
    {
        if(control_thread_.joinable())
        {
            control_thread_.join();
        }
        if(keyboard_thread_.joinable())
        {
            keyboard_thread_.join();
        }
    }
    void RunForwardBack(float velocity)
    {
        position_.x_ += velocity * std::cos(position_.orientation_);
        position_.y_ += velocity * std::sin(position_.orientation_);
    }
    void RunLeftRight(float velocity)
    {
        position_.x_ -= velocity * std::sin(position_.orientation_);
        position_.y_ += velocity * std::cos(position_.orientation_);
    }
    void Rotate(float angular)
    {
        position_.orientation_ += angular;
        if(position_.orientation_ < 0)
        {
            position_.orientation_ += 2*M_PI;
        }
        if(position_.orientation_ > 2*M_PI)
        {
            position_.orientation_ -= 2*M_PI;
        }
    }
    void CanControl()
    {
        ros::Rate rate(100);
        while(ros::ok())
        {
            car_pose_.stamp = ros::Time::now();
            car_pose_.x = GetX();
            car_pose_.y = GetY();
            car_pose_.orientation = GetOrientation();
            car_pose_pub_.publish(car_pose_);
            rate.sleep();
        }
    }
    void KeyboardRead()
    {
        while(ros::ok())
        {
            int key = keyboard.Read();
            if(key == 'w')
            {
                RunForwardBack(0.1);
            }
            else if(key == 's')
            {
                RunForwardBack(-0.1);
            }
            else if(key == 'a')
            {
                RunLeftRight(0.1);
            }
            else if(key == 'd')
            {
                RunLeftRight(-0.1);
            }
            else if(key == 'q')
            {
                Rotate(0.05);
            }
            else if(key == 'e')
            {
                Rotate(-0.05);
            }
        }
    }
    CarPosition GetPosition()
    {
        return position_;
    }
    float GetX()
    {
        return position_.x_;
    }
    float GetY()
    {
        return position_.y_;
    }
    float GetOrientation()
    {
        return position_.orientation_;
    }
    private:
    CarPosition position_;
    int width_;
    int height_;
    Keyboard keyboard;
    std::thread control_thread_;
    std::thread keyboard_thread_;
    ros::NodeHandle nh;
    ros::Publisher car_pose_pub_;
    common_msgs::CarPose car_pose_;
};

#endif

• laser_pub.cpp

#include <ros/ros.h>
#include "common_msgs/CarPose.h"
#include "sensor_msgs/LaserScan.h"
#include "car.h"
#include <thread>

// 模拟雷达数据发布类
class LaserPub
{
    public:
    LaserPub():
    car(1, 1, 0, 0.6, 0.4),
    angle_min(0),angle_max(6.28),angle_increment(0.017453),time_increment(0.00028),scan_time(0.1),range_min(0.1),range_max(8),
    scan_index(0),scan_seq(1),
    map_width(8),map_height(5)
    {
        laser_pub = nh.advertise<sensor_msgs::LaserScan>("laser_scan", 1);
        laser_scan_data_.ranges.resize(360);
        scan_thread_ = std::thread(&LaserPub::Scan, this);
    }
    ~LaserPub()
    {
        if(scan_thread_.joinable())
        {
            scan_thread_.join();
        }
    }
    // 单次扫描，传入参数是本次扫描的序号（0～359）
    float ScanOnce(int index)
    {
        CarPosition car_pose = car.GetPosition();
        float angle = car_pose.orientation_ - angle_increment * index;
        if(angle < 0)
            angle += 2*M_PI;
        if(angle <= M_PI)
        {
            float x = (map_height - car_pose.y_) / std::tan(angle) + car_pose.x_;
            if(x >= 0 && x <= map_width)
            {
                return std::sqrt((x - car_pose.x_) * (x - car_pose.x_) + (map_height - car_pose.y_) * (map_height - car_pose.y_));
            }
            else if(x < 0)
            {
                float y = (0 - car_pose.x_) * std::tan(angle) + car_pose.y_;
                return std::sqrt((0 - car_pose.x_) * (0 - car_pose.x_) + (y - car_pose.y_) * (y - car_pose.y_));
            }
            else if(x > map_width)
            {
                float y = (map_width - car_pose.x_) * std::tan(angle) + car_pose.y_;
                return std::sqrt((map_width - car_pose.x_) * (map_width - car_pose.x_) + (y - car_pose.y_) * (y - car_pose.y_));
            }
        }
        else
        {
            float x = (0 - car_pose.y_) / std::tan(angle) + car_pose.x_;
            if(x >= 0 && x <= map_width)
            {
                return std::sqrt((x - car_pose.x_) * (x - car_pose.x_) + (0 - car_pose.y_) * (0 - car_pose.y_));
            }
            else if(x < 0)
            {
                float y = (0 - car_pose.x_) * std::tan(angle) + car_pose.y_;
                return std::sqrt((0 - car_pose.x_) * (0 - car_pose.x_) + (y - car_pose.y_) * (y - car_pose.y_));
            }
            else if(x > map_width)
            {
                float y = (map_width - car_pose.x_) * std::tan(angle) + car_pose.y_;
                return std::sqrt((map_width - car_pose.x_) * (map_width - car_pose.x_) + (y - car_pose.y_) * (y - car_pose.y_));
            }
        }
    }
    // 模拟雷达数据发布
    void LaserDataPublish()
    {
        laser_scan_data_.angle_min = angle_min;
        laser_scan_data_.angle_max = angle_max;
        laser_scan_data_.angle_increment = angle_increment;
        laser_scan_data_.time_increment = time_increment;
        laser_scan_data_.scan_time = scan_time;
        laser_scan_data_.range_min = range_min;
        laser_scan_data_.range_max = range_max;
        laser_pub.publish(laser_scan_data_);
    }
    // 雷达扫描线程，频率10Hz
    void Scan()
    {
        ros::Rate rate(3600);
        while(ros::ok())
        {
            if(scan_index == 0)
            {
                laser_scan_data_.header.seq = scan_seq;
                laser_scan_data_.header.stamp = ros::Time::now();
                laser_scan_data_.header.frame_id = "map";
            }
            laser_scan_data_.ranges[scan_index] = ScanOnce(scan_index);
            scan_index++;
            if(scan_index == 360)
            {
                LaserDataPublish();
                scan_index = 0;
            }
            rate.sleep();
        }
    }
    int scan_index;
    int scan_seq;
    private:
    ros::NodeHandle nh;
    ros::Subscriber car_pose_sub;
    ros::Publisher laser_pub;
    sensor_msgs::LaserScan laser_scan_data_;
    Car car;
    std::thread scan_thread_;

    double angle_min;
    double angle_max;
    double angle_increment;
    double time_increment;
    double scan_time;
    double range_min;
    double range_max;

    int map_width;
    int map_height;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "laser_pub_node");
    LaserPub laser_pub;
    ros::AsyncSpinner async_spinner(3);
    async_spinner.start();
    ros::waitForShutdown();
    return 0;
}

• laser_correct.cpp

#include <ros/ros.h>
#include <opencv2/opencv.hpp>
#include "common_msgs/CarPose.h"
#include "sensor_msgs/LaserScan.h"

#include <Eigen/Eigen>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <Eigen/Eigenvalues>

// 使用opencv将机器人位姿显示出来
class CarDraw
{
    public:
    CarDraw()
    {
        width_ = 60;
        height_ = 40;
        vertex_.resize(4);
    }
    void Draw(cv::Mat &img)
    {
        cv::line(img, vertex_[0], vertex_[1], cv::Scalar(0,0,255), 2);
        cv::line(img, vertex_[1], vertex_[2], cv::Scalar(0,0,255), 2);
        cv::line(img, vertex_[2], vertex_[3], cv::Scalar(0,0,255), 2);
        cv::line(img, vertex_[3], vertex_[0], cv::Scalar(0,0,255), 2);
    }
    // 手写了坐标变换，可用Eigen库替换
    void Rotate(float angle)
    {
        float cos_theta = std::cos(angle);
        float sin_theta = std::sin(angle);
        cv::Point2f vertex;
        vertex.x = width_/2;
        vertex.y = height_/2;
        vertex_[0].x = cos_theta * vertex.x - sin_theta * vertex.y;
        vertex_[0].y = sin_theta * vertex.x + cos_theta * vertex.y;
        vertex_[0].x += center_.x;
        vertex_[0].y += center_.y;
        vertex.x = -width_/2;
        vertex.y = height_/2;
        vertex_[1].x = cos_theta * vertex.x - sin_theta * vertex.y;
        vertex_[1].y = sin_theta * vertex.x + cos_theta * vertex.y;
        vertex_[1].x += center_.x;
        vertex_[1].y += center_.y;
        vertex.x = -width_/2;
        vertex.y = -height_/2;
        vertex_[2].x = cos_theta * vertex.x - sin_theta * vertex.y;
        vertex_[2].y = sin_theta * vertex.x + cos_theta * vertex.y;
        vertex_[2].x += center_.x;
        vertex_[2].y += center_.y;
        vertex.x = width_/2;
        vertex.y = -height_/2;
        vertex_[3].x = cos_theta * vertex.x - sin_theta * vertex.y;
        vertex_[3].y = sin_theta * vertex.x + cos_theta * vertex.y;
        vertex_[3].x += center_.x;
        vertex_[3].y += center_.y;

        vertex_[0].y = 500 - vertex_[0].y;
        vertex_[1].y = 500 - vertex_[1].y;
        vertex_[2].y = 500 - vertex_[2].y;
        vertex_[3].y = 500 - vertex_[3].y;
    }
    // 设置机器人中心位置
    void SetCenter(cv::Point2f center)
    {
        center_ = center;
    }
    private:
    std::vector<cv::Point2f> vertex_;
    cv::Point2f center_;
    int width_;
    int height_;
};

// 雷达数据纠正类
class LaserCorrect
{
    public:
    LaserCorrect()
    {
        car_pose_sub_ = nh.subscribe("car_pose", 1, &LaserCorrect::CarPoseCallback, this);
        laser_scan_sub_ = nh.subscribe("laser_scan", 1, &LaserCorrect::LaserScanCallback, this);
    }
    // 里程计数据回调函数，更新机器人位置，并更新里程计缓存队列
    void CarPoseCallback(common_msgs::CarPose car_pose)
    {
        car_pose_[0] = car_pose.x;
        car_pose_[1] = car_pose.y;
        car_pose_[2] = car_pose.orientation;
        car.SetCenter(cv::Point2f(car_pose.x*100, car_pose.y*100));
        car.Rotate(car_pose.orientation);
        PosePushBuffer(car_pose);
    }
    // 雷达数据回调函数，在其中纠正雷达数据
    void LaserScanCallback(sensor_msgs::LaserScan laser_scan)
    {
        laser_scan_data_ = laser_scan;
        cv::Mat map(500, 800, CV_8UC3, cv::Scalar(255,255,255));
        for(size_t i = 0; i < laser_scan_data_.ranges.size(); i++)
        {
            float angle = GetRobotOrientation() - laser_scan_data_.angle_min - i * laser_scan_data_.angle_increment;
            if(angle < 0)
                angle += 2*M_PI;
            float sin_theta = std::sin(angle);
            float cos_theta = std::cos(angle);
            float x = laser_scan_data_.ranges[i] * cos_theta + GetRobotX();
            float y = laser_scan_data_.ranges[i] * sin_theta + GetRobotY();
            cv::circle(map, cv::Point(x*100, 500-y*100), 3, cv::Scalar(0,0,0),-1);
        }
        car.Draw(map);
        if(car_pose_buffer_.size() >= 20)
            LaserDataCorrect(laser_scan_data_, map);
        cv::imshow("map", map);
        cv::waitKey(1);
    }
    float GetRobotX()
    {
        return car_pose_[0];
    }
    float GetRobotY()
    {
        return car_pose_[1];
    }
    float GetRobotOrientation()
    {
        return car_pose_[2];
    }
    void PosePushBuffer(common_msgs::CarPose pose)
    {
        if(car_pose_buffer_.size() == 20)
        {
            car_pose_buffer_.erase(car_pose_buffer_.begin());
        }
        car_pose_buffer_.push_back(pose);
    }
    // 雷达数据纠正函数
    void LaserDataCorrect(const sensor_msgs::LaserScan& laser_scan_msg, cv::Mat& img)
    {
        double laser_start_time_stamp = GetRosTime(laser_scan_msg.header.stamp);
        double laser_last_time_stamp = laser_start_time_stamp + laser_scan_msg.scan_time;

        Eigen::Vector3d final_odom_pose;
        // 获取雷达最后一帧数据对应的里程计位置
        FindBasePoseWithLaserPoint(laser_last_time_stamp, final_odom_pose);
        
        for(int i = 0; i < laser_scan_msg.ranges.size(); ++i)
        {
            double theta_tmp = laser_scan_msg.angle_min + i * laser_scan_msg.angle_increment;
            double time_tmp = laser_start_time_stamp + i * laser_scan_msg.time_increment;

            Eigen::Vector3d pose_tmp, pose_tmp_aligned;
            Eigen::Vector3d base_pose_tmp;

            pose_tmp(0) = laser_scan_msg.ranges[i] * std::cos(-theta_tmp);
            pose_tmp(1) = laser_scan_msg.ranges[i] * std::sin(-theta_tmp);
            pose_tmp(2) = 1;

            // 找到当前雷达数据对应的里程计位置
            FindBasePoseWithLaserPoint(time_tmp, base_pose_tmp);
            // 根据里程计数据对雷达数据进行纠正
            PointCoordinateAlign(pose_tmp, base_pose_tmp, pose_tmp_aligned, final_odom_pose);

            cv::circle(img, cv::Point(pose_tmp_aligned(0)*100, 500-pose_tmp_aligned(1)*100), 3, cv::Scalar(255,0,0),-1);
        }
    }

    double GetRosTime(ros::Time time)
    {
        return (double)time.nsec/(1e9) + time.sec;
    }
    // 要注意里程计的角度是0～2*M_PI，注意过零处理和防止超出角度范围
    void FindBasePoseWithLaserPoint(double time_stamp, Eigen::Vector3d& base_odom_pose)
    {
        int  i = 0;
        for(i = 0; i < car_pose_buffer_.size() - 1; ++i)
        {
            if(time_stamp >= GetRosTime(car_pose_buffer_[i].stamp) && time_stamp < GetRosTime(car_pose_buffer_[i + 1].stamp))
                break;
        }
        if(i == car_pose_buffer_.size() - 1) 
        {
            Eigen::Vector3d delta_pose;
            delta_pose(0) = car_pose_buffer_[i].x - car_pose_buffer_[i-1].x;
            delta_pose(1) = car_pose_buffer_[i].y - car_pose_buffer_[i-1].y;
            delta_pose(2) = car_pose_buffer_[i].orientation - car_pose_buffer_[i-1].orientation;
            if(delta_pose(2) > M_PI)
            {
                delta_pose(2) -= 2*M_PI;
            }
            if(delta_pose(2) < -M_PI)
            {
                delta_pose(2) += 2*M_PI;
            }
            double delta_time = GetRosTime(car_pose_buffer_[i].stamp) - GetRosTime(car_pose_buffer_[i-1].stamp);
            double incre_time = time_stamp - GetRosTime(car_pose_buffer_[i].stamp);

            base_odom_pose(0) = car_pose_buffer_[i].x + delta_pose(0) * incre_time/delta_time;
            base_odom_pose(1) = car_pose_buffer_[i].y + delta_pose(1) * incre_time/delta_time;
            base_odom_pose(2) = car_pose_buffer_[i].orientation + delta_pose(2) * incre_time/delta_time;
        }
        else
        {
            Eigen::Vector3d delta_pose;
            delta_pose(0) = car_pose_buffer_[i+1].x - car_pose_buffer_[i].x;
            delta_pose(1) = car_pose_buffer_[i+1].y - car_pose_buffer_[i].y;
            delta_pose(2) = car_pose_buffer_[i+1].orientation - car_pose_buffer_[i].orientation;
            if(delta_pose(2) > M_PI)
            {
                delta_pose(2) -= 2*M_PI;
            }
            if(delta_pose(2) < -M_PI)
            {
                delta_pose(2) += 2*M_PI;
            }
            double delta_time = GetRosTime(car_pose_buffer_[i+1].stamp) - GetRosTime(car_pose_buffer_[i].stamp);
            double incre_time = time_stamp - GetRosTime(car_pose_buffer_[i].stamp);

            base_odom_pose(0) = car_pose_buffer_[i].x + delta_pose(0) * incre_time/delta_time;
            base_odom_pose(1) = car_pose_buffer_[i].y + delta_pose(1) * incre_time/delta_time;
            base_odom_pose(2) = car_pose_buffer_[i].orientation + delta_pose(2) * incre_time/delta_time;
        }
        if(base_odom_pose(2) > 2*M_PI)
            base_odom_pose(2) -= 2*M_PI;
        if(base_odom_pose(2) < 0)
            base_odom_pose(2) += 2*M_PI;
    }
    // 将雷达数据对齐至雷达最后一帧
    void PointCoordinateAlign(Eigen::Vector3d& point_pose, 
                              Eigen::Vector3d& base_pose,
                              Eigen::Vector3d& point_pose_aligned,
                              Eigen::Vector3d& base_pose_aligned)
    {
        Eigen::Matrix3d tf_pose_to_pose_aligned;
        tf_pose_to_pose_aligned.setZero();
        Eigen::Matrix2d tf_base_to_pose;
        tf_base_to_pose.setZero();
        double delta_theta = base_pose_aligned(2);

        tf_base_to_pose(0,0) = std::cos(-delta_theta);
        tf_base_to_pose(0,1) = -std::sin(-delta_theta);
        tf_base_to_pose(1,0) = std::sin(-delta_theta);
        tf_base_to_pose(1,1) = std::cos(-delta_theta);

        Eigen::Vector2d delta_pose;
        delta_pose(0) = base_pose(0) - base_pose_aligned(0);
        delta_pose(1) = base_pose(1) - base_pose_aligned(1);

        // 将里程计位移变换到雷达坐标系
        delta_pose = tf_base_to_pose * delta_pose;

        delta_theta = base_pose(2) - base_pose_aligned(2);
        if(delta_theta > M_PI)
        {
            delta_theta -= 2*M_PI;
        }
        if(delta_theta < -M_PI)
        {
            delta_theta += 2*M_PI;
        }

        // 两帧雷达数据之间雷达坐标系的变换矩阵
        tf_pose_to_pose_aligned(0,0) = std::cos(delta_theta);
        tf_pose_to_pose_aligned(0,1) = -std::sin(delta_theta);
        tf_pose_to_pose_aligned(1,0) = std::sin(delta_theta);
        tf_pose_to_pose_aligned(1,1) = std::cos(delta_theta);
        tf_pose_to_pose_aligned(0,2) = delta_pose(0);
        tf_pose_to_pose_aligned(1,2) = delta_pose(1);
        tf_pose_to_pose_aligned(2,2) = 1;
        
        Eigen::Matrix3d tf_pose_to_base;
        tf_pose_to_base.setZero();
        delta_theta = base_pose_aligned(2);

        tf_pose_to_base(0,0) = std::cos(delta_theta);
        tf_pose_to_base(0,1) = -std::sin(delta_theta);
        tf_pose_to_base(1,0) = std::sin(delta_theta);
        tf_pose_to_base(1,1) = std::cos(delta_theta);
        tf_pose_to_base(0,2) = base_pose_aligned(0);
        tf_pose_to_base(1,2) = base_pose_aligned(1);
        tf_pose_to_base(2,2) = 1;

        // 求得纠正后的在世界坐标系下的雷达数据
        point_pose_aligned = tf_pose_to_base * tf_pose_to_pose_aligned * point_pose;
    }
    private:
    ros::NodeHandle nh;
    ros::Subscriber car_pose_sub_;
    ros::Subscriber laser_scan_sub_;
    float car_pose_[3];
    std::vector<common_msgs::CarPose> car_pose_buffer_;
    Eigen::Matrix3d tf_laser_to_base_;
    CarDraw car;
    sensor_msgs::LaserScan laser_scan_data_;
};

int main(int argc, char** argv)
{
    ros::init(argc, argv, "laser_correct_node");
    LaserCorrect laser_correct;
    ros::spin();
    return 0;
}

程序中使用到了一个自定义的msg：

• CarPose.msg

time stamp
float64 x
float64 y
float64 orientation

---

纠正效果：

其中黑色是原始数据，蓝色是纠正后的数据，可以看到纠正之后基本与场地实际边界重合，达到纠正目的。