CN113146651A - 一种沏茶机器人及其控制方法 - Google Patents

Abstract

本发明公开了一种沏茶机器人及其控制方法，其中，沏茶机器人包括：沏茶主控制器、故障检测模块和沏茶从控模块，所述故障检测模块，用于检测沏茶从控模块内部是否发生驱动故障，将检测结果反馈至沏茶主控制器；所述沏茶主控制器，用于向沏茶从控模块发送启动指令，当检测结果为驱动故障时，向沏茶从控模块发送替换指令；所述沏茶从控模块，用于在接收启动指令后，启动沏茶流程，接收替换指令后，采用备用驱动替换故障驱动后继续沏茶流程。本发明的沏茶机器人存在故障检测模块，可以及时发现驱动故障、且设置故障应对策略、有效避免更多硬件损坏、提升机器人使用体验以及机器人使用寿命。

Description

一种沏茶机器人及其控制方法

技术领域

本发明属于沏茶机器人领域，更具体地，涉及一种沏茶机器人及其控制方法。

背景技术

饮茶是中国的传统文化之一，也是当今很多人不可或缺的生活方式之一，传统的沏茶都由人手动操作冲泡，操作略显麻烦，并容易被热水烫着，无法满足当今智能化的需求。

目前的沏茶机器人，对于机械臂的舵机，均采用开环控制，由于沏茶机器人具有非线性、强耦合等问题，在舵机损坏时不能及时发现及记录，也没有能及时替代的策略，可能会造成更多硬件的损坏。

由此可见，现有的沏茶机器人存在不能及时发现驱动单元故障、且没有故障应对策略、导致更多硬件损坏、机器人使用体验差、机器人使用寿命短的技术问题。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种沏茶机器人及其控制方法，由此解决现有的沏茶机器人存在不能及时发现驱动单元故障、且没有故障应对策略、导致更多硬件损坏、机器人使用体验差、机器人使用寿命短的技术问题。

为实现上述目的，按照本发明的一个方面，提供了一种沏茶机器人，包括：沏茶主控制器、故障检测模块和沏茶从控模块，

所述故障检测模块，用于检测沏茶从控模块内部是否发生驱动故障，将检测结果反馈至沏茶主控制器；

所述沏茶主控制器，用于向沏茶从控模块发送启动指令，当检测结果为驱动故障时，向沏茶从控模块发送替换指令；

所述沏茶从控模块，用于在接收启动指令后，启动沏茶流程，接收替换指令后，采用备用驱动替换故障驱动后继续沏茶流程。

进一步地，所述沏茶从控模块包括：左臂模块、右臂模块和智能茶盘模块，

所述左臂模块，用于夹持盖碗移动至设定点进行清洗、接水泡茶和倒茶；

所述右臂模块，用于清洗智能茶盘模块上的茶杯、左臂模块夹持的盖碗以及往盖碗中注水泡茶；

所述智能茶盘模块，用于清洗时的翻转茶杯和倒茶后的递茶。

进一步地，所述右臂模块包括：右臂控制器、右机械臂和注水模块，

所述右臂控制器，用于在收到启动指令后，控制右机械臂和注水模块进行清洗和注水泡茶；

所述右机械臂，用于清洗智能茶盘模块上的茶杯、左臂模块夹持的盖碗以及往盖碗中注水泡茶；

所述注水模块，用于在右机械臂的清洗和注水泡茶过程执行注水操作。

进一步地，所述右机械臂包括依次连接的第一右驱动器、第二右驱动器、第三右驱动器、第四右驱动器、第五右驱动器和第六右驱动器，

所述第一右驱动器，用于控制右机械臂在水平方向上转动；

所述第二右驱动器，用于控制右机械臂的伸展高度；

所述第三右驱动器为第二右驱动器的备用驱动；

所述第四右驱动器，用于控制右机械臂的翻转，从而倾斜茶杯；

所述第五右驱动器为第四右驱动器的备用驱动；

所述第六右驱动器，用于控制注水模块中水管的空间位置。

进一步地，所述注水模块中设置抽水泵、加热器和流量传感器，

所述抽水泵，用于在右臂控制器的控制下抽水用于清洗和注水泡茶；

所述加热器，用于在右臂控制器的控制下加热泡茶用水，

所述流量传感器，用于控制注水流量。

进一步地，所述左臂模块包括：左臂控制器、左机械臂和夹爪，

所述左臂控制器，用于在收到启动指令后，控制左机械臂进行清洗、接水泡茶和倒茶；

所述左机械臂包括第一左驱动器、第二左驱动器、第三左驱动器、第四左驱动器、第五左驱动器、第六左驱动器，第一左驱动器用于控制左机械臂在水平方向上转动，第二左驱动器用于控制左机械臂的伸展高度，第三左驱动器为第二左驱动器的备用驱动，第四左驱动器用于控制左机械臂的翻转，从而倾斜茶杯，第五左驱动器为第四左驱动器的备用驱动，第六左驱动器用于控制夹爪的张开和闭合。

进一步地，所述智能茶盘模块包括茶盘控制器和多个舵机复合单元，

所述茶盘控制器，用于在收到启动指令后，控制舵机复合单元工作；

所述舵机复合单元，用于通过驱动和夹爪配合实现茶杯的翻转、扣盖、旋转和传递。

进一步地，所述沏茶从控模块中的每个驱动均与对应的转角传感器通信，所述转角传感器用于测量对应驱动的转角，所述沏茶主控制器用于通过角度闭环控制，修正转角传感器测量到的实际值与设定值之间的偏差。

按照本发明的另一方面，提供了一种沏茶机器人的控制方法，包括：

沏茶主控制器沏茶从控模块发送启动指令，沏茶从控模块在接收启动指令后，启动沏茶流程，沏茶流程依次包括：清洗过程、注水过程、倒茶过程、递茶过程；

在沏茶流程中，故障检测模块检测沏茶从控模块内部是否发生驱动故障，将检测结果反馈至沏茶主控制器，当检测结果为驱动故障时，沏茶主控制器向沏茶从控模块发送替换指令；

沏茶从控模块接收替换指令后，采用备用驱动替换故障驱动后继续沏茶流程。

进一步地，所述驱动故障的检测与替换包括：

故障检测模块在转角传感器检测到的实际值与当前步骤设定值的偏差达到设定的阈值时，判定为驱动故障；

若第二左驱动器发生故障使用第三左驱动器替换，若第二右驱动器发生故障使用第三右驱动器替换，若第四左驱动器发生故障使用第五左驱动器替换，若第四右驱动器发生故障使用第五右驱动器替换，替换过程即齐次变换矩阵映射关系的转化，若第二左驱动器发生故障使用第三左驱动器替换，左机械臂末端与基座的齐次变换矩阵映射关系由

转化为：

其中，数字0表示基座，1表示第一左驱动器，2表示第二左驱动器，3表示第三左驱动器，4表示第四左驱动器，6表示第六左驱动器，带左上标和左下标的T表示左下标对应部件相对左上标对应的部件的齐次变换矩阵。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，能够取得下列有益效果：

(1)本发明的沏茶机器人存在故障检测模块，可以及时发现驱动故障、且设置故障应对策略、有效避免更多硬件损坏、提升机器人使用体验以及机器人使用寿命。

(2)本发明采用了左机械臂、右机械臂和智能茶盘协同控制策略，使左机械臂、右机械臂和智能茶盘的动作在时间和空间上进行配合，按照功夫茶流程进行动作，流畅且美观地完成沏茶过程。

(3)本发明采用了多控制器构成的主从控制系统，不仅降低了控制器之间接线的复杂度，提高了系统的可靠性；而且在系统发生故障时，可以更精准地找到故障发生的模块，进行维护。

(4)本发明在注水口处采用流量传感器，进行闭环控制方式，控制注水流量保持稳定，即使在水压不同的情况下也能够保证注水时不会溢出或者过少，并解决饮用水桶中水余量变化而造成的水压不稳定的问题。

(5)本发明采用了角度闭环控制，修正转角传感器测量到的实际值与系统设定值之间的偏差，相比于其他沏茶机器人中电机和舵机的开环控制，提高舵机或电机转角大小的控制精度，使得左、右机械臂以及舵机复合单元能够更准确地达到目标位置。

(6)本发明采用了备用驱动的控制策略，对于左、右机械臂受力较大的2、4关节舵机或电机(即第二左驱动器、第四左驱动器、第二右驱动器、第四右驱动器)，若检测到舵机或电机损坏故障，则启用备用舵机或电机并动态地改变机械臂的DH参数，则其末端与基座的齐次变换矩阵映射关系也随之改变，使得机械臂能够以新的映射关系所对应的姿态角达到目标位置。

附图说明

图1是本发明实施例提供的一种沏茶机器人的控制示意图；

图2是是本发明实施例提供的一种沏茶机器人的结构示意图；

图3是本发明实施例提供的一种沏茶机器人的控制方法流程图；

在所有附图中，相同的附图标记用来表示相同的元件或结构，其中：

1为沏茶主控制器，2为故障检测模块，3为左臂控制器，4为左机械臂，5为夹爪，6为盖碗，7为右臂控制器，8为右机械臂，9为水桶，10为食品级硅胶管，11为滤网，12为净水器，13为加热器，14为抽水泵，15为流量传感器，16为注水不锈钢管，17为第一左驱动器，18为第二左驱动器，19为第三左驱动器，20为第四左驱动器，21为第五左驱动器，22为第六左驱动器，23为第一右驱动器，24为第二右驱动器，25为第三右驱动器，26为第四右驱动器，27为第五右驱动器，28为第六右驱动器，29为茶盘控制器，30为茶盘，31为复合驱动单元，32为夹持机构，33为电源模块。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

如图1所示，一种沏茶机器人，包括：沏茶主控制器、故障检测模块和沏茶从控模块，

所述故障检测模块，用于检测沏茶从控模块内部是否发生驱动故障，将检测结果反馈至沏茶主控制器；

所述沏茶主控制器，用于向沏茶从控模块发送启动指令，当检测结果为驱动故障时，向沏茶从控模块发送替换指令；

所述沏茶从控模块，用于在接收启动指令后，启动沏茶流程，接收替换指令后，采用备用驱动替换故障驱动后继续沏茶流程。

如图2所示，一种沏茶机器人，包括：沏茶主控制器1、故障检测模块2、沏茶从控模块和电源模块33，

所述沏茶主控制器1与沏茶从控模块通信，向沏茶从控模块发送启动指令，启动沏茶流程，沏茶流程包括清洗过程、注水过程、倒茶过程、递茶过程，电源模块33用于给沏茶主控制器1、故障检测模块2、沏茶从控模块供电。

沏茶从控模块包括左臂模块、右臂模块和智能茶盘模块。左臂模块、右臂模块和智能茶盘模块分别由一块独立的控制器进行控制。左臂模块、右臂模块和智能茶盘模块对应的控制器在接收到沏茶主控制器发出的指令后进行相应的沏茶流程。

所述左臂模块包括左臂控制器3、左机械臂4、夹爪5，左臂模块，用于夹持盖碗6移动至设定点进行清洗、接水泡茶和倒茶；

所述右臂模块包括右臂控制器7、右机械臂8、注水模块，右臂模块用于清洗智能茶盘上的茶杯、左机械臂夹持的盖碗以及往盖碗中注水泡茶；

所述注水模块包括水桶9、食品级硅胶管10、滤网11、净水器12、加热器13、抽水泵14、流量传感器15、注水不锈钢管(即水管)16。注水不锈钢管连接在右机械臂末端，其位置随右机械臂运动，抽水泵、加热器的开关信号由右臂控制器控制。

所述左机械臂包含6个驱动器(即关节舵机或电机)，第一左驱动器17、第二左驱动器18、第三左驱动器19、第四左驱动器20、第五左驱动器21、第六左驱动器22，第一左驱动器控制左机械臂在水平方向上转动；第二左驱动器控制左机械臂的伸展高度；第三左驱动器为第二左驱动器的备用舵机或电机；第四左驱动器控制左机械臂的翻转，从而倾斜茶杯；第五左驱动器为第四左驱动器的备用舵机或电机；第六左驱动器控制夹爪的张开和闭合。

所述右机械臂包含6个驱动器(即关节舵机或电机)，第一右驱动器23、第二右驱动器24、第三右驱动器25、第四右驱动器26、第五右驱动器27、第六右驱动器28，第一右驱动器控制右机械臂在水平方向上转动；第二右驱动器控制右机械臂的伸展高度；第三右驱动器为第二右驱动器的备用舵机或电机；第四右驱动器控制右机械臂的翻转，从而倾斜茶杯；第五右驱动器为第四右驱动器的备用舵机或电机；第六右驱动器控制水管的空间位置。

所述左机械臂、右机械臂的每个舵机或电机均使用一个独立的转角传感器对当前舵机或电机的转角进行检测。

所述智能茶盘模块包括茶盘控制器29、茶盘30和多个舵机复合单元，用于清洗时的翻转茶杯和倒茶后的递茶；

舵机复合单元包括复合驱动单元31和夹持机构32。复合驱动单元的每个舵机或电机均使用一个独立的转角传感器对当前舵机或电机的转角进行检测。复合驱动单元31和夹持机构32连接，实现茶杯的翻转、扣盖和旋转，在清洗过程将茶杯正反面清洗干净，并且在递茶过程给使用者递茶。

沏茶主控制器采用了舵机或电机角度闭环控制算法，修正转角传感器测量到的实际值与系统设定值之间的偏差，提高了舵机或电机转角大小的控制精度，使得左机械臂、右机械臂以及舵机复合单元能够更准确地达到目标位置。

本发明引入了故障自动检测策略，机器人运行时常见的两种故障为：舵机或电机损坏、通信模块损坏。在机器人运行期间，故障检测模块用于在所述转角传感器检测到的实际转角与当前步骤设定值的偏差达到设定的阈值时，判定为舵机损坏故障；在主控器给沏茶从控模块发送指令时，若没有收到来自沏茶从控模块的应答信号，则判定为通信模块损坏。

本发明引入了故障自动记录策略，故障检测模块在检测到舵机或电机损坏、通信模块损坏两种故障时，通过主控器向移动终端的服务器发送信号，将故障日志自动保存在移动终端服务器的数据库中，并可以通过移动终端进行查阅。本发明采用了故障自动检测与记录策略，对于沏茶机器人中常见的舵机或电机损坏、通讯模块损坏的问题，能够自动进行检测并保存日志信息到移动终端的服务器中，并可以通过移动终端进行调阅，便于及时维护。

对于左、右机械臂受力较大的2、4关节舵机或电机(即第二左驱动器、第四左驱动器、第二右驱动器、第四右驱动器)，采用了备用舵机或电机，若故障检测模块检测到舵机或电机损坏故障，则启用备用舵机或电机，则其对应的DH参数也随之改变，因此其末端与基座的齐次变换矩阵映射关系也随之改变，使得机械臂能够以新的映射关系所对应的姿态角达到目标位置。若2关节舵机或电机(第二左驱动器、第二右驱动器)发生故障切换至3关节舵机或电机(第三左驱动器、第三右驱动器)时，机械臂末端与基座的齐次变换矩阵映射关系由

转化为：

若4关节舵机或电机(第四左驱动器、第四右驱动器)发生故障切换至5关节舵机或电机(第五左驱动器、第五右驱动器)时，机械臂末端与基座的齐次变换矩阵映射关系转换为：

若2、4关节舵机或电机同时发生故障切换至3、5关节舵机或电机时，机械臂末端与基座的齐次变换矩阵映射关系转换为：

其中，数字0表示基座(即0关节舵机或电机)，1表示1关节舵机或电机(即第一左驱动器、第一右驱动器)，2表示2关节舵机或电机(即第二左驱动器、第二右驱动器)，3表示3关节舵机或电机(即第三左驱动器、第三右驱动器)，4表示4关节舵机或电机(即第四左驱动器、第四右驱动器)，5表示5关节舵机或电机(即第五左驱动器、第五右驱动器)，6表示6关节舵机或电机(即第六左驱动器、第六右驱动器)，

表示1关节舵机或电机相对基座的齐次变换矩阵，带左上标和左下标的T表示左下标对应关节舵机或电机相对左上标对应的关节舵机或电机的齐次变换矩阵。

故障检测模块与沏茶从控模块协作，使得在舵机或电机损坏时机械臂仍然能够正常运行，提高了机器人的稳定性，并解决了舵机或电机损坏异常导致盖碗及其夹持机构和注水模块掉落损坏的问题。

所述注水模块，在不锈钢管和抽水泵连接的硅胶软管处安装流量传感器，相比于时间控制出水量的方法，采用流量传感器进行闭环控制，控制注水流量保持稳定，避免注水溢出，并解决饮用水桶中水余量变化而造成的水压不稳定的问题。

左机械臂、右机械臂和智能茶盘舵机复合单元的动作，在时间和空间上保持同步，按照功夫茶流程进行动作，确保功夫茶沏茶流程顺利完成。

沏茶主控制器可以采用基于STM32F103微控制器的STM32控制板。

沏茶主控制器和移动终端之间通过无线通信进行信号传输，移动终端主要为手机或平板电脑，用于在选择功能后通过无线通信给沏茶主控制器发送控制指令。

沏茶主控制器和故障检测模块组成主控模块，主控模块与沏茶从控模块之间通过总线进行连接通信，通信接口方式为CAN或RS485。本发明采用了多控制器构成的主从控制系统，控制器之间通过总线连接，不仅降低了控制器之间接线的复杂度，提高了系统的可靠性；而且在系统发生故障时，可以更精准地找到故障发生的模块，进行维护。

左臂控制器可以采用基于STM32F103微控制器的STM32控制板，用于控制左机械臂的各关节舵机运动，以及控制盖碗机构的打开、合上、倾斜盖碗操作，左机械臂由6个关节舵机组成，其末端为盖碗机构，用于夹持盖碗，在注水阶段打开盖碗盖子，注水阶段结束合上盖碗，倒茶时倾斜盖碗使茶水均匀倒入茶杯中。

茶盘控制器可以采用基于STM32F103微控制器的STM32控制板，用于控制若干个舵机复合单元对茶杯进行茶杯的翻转、扣盖和旋转操作，每个舵机复合单元由3个复合驱动单元(即关节舵机)和夹持机构组成，3个关节舵机有3个方向的自由度，能够进行茶杯的翻转、扣盖和旋转操作，夹持机构用于夹持茶杯。

右臂控制器可以采用基于STM32F103微控制器的STM32控制板，用于控制右机械臂的各关节舵机运动、注水模块在清洗和沏茶时移动至指定点进行开关水操作，右机械臂由6关节舵机组成，其末端是注水模块，注水模块包括饮用水桶、食品级硅胶管、滤网、净水器、加热器、抽水泵、流量传感器和注水不锈钢管。注水不锈钢管连接在右机械臂末端，其位置随右机械臂运动，抽水泵、加热器的开关信号由右臂控制器控制，其中流量传感器稳定控制了每个注水操作需要的水流量大小。

本发明的正中央是智能茶盘模块，沏茶主控制器和故障检测模块位于智能茶盘上的正前方，舵机复合单元位于智能茶盘的正上方，智能茶盘控制器位于智能茶盘上的后方，电源模块位于智能茶盘后；

左臂模块位于智能茶盘左方，左臂控制器位于左机械臂底部，盖碗夹持机构位于左机械臂末端；

右臂模块位于智能茶盘右方，右臂控制器位于右机械臂底部，注水模块的不锈钢管位于右机械臂末端；

注水模块的不锈钢管与流量传感器和抽水泵相连，通过一条食品级硅胶管、加热器、净水器和滤网，最终接入水桶中的水中。

移动终端、位于智能茶盘远端，通过WI-FI或者蓝牙与沏茶主控制器进行无线通信。

参见图3，本发明在选择自动运行时首先进行清洗过程，具体为右机械臂移动至茶盘上方清洗茶杯，然后扶持舵机旋转压住茶杯，翻转舵机翻转使茶杯反面朝上，接着右机械臂移动至茶盘上方清洗茶杯，然后翻转舵机翻转使茶杯正面朝上，依此循环2次，则清洗过程结束；

接下来进行沏茶过程，包含注水过程、倒茶过程、递茶过程。具体为左、右机械臂同时移动至注水目标点，然后左机械臂夹爪张开打开盖碗打开抽水泵，右机械臂往盖碗中注水，接着关闭抽水泵，右机械臂回到工作原点，然后左机械臂夹爪闭合关闭盖碗，并移动到茶盘上方，接着倾斜盖碗使茶缓缓倒入茶杯，并在茶杯上方来回运动3次使茶水均匀倒入茶杯中，然后左机械臂回到工作原点，最后旋转舵机旋转给用户递茶。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种沏茶机器人，其特征在于，包括：沏茶主控制器、故障检测模块和沏茶从控模块，

所述故障检测模块，用于检测沏茶从控模块内部是否发生驱动故障，将检测结果反馈至沏茶主控制器；

所述沏茶主控制器，用于向沏茶从控模块发送启动指令，当检测结果为驱动故障时，向沏茶从控模块发送替换指令；

所述沏茶从控模块，用于在接收启动指令后，启动沏茶流程，接收替换指令后，采用备用驱动替换故障驱动后继续沏茶流程。

2.如权利要求1所述的一种沏茶机器人，其特征在于，所述沏茶从控模块包括：左臂模块、右臂模块和智能茶盘模块，

所述左臂模块，用于夹持盖碗移动至设定点进行清洗、接水泡茶和倒茶；

所述右臂模块，用于清洗智能茶盘模块上的茶杯、左臂模块夹持的盖碗以及往盖碗中注水泡茶；

所述智能茶盘模块，用于清洗时的翻转茶杯和倒茶后的递茶。

3.如权利要求2所述的一种沏茶机器人，其特征在于，所述右臂模块包括：右臂控制器、右机械臂和注水模块，

所述右臂控制器，用于在收到启动指令后，控制右机械臂和注水模块进行清洗和注水泡茶；

所述右机械臂，用于清洗智能茶盘模块上的茶杯、左臂模块夹持的盖碗以及往盖碗中注水泡茶；

所述注水模块，用于在右机械臂的清洗和注水泡茶过程执行注水操作。

4.如权利要求3所述的一种沏茶机器人，其特征在于，所述右机械臂包括依次连接的第一右驱动器、第二右驱动器、第三右驱动器、第四右驱动器、第五右驱动器和第六右驱动器，

所述第一右驱动器，用于控制右机械臂在水平方向上转动；

所述第二右驱动器，用于控制右机械臂的伸展高度；

所述第三右驱动器为第二右驱动器的备用驱动；

所述第四右驱动器，用于控制右机械臂的翻转，从而倾斜茶杯；

所述第五右驱动器为第四右驱动器的备用驱动；

所述第六右驱动器，用于控制注水模块中水管的空间位置。

5.如权利要求3所述的一种沏茶机器人，其特征在于，所述注水模块中设置抽水泵、加热器和流量传感器，

所述抽水泵，用于在右臂控制器的控制下抽水用于清洗和注水泡茶；

所述加热器，用于在右臂控制器的控制下加热泡茶用水，

所述流量传感器，用于控制注水流量。

6.如权利要求2-5任一所述的一种沏茶机器人，其特征在于，所述左臂模块包括：左臂控制器、左机械臂和夹爪，

所述左臂控制器，用于在收到启动指令后，控制左机械臂进行清洗、接水泡茶和倒茶；

所述左机械臂包括第一左驱动器、第二左驱动器、第三左驱动器、第四左驱动器、第五左驱动器、第六左驱动器，第一左驱动器用于控制左机械臂在水平方向上转动，第二左驱动器用于控制左机械臂的伸展高度，第三左驱动器为第二左驱动器的备用驱动，第四左驱动器用于控制左机械臂的翻转，从而倾斜茶杯，第五左驱动器为第四左驱动器的备用驱动，第六左驱动器用于控制夹爪的张开和闭合。

7.如权利要求2-5任一所述的一种沏茶机器人，其特征在于，所述智能茶盘模块包括茶盘控制器和多个舵机复合单元，

所述茶盘控制器，用于在收到启动指令后，控制舵机复合单元工作；

所述舵机复合单元，用于通过驱动和夹爪配合实现茶杯的翻转、扣盖、旋转和传递。

8.如权利要求1-6任一所述的一种沏茶机器人，其特征在于，所述沏茶从控模块中的每个驱动均与对应的转角传感器通信，所述转角传感器用于测量对应驱动的转角，所述沏茶主控制器用于通过角度闭环控制，修正转角传感器测量到的实际值与设定值之间的偏差。

9.如权利要求1-8任一所述的一种沏茶机器人的控制方法，其特征在于，包括：

沏茶主控制器沏茶从控模块发送启动指令，沏茶从控模块在接收启动指令后，启动沏茶流程，沏茶流程依次包括：清洗过程、注水过程、倒茶过程、递茶过程；

在沏茶流程中，故障检测模块检测沏茶从控模块内部是否发生驱动故障，将检测结果反馈至沏茶主控制器，当检测结果为驱动故障时，沏茶主控制器向沏茶从控模块发送替换指令；

沏茶从控模块接收替换指令后，采用备用驱动替换故障驱动后继续沏茶流程。

10.如权利要求9所述的控制方法，其特征在于，所述驱动故障的检测与替换包括：

故障检测模块在转角传感器检测到的实际值与当前步骤设定值的偏差达到设定的阈值时，判定为驱动故障；

若第二左驱动器发生故障使用第三左驱动器替换，若第二右驱动器发生故障使用第三右驱动器替换，若第四左驱动器发生故障使用第五左驱动器替换，若第四右驱动器发生故障使用第五右驱动器替换，替换过程即齐次变换矩阵映射关系的转化，若第二左驱动器发生故障使用第三左驱动器替换，左机械臂末端与基座的齐次变换矩阵映射关系由

转化为：

其中，数字0表示基座，1表示第一左驱动器，2表示第二左驱动器，3表示第三左驱动器，4表示第四左驱动器，6表示第六左驱动器，带左上标和左下标的T表示左下标对应部件相对左上标对应的部件的齐次变换矩阵。

Images