SICK激光雷達LMS511測量數據說明

幀結構說明

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等數據的描述如下：

在length（byte）一列中，對不同參數的字節數做了說明，VersionNumber為2個字節。通過對比幀結構說明和實際獲取的數據中發現，VersionNumber只有1個字節，並且后面其他參數也有類似的情況出現。所以這個字節數是否代表參數最大占有字節數。
在一個版本的中文手冊《LMS5XX 中文操作手冊》的最后一頁發現這樣的描述：

通過對比這份說明和實際的數據，則能很好的對應，所以以這份說明的幀結構為參考依據。

數據舉例

幀結構中，每個字段之間是空格（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