(19)国家知识产权局 (12)发明 专利申请 (10)申请公布号 (43)申请公布日 (21)申请 号 202210785947.2 (22)申请日 2022.07.04 (71)申请人 中国科学院沈阳自动化研究所 地址 110016 辽宁省沈阳市沈河区南塔街 114号 (72)发明人 姜勇　王洪光　王竣禾　 (74)专利代理 机构 沈阳科苑专利商标代理有限 公司 210 02 专利代理师 周宇 (51)Int.Cl. B25J 9/16(2006.01) (54)发明名称 一种机械臂姿态速度规划控制方法 (57)摘要 本发明涉及一种机械臂姿态速度规划控制 方法， 属于轨迹规划领域。 本发明包括： 步骤一， 获取机械臂末端连杆坐标系当前姿态与 目标姿 态之间的旋转变换矩阵， 并将其表 示成欧拉角形 式。 步骤二， 将欧拉角的各个角度乘以阻尼系数， 得到机械臂末端连杆坐标系下期望旋转速度， 然 后通过机器人正向运动学矩阵， 将其转换到机械 臂基坐标系下。 步骤三， 按按照 欧拉角的旋转次 序依次对绕基坐标系各个轴的旋转进行速度控 制， 三个控制周 期后重复上述两个步骤， 直到机 械臂末端到 达目标姿态。 权利要求书1页 说明书3页 附图1页 CN 115008468 A 2022.09.06 CN 115008468 A 1.一种机 械臂姿态速度规划控制方法， 其特 征如下， 包括以下步骤。 步骤1： 获取机械臂末端连杆坐标系当前姿态与目标姿态之间的旋转变换矩阵， 并将其 表示成欧拉角形式； 步骤2： 将欧拉角的各个角度分别乘以阻尼系数， 得到机械臂末端连杆坐标系下的期望 旋转速度； 然后通过机器人正向运动学矩阵， 将其转换到机械臂基坐标系下， 得到机械臂基 坐标系下的基坐标 下末端连 杆坐标系的期望 速度。 步骤3： 按照欧拉角的旋转 次序依次对绕基坐标系各个轴的旋转进行速度控制， 三个控 制周期后重复上述两个步骤， 直到 机械臂末端到 达目标姿态。 2.根据权利要求书1所述的一种机械臂姿态速度规划控制方法， 其特征在于， 所述将旋 转矩阵转换成内旋形式表达的欧拉角包括： 定义旋转次序： β 、 γ、 α； 计算旋转矩阵到内旋形式欧拉角的变换公式如下： 其中， RP表示目标姿态到当前姿态之间的旋转变换矩阵， β 为机 械臂末端 首先绕着 基坐标系的z轴旋转的角度， γ为然后绕着旋转后坐标系的y轴旋转的角 度， α 为最后绕着旋转后坐标系的x轴旋转的角度。 3.根据权力要求书1所述的一种机械臂姿态速度规划控制方法， 其特征在于， 所述基坐 标下末端 连杆坐标系的期 望速度为： [wx,wy,wz]， 用于利用雅可比矩阵， 通过机械臂微分运 动学对机 械臂进行速度控制。 4.根据权力要求书1所述的一种机械臂姿态速度规划控制方法， 其特征在于， 所述按照 欧拉角的旋转次序， 一个周期 内只对期望旋转速度中的一个分量进行控制； 且因为仅对姿 态进行控制， 因此 单一周期的速度分量有如下三种情况： 其中， v中前3个分量为线速度， 后3个分量为角速度， 分别对应当前周期的期望旋转速 度。权　利　要　求　书 1/1 页 2 CN 115008468 A 2一种机械臂姿态 速度规划控制方 法 技术领域 [0001]本发明涉及轨 迹规划领域， 具体地说是一种机 械臂姿态速度规划控制方法。 背景技术 [0002]在控制机械臂运动时， 一般需要首先对从起始点到终止点之间的路径进行规划， 以实现避障， 同时使机械臂 运动轨迹更加平滑， 进而确保安全性和顺滑性。 轨迹具体的方法 有多项式插值的轨迹规划， 贝塞尔曲线轨迹规划， 基于B样条插值的轨迹规划 等。 在对姿态 规划的时候， 首先需要使用四元数等适合姿态插值的方式进行表示， 再使用轨迹规划方法 进行规划出 各个路径点的角度， 角速度等 值。 发明内容 [0003]本发明解决的是在控制机械臂姿态调整时， 必须先进行姿态规划的问题。 通过使 用欧拉角表示期望旋转， 并在阻尼系 数的作用下， 得到机械臂末端连杆坐标系下旋转的速 度表示。 最后将其转换到机械臂基坐标系 下， 然后按照欧拉角旋转次序分别对绕各个坐标 系的旋转速度进行控制， 实现不需要提前规划就可以实现姿态 平滑变化的效果。 [0004]本发明采用的技 术方案是： [0005]一种机械臂姿态速度规划控制方法， 包括以下步骤。 [0006]步骤1： 获取机械臂末端连杆坐标系当前姿态与目标姿态之间的旋转变换矩阵， 并 将其表示成欧拉角形式； [0007]步骤2： 将欧拉角的各个角度分别乘以阻尼系数， 得到机械臂末端连杆坐标系下的 期望旋转速度； 然后通过机器人正向运动学矩阵， 将其转换到机械臂基坐标系下， 得到机械 臂基坐标系下的基坐标 下末端连 杆坐标系的期望 速度。 [0008]步骤3： 按照欧拉角的旋转次序依次对绕基坐标系各个轴的旋转进行速度 控制， 三 个控制周期后重复上述两个步骤， 直到 机械臂末端到 达目标姿态。 [0009]所述将旋转矩阵转换成内旋形式表达的欧拉角包括： [0010]定义旋转次序： β 、 γ、 α； [0011]计算旋转矩阵到内旋形式欧拉角的变换公式如下： [0012] [0013]其中， RP表示目标姿态到当前姿态之间的旋转变换矩阵， β 为机械臂末端 首先绕着 基坐标系的z轴旋转的角度， γ为然后绕着旋转后坐标系的y轴旋转 的角度， α 为 最后绕着旋转后坐标系的x轴旋转的角度。说　明　书 1/3 页 3 CN 115008468 A 3