(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210729157.2 (22)申请日 2022.06.24 (71)申请人 中国科学院空间应用工程与技 术中 心 地址 100094 北京市海淀区邓庄南路9号 (72)发明人 梁骥　闫硕　徐金中　于歌　 孙广彬　郭丽丽　 (74)专利代理 机构 北京轻创知识产权代理有限 公司 11212 专利代理师 陈晓斌 (51)Int.Cl. B25J 9/16(2006.01) B25J 18/00(2006.01) G06F 30/20(2020.01) G06K 9/62(2022.01)G06F 111/08(2020.01) G06F 119/14(2020.01) (54)发明名称 机械臂运动轨迹规划方法、 系统、 存储介质 和电子设备 (57)摘要 本发明涉及一种机械臂 运动轨迹规划方法、 系统、 存储介质和电子设备， 包括： 采用预设线 性 高斯控制器获取机械臂的名义轨迹， 根据名义轨 迹得到机械臂的高斯混合模型； 基于高斯混合模 型， 获取机械臂的局部动力学模型； 根据LQR算法 获取局部动力学模型对应的线性高斯控制器控 制系数； 根据控制系数对预设线性高斯控制器进 行修正并进行迭代训练， 直至将符合条件的线性 高斯控制器确定为目标线性高斯控制器， 利用目 标线性高斯控制器生成机械臂的目标运动轨迹。 本发明将动力学模型的拟合分为由粗到精的两 个过程， 极大减少动力学模型的拟合难度， 提高 样本的利用率、 降低所需采样的数目， 并通过LQR 方法， 提升 了对于机械臂轨迹的优化效率。 权利要求书4页 说明书13页 附图4页 CN 115070764 A 2022.09.20 CN 115070764 A 1.一种机 械臂运动轨 迹规划方法， 其特 征在于， 包括： S1、 在每一预设时刻内， 采用预设线性高斯控制器获取待控制机械臂的至少一条名义 轨迹， 并根据所有的名义轨 迹得到所述待控制机 械臂的高斯混合模型； S2、 基于所述高斯混合模型， 获取所述待控制机械臂在每一预设时刻对应的局部动力 学模型； S3、 根据L QR算法， 获取每 个局部动力学模型对应的线性高斯控制器的控制系数； S4、 根据所有的控制系数对所述预设线性高斯控制器进行修正， 并将修正后的线性高 斯控制器作为所述预设线性高斯控制器并循环执行S1， 直至将符合预设条件的控制系数所 对应的修正后的线性高斯控制器确定为目标线性高斯控制器， 利用所述目标线性高斯控制 器生成所述待控制机 械臂的目标运动轨 迹。 2.根据权利要求1所述的机械臂运动轨迹规划方法， 其特征在于， 所述预设线性高斯控 制器模型为： p(at|st)＝N(Ktst+kt， δat)， 其中， p(at|st)为所述预设线性高斯控制器模型， Kt 为所述预设线性高斯控制器模型在预设时刻t的控制系数， kt为所述预设线性高斯控制器 模型在预 设时刻t的线性增量， δat为所述预设线性高斯控制器模型在预设时刻t的初始协方 差矩阵， t为任一预设时刻。 3.根据权利要求2所述的机械臂运动轨迹规划方法， 其特征在于， 所述根据所有的名义 轨迹得到所述待控制机 械臂的高斯混合模型， 包括： 基于所述所有的名义轨迹， 得到并根据每一时刻的状态动作随机变量， 得到对应的高 斯分布概率密度函数， 并对所有的高斯分布概率密度函数进行拟合， 得到所述高斯混合模 型概率密度， 其中， 任意一条名义轨迹为： 所述状 态动作随机变量为： [st， at， st+1]， st∈R1×m， at∈R1×n， st+1∈R1×m， 所述高斯混合模型概率密 度为： s0为所述待控制机械臂及环境的初始状态， a0 为所述待控制机械臂的初始机械臂动作， st为所述待控制机械臂及环境在t预设时刻的状 态， at为所述待控制机械臂在t预设时刻的机械臂动作， zt＝[st， at， st+1]， K 是所述高斯混合模型中的子高斯模型的数量， 是第k个高斯分布概率密度函 数， ak是状态动作随机变量属 于子高斯模型k的概率， μk是子高斯模型k的均值， δk是子高斯 模型k的协方差； 根据所述高斯混合模型概率密度， 得到所述高斯混合模型的对数似然函数， 其中， 所述 对数似然函数为： 采用EM算法对所述对数似然函数进行估算， 得到所述高斯混合模型。 4.根据权利要求3所述的机 械臂运动轨 迹规划方法， 其特 征在于， 所述S2具体包括： S21、 将所述每一预设时刻的状态动作随机变量[st， at， st+1]代入后验概率估计公式， 得 到所述每一预设时刻的状态动作随机变量的后验概 率估计值γtk； S22、 根据第一预设公式和所述每一预设时刻的状态动作随机变量的后验概率估计向 量γt， 得到所述每一预设时刻的状态动作随机变量的共轭先验均值 并根据所述第二预 设公式、 所述后验概率估计公式和所述每一预设时刻的状态动作随机变量的共轭先验均值权　利　要　求　书 1/4 页 2 CN 115070764 A 2得到所述每一预设时刻的状态动作随机变量的共 轭先验方差 S23、 根据第三预设公式和所述每一预设时刻的状态动作随机变量的共轭先验均值 得到所述每一预设时刻的状态动作随机变量的概率密度函数均值 μt， 并根据第四预设公式 和所述每一 预设时刻的状态动 作随机变 量的共轭先验方差 得到所述每一 预设时刻的状 态动作随机变量的概 率密度函数 方差∑t； S24、 根据所述每一预设时刻的状态动作随机变量对应的联合分布方差和第五预设公 式， 得到所述每一预设时刻的状态动作随机变量对应的 并根据所述每一预设时刻的 状态动作随机变量对应的联合分布均值和 第六预设公式， 得到所述每一预设时刻的状态动 作随机变量对应的ft； S25、 根据 所述每一预设时刻的状态动作随机变量对应的联合分布方差、 所述每一预设 时刻的状态动作随机变量对应的 和第七预设公式， 得到所述每一预设时刻的状态动作 随机变量对应的δst； S26、 根据所述每一预设时刻的状态动作随机变量对应的 ft、 δst得到所述每一预 设时刻的局部动力学模型； 其中， 所述每一预设时刻的局部动力学模型为： 所述后验概率估计公式为： 所述第一 预设公式为： 所述第二预设公式为： 所述第三预设公式为： 所 述 第 四 预 设 公 式 为 ： 所 述 第 五 预 设 公 式 为 ： 所述第六 预设公式为： ft＝[ μ(m+n+1)…μ(2m+n)]T‑Fsat[ μ(m+n‑1)…μ(m+n)]T； 所述第七预设公式为： 权　利　要　求　书 2/4 页 3 CN 115070764 A 3