一.整体思路
1.基于速度控制运动模型
假设没有噪声，控制时间间隔为内机器人速度v和角速度w保持不变，则机器人围绕半径为r的圆周运动。
其中，。
根据v 和w即可预测出下一次机器人位姿（x、y、），根据此也可以判断机器人是否撞到障碍物。
2.构建可行的速度空间
其中，坐标系x轴为角速度w，y轴为线速度v。假如给定机器人v和w，那么机器人在运动过程中就有可能碰到障碍物，也就是图中wall。
不同的速度指令（v，w）会得到不同的运动半径，同样的时间间隔到达不同的终止位置。有些位置是安全的，有些会与障碍物发生碰撞。
其中方块1即可推倒出方块2 。
3.速度窗口空间
考虑到机器人在运动过程中最大加速度的约束，在当前速度配置处以固定的小时间间隔开一个速度窗口空间。也就是如下红色方块。
我们即可得到新的机器人速度约束，如下：
4.可行速度空间
在根据整个速度（v，w）上下限（如下红色方块）， 可以获得新的机器人速度约束：
速度约束： 
结合上述三种速度约束（），我们即可获得可行速度空间，如下：
5.在可行速度空间中选择最优的速度控制指令
根据如下评价函数，选择最优的速度（v，w）：
6.参考
自主移动机器人_浙江大学_中国大学MOOC(慕课)
