(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210892069.4 (22)申请日 2022.07.27 (71)申请人 北京航空航天大 学 地址 100191 北京市海淀区学院路37号北 京航空航天大 学 (72)发明人 王君臣　竺佳宇　卢春姮　孙振　 (74)专利代理 机构 北京知元同创知识产权代理 事务所(普通 合伙) 11535 专利代理师 聂稻波 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种机械臂末端六维力传感器重力和惯性 力补偿方法 (57)摘要 一种机械臂末端六维力传感器重力与惯性 力补偿的方法， 采用鲁棒统计学方法， 对力传感 器和负载的相关参数进行标定， 获得末端负载的 质量， 末端负载质心相对力传感器测量坐标系的 位置以及六维力传感器力和力矩的零位偏移。 标 定后在实际使用中， 通过计算惯 性力补偿测量结 果， 获取真实交互力。 本发明通的方法无需附加 惯性力测量单元； 最后同时对末端重力、 惯性力 及零位偏移进行补偿， 获得机械臂与环境及操作 者的真实交 互力信号。 权利要求书3页 说明书5页 附图2页 CN 115157260 A 2022.10.11 CN 115157260 A 1.一种机械臂末端六维力传感器重力和惯性力补偿方法， 其特征在于， 所述方法包括： (1)六维力传感器标定、 (2)惯性力的计算、 (3)重力和惯性力及零 位偏移补偿三个步骤； 所述(1)六维力传感器标定包括： 建立坐标系， 设六维力传感器(001)测量坐标系为{b}， 不失一般性， 认定其与机械臂末 端法兰坐标系重合； 在末端负载(002)的质心处建立坐标系{c}， 令其姿态与坐标系{b}相 同； 控制机器人到N个不同的预设位姿， 记坐标系{b}的第i个姿态在机器人基坐标系{s}中 的表示为Ri， 六维力传感器(0 01)的信号构成可用下式表达： 其中， 为六维力传感器在第i个姿态下测得的力矩和力， 两者合成一力旋 量； 为六维力传感器力矩和 力的零位偏移， 为末端负载(002)的质量， g 为当地的重力加速度， 为末端负载质心相对坐标系{b}原 点的位置， 表 示重力方向。 2.根据权利要求1所述的机械臂末端六维力传感器重力和惯性力补偿方法， 其特征在 于， 所述(1)六维力传感器标定进一 步包括： 1.1.标定末端负载(002)的质量； 任选力的两次测量i,j， 通过对式(b)运用作差法消除 零位偏移可 得： 对上述等式(c)两边求模长， 并对N次测量中任意两次测量进行处理并取均值后可得末 端负载(0 02)质量的一个估计值 式中 表示N次测量中任意两次测量的排列组合的集 合， 为集合中的元 素数。 3.根据权利要求2所述的机械臂末端六维力传感器重力和惯性力补偿方法， 其特征在 于， 所述(1)六维力传感器标定在1.1后进一 步包括： 1.2.异常值剔除； 为使估计值更加逼近 的真值， 利用 “三西格玛 ”原则筛选出 测量 对中含粗大误差的异常对并将其舍去： 当 时， 认定相 应的测量对(j,k)为异常对， 并将其在 中剔除； 更新后权　利　要　求　书 1/3 页 2 CN 115157260 A 2的数据集记为 集合中元素数记为 该数据集具有较 高的鲁棒性； 在式(e)中用 代替 遍历 中的测量对， 计算质量的均 值得到 后续所有遍历， 均基于剔除异常测 量对后的鲁棒数据集 进行。 4.根据权利要求3所述的机械臂末端六维力传感器重力和惯性力补偿方法， 其特征在 于， 所述(1)六维力传感器标定在1.2后进一 步包括： 1.3.标定末端负载(002)质心在力传感器测量坐标系{b}中的位置； 为求pcom， 类似地对 力矩按式(a)作差处 理： 其中， 记向量 的斜对称矩阵为： 将 中的测量对都按式(g)处 理后写成矩阵块相乘的形式： 其中， 进而用最小二乘法求得pcom的估计值： 5.根据权利要求4所述的机械臂末端六维力传感器重力和惯性力补偿方法， 其特征在 于， 所述(1)六维力传感器标定在1.3后进一 步包括： 1.4.标定六维力传感器(001)的零位偏移； 将求得的 代入式(a)(b)求得力矩 和力的零 位偏移估计值： 6.根据权利要求5所述的机械臂末端六维力传感器重力和惯性力补偿方法， 其特征在 于， 所述(2)惯性力的计算包括： 在末端负载的质心位置 处建立坐标系{ c}， 齐次变换矩阵Tcb＝(I3,‑pcom)表示{b}在{c} 中的表达， pcom由步骤(1)中标定得到， 记 为末端负载的惯性张量矩阵； 将末端负 载 看作单个刚体， 用牛 顿‑欧拉公式计算末端负载 所受惯性力： 其中， 是作用在末端负载上的合外力旋量， 为空间惯量权　利　要　求　书 2/3 页 3 CN 115157260 A 3