[发明专利]一种抗未知扰动的全向移动机器人编队滚动优化控制方法

权利要求书

1.一种抗未知扰动的全向移动机器人编队滚动优化控制方法，其特征在于，包括下述步骤：

准备工作；建立模型及初始化；迭代计算；输出结果，

所述的准备工作包括设定或输入下列参数：总运行时间为T，采样间隔为t_s，设定每个机器人的质量为m_b，车体的长为2a，宽为2b，车体的转动惯量为I_b，每个轮子的转动惯量为I_w，每个轮子的半径为R_w,，控制输入范围[u_min u_max]，通信范围R_d，临界避碰距离R_coll以及临界避障距离R_obs，参考轨迹为x_r，设定编队中机器人个数为M，期望的队形图案为P^d，设置滚动优化控制的预测时域为N_p，加权矩阵为Q、R，设置粒子群优化求解器的种群数量N_pop，最大迭代次数t_pmax、最小惯性权重系数ω_pmin∈(0,1)、最大惯性权重系数ω_pmax∈(ω_pmin,1)，设定反馈补偿增益K₁、K₂；上述信息对于后续计算来说均为已知量，部分参数的数值为外部直接输入，部分参数为提前计算得到的结果，

所述的建立模型及初始化包括下述步骤：

步骤一：根据期望编队图案P^d，每个机器人自身状态x_i，参考轨迹x_r，考虑避碰与避障约束，构建多领航者-跟随者编队的数学模型，得到每个机器人的期望状态向量其中每个机器人自身状态x_i，参考轨迹x_r均为外部输入；

步骤二：根据行为分解建立机器人运动学模型，根据拉格朗日方程建立机器人动力学模型，在此基础上，构建运动学-动力学理想一体化模型，并将此模型作为滚动优化控制的非线性预测模型，更进一步，考虑模型中存在的外部扰动与建模不确定性，建立未知扰动下的实际一体化模型；

步骤三：设定初始时刻k＝0，初始化每个机器人i在全局坐标系X_aO_aY_a在下的起始状态量x_i(0)，起始速度量

所述的迭代计算包括下述步骤：

步骤四：每个机器人i接收通信范围R_d内每个邻居机器人j的假设预测控制输入序列并将自己的假设预测控制输入序列发送给通信范围R_d内的所有邻居机器人；

步骤五：每个机器人i探测通信范围R_d内是否存在障碍物，若存在则保存每个障碍物j的最近点p_obs,j；

步骤六：每个机器人i计算自己在[k,k+N_p]内的预测期望参考状态并结合步骤二中的预测模型，建立分布式滚动优化控制框架；

步骤七：每个机器人结合粒子群优化器，独立求解k时刻的滚动优化问题，得到该时刻自身的最优预测控制输入序列

所述的步骤七包括下述具体执行过程：

步骤八：设粒子群算法迭代次数t_p＝1，每个机器人初始化N_pop个粒子，每个粒子的位置对应一个预测控制输入序列，每一个粒子的维度为4×N_p，设定搜索范围为[u_min u_max]，搜索速度限制[v_min v_max]，随机确定每个粒子的速度和位置，v_min v_max的值由外部输入；

步骤九：每个机器人计算每个粒子的代价函数J_id,id＝1,...,N_pop，将当前粒子位置记为pbest_id(t_p)，将最优粒子的位置记为gbest(t_p)，所描述的最优粒子是整个种群中代价函数值最小的粒子；

步骤十：更新种群中所有粒子的位置和速度，将位置限幅在[u_min u_max]，将速度限幅在[v_min v_max]

步骤十一：每个粒子将更新后的代价函数值与存储在pbest_id(t_p)中的粒子代价函数值进行比较，将代价函数值更小的解更新到局部最优解pbest_id(t_p+1)，并从中选取最优粒子更新得到全局最优解gbest(t_p+1)，即更新得到目前搜索到的最优预测控制输入序列；

步骤十二：令t_p＝t_p+1，判断是否满足t_p＞t_pmax，满足则停止搜索并保存全局最优解gbest(t_p+1)，将该最优解存入最优预测控制输入序列否则返回步骤十继续搜索，

所述的迭代计算还包括下述步骤：

步骤十三：每个机器人将带入步骤二中的理想一体化模型并更新得到下一时刻的最优状态估计量和最优轮子角速度估计量

步骤十四：每个机器人将中的第一项带入步骤二中的实际一体化模型，得到下一时刻的真实状态估计量和真实轮子角速度估计量

步骤十五：每个机器人i通过双闭环反馈补偿控制器，得到补偿控制输入u_c,i，进而得到实际控制输入u_a,i，并将其带入步骤二中的理想一体化模型，更新状态量x_i(k+1)和速度量

所述的输出结果包括下述步骤：

令k＝k+1，重复步骤四～步骤十五，判断是否满足满足则编队停止移动。

2.如权利要求1所述的一种抗未知扰动的全向移动机器人编队滚动优化控制方法，其特征在于：

其中，准备工作中，x_r＝[x_t,y_r,θ_r]^T为机器人编队的参考轨迹状态向量，x_r、y_r、θ_r分别为参考轨迹在全局坐标系X_aO_aY_a下的X轴坐标，Y轴坐标和方向角，

其中，准备工作中，期望的队形图案为：

P^d＝[h₁,h₂,...,h_i]i＝1,...,M    (1)

上式中，h_i＝[h_x,i,h_y,i,0]^T为机器人i的期望队形向量，h_x,i和h_y,i分别为机器人i在期望队形中的X轴坐标和Y轴坐标，

其中，步骤一中，机器人i的自身状态为x_i＝[x_i,y_i,θ_i]^T，x_i、y_i、θ_i分别为机器人i在X_aO_aY_a下的X轴坐标、Y轴坐标和方向角，期望状态向量其计算如下所示：

上式中，N_i为机器人i在通信范围R_d内的邻居集合，N_i为机器人i在R_d内的邻居机器人个数，d_i为是否能感知到参考轨迹，对于领航机器人，d_i＝1；对于跟随机器人，d_i＝0，

其中，步骤一中，编队模型定义如下：

上式中，p_i＝[x_i,y_i]^T为机器人i的位置向量，||p_i-p_j||＞R_coll与||p_i-p_obs,j||＞R_obs为距离状态约束，保证了个体间的避碰以及避障，

其中，步骤二中，全向移动机器人i的运动学模型如下所示：

上式中，为机器人坐标系X_bO_bY_b到世界坐标系X_aO_aY_a的旋转矩阵，为惯性矩阵，为机器人i在X_aO_aY_a下的速度向量，表示四个轮子的角速度，a和b分别为车体的半长轴和半短轴长度，

其中，步骤二中，全向移动机器人i的动力学模型如下所示：

上式中，为惯性矩阵，为轮子的粘滞摩擦系数矩阵，λ₁～λ₄为每个轮子的粘滞摩擦系数，u_i＝[u_i1,u_i2,u_i3,u_i4]^T为四个电机的输出力矩向量，

其中，步骤二中，全向移动机器人i的理想一体化模型如下所示：

上式中，J为J⁺的伪逆矩阵，

其中，步骤二中，不确定性干扰下的实际一体化模型如下所示：

上式中，ΔJ,ΔM,ΔD为模型不确定性项，u_d为输入扰动，f_d为包含静摩擦力的外部总扰动，

其中，步骤四中，机器人i的假设预测控制输入序列如下所示：

上式中，k+s|k表示从当前时刻k到未来时刻k+s的预测控制输入，

其中，步骤六中，滚动优化方程如下所示：

上式中，J_i为机器人i的代价函数，x_i(k+s+1|k)＝f(x_i(k+s|k),u_i(k+s|k))表示基于模型对状态值的预测，为邻居机器人j的假设预测位置向量，U为可行控制输入集合，Q和R为半正定对称矩阵，为k+s+1预测时刻的期望状态向量，表示如下：

其中，步骤七中，机器人i得到预测时域N_p内的最优预测控制输入序列如下所示：

其中，步骤九和步骤十一中，pbest_id(t_p)＝[pbest_i1,pbest_i2,…,pbest_iD]表示截止至第t_p代i(i＝1,2,…,N_pop)粒子的最优解，即截止至第t_p代i粒子对应的最优预测控制输入序列，gbest_id(t_p)＝[gbest_i1,gbest_i2,…,gbest_iD]表示截止至第t_p代的全局最优解，即截止至第t_p代的适应度值最小的粒子i对应的最优预测控制输入序列，

其中，步骤十中，粒子的位置和速度更新公式如下所示：

上式中c₁∈[1,2]、c₂∈[1,2]表示加速度常数，ω_p表示惯性权重，rand()表示(0,1)之间的随机数，惯性权重ω_p计算公式如下：

其中，步骤十三中，下一时刻的最优轮子角速度估计量计算如下：

其中，步骤十四中，补偿控制输入u_c,i计算如下所示：

其中，步骤十四中，实际控制输入如下所示：