diff --git a/Astar.m b/Astar.m
new file mode 100644
index 0000000..a3fb10f
--- /dev/null
+++ b/Astar.m
@@ -0,0 +1,65 @@
+function [path] = Astar (map, initial, goal)
+%INPUTS
+%goal a 1x2 vector stating the x and y coordinates of the goal cell
+%initial a 1x2 vector stating the x and y coordinated of the initial cell
+%map a ixj binary matrix describing occupancy with 1
+w=12;%cm, robot width
+% map=inflate_map(map); %accounting for robot width in the map
+msize=size(map);
+h=[];%heuristic values of every cell. estimated as the straight distance between the goal and cell
+for i=1:msize(1)
+    for j=1:msize(2)
+        h(i,j)=sqrt((i-goal(1))^2+(j-goal(2))^2);
+        if map(i,j)==1
+            h(i,j)=h(i,j)+1000;
+        end
+    end
+end
+
+open=[initial(1),initial(2),h(initial(1),initial(2)),-1,-1];
+%open nodes,[X(x coordinate of the cell/column),Y( y coordinate of the
+%cell/row),f(sum of weight of all previous nodes and included this+heuristic), x
+%(previous cell),y (previous cell)]
+closed=[];%explored nodes
+[m n] = min(open(:,3));
+current=open(n,:);
+
+%open min and extract
+while ~isequal(current(1:2),goal)
+    children= expand_node(current,h, msize(1), msize(2));
+    %place the children with the lower cost in the open list if they are
+    %redundant in that list
+    closed=[closed;current];
+    open=open([1:n-1,n+1:end],:); %deleting the current element from the open
+    if isempty(children)==0
+        for i=1:length(children(:,1))
+            if BinA(open(:,1:2),children(i,1:2))~=0 %if the child is already present in the open but with a more expensive cost
+                if(children(i,3)<open(BinA(open(:,1:2),children(i,1:2)),3))
+                    BinA(open(:,1:2),children(i,1:2))
+                    open(BinA(open(:,1:2),children(i,1:2)),:)=children(i,:);
+                end
+            else
+                if BinA(closed(:,1:2),children(i,1:2))==0
+                    open=[open ; children(i,:)];
+                else
+                    if (children(i,3)<closed(BinA(closed(:,1:2),children(i,1:2)),3))
+                        BinA(open(:,1:2),children(i,1:2));
+                        closed(BinA(open(:,1:2),children(i,1:2)),:)=children(i,:);
+                    end
+                end
+            end
+        end
+    end
+    [m n] = min(open(:,3));
+    current=open(n,:);
+end
+closed=[closed ;current];
+%reiterate to start to find path
+node=current;
+path=current(1:2);
+while ~isequal(node(4:5),initial)
+    path=[node(4:5); path];
+    node=closed(BinA(closed(:,1:2),node(4:5)),:);
+end
+path
+end
\ No newline at end of file
diff --git a/BinA.m b/BinA.m
new file mode 100644
index 0000000..ced9189
--- /dev/null
+++ b/BinA.m
@@ -0,0 +1,14 @@
+function [i] = BinA (a, b)
+i=0;
+if isempty(a)~=1
+    for n=1:length(a(:,1))
+        if isequal(a(n,:),b)==1
+            i=n;
+            break;
+        else
+            i=0;
+        end
+    end
+end
+end
+
diff --git a/expand_node.m b/expand_node.m
new file mode 100644
index 0000000..eaaffa4
--- /dev/null
+++ b/expand_node.m
@@ -0,0 +1,19 @@
+function [children]=expand_node (parent, h, V, H)
+children=[];
+for i=-1:1
+    for j=-1:1
+        if j~=0 && i~=0 && parent(1)+i>0 && parent(1)+i<=V && parent(2)+j>0 && parent(2)+j<=H
+            c=parent(3)+1.4142+h(parent(1)+i,parent(2)+j)-h(parent(1),parent(2));
+            if ((parent(1)+i)~=parent(4)|| (parent(2)+j)~=parent(5))% && (((j==0 || i==0) && j~=i)||(j~=0 && i~=0))
+                children=[children; [parent(1)+i, parent(2)+j, c, parent(1),parent(2)]];
+            end
+        elseif (j==0 || i==0) && j~=i && parent(1)+i>0 && parent(1)+i<=V && parent(2)+j>0 && parent(2)+j<=H
+            c=parent(3)+1+h(parent(1)+i,parent(2)+j)-h(parent(1),parent(2));
+            if ((parent(1)+i)~=parent(4)|| (parent(2)+j)~=parent(5)) %&& (((j==0 || i==0) && j~=i)||(j~=0 && i~=0))
+                children=[children; [parent(1)+i, parent(2)+j, c, parent(1),parent(2)]];
+            end
+        else
+            
+        end
+    end
+end
diff --git a/homework_map.m b/homework_map.m
new file mode 100644
index 0000000..70e3387
--- /dev/null
+++ b/homework_map.m
@@ -0,0 +1,79 @@
+% 创建地图
+%      1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0
+map = [0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0;   % 1
+       0 0 0 0 0 0 0 0 0 1 0 0 0 0 1 1 0 0 0 0;   % 2
+       0 0 0 1 1 1 1 0 0 1 0 0 0 0 0 1 1 0 0 0;   % 3 
+       0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 1 1 0 0;   % 4
+       0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 1 1 0;   % 5
+       1 1 1 1 1 1 1 0 0 0 0 0 0 0 0 1 0 0 1 1;   % 6
+       1 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 1;   % 7
+       1 0 0 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0;   % 8
+       1 0 0 0 1 1 1 1 1 1 0 0 1 1 0 0 0 0 0 0;   % 9
+       1 0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0;   %10
+       0 0 0 0 0 0 0 0 0 0 1 1 0 0 0 0 0 0 0 0;   %11
+       0 0 0 0 0 0 0 0 0 0 0 0 0 1 1 1 1 1 0 0;   %12
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %13
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %14
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %15
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0;   %16
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 1;   %17
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;   %18
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;   %19
+       0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0;]; %20
+
+% 设置起点和终点
+start = [2, 2];
+goal = [19, 19];
+
+% 调用A*算法
+path = Astar(map, start, goal);
+
+visualize_path(map, start, goal, path);
+
+function visualize_path(map, start, goal, path)
+    % 可视化地图，0为空地，1为障碍
+    imagesc(map);
+    colormap(flipud(gray));          % 灰度图（0白 1黑）
+    axis equal tight;
+    hold on;
+
+    % 蓝色起点
+    plot(start(2), start(1), 'bo', 'MarkerSize', 10, 'LineWidth', 2);
+
+    % 红色终点
+    plot(goal(2), goal(1), 'ro', 'MarkerSize', 10, 'LineWidth', 2);
+
+    if ~isempty(path)
+    % 如果起点不等于路径首元素，插入
+        if ~isequal(path(1,:), start)
+            path = [start; path];
+        end
+    % 如果终点不等于路径末元素，插入
+        if ~isequal(path(end,:), goal)
+            path = [path; goal];
+        end
+
+    % 显示路径线
+        plot(path(:,2), path(:,1), 'g-', 'LineWidth', 2); % 绿色路径线
+        plot(path(:,2), path(:,1), 'go', 'MarkerSize', 4, 'MarkerFaceColor', 'g'); % 每个点画圈
+    end
+
+    % 网格线
+    [rows, cols] = size(map);
+    for i = 1:rows+1
+        line([0.5, cols+0.5], [i-0.5, i-0.5], 'Color', 'k'); % 横线
+    end
+    for j = 1:cols+1
+        line([j-0.5, j-0.5], [0.5, rows+0.5], 'Color', 'k'); % 竖线
+    end
+
+    set(gca, ...
+        'XTick', 1:cols, ...
+        'YTick', 1:rows, ...
+        'XTickLabel', 1:cols, ...
+        'YTickLabel', 1:rows, ...
+        'XAxisLocation', 'top', ...      % x轴
+        'YDir', 'reverse');              % y轴
+    title('地图');
+    hold off;
+end
\ No newline at end of file