CN113103233A - 一种清洗机器人系统及力控清洗方法 - Google Patents

Abstract

本发明涉及机器人技术领域，公开一种清洗机器人系统及力控清洗方法。所述清洗机器人系统包括机械臂、力控浮动刷头和传感器单元，机械臂具有多个自由度；力控浮动刷头通过传感器单元与机械臂连接，力控浮动刷头包括浮动机构和刷头，刷头通过浮动机构与机械臂连接；传感器单元包括力传感器、位移传感器和倾角传感器，力传感器用于实时感知刷头的清洗力度，位移传感器用于实时感知浮动机构的位置伸缩，倾角传感器用于实时感知刷头的姿态。本发明提高了清洗效率和智能化程度，节约了水资源，提高了清洗质量，能够更有效地清除污渍，保证清洗质量，获得一致的清洗效果，进行全方位接触式清洗，避免清洗过程中对被清洗物体表面涂层的损坏。

Description

一种清洗机器人系统及力控清洗方法

技术领域

本发明涉及机器人技术领域，尤其涉及一种清洗机器人系统及力控清洗方法。

背景技术

目前，在汽车零部件清洗领域、工业产品零部件清洗领域以及高铁动车转向架及其组件清洗领域，主要采用人工手动清洗的方法，清洗环境较为恶劣，而且工作量也大，造成清洗效率低，人力成本高。

若采用自动化的清洗设备，非接触式的高压水枪清洗方式不仅严重浪费水资源，还存在无法清洗干净的局部区域，造成清洗质量不合格。

发明内容

基于以上问题，本发明的目的在于提供一种清洗机器人系统及力控清洗方法，清洗效率高，清洗质量好，节省人力成本和水资源。

为达上述目的，本发明采用以下技术方案：

一种清洗机器人系统，包括：

机械臂，具有多个自由度；

力控浮动刷头，通过传感器单元与所述机械臂连接，所述力控浮动刷头包括浮动机构和刷头，所述刷头通过所述浮动机构与所述机械臂连接；

传感器单元，与所述机械臂末端连接，包括力传感器、位移传感器和倾角传感器，所述力传感器用于实时感知所述刷头的清洗力度，所述位移传感器用于实时感知所述浮动机构的位置伸缩，所述倾角传感器用于实时感知所述刷头的姿态；

所述力控浮动刷头还包括控制器和伺服控制单元，所述控制器通过所述伺服控制单元实时调整所述浮动机构在Z方向上的行程，采用双清洗力阈值，当刷头清洗力度大于阈值上限时，自动调整浮动机构Z方向上行程，使刷头清洗力逐渐减小，当刷头清洗力度小于阈值下限时，自动调整浮动机构Z方向上行程，使刷头清洗力逐渐增大，以使所述刷头与被清洗物体之间维持恒定的接触清洗压力。

作为本发明的清洗机器人系统的优选方案，所述力控浮动刷头还包括气动马达和气动阀控制单元，所述气动马达的一端与所述浮动机构连接，另一端与所述刷头连接，所述控制器通过所述气动阀控制单元控制所述气动马达，以使所述刷头在所述被清洗物体的表面以预设速度旋转，进行接触式清洗。

作为本发明的清洗机器人系统的优选方案，所述力控浮动刷头还包括水管和水管控制单元，所述水管与所述刷头连通，在所述刷头清洗时，所述控制器通过所述水管控制单元按照设定水流速度进行所述水管的喷水控制。

作为本发明的清洗机器人系统的优选方案，所述刷头的形状为圆型、方型或梳齿型，所述刷头的材料为纳米丝、尼龙、硬毛、软毛或海绵。

一种力控清洗方法，采用如上所述的清洗机器人系统，包括以下步骤：

在机械臂运动学模型和动力学模型的基础上，结合被清洗物体形状和尺寸先验知识，设计清洗轨迹，使刷头与被清洗物体表面贴合，清洗轨迹包含直线清洗轨迹、Z字型清洗轨迹、同心圆清洗轨迹、偏心圆清洗轨迹等，满足清洗应用要求；

机械臂采用拖拽示教方式，直接在被清洗物体表面生成清洗轨迹，清洗时，同时采用刷头位置闭环与力闭环的控制方法，形成双闭环控制方法，根据力控闭环的实时反馈信息，获取刷头在物体表面X/Y/Z方向上所受力的信息，实时调整机械臂的清洗轨迹，以及浮动机构在Z方向上的行程，控制刷头与被清洗物体表面接触力的大小，保证刷头与被清洗物体之间维持恒定的接触清洗压力，并使刷头跟随规划好的清洗轨迹，进行接触式柔性清洗。

作为本发明的力控清洗方法的优选方案，

在进行力控前，进行动力学建模，根据关节空间下的动力学方程：

其中，Γ表示作用在机械臂上的力矩，q为广义关节坐标，M(q)为关节空间的惯性矩阵，

为离心力和科氏力耦合矩阵，G(q)为重力载荷；

建模时，采用Newton-Euler法，在Newton-Euler法中，

Newton方程为：

其中，m为连杆质量，

为连杆速度的一阶导数；

Euler方程为：

其中，I_Ci为惯性张量矩阵，ω_i为连杆旋转角速度；

将力/力矩在Z轴上投影，对于刚体平动和转动，有以下关系式：

其中，

作为本发明的力控清洗方法的优选方案，

在动力学建模时，引入惯性张量作为参数，惯性张量I是对整个体积内所有的定位刚体的点的向量与刚体密度的积分，如下式：

I＝∫_V[(p^T)I₃-pp^T]ρdv

其中，p为位置坐标，即

I₃为3阶单位矩阵，即

ρ为刚体密度；

算出其惯性张量为：

在计算机械臂各连杆的惯性张量之后，将其作为参数之一进行动力学分析，在计算力矩时，分为向外迭代法和向内迭代法两部分，向外迭代法是计算连杆的角速度和线速度，分别通过以下两个式子进行计算：

其中，

为第i+1杆相对第i杆的旋转矩阵，为一个3×3的矩阵；

ⁱP_i+1为第i+1杆相对第i杆的位置向量；

ⁱ⁺¹ω_i+1对θ求导，即可求得各连杆的角加速度；ⁱ⁺¹ω_i+1对位置向量P求导，即可求得各连杆的线加速度，分别如下两式所示：

在求出各连杆质心的线加速度和角加速度之后，通过Newton-Euler方程求得作用在第i杆的质心上的力/力矩，分别如下两式所示：

以上式子组成了向外迭代法的迭代公式，通过迭代公式，由第0连杆到机械臂最后一个连杆向外迭代，求出各连杆质心所受力/力矩大小。

作为本发明的力控清洗方法的优选方案，

在计算各连杆质心的相关量之后，利用向内迭代法继续进行求解；

向内迭代法是直接从上述求得的质心所受力/力矩的前提出发，根据各连杆的受力/力矩平衡大小关系，计算出各关节电机处的力/力矩大小；

根据各连杆的受力平衡关系，从第i杆上看，列出连杆力平衡方程为：

其中，ⁱf_i和ⁱ⁺¹f_i+1分别为第i和第i+1关节受力，ⁱⁱF_i为第i杆质心受力，

为第i杆相对i+1杆的旋转矩阵；

根据力矩平衡关系，列出连杆的力矩平衡方程为：

其中，ⁱn_i和ⁱ⁺¹n_i+1分别为第i和第i+1关节力矩，ⁱP_i+1为第i杆相对i+1杆质心的位置向量；

将关节所受的力往Z轴上投影，从而消除平动和转动的内力项：

以上式子是向内迭代法的迭代方程；

由机械臂末端最后一个关节向第1个关节进行力矩迭代计算，便得到机械臂动力学模型。

作为本发明的力控清洗方法的优选方案，在得到机械臂动力学模型后，采用计算力矩法进行控制，控制器表达式由下式给出：

本发明的有益效果为：

本发明提供的清洗机器人系统及力控清洗方法，清洗机器人系统包括机械臂、力控浮动刷头和传感器单元，机械臂具有多个自由度；力控浮动刷头设置于机械臂的末端，力控浮动刷头包括浮动机构和刷头，刷头通过浮动机构与机械臂连接；传感器单元包括力传感器、位移传感器和倾角传感器，力传感器用于实时感知刷头的清洗力度，位移传感器用于实时感知浮动机构的位置伸缩，倾角传感器用于实时感知刷头的姿态，在机械臂运动学模型和动力学模型的基础上，结合力控闭环的实时反馈信息，以及被清洗物体形状和尺寸先验知识，设计清洗轨迹，使刷头与被清洗物体表面贴合，保证刷头与被清洗物体之间维持恒定的接触清洗压力；机械臂采用拖拽示教方式，直接在被清洗物体表面生成清洗轨迹，清洗时，同时采用刷头位置闭环与力闭环的控制方法，形成双闭环控制方法，实时调整机械臂的清洗轨迹，以及浮动机构在Z方向上的行程，控制刷头与被清洗物体表面接触力的大小，进行接触式柔性清洗。

本发明提供的清洗机器人系统及力控清洗方法，采用基于力控浮动刷头的清洗机器人代替传统的人工清洗，使人工不用工作在恶劣的清洗环境，并提高了清洗效率和智能化程度；采用力控浮动刷头实现清洗刷头与被清洗物体表面接触，采用双清洗力阈值，进行柔性清洗，相比于高压水冲洗的方法，节约了水资源，提高了清洗质量，能够更有效地清除污渍；同时采用刷头位置闭环与力闭环的控制方法，采用力矩法对机械臂末端清洗力的大小进行控制，相比于传统的仅采用位置控制方法，力矩控制可以保证刷头与被清洗物体表面的触清洗压力维持恒定，既保证力控浮动刷头与动车组零部件接触有一定力度，保证清洗质量，又保证接触力不能太大，避免清洗过程中对被清洗物体表面涂层的损坏，获得一致的清洗效果；在进行清洗轨迹规划时，机械臂采用拖拽示教方式，直接在被清洗物体表面模拟人工清洗方式生成清洗轨迹，不仅操作简单，而且能保证覆盖被清洗物体表面的每个位置，进行全方位接触式清洗。

附图说明

为了更清楚地说明本发明实施例中的技术方案，下面将对本发明实施例描述中所需要使用的附图作简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据本发明实施例的内容和这些附图获得其他的附图。

图1为本发明具体实施例提供的清洗机器人系统组成图；

图2为本发明具体实施例提供的清洗机器人系统结构示意图；

图3是本发明具体实施方式提供的力控清洗方法的原理示意图。

具体实施方式

为使本发明解决的技术问题、采用的技术方案和达到的技术效果更加清楚，下面将结合附图对本发明实施例的技术方案作进一步的详细描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

在本发明的描述中，需要说明的是，术语“中心”、“上”、“下”、“左”、“右”、“竖直”、“水平”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”、仅用于描述目的，而不能理解为指示或暗示相对重要性。其中，术语“第一位置”和“第二位置”为两个不同的位置。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以具体情况理解上述术语在本发明中的具体含义。

如图1和图2所示，本实施例提供一种清洗机器人系统，用于解决针对汽车零部件、工业产品零部件、高铁动车转向架及其组件清洗环境恶劣、工作量大、效率低、清洗质量和智能化程度不高的问题，该清洗机器人系统包括机械臂、力控浮动刷头和传感器单元。其中，机械臂具有多个自由度。力控浮动刷头通过传感器单元与机械臂连接，力控浮动刷头包括浮动机构和刷头，刷头通过浮动机构与机械臂连接。传感器单元与机械臂末端连接，传感器单元包括力传感器、位移传感器和倾角传感器，力传感器用于实时感知刷头的清洗力度，位移传感器用于实时感知浮动机构的位置伸缩，倾角传感器用于实时感知刷头的姿态。

采用基于力控浮动刷头的清洗机器人代替传统的人工清洗，使人工不用工作在恶劣的清洗环境，并提高了清洗效率和智能化程度，采用力控浮动刷头实现清洗刷头与被清洗物体表面接触，采用双清洗力阈值，进行柔性清洗，相比于高压水冲洗的方法，节约了水资源，提高了清洗质量，能够更有效地清除污渍，同时采用刷头位置闭环与力闭环的控制方法，采用力矩法对机械臂末端清洗力的大小进行控制，相比于传统的仅采用位置控制方法，力矩控制可以保证刷头与被清洗物体表面的触清洗压力维持恒定，既保证力控浮动刷头与动车组零部件接触有一定力度，保证清洗质量，又保证接触力不能太大，避免清洗过程中对被清洗物体表面涂层的损坏，获得一致的清洗效果，在进行清洗轨迹规划时，机械臂采用拖拽示教方式，直接在被清洗物体表面模拟人工清洗方式生成清洗轨迹，不仅操作简单，而且能保证覆盖被清洗物体表面的每个位置，进行全方位接触式清洗。

力控浮动刷头还包括控制器和伺服控制单元，控制器通过伺服控制单元实时调整浮动机构在Z方向上的行程，采用双清洗力阈值，当刷头清洗力度大于阈值上限时，自动调整浮动机构Z方向上行程，使刷头清洗力逐渐减小，当刷头清洗力度小于阈值下限时，自动调整浮动机构Z方向上行程，使刷头清洗力逐渐增大，以使刷头与被清洗物体之间维持恒定的接触清洗压力，避免清洗过程中对被清洗物体表面涂层的损坏，实现接触式柔性清洗，获得一致的清洗效果。控制器集成了恒力控制和浮动机构行程调整功能，采用了力闭环的控制方法，根据力传感器获取刷头与被清洗物体表面接触力的大小。机械臂在三维空间中进行清洗轨迹规划，清洗轨迹包含直线清洗轨迹、Z字型清洗轨迹、同心圆清洗轨迹、偏心圆清洗轨迹等，满足清洗应用要求。

力控浮动刷头还包括气动马达和气动阀控制单元，气动马达的一端与浮动机构连接，另一端与刷头连接，控制器通过气动阀控制单元控制气动马达，以使刷头在被清洗物体的表面以预设速度旋转，进行接触式清洗。采用双清洗力阈值，当刷头清洗力度大于阈值上限时，可自动调整浮动机构Z方向上行程，使刷头清洗力逐渐减小，当刷头清洗力度小于阈值下限时，可自动调整浮动机构Z方向上行程，使刷头清洗力逐渐增大。

力控浮动刷头还包括水管和水管控制单元，水管与刷头连通，在刷头清洗时，控制器通过水管控制单元按照设定水流速度进行水管的喷水控制。

可以根据不同被清洗物体表面形状，刷头的形状为圆型、方型或梳齿型，根据不同被清洗物体表面材料，刷头的材料为纳米丝、尼龙、硬毛、软毛或海绵。

本实施例提供的清洗机器人系统，采用基于力控浮动刷头的清洗机器人代替传统的人工清洗，使人工不用工作在恶劣的清洗环境，并提高了清洗效率和智能化程度，采用力控浮动刷头实现清洗刷头与被清洗物体表面接触，采用双清洗力阈值，进行柔性清洗，相比于高压水冲洗的方法，节约了水资源，提高了清洗质量，能够更有效地清除污渍，同时采用刷头位置闭环与力闭环的控制方法，采用力矩法对机械臂末端清洗力的大小进行控制，相比于传统的仅采用位置控制方法，力矩控制可以保证刷头与被清洗物体表面的触清洗压力维持恒定，既保证力控浮动刷头与动车组零部件接触有一定力度，保证清洗质量，又保证接触力不能太大，避免清洗过程中对被清洗物体表面涂层的损坏，获得一致的清洗效果，在进行清洗轨迹规划时，机械臂采用拖拽示教方式，直接在被清洗物体表面模拟人工清洗方式生成清洗轨迹，不仅操作简单，而且能保证覆盖被清洗物体表面的每个位置，进行全方位接触式清洗。

如图3所示，本实施例提供一种力控清洗方法，用于解决针对汽车零部件、工业产品零部件、高铁动车转向架及其组件清洗环境恶劣、工作量大、效率低、清洗质量和智能化程度不高的问题，采用上述的清洗机器人系统，包括以下步骤：

在机械臂运动学模型和动力学模型的基础上，结合被清洗物体形状和尺寸先验知识，设计清洗轨迹，使刷头与被清洗物体表面贴合，清洗轨迹包含直线清洗轨迹、Z字型清洗轨迹、同心圆清洗轨迹和偏心圆清洗轨迹；

机械臂采用拖拽示教方式，直接在被清洗物体表面生成清洗轨迹，清洗时，同时采用刷头位置闭环与力闭环的控制方法，形成双闭环控制方法，根据力控闭环的实时反馈信息，获取刷头在物体表面X、Y、Z方向上所受力的信息，实时调整机械臂的清洗轨迹，以及浮动机构在Z方向上的行程，控制刷头与被清洗物体表面接触力的大小，以使刷头与被清洗物体之间维持恒定的接触清洗压力，并使刷头跟随规划的预设清洗轨迹，进行接触式柔性清洗。

如图1所示，清洗机器人系统，包括：机械臂、传感器单元、力控浮动刷头；传感器单元包括力传感器、位移传感器、倾角传感器，力传感器实时感知清洗力度，位移传感器实时感知浮动机构的位置伸缩，倾角传感器感知刷头姿态；力控浮动刷头包括浮动机构、气动马达、刷头、水管、力控浮动刷头控制器、伺服控制单元、气动阀控制单元和水管控制单元。

如图2所示，机械臂末端与传感器单元连接，传感器单元与浮动机构连接，浮动机构与气动马达连接，气动马达与刷头连接。

采用基于力控浮动刷头的清洗机器人代替传统的人工清洗，使人工不用工作在恶劣的清洗环境，并提高了清洗效率和智能化程度，采用力控浮动刷头实现清洗刷头与被清洗物体表面接触，采用双清洗力阈值，进行柔性清洗，相比于高压水冲洗的方法，节约了水资源，提高了清洗质量，能够更有效地清除污渍，同时采用刷头位置闭环与力闭环的控制方法，采用力矩法对机械臂末端清洗力的大小进行控制，相比于传统的仅采用位置控制方法，力矩控制可以保证刷头与被清洗物体表面的触清洗压力维持恒定，既保证力控浮动刷头与动车组零部件接触有一定力度，保证清洗质量，又保证接触力不能太大，避免清洗过程中对被清洗物体表面涂层的损坏，获得一致的清洗效果，在进行清洗轨迹规划时，机械臂采用拖拽示教方式，直接在被清洗物体表面模拟人工清洗方式生成清洗轨迹，不仅操作简单，而且能保证覆盖被清洗物体表面的每个位置，进行全方位接触式清洗。

在进行力控前，进行动力学建模，根据关节空间下的动力学方程：

其中，Γ表示作用在机械臂上的力矩，q为广义关节坐标，M(q)为关节空间的惯性矩阵，

为离心力和科氏力耦合矩阵，G(q)为重力载荷；

建模时，采用Newton-Euler法，在Newton-Euler法中，

Newton方程为：

其中，m为连杆质量，

为连杆速度的一阶导数；

Euler方程为：

其中，I_Ci为惯性张量矩阵，ω_i为连杆旋转角速度；

将力/力矩在Z轴上投影，对于刚体平动和转动，有以下关系式：

其中，

在动力学建模时，引入惯性张量作为参数，惯性张量I是对整个体积内所有的定位刚体的点的向量与刚体密度的积分，如下式：

I＝∫_V[(p^Tp)I₃-pp^T]ρdv

其中，p为位置坐标，即

I₃为3阶单位矩阵，即

ρ为刚体密度；

算出其惯性张量为：

在计算机械臂各连杆的惯性张量之后，将其作为参数之一进行动力学分析，在计算力矩时，分为向外迭代法和向内迭代法两部分，向外迭代法是计算连杆的角速度和线速度，分别通过以下两个式子进行计算：

其中，

为第i+1杆相对第i杆的旋转矩阵，为一个3×3的矩阵；

ⁱP_i+1为第i+1杆相对第i杆的位置向量；ⁱ⁺¹ω_i+1对θ求导，即可求得各连杆的角加速度；ⁱ⁺¹ω_i+1对位置向量P求导，即可求得各连杆的线加速度，分别如下两式所示：

在求出各连杆质心的线加速度和角加速度之后，通过Newton-Euler方程求得作用在第i杆的质心上的力/力矩，分别如下两式所示：

以上式子组成了向外迭代法的迭代公式，通过迭代公式，由第0连杆到机械臂最后一个连杆向外迭代，求出各连杆质心所受力/力矩大小。

在计算各连杆质心的相关量之后，利用向内迭代法继续进行求解；

向内迭代法是直接从上述求得的质心所受力/力矩的前提出发，根据各连杆的受力/力矩平衡大小关系，计算出各关节电机处的力/力矩大小；

根据各连杆的受力平衡关系，从第i杆上看，列出连杆力平衡方程为：

其中，ⁱf_i和ⁱ⁺¹f_i+1分别为第i和第i+1关节受力，ⁱⁱF_i为第i杆质心受力，

为第i杆相对i+1杆的旋转矩阵；

根据力矩平衡关系，列出连杆的力矩平衡方程为：

其中，ⁱn_i和ⁱ⁺¹n_i+1分别为第i和第i+1关节力矩，ⁱP_i+1为第i杆相对i+1杆质心的位置向量；

将关节所受的力往Z轴上投影，从而消除平动和转动的内力项：

以上式子是向内迭代法的迭代方程；

由机械臂末端最后一个关节向第1个关节进行力矩迭代计算，便得到机械臂动力学模型。

在得到机械臂动力学模型后，采用计算力矩法进行控制，控制器表达式由下式给出：

在计算力矩反馈中，将反馈的误差信号置于力矩计算之前，对控制力矩需要更高频率的计算，因此其工作在较高的控制频率(如1KHz)上，控制频率比较高，因此控制精度也较高。在计算力矩控制的基础下，可以做出相对复杂的控制系统，如本发明涉及的一种基于力控浮动刷头的清洗机器人系统，一方面要求清洗机器人按规划好的清洗路径(含角度、速度、加速度等信息)运行；一方面又要求机器人遭受外力干扰(如清洗工件表面不平整等)后依然能够保持规划好的清洗轨迹。

在机械臂运动学模型和以上动力学模型的基础上，结合力控闭环的实时反馈信息，以及被清洗物体形状和尺寸先验知识，设计合理的清洗轨迹，与被清洗物体表面贴合，保证刷头与被清洗物体之间维持恒定的接触清洗压力。

基于力控浮动刷头的清洗机器人轨迹规划既可以在关节空间完成，也可以在笛卡尔空间实现，对于点对点规划，一般在关节空间进行，而对于对末端轨迹有要求的连续轨迹规划，一般在笛卡尔空间进行，不同的是，在笛卡尔空间进行轨迹规划时，需要将笛卡尔空间的位姿变换到关节空间。本发明在轨迹规划时采用拖拽示教方式，机械臂采用拖拽示教方式，直接在被清洗物体表面生成清洗轨迹。清洗时，采用刷头位置闭环的控制方法，实时修正机械臂的清洗轨迹，使刷头能够跟随规划好的轨迹，进行清洗。

轨迹规划后，得到一组步长相同的轨迹点，以这些轨迹点作为末端位姿，通过运动学逆解便可得到每次各关节的期望关节角度。

同时采用刷头位置闭环与力闭环的控制方法，形成双闭环，根据力传感器实时获取刷头在物体表面X/Y/Z方向上所受力的信息，实时调整机械臂的清洗轨迹，以及浮动机构Z方向上行程，控制刷头与物体表面接触力的大小，进行接触式柔性清洗。并且采用了双清洗力阈值，当刷头清洗力度大于阈值上限时，可自动调整浮动机构Z方向上行程，使刷头清洗力逐渐减小，当刷头清洗力度小于阈值下限时，可自动调整浮动机构Z方向上行程，使刷头清洗力逐渐增大。

本实施例提供的力控清洗方法，采用基于力控浮动刷头的清洗机器人代替传统的人工清洗，使人工不用工作在恶劣的清洗环境，并提高了清洗效率和智能化程度，采用力控浮动刷头实现清洗刷头与被清洗物体表面接触，采用双清洗力阈值，进行柔性清洗，相比于高压水冲洗的方法，节约了水资源，提高了清洗质量，能够更有效地清除污渍，同时采用刷头位置闭环与力闭环的控制方法，采用力矩法对机械臂末端清洗力的大小进行控制，相比于传统的仅采用位置控制方法，力矩控制可以保证刷头与被清洗物体表面的触清洗压力维持恒定，既保证力控浮动刷头与动车组零部件接触有一定力度，保证清洗质量，又保证接触力不能太大，避免清洗过程中对被清洗物体表面涂层的损坏，获得一致的清洗效果，在进行清洗轨迹规划时，机械臂采用拖拽示教方式，直接在被清洗物体表面模拟人工清洗方式生成清洗轨迹，不仅操作简单，而且能保证覆盖被清洗物体表面的每个位置，进行全方位接触式清洗。

注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (9)

1.一种清洗机器人系统，其特征在于，包括：

机械臂，具有多个自由度；

力控浮动刷头，通过传感器单元设置于与所述机械臂连接的末端，所述力控浮动刷头包括浮动机构和刷头，所述刷头通过所述浮动机构与所述机械臂连接；

传感器单元，与所述机械臂末端连接，包括力传感器、位移传感器和倾角传感器，所述力传感器用于实时感知所述刷头的清洗力度，所述位移传感器用于实时感知所述浮动机构的位置伸缩，所述倾角传感器用于实时感知所述刷头的姿态；

所述力控浮动刷头还包括控制器和伺服控制单元，所述控制器通过所述伺服控制单元实时调整所述浮动机构在Z方向上的行程，采用双清洗力阈值，当刷头清洗力度大于阈值上限时，自动调整浮动机构Z方向上行程，使刷头清洗力逐渐减小，当刷头清洗力度小于阈值下限时，自动调整浮动机构Z方向上行程，使刷头清洗力逐渐增大，以使所述刷头与被清洗物体之间维持恒定的接触清洗压力。

2.根据权利要求1所述的清洗机器人系统，其特征在于，所述力控浮动刷头还包括气动马达和气动阀控制单元，所述气动马达的一端与所述浮动机构连接，另一端与所述刷头连接，所述控制器通过所述气动阀控制单元控制所述气动马达，以使所述刷头在所述被清洗物体的表面以预设速度旋转，进行接触式清洗。

3.根据权利要求1所述的清洗机器人系统，其特征在于，所述力控浮动刷头还包括水管和水管控制单元，所述水管与所述刷头连通，在所述刷头清洗时，所述控制器通过所述水管控制单元按照设定水流速度进行所述水管的喷水控制。

4.根据权利要求1所述的清洗机器人系统，其特征在于，所述刷头的形状为圆型、方型或梳齿型，所述刷头的材料为纳米丝、尼龙、硬毛、软毛或海绵。

5.一种力控清洗方法，其特征在于，采用如权利要求1-4任一项所述的清洗机器人系统，包括以下步骤：

在机械臂运动学模型和动力学模型的基础上，结合被清洗物体形状和尺寸先验知识，设计清洗轨迹，使刷头与被清洗物体表面贴合，清洗轨迹包含直线清洗轨迹、Z字型清洗轨迹、同心圆清洗轨迹和偏心圆清洗轨迹；

机械臂采用拖拽示教方式，直接在被清洗物体表面生成清洗轨迹，清洗时，同时采用刷头位置闭环与力闭环的控制方法，形成双闭环控制方法，根据力控闭环的实时反馈信息，获取刷头在物体表面X、Y、Z方向上所受力的信息，实时调整机械臂的清洗轨迹，以及浮动机构在Z方向上的行程，控制刷头与被清洗物体表面接触力的大小，以使刷头与被清洗物体之间维持恒定的接触清洗压力，并使刷头跟随规划的预设清洗轨迹，进行接触式柔性清洗。

6.根据权利要求5所述的力控清洗方法，其特征在于，

在进行力控前，进行动力学建模，根据关节空间下的动力学方程：

其中，Γ表示作用在机械臂上的力矩，q为广义关节坐标，M(q)为关节空间的惯性矩阵，

为离心力和科氏力耦合矩阵，G(q)为重力载荷；

建模时，采用Newton-Euler法，在Newton-Euler法中，

Newton方程为：

其中，m为连杆质量，

为连杆速度的一阶导数；

Euler方程为：

其中，I_Ci为惯性张量矩阵，ω_i为连杆旋转角速度；

将力/力矩在Z轴上投影，对于刚体平动和转动，有以下关系式：

其中，

7.根据权利要求6所述的力控清洗方法，其特征在于，

在动力学建模时，引入惯性张量作为参数，惯性张量I是对整个体积内所有的定位刚体的点的向量与刚体密度的积分，如下式：

I＝∫_V[(p^Tp)I₃-pp^T]ρdv

其中，p为位置坐标，即

I₃为3阶单位矩阵，即

ρ为刚体密度；

算出其惯性张量为：

在计算机械臂各连杆的惯性张量之后，将其作为参数之一进行动力学分析，在计算力矩时，分为向外迭代法和向内迭代法两部分，向外迭代法是计算连杆的角速度和线速度，分别通过以下两个式子进行计算：

其中，

为第i+1杆相对第i杆的旋转矩阵，为一个3×3的矩阵；

ⁱP_i+1为第i+1杆相对第i杆的位置向量；

ⁱ⁺¹ω_i+1对θ求导，即可求得各连杆的角加速度；ⁱ⁺¹ω_i+1对位置向量P求导，即可求得各连杆的线加速度，分别如下两式所示：

在求出各连杆质心的线加速度和角加速度之后，通过Newton-Euler方程求得作用在第i杆的质心上的力/力矩，分别如下两式所示：

以上式子组成了向外迭代法的迭代公式，通过迭代公式，由第0连杆到机械臂最后一个连杆向外迭代，求出各连杆质心所受力/力矩大小。

8.根据权利要求7所述的力控清洗方法，其特征在于，

在计算各连杆质心的相关量之后，利用向内迭代法继续进行求解；

向内迭代法是直接从上述求得的质心所受力/力矩的前提出发，根据各连杆的受力/力矩平衡大小关系，计算出各关节电机处的力/力矩大小；

根据各连杆的受力平衡关系，从第i杆上看，列出连杆力平衡方程为：

其中，ⁱf_i和ⁱ⁺¹f_i+1分别为第i和第i+1关节受力，ⁱⁱF_i为第i杆质心受力，

为第i+1杆相对i杆的旋转矩阵；

根据力矩平衡关系，列出连杆的力矩平衡方程为：

其中，ⁱⁱn_i和ⁱ⁺¹n_i+1分别为第i和第i+1关节力矩，ⁱP_i+1为第i+1杆相对i杆质心的位置向量；

将关节所受的力往Z轴上投影，从而消除平动和转动的内力项：

以上式子是向内迭代法的迭代方程；

由机械臂末端最后一个关节向第1个关节进行力矩迭代计算，便得到机械臂动力学模型。

9.根据权利要求8所述的力控清洗方法，其特征在于，在得到机械臂动力学模型后，采用计算力矩法进行控制，控制器表达式由下式给出：

Images