RoboSense雷达驱动时间戳分析

欢迎大家来到IT世界,在知识的湖畔探索吧!

这是速腾最早的产品RS-16，目前的主流产品应该是M平台的固态激光雷达以及R平台的多线机械雷达，不过点云的解析的基本原理都是类似的。

雷达点云的时间戳对多传感器同步、畸变矫正等有很大的帮助。下文以速腾RS-16为例，分析其雷达驱动(
https://github.com/RoboSense-LiDAR/rslidar_sdk)中关于点云时间戳的细节。

1. pps时间同步

RS-16支持PPS+GPRMC的时间同步方式，

欢迎大家来到IT世界,在知识的湖畔探索吧!

图1 PPS时间同步

当利用PPS信号同步时间后，会在MSOP Package中的Header中将时间戳同步，

图2 MSOP数据结构

帧头 Header 共 42byte，21~30byte存储时间戳，分辨率为1us。

图3 时间戳数据结构

2. 雷达驱动中的时间戳

如图2所示，点云是原始数据以MSOP协议的格式发送，一个Package包含了一个Header，其中有一个时间戳，还有12个数据Block(BLOCKS_PER_PKT)，每个Block中有个水平旋转角度和32组channel的数据(CHANNELS_PER_BLOCK)，对于RS-16来说，刚好是两组激光束(16×2)的数据(LASER_NUM)。

RS-16的一些参数，

template <typename T_Point> inline const LidarConstantParameter DecoderFactory<T_Point>::getRS16ConstantParam() { LidarConstantParameter ret_param; ret_param.MSOP_ID = 0xA050A55A0A05AA55; ret_param.DIFOP_ID = 0xA00FFA5; ret_param.BLOCK_ID = 0xEEFF; ret_param.PKT_RATE = 750; ret_param.BLOCKS_PER_PKT = 12; ret_param.CHANNELS_PER_BLOCK = 32; ret_param.LASER_NUM = 16; ret_param.DSR_TOFFSET = 2.8; ret_param.FIRING_FREQUENCY = 0.009; ret_param.RX = 0.03825; ret_param.RY = -0.01088; ret_param.RZ = 0; return ret_param; }

欢迎大家来到IT世界,在知识的湖畔探索吧!

在驱动中支持两种点的类型，XYZI和XYZIRT，除了反射率还有RING和时间戳的信息，本文默认使用XYZIRT格式。

欢迎大家来到IT世界,在知识的湖畔探索吧!#======================================= # Custom Point Type (XYZI, XYZIRT) #======================================= set(POINT_TYPE XYZIRT)

PKT_RATE=750表示每秒发送750个Package，对于10HZ的输出，一帧完整点云所需要的时间为0.1s，在0.1秒内发出了0.1*750=7575个Package，同时每个Package中包含12个Block，则一帧完整点云共75*12=900个Block。

2.1 get_point_time_func_函数

decoder_base.hpp—–>

std::function<double(const uint8_t*)> get_point_time_func_; ...... /* Point time function*/ if (RS_HAS_MEMBER(T_Point, timestamp)) ///< return the timestamp of the first block in one packet { if (this->param_.use_lidar_clock) { get_point_time_func_ = [this](const uint8_t* pkt) { return getLidarTime(pkt); }; } else { get_point_time_func_ = [this](const uint8_t* pkt) { double ret_time = getTime() - (this->lidar_const_param_.BLOCKS_PER_PKT - 1) * this->time_duration_between_blocks_; return ret_time; }; } } else { get_point_time_func_ = [this](const uint8_t* pkt) { return 0; }; }

在驱动的配置文件中，每个雷达的driver都需要配置use_lidar_clock: false，如果设置为true，则表示使用lidar的时钟，经过PPS+GPRMC同步后的信号该选项要设置为true，否则会使用驱动运行所在的控制器中的系统时间。

假设使用PPS同步后的时间，

decoder_RS16.hpp—–>

欢迎大家来到IT世界,在知识的湖畔探索吧!template <typename T_Point> inline double DecoderRS16<T_Point>::getLidarTime(const uint8_t* pkt) { return this->template calculateTimeYMD<RS16MsopPkt>(pkt); } 

decoder_base.hpp—–>

template <typename T_Point> template <typename T_Msop> inline double DecoderBase<T_Point>::calculateTimeYMD(const uint8_t* pkt) { #ifdef _MSC_VER long timezone = 0; _get_timezone(&timezone); #endif const T_Msop* mpkt_ptr = reinterpret_cast<const T_Msop*>(pkt); std::tm stm; memset(&stm, 0, sizeof(stm)); stm.tm_year = mpkt_ptr->header.timestamp.year + 100; stm.tm_mon = mpkt_ptr->header.timestamp.month - 1; stm.tm_mday = mpkt_ptr->header.timestamp.day; stm.tm_hour = mpkt_ptr->header.timestamp.hour; stm.tm_min = mpkt_ptr->header.timestamp.minute; stm.tm_sec = mpkt_ptr->header.timestamp.second; return std::mktime(&stm) + static_cast<double>(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.ms)) / 1000.0 + static_cast<double>(RS_SWAP_SHORT(mpkt_ptr->header.timestamp.us)) / .0 - static_cast<double>(timezone); } 

上述代码直接取MSOP中的header中的时间戳，安装数据结构填充，并转换。

假设使用控制器系统时间，

获取系统时间(system_clock)，这里使用的是system_clock感觉有些问题，控制器联网时间同步后时间会跳变吧？

utility/time.h—–>

欢迎大家来到IT世界,在知识的湖畔探索吧!inline double getTime(void) { const auto t = std::chrono::system_clock::now(); const auto t_sec = std::chrono::duration_cast<std::chrono::duration<double>>(t.time_since_epoch()); return (double)t_sec.count(); } 

获取系统当前的时间，同时减去当前Package中Block所用的时间，相当于估计一个该Package初始的发射时间。

get_point_time_func_ = [this](const uint8_t* pkt) { double ret_time = getTime() - (this->lidar_const_param_.BLOCKS_PER_PKT - 1) * this->time_duration_between_blocks_; return ret_time; 

2.2 点的时间戳赋值

time_duration_between_blocks_表示两个block之间的时间，

欢迎大家来到IT世界,在知识的湖畔探索吧! this->time_duration_between_blocks_ = (60 / static_cast<float>(this->rpm_)) / ((this->lidar_const_param_.PKT_RATE * 60 / this->rpm_) * this->lidar_const_param_.BLOCKS_PER_PKT); int fov_start_angle = RS_SWAP_SHORT(dpkt_ptr->fov.start_angle); int fov_end_angle = RS_SWAP_SHORT(dpkt_ptr->fov.end_angle); int fov_range = (fov_start_angle < fov_end_angle) ? (fov_end_angle - fov_start_angle) : (RS_ONE_ROUND - fov_start_angle + fov_end_angle); int blocks_per_round = (this->lidar_const_param_.PKT_RATE / (this->rpm_ / 60)) * this->lidar_const_param_.BLOCKS_PER_PKT; this->fov_time_jump_diff_ = this->time_duration_between_blocks_ * (fov_range / (RS_ONE_ROUND / static_cast<float>(blocks_per_round))); 

decoder_RS16.hpp—–>

template <typename T_Point> inline RSDecoderResult DecoderRS16<T_Point>::decodeMsopPkt(const uint8_t* pkt, std::vector<T_Point>& vec, int& height, int& azimuth) { //1.获取当前Package的时间戳 double block_timestamp = this->get_point_time_func_(pkt); ...... for (size_t blk_idx = 0; blk_idx < this->lidar_const_param_.BLOCKS_PER_PKT; blk_idx++) { if (blk_idx == 0) { azi_diff = static_cast<float>((RS_ONE_ROUND + RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx + 1].azimuth) - cur_azi) % RS_ONE_ROUND); } else { azi_diff = static_cast<float>((RS_ONE_ROUND + cur_azi - RS_SWAP_SHORT(mpkt_ptr->blocks[blk_idx - 1].azimuth)) % RS_ONE_ROUND); // 2.遍历该Package中的所有block,每个block中的时间都会根据基准时间戳上累加time_duration_between_blocks_ block_timestamp = (azi_diff > 100) ? (block_timestamp + this->fov_time_jump_diff_) : (block_timestamp + this->time_duration_between_blocks_); } ...... for (size_t channel_idx = 0; channel_idx < this->lidar_const_param_.CHANNELS_PER_BLOCK; channel_idx++) { ...... // 3.遍历每个block中的channel，即32个channel的数据，对应RS-16两组数据，设置反射率，RING，点的时间戳 T_Point point; setX(point, x); setY(point, y); setZ(point, z); setIntensity(point, intensity); setRing(point, this->beam_ring_table_[channel_idx % 16]); if (this->echo_mode_ != ECHO_DUAL && channel_idx > 15) { setTimestamp(point, block_timestamp + this->time_duration_between_blocks_ / 2); } else { setTimestamp(point, block_timestamp); } vec.emplace_back(std::move(point)); } } } 

上述解析MSOP报文的主要流程：

1. 获取当前Package的时间戳；
2. 遍历该Package中的所有block,每个block中的时间都会根据基准时间戳上累加time_duration_between_blocks_；
3. 遍历每个block中的channel，即32个channel的数据，对应RS-16两组数据，设置反射率，RING，点的时间戳。

从目前的驱动代码中可以看到，该Block中所有点的时间戳是相同的，还没有细化到每个点的时间戳。因为雷达转一圈，获取一帧的数据的时间是0.1秒(10HZ，600RPM)，共有900个Block，同一Block中的点的时间戳是相同的，时间的分辨率大概为0.1/900=0.111e-3,比ms级别精度略高一些。

2.3 ROS中Header时间的赋值

下面的代码处理完一帧点云所有的MSOP数据后，将一帧点云的时间戳设置为scan_msg.timestamp，而scan_msg.timestamp在驱动处理时，会被设置为系统时间(system_clock)或者是Scan中的最后一个Package中的时间戳(pps同步)。

lidar_driver_impl.hpp—–>

欢迎大家来到IT世界,在知识的湖畔探索吧!template <typename T_Point> inline bool LidarDriverImpl<T_Point>::decodeMsopScan(const ScanMsg& scan_msg, PointCloudMsg<T_Point>& point_cloud_msg) { ...... std::vector<std::vector<T_Point>> pointcloud_one_frame; int height = 1; pointcloud_one_frame.resize(scan_msg.packets.size()); for (int i = 0; i < static_cast<int>(scan_msg.packets.size()); i++) { std::vector<T_Point> pointcloud_one_packet; RSDecoderResult ret = lidar_decoder_ptr_->processMsopPkt(scan_msg.packets[i].packet.data(), pointcloud_one_packet, height); switch (ret) { case RSDecoderResult::DECODE_OK: case RSDecoderResult::FRAME_SPLIT: pointcloud_one_frame[i] = std::move(pointcloud_one_packet); break; case RSDecoderResult::WRONG_PKT_HEADER: reportError(Error(ERRCODE_WRONGPKTHEADER)); break; case RSDecoderResult::PKT_NULL: reportError(Error(ERRCODE_PKTNULL)); break; default: break; } } for (auto iter : pointcloud_one_frame) { output_point_cloud_ptr->insert(output_point_cloud_ptr->end(), iter.begin(), iter.end()); } point_cloud_msg.point_cloud_ptr = point_cloud_transform_func_(output_point_cloud_ptr, height); point_cloud_msg.height = height; point_cloud_msg.width = point_cloud_msg.point_cloud_ptr->size() / point_cloud_msg.height; setPointCloudMsgHeader(point_cloud_msg); //将一帧点云的时间戳设置为scan_msg.timestamp point_cloud_msg.timestamp = scan_msg.timestamp; if (point_cloud_msg.point_cloud_ptr->size() == 0) { reportError(Error(ERRCODE_ZEROPOINTS)); return false; } return true; } template <typename T_Point> inline void LidarDriverImpl<T_Point>::processMsop() { while (msop_pkt_queue_.size() > 0) { PacketMsg pkt = msop_pkt_queue_.popFront(); int height = 1; int ret = lidar_decoder_ptr_->processMsopPkt(pkt.packet.data(), *point_cloud_ptr_, height); scan_ptr_->packets.emplace_back(std::move(pkt)); if ((ret == DECODE_OK || ret == FRAME_SPLIT)) { if (ret == FRAME_SPLIT) { PointCloudMsg<T_Point> msg(point_cloud_transform_func_(point_cloud_ptr_, height)); msg.height = height; msg.width = point_cloud_ptr_->size() / msg.height; setPointCloudMsgHeader(msg); if (driver_param_.decoder_param.use_lidar_clock == true) { msg.timestamp = lidar_decoder_ptr_->getLidarTime(pkt.packet.data()); } else { msg.timestamp = getTime(); } if (msg.point_cloud_ptr->size() == 0) { reportError(Error(ERRCODE_ZEROPOINTS)); } else { runCallBack(msg); } //设置ScanMsg的时间戳 setScanMsgHeader(*scan_ptr_); runCallBack(*scan_ptr_); point_cloud_ptr_.reset(new typename PointCloudMsg<T_Point>::PointCloud); scan_ptr_.reset(new ScanMsg); } } else { reportError(Error(ERRCODE_WRONGPKTHEADER)); msop_pkt_queue_.clear(); std::this_thread::sleep_for(std::chrono::milliseconds(100)); } } msop_pkt_queue_.is_task_finished_.store(true); } 

lidar_driver_impl.hpp—–>

template <typename T_Point> inline void LidarDriverImpl<T_Point>::setScanMsgHeader(ScanMsg& msg) { //设置为系统时间，system_clock或者是Scan中的最后一个Package中的时间戳(pps同步) msg.timestamp = getTime(); if (driver_param_.decoder_param.use_lidar_clock == true) { msg.timestamp = lidar_decoder_ptr_->getLidarTime(msg.packets.back().packet.data()); } msg.seq = scan_seq_++; msg.frame_id = driver_param_.frame_id; } 

ROS发送点云，

欢迎大家来到IT世界,在知识的湖畔探索吧!inline void PointCloudRosAdapter::sendPointCloud(const LidarPointCloudMsg& msg) { point_cloud_pub_.publish(toRosMsg(msg)); } 

将rs_msg.timestamp的时间戳赋值给sensor_msgs::PointCloud2数据的header。

inline sensor_msgs::PointCloud2 toRosMsg(const LidarPointCloudMsg& rs_msg) { sensor_msgs::PointCloud2 ros_msg; pcl::toROSMsg(*rs_msg.point_cloud_ptr, ros_msg); ros_msg.header.stamp = ros_msg.header.stamp.fromSec(rs_msg.timestamp); ros_msg.header.frame_id = rs_msg.frame_id; ros_msg.header.seq = rs_msg.seq; return std::move(ros_msg); } 

3.更精确的点的时间的计算

目前速腾驱动中的点的时间精细化到每个Block的时间，接下来会升级为细化到每个点的时间，这样后续做点云的畸变矫正就会非常方便。

在每个 MSOP Packet 中，有 12 个 Block，每个 Block 有 2 组完整的 16 线激光数据，因此一个 Packet 中有 24 组完整的激光数据。16 通道激光完成一轮发射和充能需要 55.5us。两线激光之间发射的时间间隔是 2.8us，16 线激光完成一轮发射时间为 2.816=44.8us，并有 10.7us 的额外间隔时间，所以一轮发射和充电的时间周期是 2.816+10.7=55.5us。

假设激光的序号 data_index 是 1-16，激光发射轮数序列为 sequence_index 是 1-24。每 个 MSOP Packet 的时间戳是第 1 个激光点的时间，为了计算每个激光点的时间，需要将每个激光点的时间偏移量加到时间戳上。

时间偏移量 Time_offset:

_=55.5∗(_−1)+2.8∗(_−1)

精确的每个激光点的时间 Exact_point_time：

__=+_

下图为RS-16MSOP中每个激光点的时间偏移量，单位为us。

图4 MSOP中每个激光点的时间偏移量