[发明专利]基于模型预测阻抗控制的双机器人同步加工方法及系统

权利要求书

1.一种基于模型预测阻抗控制的双机器人同步加工方法，其特征在于，包括：

S1，以所述双机器人的交互力力矩和运动力矩为输入变量，以关节角度和关节角速度为状态变量，以各所述机器人在三维空间中的末端位姿、末端速度和末端所受的外力为输出变量，基于动力学方程建立双机器人的状态空间控制模型，所述状态空间控制模型为：

τ＝τ_u+τ_v

其中，x₁为双机器人系统的关节角度状态矩阵变量，x₂为双机器人系统的关节角速度状态矩阵变量，和分别为x₁和x₂的导数；M(x₁)为双机器人系统的惯量矩阵，M^-1(x₁)为M(x₁)的逆矩阵；τ为双机器人系统的关节合力矩阵，τ_u和τ_v分别为交互力力矩和运动力矩；J(x₁)为双机器人系统的雅可比矩阵，J^T(x₁)为J(x₁)的转置矩阵；F(x₁,x₂)为双机器人系统的外力矩阵；C(x₁,x₂)为双机器人系统的科氏力矩阵；G(x₁)为双机器人系统的重力矩阵；f为双机器人系统的关节摩擦力矩阵；

所述关节摩擦力矩阵为：

f＝(f₁,f₂)

其中，f_i为第i机器人的关节摩擦力，i＝1,2；f_a,j、f_b,j、f_c,j分别为第一摩擦力系数、第二摩擦力系数和第三摩擦力系数，用于修正摩擦力模型与实测摩擦力之间的对应关系；为的第i机器人的关节角速度；

S2，根据当前时刻各所述机器人的状态变量和输出变量，利用自适应变阻尼控制方法计算交互力力矩值，并将所述状态空间控制模型中的交互力力矩更新为所述交互力力矩值，所述交互力力矩值为：

Γ(t)＝Γ(t-T)+α(F₁(t-T)-F₂(t-T))/D_h

其中，τ_u,i为第i机器人的交互力力矩值，i＝1,2；J_i为第i机器人的雅可比矩阵，为J_i的转置矩阵；K_h为机器人的刚度系数，P_i为第i机器人的末端位姿；P_i^r为第i机器人的末端参考位姿；D_h为机器人的阻尼系数，ΔD为阻尼系数的变化量；V_i为第i机器人的末端速度；V_i^r为第i机器人的末端参考速度；Γ(t)为积分函数，t为当前时刻；为e_x估计值的导数，e_x为双机器人系统末端参考位姿与末端位姿之间的偏差，δ为预设系数；T为采样周期；α为力反馈系数；F₁(t-T)为第一机器人在t-T时刻所受的外力，F₂(t-T)为第二机器人在t-T时刻所受的外力；

S3，以各所述机器人对应的输出变量相同为优化目标，联合所述输出变量满足的状态约束和输入约束，利用非线性模型预测控制算法建立带约束的优化问题，以求解所述状态空间控制模型中的运动力矩，所述优化目标为：

其中，J(x(t),τ_v(t))为代价函数，x(t)和τ_v(t)分别为所述状态空间控制模型中的状态变量和运动力矩；T_p为预测步长；Ψ(x_e(s),τ_v(s))为滚动代价项，x_e(s)为误差状态，s为积分时间变量；Φ(x_e(t+T_p),τ_v(t+T_p))为终端代价项；

所述滚动代价项和终端代价项分别为：

Φ(x_e(t+T_p),τ_v(t+T_p))＝x_e(t+T_p)Hx_e(t+T_p)^T

其中，x_e(s)＝(e_P(s),e_V(s),e_F(s),e_i,P(s),e_i,V(s),e_i,F(s))，e_P(s)、e_V(s)、e_F(s)分别为双机器人系统的位姿偏差、速度偏差和所受力偏差，e_i,P(s)为第i机器人的末端位姿与末端参考位姿的偏差，e_i,V(s)为第i机器人的末端速度与末端参考速度的偏差，e_i,F(s)为第i机器人的末端所受外力与末端参考外力的偏差，为x_e(s)的转置矩阵；Q为状态系数矩阵，R为输入系数矩阵，H为终端状态矩阵；

所述状态约束和输入约束为：

其中，x为状态变量，为双机器人的状态约束集合，为第i机器人的参考雅克比矩阵，J_i为第i机器人的雅克比矩阵，Λ为速度精度矩阵，为双机器人系统的终端状态约束集合，W为输出目标变换矩阵，为雅克比矩阵J(x₁)的上界，为极限输入力矩，为科氏力矩阵C(x₁,x₂)的上界，为重力矩阵G的上界，为惯量矩阵M(x₁)的上界，为最大末端加速度，K^ξ为稳定性系数矩阵，为输入约束集合；

S4，分别将各所述交互力力矩与所述运动力矩叠加以得到相应的合力矩，根据所述合力矩中对应于各所述机器人的力矩值驱动相应的机器人。

2.一种基于模型预测阻抗控制的双机器人同步加工系统，其特征在于，包括：

模型建立模块，用于以所述双机器人的交互力力矩和运动力矩为输入变量，以关节角度和关节角速度为状态变量，以各所述机器人在三维空间中的末端位姿、末端速度和末端所受的外力为输出变量，基于动力学方程建立双机器人的状态空间控制模型，所述状态空间控制模型为：

τ＝τ_u+τ_v

其中，x₁为双机器人系统的关节角度状态矩阵变量，x₂为双机器人系统的关节角速度状态矩阵变量，和分别为x₁和x₂的导数；M(x₁)为双机器人系统的惯量矩阵，M^-1(x₁)为M(x₁)的逆矩阵；τ为双机器人系统的关节合力矩阵，τ_u和τ_v分别为交互力力矩和运动力矩；J(x₁)为双机器人系统的雅可比矩阵，J^T(x₁)为J(x₁)的转置矩阵；F(x₁,x₂)为双机器人系统的外力矩阵；C(x₁,x₂)为双机器人系统的科氏力矩阵；G(x₁)为双机器人系统的重力矩阵；f为双机器人系统的关节摩擦力矩阵；

所述关节摩擦力矩阵为：

f＝(f₁,f₂)

其中，f_i为第i机器人的关节摩擦力，i＝1,2；f_a,j、f_b,j、f_c,j分别为第一摩擦力系数、第二摩擦力系数和第三摩擦力系数，用于修正摩擦力模型与实测摩擦力之间的对应关系；为的第i机器人的关节角速度；

交互力力矩计算及更新模块，用于根据当前时刻各所述机器人的状态变量的值和输出变量的值，利用自适应变阻尼控制方法计算交互力力矩值，并将所述状态空间控制模型中的交互力力矩更新为所述交互力力矩值，所述交互力力矩值为：

Γ(t)＝Γ(t-T)+α(F₁(t-T)-F₂(t-T))/D_h

其中，τ_u,i为第i机器人的交互力力矩值，i＝1,2；J_i为第i机器人的雅可比矩阵，为J_i的转置矩阵；K_h为机器人的刚度系数，P_i为第i机器人的末端位姿；P_i^r为第i机器人的末端参考位姿；D_h为机器人的阻尼系数，ΔD为阻尼系数的变化量；V_i为第i机器人的末端速度；V_i^r为第i机器人的末端参考速度；Γ(t)为积分函数，t为当前时刻；为e_x估计值的导数，e_x为双机器人系统末端参考位姿与末端位姿之间的偏差，δ为预设系数；T为采样周期；α为力反馈系数；F₁(t-T)为第一机器人在t-T时刻所受的外力，F₂(t-T)为第二机器人在t-T时刻所受的外力；

运动力矩求解模块，用于以各所述机器人对应的输出变量相同为优化目标，联合所述输出变量满足的状态约束和输入约束，利用非线性模型预测控制算法建立带约束的优化问题，以求解所述状态空间控制模型中的运动力矩，所述优化目标为：

其中，J(x(t),τ_v(t))为代价函数，x(t)和τ_v(t)分别为所述状态空间控制模型中的状态变量和运动力矩；T_p为预测步长；Ψ(x_e(s),τ_v(s))为滚动代价项，x_e(s)为误差状态，s为积分时间变量；Φ(x_e(t+T_p),τ_v(t+T_p))为终端代价项；

所述滚动代价项和终端代价项分别为：

Φ(x_e(t+T_p),τ_v(t+T_p))＝x_e(t+T_p)Hx_e(t+T_p)^T

其中，x_e(s)＝(e_P(s),e_V(s),e_F(s),e_i,P(s),e_i,V(s),e_i,F(s))，e_P(s)、e_V(s)、e_F(s)分别为双机器人系统的位姿偏差、速度偏差和所受力偏差，e_i,P(s)为第i机器人的末端位姿与末端参考位姿的偏差，e_i,V(s)为第i机器人的末端速度与末端参考速度的偏差，e_i,F(s)为第i机器人的末端所受外力与末端参考外力的偏差，为x_e(s)的转置矩阵；Q为状态系数矩阵，R为输入系数矩阵，H为终端状态矩阵；

所述状态约束和输入约束为：

其中，x为状态变量，为双机器人的状态约束集合，为第i机器人的参考雅克比矩阵，J_i为第i机器人的雅克比矩阵，Λ为速度精度矩阵，为双机器人系统的终端状态约束集合，W为输出目标变换矩阵，为雅克比矩阵J(x₁)的上界，为极限输入力矩，为科氏力矩阵C(x₁,x₂)的上界，为重力矩阵G的上界，为惯量矩阵M(x₁)的上界，为最大末端加速度，K^ξ为稳定性系数矩阵，为输入约束集合；

驱动模块，用于分别将各所述交互力力矩与所述运动力矩叠加以得到相应的合力矩，根据所述合力矩中对应各所述机器人的力矩值驱动相应的机器人。