一种适用于半球形内部空间的六轴机械臂运动控制方法与流程

1.本发明属于工业机械人运动控制相关领域，具体涉及一种适用于半球形内部空间的六轴机械臂运动控制方法。


背景技术：

2.核电厂停堆大修期间，需在反应堆压力容器、主泵、蒸汽发生器等容器内进行检维修作业，由于核电环境辐射特殊性，特别是核岛设备，如蒸汽发生器一次侧水室，类似半球形空间(中间隔板使其分割成对称的四分之一球形)，属于辐射高剂量区，加之高温的恶劣工作条件，在目前人工作业过程中，人员受照剂量和体力消耗都较大，且作业质量受人因因素影响较大，放射性污染的风险和人因风险也较大。


技术实现要素：

3.本发明的目的在于提供一种适用于半球形内部空间的六轴机械臂运动控制方法，能够针对现有的核电容器内作业的复杂环境，借助远程控制的方式替代人工完成核电容器内检维修作业，以彻底实现高危区机器替代作业，同时提高安全性及可靠性，减少人因失误。
4.本发明的技术方案如下：一种适用于半球形内部空间的六轴机械臂运动控制方法，包括如下步骤：
5.步骤1：实现点到点在线运动算法；
6.步骤2：机器人运动实现。
7.所述的步骤1中点到点在线运动算法实现包括从计算周期开始，计算机更新运动指令，采用运动学正解解算矩阵变换模块，获取当前机器人空间位置，规划相对空间运动轨迹，再采用运动学逆解解算矩阵变换模块，对各关节运动轨迹进行规划，如符合安全准则，则同步控制六关节运动，如违反安全准则，则返回“规划相对空间运动轨迹”；直至接收到“停止运行”指令，则计算周期结束，否则返回至“更新运动指令”。
8.所述的步骤2中机器人运动实现过程包括启动机器人后，在笛卡尔坐标系下建立机器人正逆解模型，然后引入球面坐标系，确定直观的相对坐标，接收运动指令，获得一个1/4球形内运动控制算法；再通过镜面投影，获得另一1/4球形内运动控制算法，最终获得半球形内运动控制算法。
9.所述的步骤2包括建立机械臂d-h模型运用运动学正逆解算法。
10.所述的步骤2的算法实现时舍去多解保留单一解，使机械臂在四分之一球形内按照既定轨迹实现自动运动控制；通过镜面投影，使机械臂在对称分布的另一四分之一球形内同步运动控制，从而满足半球形内空间的运动控制。
11.本发明的有益效果在于：1)基于六轴机械臂特殊的rrrprr构型，构建笛卡尔坐标系和球面坐标系双模空间系运动数理模型，极大地方便了绝对坐标与相对坐标的换算，为定位精度的定量分析提供了直观的方法；2)按照常规笛卡尔坐标系运动建模方法建立机器
300
°
0θ34090
°
d050-90
°
l3θ5600
°
l4θ6[0026][0027][0028][0029][0030][0031][0032]
根据式
⑴
～式
⑹
可得机器人手爪末端相对于安装基准轴系的运动正解模型
[0033][0034]
即
[0035][0036][0037]
[0038][0039]
其中s、c分别为sin和cos简写，和分别为齐次矩阵的第1～4列向量。
[0040]
那么末端负载在全局坐标系下的位置为：
[0041][0042]
球面坐标变换解：
[0043]
假设机器人安装基准轴系与球面坐标系重合(实际上有一定平移偏差，此处为简化计算，暂不做补偿考虑)，那么有
[0044][0045]
根据式
⑺
和式(13)可进行球面坐标系和机器人直角坐标系间的变换运算。
[0046]
逆解模型：
[0047]
取末端负载位姿表示为
[0048][0049]
那么坐标系{0}与坐标系{6}间的空间转换矩阵为
[0050][0051]
那么借由式
⑹
可得
[0052][0053]
同时另由式
⑹
、
⑺
可得
[0054][0055]
令l3＝0，则由式
⒂
、
⒃
可得到两组解
[0056][0057]
由式
⑹
和式
⒁
可得
[0058][0059]
根据本机器人转换矩阵特性有
[0060][0061]
那么根据式
⑽
和式
⒅
联立求解方程组可得
[0062][0063]
或
[0064][0065]
或满足
[0066]
的无限多解。其中
[0067][0068]
从而获得机器人运动逆解模型的解析解方程。
[0069]
步骤2：机器人运动实现
[0070]
如图2所示，机器人运动实现过程启动机器人后，在笛卡尔坐标系(绝对坐标)下建
立机器人正逆解模型，如步骤1中所述的正解模型、逆解模型，然后引入球面坐标系(相对坐标)，以确定直观的相对坐标，通过作业人员实时操控，接收运动指令，获得一个1/4球形内运动控制算法；再通过镜面投影，获得另一1/4球形内运动控制算法，最终获得半球形内运动控制算法。
[0071]
六轴机械臂为rrrprr六轴串联构型(如图3所示)，建立机械臂d-h模型(d-h参数见表1)，运用运动学正逆解算法，避免空间干涉状况，为避免程序计算溢出，在算法实现时可舍去多解保留单一解，使机械臂在四分之一球形内按照既定轨迹实现自动运动控制；通过镜面投影，使机械臂在对称分布的另一四分之一球形内同步运动控制，从而满足半球形内空间的运动控制。
[0072]
机器人运动最关键的是把操作人员基于大地坐标系的直观操作转换为机器人多轴的同步运动，为此，机器人运动实现采用如图1所示ptp在线运动算法流程：在每一个计算周期(或采样周期)，人机交互模块接收来自操作人员的上、下、左、右、前、后及三轴向偏转等空间复合指令动作，解析为从当前初始点运动至下一轨迹点的指令，通过机器人运动学正解获取当前机器人空间位姿，再根据点到点关系规划相对空间位移轨迹，通过机器人运动学逆解方程获得各关节运动轨迹，在符合安全准则情况下同步各关节实施相应的运动。
[0073]
如图2所示，该算法流程为内嵌的在线计算流程，计算周期、轨迹规划密度和运行速度均可调，可使机器人运动体现出实时、平滑、灵敏等特点。
[0074]
如图3所示，基于机械臂ppprpp构型，在每个关节上建立直角坐标系，得出机械臂d-h模型(d-h参数见表1)。
[0075]
如图4所示，基于半球形空间被中间隔板分割成对称的两个四分之一球形，获得一个四分之一球形内运动控制算法后，通过镜面投影即可推导出另一个四分之一球形内的运动控制算法，在保证运动可达性的基础上，大大简化了运算量。


技术特征：
1.一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于，包括如下步骤：步骤1：实现点到点在线运动算法；步骤2：机器人运动实现。2.如权利要求1所述的一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于：所述的步骤1中点到点在线运动算法实现包括从计算周期开始，计算机更新运动指令，采用运动学正解解算矩阵变换模块，获取当前机器人空间位置，规划相对空间运动轨迹。3.如权利要求2所述的一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于：所述的步骤1采用运动学逆解解算矩阵变换模块，对各关节运动轨迹进行规划，如符合安全准则，则同步控制六关节运动，如违反安全准则，则返回“规划相对空间运动轨迹”；直至接收到“停止运行”指令，则计算周期结束，否则返回至“更新运动指令”。4.如权利要求1所述的一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于：所述的步骤2中机器人运动实现过程包括启动机器人后，在笛卡尔坐标系下建立机器人正逆解模型。5.如权利要求4所述的一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于：所述的步骤2引入球面坐标系，确定相对坐标，接收运动指令，获得一个1/4球形内运动控制算法；再通过镜面投影，获得另一1/4球形内运动控制算法，最终获得半球形内运动控制算法。6.如权利要求1所述的一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于：所述的步骤2包括建立机械臂d-h模型运用运动学正逆解算法。7.如权利要求6所述的一种适用于半球形内部空间的六轴机械臂运动控制方法，其特征在于：所述的步骤2的算法实现时舍去多解保留单一解，使机械臂在四分之一球形内按照既定轨迹实现自动运动控制；通过镜面投影，使机械臂在对称分布的另一四分之一球形内同步运动控制，从而满足半球形内空间的运动控制。

技术总结
本发明属于工业机械人运动控制相关领域，具体涉及一种适用于半球形内部空间的六轴机械臂运动控制方法。步骤1：实现点到点在线运动算法；步骤2：机器人运动实现。有益效果在于：1)基于六轴机械臂特殊的RRRPRR构型，构建笛卡尔坐标系和球面坐标系双模空间系运动数理模型，极大地方便了绝对坐标与相对坐标的换算，为定位精度的定量分析提供了直观的方法；2)按照常规笛卡尔坐标系运动建模方法建立机器人正逆解模型，然后引入球面坐标变化解统一球面坐标系下的模型表达，作为作业人员实时操控的数理算法基础；3)获得一个四分之一球形内运动控制算法后，通过镜面投影即可推导出另一个四分之一球形内的运动控制算法，大大简化了运算量。大大简化了运算量。大大简化了运算量。


技术研发人员：肖可 胡娜 刘涛 杨斌
受保护的技术使用者：中核武汉核电运行技术股份有限公司
技术研发日：2023.04.21
技术公布日：2023/9/20