SICK激光雷达LMS511测量数据说明

2021-12-01 01:27:51

帧结构说明

LMS511的官方手册存在几个版本，在《Laser Measurement Systems of the LMS500 Product Family》的英文手册中，对单次（连续）获取测量结果的返回帧结构的说明中，容易误导用户（也可能是我没注意到细节）。

例如，对单次返回的帧结构，手册上是这样描述的。

单次获取对应的十六进制命令为：

02 73 52 4E 20 4C 4D 44 73 63 61 6E 64 61 74 61 20 31 03

接收数据的命令格式：

sRA LMDscandata VersionNumber DeviceNumber SerialNumber DeviceStatus MessageCounter ScanCounter PowerUpDuration TransmissionDuration InputStatus OutputStatus ReservedByteA ScanningFrequency MeasurementFrequency NumberEncoders [EncoderPosition EncoderSpeed] NumberChannels16Bit [MeasuredDataContent ScalingFactor ScalingOffset StartingAngle AngularStepWidth NumberData [Data_1 Data_n]] NumberChannels8Bit [MeasuredDataContent ScalingFactor ScalingOffset StartingAngle AngularStepWidth [NumberData Data_1 Data_n] Position [XPosition YPosition ZPosition XRotation YRotation ZRotation RotationType] Name [DeviceName] Comment [CommentContent] TimeInfo [Year Month Day Hour Minute Second Microseconds] EventInfo [EventType EncoderPosition EventTime AngularPosition]

在该手册中对VersionNumber DeviceNumber等数据的描述如下：

SICK激光雷达LMS511测量数据说明

在length（byte）一列中，对不同参数的字节数做了说明，VersionNumber为2个字节。通过对比帧结构说明和实际获取的数据中发现，VersionNumber只有1个字节，并且后面其他参数也有类似的情况出现。所以这个字节数是否代表参数最大占有字节数。

在一个版本的中文手册《LMS5XX 中文操作手册》的最后一页发现这样的描述：

SICK激光雷达LMS511测量数据说明

通过对比这份说明和实际的数据，则能很好的对应，所以以这份说明的帧结构为参考依据。

数据举例

SICK激光雷达LMS511测量数据说明

帧结构中，每个字段之间是空格（20）分开的。值得注意的是，LMS511输出数据均是字符的ASCII码，需要将其转化为十六进制的字符，再转为十进制。输出的测量数据，有的是3个字节，有的是2个字节，甚至1个字节，所以只能以空格来作为划分依据。

以下是读取原始数据文本，并绘制二维扫描图的matlab程序。

clear all

clc

angle_orign_temp = [];

angle_step_temp = [];

data_length_temp = [];

data_range_temp =[];

data_range = [];

file_addr = 'data.txt';

data = textread(file_addr,'%s')';                             %以字符串的形式读取原始数据

data_orign_dec = hex2dec(data)';

flag = [48,48,48,48,48,48,48,48];                           %原始帧结构中的预留标志段，8个0，十进制

flag_postion = findstr(data_orign_dec,flag );          %寻找标志段所在位置

data_orign_use = data_orign_dec(flag_postion+8:end);   

%LMS511以空格来区分不同字段，每个字段有各自的含义，只取有用部分

spc_location = find(data_orign_use == 32);           %找出空格字符所在的下标

%提取起始角度

for i1 = spc_location(1)+1: spc_location(2)-1

    angle_orign_temp = strcat(angle_orign_temp,char(data_orign_use(i1)));

end

angle_orign = hex2dec(angle_orign_temp);

angle_orign = (angle_orign/10000)*pi/180;

%提取角度分辨率

for i2 = spc_location(2)+1: spc_location(3)-1

    angle_step_temp = strcat(angle_step_temp,char(data_orign_use(i2)));

end

angle_step = hex2dec(angle_step_temp);

angle_step = (angle_step/10000)*pi/180;

%提取测量点数

for i3 = spc_location(3)+1: spc_location(4)-1

    data_length_temp = strcat(data_length_temp,char(data_orign_use(i3)));

end

data_length = hex2dec(data_length_temp);

data_polar =zeros(2,data_length);

data_rec =zeros(2,data_length);

%将数据转换成有效的十进制数

for i = 1:data_length

	for j = spc_location(i+3)+1:spc_location(i+4)-1

        data_range_temp = strcat(data_range_temp,char(data_orign_use(j)));

	end

   data_range(i) = hex2dec(data_range_temp);

   data_range_temp = [];

end

for w = 1:data_length

    if	w == 1

         data_polar(1,w) = angle_orign;

         data_polar(2,w) =  data_range(w);

    else

        data_polar(1,w) =  data_polar(1,w-1) + angle_step;

        data_polar(2,w) =  data_range(w);

    end

end

%极坐标到直角坐标变换

for x = 1:data_length

    data_rec(1,x) = cos(data_polar(1,x))*data_polar(2,x);

    data_rec(2,x) = sin(data_polar(1,x))*data_polar(2,x);

end

plot(data_rec(1,:),data_rec(2,:),'.')

注意事项

matlab代码有两点需要注意：

  • 原始数据的文本格式。我的数据文本文件中每个数据以空格为分割，不含有其他字符。
  • 我的初始角度没考虑负角度，因为工作限定，角度都非负，所以有负角度的情况需要微调（原始数据会以补码形式给出）。
%修正部分代码，如果起始角度为负数的话，增加一个判断

%提取起始角度

for i1 = spc_location(1)+1: spc_location(2)-1

    angle_orign_temp = strcat(angle_orign_temp,char(data_orign_use(i1)));

end

if(angle_orign_temp(1) == 'F')

    angle_orign = hex2dec(angle_orign_temp) - 2^32;    %起始角度为负，用8位十六进制表示，最高位为F

    angle_orign = (angle_orign/10000)*pi/180;

else

    angle_orign = hex2dec(angle_orign_temp);

    angle_orign = (angle_orign/10000)*pi/180;

end

%提取角度分辨率

后续程序改进

有网友问，这段程序可不可以用来打开SICK官方软件SOPAS保存下来的log数据文件，答案是不可以！，因为数据里面穿插有<>字符。实际上只要先去掉<>，则剩下的就可以直接使用上述代码。

算了好人做到底，直接给可用于读取log文件数据的MATLAB代码，如下：

clear all

clc

angle_orign_temp = [];

angle_step_temp = [];

data_length_temp = [];

data_range_temp =[];

data_range = [];

data_orign = importdata('data.log');

[row,col]=size(data_orign);

for i_frame = 1:row

data_orign_temp1 = strcat(data_orign{i_frame,1});                %把读到的按行存的cell格式转换成字符串

header_pos = findstr(data_orign_temp1,'<02>');

data_orign_temp11= data_orign_temp1(header_pos:end);

data_orign_temp11(find(data_orign_temp11 == '<')) = [];     %去掉字符串中的空格

data_orign_temp2 = strrep(data_orign_temp11,'>',' ');        %用空格替换>

data_orign_temp3 = regexp(data_orign_temp2, ' ', 'split');  %按空格将string分割成数组

data_orign_dec = hex2dec(data_orign_temp3)';          %进制转换

flag = [48,48,48,48,48,48,48,48];                           %原始帧结构中的预留标志段，8个0，十进制

flag_postion = findstr(data_orign_dec,flag );          %寻找标志段所在位置

data_orign_use = data_orign_dec(flag_postion+8:end);   

%LMS511以空格来区分不同字段，每个字段有各自的含义，只取有用部分

spc_location = find(data_orign_use == 32);           %找出空格字符所在的下标

%提取起始角度

for i1 = spc_location(1)+1: spc_location(2)-1

    angle_orign_temp = strcat(angle_orign_temp,char(data_orign_use(i1)));

end

if(angle_orign_temp(1) == 'F')

angle_orign = hex2dec(angle_orign_temp) - 2^32; %起始角度未负的话，用8位十六进制表示，最高位为F

angle_orign = (angle_orign/10000)*pi/180;

else

    angle_orign = hex2dec(angle_orign_temp);

    angle_orign = (angle_orign/10000)*pi/180;

end

%提取角度分辨率

for i2 = spc_location(2)+1: spc_location(3)-1

    angle_step_temp = strcat(angle_step_temp,char(data_orign_use(i2)));

end

angle_step = hex2dec(angle_step_temp);

angle_step = (angle_step/10000)*pi/180;

%提取测量点数

for i3 = spc_location(3)+1: spc_location(4)-1

    data_length_temp = strcat(data_length_temp,char(data_orign_use(i3)));

end

data_length = hex2dec(data_length_temp);

data_polar =zeros(2,data_length);

data_rec =zeros(2,data_length);

%将数据转换成有效的十进制数

for i = 1:data_length

	for j = spc_location(i+3)+1:spc_location(i+4)-1

        data_range_temp = strcat(data_range_temp,char(data_orign_use(j)));

	end

   data_range(i) = hex2dec(data_range_temp);

   data_range_temp = [];

end

i = 1;

for w = 1:data_length

    if	w == 1

         data_polar(1,w) = angle_orign;

         data_polar(2,w) =  data_range(w);

    else

        data_polar(1,w) =  data_polar(1,w-1) + angle_step;

        data_polar(2,w) =  data_range(w);

    end

end

%极坐标到直角坐标变换

for x = 1:data_length

    data_rec(1,x) = cos(data_polar(1,x))*data_polar(2,x);

    data_rec(2,x) = sin(data_polar(1,x))*data_polar(2,x);

end

% figure

% plot(data_polar(1,:),data_polar(2,:),'.')

plot(data_rec(1,:),data_rec(2,:),'.')

angle_orign_temp = [];

angle_step_temp = [];

data_length_temp = [];

data_range_temp =[];

data_range = [];

pause(1);

end
上一篇：1016: [JSOI2008]最小生成树计数 - BZOJ


下一篇：linq给list集合数据分页