[发明专利]基于全状态阻抗控制器的移动操作机器人通用控制方法

权利要求书

1.基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，包括以下步骤：

步骤1、建立移动操作机器人的运动学模型和动力学模型，所述移动操作机器人包括移动底盘(1)和机械臂(2)；

步骤2、在步骤1建立的运动学模型和动力学模型基础上，建立带权重矩阵的全状态阻抗控制器；

步骤3、根据目标物体(3)与机械臂(2)末端之间的距离，选定移动操作机器人的工作模式，并给定选定的移动操作机器人的工作模式下对应的权重矩阵的值，将所述权重矩阵的值带入步骤2得到的带权重矩阵的全状态阻抗控制器中计算，得到机械臂(2)的期望关节力矩和移动底盘(1)的虚拟力矩；

步骤4、将步骤3得到移动底盘(1)的虚拟力矩加上扰动力矩，经过计算得到移动底盘(1)的速度指令，并发送给底层速度控制器；将步骤3得到的机械臂(2)的期望关节力矩加上扰动力矩，发送给机械臂底层力矩控制器。

2.根据权利要求1所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤1中，建立的移动操作机器人的运动学模型为：

式中，为机械臂末端坐标系在世界坐标系下的位姿描述矩阵，为底盘坐标系在世界坐标系下的位姿描述矩阵，为机械臂基座坐标系在底盘坐标系下的位姿描述矩阵，为机械臂末端坐标系在基座标系下的位姿描述矩阵；为底盘坐标系在世界坐标系下的旋转矩阵，为机械臂末端坐标系在基座标系下的旋转矩阵；r＝(r_x,r_y,r_θ)为移动底盘(1)的3个自由度，三个元素分别代表底盘在x，y方向的位置和与x轴的夹角；(x₀,y₀,z₀)依次为机械臂基座标系原点在底盘坐标系x,y,z方向上的位置；(x_q,y_q,z_q)依次为机械臂末端在机械臂基座标系x,y,z方向上的位置；q＝(q₁～q₇)为机械臂的7个自由度，分别表示机械臂7个关节位置。

3.根据权利要求1所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤2中，带权重矩阵的全状态阻抗控制器基于移动操作机器人整体的动力学模型结合阻抗控制原理在笛卡尔空间建立。

4.根据权利要求3所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤2中，建立的全状态阻抗控制器为：

式中，是一个10维的向量，表示移动操作机器人各个关节的输入力矩；τ表示移动操作机器人关节期望力矩向量；q_wb＝(r,q)是一个10维的向量，代表移动操作机器人的各个关节位置，g为移动操作机器人整体的重力矩阵；C为移动操作机器人整体的科氏力和离心力矩阵。

5.根据权利要求4所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤2中，τ＝W^-1M^-1J^TΛ_WΛ^-1F+(I-W^-1M^-1J^TΛ_WJM^-1)τ₀，式中，M,J分别表示移动操作机器人整体的惯量矩阵和雅可比矩阵；Λ表示移动操作机器人笛卡尔空间的惯量矩阵；Λ_W表示笛卡尔空间带权重惯量矩阵；W表示权重矩阵；τ表示移动操作机器人关节期望力矩向量；F表示移动操作机器人末端与交互环境之间在笛卡尔空间广义外力；I为单位矩阵；τ₀表示零空间力矩。