CN114986498A - 一种移动操作臂协同控制方法 - Google Patents

Abstract

本发明的一种移动操作臂协同控制方法，其包括：建立四轮移动底盘和操作臂的运动学模型，联立获得移动操作臂运动学模型；利用移动操作臂运动学模型，完成相机内外参标定；采集目标物的位姿轨迹并完成坐标转换与投影平移处理；设计视觉伺服跟踪控制器，实现四轮移动底盘对目标物投影平移后的轨迹跟踪；设计PID和自抗扰视觉伺服抓取控制器，联立视觉伺服跟踪控制器构建联合控制系统以对移动操作臂进行协同控制。本发明控制方法有效解决了动态抓取过程中精度低、响应速度慢、抗干扰性差的问题，使其适用于新材料用智能装备制备材料过程的物料精准取送，实现了特殊工况条件下的物料自动控制。

Description

一种移动操作臂协同控制方法

技术领域

本发明属于智能装备控制技术领域，涉及一种移动操作臂协同控制方法。

背景技术

随着新材料产业的技术发展，在电子信息、医疗器械、航空航天和国防军工等领域对于不同构件所需要的成型材料种类和性能要求也各不相同，对传统制备装备和检测装备提出更高的要求。例如近年来快速发展的非晶合金材料，具有高强、高弹、高硬等诸多的优异性能，具有广阔的应用前景。由于非晶合金成型需要在高真空环境内快速凝固，所以制约非晶合金构件应用的瓶颈问题，就是缺乏非晶合金高端制备装备。此外国外对于高温合金定向凝固设备和粉体制备装备进行全面的技术封锁，制约了新材料的发展。

而新材料智能装备领域中，移动操作臂展现出了巨大的潜力和优势。然而，移动操作臂这种串联组合系统的协调规划与轨迹跟踪技术仍然存在不足，例如协调规划跟踪效率低、控制精度低、抗干扰性差等缺点，制约了移动操作臂在智能装备领域的应用。

常见的协同控制方法包括整体规划法和子系统规划法。整体规划法是将四轮移动底盘和操作臂看成一个完整的系统，考虑的时候将移动四轮移动底盘看成有条件限制的自由度表达，但是冗余自由度会导致移动操作臂关节空间解的无限性和奇异位形等问题。而子系统规划法是将四轮移动底盘和操作臂看成两个子系统来分别规划，让目标物处于操作臂的最佳操作区然后完成抓取，虽然这样能解决冗余自由度的问题，但是设立最佳操作区不适合于动态抓取。除了协调规划问题，常见的视觉伺服动态抓取控制方法也存在很多问题。滑膜控制适用于不确定系统，但是输入往往存在抖动；鲁棒控制能够补偿非结构与结构不确定性特点，但是系统上界的确定不容易，系统容易失稳；自适应神经网络结合输出反馈的控制方法，精确性较好，但是跟踪响应速度慢。因此，对于以上问题，急需提出一种具有精确、快速、抗干扰性好的移动操作臂协同控制方法。

发明内容

本发明的目的是提供一种移动操作臂协同控制方法，解决了动态抓取过程中精度低、响应速度慢、抗干扰性差的问题，从而更加适于实用。

本发明提供一种移动操作臂协同控制方法，包括以下步骤：

S1、建立四轮移动底盘的运动学模型和操作臂的运动学模型，联立获得移动操作臂的运动学模型，并设定初始参数；

S2、利用移动操作臂运动学模型，完成相机的内外参标定；

S3、利用相机采集目标物的位姿轨迹并完成坐标转换与投影平移处理；

S4、根据四轮移动底盘的运动模型设计基于模型预测控制的视觉伺服跟踪控制器，以实现四轮移动底盘对目标物投影平移后的轨迹跟踪；

S5、针对操作臂抓取设计PID和自抗扰视觉伺服抓取控制器，联立视觉伺服跟踪控制器构建联合控制系统以对移动操作臂进行协同控制。

在本发明的移动操作臂协同控制方法中，步骤S1包括：

S11、建立四轮移动底盘的运动学模型，四轮移动底盘定义在二维平面XWY平面，其中W代表世界坐标系原点，下式(1)是四轮移动底盘运动学方程：

上式中(x,y)是四轮移动底盘后轴中心位置坐标；

是四轮移动底盘与世界坐标系X轴的夹角；l为四轮移动底盘的车轮轴距，v为后轴当前速度，σ为四轮移动底盘航向与前轮转向之间的夹角；

定义状态变量为

控制量为u＝[v,σ]^T，对于在参考轨迹中的任意一点用带下标的s表示，把式(1)用f函数表示，利用泰勒级数展开并忽略高阶项完成线性化：

定义状态误差量

如下：

对式(3)进行前向欧拉离散化，其中T为采样周期，k、k+1分别为当前采样和后一采样时刻，整理可得

式(2)中雅克比矩阵带入上式(4)中可得离散线性化运动学方程：

S12、利用DH参数法建立操作臂运动学模型，用cθ_i来表示cosθ_i；用sθ_i来表示sinθ_i；cα_i-1代表cosα_i-1；sα_i-1代表sinα_i-1；

上式中i表示关节号，a_i-1为操作臂的关节轴i-1和关节轴i之间公垂线的连杆长度、α_i-1为关节轴i-1和关节轴i之间的的连杆转角、d_i-1为关节轴i-1上的连杆偏距，θ_i为关节轴i-1和关节轴i公共轴线旋转的关节角；

将UR5操作臂的DH参数表带入式(6)得出操作臂末端坐标系6相对操作臂基座坐标系0的变换矩阵：

S13、联立四轮移动底盘和操作臂建立移动操作臂运动学模型，定义M代表四轮移动底盘坐标系，W代表世界坐标系，S代表目标物坐标系；在操作臂末端带有二指夹爪，设四轮移动底盘坐标系M在世界坐标系W下的位置为^WP_M＝(^wx_M,^wy_M,^wz_M)，设四轮移动底盘坐标系M在世界坐标系W下的姿态角等于

则可得到四轮移动底盘坐标系M相对于世界坐标系W的转换矩阵

操作臂基座坐标系0在M坐标系下的坐标为^MP₀＝(^Mx₀,^My₀,^Mz₀)，定义^Mx₀＝e，^My₀＝0，^Mz₀＝h，于是得到操作臂基座坐标系0相对于四轮移动底盘坐标系M的变换矩阵：

通过

得到操作臂末端坐标系6在世界坐标系W中的位姿变换矩阵

设操作臂末端坐标系6相对于世界坐标系W的位置为^WP₆＝(^Wx₆,^Wy₆,^Wz₆)，姿态为^WΩ₆＝(^Wα₆,^Wβ₆,^Wγ₆)，则得到操作臂末端坐标系6相对于四轮移动底盘坐标系M的位置为：^MP₆＝(^Wx₆-^Wx_M,^Wy₆-^Wy_M,^Wz₆-^Wz_M)；操作臂基座与四轮移动底盘固连，得操作臂末端坐标系6相对于基座的坐标⁰P₆＝(^Wx₆-^Wx_M-l,^Wy₆-^Wy_M,^Wz₆-^Wz_M-h)，完成坐标变换后利用参数法逆解到关节空间完成位置逆解；

姿态的逆解就是操作臂六自由度的逆解，即Ω₆＝^WΩ₆＝(^Wα₆,^Wβ₆,^Wγ₆)，利用参数法逆解到关节空间便完成了姿态逆解。

在本发明的移动操作臂协同控制方法中，步骤S2包括：

S21、内参标定决定相机内部映射关系，通过从不同的角度拍摄已经打印好的黑白棋盘获得角点信息，提取亚像素角点信息，计算反向投影误差，完成内参标定；

S22、外参标定即求得相机坐标系相对于操作臂末端的工具坐标系的转换矩阵

。

在本发明的移动操作臂协同控制方法中，步骤S3具体为：

S31、定义工具坐标系E与操作臂末端坐标系6重合，即工具坐标系相对于操作臂末端坐标系6的转换矩阵为单位矩阵，即求得相机坐标系相对于操作臂末端坐标系6的转换矩阵

进而求得相机坐标系相对于世界坐标系的转换矩阵

S32、利用相机采集目标物的位姿轨迹L_C，根据相机坐标系相对于世界坐标系的转换矩阵将位姿轨迹L_C转换成在世界坐标系的位姿轨迹L_W；

S33、将目标物在世界坐标系的位姿轨迹L_W投影到四轮移动底盘后轮中心点坐标平面得到轨迹L₂，然后将投影后的轨迹L₂再根据四轮移动底盘膨胀半径和操作臂最小抓取半径沿着Y方向平移r，得到平移投影后的轨迹L_M，r是膨胀半径和最小抓取半径之和。

在本发明的移动操作臂协同控制方法中，步骤S4包括：

S41、根据步骤S1中已经建立的四轮移动底盘运动学模型，定义新的状态变量：

k-1、k、k+1分别为前一采样时刻、当前采样和后一采样时刻，新的控制量的变化量为：

则推导得到新的状态空间表达式：

定义输出方程为：

式中

表示维数为控制量个数的单位矩阵，此处为2；

表示维数为状态量个数的单位矩阵，此处为3；定义预测时域为N_P,预测时域为N_C，对式(10)逐步嵌套迭代可得：

对式(11)逐步嵌套迭代可得：

若定义：

则输出方程(13)可改写为：

Y＝SΨ(k)+ΘΔU (16)

S42、由于状态量是误差形式，定义输出参考值为：Y_s＝[0 0 0 0 0 0]，设

G＝SΨ(k)，其中

表示维数为N_P的单位矩阵，

表示克罗内克积，Q为半正定状态加权矩阵，R为正定的控制加权矩阵，两者取对角阵且都是根据实际要求自取的常量，Q元素变大意味着跟踪偏差更快趋近于0，R元素变大意味着控制输入越小；定义优化目标函数J＝(Y-Y_s)^TQ_Q(Y-Y_s)+ΔU^TR_RΔU，化简后去掉常数得：

J＝ΔU^T(Θ^TQ_QΘ+R_R)ΔU+2G^TQ_QΘΔU (17)

令Z＝Θ^TQ_QΘ+R_R，h＝G^TQ_QΘ，则求解最优化问题：

等价于求：

模型预测控制问题转变成了标准二次型规划问题，通过求目标函数最优化问题来得到控制量u＝[v,σ]^T的离散值，实现对平移投影后轨迹L_M的跟踪。

在本发明的移动操作臂协同控制方法中，步骤S5包括：

S51、联合控制系统的输入即相机采集目标物的位姿轨迹L_C，位姿轨迹L_C经过坐标变换和投影平移得到基于模型预测控制的视觉伺服跟踪控制器的待跟踪轨迹L_M，视觉伺服跟踪控制器的输出控制量为u＝[v,σ]^T；

S52、把相机采集目标物相对于世界坐标系W的位姿轨迹L_W与工具坐标系E相对于世界坐标系W的位姿轨迹的误差作为PID控制器的输入即：e(t)＝P_goal-P_ee，P_goal是相机采集目标物相对于世界坐标系W的位姿轨迹L_W的位姿离散值，P_ee是工具坐标系E相对世界坐标系W的位姿轨迹的位姿离散值，e(t)是位姿差值；定义K_p、K_i、K_d是比例积分微分系数，利用PID控制器得到末端输出速度量：

设V_ee(t)＝Z(V_eepid(t))，其中Z(X)代表自抗扰控制器的控制函数，V_ee(t)是整个PID和自抗扰视觉伺服抓取控制器的速度输出；利用雅克比矩阵J得到操作臂关节速度V_joint＝J(q)^-1V_ee；

S53、根据视觉伺服跟踪控制器的输出控制量为u＝[v,σ]^T和四轮移动底盘M坐标系相对于世界坐标系W的转换矩阵

得到四轮移动底盘坐标系M相对于世界坐标系W的实际轨迹；利用控制箱功能通过关节速度控制得到累计的关节角反馈，利用转换矩阵

和

得到工具坐标系E相对M坐标系的转换矩阵：

通过

得到工具坐标系E相对于世界坐标系W的转换矩阵

得到工具坐标系E相对于世界坐标系W的实际位姿轨迹，此实际位姿轨迹将与相机采集目标物相对于W坐标系的位姿轨迹误差作为PID控制器的输入；

S54、为了应对响应速度问题增加了卡尔曼滤波速度前馈部分，相机采集目标物的位姿轨迹L_C进行坐标变换和投影平移前先通过卡尔曼滤波处理。

本发明的一种移动操作臂协同控制方法，至少具有以下有益效果：

首先，针对四轮移动底盘提出了“投影平移跟踪”，设计了基于模型预测控制的“同姿异位”的视觉伺服跟踪控制器，有效解决了联合控制系统的协调规划问题。其次，设计了基于PID和自抗扰视觉伺服抓取控制器，还增加卡尔曼滤波速度前馈部分，增加了控制系统的精度和抗干扰性，卡尔曼滤波速度前馈加快了控制系统的响应速度，更加满足移动操作臂在动态抓取过程中对于时效性的高要求。本发明控制方法有效解决了动态抓取过程中精度低、响应速度慢、抗干扰性差的问题，适用于新材料用智能装备制备材料过程的物料精准取送，实现了特殊工况条件下的物料自动控制。

附图说明

图1为本发明设计的控制方法步骤流程图；

图2为本发明四轮移动底盘运动学模型图；

图3为本发明移动操作臂结构示意图；

图4为本发明四轮移动底盘轨迹跟踪示意图；

图5为本发明联合控制系统的控制框图；

图6为本发明增加了卡尔曼滤波速度前馈部分的联合控制系统的控制框图；

图7为本发明无扰动X方向位置跟踪阶跃响应仿真图；

图8为本发明有扰动X方向位置跟踪阶跃响应仿真图；

图9为本发明无噪声扰动下X方向位置跟踪斜坡响应仿真图；

图10为本发明白噪声扰动下X方向位置跟踪斜坡响应仿真图。

具体实施方式

下面结合附图和实施例子对本发明作进一步详细的说明。以使本领域的技术人员可以更好地理解本发明并能予以实施，但所举实施例不作为对本发明的限定。

本发明的一种移动操作臂协同控制方法，是一种基于MPC融合改进PID的移动操作臂联合控制方法，如图1所示，包括以下步骤：

S1、建立四轮移动底盘的运动学模型和操作臂的运动学模型，联立获得移动操作臂的运动学模型，并设定初始参数，具体包括以下步骤：

S11、如图2所示，建立四轮移动底盘的运动学模型，四轮移动底盘定义在二维平面XWY平面，其中W代表世界坐标系原点，下式(1)是四轮移动底盘运动学方程：

上式中(x,y)是四轮移动底盘后轴中心位置坐标；

是四轮移动底盘与世界坐标系X轴的夹角；l为四轮移动底盘的车轮轴距，v为后轴当前速度，σ为四轮移动底盘航向与前轮转向之间的夹角。

定义状态变量为

控制量为u＝[v,σ]^T，对于在参考轨迹中的任意一点用带下标的s表示，把式(1)用f函数表示，利用泰勒级数展开并忽略高阶项完成线性化：

定义状态误差量

如下：

对式(3)进行前向欧拉离散化，其中T为采样周期，k、k+1分别为当前采样和后一采样时刻，整理可得：

式(2)中雅克比矩阵带入上式(4)中可得离散线性化运动学方程：

S12、利用DH参数法建立操作臂运动学模型，为了简化数学描述，用cθ_i来表示cosθ_i；用sθ_i来表示sinθ_i；cα_i-1代表cosα_i-1；sα_i-1代表sinα_i-1；

上式中i表示关节号，a_i-1为操作臂的关节轴i-1和关节轴i之间公垂线的连杆长度、α_i-1为关节轴i-1和关节轴i之间的的连杆转角、d_i-1为关节轴i-1上的连杆偏距，θ_i为关节轴i-1和关节轴i公共轴线旋转的关节角。

将UR5操作臂的DH参数表带入式(6)得出操作臂末端坐标系6相对操作臂基座坐标系0的变换矩阵：

S13、联立四轮移动底盘和操作臂建立移动操作臂运动学模型，定义M代表四轮移动底盘坐标系，W代表世界坐标系，S代表目标物坐标系。在操作臂末端带有二指夹爪，设四轮移动底盘坐标系M在世界坐标系W下的位置为^WP_M＝(^wx_M,^wy_M,^wz_M)，设四轮移动底盘坐标系M在世界坐标系W下的姿态角等于

则可得到四轮移动底盘坐标系M相对于世界坐标系W的转换矩阵

操作臂基座坐标系0在M坐标系下的坐标为^MP₀＝(^Mx₀,^My₀,^Mz₀)，定义^Mx₀＝e，^My₀＝0，^Mz₀＝h，于是得到操作臂基座坐标系0相对于四轮移动底盘坐标系M的变换矩阵：

通过

得到操作臂末端坐标系6在世界坐标系W中的位姿变换矩阵

不妨设操作臂末端坐标系6相对于世界坐标系W的位置为^WP₆＝(^Wx₆,^Wy₆,^Wz₆)，姿态为^WΩ₆＝(^Wα₆,^Wβ₆,^Wγ₆)，于是可得到操作臂末端坐标系6相对于四轮移动底盘坐标系M的位置为：^MP₆＝(^Wx₆-^Wx_M,^Wy₆-^Wy_M,^Wz₆-^Wz_M)，由于操作臂基座与四轮移动底盘固连，于是可得操作臂末端坐标系6相对于基座的坐标⁰P₆＝(^Wx₆-^Wx_M-l,^Wy₆-^Wy_M,^Wz₆-^Wz_M-h)，完成坐标变换后就利用参数法逆解到关节空间便完成了位置逆解。

姿态由于考虑同姿态跟踪，于是姿态的逆解就是操作臂六自由度的逆解，即Ω₆＝^WΩ₆＝(^Wα₆,^Wβ₆,^Wγ₆)，利用参数法逆解到关节空间便完成了姿态逆解。

S2、利用移动操作臂运动学模型，完成移动操作臂中的相机的内外参标定，具体包括：

S21、内参标定决定相机内部映射关系，通过从不同的角度拍摄已经打印好的黑白棋盘获得角点信息，提取亚像素角点信息，计算反向投影误差，完成内参标定；

S22、外参标定即求得相机坐标系相对于操作臂末端的工具坐标系的转换矩阵

。

如图3所示，工具坐标系是E，目标物的坐标系定义为S，操作臂的基座坐标系是0，相机坐标系是C。因为相机固定在操作臂末端，所以

是固定不变的，则取两次测量，第一次测量为：

在操作臂移动到另一个位姿之后第二次测量为：

其中

和

分别是第一次和第二次目标物坐标系相对于0坐标系的转换矩阵；

和

分别是第一次和第二次工具坐标系相对于0坐标系的转换矩阵；

和

分别是第一次和第二次目标物坐标系相对于相机坐标系的转换矩阵。因为相机固定，则可得

可以借助Easy_handeye软件包来完成对

的求解。

S3、利用相机采集目标物的位姿轨迹并完成坐标转换与投影平移处理，如图4所示；

S31、定义工具坐标系E与操作臂末端坐标系6重合，即工具坐标系相对于操作臂末端坐标系6的转换矩阵为单位矩阵，即可求得相机坐标系相对于操作臂末端坐标系6的转换矩阵

根据步骤S13中已经求得的

便可求得相机坐标系相对于世界坐标系的转换矩阵

S32、利用深度摄像机D435i采集目标物O₁的位姿轨迹L_C，由于此位姿轨迹是相对于相机坐标系C，可根据步骤S22中已经求得的

将轨迹L_C转换到世界坐标系下，得到其在世界坐标系的位姿轨迹L_W，

S33、将目标物O₁投影于四轮移动底盘后轮中心点坐标平面得到O₂，目标物O₁在世界坐标系的位姿轨迹L_W投影到四轮移动底盘后轮中心点坐标平面得到轨迹L₂，然后将O₂根据四轮移动底盘膨胀半径和操作臂最小抓取半径沿着Y方向平移r到O₃得到平移投影后的轨迹L_M，其中r是膨胀半径和最小抓取半径之和。

S4、根据四轮移动底盘的运动模型设计基于模型预测控制的视觉伺服跟踪控制器，以实现四轮移动底盘对目标物投影平移后的轨迹的跟踪，具体包括：

S41、根据步骤S1中已经建立的四轮移动底盘运动学模型，定义新的状态变量：

定义k-1、k、k+1分别为前一采样时刻、当前采样和后一采样时刻，新的控制量的变化量为：

则推导得到新的状态空间表达式：

定义输出方程为：

式中

表示维数为控制量个数的单位矩阵，此处为2；

表示维数为状态量个数的单位矩阵，此处为3；定义预测时域为N_P,预测时域为N_C，对式(10)逐步嵌套迭代可得：

对式(11)逐步嵌套迭代可得：

若定义：

则输出方程(13)可改写为：

Y＝SΨ(k)+ΘΔU (16)

S42、由于状态量是误差形式，定义输出参考值为：Y_s＝[0 0 0 0 0 0]，设

G＝SΨ(k)，其中

表示维数为N_P的单位矩阵，

表示克罗内克积，Q为半正定状态加权矩阵，R为正定的控制加权矩阵，两者取对角阵且都是根据实际要求自取的常量，Q元素变大意味着跟踪偏差更快趋近于0，R元素变大意味着控制输入越小；定义优化目标函数J＝(Y-Y_s)^TQ_Q(Y-Y_s)+ΔU^TR_RΔU，化简后去掉常数得：

J＝ΔU^T(Θ^TQ_QΘ+R_R)ΔU+2G^TQ_QΘΔU (17)

令Z＝Θ^TQ_QΘ+R_R，h＝G^TQ_QΘ，则求解最优化问题：

等价于求：

模型预测控制问题转变成了标准二次型规划问题，标准二次型规划问题的解决有开源的函数，通过求目标函数最优化问题来得到控制量u＝[v,σ]^T的离散值，便实现对平移投影后的轨迹L_M的跟踪，四轮移动底盘的视觉伺服跟踪控制器设计完毕。

S5、针对操作臂抓取设计PID和自抗扰视觉伺服抓取控制器，联立视觉伺服跟踪控制器构建联合控制系统以对移动操作臂进行协同控制，具体包括：

S51、联合控制系统的输入即相机采集目标物的位姿轨迹L_C，位姿轨迹L_C经过坐标变换得到相对于世界坐标系W的位姿轨迹L_W，再经过投影平移得到基于模型预测控制的视觉伺服跟踪控制器的待跟踪轨迹L_M，视觉伺服跟踪控制器的输出控制量u＝[v,σ]^T；

S52、如步骤S51中已经得到的相机采集目标物相对于世界坐标系W的位姿轨迹L_W，把此位姿轨迹L_W与工具坐标系E相对于世界坐标系W的位姿轨迹的误差作为PID控制器的输入：e(t)＝P_goal-P_ee，P_goal是相机采集目标物相对于世界坐标系W的位姿轨迹L_W的位姿离散值；P_ee是工具坐标系E相对世界坐标系W的位姿轨迹的位姿离散值；e(t)是位姿差值，定义K_p、K_i、K_d是比例积分微分系数，利用PID控制器得到末端输出速度量：

不妨设V_ee(t)＝Z(V_eepid(t))，其中Z(X)代表自抗扰控制器的控制函数，V_ee(t)是整个PID和自抗扰视觉伺服抓取控制器的速度输出。雅克比矩阵通常用来反应关节速度和操作臂末端的笛卡尔速度关系，且可根据位姿信息求得，因此利用雅克比矩阵J可以输出操作臂关节速度V_joint＝J^-1V_ee，由于操作臂控制箱有关节速度控制功能，因此便可根据得到的关节速度控制操作臂运动。此部分即“相对静止”视觉伺服抓取控制器部分的设计。

S53、基础联合控制系统如图5所示，根据步骤S51和S52可以分别得到四轮移动底盘的控制量u＝[v,σ]^T和操作臂关节速度V_joint。经过自带的里程计累计计算得到四轮移动底盘坐标系M相对于世界坐标系W的转换矩阵

从而可以得到四轮移动底盘M坐标系运动相对于W坐标系的实际轨迹，而利用控制箱功能通过关节速度控制可以得到累计的关节角反馈，再利用步骤S12中的

可以得到末端6坐标系相对于0坐标系的位姿，再利用步骤S13中的

和步骤S22中的

可以得到工具坐标系E相对M坐标系的转换矩阵：

因此工具坐标系E相对于四轮移动底盘M坐标系的实际位姿轨迹也可得到。利用

和

可以得到工具坐标系E相对于世界坐标系W的转换矩阵：

于是可以得到工具坐标系E相对于世界坐标系W的实际位姿轨迹，如上面步骤S52中，而此实际位姿轨迹将与相机采集目标物相对于世界坐标系W的位姿轨迹误差作为PID控制器的输入。至此基础联合控制系统设计完毕。

S54、如图6所示，在基础联合控制系统的基础上，为了应对响应速度问题增加了卡尔曼滤波速度前馈部分。在相机采集目标物的位姿轨迹后，首先利用卡尔曼滤波与预测来得到目标物的位姿轨迹预测，预测的位姿轨迹接着坐标变换用于得到相机采集目标物相对于W坐标系的位姿轨迹，接下来PID和自抗扰视觉伺服抓取控制器的设计同步骤S52一样，而通过卡尔曼滤波与预测可以得到预测速度，把此目标物预测速度作为速度前馈同自抗扰控制器输出累加从而来得到末端工具速度。为了验证还在此累加部分和自抗扰控制器中间加一个噪声干扰N。至此，带卡尔曼滤波速度前馈的联合控制系统设计完毕。

为了验证本方法在求解路径规划问题的有效性，对比传统PID控制器和本发明提出的控制系统做两组仿真实验。系统环境为Windows 10操作系统和MATLAB 2016a。

第一组是进行阶跃信号输入实验，实际物理意义即操作臂抓取静止不动的物体。因为静止物体速度为0，因此没有考虑加上速度前馈。在操作臂基坐标系下本实验只考虑X方向给定固定位置2m，从0开始阶跃信号，对比有无速度扰动仿真，其中噪声干扰的干扰量是N＝0.5sin(t+1)，实验结果如图7和图8所示。对比可以看出位置跟踪，本发明设计的控制系统相比传统PID控制器响应速度更快，超调量更小。在有扰动情况下，本发明设计的控制系统抗干扰能力更强，稳定调节时间更短，虽有小的抖动，但位置基本在x＝2处附近，误差小能够保证在目标值附近。

第二组是设定目标匀速运动，即系统是斜坡输入，在该实验中把传统PID和本发明设计的控制系统对比，模拟目标物体以1m/s的速度沿X方向运动，初始位置在x＝1.5m处，在有无速度扰动情况下，让操作臂分别按照有无速度前馈来对比，仿真图如下图9和图10。传统PID、无速度前馈跟踪和有速度前馈跟踪基本都能跟踪上，传统PID需要3.2s，本发明的无速度前馈跟踪需要1.5秒，有速度前馈只需要0.1秒。但是都存在稳定误差，但是误差量极小，可以满足实际需求。对比这三种方式，发现带有速度前馈的响应更快，跟踪效果更好。抗扰性分析发现，若是加入白噪声，这三种方式都会出现不同程度的抖动，传统PID和无速度前馈跟踪控制器都逐渐不稳定，但是有速度前馈跟踪控制器的抗扰性更好，最终趋于稳定状态。

经过上述分析，证明了本发明算法的有效性。

以上所述仅为本发明的较佳实施例，并不用以限制本发明的思想，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (6)

1.一种移动操作臂协同控制方法，其特征在于，包括以下步骤：

S1、建立四轮移动底盘的运动学模型和操作臂的运动学模型，联立获得移动操作臂的运动学模型，并设定初始参数；

S2、利用移动操作臂运动学模型，完成相机的内外参标定；

S3、利用相机采集目标物的位姿轨迹并完成坐标转换与投影平移处理；

S4、根据四轮移动底盘的运动模型设计基于模型预测控制的视觉伺服跟踪控制器，以实现四轮移动底盘对目标物投影平移后的轨迹跟踪；

S5、针对操作臂抓取设计PID和自抗扰视觉伺服抓取控制器，联立视觉伺服跟踪控制器构建联合控制系统以对移动操作臂进行协同控制。

2.如权利要求1所述的移动操作臂协同控制方法，其特征在于，步骤S1包括：

S11、建立四轮移动底盘的运动学模型，四轮移动底盘定义在二维平面XWY平面，其中W代表世界坐标系原点，下式(1)是四轮移动底盘运动学方程：

上式中(x,y)是四轮移动底盘后轴中心位置坐标；

是四轮移动底盘与世界坐标系X轴的夹角；l为四轮移动底盘的车轮轴距，v为后轴当前速度，σ为四轮移动底盘航向与前轮转向之间的夹角；

定义状态变量为

控制量为u＝[v,σ]^T，对于在参考轨迹中的任意一点用带下标的s表示，把式(1)用f函数表示，利用泰勒级数展开并忽略高阶项完成线性化：

定义状态误差量

如下：

对式(3)进行前向欧拉离散化，其中T为采样周期，k、k+1分别为当前采样和后一采样时刻，整理可得

式(2)中雅克比矩阵带入上式(4)中可得离散线性化运动学方程：

S12、利用DH参数法建立操作臂运动学模型，用cθ_i来表示cosθ_i；用sθ_i来表示sinθ_i；cα_i-1代表cosα_i-1；sα_i-1代表sinα_i-1；

上式中i表示关节号，a_i-1为操作臂的关节轴i-1和关节轴i之间公垂线的连杆长度、α_i-1为关节轴i-1和关节轴i之间的的连杆转角、d_i-1为关节轴i-1上的连杆偏距，θ_i为关节轴i-1和关节轴i公共轴线旋转的关节角；

将UR5操作臂的DH参数表带入式(6)得出操作臂末端坐标系6相对操作臂基座坐标系0的变换矩阵：

S13、联立四轮移动底盘和操作臂建立移动操作臂运动学模型，定义M代表四轮移动底盘坐标系，W代表世界坐标系，S代表目标物坐标系；在操作臂末端带有二指夹爪，设四轮移动底盘坐标系M在世界坐标系W下的位置为^WP_M＝(^wx_M,^wy_M,^wz_M)，设四轮移动底盘坐标系M在世界坐标系W下的姿态角等于

则可得到四轮移动底盘坐标系M相对于世界坐标系W的转换矩阵

操作臂基座坐标系0在M坐标系下的坐标为^MP₀＝(^Mx₀,^My₀,^Mz₀)，定义^Mx₀＝e，^My₀＝0，^Mz₀＝h，于是得到操作臂基座坐标系0相对于四轮移动底盘坐标系M的变换矩阵：

通过

得到操作臂末端坐标系6在世界坐标系W中的位姿变换矩阵

设操作臂末端坐标系6相对于世界坐标系W的位置为^WP₆＝(^Wx₆,^Wy₆,^Wz₆)，姿态为^WΩ₆＝(^Wα₆,^Wβ₆,^Wγ₆)，则得到操作臂末端坐标系6相对于四轮移动底盘坐标系M的位置为：^MP₆＝(^Wx₆-^Wx_M,^Wy₆-^Wy_M,^Wz₆-^Wz_M)；操作臂基座与四轮移动底盘固连，得操作臂末端坐标系6相对于基座的坐标⁰P₆＝(^Wx₆-^Wx_M-l,^Wy₆-^Wy_M,^Wz₆-^Wz_M-h)，完成坐标变换后利用参数法逆解到关节空间完成位置逆解；

姿态的逆解就是操作臂六自由度的逆解，即Ω₆＝^WΩ₆＝(^Wα₆,^Wβ₆,^Wγ₆)，利用参数法逆解到关节空间便完成了姿态逆解。

3.如权利要求2所述的移动操作臂协同控制方法，其特征在于，步骤S2包括：

S21、内参标定决定相机内部映射关系，通过从不同的角度拍摄已经打印好的黑白棋盘获得角点信息，提取亚像素角点信息，计算反向投影误差，完成内参标定；

S22、外参标定即求得相机坐标系相对于操作臂末端的工具坐标系的转换矩阵

4.如权利要求3所述的移动操作臂协同控制方法，其特征在于，步骤S3具体为：

S31、定义工具坐标系E与操作臂末端坐标系6重合，即工具坐标系相对于操作臂末端坐标系6的转换矩阵为单位矩阵，即求得相机坐标系相对于操作臂末端坐标系6的转换矩阵

进而求得相机坐标系相对于世界坐标系的转换矩阵

S32、利用相机采集目标物的位姿轨迹L_C，根据相机坐标系相对于世界坐标系的转换矩阵将位姿轨迹L_C转换成在世界坐标系的位姿轨迹L_W；

S33、将目标物在世界坐标系的位姿轨迹L_W投影到四轮移动底盘后轮中心点坐标平面得到轨迹L₂，然后将投影后的轨迹L₂再根据四轮移动底盘膨胀半径和操作臂最小抓取半径沿着Y方向平移r，得到平移投影后的轨迹L_M，r是膨胀半径和最小抓取半径之和。

5.如权利要求1所述的移动操作臂协同控制方法，其特征在于，步骤S4具体为：

S41、根据步骤S1中已经建立的四轮移动底盘运动学模型，定义新的状态变量：

k-1、k、k+1分别为前一采样时刻、当前采样和后一采样时刻，新的控制量的变化量为：

则推导得到新的状态空间表达式：

定义输出方程为：

式中

表示维数为控制量个数的单位矩阵，此处为2；

表示维数为状态量个数的单位矩阵，此处为3；定义预测时域为N_P,预测时域为N_C，对式(10)逐步嵌套迭代可得：

对式(11)逐步嵌套迭代可得：

若定义：

则输出方程(13)可改写为：

Y＝SΨ(k)+ΘΔU (16)

S42、由于状态量是误差形式，定义输出参考值为：Y_s＝[0 0 0 0 0 0]，设

G＝SΨ(k)，其中

表示维数为N_P的单位矩阵，

表示克罗内克积，Q为半正定状态加权矩阵，R为正定的控制加权矩阵，两者取对角阵且都是根据实际要求自取的常量，Q元素变大意味着跟踪偏差更快趋近于0，R元素变大意味着控制输入越小；定义优化目标函数J＝(Y-Y_s)^TQ_Q(Y-Y_s)+ΔU^TR_RΔU，化简后去掉常数得：

J＝ΔU^T(Θ^TQ_QΘ+R_R)ΔU+2G^TQ_QΘΔU (17)

令Z＝Θ^TQ_QΘ+R_R，h＝G^TQ_QΘ，则求解最优化问题：

等价于求：

模型预测控制问题转变成了标准二次型规划问题，通过求目标函数最优化问题来得到控制量u＝[v,σ]^T的离散值，实现对平移投影后轨迹L_M的跟踪。

6.如权利要求1所述的移动操作臂协同控制方法，其特征在于，步骤S5具体为：

S51、联合控制系统的输入即相机采集目标物的位姿轨迹L_C，位姿轨迹L_C经过坐标变换和投影平移得到基于模型预测控制的视觉伺服跟踪控制器的待跟踪轨迹L_M，视觉伺服跟踪控制器的输出控制量为u＝[v,σ]^T；

S52、把相机采集目标物相对于世界坐标系W的位姿轨迹L_W与工具坐标系E相对于世界坐标系W的位姿轨迹的误差作为PID控制器的输入即：e(t)＝P_goal-P_ee，P_goal是相机采集目标物相对于世界坐标系W的位姿轨迹L_W的位姿离散值，P_ee是工具坐标系E相对世界坐标系W的位姿轨迹的位姿离散值，e(t)是位姿差值；定义K_p、K_i、K_d是比例积分微分系数，利用PID控制器得到末端输出速度量：

设V_ee(t)＝Z(V_eepid(t))，其中Z(X)代表自抗扰控制器的控制函数，V_ee(t)是整个PID和自抗扰视觉伺服抓取控制器的速度输出；利用雅克比矩阵J得到操作臂关节速度V_joint＝J(q)^-1V_ee；

S53、根据视觉伺服跟踪控制器的输出控制量为u＝[v,σ]^T和四轮移动底盘M坐标系相对于世界坐标系W的转换矩阵

得到四轮移动底盘坐标系M相对于世界坐标系W的实际轨迹；利用控制箱功能通过关节速度控制得到累计的关节角反馈，利用转换矩阵

和

得到工具坐标系E相对M坐标系的转换矩阵：

通过

得到工具坐标系E相对于世界坐标系W的转换矩阵

得到工具坐标系E相对于世界坐标系W的实际位姿轨迹，此实际位姿轨迹将与相机采集目标物相对于W坐标系的位姿轨迹误差作为PID控制器的输入；

S54、为了应对响应速度问题增加了卡尔曼滤波速度前馈部分，相机采集目标物的位姿轨迹L_C进行坐标变换和投影平移前先通过卡尔曼滤波处理。

Images