(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210989731.8 (22)申请日 2022.08.17 (71)申请人 北京理工大 学 地址 100081 北京市海淀区中关村南大街5 号 (72)发明人 翟弟华　张思华　熊雨涵　林俊丞　 夏元清　邹伟东　詹玉峰　戴荔　 孙中奇　张金会　闫莉萍　崔冰　 (74)专利代理 机构 北京理工大 学专利中心 11120 专利代理师 李爱英 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 基于控制障碍函数的模型不确定机器人安 全控制方法 (57)摘要 本发明涉及基于控制障碍函数的模型不确 定机器人安全控制方法， 属于机器人控制技术领 域， 特别涉及一种机器人动力学模 型不确定情况 下， 基于控制障碍函数实现安全控制的方法。 本 发明采用高斯过程回归 方法， 使用较少的数据快 速得到较为准确的模型估计值， 利用高斯过程的 学习能力， 以一定置信度获得模型的不确定部 分， 从而采用基于控制障碍函数的方法完成机器 人的安全 控制。 权利要求书3页 说明书7页 附图2页 CN 115256395 A 2022.11.01 CN 115256395 A 1.基于控制障碍函数的模型不确定机器人安全控制方法， 其特征在于该方法的步骤包 括： 第一步， 建立机器人动力学估计模型； 第二步， 获得满足安全约束的机器人关节速度， 并对机器人关节速度进行数值积分， 获 得安全的机器人关节位置； 第三步， 跟踪第二步获得的安全的机器人关节位置， 得到跟踪过程中的机器人关节速 度、 机器人关节加速度和机器人关节力矩； 第四步， 跟踪第一步建立的机器人动力学估计模型， 得到跟踪过程中的模型关节加速 度； 第五步， 求取第三步得到的机器人关节加速度与第四步得到的模型关节加速度的差 值， 得到模型误差数据； 第六步， 根据第三步得到的机器人关节速度、 机器人关节加速度和机器人关节力矩以 及第五步得到的模型误差数据进行高斯过程回归， 得到 机器人动力学精确模型； 第七步， 根据第六步得到的机器人动力学精确模型， 得到满足安全约束的控制指令， 并 将获得的满足安全约束的控制指令对机器人进行安全控制， 完成基于控制障碍函数的模型 不确定机器人安全 控制。 2.根据权利要求1所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的第一 步中， 建立的机器人动力学估计模型为： 其中， 表示n个关节的位置， 是机器人的惯性矩阵， 表示离心力与科氏力矩阵， 表示重力项， 分别为M(q)， G(q)的估计值， 为模型误差； u为控制输入。 3.根据权利要求1或2所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其 特征在于： 所述的第二 步中， 采用二次规划模型获得满足安全约束的机器人关节速度。 4.根据权利要求3所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的二次规划模型为： qmin≤q≤qmax 其中， 表示满足安全约束的机器人关节速度， q表示当前机器人关节位置； 表示当 前机器人关节速度， 表示期望关节速度， e＝x(q) ‑xd(t)表示机 器人末 端位置跟踪误差， t表示当前时间， γ>0， γ为一常数， 是一设定值， 表示伪逆矩阵； x(q)为 当前机器人末端位置， xd(t)为期望机器人末端位置， 为xd(t)的导数； 为x(q)的导权　利　要　求　书 1/3 页 2 CN 115256395 A 2数且 Jy(q)表示机器人雅各比矩阵； qmin表示机器人最小关节位置， qmax表示机 器人最大关节位置， h(q)表示控制障碍函数， 表示h(q)的导数， α(h(q) )表示单调递增 函数， 且α(0)＝0 。 5.根据权利要求4所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 对 进行数值积分获得安全的机器人关节位置qsafe。 6.根据权利要求5所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的第三步中， 采用PID控制模型跟踪安全的机器人关节位置， 第四步中采用采用 PID控制模型跟踪机器人动力学估计模型。 7.根据权利要求6所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的PID控制模型的输入为 其中， kp， kd， ki为需要设计的控制参数， 在跟踪过程中， 将控制时间[0,T*]离散成N个时 刻tj， j＝1,2, ...,N， 在每个时刻反馈当前机器人 关节位置q(tj)， 更新控制输入从而完成轨 迹跟踪， 在整个控制过程中， 每个时刻的关节位置q(tj)， 关节速度 PID控制输入upid (tj)作为高斯回归训练数据的输入值 模型误差数据 作为训练数据的输出值 并保存数据集 [Xtrain,Ytrain]。 8.根据权利要求7所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的第六步中， 进行高斯过程回归的方法为： 每次选取100组连续数据 进行训练， 通过训练结果， 对当前机器人关节 位置下模型误差进行预测， 预测结果表示为： 其中表示预测均 值， δ表示预测方差， k与置信区间的选取相关， 基于模型误差数据的预测结果， 为保存的数据集[Xtrain,Ytrain]中的第i个子集。 9.根据权利要求1所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的第七步中， 采用轨 迹跟踪二次规划模型获得满足安全约束的控制指令 。 10.根据权利要求9所述的基于控制障碍函数的模型不确定机器人安全控制方法， 其特 征在于： 所述的轨 迹跟踪二次规划模型为：权　利　要　求　书 2/3 页 3 CN 115256395 A 3