如何将本地雷达激光转换为全球位置？

Question

我需要将雷达传感器数据从局部坐标转换为全局坐标。我的做法是先将雷达激光数据转换为局部坐标，然后通过/tf得到旋转四元数，将四元数转换为偏航角，通过旋转矩阵得到旋转坐标。最后根据/odom获取的机器人位置，计算出全局坐标。

def getPos(self):
    pos = self.odom_data.pose.pose.position
    rotation = self.tf_data.transforms[0].transform.rotation
    roll, pitch, yaw = euler_from_quaternion((rotation.x, rotation.y,
                                                  rotation.z, rotation.w))
    return [pos.x, pos.y, yaw]

def getScan(self):
    scan = self.scan_data.ranges
    rpos = self.getPos()
    a_start = self.scan_data.angle_min
    a_increment = self.scan_data.angle_increment
    scan_xy = []
    for i in range(len(scan)):
        if scan[i] < 0.1:
           continue
        x = scan[i] * math.cos(a_start + i * a_increment)
        y = scan[i] * math.sin(a_start + i * a_increment)
        x = x * math.cos(rpos[2]) - y * math.sin(rpos[2]) + rpos[0]
        y = y * math.cos(rpos[2]) + x * math.sin(rpos[2]) + rpos[1]
        scan_xy.append([x, y])
     return np.array(scan_xy)

上面的代码是我的计算过程。我想将激光雷达的局部坐标位置转换为全局坐标位置，但是现在转换后并没有达到我想要的效果。上面的代码问题出在哪里？

Answer 1

您的方法的问题在于，当您更新

x

然后使用新的

x

来计算

y

时。这是错误的做法，因为

y

的计算取决于原始

x

值，而不是更新后的值。

因此，要解决此问题，您需要在更新

x

之前将新的

x

值临时存储在单独的变量中。实际上我犯了你在之前的项目中犯的同样的错误。你离得并不远，只是错过了一些细节。

这是带有虚拟数据的建议代码：

import math
import numpy as np

def euler_from_quaternion(quat):
    x, y, z, w = quat
    return (0, 0, math.atan2(2.0*(y*z + w*x), w*w - x*x - y*y + z*z))

class SensorDataTransform:
    def __init__(self, odom_data, scan_data, tf_data):
        self.odom_data = odom_data
        self.scan_data = scan_data
        self.tf_data = tf_data

    def getPos(self):
        pos = self.odom_data['pose']['pose']['position']
        rotation = self.tf_data['transforms'][0]['transform']['rotation']
        _, _, yaw = euler_from_quaternion((rotation['x'], rotation['y'], rotation['z'], rotation['w']))
        return [pos['x'], pos['y'], yaw]

    def getScan(self):
        scan = self.scan_data['ranges']
        rpos = self.getPos()
        a_start = self.scan_data['angle_min']
        a_increment = self.scan_data['angle_increment']
        scan_xy = []
        for i in range(len(scan)):
            if scan[i] < 0.1: 
                continue
            x_local = scan[i] * math.cos(a_start + i * a_increment)
            y_local = scan[i] * math.sin(a_start + i * a_increment)
            x_global = x_local * math.cos(rpos[2]) - y_local * math.sin(rpos[2]) + rpos[0]
            y_global = y_local * math.cos(rpos[2]) + x_local * math.sin(rpos[2]) + rpos[1]
            scan_xy.append([x_global, y_global])
        return np.array(scan_xy)

odom_data = {'pose': {'pose': {'position': {'x': 1, 'y': 2}}}}
tf_data = {'transforms': [{'transform': {'rotation': {'x': 0, 'y': 0, 'z': 0.7071068, 'w': 0.7071068}}}]}

scan_data = {
    'ranges': [1.0, 1.5, 2.0],
    'angle_min': -math.pi / 4,
    'angle_increment': math.pi / 4 
}

transformer = SensorDataTransform(odom_data, scan_data, tf_data)
print(transformer.getScan())

这给你：

[[1.70710678 1.29289322]
 [2.5        2.        ]
 [2.41421356 3.41421356]]