如何在Python中从LIDAR传感器(LMS511)数据中提取x和y坐标?

cgh8pdjw  于 2023-04-22  发布在  Python
关注(0)|答案(1)|浏览(140)

我有一个LIDAR传感器,特别是SICK LMS511,它以以下格式输出连续数据:
sRA LMDscandata 0 1 151FDC8 0 0 EC4B EDD5 85BF655E 85BF9621 0 0 3E 0 0 2710 21C 0 1 DIST1 3F800000 0000000 4A0E5 1A0B B5 305 30F 312 315 320 325 328 32F 0 0 0 B62 B35 B16 AD8 AC9 742 753 767779 791 7A8 7B8 764 782 793 7B1 7C8 7E5 7FF 817 807 806 834 824 816 802 7F8 7E8 7DA 7C9 7B0 7AF 797 789 780 771 767 781 7AB 7A6 796 788 77F 77E 771 76B 769 751 74A 74273A 732 731 724 71C 71C 716 70F 707 701 701 6FC 6F2 6F2 6E9 6EC 6E7 6E5 6E3 6E4 6DA 6D6 6D5 6D5 6D6 6D4 6D8 6D7 6D2 6CE 6D2 6D4 6D4 6D4 6CE 6D0 6D8 6E3 6DC 6E1 6E4 6E4 6E9 6E9 6FA 6ED 6F76F7 702 70A 707 712 710 71A 720 726 728 730 73C 740 74A 751 759 765 76D 770 787 78A 796 7A3 7A9 7B2 7C6 7D5 7E2 7E9 7FC 808 809 828 837 848 85B 86B 87B 88C 89B 8B3 8D18E8 8F8 90F 91C 93E 957 971 989 96A 94E 974 992 9B9 9CC 9E5 A11 A88 AD7 B09 B2F B59 B8A BB5 BE8 C1E C54 C85 CBD D07 D3A D81 DC6 0 0 0 0 0
我想从这个数据中提取x和y坐标。我如何在Python中做到这一点?
注:请注意,这只是传感器的一个样本数据,它以这种格式输出连续数据。
我尝试使用Python从LIDAR数据中提取x和y坐标。具体来说,我尝试使用Python解析数据并提取坐标。但是,我没有成功提取坐标。

tyg4sfes

tyg4sfes1#

报文结构

有问题的电报记录在Telegram Listing参考文件中,从第97-115页开始。
电报由空格分隔的令牌组成,数字通常是十六进制的。首先,有一个可以称为报头的18个令牌。我已经简要地注解了您的示例中的报头:

Command type           sRA
Command                LMDscandata
Version                0
Device number          1
Serial number          151FDC8
Device status          0 0
Telegram counter       EC4B
Scan counter           EDD5
Time since startup     85BF655E
Time of transmit       85BF9621
Digital inputs         0 0
Digital outputs        3E 0
Reserved/layer angle   0
Scanning frequency     2710          (100 Hz)
Measurement frequency  21C           (54 kHz)

接下来是报文的可变部分,由8个部分组成。每个部分以计数标记开始,表示该部分由多少个数据块(如果有的话)组成。当传感器未配置为提供特定类型的数据时,计数将为0,紧接着是下一个部分。

# of encoder blocks         0
[<encoder info>]
# of 16-bit channel blocks  1
[<channel blocks>]
# of 8-bit channel blocks   0
[<channel blocks>]
Position                    0
[<position info>]
Device name                 0
[<name text>]
Comment                     0
[<comment text>]
Time                        0
[<timestamp>]
Events                      0 
[<event info>]

在您的情况下,情况很简单,因为只有1个16位通道数据块。该块的布局为:

Content                 DIST1     (Distance values of first pulse)
Scaling factor          3F800000  (1x)
Scale factor offset     00000000
Start angle             4A0E5     (30.3333 deg)
Angular step size       1A0B      (0.6667 deg)
Value count             B5        (181)
Data                    305 ... DC6

解析报文

有了这些,我们可以为您的特定传感器配置提供一个基本的解析器:

  • 使用空格作为分隔符,将电报字符串拆分为令牌。
  • 检查它是否是预期的命令类型和命令
  • 检查是否有0个编码器有效负载块
  • 检查是否正好有1个16位通道块
  • 检查它是否为“DIST 1”块
  • 确定使用的比例因子(1x或2x)
  • 解析起始Angular 和Angular 步长,并将它们缩放到度
  • 解析值计数
  • 抓取适当数量的值标记,丢弃其余的**解析每个值,并按比例因子缩放
  • 计算每个测量值对应的Angular (start_angle + step * n

在普通Python中的一个粗略实现可能看起来像这样:

def parse_telegram(telegram):
    tokens = telegram.split(' ')
    assert(len(tokens) > (18 + 8)) # Minimum valid length
    header = tokens[:18]
    assert(header[0] == 'sRA') # Correct command type
    assert(header[1] == 'LMDscandata') # Correct command
    sections = tokens[18:]
    assert(int(sections[0]) == 0) # No encoder data
    assert(int(sections[1]) == 1) # Exactly 1 16-bit channel block
    assert(sections[2] == 'DIST1') # Expected distance data
    assert(sections[3] in ['3F800000', '40000000'])
    scale_factor = 1 if sections[3] == '3F800000' else 2
    assert(sections[4] == '00000000')
    start_angle = int(sections[5], 16) / 10000.0
    angle_step = int(sections[6], 16) / 10000.0
    value_count = int(sections[7], 16)
    values = list(map(lambda x: int(x, 16) * scale_factor, sections[8:8+value_count]))
    # The following could be cached to avoid recalculation
    # since it will be the same until sensor config is changed...
    angles = [start_angle + angle_step * n for n in range(value_count)]
    return (values, angles)

该函数返回一个由两个列表组成的元组--第一个包含距离,第二个包含相应的波束Angular 。我们可以使用matplotlib在极坐标图上绘制这个结果,看看它是否有意义:

Polar转Cartesian

将极坐标转换为笛卡尔坐标只需要应用一些基本的三角函数:
x = r × cos(θ)
y = r × sin(θ)
在普通Python中:

def to_cartesian(distances, angles):
    x = list(map(lambda r, t: r * math.cos(math.radians(t)), distances, angles))
    y = list(map(lambda r, t: r * math.sin(math.radians(t)), distances, angles))
    return (x, y)

再次,快速绘图以检查结果是否有意义:

相关问题