MathorCup 2022 C题无人驾驶泊车问题.2（若干论文阅读）

cuboid[center_: {0, 0}, dim_, radius_: 0] := Rectangle[center - dim/2, center + dim/2, RoundingRadius -> 0.01];
move2D[shape_, pose_] := Translate[Rotate[shape, pose[[3]], {0, 0}], pose[[1 ;; 2]]];
L = 1.64; 
\[Delta]max = 25 Degree ;
bicycle[pose_, \[Delta]_] := {
   rearWheel = cuboid[{0, 0}, {0.4, 0.1}, 0.1];
   frontWheel = move2D[rearWheel, {L, 0, \[Delta]}];
   trunk = cuboid[{L/2, 0}, {L, 0.02}, 0.1];
   move2D[{Blue, frontWheel, rearWheel, Black, trunk, Red, Circle[{L, 0}, 0.22, {0, \[Delta]}]}, pose]
   };
Manipulate[
 pose = Flatten@{p, \[Theta]};
 dirvec = AngleVector[\[Theta]];
 vertvec = {-dirvec[[2]], dirvec[[1]]};
 p1 = p + L*dirvec;
 dl = Norm[goal - p];
 \[Alpha] = VectorAngle[goal - p, {1, 0}] - \[Theta];
 \[Delta] = ArcTan[2*L*Sin[\[Alpha]]/dl];
 R = Abs[dl/2/Sin[\[Alpha]]];
 c = p + Sign[\[Alpha]]*R*vertvec;
 a1 = -VectorAngle[p - c, {1, 0}];
 a2 = -VectorAngle[goal - c, {1, 0}];
 Graphics[{bicycle[pose, \[Delta]], Point[c], AbsoluteThickness[1], 
   Line[{p1, p1 + AngleVector[\[Theta] + \[Delta]]*0.3}], AbsoluteDashing[{6, 3}], Black, Line[{p, p1 + dirvec*0.3}], Gray, Line[{p, c}], Line[{c, goal}], Line[{goal, p}], Line[{c, p1}], Orange, Circle[c, R(*,{a1,a2}*)], AbsolutePointSize[8], White, Point[p], Red, Point[c], Darker@Green, Point[goal], Red, Text[Style[ "\[Delta]=" <> ToString@Round[\[Delta]*180/Pi, 0.01] <> "\[Degree]", FontSize -> 16], p1 + dirvec*0.5], Text["\!\(\*SubscriptBox[\(d\), \(l\)]\)=" <> ToString@Round[dl, 0.01], (p + goal)/2 + {0, 0.1}]}, 
  ImageSize -> 600, PlotRange -> 1.5 {{-1.5, 1.5}, {-0.5, 1.5}}, 
  Axes -> False], {{p, {0, 0}}, Locator, Appearance -> Graphics@Point[{0, 0}]}, {{goal, {0.16, 0.12}}, Locator, Appearance -> Graphics[{Green, Point[{0, 0}]}]}, {{\[Theta], Pi/6}, 0, 2 Pi, 0.01}, TrackedSymbols :> True, Initialization :> {goal = {0.16, 0.12}}]

https://zhuanlan.zhihu.com/p/74380868

https://blog.csdn.net/robinvista/article/details/104582045

https://blog.csdn.net/robinvista/article/details/106279968

https://ww2.mathworks.cn/help/nav/ref/plannerhybridastar.html

https://github.com/karlkurzer/path_planner

https://github.com/ApolloAuto/apollo/tree/master/modules/planning/open_space

https://blog.csdn.net/robinvista/article/details/118538616

https://github.com/hbanzhaf/steering_functions

https://blog.csdn.net/muyilove/article/details/82346050