请详细解释:VehiSteer(xirand, Cveh, T, Pval) xnearest←Nearest(T, xirand) xnew = Kine Drive(xnearest, xirand, Cveh) σnew = Trajectory(xnew, xnearest) g(xnew) = g(xnearest) + c(xnew, xnearest) if ObstacleFree(σnew) then for xnear ∈ Xfeasible do C(xnew, xnear) = kdd + ka∑ki=1Δα + kP∑ki=1pi if g(xnew) > g(xnearest) + c(xnew, xnear) g(xnew) = g(xnearest) + c(xnew, xnear) xopt←xnear,σnew←Connection(xnear, xopt) V←V ∪ {xnew}, E←E ∪ {σnew} xopt←Parent(xnew) if xnew ∈ Xdes then xdes←xnew σ = Parental\_Nodes\_Connection(xdes, xstart, T) cdes = g(xdes) end if end if end for else next\_sample end if Return xnew, σnew
时间: 2023-05-29 12:01:16 浏览: 161
这是一段伪代码,描述了一个基础的无人车路径规划算法,以下是对每一行的解释:
1. VehiSteer(xirand, Cveh, T, Pval):函数名称及其输入参数。
2. xnearest←Nearest(T, xirand):找到最近的点xnearest。
3. xnew = KineDrive(xnearest, xirand, Cveh):通过运动学模型计算新的状态xnew。
4. σnew = Trajectory(xnew, xnearest):得到从xnearest到xnew的轨迹。
5. g(xnew) = g(xnearest):新状态的值赋值成与最近状态相同的值。
6. c(xnew, xnearest):从xnearest到xnew的代价。
7. if ObstacleFree(σnew) then:如果从xnearest到xnew的轨迹没有障碍物。
8. for xnear ∈ Xfeasible do:对于可行的周围状态xnear。
9. C(xnew, xnear) = kdd:代价更新。
10. ka∑ki=1Δα:α(可调节的权重因子)的代价。
11. kP∑ki=1pi:pi(路径分段的长度)的代价。
12. if g(xnew) > g(xnearest):如果新状态的值大于最近状态的值。
13. c(xnew, xnear):从xnearest到xnew的代价。
14. g(xnew) = g(xnearest):新状态的值赋值成最近状态的值。
15. c(xnew, xnear):从xnear到xnew的代价。
16. xopt←xnear, σnew←Connection(xnear, xopt):寻找当前最佳状态xopt和代价路径σnew。
17. V←V ∪ {xnew}, E←E ∪ {σnew}:将新状态和路径添加到图中的节点和边列表中。
18. xopt←Parent(xnew):将当前状态的父状态设置为xopt。
19. if xnew ∈ Xdes then:如果新状态在目标状态集合中。
20. xdes←xnew:将新状态设置为目标状态。
21. σ = Parental_Nodes_Connection(xdes, xstart, T):从目标状态到起始状态的代价路径。
22. cdes = g(xdes):从目标状态到起始状态的代价。
23. end if:if语句结束。
24. end if:if语句结束。
25. end for:for循环结束。
26. else next\_sample:如果从xnearest到xnew的轨迹有障碍物,那么选择下一个采样点。
27. end if:if语句结束。
28. Return xnew, σnew:返回新状态和代价路径。
阅读全文