소개

전회의 ROS + MATLAB에서 레이저 값을 얻고 그려 보았습니다. 으로부터의 계속으로, 다음은 레이저를 이용해 물체 검출을 해 보려고 생각합니다

목적

ROS와 MATLAB의 공연

준비하는 것

· Raspi (Ubuntu mate 16.04)
· PC (Matlab이 들어있는 PC (2017a 이상 권장))
→Robotics System Toolbox를 넣어주세요
・UST-10(Hokuyo 전기)

알고리즘

레이저 데이터는 각도와 거리로 게시됩니다.
물체를 검출하고 싶은 경우는 거리와 각도만으로 생각해야 합니다.
매우 쉽게


이런 느낌입니다.
즉, 배열을 보고 갈라진 곳을 보면 됩니다!
임계 값이나 보는 범위의 보정은 필요하지만 기본은이 알고리즘으로 충분합니다.
덧붙여서 이번 내 프로그램은 보는 범위를 1m로 하고 있습니다.
즉 1m에 비해 값이 다소 작아진 곳은 물체가 있다는 것입니다.
LRS의 성능에 따라 다르지만 이것은 10m까지 볼 수 있지만 아깝다.

코드

기능

이전 코드도 함께 쓸 수 있지만 detect_obs라는 함수를 새로 만듭니다. 이 함수는 거리 정보와 각도 정보를 얻어 물체의 위치 (역치가 바뀐 위치, 물체의 검출이 끝난 위치)와 그 수를 받습니다.

detect_obs.m

function [obs_s_set, obs_f_set, obs_num] = detect_obs(distance, angle)

%物体検出パラメータ
distance_num = length(distance);%データの数
%1m以内を考慮する
basic_dis = 1 * ones(1, distance_num);
%検出サイズ
obs_size  = 0.15;
margin = 0.5;%誤差
%検出閾値（これを超えてたらなんかある）
dis_dif = 0.3;

%basicとの差を見る
dif = basic_dis - distance;

%記録用
j = 1;

%判定
obs_flag = false;

%大きくなったところを記録
for i = 30 : 1: distance_num-30
    if dif(i) < 0
       dif(i) = 0;        
    end

    if abs(dif(i)) > dis_dif
        obs_flag = true;
        start_p = i;
    end

    if obs_flag == true
        if abs(dif(i)) < dis_dif
            obs_flag = false;
            fin_p = i-1;
            %開始点と終了点の位置
            D_x_st = distance(start_p) * cos(angle(start_p));
            D_y_st = distance(start_p) * sin(angle(start_p));
            D_x_en = distance(fin_p) * cos(angle(fin_p));
            D_y_en = distance(fin_p) * sin(angle(fin_p));

            %物体の大きさ推定
            obs_temp_size = sqrt((D_x_st - D_x_en)^2+(D_y_st - D_y_en)^2);
            %狙い通りの大きさだったらカウント
            if obs_temp_size < obs_size + margin && obs_temp_size > obs_size - margin
                obs_s_set(j, :) = [D_x_st, D_y_st];
                obs_f_set(j, :) = [D_x_en, D_y_en];
                j = j + 1;
             end
        end
    end
end

obs_num = j -1;
if obs_num == 0
    obs_s_set = 0;
    obs_f_set = 0;
end

작동하지 않는 경우 매개변수 또는 임계값을 만지십시오.

그리기입니다.

Laser_func.m

%% Laser_func
function laser_func(obj, message, src);

%% global変数(Publishノード）
global matlab_pub_node

%% massageを受信
distance = ((message.Ranges)');%距離データ（）
distance_num = length(distance);%データの数
Max_angle = message.AngleMax;%最大角度
min_angle = message.AngleMin;%最小角度
angle_step = message.AngleIncrement;%ステップ角度
angle = min_angle: angle_step :Max_angle;%角度行列

%Laserのスペック
max_det_dis = 25;

%外れ値処理（値がとれずに35となった場合）
for n = 1:1:distance_num
    if distance(n) > max_det_dis && n > 1%端以外の外れ値は1つ前の角度の距離と同様
        distance(n) = distance(n-1);        
    elseif n == 1 && distance(n) > max_det_dis%端は0に
        distance(n) = 0;
    end
end

%% ここから下に処理を書いてください %%%%%%%%%%%%%%
%distanceは距離データ行列
%angleは角度行列

%% 物体検出
%物体検出
%obs_s_set, obs_f_set, obs_num = detect_obs(distance, angle);
[obs_s_set, obs_f_set, obs_num] = detect_obs(distance, angle)

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

%% 描画
refresh%リセット

for n_all = 1:1:distance_num
    X_all_Data(n_all) = distance(n_all) * cos(angle(n_all));
    Y_all_Data(n_all) = distance(n_all) * sin(angle(n_all));
end

plot(X_all_Data, Y_all_Data, 'LineWidth', 3, 'Color', [0 0 1]);
hold on

% 障害物描画
obs_size = 0.2;
if obs_num > 0
    for i_3 = 1: 1: obs_num
        %センターポジション取得
        Position_X_Data = (obs_f_set(i_3, 1) + obs_s_set(i_3, 1))/2;
        Position_Y_Data = (obs_f_set(i_3, 2) + obs_s_set(i_3, 2))/2;
        for n_ob = 0:1:100
            X_Data(n_ob+1) = Position_X_Data + obs_size * cos((n_ob)*2*pi/100);
            Y_Data(n_ob+1) = Position_Y_Data + obs_size * sin((n_ob)*2*pi/100);
        end
        plot(X_Data, Y_Data, 'LineWidth', 3, 'Color', [0 0.3 0]);
        clear X_Data Y_Data;
        hold on
    end
end

%描画範囲
XLIM = max(X_all_Data);
YLIM = max(Y_all_Data);
xlim([-XLIM XLIM]);
ylim([-YLIM YLIM]);
hold off

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%

메인 함수입니다.

main.m

%% 初期化作業
clear
close all
%ROS初期化
%raspiのIPを入力（変更する場合）
rosinit('http://192.168.1.145:11311')

%% 各ノード立ち上げ
% Publisher Global 宣言
global matlab_pub_node

% Publisher
% Topic : /cmd_vel
% Topic Type : geometry_msgs/Twist
matlab_pub_node = rospublisher('/cmd_vel', 'geometry_msgs/Twist');

% Subscriber
% Topic : /Scan
% Topic Type : MatlabはSubscribeする場合はTopic名のみで受信可能
% Callback_func : laser_func
% Topicを受信し次第@laser_funcを実行（同フォルダ内にあります）
matlab_sub_node = rossubscriber('/scan', @laser_func);%レーザの値を取得してくるSubscriber

%% またはreceiveでも値を得ることが可能

%% 片付け
%rosshutdown

간단합니다!
MATLAB은 매우 비싸기 때문에 그런 의미에서는 사도이지만, 쉽게 놀고 싶을 때는 매우 유용합니다.

결과입니다
실제 영상


Matlab에 있다면,


Github에 올렸습니다.
확인 부탁드립니다.
htps : // 기주 b. 코 m / 슈니치 09 / 쿠이타 / t 리에 / 마 s r / ㄱ r / ㄱ r_에서 c

Twitter도 시작했습니다.
htps : // 라고 해서 r. 코m/슈니치세키구 1
관심이 있으시면 팔로우해주세요
다음은 모터를 움직입니다