CN114227672A - 机械臂安全碰撞轨迹规划方法、装置、存储介质及设备 - Google Patents

Abstract

本发明涉及一种机械臂安全碰撞轨迹规划方法，通过采用五次B样条轨迹插值的方法，确立以运行时间量、运行过程中安全碰撞量两个指标为优化目标，考虑运动学约束条件中的位置、速度、加速度，利用快速非支配多目标优化算法进行多目标轨迹规划最优解求解，从而解决了经颅磁医疗机器人的机械臂运行轨迹安全性不可控、难以优化轨迹不连续和末端避障问题，以提高机器人辅助经颅磁系统的治疗效率和碰撞安全性。

Description

机械臂安全碰撞轨迹规划方法、装置、存储介质及设备

技术领域

本发明涉及康复医疗机械臂及轨迹规划技术领域，具体是关于一种辅助经颅磁刺激的机械臂安全碰撞轨迹规划方法、装置、存储介质及设备。

背景技术

经颅磁刺激是一种无痛的非侵入性的大脑神经刺激方法，已被应用于临床和科研领域，在康复科、儿科(自闭症、脑瘫等)、神经心理科(精分症、抑郁症等)等各个方面都得到了应用。目前已有辅助经颅磁线圈定位的机器人系统的报道，而且相关文献已经证明机器人辅助经颅磁线圈定位的方法可以显著提高刺激精度。

在机器人轨迹规划领域，多目标优化算法经过良好的训练能够求解出多条有效解集来满足需求，并展现出极好的优化性能。然而，目前已有的机器人轨迹多目标优化领域大多以工业应用为主，优化目标因素也倾向于工业应用的机器人本体。例如，中国专利申请CN103235513A公开了一种基于遗传算法的移动机械臂轨迹规划优化方法，但该方法优化后的关节1、3的加速度仍然出现不连续的情况，容易增大机械臂关节振动，加速零件磨损。

后来也踊跃出其他多目标优化算法改善了参数设置、结构更简单，更易跟随最优解。现有技术中提到的大多是粒子群算法，但是该算法主要用于连续性问题，经颅磁治疗过程中的轨迹并不保证连续，该算法在这种连续治疗轨迹运行过程中无法实现优化。在少数医疗机器人场景中，经颅磁辅助机器人相关文献中缺少刺激线圈与人体头部接触碰撞安全性轨迹规划方面的研究，例如，中国专利申请CN105676636A公开了一种基于NSGA-II算法的冗余度空间机械臂多目标优化方法，但该方法轨迹曲线阶数过高，增加计算难度，而且在运行过程中也不具有躲避障碍物的功能。

考虑到线圈与头部的接触情况对刺激效果的显著影响，因此需要一种既高效运行又能使得机械臂安全与人体头部不连续轨迹接触碰撞的方法，来使得机械臂辅助增加线圈和患者头部的安全贴合的问题。

发明内容

针对上述问题，本发明的目的是提供一种辅助经颅磁刺激的机械臂安全碰撞轨迹规划方法、装置、存储介质及设备，能够解决经颅磁医疗机器人的运行轨迹安全性不可控、难以优化轨迹不连续和末端避障问题，以提高机器人辅助经颅磁系统的治疗效率和碰撞安全性，为后续复杂的与人交互类机器人运行轨迹优化应用提供新思路。

为实现上述目的，本发明采取以下技术方案：

本发明所述的机械臂安全碰撞轨迹规划方法，包括如下步骤：

建立六轴机械臂的运动学模型，根据各个关节角度和末端执行器的位置建立六轴机械臂的正逆运动学方程，将机械臂空间坐标转化为笛卡尔坐标，进而描述关节空间坐标；

基于B样条函数插值方法初步拟合出末端执行器以及机械臂各个运动关节的运动方程，将机械臂各个关节的角度、速度、加速度和动力学参数设置为优化比较对象；

根据机械臂需要接触人体头部的工作任务要求，设定面向人体头部碰撞的优化目标函数集；

采用快速非支配多目标优化算法，考虑机械臂运动时各关节的速度、加速度和加加速度的物理极限，对B样条曲线的时间节点进行寻优，获得Pareto非支配解集；按照不同的权重分配原则在已获得的Pareto非支配解集中选取最优解，该解描述B样条曲线时间节点以及安全碰撞损害系数，进而生成相应时间-碰撞优化轨迹。

所述的机械臂安全碰撞轨迹规划方法，优选地，设定机械臂面向人体头部碰撞的优化目标函数的方法包括：

设定两个优化目标：运行时间最短和碰撞损害最小，所述两个优化目标的函数为运行时间目标函数和安全接触目标函数。

所述的机械臂安全碰撞轨迹规划方法，优选地，设定运行时间目标函数的方法为：

假设机械臂运行的一条轨迹，从起始点到终止点，一共经过n个路径点，所述n个路径点包含始末点，将n个路径点通过运动学求解与之对应每个关节空间下的n个关节角度值；设两个相邻节点的时间间隔为Δt_i；Δt_i为第i个节点对应的时间；因此，机械臂运行轨迹的总时间如下式所示：

式中，n为路径点，i为节点的序号；Δt_i为第i个节点对应的间隔时间；t_i+1为第i+1节点的时间，t_i为第i节点的时间。

所述的机械臂安全碰撞轨迹规划方法，优选地，设定安全接触目标函数的方法为：

通过MSI来评价机械臂对头部的伤害，MSI的计算公式如下：

式中，ΔT指时间间隔，g为重力加速度，I_E表示机械臂末端等效质量，I_H为人体头部的等效质量，v₀表示碰撞发生迁移时刻机械臂末端的速度；其中，A是由碰撞动力学模型决定的常数，且按下式计算：

式中，

T为碰撞持续时间；ΔT指时间间隔。

所述的机械臂安全碰撞轨迹规划方法，优选地，设定机械臂面向人体头部碰撞的优化目标函数的方法还包括：耦合运行时间目标函数和安全接触目标函数作为面向人体头部碰撞的优化目标函数，如下式所示：

式中，SRT为时间最短的优化目标函数；LCD为安全接触目标函数；v_i为第i个关节连杆末端线速度。

所述的机械臂安全碰撞轨迹规划方法，优选地，采用快速非支配多目标优化算法的具体方法如下：

(1)根据各个机械臂的关节最大允许速度v_Max确定各个路径点时间间隔范围t_i∈[Tm_i,TM_i]，其中Tm_i为相近两个节点之间最大极限速度下运动时间，TM_i是通过实验获得；在求得的运动时间间隔范围内随机生成初始父代种群G₀，种群数N；

(2)根据B样条函数关节轨迹通过优化目标函数计算适应值，在非支配排序后，通过遗传算法的三个基本操作得到第一代子代种群g₁，所述三个基本操作为选择、交叉和变异；

(3)从第二代开始，将父代与子代种群合并得到种群数为2N的种群R_t，通过优化函数式，计算适应值，将R_t种群中的2N个体按照非支配序号重新分类排序得到等级C_n并且对每个非支配层中的个体进行拥挤度计算，依据非支配关系以及个体的拥挤度挑选合适的个体形成新的种群数量为N的父代种群G_t+1；

(4)通过选择、交叉和变异的遗传算法三个基本操作得到子代种群g_t+1；

(5)重复(3)到(4)，直到满足程序结束的条件；最终得到满足约束条件的Pareto最优解。

本发明所述的机械臂安全碰撞轨迹规划装置，包括：

第一处理单元，用于建立六轴机械臂的运动学模型，根据各个关节角度和末端执行器的位置建立六轴机械臂的正逆运动学方程，将机械臂空间坐标转化为笛卡尔坐标，进而描述关节空间坐标；

第二处理单元，用于基于B样条函数插值方法初步拟合出末端执行器以及各个运动关节的运动方程，将机械臂各个关节的角度、速度、加速度和动力学参数设置为优化对象；

第三处理单元，用于根据医疗机械臂工作任务要求，设定面向人体头部碰撞的优化目标函数，所述优化目标函数既能满足机械臂的高效运作，又能保证机械臂与人头部接触时碰撞损害最小；

第四处理单元，用于采用快速非支配多目标优化算法，考虑机械臂运动时各关节的速度、加速度和加加速度的物理极限，对B样条曲线的时间节点进行寻优，获得Pareto非支配解集；按照不同的权重分配原则在已获得的Pareto非支配解集中选取解，该解描述B样条曲线时间节点以及安全碰撞损害系数，进而生成相应时间-碰撞优化轨迹。

本发明所述的计算机存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述的机械臂安全碰撞轨迹规划方法步骤。

本发明所述的计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述机械臂安全碰撞轨迹规划方法步骤。

本发明由于采取以上技术方案，其具有以下优点：

本发明对医疗类机器人的机械臂采用五次B样条轨迹插值的方法，确立以运行时间量、运行过程中安全碰撞量两个指标为优化目标，考虑运动学约束条件中的位置、速度、加速度，利用带有精英保留策略的快速非支配多目标优化算法进行多目标轨迹规划最优解求解，该方法解决经颅磁医疗机器人的机械臂的运行轨迹安全性不可控、难以优化轨迹不连续和末端避障问题，以提高机器人辅助经颅磁系统的治疗效率和碰撞安全性，为后续复杂的与人交互类机器人运行轨迹优化应用提供新思路。

附图说明

图1是本发明的六自由度协作机械臂试验平台整体框架模块示意图；

图2是本发明的轨迹规划验证所用六自由度协作机械臂结构示意图；

图3是本发明机械臂轨迹规划算子代数的计算流程图；

图4是本发明多目标函数路线图；

图5是快速非支配多目标优化优化算法流程图；

图6是节点1的运动时间和安全碰撞综合优化的Pareto前沿面；

图7是未优化的关节轨迹位置示意图；

图8是优化后的关节轨迹位置示意图；

图9是未优化的关节速度示意图；

图10是优化后的关节速度示意图；

图11是未优化的关节加速度示意图；

图12是优化后的关节加速度示意图；

图13是优化前后速度变化量之和的比较示意图；

图14是优化前后加速度变化量之和的比较示意图；

图15是优化前后碰撞损害系数和的比较示意图；

图中各附图标记为：

1-底座1；2-大臂；3-小臂；4-第一腕部；5-第二腕部；6-第三腕部。

具体实施方式

以下将结合附图对本发明的较佳实施例进行详细说明，以便更清楚理解本发明的目的、特点和优点。应理解的是，附图所示的实施例并不是对本发明范围的限制，而只是为了说明本发明技术方案的实质精神。

本发明提供一种机械臂安全碰撞轨迹规划方法，通过采用五次B样条轨迹插值的方法，确立以运行时间量、运行过程中安全碰撞量两个指标为优化目标，考虑运动学约束条件中的位置、速度、加速度，利用快速非支配多目标优化算法进行多目标轨迹规划最优解求解，从而解决了经颅磁医疗机器人的机械臂运行轨迹安全性不可控、难以优化轨迹不连续和末端避障问题，以提高机器人辅助经颅磁系统的治疗效率和碰撞安全性。

本发明提供的机械臂安全碰撞轨迹规划方法，包括如下步骤：

步骤1：建立六轴机械臂的运动学模型，根据各个关节角度和末端执行器的位置建立六轴机械臂的正逆运动学方程，将机械臂空间坐标转化为笛卡尔坐标，进而描述关节空间坐标。

具体的，如图1所示，本发明为了验证本发明中所提出的优化算法的有效性，在一个六自由度协作机械臂平台上进行了实验仿真测试。其中，如图2所示，机械臂是有6个关节的串联型机械臂，结构包括：底座1、大臂2、小臂3、第一腕部4、第二腕部5和第三腕部6。

为了便于处理操作臂的复杂几何参数建立D-H坐标系，每个连杆可以用四个参数来描述，即连杆转角α_i-1、连杆长度a_i-1、连杆偏距d_i和连杆扭角θ_i，参数如表1所示。

表1机械臂的D-H参数

根据各个连杆坐标系{i}相对于坐标系{i-1}的参数可以获得每个机械臂连杆位姿齐次变换矩阵

方程为：

式中，α_i-1为连杆转角；α_i-1为连杆长度；d_i为连杆偏距；θ_i为连杆扭角。

上述方程构成了机械臂的正运动学方程，可以表达出如何计算机械臂第i关节坐标系{i}相对于第i-1关节坐标系{i-1}的位姿。

其中，机械臂位姿验证需要运动学逆解，运动学逆问题求解是指可以在规定末端执行机构位置时确定每个关节的值，从而反向计算机械臂的预定姿态时各个关节的位置。

具体地，通过以下方式计算机械臂的预定姿态时各个关节的位置：

当

中的数值已知时，我们通过方程(2)反求θ_i：

式中，n_x，n_y，n_z，o_x，o_y，o_z，a_x，a_y，a_z分别为空间旋转算子；p_x，p_y，p_z分别为空间平移算子。

为机械臂第6关节坐标系{6}相对于基座坐标系{0}的齐次方程；

为机械臂第六关节坐标系{6}相对于第i关节坐标系{i}的齐次方程。

在空间中进行一段轨迹运行，完成机械臂准备工作。

根据所获得机械臂每一个连杆的齐次变换矩阵，将各个变换矩阵相乘，就可以得到机械臂末端执行机构相对于基座标的齐次变换矩阵，即：

式中，θ₁,θ₂,θ₃,θ₄,θ₅,θ₆分别为各连杆扭角；

令

可以得到机构相对基座标的位姿变换矩阵，分别为：

r₁₁＝c₆[c₅(s₁s₄+c₁c₂₃c₄)-c₁s₂₃s₅]-s₆(c₁c₂₃s₄-s₁c₄)

r₁₂＝-s₆[c₅(s₁s₄+c₁c₂₃c₄)-c₁s₂₃s₅]-c₆(c₁c₂₃s₄-s₁c₄)

r₁₃＝-c₁s₂₃c₅-s₅(c₁c₂₃c₄+s₁s₄)

r₁₄＝a₄c₁c₂₃-d₄c₁s₂₃+a₃c₁c₂

r₂₁＝c₆[c₅(s₁c₂₃c₄-c₁s₄)-s₁s₂₃s₅]-s₆(s₁c₂₃s₄-c₁c₄)

r₂₂＝-s₆[c₅(s₁c₂₃s₄-c₁c₄)-s₁s₂₃s₅]-c₆(s₁c₂₃s₄-c₁c₄)

r₂₃＝-s₁s₂₃c₅-s₅(s₁c₂₃c₄-c₁s₄)

r₂₄＝a₄s₁c₂₃-d₄s₁s₂₃+a₃s₁c₂

r₃₁＝c₆(c₂₃s₅-s₂₃c₄c₅)+s₂₃s₄s₆

r₃₂＝s₂₃s₄c₆-s₆(c₂₃s₅-s₂₃c₄c₅)

r₃₃＝s₂₃c₄s₅-c₂₃c₅

r₃₄＝-d₄c₂₃-a₄s₂₃-a₃s₂

r₄₁＝r₄₂＝r₄₃＝0,r₄₄＝1

根据一系列逆变换的分离变量求解，逆解出该机械臂各个关节变量的求解方程为：

上述方程中其中，c_i、s_i、c_ij和s_ij分别指cosθ_i、sinθ_i、cosθ_i cosθ_j-sinθ_isinθ_j与sinθ_icosθ_j+cosθ_isinθ_j；r_ij为齐次变换矩阵中的算子；

其中：

根据逆运动学分析，当θ₅＝0时，机械臂会处于机械结构的奇异位形，此时的关节轴4和关节轴6成一条直线，机器人末端连杆的运动只一种。在这种情况下，所有可能的解都是θ₄和θ₆的和或差。如果在奇异解情况下，可以任意选取θ₄，之后计算θ₆时，可以参照选取。

从笛卡尔空间的给定路径出发，进行运动学正、逆变换，得到关节空间的经点序列，如表2所示。

表2关节空间的经点序列

步骤2：基于B样条函数插值方法初步拟合出末端执行器以及机械臂各个运动关节的运动方程，将机械臂各个关节的角度、速度、加速度和动力学参数设置为优化比较对象。

具体的，在动态医疗环境中进行作业时，为了提高其对头部动态环境的适应性，基于B样条函数拟合出末端执行器以及各个运动关节的运动方程。其中，B样条函数如式(5)所示：

式中，Q_i(i＝0,1,L,n)为控制顶点，B_i,k(u)(i＝0,1,…,n)为k次规范B样条基函数，

为归一化时间向量，p(u)为时刻u处的关节位置。

可由德布尔递推公式求出基函数B_i,k(u)的表达式：

根据n+1个位置点P_i，得到符合约束条件的n+1个方程：

用n+1个方程求n+k个控制顶点显然不够，所以需要另外补充k-1个约束条件才能联立求解。一般取B样条曲线各阶导数，如取5阶B样条曲线，指定初始点速度、加速度，终止点速度、加速度。k次B样条的r阶导数，是k-r次B样条函数，则r阶导数公式：

如图3所示，在机械臂几何和运动学约束的基础上，设置约束的数值，结合步骤1中的轨迹规划算子，保证优化结果满足约束需求。

步骤3：根据机械臂需要接触人体头部的工作任务要求，设定面向人体头部碰撞的优化目标函数集，所述优化目标函数既能满足医疗机械臂的高效运作，又能保证机械臂与人头部接触时碰撞损害最小。

具体的，本发明设定两个优化目标：运行时间最短、碰撞损害最小。时间最短指的是机器人通过所有路径点所用的总时间最短；碰撞损害最小追求的是机器人运行过程中执行关节的碰撞伤害系数最小。

(1)运行时间目标函数：

假设机器人运行的一条轨迹，从起始点到终止点，一共经过n个路径点(包含始末点)，将n个路径点通过运动学求解与之对应每个关节空间下的n个关节角度值。设两个相邻节点的时间间隔为Δt_i；为第i个节点对应的时间。所以机器人运行轨迹的总时间如式所示。

式中，n为路径点，i为节点的序号；Δt_i为第i个节点对应的间隔时间；t_i+1为第i+1节点的时间，t_i为第i节点的时间。

(2)安全接触目标函数：

为了评估机器人碰撞对人头部造成的伤害，通常将HIC作为评价标准。MSI是从HIC发展来的另一个评价机械臂对头部伤害的标准。但是MSI的优点是不需要人体头部的加速度信息。MSI的计算公式如下：

ΔT通常指时间间隔，g为重力加速度，I_E表示机械臂末端等效质量，I_H为人体头部的等效质量，v₀表示碰撞发生迁移时刻机械臂末端的速度；A是由碰撞动力学模型决定的常数，且按下式计算：

式中，

T为碰撞持续时间，因此A由T决定。

结合图4，本发明中的机械臂要求既能短时间内到达指定位置完成作业，同时也要保证与人体头部安全碰撞。由MSI可知，降低末端机械臂速度是减少碰撞伤害的最主要的方式之一，但是降低速度明显会增长机械臂工作时间。所以优化机械臂关节运动时间和安全碰撞系数两个耦合且矛盾的性能指标，符合多目标优化模型建立的前提条件。由于机械臂在运行过程中时间间隔ΔT通常指定为15ms或36ms，机械臂末端与人体头部等效质量通常为固定值，因此仅考虑优化运动过程中的碰撞迁移时刻的末端速度。本发明的机械臂轨迹优化问题数学定义如下：

式中，SRT为时间最短的优化目标函数；LCD为安全接触目标函数。

步骤4：采用基于快速非支配多目标优化算法，考虑机械臂运动时各关节的速度、加速度和加加速度的物理极限，对B样条曲线的时间节点进行寻优，获得Pareto非支配解集。根据目标优化函数的权重占比在已获得的Pareto非支配解集中选取最优解，该解描述B样条曲线时间节点以及安全碰撞损害系数，进而生成相应时间-碰撞优化轨迹。

通过快速非支配多目标优化优化算法，如图5所示，得到运动时间和安全碰撞综合优化的Pareto前沿面，可提供给决策者一个较佳的选择空间。其采用快速非支配多目标优化算法的具体方法如下：

(1)根据各个机械臂的关节最大允许速度v_Max确定各个路径点时间间隔范围t_i∈[Tm_i,TM_i]，其中Tm_i为相近两个节点之间最大极限速度下运动时间，TM_i是通过实验获得，本发明选取60s作为上限。在求得的运动时间间隔范围内随机生成初始父代种群G₀，种群数N。

(2)根据B样条函数关节轨迹通过优化目标函数计算适应值。在非支配排序后，通过遗传算法的三个基本操作得到第一代子代种群g₁，三个基本操作为选择、交叉和变异。

(3)从第二代开始，将父代与子代种群合并得到种群数为2N的种群R_t，通过式优化函数式，计算适应值，将R_t种群中的2N个体按照非支配序号重新分类排序得到等级C_n并且对每个非支配层中的个体进行拥挤度计算，依据非支配关系以及个体的拥挤度挑选合适的个体形成新的种群数量为N的父代种群G_t+1。

(4)通过遗传算法的三个基本操作得到子代种群g_t+1。

(5)重复(3)到(4)，直到满足程序结束的条件。通过以上流程最终得到满足约束条件的Pareto最优解。

关节运动时间和安全碰撞系数是两个耦合且矛盾的运动性能指标，碰撞制约着时间最优性能。

选择合适的控制点初始解，开始迭代优化算法。根据约束条件及快速非支配多目标优化算法中各个参数设置可知，快速非支配多目标优化算法的参数设置如下：

初始种群大小：N＝200；

交叉概率：P＝0.8；

最大种群遗传代数：G＝100；

变异概率：M＝0.1。

通过快速非支配多目标优化优化算法，以节点1为例，得到运动时间和安全碰撞综合优化的Pareto前沿面如图6所示。由于存在目标之间的冲突和无法比较的现象，一个解在某个目标上是最好的，但在其他的目标上可能是最差的。因此这些非支配解较其他解而言拥有最少的目标冲突，可提供给决策者一个较佳的选择空间。

由图6可知，越靠近Y轴的点，机械臂执行轨迹的时间越短，越靠近X轴的点机械臂的碰撞伤害越小。可知关节运动时间和安全碰撞系数是两个耦合且矛盾的运动性能指标，碰撞制约着时间最优性能。

因为医疗机器人工作过程中在安全交互下，我们最关心的是治疗效率，所以在此目标函数中可以使运动时间权重系数略大于冲击权重系数，故选取时间函数的权重为0.6,安全碰撞函数的权重为0.4，得到运动时间和安全碰撞累计综合优化后的时间序列，如表3所示。

表3优化前后的关节运动时间序列

B样条基函数矩阵的值是通过输入结点和B样条的度数得到的。采用五次B样条作为插值曲线，设定所有关节的启停速度、加速度均为0。

由表3可以得到最优时间节点向量为t＝[0，8.65，22.77，44.96，56.07，63.26，69.09，82.66，95.1，112.72，118.16]，各个关节的优化前后位置、速度、加速度和冲击曲线可以求得。

由图7和图8可见，优化后机器人可以以更短的时间依次到达运动轨迹所必须经过的位置点，交优化之前的总运动时间缩短了33.07s，并且与优化之前的位置点保持一致，没有出现位置误差，说明基于快速非支配多目标优化算法在轨迹规划中可以提高机器人的作业效率。

由图9至12可见，从速度一时间、加速度一时间曲线图可以得出，优化后各个关节在运动开始会以较高的速度进行运动，保证了机器人的执行效率。速度改善特别明显的关节是3、6关节(由于第4关节没有发生位置变化，不再考虑其优化前后比较)，优化前后的优化比率分别为33.48％和32.56％，如图13所示。

同理，加速度改善特别明显的关节是3、5、6关节，优化前后的优化比率分别为41.17％，56.17和21.01％，如图14所示。加速度的减少改善可以减少机械臂扭转中产生的损害，可以延长使用寿命。

最终对比两个路径的LCD，每个关节安全碰撞损害系数改善均明显，优化前后的优化比率最低为49.4％，最高可达62.64％，如图15所示。证明有效的改善机械臂对人体头部产生的碰撞伤害。

3个关节的加速度和LCD都得到了有效改善，较优化之前的加速度明显减小。这说明快速非支配多目标优化优化算法可以有效地减小机器人运动过程中的冲击。理论上证明，优化后的轨迹有效的降低了机器人与患者之间的碰撞损伤。

本发明还提供一种机械臂安全碰撞轨迹规划装置，包括：

第一处理单元，用于建立六轴机械臂的运动学模型，根据各个关节角度和末端执行器的位置建立运动学方程，将机械臂空间坐标转化为笛卡尔坐标，进而描述关节空间坐标；

第二处理单元，用于基于B样条函数插值方法初步拟合出末端执行器以及各个运动关节的运动方程，将机械臂各个关节的角度、速度、加速度和动力学参数设置为优化对象；

第三处理单元，用于根据医疗机械臂工作任务要求，设定面向人体头部碰撞的优化目标函数，所述优化目标函数既能满足机械臂的高效运作，又能保证机械臂与人头部接触时碰撞损害最小；

第四处理单元，用于采用快速非支配多目标优化算法，考虑机械臂运动时各关节的速度、加速度和加加速度的物理极限，对B样条曲线的时间节点进行寻优，获得Pareto非支配解集；按照不同的权重分配原则在已获得的Pareto非支配解集中选取解，该解描述B样条曲线时间节点以及安全碰撞损害系数，进而生成相应时间-碰撞优化轨迹。

本发明还提供一种计算机存储介质，其上存储有计算机程序，所述计算机程序被处理器执行时实现上述的机械臂安全碰撞轨迹规划方法步骤。

本发明还提供一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述机械臂安全碰撞轨迹规划方法步骤。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (9)

1.一种机械臂安全碰撞轨迹规划方法，其特征在于，包括如下步骤：

建立六轴机械臂的运动学模型，根据各个关节角度和末端执行器的位置建立六轴机械臂的正逆运动学方程，将机械臂空间坐标转化为笛卡尔坐标，进而描述关节空间坐标；

基于B样条函数插值方法初步拟合出末端执行器以及机械臂各个运动关节的运动方程，将机械臂各个关节的角度、速度、加速度和动力学参数设置为优化比较对象；

根据机械臂需要接触人体头部的工作任务要求，设定面向人体头部碰撞的优化目标函数集；

采用快速非支配多目标优化算法，考虑机械臂运动时各关节的速度、加速度和加加速度的物理极限，对B样条曲线的时间节点进行寻优，获得Pareto非支配解集；按照不同的权重分配原则在已获得的Pareto非支配解集中选取最优解，该解描述B样条曲线时间节点以及安全碰撞损害系数，进而生成相应时间-碰撞优化轨迹。

2.根据权利要求1所述的机械臂安全碰撞轨迹规划方法，其特征在于，设定机械臂面向人体头部碰撞的优化目标函数的方法包括：

设定两个优化目标：运行时间最短和碰撞损害最小，所述两个优化目标的函数为运行时间目标函数和安全接触目标函数。

3.根据权利要求2所述的机械臂安全碰撞轨迹规划方法，其特征在于，设定运行时间目标函数的方法为：

假设机械臂运行的一条轨迹，从起始点到终止点，一共经过n个路径点，所述n个路径点包含始末点，将n个路径点通过运动学求解与之对应每个关节空间下的n个关节角度值；设两个相邻节点的时间间隔为Δt_i；Δt_i为第i个节点对应的时间；因此，机械臂运行轨迹的总时间如下式所示：

式中，n为路径点，i为节点的序号；Δt_i为第i个节点对应的间隔时间；t_i+1为第i+1节点的时间，t_i为第i节点的时间。

4.根据权利要求2所述的机械臂安全碰撞轨迹规划方法，其特征在于，设定安全接触目标函数的方法为：

通过MSI来评价机械臂对头部的伤害，MSI的计算公式如下：

式中，ΔT指时间间隔，g为重力加速度，I_E表示机械臂末端等效质量，I_H为人体头部的等效质量，v₀表示碰撞发生迁移时刻机械臂末端的速度；其中，A是由碰撞动力学模型决定的常数，且按下式计算：

式中，

T为碰撞持续时间；ΔT指时间间隔。

5.根据权利要求4所述的机械臂安全碰撞轨迹规划方法，其特征在于，设定机械臂面向人体头部碰撞的优化目标函数的方法还包括：耦合运行时间目标函数和安全接触目标函数作为面向人体头部碰撞的优化目标函数，如下式所示：

式中，SRT为时间最短的优化目标函数；LCD为安全接触目标函数；v_i为第i个关节连杆末端线速度。

6.根据权利要求1所述的机械臂安全碰撞轨迹规划方法，其特征在于，采用快速非支配多目标优化算法的具体方法如下：

(1)根据各个机械臂的关节最大允许速度v_Max确定各个路径点时间间隔范围t_i∈[Tm_i，TM_i]，其中Tm_i为相近两个节点之间最大极限速度下运动时间，TM_i是通过实验获得；在求得的运动时间间隔范围内随机生成初始父代种群G₀，种群数N；

(2)根据B样条函数关节轨迹通过优化目标函数计算适应值，在非支配排序后，通过遗传算法的三个基本操作得到第一代子代种群g₁，所述三个基本操作为选择、交叉和变异；

(3)从第二代开始，将父代与子代种群合并得到种群数为2N的种群R_t，通过优化函数式，计算适应值，将R_t种群中的2N个体按照非支配序号重新分类排序得到等级C_n并且对每个非支配层中的个体进行拥挤度计算，依据非支配关系以及个体的拥挤度挑选合适的个体形成新的种群数量为N的父代种群G_t+1；

(4)通过选择、交叉和变异的遗传算法三个基本操作得到子代种群g_t+1；

(5)重复(3)到(4)，直到满足程序结束的条件；最终得到满足约束条件的Pareto最优解。

7.一种机械臂安全碰撞轨迹规划装置，其特征在于，包括：

第一处理单元，用于建立六轴机械臂的运动学模型，根据各个关节角度和末端执行器的位置建立六轴机械臂的正逆运动学方程，将机械臂空间坐标转化为笛卡尔坐标，进而描述关节空间坐标；

第二处理单元，用于基于B样条函数插值方法初步拟合出末端执行器以及各个运动关节的运动方程，将机械臂各个关节的角度、速度、加速度和动力学参数设置为优化对象；

第三处理单元，用于根据医疗机械臂工作任务要求，设定面向人体头部碰撞的优化目标函数，所述优化目标函数既能满足机械臂的高效运作，又能保证机械臂与人头部接触时碰撞损害最小；

第四处理单元，用于采用快速非支配多目标优化算法，考虑机械臂运动时各关节的速度、加速度和加加速度的物理极限，对B样条曲线的时间节点进行寻优，获得Pareto非支配解集；按照不同的权重分配原则在已获得的Pareto非支配解集中选取解，该解描述B样条曲线时间节点以及安全碰撞损害系数，进而生成相应时间-碰撞优化轨迹。

8.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被处理器执行时实现权利要求1-6中任一项所述的机械臂安全碰撞轨迹规划方法步骤。

9.一种计算机设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现权利要求1-6任意一项所述机械臂安全碰撞轨迹规划方法步骤。

Images