[发明专利]一种冗余双臂服务机器人实时避碰的轨迹规划方法及平稳运行方法

摘要

一种冗余双臂服务机器人实时避碰的轨迹规划方法，通过分析分析主臂连杆与环境障碍物间的最短距离、从臂的连杆作为障碍物时主臂连杆与障碍物的关系、从臂连杆与环境障碍物之间的关系、主臂连杆视为障碍物时，从臂连杆与障碍物之间关系，该方法简单易行，容易操作，能够实时改变避障速度，各关节运动连续、平稳，且可保证双臂各连杆间及连杆与环境障碍物间的最近距离始终大于设定的安全距离。

主权项

1.一种冗余双臂服务机器人实时避碰的轨迹规划方法，其特征在于它包括以下步骤：(1)分析主臂的第i个连杆与环境障碍物k之间的最短距离：取主臂的第i个连杆、从臂的第j个连杆以及环境障碍物k为研究对象，用点分别代表主臂第i连杆的起始关节与末端关节，其在基坐标系O₀‑x₀y₀z₀下的坐标分别用表示，用表示双臂胶囊体包围模型中主臂的第i连杆的长度，则该主臂的第i连杆的单位向量为：其中，代表此刻主臂的第i个连杆轴线上与环境障碍物k相距最近的点，的位置与值的正负有关；为的转置，为机器人视觉信息定位的环境障碍物k在冗余双臂机器人参考坐标系O₀‑x₀y₀z₀下的位置坐标，则主臂的第i连杆的轴线上距离环境障碍物k的最近点在基坐标系下的位置为：其中，i代表主臂的第i个连杆，k代表第k个环境障碍物，主臂的第i连杆轴线上距离环境障碍物k的最近点记为则环境障碍物k的中心与主臂的第i个连杆轴线之间的最短距离为：考虑到实际机械臂的模型不能简单的用一条线段来表示，因此实际上环境障碍物k与主臂的第i个连杆间的最短距离为：其中，代表冗余双臂机器人的胶囊体简化模型中主臂第i个连杆的半径，为环境障碍物k的半径；(2)分析主臂与从臂在协调搬运物体时，从臂的第j个连杆视为主臂第i个连杆的障碍物时主臂的第i个连杆与障碍物之间的几何关系：2.1用点表示从臂第j连杆的起始关节，其在冗余双臂机器人参考坐标系O₀‑x₀y₀z₀下的坐标用表示，将从臂第j连杆的起始关节视为主臂第i个连杆的障碍物，此时主臂第i连杆轴线上距离障碍物的最近点记为同步骤(1)一样可求得主臂第i连杆的轴线上距离障碍物21的最近距离为：考虑到实际的机械臂模型不能简单的用一条线段来表示，因此实际上障碍物与主臂第i个连杆之间的最短距离为：其中，代表冗余双臂机器人的胶囊体简化模型中从臂第j个连杆的半径；2.2用点表示从臂第j连杆的末端关节，其在冗余双臂机器人参考坐标系O₀‑x₀y₀z₀下的坐标用表示，将从臂第j个连杆的末端关节视为主臂第i个连杆15的障碍物，此时同步骤(1)一样可求得主臂的第i个连杆轴线上与障碍物间的最近距离为：主臂的第i连杆轴线上距离障碍物最近的点记为进而求得实际上主臂第i个连杆与障碍物间的最近距离为：利用步骤(1)和步骤(2)即可求得环境障碍物k在主臂各个连杆上的最近距离d^*_l1以及从臂的每个连杆起始关节和末端关节视为主臂的障碍物时，其分别到主臂每个连杆上的最近距离d^*_l2，d^*_l3，进一步可得到环境障碍物k以及从臂上包括起始关节与末端关节在内的连杆的关节，视为主臂的障碍物时，此时，障碍物包括环境障碍物k及从臂上每个连杆的关节，与主臂的最近距离为：此时，对应的主臂的连杆轴线上与障碍物，包括环境障碍物k及从臂上每个连杆的关节，相距最近的点记为x_l0，称其为标志点；(3)分析从臂的第j连杆与环境障碍物k之间的最短距离：从臂的第j个连杆与环境障碍物k之间的几何关系，用点分别代表从臂第j连杆的起始关节与末端关节，其在冗余双臂机器人参考坐标系O₀‑x₀y₀z₀下的坐标分别用表示，为双臂的胶囊体包围模型中从臂的第j连杆的长度，则该从臂的第j连杆的单位向量为：其中，代表此刻从臂的第j连杆轴线上与环境障碍物k相距最近的点，的位置与值的正负有关，为的转置，则从臂的第j连杆轴线上距离环境障碍物k的最近点在基坐标系下的位置为：其中，j代表从臂的第j个连杆，k代表第k个环境障碍物，则从臂第j个连杆轴线上与环境障碍物k间的最短距离则为：考虑到实际机械臂的模型不能简单的用一条线段来表示，因此实际上从臂的第j个连杆与环境障碍物k间的最短距离为：从臂的第j个连杆的轴线上距离环境障碍物k的最近点记为其中，代表冗余双臂机器人的胶囊体简化模型中从臂第j个连杆的半径；(4)分析主臂与从臂在协调搬运物体时，主臂的第i个连杆视为从臂第j个连杆的障碍物，此时障碍物用主臂第i个连杆的起始关节和末端关节代替，时从臂的第j个连杆与障碍物之间的几何关系：4.1将主臂第i连杆的起始关节视为从臂第j个连杆的障碍物时，利用步骤(2)的原理可求实际上从臂的第j个连杆与障碍物间最近距离为：从臂的第j个连杆轴线上距离障碍物最近的点记为4.2将主臂第i连杆的末端关节视为从臂的第j个连杆障碍时，利用步骤(2)的原理求得从臂第j个连杆与障碍物间的最近距离为：从臂第j个连杆的轴线上距离障碍物的最近点记为利用步骤(3)和步骤(4)即可求得环境障碍物k在从臂各个连杆上的最近距离d^*_f1以及主臂各连杆的起始关节和末端关节视为从臂的障碍物时，其分别到从臂各个连杆上的最近距离d^*_f2，d^*_f3，进一步可得到环境障碍物k以及主臂每个连杆的关节，包括起始关节与末端关节，视为从臂的障碍物时，与从臂的最近距离为：此时障碍物包括环境障碍物k及主臂上每个连杆的关节，对应的从臂的连杆轴线上与障碍物相距最近的点记为x_f0，同样称其为标志点；(5)利用连杆变换矩阵实时求解出机械臂各关节点的位置信息，当检测到障碍物与标志点的距离d₀小于设定的阈值时，标志点将产生一个避障速度，则关节角速度函数为：其中，J为末端雅克比矩阵，J⁺为J的广义逆，为末端速度，I为单位矩阵，J₀为改进前的标志点的雅克比矩阵，当标志点的位置确定后，其值可由运动学方程直接求出；为预先设置的三维避障速度，等式右边的第一项为满足机械手末端轨迹跟踪所需的速度，第二项是在不影响末端轨迹的前提下标志点的避障速度；由于机械臂处于三维空间状态，则避障速度也将是三维的，为简化计算量，提高轨迹规划算法效率，利用改进的缩减避障运动操作空间的方法得到机器人实时避障的轨迹。