方法示意图： 控制率公式： 其中theta是当前航向角与路径航向角之差，e为横向误差，v为车辆速度，lambda为控制参数。 算法步骤如下： 1. 根据当前定位结果找到路径最邻近点。 2. 计算该点与定位结果横向误差e与航线误差theta。 3. 根据控制率公式计算出前轮 ...

首先要确定反馈量和控制量，这里反馈量用的是车和最近路径点的横向误差，控制量用的是前轮转角。 算法如下： 1. 根据当前定位结果找到路径最邻近点。 2. 计算该点与定位结果横向误差。 3. 以该误差作为反馈测量值通过pid生成控制量即前轮转角。 4. 将前轮转角转化为航向角，带入运动模型 ...

最近在看二次规划方法，对于等式约束的二次规划问题，可以使用拉格朗日方法求解。 推导方法如《最优化理论与算法（第2版）》书上所述： 这里代码如下（代码中给了三个例子）： 结果如下： 图中红线为约束条件，曲面为待求解问题函数，红点为问题的解，蓝点为二次规划问题最小值 ...

可以根据状态量(位置，速度，加速度)的起始和结束值列出6个方程，组成方程组解该问题。 1. 列出起始状态： 2. 列出终止状态： 3. 写成矩阵形式： 求解c即可。 下面是从横向-5米到5米的生成的路径。 代码如下： 结果如下： ...

二次规划： 目标函数是决策变量的二次函数，约束条件是线性函数。 二次规划标准模型： \[min\quad f=\frac{1}{2}X^THX+C^TX \] \[s.t.\begin{cases} \quad AX\leq b\\ Aeq\cdot X=beq\\ L ...

HSoptflow.m main.m 老外写的函数，拿来研究研究。 ...

1.线性规划问题 如果目标函数和约束条件都是线性函数，则该模型称为线性规划。 [x,f_opt,flag,c]=linprog(f,A,B,Aeq,Beq,xm,xM,x0,opt) 参数说明： X: 解 f_opt: 最优值 Flag：大于零表示求解成功，否则求解出问题 C ...

该方法也是一种路径规划算法，不过障碍物过多的时候建立势场可能比较耗时，而且容易陷入局部最优。 算法流程如下： 1. 对于栅格场景中每一个像素分别计算到终点的距离，距离越大，则对该像素赋值越大，结束得到引力场。 2. 对于栅格场景中每一个像素分别计算到所有障碍物的距离，距离越大，则对该像素赋值 ...