动态环境下机械臂分层运动规划方法及设备

1.动态环境下机械臂分层运动规划方法，其特征在于，具体步骤如下：步骤1：设定机械臂的目标位置，同时考虑静态障碍物的位置，规划机械臂每个关节当前位置到目标位置的无碰撞初始全局路径；步骤2：对初始全局路径进行平滑处理，规划机械臂运动轨迹；步骤3：根据规划的机械臂轨迹，生成机械臂各关节角速度控制量；步骤4：在规避静态障碍物的基础上检测到机械臂移动范围中包含动态障碍物时，结合动态控制障碍函数构建动态避障框架，在动态避障框架中引入信号时序逻辑进行任务约束，控制机械臂进行动态避障。 2.根据权利要求1所述的动态环境下机械臂分层运动规划方法，其特征在于：步骤1中，通过快速行进树算法FMT*产生全局路径，同时考虑机械臂自身约束以及与静态障碍物的安全距离约束：在机械臂的关节空间中，FMT*算法需要连接采样点以形成搜索树，并寻找从起始位置到目标位置的最优路径；在进行单步拓展时，FMT*算法在找到单步最优连接后利用几何包络法进行碰撞检测，确保路径与任何障碍物不发生碰撞才将路径加入路径树中；构建包络模型，使用不同半径圆柱体包络机械臂的关节和连杆，用球体包络静态障碍物、动态障碍物，并借助包络模型进行碰撞检测。 3.根据权利要求2所述的动态环境下机械臂分层运动规划方法，其特征在于，采用三次样条插值方法对FMT*算法生成的初始全局路径进行平滑处理过程如下：将机械臂第i个关节对应的初始全局路径表示为一组数量为n的序列路径节点 ，q表示机械臂关节的角度，采用分段三次样条插值方法对整个路径进行拟合，生成平滑且连续的轨迹来连接这些离散的路径节点；对第i个关节的每两个相邻路径节点 和 之间的区间 ，构造分段三次多项式 ： ，式中， 表示当前时间， 是多项式系数，通过路径节点 的位置约束、连续性约束和边界条件求解，j表示第j个路径节点；通过整合所有路径节点 的位置约束、连续性约束和边界条件，得到求解所有两个相邻路径节点之间的分段三次多项式系数的方程组，通过求解该方程组确定出每一段多项式的系数 ；将所有分段三次多项式进行连接，就形成了一条不仅精确通过所有路径节点 ，而且在速度和加速度上都连续变化的平滑轨迹，满足机械臂各关节角速度控制量的要求。 4.根据权利要求3所述的动态环境下机械臂分层运动规划方法，其特征在于，使用线性二次调节器LQR生成的机械臂各关节角速度控制量的过程如下：设机械臂在三维空间中运动的状态量机械臂各关节角度表示为 ，其中 表示第i个机械臂关节的角度；设机械臂关节角速度控制量 ，机械臂的状态空间方程表示如下： ，式中，零矩阵 与单位矩阵 ，机械臂关节角速度控制量u与状态量 之间满足下式： ，LQR的目标函数最小化J表示如下： ，式中，Q为半正定矩阵，用于惩罚状态误差；R为正定矩阵，用于机械臂各关节角速度控制量的惩罚控制量的能量消耗，通过调节Q和R的权重，平衡机械臂的状态跟踪精度和机械臂各关节角速度控制量的能耗；使用LQR求解机械臂关节角速度控制量：将代数黎卡提方程ARE的解作为LQR的控制量，设存在正定矩阵P满足ARE方程： ，获得最优反馈增益矩阵K表示： ，由此得到机械臂各关节角速度最优控制量如下式所示： ，式中，K为LQR增益矩阵，q init 与q goal 分别为机械臂当前位姿与目标位姿，在机械臂执行轨迹跟踪任务时，若当前时刻点小于分段三次样条插值的最大时刻，q goal 为下一时刻对应的关节角度值；若当前时刻大于分段三次样条插值的最大时刻，则q goal 为最大时刻对应的分段三次样条插值位姿。 5.根据权利要求4所述的动态环境下机械臂分层运动规划方法，其特征在于，控制机械臂进行动态避障具体步骤包括：当线性二次调节器跟踪过程中不存在动态障碍物且不存在信号时序逻辑任务，则由线性二次调节器控制机械臂执行机械臂运动轨迹；当线性二次调节器跟踪过程中存在动态障碍物时，根据动态障碍物计算得到动态控制障碍函数；当线性二次调节器跟踪过程中还存在信号时序逻辑任务时，使用信号时序逻辑范式描述时序任务，解析信号时序逻辑范式得到原子谓词，根据原子谓词设计时变控制障碍函数，结合信号时序逻辑时序约束和时变控制障碍函数重新进行机械臂运动轨迹规划，确保机械臂不仅能够在动态环境中安全避开动态障碍物，还能严格按照信号时序任务的逻辑执行路径规划；将动态控制障碍函数和时变控制障碍函数作为约束条件，通过求解机械臂各关节角速度控制量的二次规划问题，得到使机械臂避开动态障碍物的同时完成时序约束的机械臂各关节角速度控制量，使机械臂无碰撞到达目标位置。 6.根据权利要求5所述的动态环境下机械臂分层运动规划方法，其特征在于，在规避静态障碍物的基础上，同时避开动态障碍物的动态避障框架中构建动态控制障碍函数DCBF的工作流程如下：将机械臂的关节和连杆分别包络简化为不同半径圆柱体，将动态障碍物包络简化为球体；对每个连杆都要构造DCBF，通过对安全距离的约束，利用DCBF确保机械臂在运动过程中连杆始终与动态障碍物保持安全距离，表达式如下： ，式中，D为动态障碍物球体的球心到连杆包络圆柱轴线的距离， 为动态障碍物球体半径 与连杆包络圆柱内径 之和；h表示动态控制障碍函数；只要机械臂的DCBF初始状态 即可确保机械臂的前向不变性，保证代表连杆的圆柱体不会与代表动态障碍物球体接触，机械臂就不会进入碰撞的危险状态，DCBF的一阶导数 满足下式条件： ，式中， 为收敛速率的系数；进一步展开得到机械臂约束条件： ，式中， 为动态障碍物的当前速度， 为距离D的一阶导数；设动态障碍物球体球心为 ，机械臂的一条代表连杆的圆柱体两端关节为 与 ，利用下式计算 在 轴线上的投影向量与向量 自身长度的比值z： ，式中， 代表从 指向 的向量； 代表从 指向 的向量； 代表从 指向 的向量的模长；通过比值z判断代表动态障碍物球体球心的垂足O的位置，若 则垂足O落在代表机械臂连杆的圆柱体轴线 上；否则垂足O落在代表机械臂连杆的圆柱体轴线 的延长线上，若 ，则代表从动态障碍物的球体球心 到代表机械臂连杆的圆柱体的关节端 处的距离最近，距离计算公式为 ；若 ，则代表动态障碍物球体球心 到代表机械臂连杆的圆柱体关节端 处的距离最近，距离计算公式为 ；根据比值z得到不同情况下距离D表示如下： ，根据向量表示法则，结合机械臂正运动学，通过距离D的表达式得到距离D中各向量的一阶导数： ， ， ，式中，J i 表示机械臂第i个关节的速度雅可比矩阵，v o 表示动态障碍物的瞬时移动速度，u表示机械臂关节角速度控制量；根据向量求导法则，计算得到 在不同比值z情况下的表达式：当 时： ，当 时： ，当 时： ，通过比值z的取值范围确定动态障碍物与连杆的相对位置，将机械臂约束条件： ，转化为对机械臂关节角速度控制量u的线性不等式约束，如下式所示： ，通过将机械臂关节角速度控制量u的线性不等式约束进行移项成 的形式求解二次规划QP问题； 和 分别表示不等式约束的系数矩阵和右侧向量， 和 的表达式分别如下式所示： ， 。 7.根据权利要求6所述的动态环境下机械臂分层运动规划方法，其特征在于：当线性二次调节器LQR跟踪过程中存在动态障碍物且存在时序任务时，使用信号时序逻辑STL范式描述时序任务，STL通过时序运算符和谓词组合，准确表达机械臂在时间维度上的行为约束，STL的时序运算符包括始终运算符、最终运算符、直到运算符；在路径规划过程中结合STL约束和时变控制障碍函数TVCBF，确保机械臂不仅能够在动态环境中避障，还能严格按照任务逻辑执行路径规划，TVCBF的具体构造步骤为：对任务分解与谓词提取：对使用STL范式描述的时序任务进行语法树解析，提取STL范式描述时序任务中的原子谓词 ： ，式中， 为STL的谓词函数，x为机械臂各关节角速度控制量u；针对STL逻辑的不同时序运算符设计时间窗口内的调制函数 ：对于STL范式中的存在运算符，构造递减函数： ，式中，a、b为时刻，c为缩放系数， 为保证机械臂稳定的偏移量，t为时间，对于STL范式中的始终运算符，构造递增函数： ，式中，k与 为保证 满足不等式条件的调整参数， 为保证机械臂稳定的偏移量；e为常数，取值2.71828；设计时间窗口外的调制函数 ：以机械臂各关节角速度控制量连续为目的设计时间窗口外的分段调制函数，使时间窗口外TVCBF对机械臂的控制效果逐渐衰弱到0，即要求调制函数值随时间衰减且恒大于0，构造时间窗口外的调制函数 表示如下： ，式中 与 为调整参数，满足 以保证 ；通过合并时间窗口内的调制函数 和时间窗口外的调制函数 获得全时域的调制函数 ： ，式中， 为时间窗口 内的调制函数值， 时间窗口 的调制函数值；对于存在运算符和始终运算符，TVCBF表示为： ，式中， 为机械臂连杆从当前位置到目标位置的欧几里得距离；对于STL范式中直到运算符，通过平滑最小函数组合的l种情况构造TVCBF为： ，式中，l为时序约束数目， 为直到运算符的信号时序逻辑范式对应的时变控制障碍函数； 表示取最小值操作的平滑近似；构造时变控制障碍函数 的时变安全集合： ，式中， 为m自由度机械臂关节角度定义域， 为安全条件，即满足STL约束；设计TVCBF需满足下式，以保证安全集合 在规定时间范围内机械臂具有前向不变性： ，为了构建QP问题，对上式不等式进行移项，改写成 的形式： ，式中， 和 分别表示不等式约束的系数矩阵和右侧向量： ， ，将DCBF与TVCBF的约束条件与LQR控制器生成的机械臂各关节角速度最优控制量 相结合，实现机械臂在动态环境中同时满足STL时序约束与动态避障需求，构建如下式所示的QP问题： ，式中，H表示权重矩阵，f表示成本向量，通过上式求解QP问题，得到同时满足STL时序约束与动态避障需求的最终机械臂关节角速度控制量u，使得机械臂在轨迹跟踪过程中能够避免与动态障碍物发生碰撞的同时完成指定的时序任务。 8.一种计算机设备，其特征在于，包括处理器和存储器，所述处理器与存储器电性连接，存储器用于存储指令和数据，处理器用于执行权利要求1-7中任一项所述的动态环境下机械臂分层运动规划方法。 9.一种计算机可读存储介质，其特征在于：所述计算机可读存储介质存储有计算机程序，所述计算机程序配置为由处理器调用时实现权利要求1-7中任一项所述的动态环境下机械臂分层运动规划方法。