(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210579112.1 (22)申请日 2022.05.26 (71)申请人 江西省智能产业 技术创新研究院 地址 330000 江西省南昌市小蓝经开区汇 仁大道266号11号楼 (72)发明人 聂志华　郑友胜　薛蕙蓉　何晶　 杨德宸　 (74)专利代理 机构 北京清亦华知识产权代理事 务所(普通 合伙) 11201 专利代理师 何世磊 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 机械臂轨迹修正方法、 系统、 计算机及可读 存储介质 (57)摘要 本发明提供了一种机械臂轨迹修正方法、 系 统、 计算机及可读存储介质， 该方法包括通过路 径规划算法预先模拟出执行器将要经过的理论 运动路径； 分别计算出每两个 路径点之间各个关 节的理论运动轨迹， 并获取执行器在理论运动轨 迹中生成的理论线加速度和理论旋转角； 使伺服 控制器控制执行器依次按照各个路径点进行运 动； 通过惯 性传感器分别采集执行器经过每一路 径点产生的实际线加速度和实际旋转角， 并计算 出实际值与理论值之间的误差值； 再根据误差值 重新规划出执行器的执行路径， 以使执行器按照 执行路径进行运动。 通过上述方式能够在机械臂 运动的过程中实时获取到机械臂的误差值， 以实 时纠正机械臂的执行轨迹， 具有广阔的发展前 景。 权利要求书2页 说明书10页 附图2页 CN 114800523 A 2022.07.29 CN 114800523 A 1.一种机械臂轨迹修正方法， 应用于机械臂中， 所述机械臂包括底座以及与所述底座 连接的执行器， 所述底座与所述执行器之间设有若干关节以及伺服控制 器， 所述执行器内 设有惯性传感器， 其特 征在于， 所述方法包括： 通过路径规划算法预先模拟出所述执行器将要经过的理论运动路径， 所述理论运动路 径包括若干路径点； 通过预设函数分别计算出每两个所述路径点之间各个所述关节的理论运动轨迹， 并获 取所述执行器在所述理论 运动轨迹中生成的理论线加速度和理论旋转角； 将所述理论运动路径下发至所述伺服控制器， 以使所述伺 服控制器控制所述执行器依 次按照各个所述路径点进行运动； 通过所述惯性传感器分别采集所述执行器经过每一所述路径点产生的实际线加速度 和实际旋转角， 并计算出所述 实际线加速度、 所述实际旋转角与所述理论线加速度、 理论旋 转角之间的误差值； 通过所述路径规划算法根据 所述误差值重新规划出所述执行器的执行路径， 并将所述 执行路径下发至所述伺服控制器， 以使所述伺服控制器控制所述执行器按照所述执行路径 进行运动。 2.根据权利要求1所述的机械臂轨迹修正方法， 其特征在于： 所述通过路径规划算法预 先模拟出 所述执行器将要经 过的理论 运动路径的步骤 包括： 基于所述底座构建出空间坐标系， 并在所述空间坐标系中计算出所述执行器的位置变 量以及姿态变量， 其中， 所述位置变量用笛卡尔坐标表示： [x,y,z]， 所述姿态变量用RPY角 表示： [α， β， γ]； 基于所述路径规划算法根据所述位置变量以及所述姿态变量分别假定出若干所述路 径点， 其中， 第i个所述路径点表示 为： Pi＝[xi yi zi αi βi γi]T； 其中， Pi表示列向量， T表示 转置。 3.根据权利要求2所述的机械臂轨迹修正方法， 其特征在于： 所述通过所述惯性传感器 分别采集所述执 行器经过每一所述路径点产生的实际线加速度和实际旋转角的步骤 包括： 通过所述惯性传感器在所述空间坐标系中采集所述执行器相对于所述底座的旋转角， 并根据所述旋转角计算出旋转矩阵R， 以根据所述旋转矩阵R获取到所述执行器的实际旋转 角； 通过所述惯性传感器在所述空间坐标系中采集所述执行器相对于x轴、 y轴以及z轴的 平移加速度 并根据所述平 移加速度 获取到所述执 行器的实际线加速度。 4.根据权利要求1所述的机械臂轨迹修正方法， 其特征在于： 所述惯性传感器包括磁力 计、 两个陀螺仪以及加速度计， 所述磁力计、 两个所述陀螺仪以及所述加速度计分别固设于 所述执行器的表面。 5.根据权利要求1所述的机械臂轨迹修正方法， 其特征在于： 所述通过所述路径规划算 法根据所述误差值重新规划出所述执行器的执行路径， 并将所述执行路径下发至所述伺服 控制器， 以使所述伺服控制 器控制所述执行器按照所述执行路径进行运动的步骤之后， 所 述方法还 包括： 当所述执行器运动完成时， 将所述执行路径发送至控制平台， 并通过所述控制平台计权　利　要　求　书 1/2 页 2 CN 114800523 A 2算出所述执行路径与所述理论 运动路径之间的重合 率。 6.一种机械臂轨迹修正系统， 应用于机械臂中， 所述机械臂包括底座以及与所述底座 连接的执行器， 所述底座与所述执行器之间设有若干关节以及伺服控制 器， 所述执行器内 设有惯性传感器， 其特 征在于， 所述系统包括： 模拟模块， 用于通过路径规划算法预先模拟出所述执行器将要经过的理论运动路径， 所述理论 运动路径包括若干路径点； 第一计算模块， 用于通过预设函数分别计算出每两个所述路径点之间各个所述关节的 理论运动轨迹， 并获取所述执行器在所述理论运动轨迹中生成的理论线加速度和理论旋转 角； 第一执行模块， 用于将所述理论运动路径下发至所述伺服控制器， 以使所述伺服控制 器控制所述执 行器依次按照各个所述路径点进行运动； 第二计算模块， 用于通过所述惯性传感器分别采集所述执行器经过每一所述路径点产 生的实际线加速度和实际旋转角， 并计算出所述实际线加速度、 所述实际旋转角与所述理 论线加速度、 理论旋转角之间的误差值； 第二执行模块， 用于通过所述路径规划算法根据 所述误差值重新规划出所述执行器的 执行路径， 并将所述执行路径下发至所述伺服控制器， 以使所述伺服控制器控制所述执行 器按照所述执 行路径进行运动。 7.根据权利要求6所述的机 械臂轨迹修正系统， 其特 征在于： 所述模拟模块具体用于： 基于所述底座构建出空间坐标系， 并在所述空间坐标系中计算出所述执行器的位置变 量以及姿态变量， 其中， 所述位置变量用笛卡尔坐标表示： [x,y,z]， 所述姿态变量用RPY角 表示： [α， β， γ]； 基于所述路径规划算法根据所述位置变量以及所述姿态变量分别假定出若干所述路 径点， 其中， 第i个所述路径点表示 为： Pi＝[xi yi zi αi βi γi]T； 其中， Pi表示列向量， T表示 转置。 8.根据权利要求7所述的机械臂轨迹修正系统， 其特征在于： 所述第 二计算模块具体用 于： 通过所述惯性传感器在所述空间坐标系中采集所述执行器相对于所述底座的旋转角， 并根据所述旋转角计算出旋转矩阵R， 以根据所述旋转矩阵R获取到所述执行器的实际旋转 角； 通过所述惯性传感器在所述空间坐标系中采集所述执行器相对于x轴、 y轴以及z轴的 平移加速度 并根据所述平 移加速度 获取到所述执 行器的实际线加速度。 9.一种计算机， 包括存储器、 处理器以及存储在所述存储器上并可在所述处理器上运 行的计算机程序， 其特征在于， 所述处理器执行所述计算机程序时实现如权利要求1至5中 任意一项所述的机 械臂轨迹修正方法。 10.一种可读存储介质， 其上存储有计算机程序， 其特征在于， 该程序被处理器执行时 实现如权利要求1至 5中任意一项所述的机 械臂轨迹修正方法。权　利　要　求　书 2/2 页 3 CN 114800523 A 3