雷达数据的运动校准和里程计时间同步

[weijiz]

雷达固定在机器人上，当机器人运动时由于LaserScan每个beam存在时间差就会产生运动误差。同时如果里程计和雷达本身存在时间差，也会造成扫描误差。不过这两个误差的表现形式有所区别。

首先是雷达的运动误差。如果在rviz中laser坐标系下看雷达数据。当机器人在原地快速旋转时，会发现雷达的x轴左右两边的数据是明显不一致的。如下图所示

右前方基本是两条平行线，当这个部分运动到雷达正前方时会发生形变

两条直线不再平行。这是由于x轴两边的数据正好对应差了一个scan time。雷达产生的相对移动是最大的。

通过计算实际时间差可以把这个修正回来。

图中红色就是修正后的数据。

另一个主要的误差来自odom和scan的时间戳不同步。它会造成的误差表现是如果机器人在原地匀速旋转那么雷达数据和里程计当前对应的雷达数据就会差一个固定角度。如下图所示。

其中绿色是真值红色是测量值。同时这个角度也很容易算出是delta_t * w, 其中delta_t是时间戳差值，w是旋转角速度。

所以我们很容易通过现象去判断是否存在此误差。如果机器人原地旋转时在rviz中，odom坐标系下，你发现雷达数据随着角速度的不同有明显的整体的角度变化。那就说明存在一个雷达和里程计的时间戳是不同步的，有可能某个传感器的时间戳不准。

[小助理]

雷达数据的运动校准和里程计时间同步

1. 雷达的运动误差

当机器人在运动时，由于LaserScan每个beam之间存在时间差，会导致运动误差。这种误差在机器人原地快速旋转时尤为明显。具体表现为雷达数据在x轴左右两边的数据不一致。

示例现象：

• 当机器人在原地快速旋转时，雷达数据在x轴左右两边的数据会形成明显的不对称。
• 右前方的数据可能是一条平行线，但当这部分数据运动到雷达正前方时会发生形变，两条直线不再平行。

修正方法：

• 通过计算实际时间差来修正雷达数据。假设雷达的扫描时间为scan_time，则可以通过以下公式进行修正：

import numpy as np

def correct_laser_data(laser_data, scan_time, robot_velocity):
    """
    修正雷达数据以消除运动误差
    :param laser_data: numpy数组，包含雷达数据
    :param scan_time: float，雷达扫描时间
    :param robot_velocity: float，机器人线速度
    :return: 修正后的雷达数据
    """
    num_beams = len(laser_data)
    corrected_data = np.zeros(num_beams)
    
    for i in range(num_beams):
        # 计算每个beam的时间差
        time_diff = (i / num_beams) * scan_time
        # 计算由于时间差导致的位移
        displacement = robot_velocity * time_diff
        # 修正雷达数据
        corrected_data[i] = laser_data[i] - displacement
    
    return corrected_data

示例使用：

# 假设雷达数据
laser_data = np.array([1.0, 1.1, 1.2, 1.3, 1.4, 1.5, 1.6, 1.7, 1.8, 1.9])
scan_time = 0.1  # 秒
robot_velocity = 0.5  # 米/秒

corrected_data = correct_laser_data(laser_data, scan_time, robot_velocity)
print("修正后的雷达数据:", corrected_data)

2. 里程计和雷达的时间戳不同步

如果里程计和雷达的时间戳不同步，会导致雷达数据和里程计数据之间存在固定的角度偏差。这种偏差在机器人原地匀速旋转时尤为明显。

示例现象：

• 如果机器人在原地匀速旋转，雷达数据在rviz中odom坐标系下会有明显的整体角度变化。
• 这个角度偏差可以通过delta_t * w计算，其中delta_t是时间戳差值，w是旋转角速度。

修正方法：

• 通过调整时间戳来同步雷达和里程计的数据。假设雷达数据的时间戳为t_scan，里程计数据的时间戳为t_odom，则可以通过以下公式进行修正：

import rospy
from sensor_msgs.msg import LaserScan
from nav_msgs.msg import Odometry

class TimeSyncCorrector:
    def __init__(self):
        self.laser_sub = rospy.Subscriber('/scan', LaserScan, self.laser_callback)
        self.odom_sub = rospy.Subscriber('/odom', Odometry, self.odom_callback)
        self.laser_pub = rospy.Publisher('/corrected_scan', LaserScan, queue_size=10)
        
        self.last_odom_time = None
        self.last_laser_time = None
        self.odom_data = None

    def laser_callback(self, msg):
        self.last_laser_time = msg.header.stamp
        if self.last_odom_time is not None:
            self.correct_and_publish(msg)

    def odom_callback(self, msg):
        self.last_odom_time = msg.header.stamp
        self.odom_data = msg

    def correct_and_publish(self, laser_msg):
        if self.last_odom_time is None or self.last_laser_time is None:
            return
        
        delta_t = (self.last_laser_time - self.last_odom_time).to_sec()
        w = self.odom_data.twist.twist.angular.z
        
        # 计算角度偏差
        angle_offset = delta_t * w
        
        # 修正雷达数据
        corrected_angles = [angle + angle_offset for angle in laser_msg.ranges]
        
        # 创建新的LaserScan消息
        corrected_msg = LaserScan()
        corrected_msg.header = laser_msg.header
        corrected_msg.angle_min = laser_msg.angle_min
        corrected_msg.angle_max = laser_msg.angle_max
        corrected_msg.angle_increment = laser_msg.angle_increment
        corrected_msg.time_increment = laser_msg.time_increment
        corrected_msg.scan_time = laser_msg.scan_time
        corrected_msg.range_min = laser_msg.range_min
        corrected_msg.range_max = laser_msg.range_max
        corrected_msg.ranges = corrected_angles
        corrected_msg.intensities = laser_msg.intensities
        
        # 发布修正后的雷达数据
        self.laser_pub.publish(corrected_msg)

if __name__ == '__main__':
    rospy.init_node('time_sync_corrector')
    corrector = TimeSyncCorrector()
    rospy.spin()

示例使用：

• 运行上述ROS节点，订阅/scan和/odom话题，发布修正后的雷达数据到/corrected_scan话题。

通过以上方法，可以有效地校准雷达数据的运动误差和里程计时间戳不同步的问题，提高机器人的定位和导航精度。

[weijiz]

@weijiz 还有一点，如果不进行运动校准，直接在odom坐标系下看scan数据，你会发现之前存在的运动校准误差消失了。这是因为rviz在odom下显示scan数据的时候不是按照整体显示的，它会计算每个点的时间戳去显示。相当于完成了运动校准。所以如果你的运动校准计算正确，那么你的计算数据在odom下就应该和rviz的原始数据重合。