[发明专利]一种物流搬运设备S曲线加减速速度规划与控制方法

权利要求书

1.一种物流搬运设备S曲线加减速速度规划与控制方法，其特征在于：包括以下步骤：

步骤1：获取目标位置，根据目标位置和约束条件规划速度曲线段，确定S曲线形式；如果S曲线为四段S曲线，则通过二分迭代法重新规划速度曲线段，执行步骤2；否则根据确定的S曲线形式执行步骤2；

步骤2：计算S曲线各速度曲线段分段点的位移和速度；

步骤3：根据位置反馈和分段位移比较，确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制；

所述确定所在的速度曲线段，对速度曲线进行插补输出，同时对位移进行闭环比例控制包括以下过程：

采用中点插补算法对曲线段进行插补，即在速度曲线上的两个点(T_N,V_N)和(T_N+2,V_N+2)中增加中点插补点(T_N+1,V_N+2)，设运动过程的总位移S_Sum＝2(S_Low+S_Mid+S_High)+S_V+S_Final；依次执行(1)、(2)、(3)、(4)、(5)、(6)、(7)、(8)、(9)；

(1)当S＞2(S_Mid+S_High)+S_Low+S_V+S_Final时，为加加速段，其中：

设定位移

输出速度t∈[0,T_Ramp]；

(2)当不符合(1)条件，且S＞2S_High+S_Mid+S_Low+S_V+S_Final时，为匀加速段，其中：

设定位移

输出速度V_t＝v₁+a_maxt+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

Kp为位置环增益；

(3)当不符合(1)和(2)条件，且S＞S_High+S_Mid+S_Low+S_V+S_Final时，为减加速段，其中：

设定位移

输出速度t∈[T_Ramp,0]；

(4)当不符合(1)-(3)条件，且S＞S_High+S_Mid+S_Low+S_Final时，为匀速段，其中：

设定位移S_t＝S_Low+S_Mid+S_High+v₃t；

输出速度V_t＝v₃+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(5)当不符合(1)-(4)条件，且S＞S_Mid+S_Low+S_Final时，为加减速段，其中：

设定位移

输出速度t∈[0,T_Ramp]；

(6)当不符合(1)-(5)条件，且S＞S_Low+S_Final时，为匀减速段，其中：

设定位移

输出速度V_t＝v₅-a_maxt+(S_t-S_Sum+S)K_P，t∈[0,T_Ramp]；

(7)当不符合(1)-(6)条件，且S＞S_Final时，为减减速段，其中：

设定位移

输出速度t∈[T_Ramp,0]；

(8)当不符合(1)-(7)条件，且S≤S_Final时，为低速定位段，其中：

输出速度V_t＝v_s；

(9)当实际位置与目标位置偏差绝对值满足给定的允许误差时，输出速度V_t＝0，运动过程结束；否则返回(1)。