障碍检测和路径规划的MATLAB仿真

可以根据当前所在位置的图像，给出坐标。判断出障碍物的个数和给出最优的可以路线 实现简单的路径规划功能

%/**************************************
%;function: caculate angle of motion
%   inputs:   x_z_all:深度图对应的坐标值地图 
%             R_window：移动窗口的半径大小
%
%   outputs: val
%   the value of the resopond angle of the position to centre
%****************************************/

function [valB_PosB_dFree_Pos]=get_beta2（x_z_allR_window）

pi =3.14159;
[K1K2] = size（x_z_all）;%取出数组的长度
val_rsc（）=0;
val（360/5）=0;

bedal（）=0.0;%保存为一维的数组
k=1;
t（360/5）=0;%平均次数
for i=1:K1
    for j=1:K2
        if （（（i-K1/2-0.5）^2+（j-K2/2-0.5）^2）<=（R_window）^2）%保证是圆形移动窗口 R_window*R_window
            if（（j-K2/2-0.5）>0）%属于一 二 象限
                bedal（k）=atan（（j-K2/2-0.5）/（i-K1/2-0.5））;  %k对应是列扫描过去的
                  if （bedal（k）<0）
                        bedal（k）=bedal（k）+pi;
                  

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       3930  2010-05-14 16:50  新建文件夹\get_beta2.m

     文件        246  2010-05-15 21:38  新建文件夹\readme.txt

     文件     236278  2010-05-20 22:59  新建文件夹\test1.bmp

     文件       1336  2010-05-14 17:00  新建文件夹\test1.m

     目录          0  2010-05-20 23:01  新建文件夹

----------- ---------  ---------- -----  ----

               241790                    5