基于二次规划的空间避障算法

本文算法并非原创，侵删
参考论文：Nonsmooth Barrier Functions With Applications to Multi-Robot Systems

模型

基于二次规划的模型如下所示，避障时只需调用二次规划的相关函数求解每次运动的速度即可：

目标函数保证物体向着目标方向移动，约束条件保证不撞到障碍物，决策变量为下一时刻的速度

其中P为机器人当前位置，Paim为机器人目标位置。Dab为机器人与障碍物之间的距离，hab=Dab-D称之为安全余量，α为调节因子，用来调节安全余量和斥力之间得关系。

下面详细讲解如何通过二次规划实现避障方法，机器人避障过程中的示意图如下：

将上述条件带入，可以求得目标函数为：

vx,vy分别代表了机器人当前位置和目标位置在x、y方向上得矢量长度，在每一次控制过程中，其是固定值，对-(vnxvx+vnyvy)求最小值，也就意味着对vnx,vny求最大值。即希望在每次控制过程中使机器人尽可能的向着目标方向前进。

而用1/2（vnx2+vny2）求最小值，是不希望vn 过大，使其越过障碍物。

将A变换后得：A=[cos0,sin0]，因此，二次规划得约束条件为:

变换得：

由上图可得，

因此，这个约束条件的意义在于，保证在一定时间内，机器人与障碍物之间的距离保持一定的安全余量。例如，控制周期为1ms，α=10，则可以保证在当前的vn下，经过100个控制周期才会使安全余量为0.

仿真结果

在每一个控制周期求解下一时刻速度，即可达到避障的效果：

因为我放置了两个障碍物，所以走了个S形