(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210962840.0 (22)申请日 2022.08.11 (71)申请人 浙江大学 地址 310058 浙江省杭州市西湖区余杭塘 路866号 (72)发明人 杨灿军　涂章鹏　朱元超　凌宇成　 武鑫　吴威涛　 (74)专利代理 机构 杭州天勤知识产权代理有限 公司 33224 专利代理师 彭剑 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种基于模糊决策的人机共融机械臂控制 方法 (57)摘要 本发明公开了一种基于模糊决策的人机共 融机械臂控制方法， 控制过程包括环境获取阶 段、 自主规划阶段、 运动映射阶段、 置信度计算阶 段、 融合与控制阶段、 力反馈阶段。 本发 明使用模 糊决策模拟人类决策行为， 确定置信度与权重 之 间的非线性 关系， 融合自主控制与操作者的指令 进行控制； 人机共融控制过程中操作者可以修改 接近物体的轨迹与抓取姿态； 人机共融的控制方 法可以减 轻远程操作者的操作 负担， 降低长时间 操作的精神压力， 并且在控制过程中提供在线实 时的力反馈， 帮助操作者理解自主控制的意图， 为后续操作者决策提供辅助。 权利要求书4页 说明书10页 附图5页 CN 115179296 A 2022.10.14 CN 115179296 A 1.一种基于模糊决策的人机共融机械臂控制方法， 其特征在于， 采用的自主控制系统 包含相机、 主 端机械手与从端机 械臂； 控制过程包括以下 六个阶段： (1)环境获取阶段 通过安装在机器人上的相机或布置在工作环境中的相机采集目标物与周围工作空间 的实时图像； 并经过图像识别技术处理得到目标物位置pg与障碍物位置po， 发送给自主控制 系统； (2)自主规划阶段 获取自主控制系统下一 步的目标位置； (3)运动映射阶段 将操作员的输入动作映射到从端机械臂 的运动中； 主端机械手采集操作员的输入， 通 过坐标变换关系将其映射到从端机 械臂上； (4)置信度计算阶段 计算自主控制与操作者的置信度， 用于融合过程的权 重分配； (5)融合与控制阶段 由置信度计算自主控制和操作者的权重， 融合自主控制和操作者的目标位置， 得到最 终的机械臂目标位置， 并控制机 械臂到达； (6)力反馈阶段 操作者在控制机械臂过程中得到自主控制的力反馈， 获得自主控制的意图， 提高操作 者和机械臂的协作性。 2.根据权利要求1所述的基于模糊决策的人机共融机械臂控制方法， 其特征在于， 自主 规划阶段， 获取自主控制系统下一 步的目标位置具体过程 为： 定义目标物的模型为 X(k)＝AX(k+1)+BU(k)+W(k) Z(k)＝HX(k)+V(k) 其中， X(k)是k时刻物体的状态变量， U(k)是k时刻物体外部输入的控制值， A和B 是系统 矩阵和输入矩阵， W(k)是过程噪声， Z(k)是k时刻的测量值， H是测量矩阵， V(k)是测量噪声； 状态变量X设定为 其中， {xG， yG， zG}T表示目标物在末端坐标系下的位置， 表示目标物在末端 坐标系下 的线速度， {γG， βG， αG}T和 表示在末端坐标系下目标物的欧拉角及 其角速度； 模型的系数矩阵设置为 A＝{a， a， a， a， a， a } B＝0 H＝diag[1， 0， 1， 0， 1， 0， 1， 0， 1， 0， 1， 0] 目标物与机 械臂末端之间的误差为e＝{ep， eq}∈R6， ep和eq分别为位置误差和姿态误差 ep＝{xG， yG， zG}T权　利　要　求　书 1/4 页 2 CN 115179296 A 2通过PID控制器计算自主控制下机械臂的末端速度， 通过卡尔曼滤波器得到物体速度 估计值 作为前馈补偿PID控制器引入的延迟 其中， x＝(p， q)∈R3×SO(3)代表机械臂末端的位置和姿态； 为机械臂末端坐标系 下自主控制系统的下个目标点， 经 过坐标变换 得到机械臂基坐标系下的下一个目标位置pr 其中， 表示机械臂基坐标系到末端坐标系的变换矩阵。 3.根据权利要求2所述的基于模糊决策的人机共融机械臂控制方法， 其特征在于， 自主 规划阶段， 当目标位置pr与障碍物接近时， 为了避免发生碰撞， 采用局部避障策略用于实时 运动控制； 具体为： 以障碍物为球心定义一个球面， 保证目标位置沿球面移动而不触碰障碍物， 根据目标 位置和障碍物的距离， 定义目标重映射 函数为 式中， po∈R3是障碍物的中心， pr∈R3是机械臂末端的目标位置， r是内圆的半径， R是外 圆的半径。 4.根据权利要求1所述的基于模糊决策的人机共融机械臂控制方法， 其特征在于， 运动 映射阶段， 从端机 械臂的末端位置是通过计算主 端机械手相对于其初始位置的位移获得的 ph(t)＝ph(t‑1)+s(pm(t)‑pm(t‑1)) 其中， ph(t)和ph(t‑1)分别为从端机械臂的当前位置和上次位置， pm(t)和pm(t‑1)分别 为主端机械手的当前位置和上次位置， s表示比例因子， 用于调整位移映射的比例； 从端机 械臂的末端方向映射 为 其中， 表示从CSB到CMB的旋转变换， 表示主端机械手末端在CMB下的方向， 表示从端机械臂末端在CME下的方向； CSB表示从端机械臂的基坐标系， CMB表示主端机械手的 基坐标系， CME表示主端机械手的末端坐标系。 5.根据权利要求1所述的基于模糊决策的人机共融机械臂控制方法， 其特征在于， 置信 度计算阶段， 计算自主控制的置信度过程 为：权　利　要　求　书 2/4 页 3 CN 115179296 A 3