diff --git a/src/FindRSPath_plus.m b/src/FindRSPath_plus.m
new file mode 100644
index 0000000..598a290
--- /dev/null
+++ b/src/FindRSPath_plus.m
@@ -0,0 +1,17 @@
+function path = FindRSPath_plus(x,y,phi,veh)
+    rmin = veh.MIN_CIRCLE; %minimum turning radius
+    x = x/rmin;
+    y = y/rmin;
+    reedsConnObj = reedsSheppConnection;
+    startPose = [0 0 0];
+    goalPose = [x y phi];
+    [pathSegObj,~] = connect(reedsConnObj,startPose,goalPose);
+    test = pathSegObj{1};
+    type = [];
+    value = zeros(5,1);
+    for i = 1:5
+        type = [type,test.MotionTypes{i}];
+        value(i) = test.MotionDirections(i)*test.MotionLengths(i);
+    end
+    path = RSPath(type,value(1),value(2),value(3),value(4),value(5));
+end
\ No newline at end of file
diff --git a/src/HybridAStar.m b/src/HybridAStar.m
index 4340b83..99c7ce5 100644
--- a/src/HybridAStar.m
+++ b/src/HybridAStar.m
@@ -22,21 +22,24 @@
     while ~isempty(Open)
         % pop the least cost node from open to close
         [wknode,Open] = PopNode(Open,cfg);
-        [isok,idx] = inNodes(wknode,Close);
+        [isok1,idx] = inNodes(wknode,Close);
         
         % 判断是否在Close集合内
-        if isok
+        if isok1
             Close(idx) = wknode;
         else
             Close = [Close, wknode];
         end
 
         % 以wknode为根节点生成搜索树，使用Reeds-Shepp方法基于车辆单轨模型进行运动学解析拓展子结点
-        [isok,path] = AnalysticExpantion([wknode.x,wknode.y,wknode.theta],End,veh,cfg);
-        if  isok
+        [isok2,path] = AnalysticExpantion([wknode.x,wknode.y,wknode.theta],End,veh,cfg);
+        if  isok2
             %把wknode从idx移到Close集合最后面
-            Close(end+1) = wknode;
-            Close(idx) = [];
+            if isok1
+                Close(end+1) = wknode;
+                Close(idx) = [];
+            else
+            end
             [x,y,th,D,delta] = getFinalPath(path,Close,veh,cfg);
             break % 如果能直接得到RS曲线，则跳出while循环
         end
diff --git a/src/Parking.mp4 b/src/Parking.mp4
new file mode 100644
index 0000000..9c0ff56
Binary files /dev/null and b/src/Parking.mp4 differ