(12)发明专利申请
(10)申请公布号 CN 112497208 A(43)申请公布日 2021.03.16
(21)申请号 202011141893.3(22)申请日 2020.10.22
(71)申请人 西安交通大学
地址 710049 陕西省西安市咸宁西路28号(72)发明人 赵飞 赵健壮 吴玉强 孙铮
梅雪松 王恪典 (74)专利代理机构 西安通大专利代理有限责任
公司 61200
代理人 马贵香(51)Int.Cl.
B25J 9/16(2006.01)
权利要求书2页 说明书9页 附图5页
(54)发明名称
基于全状态阻抗控制器的移动操作机器人通用控制方法(57)摘要
本发明公开了基于全状态阻抗控制器的移动操作机器人通用控制方法，建立移动操作机器人整体的运动学与动力学方程，在此基础上建立了笛卡尔空间带权重矩阵的全状态阻抗控制器；根据目标物体与机器人末端之间的距离，模仿人的行为模式，设计了操作、定位‑操作、定位三种运动模式，并设定了三种模式下对应的参数，本发明能够同时适应移动操作机器人的接触和非接触两种场景，实现对移动底盘和机械臂的协同控制，且柔顺操作的效果较好。
CN 112497208 ACN 112497208 A
权 利 要 求 书
1/2页
1.基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，包括以下步骤：
步骤1、建立移动操作机器人的运动学模型和动力学模型，所述移动操作机器人包括移动底盘(1)和机械臂(2)；
步骤2、在步骤1建立的运动学模型和动力学模型基础上，建立带权重矩阵的全状态阻抗控制器；
步骤3、根据目标物体(3)与机械臂(2)末端之间的距离，选定移动操作机器人的工作模式，并给定选定的移动操作机器人的工作模式下对应的权重矩阵的值，将所述权重矩阵的值带入步骤2得到的带权重矩阵的全状态阻抗控制器中计算，得到机械臂(2)的期望关节力矩和移动底盘(1)的虚拟力矩；
步骤4、将步骤3得到移动底盘(1)的虚拟力矩加上扰动力矩，经过计算得到移动底盘(1)的速度指令，并发送给底层速度控制器；将步骤3得到的机械臂(2)的期望关节力矩加上扰动力矩，发送给机械臂底层力矩控制器。
2.根据权利要求1所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤1中，建立的移动操作机器人的运动学模型为：
式中，为机械臂末端坐标系在世界坐标系下的位姿描述矩阵，为底盘坐
标系在世界坐标系下的位姿描述矩阵，为机械臂基座坐标系在底盘坐标系下的位姿描述矩阵，
为机械臂末端坐标系在基座标系下的位姿描述矩阵；
为底盘坐标系
在世界坐标系下的旋转矩阵，为机械臂末端坐标系在基座标系下的旋转矩阵；r＝
(rx,ry,rθ)为移动底盘(1)的3个自由度，三个元素分别代表底盘在x，y方向的位置和与x轴的夹角；(x0,y0,z0)依次为机械臂基座标系原点在底盘坐标系x,y,z方向上的位置；(xq,yq,zq)依次为机械臂末端在机械臂基座标系x,y,z方向上的位置；q＝(q1～q7)为机械臂的7个自由度，分别表示机械臂7个关节位置。
3.根据权利要求1所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤2中，带权重矩阵的全状态阻抗控制器基于移动操作机器人整体的动力学模型结合阻抗控制原理在笛卡尔空间建立。
4.根据权利要求3所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤2中，建立的全状态阻抗控制器为：
式中，是一个10维的向量，表示移动操作机器人各个关节的输入力矩；τ表
示移动操作机器人关节期望力矩向量；qwb＝(r,q)是一个10维的向量，代表移动操作机器人的各个关节位置，g为移动操作机器人整体的重力矩阵；C为移动操作机器人整体的科氏力
2
CN 112497208 A
权 利 要 求 书
2/2页
和离心力矩阵。
5.根据权利要求4所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤2中，τ＝W‑1M‑1JTΛWΛ‑1F+(I‑W‑1M‑1JTΛWJM‑1)τ式中，M,J分别表示移动操0，作机器人整体的惯量矩阵和雅可比矩阵；Λ表示移动操作机器人笛卡尔空间的惯量矩阵；ΛW表示笛卡尔空间带权重惯量矩阵；W表示权重矩阵；τ表示移动操作机器人关节期望力矩向量；F表示移动操作机器人末端与交互环境之间在笛卡尔空间广义外力；I为单位矩阵；τ0表示零空间力矩。
6.根据权利要求1所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤3中，移动操作机器人的工作模式有三种，三种工作模式模仿人的行为模式建立：
记dmax(x,y)为目标物体(3)与机械臂(2)末端在x,y方向上距离的较大值；机械臂(2)工作空间在xy平面投影的半径为rws，
在采用操作模式下，只有机械臂(2)运动；在定位‑操作模式下，机械臂(2)和底盘(1)协同运动；在定位模式下，仅底盘(1)运动。
7.根据权利要求5所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤3中，
在操作模式下：比例矩阵H为diag(1，1，1，5，5，5，5，5，5，5)，底盘虚拟的惯量Madm为diag(80,80,20)，零空间期望的刚度值Kn为10；
在操作模式下：比例矩阵H为diag diag(150，150，150，1，1，1，1，1，1，1)，底盘虚拟的惯
零空间期望的刚度值Kn为5；量Madm为diag(120,120,40)，
在操作模式下：比例矩阵H为diag(1，1，1，1，1，1，1，1，1，1)，底盘虚拟的惯量Madm为diag(100,100,30)，零空间期望的刚度值Kn为2。
8.根据权利要求7所述基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，所述步骤4中，速度指令的计算公式为：中，
式
分别表示t时刻和t‑1时刻对应的底盘的期望速度，Δt表示时间间隔，Madm为底盘
虚拟的惯量，Dadm为底盘虚拟的惯量和阻尼矩阵，表示t时刻对应的移动底盘期望的虚拟力矩。
3
CN 112497208 A
说 明 书
1/9页
基于全状态阻抗控制器的移动操作机器人通用控制方法
技术领域
[0001]本发明涉及机器人自动化领域，尤其涉及移动操作机器人在接触和非接触任务中的协同控制和柔顺操作问题。
背景技术
[0002]移动操作机器人是指将轻量化的协作机械臂集成在多功能移动底盘上，这类复合式机器人兼具协作机械臂的灵巧和移动底盘工作空间大的优点，功能多样，适用范围广，随着制造业的转型升级也越来越受到重视。[0003]移动操作机器人较为复杂，包含环境感知、路径规划、运动控制等多个技术模块，其中运动控制是关键基础技术，对移动操作机器人的作业效果发挥着重要作用。根据是否
可以分为两类：一是针对自由运动的协调控制问题(非接触)，聚焦移动底与操作环境接触，
盘和机械臂之间存在相对运动时的协调控制问题；二是在环境交互作用下的接触作业控制问题(接触)，要解决存在交互力情况下的协同控制问题。而对于具体采用的控制方式，分为基于力矩控制和速度控制两种。目前，环境交互作用下的接触作业的控制仍然是难题，且大部分已提出的方法只适应上述两种情况之一，而所提基于速度控制的方法，虽然能够适应两种场景，但是动态响应迟缓，交互性能差，因此应用受限。发明内容
[0004]针对现有技术中存在的问题，本发明提出了一种基于全状态阻抗控制器的移动操作机器人通用控制方法，能够同时适应接触和非接触两种场景下的操作任务，实现柔顺操作，简单高效。
[0005]为了解决上述技术问题，本发明通过以下技术方案予以实现：[0006]基于全状态阻抗控制器的移动操作机器人通用控制方法，其特征在于，包括以下步骤：
[0007]步骤1、建立移动操作机器人的运动学模型和动力学模型，所述移动操作机器人包括移动底盘和机械臂；[0008]步骤2、在步骤1建立的运动学模型和动力学模型基础上，建立带权重矩阵的全状态阻抗控制器；[0009]步骤3、根据目标物体与机械臂末端之间的距离，选定移动操作机器人的工作模式，并给定选定的移动操作机器人的工作模式下对应的权重矩阵的值，将所述权重矩阵的值带入步骤2得到的带权重矩阵的全状态阻抗控制器中计算，得到机械臂的期望关节力矩和移动底盘的虚拟力矩；[0010]步骤4、将步骤3得到移动底盘的虚拟力矩加上扰动力矩，经过计算得到移动底盘的速度指令，并发送给底层速度控制器；将步骤3得到的机械臂的期望关节力矩加上扰动力矩，发送给机械臂底层力矩控制器。[0011]进一步的，步骤1中，建立的移动操作机器人的运动学模型为：
4
CN 112497208 A
说 明 书
2/9页
[0012]
[0013]式中，为机械臂末端坐标系在世界坐标系下的位姿描述矩阵，为底
盘坐标系在世界坐标系下的位姿描述矩阵，为机械臂基座坐标系在底盘坐标系下的位姿描述矩阵，
为机械臂末端坐标系在基座标系下的位姿描述矩阵；
为底盘坐
标系在世界坐标系下的旋转矩阵，为机械臂末端坐标系在基座标系下的旋转矩阵；r
＝(rx,ry,rθ)为移动底盘(1)的3个自由度，三个元素分别代表底盘在x，y方向的位置和与x
轴的夹角；(x0,y0,z0)依次为机械臂基座标系原点在底盘坐标系x,y,z方向上的位置；(xq,yq,zq)依次为机械臂末端在机械臂基座标系x,y,z方向上的位置；q＝(q1～q7)为机械臂的7个自由度，分别表示机械臂7个关节位置。[0014]进一步的，步骤2中，带权重矩阵的全状态阻抗控制器基于移动操作机器人整体的动力学模型结合阻抗控制原理在笛卡尔空间建立。[0015]进一步的，步骤2中，建立的全状态阻抗控制器为：
[0016][0017]
式中，是一个10维的向量，表示移动操作机器人各个关节的输入力
矩；τ表示移动操作机器人关节期望力矩向量；qwb＝(r,q)是一个10维的向量，代表移动操作机器人的各个关节位置，g为移动操作机器人整体的重力矩阵；C为移动操作机器人整体的科氏力和离心力矩阵。[0018]进一步的，步骤2中，τ＝W‑1M‑1JTΛWΛ‑1F+(I‑W‑1M‑1JTΛWJM‑1)τ式中，M,J分别表示0，移动操作机器人整体的惯量矩阵和雅可比矩阵；Λ表示移动操作机器人笛卡尔空间的惯量矩阵；ΛW表示笛卡尔空间带权重惯量矩阵；W表示权重矩阵；τ表示移动操作机器人关节期望力矩向量；F表示移动操作机器人末端与交互环境之间在笛卡尔空间广义外力；I为单位矩阵；τ0表示零空间力矩。[0019]进一步的，步骤3中，移动操作机器人的工作模式有三种，三种工作模式模仿人的行为模式建立：[0020]记d机械臂工作空间max(x,y)为目标物体与机械臂末端在x,y方向上距离的较大值；在xy平面投影的半径为rws，
[0021]
[0022]
在采用操作模式下，只有机械臂运动；在定位‑操作模式下，机械臂和底盘协同运
动；在定位模式下，仅底盘运动。[0023]进一步的，步骤3中，[0024]在操作模式下：比例矩阵H为diag(1，1，1，5，5，5，5，5，5，5)，底盘虚拟的惯量Madm为
5
CN 112497208 A
说 明 书
3/9页
diag(80,80,20)，零空间期望的刚度值Kn为10；[0025]在操作模式下：比例矩阵H为diag diag(150，150，150，1，1，1，1，1，1，1)，底盘虚拟的惯量Madm为diag(120,120,40)，零空间期望的刚度值Kn为5；[0026]在操作模式下：比例矩阵H为diag(1，1，1，1，1，1，1，1，1，1)，底盘虚拟的惯量Madm为diag(100,100,30)，零空间期望的刚度值Kn为2。
[0027]
速度指令的计算公式为：进一步的，步骤4中，
分别表示t时刻和t‑1时刻对应的底盘的期望速度，Δt表示时间间隔，Madm为底盘
式中，
虚拟的惯量，Dadm为底盘虚拟的惯量和阻尼矩阵，表示t时刻对应的移动底盘期望的虚拟力矩。
与现有技术相比，本发明至少具有以下有益效果：[0029]与传统的将移动底盘和机械臂分别规划的方法相比，本发明将移动操作机器人当作一个整体，只需将移动操作机器人作为一个整体，规划出末端期望轨迹即可，简便高效。本发明提出的基于全状态阻抗控制器的移动操作机器人通用控制方法，建立了移动操作机器人整体的运动学和动力学模型，基于柔顺控制方法中的阻抗控制技术，建立了机器人末端与环境之间的动态交互关系，提出了带权重的笛卡尔空间全状态阻抗控制器，能够实现与环境的柔顺交互，不会对交互环境造成伤害；同时，本方法整体基于力矩控制，控制频率高，与基于速度控制的方法相比，动态响应特性更好，更加适应环境刚度参数变化较大的场景。
[0030]进一步的，带权重矩阵的全状态阻抗控制器是基于移动操作机器人整体的动力学
权重矩阵动态可调，对应着不同的控制效果。模型结合阻抗控制原理在笛卡尔空间建立的，
[0031]进一步的，模仿人的行为模式，提出了一种简洁的运动执行的策略，设计了三种运动模式，并给出了这三种模式一般情况下的参数设置，解决了底盘‑机械臂的协同控制问题，能够同时适应接触和非接触这两种任务场景。[0032]总的来说，本发明基于全状态阻抗控制器的移动操作机器人通用控制方法，通过建立整体的全状态阻抗控制器，解决了移动操作机器人在接触(有连续外力)场景下的柔顺操作问题；通过添加权重矩阵，设计三种运动模式，解决了接触和非接触场景下的底盘‑机械臂的协同控制问题。
[0028]
附图说明
[0033]为了更清楚地说明本发明具体实施方式中的技术方案，下面将对具体实施方式描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施方式，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
[0034]图1是本发明基于全状态阻抗控制器的移动操作机器人通用控制方法的移动操作机器人系统示意图；
[0035]图2是包括本发明所述控制方法的移动操作机器人控制框架；[0036]图3是为了验证非接触操作任务，而设置的移动操作场景示意图；[0037]图4是移动操作实验中记录的末端执行器和移动底盘的运动情况；
6
CN 112497208 A[0038][0039][0040][0041][0042][0043]
说 明 书
4/9页
图5a是末端执行器的位置变化情况示意图；图5b是末端执行器的姿态变化情况示意图；图5c是移动底盘的位置变化情况示意图；
图6a是移动操作机器人完成开门任务过程中记录的外力的情况；图6b是移动操作机器人完成开门任务过程中记录的外力矩的情况。图中：1‑移动底盘；2‑机械臂；3‑目标物体；4‑笔记本电脑。
具体实施方式
[0044]为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。[0045]为使本发明的上述目的、特征和优点能更明显易懂，下文特举较佳实施例，并配合所附附图，作详细说明如下。
[0046]作为本发明的某一具体实施方式，结合图1和图2所示，一种基于力矩控制全状态阻抗控制器的移动操作机器人系统，包括：移动底盘1、机械臂2、目标物体3和笔记本电脑4。
组成了移动操作机器人；笔记本电脑4分别与移动机械臂2的基座与移动底盘1的表面固联，
底盘1和机械臂2通过IP进行通信，发送指令。图2中左侧实线框中的基于权重矩阵的全状态阻抗控制器是上层控制器，而右侧虚线框中的机械臂力矩控制器和底盘速度控制器为底层控制器。
[0047]关节力矩传感器可以获取各个关节的执行力矩进行测量，也可以对机械臂末端的接触力/力矩进行观测。[0048]本实施例中，移动底盘1为Trans‑om4s移动平台，采用4个麦克纳姆驱动，能够实现平面内全向移动，属于非完整约束系统，具有3个自由度；基于ROS系统开发，提供速度接口，自带底层速度控制器，实现对移动底盘在速度层的闭环控制；机械臂2为FRANKA Panda轻型7自由度协作机械臂。每个关节配备关节力矩传感器，可以获取各个关节的实际力矩，也可以对机械臂末端的接触力/力矩进行观测；其工作空间在xy水平面内的投影的半径为0.855m，基于ROS系统开发，提供关节力矩接口，自带底层关节力矩控制器，实现对机械臂在关节力矩层的闭环控制。笔记本电脑4也工作在ROS环境下，负责基于权重矩阵的全状态阻抗控制器(上层控制器)的计算运行，并分别与移动底盘1和机械臂2通过IP进行通信，分别发布速度和关节力矩指令到底层控制器(结合图2更好理解)。
[0049]一种基于全状态阻抗控制器的移动操作机器人通用控制方法，应用于上述的移动操作机器人(被控对象)，包括：[0050]步骤一、建立本实施例中移动操作机器人整体的10自由度的运动学和动力学模型这是步骤二建立带权重的笛卡尔空间全状态阻抗控制器的基础。此处移动底盘(1)的3个自由度表示为r＝(rx,ry,rθ)，三个元素分别代表底盘在x，y方向的位置和与x轴的夹角；机械臂的7个自由度为q＝(q1～q7)，分别表示机械臂7个关节位置；r,q可以分别通过移动底盘和机械臂的反馈接口获得。[0051]如图1所示：ΣW代表世界坐标系，ΣM代表移动底盘(1)的基座座标系，ΣB代表机械
7
CN 112497208 A
说 明 书
5/9页
臂(2)的基座标系，ΣEE代表与机械臂(2)末端执行器固连的坐标系，采用齐次坐标变换的方法获得移动操作机器人整体的正运动学模型为：
[0052]
[0053]式中为机械臂末端坐标系在世界坐标系下的位姿描述矩阵，也就是移动
为底盘坐标系在世界坐标系下的位姿描述矩阵，
为机械臂末端坐标系在基座
为机械
操作机器人整体正运动学方程，
为机械臂基座坐标系在底盘坐标系下的位姿描述矩阵，标系下的位姿描述矩阵；
为底盘坐标系在世界坐标系下的旋转矩阵，
臂末端坐标系在基座标系下的旋转矩阵；(x0,y0,z0)依次为机械臂基座标系原点在底盘坐可以通过测量移动操作机器人的三维模型获得；(xq,yq,zq)依次标系x,y,z方向上的位置，
为机械臂末端在机械臂基座标系x,y,z方向上的位置，可以通过机械臂提供的反馈接口获得；
[0054]进一步的，通过对正运动学方程求导，获得移动操作机器人整体的雅可比矩阵J，其建立的关节速度与末端速度的关系为：
[0055][0056]
式中分别为移动操作机器人末端在世界坐标系下的线速度和角速度，
为移动操作机器人的关节空间速度，其中为底盘部分的速度，为机械臂部分
的速度，分别通过移动底盘和机械臂的反馈接口获得；
分别为整体雅可比矩阵中的
移动底盘部分和机械臂的部分。整体的雅可比矩阵J在步骤二中建立全状态阻抗控制器的
过程中多次用到。
[0057]采用拉格朗日方法建立的机械臂的动力学方程写为：
[0058][0059]
式中Mf代表机械臂的惯量矩阵，正定对称；Cf代表科氏力和离心力矩阵，gf代表重
分别代表机
依
一般根据经验设定，此处设为0。
力矩阵，与机械臂的位姿相关，这三者可以通过机械臂的反馈接口获得；械臂关节期望力矩向量和关节扰动力矩向量。
次为机械臂关节位置，速度和加速度，通过机械臂的反馈接口获得。
[0060]进一步的，由于移动底盘基于速度控制，为了建立移动操作机器人整体的动力学模型且简化底盘原有的动力学模型，用一个导纳控制器取代移动底盘原本的动力学方程，从而转换为力矩控制接口，底盘动力学方程写为：
[0061][0062]
是式中Madm,Dadm分别代表底盘虚拟的惯量和阻尼矩阵，两者关系为Dadm＝3.2Madm，
8
CN 112497208 A
说 明 书
分别为移动底盘的期望速度和加速度，
6/9页
设定的值；分别代表移动底盘期望的
虚拟力矩和扰动力矩，
[0063]
是计算得到的，是根据经验的设定值，此处设定为0。
将上述的底盘力学方程和机械臂的动力学方程整合得到移动操作机器人整体的
动力学方程为：
[0064][0065][0066][0067]
进一步简化得到：
式中，qwb＝(r,q)是一个10维的向量，代表移动操作机器人的各个关节位置，依次为移动操作机器人关节速度和加速度。M,C,g分别代表移动操作机器人整体的
ext
惯量矩阵，科氏力和离心力矩阵，重力矩阵；τ,τ分别代表移动操作机器人整体的关节期望力矩向量和扰动力矩向量。[0068]步骤二，建立带权重的移动操作机器人的笛卡尔空间全状态阻抗控制器。[0069]由上述的运动学和动力学模型，采用的带权重的移动操作机器人的笛卡尔空间全状态阻抗控制器为：[0070]τ＝W‑1M‑1JTΛWΛ‑1F+(I‑W‑1M‑1JTΛWJM‑1)τ0(7)
[0071]式中M,J分别表示移动操作机器人整体的惯量矩阵和雅可比矩阵，由步骤一获得；Λ表示移动操作机器人笛卡尔空间的惯量矩阵，表达式为Λ＝J‑TMJ‑1；ΛW表示笛卡尔空间带权重惯量矩阵，表达式为ΛW＝J‑TMWMJ‑1；W表示权重矩阵，其表达式为W＝HM‑1，其中H表示比例矩阵，为对角矩阵，正定对称，通过设置不同的H矩阵对角线上的元素来获得不同的权重矩阵，实现不同的控制目的；τ表示移动操作机器人关节期望力矩向量；F表示移动操作机器人末端与交互环境之间在笛卡尔空间(移动操作机器人的任务空间)广义外力，由阻抗控制原理获得，阻抗控制是在机器人末端与环境之间虚拟一个质量‑弹簧‑阻尼系统，当机器人末端与环境之间存在交互力时，能够对外力进行顺从响应，从而实现柔顺交互与操作；因
此，F的表达式为
Λd,Dd,Kd分别表示笛卡尔空间期望的质量矩阵，阻
表示移分别表示位
尼矩阵和刚度矩阵，即虚拟的二阶系统，是设定的；笛卡尔空间位姿误差动操作机器人的实际笛卡尔位姿x与移动操作机器人的期望位姿xd的差值，
姿误差的速度和角速度，当与环境存在交互力时，x与xd不相等，因此会产生广义外力F；I为单位矩阵。
[0072]由于本实施例中的移动操作机器人具有10个自由度，是冗余系统，需要添加零空间力矩，零空间是指在不影响任务空间的任务(移动操作机器人在笛卡尔空间对广义外力F
对移动操作机器人的关节空间进行配置，实现控制目的。本实的柔顺响应)执行的情况下，施例中τ表达式为0表示零空间力矩，
的阻尼值和刚度值，是设定的值；两者关系为
Dn,Kn分别表示零空间期望qwb,0表示初始时对应的移动操作
机器人关节空间值，τ尽可0的控制目的是让移动操作机器人在不影响任务执行的前提下，
9
CN 112497208 A
说 明 书
7/9页
能保持初始的空间位形qwb,0。此处需要说明的是，τ比如通过零空间对移0还有其他的形式，动操作机器人的关节限制和可操作度进行优化等，这些形式也属于本发明的保护范围。此处采用这种形式是因为：移动操作机器人初始的空间位形qwb,0，一般是人为设定的，是比较好的位形，因此希望在任务执行过程中尽可能保持此位形。
[0073]式(7)的右边两项分别为移动操作机器人对笛卡尔空间广义外力的关节响应力矩和零空间力矩，这是基于力矩控制的冗余机器人的控制范式。[0074]进一步的，考虑移动操作机器人整体的重力向量和科氏力/离心力向量，完整的基于权重的全状态阻抗控制器可以写为：
[0075][0076]
式中是一个10维的向量，表示移动操作机器人各个关节的输入力矩；
如图2所示，其中后7个元素机械臂对应的期望关节力矩τf直接发送给机械臂底层的力矩控
制器。
[0077]移动底盘(1)是基于速度控制的，通过下式将力矩转化为速度指令：
[0078][0079]
式中分别表示t时刻和t‑1时刻对应的底盘的期望速度，如图2所示，将发送
给底盘速度控制器；Δt表示时间间隔，表示t时刻对应的移动底盘期望的虚拟力矩；
[0080]
根绝目标物体3的位置与移步骤三，模仿人类在完成移动操作任务时的行为模式，动操作机器人末端之间的距离，分为三种运动模式，对应着不同的参数设置，根据公式(8)
和机械臂对应的期望关节力矩τf；
计算出t时刻对应的底盘期望的虚拟力矩
[0081]
记dmax(x,y)为目标位置与机器人末端在x,y方向上的距离的较大值；机械臂工作空间在xy平面投影的半径为rws＝0.855m，则
[0082]
式中，当目标位置在机械臂的工作空间内时，采用操作模式，只有机械臂运动，当目标位置超过机械臂的工作空间，但在机械臂的2倍工作空间内，采用定位‑操作模式，机械臂和底盘协同运动，当目标位置超过机械臂的2倍工作空间时，采用定位模式，只有底盘运动。需要说明的是三种模式的出发点是：机械臂的定位精度一般比底盘高，因此应当存在一
同时，种只有机械臂进行操作模式，也就是操作模式来适应对操作精度要求比较高的场景；
当目标位置超过机械臂的工作空间且并不是很远时(大于机械臂的工作空间，但在两倍机械臂的工作空间内)，这时车臂协同运动，也就是定位‑操作模式，就会提高操作任务的效率；但是当目标位置很原始(超过机械臂的两倍空间时)，机械臂的运动没有太大意义，因此只需要底盘运动即可，也就是定位模式。这三种模式是可以有机结合的，针对不同的任务场景来选择。
[0084]一般情况下，三种模式下对应的参数设置如表1所示。[0085]表1种模式下对应的参数设置
[0083]
10
CN 112497208 A
说 明 书
8/9页
[0086]
[0087]
表中diag()表示H和Madm对角矩阵对角线上所设置的值；
[0089]这里需要说明的是，H矩阵对角线上的元素与qwb＝(r,q)是对应的，H矩阵对角线上前三个元素依次对应移动底盘沿着x,y方向的移动和绕z轴转动的三个自由度；对角线上后7个元素依次对应着机械臂7个关节的自由度。表1中给出的只是参考值，具体的参数可以根据具体的场景进行设置。当需要限制移动底盘的某个自由度时，只需要将H矩阵对角线上前3个中对应的值设置为150，其他两个保持为1；对于机械臂来说，只需要将H矩阵对角线上后
则对应的机械臂自由度被限制，不产生运动，其他关7个中对应的值设置为5，其他保持为1，
节正常运动；如第二个开门实验时，为了顺利完成开门任务，需要对移动底盘绕z轴的旋转自由度进行限制，其他9个自由度正常运行，此时设置的H＝diag(1,1,150,1,1,1,1,1,1,1)。
[0090]同时，在完成同一任务时，模式选择组合也是多样的，根据具体的任务场景进行确定。比如，当目标位置远大于机械臂的工作空间时，可以只采用定位模式，也可以分为两段(第一个移动操作实验A‑B‑C)：先采用定位模式，然后采用操作模式，当然还可以分为三段：首先采用定位模式，接着采用定位‑操作模式，最后采用操作模式。
[0091]为了验证基于力矩控制全状态阻抗控制器的移动操作机器人通用控制方法的有效性，同时适用于接触和非接触的场景，设计移动操作机器人典型的物料传递实验(非接触)和开门实验(接触)来进行验证。
[0092]首先对两个实验中的整个控制流程进行说明：
[0093]移动操作机器人末端期望位姿x是提前规划好的，属于已知量，根据xd获得ddmax(x,y)，结合式(10)，选定工作模式以及对应的H,Madm,Kn参数；移动底盘的位置r和机械臂的关节位置q可以通过反馈接口实时获取，将r,q带入式(1)移动操作机器人的正运动学方
[0088]
程获得实际笛卡尔位姿x，然后可以获得微分可以获得从而求得笛卡尔空间广义外
力F；通过式(2)获得移动操作机器人整体雅可比矩阵J。整个运动开始前，记录下移动操作机器人的初始空间位形qwb,0，然后通过移动底盘和机械臂的速度反馈接口，获得实时空间位形qwb，从而获得零空间力矩τ接着通过零空间力矩τH,Madm,Kn参数、雅可比矩阵J、广义0。0、外力F以及式(7)获得移动操作机器人关节期望力矩向量τ，通过τ和式(8)全状态阻抗控制器(即上层控制器)获得移动操作机器人各个关节的输入力矩τ移动操作机器人各个关节in；的输入力矩ττ而利用移in中的后7个元素f直接发送给机械臂力矩控制器(即底层控制器)；动操作机器人各个关节的输入力矩τin中的前3个元素的期望速度发送给底盘速度控制器(即底层控制器)。
[0094]
通过式(9)得到表示t时刻的底盘
笛卡尔空间位姿误差
表示与移动操作机器人的期望位姿xd的差值，
分别表示位姿误差的速度和角速度，当与环境存在交互力时，x与xd不相等，因此会产生广
11
CN 112497208 A
说 明 书
9/9页
义外力F。
[0095]图3所示为物料传递实验的场景，具体如下：移动操作机器人首先以定位模式从A点沿着x正方向移动1.7m到达B点，接着，以操作模式继续在x正方向上移动0.4m到达C点获取物料，最后以定位‑操作模式沿着x负方向移动1.5m，y轴正方向上移动1.4m，z轴负方向0.28m，到达D点，将物料摆放到桌子上的指定位置。整个过程的参数，按照所述表1进行设置，图4是整个实验过程中记录下来机器人末端和底盘的运动情况，最上方的图是记录机器人末端的位置情况，中间位置的图是基于四元数表示的机器人末端的姿态变化情况，最下方的图是移动底盘的位置情况，其中yaw表示绕z轴的转动位移。从图中可以看出，移动操作机器人按照预期目标，顺利将物料传递到指定位置，且整个过程，图4中的末端执行器位置、末端执行器姿态和移动底盘状态都没有明显突变，运动比较平滑，说明本发明方法能够适应非接触的操作任务场景。
[0096]这个实验为移动操作实验，所设计的三种运动模式，成功将物料摆放到指定位置，说明本发明能有效解决底盘‑机械臂的协同控制问题。[0097]进行开门试验，开门实验是指由机械臂2夹持门把手，然后由机械臂2和移动底盘1来协同运动将门打开，设置的笛卡尔空间阻抗刚度为Kd＝diag(850,850,850,30,30,30)，零空间刚度Kn＝5；设置底盘的导纳控制器的虚拟质量为Madm＝diag(80,80,20)；设置比例矩阵为H＝diag(1,1,150,1,1,1,1,1,1,1)，即限制底盘的绕Z轴旋转的自由度，由剩下的9个自由度协调完成开门任务。图5a记录的是整个开门过程中机器人末端的位置情况，图5b基于四元数表示的机器人末端的姿态变化情况，图5c是底盘的位置情况，其中yaw表示绕z轴的转动位移，图6a记录的是整个开门过程中机械臂2末端受到的外力，图6b整个开门过程中机械臂2末端受到的外力矩。可以看出，整个开门过程是轨迹非常平滑且柔顺，机械臂2末端的接触力的绝对值最大在20N左右；接触力矩其绝对值最大不超过2N·m，说明整个开门过程中的产生的接触力/力矩是较小的，较为柔顺。[0098]第二个开门实验，在有持续外力的情况下，底盘‑机械臂协同操作成功将门打开，说明本发明有效解决了在接触情况下的动态操作问题。因此，本发明适应移动操作机器人接触/非接触两种场景下的操作任务。[0099]通过上述两个实验，可以充分说明，本发明所提方法能够很好的适应接触和非接触情况下的开门任务，且柔顺操作效果非常好。[0100]最后应说明的是：以上实施例，仅为本发明的具体实施方式，用以说明本发明的技术方案，而非对其限制，本发明的保护范围并不局限于此，尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，其依然可以对前述实施例所记载的技术方案进行修改或可轻易想到变化，或者对其中部分技术特征进行等同替换；而这些修改、变化或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。
12
CN 112497208 A
说 明 书 附 图
1/5页
图1
图2
13
CN 112497208 A
说 明 书 附 图
2/5页
图3
图4
14
CN 112497208 A
说 明 书 附 图
3/5页
图5a
图5b
15
CN 112497208 A
说 明 书 附 图
4/5页
图5c
图6a
16
CN 112497208 A
说 明 书 附 图
5/5页
图6b
17
因篇幅问题不能全部显示，请点此查看更多更全内容