CN114932549A - 空间冗余机械臂的运动规划方法与装置 - Google Patents

空间冗余机械臂的运动规划方法与装置 Download PDF

Info

Publication number
CN114932549A
CN114932549A CN202210525278.5A CN202210525278A CN114932549A CN 114932549 A CN114932549 A CN 114932549A CN 202210525278 A CN202210525278 A CN 202210525278A CN 114932549 A CN114932549 A CN 114932549A
Authority
CN
China
Prior art keywords
constraint
planning
track
joint
collision
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202210525278.5A
Other languages
English (en)
Other versions
CN114932549B (zh
Inventor
朱战霞
吴天毅
王铮
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Northwestern Polytechnical University
Original Assignee
Northwestern Polytechnical University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Northwestern Polytechnical University filed Critical Northwestern Polytechnical University
Priority to CN202210525278.5A priority Critical patent/CN114932549B/zh
Priority claimed from CN202210525278.5A external-priority patent/CN114932549B/zh
Publication of CN114932549A publication Critical patent/CN114932549A/zh
Application granted granted Critical
Publication of CN114932549B publication Critical patent/CN114932549B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • Artificial Intelligence (AREA)
  • Evolutionary Computation (AREA)
  • Fuzzy Systems (AREA)
  • Mathematical Physics (AREA)
  • Software Systems (AREA)
  • Manipulator (AREA)
  • Numerical Control (AREA)

Abstract

本发明实施例公开了一种空间冗余机械臂的运动规划方法与装置。该方法包括：确定空间冗余机械臂的无碰初始关节轨迹；建立空间冗余机械臂的轨迹规划最优控制问题模型，并对轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型；根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹。通过本发明，解决了相关技术中空间冗余机械臂运动规划方法较为复杂且无法满足应用需求的问题，达到了能够为空间冗余机械臂规划带末端任务约束的最优关节轨迹的技术效果。

Description

空间冗余机械臂的运动规划方法与装置
技术领域
本发明涉及空间机器人系统技术领域，尤其涉及一种空间冗余机械臂的运动规划方法与装置。
背景技术
在载人航天工程的推动下，由中国自行研制的空间站已投入运营。空间站在轨运营期间，分别在核心舱和实验舱上搭载的核心舱机械臂(Core Module Manipulator，CMM)和实验舱机械臂(Experimental Module Manipulator，EMM)将在执行视觉感知任务方面发挥重要作用。视觉感知任务指利用7自由度CMM或EMM的末端手眼相机实现对目标的遥操作监控。不论是对舱外动态目标的监视，还是对舱体静态目标的检查，都需要为CMM或EMM规划一条从起始观测点到终端观测点的无碰撞、可执行的平滑轨迹，同时在整个运动过程中要使目标保持在末端手眼相机的视野范围之内。上述问题属于空间冗余机械臂带有末端任务约束的运动规划问题。
根据构造原理的不同，可将现有的运动规划方法大致归纳为三类：组合运动规划方法、基于采样的运动规划方法以及基于优化的运动规划方法。
组合运动规划方法首先需要将障碍构型空间和自由构型空间显式且完备的表示出来，然后将运动规划问题转化为自由构型空间中的路图构造和路径搜索问题。这类方法在解决运动规划问题时的优势是具有完备性，且对自由度较少的机器人在简单应用场景下的运动规划具有高效性。然而，随着机器人自由度的增加、障碍环境的复杂化以及其它约束条件的增多，显示且完备地构造自由构型空间、障碍构型空间以及路图的难度都会呈指数倍增长，因此这类方法并不适用于高自由度机器人的运动规划问题。此外，这类方法也不适用于含有微分约束的运动规划问题。
与组合运动规划方法不同，基于采样的运动规划方法通过利用碰撞检测模块来避免显式构造障碍构型空间，核心思想是用自由构型空间中的有限多个点来近似表示连通性，然后用这些点来构造搜索树，以牺牲完备性的代价换取较高的计算速度。这类方法又可细分为多查询(Multiple-query)和单查询(Single-query)两类。多查询方法通过构建具有拓扑性质的路图来实现具有多个起始-目标点对的运动规划，这类方法的典型代表有概率路线图法(Probabilistic Roadmap，PRM)以及它的各种变体，如lazy PRM，dynamic PRM以及PRM*等。单查询方法用来实现具有单个起始-目标点对的运动规划，通过增量式的方法构造一个无向树状图来快速搜索目标，这类方法的典型代表有快速搜索随机树法(Rapidly-exploring Random Tree，RRT)，快速探索密集树法(Rapidly-exploring Dense Tree，RDT)以及各种变体，如RRT-Connect，RRT*等。针对高自由度机器人不考虑任务约束的点到点规划问题，基于采样的运动规划方法可以快速计算出无碰几何路径。然而在解决带有任务约束的运动规划问题时存在采样效率低、计算复杂以及应用场景受限等不足，此外，该类方法在考虑动力学约束时需要解决复杂的两点边值问题，扩展性较差。
基于优化的运动规划依托最优控制原理，可以被描述为规划一条动态可行、服从所有状态和控制约束、并使所选代价函数最小化的高质量轨迹。其中，各种约束条件被描述成线性、非线性的等式或不等式。用来进行最优轨迹规划的方法包括启发式方法、数值方法等。启发式方法基于生物界的自然选择、自然遗传机制或动物的群体行为而设计，在运动规划领域也有着广泛的应用。比较有代表性的启发式方法有遗传算法(Genetic Algorithm，GA)和粒子群优化算法(Particle Swarm Optimization，PSO)等。启发式方法在迭代求解的过程中采用了随机搜索策略，因而不需要梯度信息，更适用于不可微和高度病态的运动规划问题。在数值优化框架内，用来求解最优轨迹规划问题的方法分为间接法和直接法两类。间接法首先根据最优控制理论来推导出问题的必要性条件，之后通过求解两点边值问题来获取结果。直接法首先将原始的连续时间问题离散为非线性规划(NonlinearProgramming，NP)问题，之后采用相应的算法对问题进行求解。由于最优控制问题在复杂约束条件下的必要性条件难以推导，且初始猜想会对问题的求解引起较大的波动，所以直接法在实际应用中更受青睐。最优轨迹规划问题是否具备凸性对直接法的求解效果有着非常重要的影响。利用直接法求解非凸问题是不确定多项式时间困难的(NondeterministicPolynomial-time hard，NP-hard)，这意味着非凸问题的求解时间是不可控的。求解非凸轨迹优化问题的算法包括协变哈密尔顿优化方法(Covariant Hamiltonian Optimizationfor Motion Planning，CHOMP)和随机轨迹优化方法(Stochastic TrajectoryOptimization for Motion Planning，STOMP)等。然而，非线性规划中的凸优化问题存在着多项式时间求解算法，可以快速求解。近年来，越来越多的学者开始将凸优化方法应用到轨迹规划中，代表性的有二阶锥规划和序列凸优化等。其中，基于序列凸优化的规划方法需要初始轨迹来启动，虽然对初始轨迹的质量要求不是很高，但这依然会影响算法的效率。基于优化的运动规划方法在机器人的运动规划中起着至关重要的作用，不仅可以直接用于解决规划问题，也可以在解耦规划中对其他方法生成的可能存在碰撞的粗轨迹进行细化。
综上，寻求一种既不需要初始猜想，又能考虑复杂约束条件的空间冗余机械臂运动规划方法显得十分必要。
针对相关技术中空间冗余机械臂运动规划方法较为复杂且无法满足应用需求的问题，尚未提出有效地解决方案。
发明内容
本发明实施例提供了一种空间冗余机械臂的运动规划方法与装置，以至少解决相关技术中空间冗余机械臂运动规划方法较为复杂且无法满足应用需求的技术问题。
根据本发明实施例的一个方面，提供了一种空间冗余机械臂的运动规划方法，包括：确定所述空间冗余机械臂的无碰初始关节轨迹；建立所述空间冗余机械臂的轨迹规划最优控制问题模型，并对所述轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，所述约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；根据所述无碰初始关节轨迹以及所述凸化处理后的轨迹规划最优控制问题模型，确定所述空间冗余机械臂的最优关节轨迹。
可选地，确定所述空间冗余机械臂的无碰初始关节轨迹，包括：利用基于快速扩展随机树(rapidly-exploring random tree，RRT)优化算法(又称为GB-RRT算法)和三次样条曲线规划所述空间冗余机械臂从起始点到目标点的所述无碰初始关节轨迹。
可选地，利用基于快速扩展随机树优化算法和三次样条曲线规划所述空间冗余机械臂从起始点到目标点的所述无碰初始关节轨迹，包括：利用基于快速扩展随机树优化算法规划所述空间冗余机械臂从所述起始点到所述目标点的无碰路径点；采用所述三次样条曲线对所述无碰路径点进行插值，得到所述空间冗余机械臂的所述无碰初始关节轨迹。
可选地，在采用所述三次样条曲线对所述无碰路径点进行插值，得到所述空间冗余机械臂的所述无碰初始关节轨迹之后，所述方法还包括：基于机械臂逆动力学确定所述无碰初始关节轨迹对应的关节力矩。
可选地，对所述轨迹规划最优控制问题模型的约束条件进行凸化处理，包括：基于速度阻尼法对所述避障约束的时间区间进行离散化处理，将所述空间冗余机械臂的连杆与障碍物的避障约束处理成关节角速度的线性不等式约束，并设置避障策略；基于单位四元数对所述空间冗余机械臂的末端手眼相机的视线锥约束进行凸化处理，将所述视线锥约束处理成单位姿态四元数的正定二次型不等式约束；基于二阶积分器和连续线性化对所述运动学约束、所述动力学约束进行凸化处理，得到凸化处理后的运动学约束与动力学约束。
可选地，根据所述无碰初始关节轨迹以及所述凸化处理后的轨迹规划最优控制问题模型，确定所述空间冗余机械臂的最优关节轨迹，包括：将所述无碰初始关节轨迹输入所述凸化处理后的轨迹规划最优控制问题模型，由所述凸化处理后的轨迹规划最优控制问题模型输出所述空间冗余机械臂的所述最优关节轨迹。
根据本发明实施例的另一个方面，还提供了一种空间冗余机械臂的运动规划装置，包括：第一确定模块，用于确定所述空间冗余机械臂的无碰初始关节轨迹；处理模块，用于建立所述空间冗余机械臂的轨迹规划最优控制问题模型，并对所述轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，所述约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；第二确定模块，用于根据所述无碰初始关节轨迹以及所述凸化处理后的轨迹规划最优控制问题模型，确定所述空间冗余机械臂的最优关节轨迹。
可选地，所述第一确定模块包括：规划单元，用于利用基于快速扩展随机树优化算法和三次样条曲线规划所述空间冗余机械臂从起始点到目标点的所述无碰初始关节轨迹。
根据本发明实施例的另一个方面，还提供了一种计算机可读存储介质，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行上述中任意一项所述的空间冗余机械臂的运动规划方法。
根据本发明实施例的另一个方面，还提供了一种处理器，所述处理器用于运行程序，其中，所述程序运行时执行上述中任意一项所述的空间冗余机械臂的运动规划方法。
本发明实施例中，确定空间冗余机械臂的无碰初始关节轨迹；建立空间冗余机械臂的轨迹规划最优控制问题模型，并对轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹。也就是说，本发明实施例不仅利用了序列凸优化方法所具有的多项式时间求解特性，而且可以为它的启动求解提供较高质量的初始参考轨迹。在规划的过程中，既不需要提前给定机械臂末端执行器的轨迹，也不需要提前设定机械臂各关节的路径点，此外，也不用某种曲线来限定关节轨迹的特性；而是直接规划机械臂从起始构型运动到目标构型，且满足复杂约束条件的关节轨迹，进而解决了相关技术中空间冗余机械臂运动规划方法较为复杂且无法满足应用需求的问题，达到了能够为空间冗余机械臂规划带末端任务约束的最优关节轨迹的技术效果。
附图说明
图1为本发明实施例提供的一种空间冗余机械臂的运动规划方法的流程图；图2为本发明实施例提供的一种空间冗余机械臂的结构示意图；图3为本发明实施例提供的一种视线锥约束的示意图；图4为本发明实施例提供的一种速度阻尼法的示意图；图5为本发明实施例提供的一种7自由度空间机械臂简化模型的示意图；图6(a)为本发明实施例提供的一种关节1-4的初始关节角变化曲线的示意图；图6(b)为本发明实施例提供的一种关节5-7的初始关节角变化曲线的示意图；图7(a)为本发明实施例提供的一种关节1-4的初始关节角速度变化曲线的示意图；图7(b)为本发明实施例提供的一种关节5-7的初始关节角速度变化曲线的示意图；图8(a)为本发明实施例提供的一种关节1-4的初始关节角加速度变化曲线的示意图；图8(b)为本发明实施例提供的一种关节5-7的初始关节角加速度变化曲线的示意图；图9(a)为本发明实施例提供的一种关节1-4的初始关节力矩变化曲线的示意图；图9(b)为本发明实施例提供的一种关节5-7的初始关节力矩变化曲线的示意图；图10为本发明实施例提供的一种目标函数值变化规律的示意图；图11为本发明实施例提供的一种目标函数值相对误差变化规律的示意图；图12(a)为本发明实施例提供的一种关节1-4的最优关节角变化曲线的示意图；图12(b)为本发明实施例提供的一种关节5-7的最优关节角变化曲线的示意图；图13(a)为本发明实施例提供的一种关节1-4的最优关节角速度变化曲线的示意图；图13(b)为本发明实施例提供的一种关节5-7的最优关节角速度变化曲线的示意图；图14(a)为本发明实施例提供的一种关节1-4的最优关节角加速度变化曲线的示意图；图14(b)为本发明实施例提供的一种关节5-7的最优关节角加速度变化曲线的示意图；图15(a)为本发明实施例提供的一种关节1-4的最优关节力矩变化曲线的示意图；图15(b)为本发明实施例提供的一种关节5-7的最优关节力矩变化曲线的示意图；图16为本发明实施例提供的一种末端姿态四元数变化曲线的示意图；图17为本发明实施例提供的一种视线角变化曲线的示意图；图18为本发明实施例提供的一种空间冗余机械臂的运动规划装置的示意图。
具体实施方式
本发明实施例提供一种空间冗余机械臂的运动规划方法，图1为本发明实施例提供的一种空间冗余机械臂的运动规划方法的流程图，如图1所示，该方法包括如下步骤：
步骤S102，确定空间冗余机械臂的无碰初始关节轨迹；
在一种可选的实施方式中，确定空间冗余机械臂的无碰初始关节轨迹，包括：利用基于快速扩展随机树优化算法和三次样条曲线规划空间冗余机械臂从起始点到目标点的无碰初始关节轨迹。需要说明的是，上述无碰初始关节轨迹为空间冗余机械臂各关节不会与障碍物发生碰撞的初始轨迹。
步骤S104，建立空间冗余机械臂的轨迹规划最优控制问题模型，并对轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；
步骤S106，根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹。
在一种可选的实施方式中，根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹，包括：将无碰初始关节轨迹输入凸化处理后的轨迹规划最优控制问题模型，由凸化处理后的轨迹规划最优控制问题模型输出空间冗余机械臂的最优关节轨迹。
需要说明的是，本发明采用了分步规划策略来为空间冗余机械臂规划满足任务约束的最优关节轨迹。也就是，在采样方法的框架内规划得到机械臂各关节的无碰初始关节轨迹；再利用序列凸优化方法规划机械臂带末端任务约束的最优关节轨迹。
本发明实施例中，确定空间冗余机械臂的无碰初始关节轨迹；建立空间冗余机械臂的轨迹规划最优控制问题模型，并对轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹。也就是说，本发明实施例不仅利用了序列凸优化方法所具有的多项式时间求解特性，而且可以为它的启动求解提供较高质量的初始参考轨迹。在规划的过程中，既不需要提前给定机械臂末端执行器的轨迹，也不需要提前设定机械臂各关节的路径点，此外，也不用某种曲线来限定关节轨迹的特性；而是直接规划机械臂从起始构型运动到目标构型，且满足复杂约束条件的关节轨迹，进而解决了相关技术中空间冗余机械臂运动规划方法较为复杂且无法满足应用需求的问题，达到了能够为空间冗余机械臂规划带末端任务约束的最优关节轨迹的技术效果。
在一种可选的实施方式中，利用基于快速扩展随机树优化算法和三次样条曲线规划空间冗余机械臂从起始点到目标点的无碰初始关节轨迹，包括：利用基于快速扩展随机树优化算法规划空间冗余机械臂从起始点到目标点的无碰路径点；采用三次样条曲线对无碰路径点进行插值，得到空间冗余机械臂的无碰初始关节轨迹。另外，在采用三次样条曲线对无碰路径点进行插值，得到空间冗余机械臂的无碰初始关节轨迹之后，上述方法还包括：基于机械臂逆动力学确定无碰初始关节轨迹对应的关节力矩。
在考虑运动学约束和避障约束的前提下，利用GB-RRT和三次样条曲线规划空间冗余机械臂从起始观测点到终端观测点的无碰初始关节轨迹。首先，利用GB-RRT为空间冗余机械臂规划无碰路径点；其次，采用三次样条曲线对无碰路径点进行插值，得到机械臂的无碰关节轨迹；最后，利用机械臂逆动力学得到对应的关节力矩。
在一种可选的实施方式中，对轨迹规划最优控制问题模型的约束条件进行凸化处理，包括：基于速度阻尼法对避障约束的时间区间进行离散化处理，将空间冗余机械臂的连杆与障碍物的避障约束处理成关节角速度的线性不等式约束，并设置避障策略；基于单位四元数对空间冗余机械臂的末端手眼相机的视线锥约束进行凸化处理，将视线锥约束处理成单位姿态四元数的正定二次型不等式约束；基于二阶积分器和连续线性化对运动学约束、动力学约束进行凸化处理，得到凸化处理后的运动学约束与动力学约束。
需要说明的是，上述结果用来启动求解基于序列凸优化的空间冗余机械臂最优轨迹规划算法。
在具体实施过程中，可以设计一种基于序列凸优化的最优关节轨迹规划算法。首先，建立包含运动学约束、动力学约束、避障约束和末端任务约束的空间冗余机械臂轨迹规划最优控制问题模型。接着，关于对模型的凸化处理方法具体内容如下：对于避障约束，基于速度阻尼法将其处理成关节角速度的线性不等式约束，并设计一种在离散时间区内的连续无碰的避障策略；对于末端手眼相机的视线锥约束，将其处理成单位姿态四元数的正定二次型不等式约束；对于运动学、动力学约束，采用了二阶积分器和连续线性化的凸化处理方法。最后，以无碰初始关节轨迹为输入，针对利用空间冗余机械臂末端手眼相机监视动态目标的情况对凸化模型进行求解。
进一步地，建立空间冗余机械臂包含避障约束、末端任务约束、运动学和动力学约束的最优轨迹规划问题模型。该模型的具体形式P0如下：
Figure BDA0003644198590000071
Figure BDA0003644198590000072
Figure BDA0003644198590000073
Θmin≤Θ≤Θmax
τmsmin≤τms≤τmsmax
Figure BDA0003644198590000074
Figure BDA0003644198590000075
Θ(0)＝Θ0,Θ(tf)＝Θf
Figure BDA0003644198590000076
问题P0是非线性、非凸最优控制问题。
对建立的最优轨迹规划问题P0进行凸化处理。具体凸化处理方法如下：
基于速度阻尼法将避障约束处理成了关节角速度
Figure BDA0003644198590000077
的线性不等式约束：
Figure BDA0003644198590000078
其中，d表示连杆与障碍物的距离，ds为安全距离，di为影响距离，ξ表示调整收敛速度的控制系数，p1表示连杆上离障碍物最近的点的矢量，JT表示与平移相关的业可比矩阵，n表示障碍物和连杆之间最近点对构成的单位矢量。
此外，为了保证避障的连续性，需要对ξ施加约束：
Figure BDA0003644198590000079
引入单位四元数表示末端姿态，将末端手眼相机的视线锥约束表示为末端姿态四元数的正定二次型不等式约束：
Figure BDA00036441985900000710
其中，
Figure BDA00036441985900000711
Figure BDA00036441985900000712
其中，Qe为末端执行器的姿态四元数，rd为末端手眼相机的期望指向，
Figure BDA00036441985900000713
为末端手眼相机的视线轴，{E}与{I}分别代表末端执行器本体坐标系和惯性坐标系。符号[·]L与[·]R与四元数相关。
以Qe为例，假设Qe＝[qv,qs]，则有
Figure BDA0003644198590000081
Figure BDA0003644198590000082
采用二阶积分器将机器臂的关节运动处理成线性等式约束：
Figure BDA0003644198590000083
Figure BDA0003644198590000084
将上述表达式整理成矩阵的形式，有
Figure BDA0003644198590000085
其中，
Figure BDA0003644198590000086
Figure BDA0003644198590000087
采用连续线性化的方法将末端姿态四元数微分运动学处理成线性等式约束：
Figure BDA0003644198590000088
其中，ωe表示末端姿态角速度。
同样，可将系统的动力学方程处理为：
Figure BDA0003644198590000089
其中，τms表示关节驱动力矩，Hm表示机械臂的惯量矩阵。
目标函数通过矩形积分法则进行近似：
Figure BDA00036441985900000810
综上，问题P0经过凸化处理后，可近似表示成凸问题P1：
Figure BDA0003644198590000091
Figure BDA0003644198590000092
Figure BDA0003644198590000093
Figure BDA0003644198590000094
Figure BDA0003644198590000095
Figure BDA0003644198590000096
Figure BDA0003644198590000097
Figure BDA0003644198590000098
Θmin≤Θ[n]≤Θmax n＝1...N
Figure BDA00036441985900000916
Figure BDA0003644198590000099
Figure BDA00036441985900000910
Figure BDA00036441985900000911
Figure BDA00036441985900000912
Figure BDA00036441985900000913
其中，
Figure BDA00036441985900000914
是为了保证模型精度而施加的信赖域约束。
下面对本发明一种可选的实施例做进一步详细描述：
1.空间冗余机械臂的运动学与动力学建模
空间冗余机械臂的运动学模型和动力学模型是运动规划的基础，所以在正式进行规划之前，要建立空间冗余机械臂的运动学和动力学模型。针对的使用对象安装在空间站这类大型空间设施上，因此可认为其在基座受控的模式下进行工作。
(1)空间冗余机械臂的运动学建模
图2为本发明实施例提供的一种空间冗余机械臂的结构示意图，如图2所示，机械臂各连杆质心及末端执行器的位置矢量ri,pe可分别表示为：
Figure BDA00036441985900000915
Figure BDA0003644198590000101
上述两式进行求导，推导可得基座受控模式下机械臂各连杆质心及末端执行器的线速度vi,ve为：
Figure BDA0003644198590000102
Figure BDA0003644198590000103
此外，各连杆质心及末端执行器的角速度ωi,ωe可分别表示为：
Figure BDA0003644198590000104
Figure BDA0003644198590000105
将末端执行器的线速度表达式
Figure BDA0003644198590000106
与角速度表达式
Figure BDA0003644198590000107
合并成矩阵的形式，则得到基座受控模式下空间冗余机械臂的速度级运动学方程为：
Figure BDA0003644198590000108
其中，
Figure BDA0003644198590000109
(2)空间冗余机械臂动力学建模
建立空间冗余机械臂的动力学模型时，假设忽略空间微重力，忽略各关节的摩擦阻力以及系统的柔性，并且假定系统在执行任务期间，质量保持恒定。故系统的总能量为系统的动能。采用拉格朗日函数，可得系统的拉格朗日动力学方程为：
Figure BDA00036441985900001010
其中，T表示系统的动能，F表示系统的广义力。
系统的动能包括各个连杆的平动和转动动能，可表示为：
Figure BDA0003644198590000111
将上式代入拉格朗日方程中，可得基座受控模式下空间冗余机械臂的动力学方程为：
Figure BDA0003644198590000112
其中，
Figure BDA0003644198590000113
与τms分别表示基座受控模式下系统的非线性力与关节驱动力矩。
2.基于GB-RRT与三次样条曲线的初始轨迹规划
(1)基于GB-RRT的路径规划
本发明设计了适用于空间冗余机械臂规划无碰初始路径点的GB-RRT算法，其伪代码如表1所示。从表中可以看出，算法Algorithm 1主要包含三个模块，分别是采样模块Random_Config_with_GB()、待扩展模块Extend()和寻路径模块FindSolutionPath()。
表1 GB-RRT伪代码
Figure BDA0003644198590000114
采样模块Random_Config_with_GB()采用了目标偏置的采样方式。首先通过设定的目标偏置概率pgoal∈(0,1)进行采样点qsample的选取；在每次采样之前，通过随机数生成器产生处于(0,1)之间的随机数prand，并与pgoal进行比较。若prand＜pgoal，采用目标构型qgoal作为采样点；否则，在构型空间中生成一个随机采样点qrand∈C。
待扩展模块Extend()是算法Algorithm 1的核心，伪代码如表2所示，从中可以看出，算法Subalgorithm 1-1包含的数据结构有“节点”和“边”。其中的子模块Nearest_Neighbor()用来返回“树”中距离采样点qsample最近的节点qnear，即
qnear＝{q|min||q-qsample||,q∈Tree.vertex}
子模块Find_Qnew()用来根据qsample与qnear提供的方向信息和给定的步长生成新节点，即
Figure BDA0003644198590000121
子模块Collision_Module()用来对qnear到qnew之间的构型进行碰撞检测。若均没有碰撞发生，则分别将qnew与qnearqnew添加到数据结构的“节点”和“边”中；接下来判断添加的新节点qnew是否是目标构型qgoal。若qnew＝qgoal，返回标志‘Reached’，否则返回标志‘Advanced’。若发生碰撞，则返回标志‘Trapped’。
表2待扩展模块Extend()的伪代码
Figure BDA0003644198590000122
在碰撞检测模块中，首先必须对机械臂和障碍物的外形进行建模。本发明分别采用六面体和圆柱体对关节和连杆本体的外形进行了建模，采用了球体对障碍物的外形进行了建模。为了避免直接对这三种几何本原进行相交检测而带来的计算量过大的问题，本发明采用了轴向包围盒对机械臂各连杆和障碍物进行包裹，从而进一步基于分离轴定理设计了碰撞检测算法。
寻路径模块FindSolutionPath()不仅要负责生成连续路径，还要采用“贪婪”策略对路径进行修剪，即对生成的连续路径，尝试从qinit连接qgoal，如果失败，从更近的位置尝试，直到连接成功，一旦qgoal被连接，再次从当前位置开始重复上述操作。
(2)基于三次样条曲线的轨迹生成
空间冗余机械臂利用GB-RRT进行路径规划得到的结果是各关节的连续路径点，并不包含与时间相关的轨迹信息，这还不能满足对序列凸优化算法的初始化要求。本发明利用三次样条曲线对每个关节的路径进行插值，使之从起始点开始，依次通过规划得到的路径点，最后达到目标点。为了保证各关节运动的同步性，对于每一段中间路径，各关节的运动时间必须相等。
假设每个关节有k个路径点，分别用θi＝[θi1,θi2,…,θik]表示。同时，可对设定的运动时间[tinit,tfinal]进行如下分划：
Δ:tinit＝t1＜t2＜…＜tk＝tfinal
在各时间节点处，有gi(tj)＝θij。此外，路径函数满足：
(1)在每一个区间[tj-1,tj]上是3次多项式；
(2)在内节点tj(2≤j≤k-1)处具有连续的2阶导；
(3)si(tj)＝gi(tj)(1≤j≤k)；
路径函数si(t)在每个区间上是3次多项式，含有4个待定系数，因此在整个插值区间上具有4(k-1)个待定系数，共计4k-6个约束：
内节点tj处
Figure BDA0003644198590000131
所有节点处si(tj)＝gi(tj)(1≤j≤k)。
由此可见，要确定4(k-1)个待定系数，还需要附加两个约束条件。按照实际要求，本发明采用了转角边界条件
Figure BDA0003644198590000132
之间便可以构造出相应的插值函数。
3.基于序列凸优化的最优轨迹规划
(1)轨迹规划最优控制问题模型建立
首先要建立空间冗余机械臂包含动力学约束、运动学约束、避障约束和末端任务约束的轨迹规划最优控制问题模型。
空间冗余机械臂的动力学方程为：
Figure BDA0003644198590000133
用单位四元数Qe表示机械臂末端固连坐标系∑E相对于惯性系∑I的姿态，则对应的单位四元数微分运动方程式为：
Figure BDA0003644198590000134
其中，ωe表示末端角速度的纯四元数表达形式，符号
Figure BDA0003644198590000141
表示四元数乘法。
在机械臂运动过程中，由于实际物理结构限制，各关节的角位移不能超过给定的范围，即表达式如下：
Θmin≤Θ≤Θmax
此外，关节驱动力矩不应超过关节电机所能提供的扭矩极限，因而有如下表达式：
τmsmin≤τms≤τmsmax
避免碰撞发生是轨迹规划的基本要求。本部分依然采用轴向包围盒对连杆外形进行处理，但不对障碍物外形进行处理。这样做的优势是确保了机械臂按照初始参考轨迹运动时可与障碍物保持一定的安全距离。另外，本部分只考虑机械臂各连杆与障碍物的避碰要求。
假定机械臂各连杆在任意时刻所占的空间区域用Slinks表示，障碍物所占空间区域用Sobs表示，则避障约束可表示为：
Figure BDA0003644198590000142
利用空间冗余机械臂的末端手眼相机执行视觉监测任务是本发明要解决的一个重要问题。为了解决该问题，本发明在末端手眼相机处对被监视目标施加了视线锥约束，这是成功执行视觉监测任务的关键，图3为本发明实施例提供的一种视线锥约束的示意图，如图3所示。被监视的目标应时刻保持在末端相机的视野范围之内。假定用
Figure BDA0003644198590000147
表示末端相机的视线轴，rd表示末端相机到目标的期望指向，φ表示末端相机的半视线角，那么视线锥约束可用下述方程表示：
Figure BDA0003644198590000143
考虑更一般的情况，需要根据实际任务需灵活的对机械臂设定起始终端约束条件，其表达式如下：
Θ(0)＝Θ0,Θ(tf)＝Θf
Figure BDA0003644198590000144
以能量最优为优化指标，目标函数可表示为关节驱动力矩的二次型形式：
Figure BDA0003644198590000145
其中，
Figure BDA0003644198590000146
表示正定的权重矩阵，tf表示运动时间，也称作终端时间。
综上，空间冗余机械臂轨迹规划的最优控制问题P0可表示为：
Figure BDA0003644198590000151
Figure BDA0003644198590000152
Figure BDA0003644198590000153
Θmin≤Θ≤Θmax
τmsmin≤τms≤τmsmax
Figure BDA0003644198590000154
Figure BDA0003644198590000155
Θ(0)＝Θ0,Θ(tf)＝Θf
Figure BDA0003644198590000156
在问题P0中，系统的动力学方程和四元数微分运动方程是非线性的，避障约束和视线锥约束是非凸的，故而问题P0是非线性、非凸最优控制问题。求解问题P0是非确定多项式时间困难的(Nondeterministic Polynomial-time Hard,NP-hard)，这意味着直接对其进行求解可能会消耗大量的时间。由于凸优化问题具有多项式时间求解算法，所以接下来需要对问题P0进行凸化处理，这样就会使问题的求解时间变得相对可控。
(2)轨迹规划模型凸化处理
基于速度阻尼法，在各离散时刻将机械臂连杆与障碍物的避障约束处理成关节角速度的线性不等式约束，此外，为了满足各时间区间内的无碰要求，还设计了一种能够确保连续无碰的避障策略。
图4为本发明实施例提供的一种速度阻尼法的示意图，如图4所示，
Figure BDA0003644198590000157
和
Figure BDA0003644198590000158
分别代表连杆与障碍物，p1和p2分别代表
Figure BDA0003644198590000159
和
Figure BDA00036441985900001510
相距最近的点，d表示
Figure BDA00036441985900001511
和
Figure BDA00036441985900001512
之间的距离，ds和di分别表示安全距离和作用距离，在ds和di之间的区域被称作“影响区域”。一旦检测到连杆与障碍物的距离小于di，即连杆进入“影响区域”后，关于d的微分约束条件就会被触发：
Figure BDA00036441985900001513
其中，ξ是用来调整收敛速度的控制系数。
对上式进行积分，得到的表达式如下：
Figure BDA00036441985900001514
从上式可以发现，如果
Figure BDA00036441985900001515
和
Figure BDA00036441985900001516
在初始时刻的距离满足d(0)＞ds，就可以保证
Figure BDA00036441985900001517
和
Figure BDA00036441985900001518
的距离一直大于ds，从而就可以避免碰撞的发生。
Figure BDA0003644198590000161
可用如下公式进行计算：
Figure BDA0003644198590000162
其中，n表示p2指向p1的单位向量，即n＝(p1-p2)/d。p1和p2可通过GJK(Gilbert–Johnson–Keerthi)算法进行计算。
Figure BDA0003644198590000163
可通过如下公式进行计算：
Figure BDA0003644198590000164
其中，JT(Θ,p1)表示连杆
Figure BDA0003644198590000165
在点p1处与平移运动相关的雅可比矩阵。
联合上述相关的表达式，可将避障约束表示成关节角速度
Figure BDA0003644198590000166
的不等式约束，其表达式如下：
Figure BDA0003644198590000167
其中，
Figure BDA0003644198590000168
由于序列凸优化在求解最优控制问题时对时间进行了离散化处理，故而避障约束仅能保证在各离散时刻无碰撞发生，因此，有必要设计一种连续避障策略来确保各时间区间内的无碰约束。对此，假定在时间步长Δt内，各连杆与障碍物的最近点对不因连杆的平移或旋转而改变。对连杆与障碍物的最小距离要求是：
Figure BDA0003644198590000169
其中，tn＝tn-1+Δt。对上式进行线性化展开，有
Figure BDA00036441985900001616
忽略高阶小量O(Δt2)，并且考虑连杆与障碍物在各时刻的最小距离接近安全距离的极限情况，即可对连杆与障碍物的最小距离要求的表达式进行松弛处理，有
d＝ds+κ1
其中，κ1＞0。将d＝ds+κ1代入
Figure BDA00036441985900001610
可得
Figure BDA00036441985900001611
同样，对
Figure BDA00036441985900001612
引入松弛变量κ2≥0，有
Figure BDA00036441985900001613
将
Figure BDA00036441985900001614
代入
Figure BDA00036441985900001615
可整理得：
Figure BDA0003644198590000171
为了保证避障约束在时间区间t∈(tn-1,tn]内的有效性，必须满足最小距离约束条件，因此有
Figure BDA0003644198590000172
其中，由于κ1与κ2分别是正数和非负数，因此可通过调节参数ξ来保证不等关系成立，且ξ的表达式为：
Figure BDA0003644198590000173
因此，只要参数ξ满足取值范围：
Figure BDA0003644198590000174
就可以保证在时间区间t∈(tn-1,tn]内无碰撞发生。
引入单位四元数来对非凸视线锥约束进行凸化处理。视线锥约束的广义表达式为：
Figure BDA0003644198590000175
其中，
Figure BDA0003644198590000176
表示相机的期望指向，
Figure BDA0003644198590000177
表示相机的视线轴方向。
为了计算
Figure BDA0003644198590000178
需要将
Figure BDA0003644198590000179
和
Figure BDA00036441985900001710
放在同一坐标系中进行表示。一般情况下，
Figure BDA00036441985900001711
在惯性系∑I中表示，记为r{I}，
Figure BDA00036441985900001712
在末端固连坐标系∑E中进行表示，记为υ{E}。现假定在末端固连坐标系∑E中处理视线锥约束，因此需要将在惯性系∑I中表示的期望指向转换到末端固连坐标系∑E中。利用末端单位姿态四元数Qe，可在末端固连坐标系∑E中将
Figure BDA00036441985900001713
表示为：
Figure BDA00036441985900001714
其中，
Figure BDA00036441985900001715
表示Qe的共轭四元数。
利用纯四元数乘积，可将视线锥约束表达为：
Figure BDA00036441985900001716
其中，
Figure BDA00036441985900001718
表示υ与r之间的夹角。
上式可进一步整理为：
Figure BDA00036441985900001717
因为
Figure BDA0003644198590000181
所以
Figure BDA0003644198590000182
可表示为Qe的二次型，其表达式为：
Figure BDA0003644198590000183
其中，
Figure BDA0003644198590000184
由于[υ{E}]R与[r{I}]L均为反对称矩阵，所以A为对称不定矩阵，且有如下表达式：
Figure BDA0003644198590000185
[υ{E}]R与[r{I}]L作为反对称矩阵，均具有虚特征值，即
λυ＝±||υ||i
λr＝±||r||i
而A由[υ{E}]R与[r{I}]L相乘得到，故A的特征值为λ与λr的乘积，有如下表达式：
λA＝±||υ||||r||
由于Qe是单位四元数的，故下述等式成立：
Figure BDA0003644198590000186
结合
Figure BDA0003644198590000187
与
Figure BDA0003644198590000188
得到表达式为：
Figure BDA0003644198590000189
当η的取值小于等于A的最小特征值，其表达式为：
η≤min(λA)＝-||υ||||r||
此时，A-ηI是正定的，故
Figure BDA00036441985900001812
是凸的。
若取η＝-||υ||||r||，
Figure BDA00036441985900001810
变为：
Figure BDA00036441985900001811
综上，可将视线锥约束整理成末端单位姿态四元数Qe的正定二次型不等式约束，其表达式为：
Figure BDA0003644198590000191
其中，A+＝(A+||υ||||r||I)。
针对具体情况，可将问题P0中的视线锥约束凸化处理为：
Figure BDA0003644198590000192
其中，
Figure BDA0003644198590000193
Figure BDA0003644198590000194
采用二阶积分器以根据关节角加速度来获取关节角速度和关节角。
在对问题P0进行凸化处理时，直接将时间区间离散化为N个节点，关节角加速度
Figure BDA0003644198590000195
为优化变量。因此，在单个离散区间内，各关节的角加速度
Figure BDA0003644198590000196
为常量，角速度
Figure BDA0003644198590000197
线性变化，而驱动力矩
Figure BDA0003644198590000198
非线性变化。
目标函数通过矩形积分法则进行近似，其表达式为：
Figure BDA0003644198590000199
关节角速度
Figure BDA00036441985900001910
与关节角Θ分别通过对关节角加速度
Figure BDA00036441985900001911
进行一阶和二阶前向积分求取：
Figure BDA00036441985900001912
Figure BDA00036441985900001913
将上述两个表达式合并成矩阵的形式，其合并后的表达式为：
Figure BDA00036441985900001914
其中，
Figure BDA00036441985900001915
Figure BDA00036441985900001916
采用连续线性化的凸化处理方法来处理四元数微分运动学和系统的动力学方程，其核心思想是根据参考轨迹对它们进行泰勒展开。当原问题被整理成凸问题后，上一次迭代求解产生的结果作为下一次迭代的参考轨迹。
在四元数微分运动方程中，末端角速度ωe与关节角速度
Figure BDA0003644198590000201
存在以下关系：
Figure BDA0003644198590000202
其中，Jmω表示与末端角速度相关的雅可比矩阵。
由于关节角速度
Figure BDA0003644198590000203
是分段线性的，所以ωe也是分段线性的，根据此性质可定义：
Figure BDA0003644198590000204
Figure BDA0003644198590000205
Figure BDA0003644198590000206
根据上式定义，可将四元数微分运动方程的前向积分近似表示成：
Figure BDA0003644198590000207
Figure BDA0003644198590000208
由于四元数乘法不是线性运算，所以
Figure BDA0003644198590000209
依然是非凸的，令
Figure BDA00036441985900002010
对上式在参考轨迹
Figure BDA00036441985900002011
处进行泰勒展开，且仅保留线性项，有
Figure BDA00036441985900002012
至此，可以得到四元数微分运动方程在凸化后的表达式：
Figure BDA00036441985900002013
系统的动力学方程同样可通过泰勒展开进行线性化。凸化后的系统动力学方程为：
Figure BDA00036441985900002014
为了提高经过凸化处理后的问题与原问题的近似性，确保求解精度，需要对关节角Θ、关节角加速度
Figure BDA00036441985900002015
以及末端姿态四元数Qe施加信赖域约束：
Figure BDA00036441985900002016
Figure BDA00036441985900002017
Figure BDA0003644198590000211
其中，ρΘ、
Figure BDA0003644198590000212
与
Figure BDA0003644198590000213
分别表示关节角、关节角加速度及末端姿态四元数的信赖域半径。
综上，原非线性、非凸最优控制问题P0可转化为凸问题P1：
Figure BDA0003644198590000214
Figure BDA0003644198590000215
Figure BDA0003644198590000216
Figure BDA0003644198590000217
Figure BDA0003644198590000218
Figure BDA0003644198590000219
Figure BDA00036441985900002110
Figure BDA00036441985900002111
Θmin≤Θ[n]≤Θmax n＝1...N
Figure BDA00036441985900002119
Figure BDA00036441985900002112
Figure BDA00036441985900002113
Figure BDA00036441985900002114
Figure BDA00036441985900002115
Figure BDA00036441985900002116
凸问题P1会被迭代求解，直到目标函数的值收敛到阈值ε以内：
||[k-1]J-[k]J||≤ε
在迭代过程中，第k次迭代的解
Figure BDA00036441985900002117
用来产生第k+1迭代的参考轨迹：
Figure BDA00036441985900002118
4.算例验证
对发明所提运动规划算法进行仿真验证的载体是一套具有对称结构的7自由度空间机械臂，图5为本发明实施例提供的一种7自由度空间机械臂简化模型的示意图，其简化模型如图5所示。进行仿真时，惯性系与基座坐标系重合。表3给出了模型的物理参数。
表3模型的物理参数
Figure BDA0003644198590000221
采用GB-RRT与三次样条曲线为空间冗余机械臂规划初始关节轨迹时的仿真参数如表4所示。
表4初始轨迹规划仿真参数
Figure BDA0003644198590000222
图6(a)为本发明实施例提供的一种关节1-4的初始关节角变化曲线的示意图，图6(b)为本发明实施例提供的一种关节5-7的初始关节角变化曲线的示意图,图7(a)为本发明实施例提供的一种关节1-4的初始关节角速度变化曲线的示意图，图7(b)为本发明实施例提供的一种关节5-7的初始关节角速度变化曲线的示意图，图8(a)为本发明实施例提供的一种关节1-4的初始关节角加速度变化曲线的示意图，图8(b)为本发明实施例提供的一种关节5-7的初始关节角加速度变化曲线的示意图，这些图分别给出了规划得到的初始关节角、关节角速度以及关节角加速度的变化曲线。图6(a)、6(b)表明，机械臂可以平缓地从起始构型运动到目标构型。图7(a)、7(b)表明规划得到的关节轨迹满足给定的边界条件。从上述图可以看出，规划的关节轨迹平滑且符合三次样条曲线的变化规律。此外，在整个运动过程中，机械臂自身没有发生碰撞，也没有与障碍物发生碰撞。将规划好的初始关节轨迹代入动力学方程，可得到初始关节力矩变化曲线，图9(a)为本发明实施例提供的一种关节1-4的初始关节力矩变化曲线的示意图，图9(b)为本发明实施例提供的一种关节5-7的初始关节力矩变化曲线的示意图。
采用基于序列凸优化的最优轨迹规划算法为空间冗余机械臂规划最优关节轨迹时的仿真参数如表5所示。此外，待监视目标的位置坐标为Pst＝[1.785,-13,2.23]Tm，权重矩阵为W＝diag(1,100,1,1,10,1e4,1)。
表5最优轨迹规划仿真参数设置
Figure BDA0003644198590000231
图10为本发明实施例提供的一种目标函数值变化规律的示意图，如图10所示，给出了目标函数值随迭代次数的变化规律，其中，绿色条形表示启动求解所用初始轨迹对应的目标函数值。图11为本发明实施例提供的一种目标函数值相对误差变化规律的示意图，如图11所示，给出了当前迭代目标函数值与上一次迭代目标函数值的相对误差变化规律，从图中可以看出，监视静态目标对应的凸问题在迭代求解17次时，目标函数值的相对误差收敛到以内。
图12(a)为本发明实施例提供的一种关节1-4的最优关节角变化曲线的示意图，图12(b)为本发明实施例提供的一种关节5-7的最优关节角变化曲线的示意图,图13(a)为本发明实施例提供的一种关节1-4的最优关节角速度变化曲线的示意图，图13(b)为本发明实施例提供的一种关节5-7的最优关节角速度变化曲线的示意图，图14(a)为本发明实施例提供的一种关节1-4的最优关节角加速度变化曲线的示意图，图14(b)为本发明实施例提供的一种关节5-7的最优关节角加速度变化曲线的示意图，这些图分别给出了规划得到的最优关节角、关节角速度以及关节角加速度的变化曲线。可以看出，规划所得关节轨迹较为平滑，保证了机械臂运动的平稳性。从图13(a)和图14(a)中可以发现，关节3-4在时间t∈[11,17]s之间的速度和加速度有较为明显的变化，这和躲避障碍物有关，在此时间段内，连杆3-4进入“影响区域”，速度阻尼器被触发，因此，连杆3-4加快运动以快速远离障碍物。图15(a)为本发明实施例提供的一种关节1-4的最优关节力矩变化曲线的示意图，图15(b)为本发明实施例提供的一种关节5-7的最优关节力矩变化曲线的示意图，如图15(a)、15(b)所示，可以看出它们保持在给定的范围之内。
图16为本发明实施例提供的一种末端姿态四元数变化曲线的示意图，图17为本发明实施例提供的一种视线角变化曲线的示意图，如图16、17所示，从图17中可以看出，末端手眼相机在机械臂的整个运动过程中都可以监视到目标。
上述结果验证了本发明所提规划方法的有效性。
根据本发明实施例的另一个方面，还提供了一种空间冗余机械臂的运动规划装置，图18为本发明实施例提供的一种空间冗余机械臂的运动规划装置的示意图，如图18所示，该空间冗余机械臂的运动规划装置包括：第一确定模块1802、处理模块1804和第二确定模块1806。下面对该空间冗余机械臂的运动规划装置进行详细说明。
第一确定模块1802，用于确定空间冗余机械臂的无碰初始关节轨迹；处理模块1804，连接至上述第一确定模块1802，用于建立空间冗余机械臂的轨迹规划最优控制问题模型，并对轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；第二确定模块1806，连接至上述处理模块1804，用于根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹。
需要说明的是，上述各个模块是可以通过软件或硬件来实现的，例如，对于后者，可以通过以下方式实现：上述各个模块可以位于同一处理器中；和/或，上述各个模块以任意组合的方式位于不同的处理器中。
本发明实施例中，该装置可以确定空间冗余机械臂的无碰初始关节轨迹；建立空间冗余机械臂的轨迹规划最优控制问题模型，并对轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；根据无碰初始关节轨迹以及凸化处理后的轨迹规划最优控制问题模型，确定空间冗余机械臂的最优关节轨迹。也就是说，本发明实施例不仅利用了序列凸优化方法所具有的多项式时间求解特性，而且可以为它的启动求解提供较高质量的初始参考轨迹。在规划的过程中，既不需要提前给定机械臂末端执行器的轨迹，也不需要提前设定机械臂各关节的路径点，此外，也不用某种曲线来限定关节轨迹的特性；而是直接规划机械臂从起始构型运动到目标构型，且满足复杂约束条件的关节轨迹，进而解决了相关技术中空间冗余机械臂运动规划方法较为复杂且无法满足应用需求的问题，达到了能够为空间冗余机械臂规划带末端任务约束的最优关节轨迹的技术效果。
此处需要说明的是，上述第一确定模块1802、处理模块1804和第二确定模块1806对应于方法实施例中的步骤S102至S106，上述模块与对应的步骤所实现的示例和应用场景相同，但不限于上述方法实施例所公开的内容。
可选地，上述第一确定模块1802包括：规划单元，用于利用基于快速扩展随机树优化算法和三次样条曲线规划空间冗余机械臂从起始点到目标点的无碰初始关节轨迹。
根据本发明实施例的另一个方面，还提供了一种计算机可读存储介质，计算机可读存储介质包括存储的程序，其中，在程序运行时控制计算机可读存储介质所在设备执行上述中任意一项的空间冗余机械臂的运动规划方法。
根据本发明实施例的另一个方面，还提供了一种处理器，处理器用于运行程序，其中，程序运行时执行上述中任意一项的空间冗余机械臂的运动规划方法。
以上所述，仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。

Claims (10)

1.一种空间冗余机械臂的运动规划方法，其特征在于，包括：
确定所述空间冗余机械臂的无碰初始关节轨迹；
建立所述空间冗余机械臂的轨迹规划最优控制问题模型，并对所述轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，所述约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；
根据所述无碰初始关节轨迹以及所述凸化处理后的轨迹规划最优控制问题模型，确定所述空间冗余机械臂的最优关节轨迹。
2.根据权利要求1所述的方法，其特征在于，确定所述空间冗余机械臂的无碰初始关节轨迹，包括：
利用基于快速扩展随机树优化算法和三次样条曲线规划所述空间冗余机械臂从起始点到目标点的所述无碰初始关节轨迹。
3.根据权利要求2所述的方法，其特征在于，利用基于快速扩展随机树优化算法和三次样条曲线规划所述空间冗余机械臂从起始点到目标点的所述无碰初始关节轨迹，包括：
利用基于快速扩展随机树优化算法规划所述空间冗余机械臂从所述起始点到所述目标点的无碰路径点；
采用所述三次样条曲线对所述无碰路径点进行插值，得到所述空间冗余机械臂的所述无碰初始关节轨迹。
4.根据权利要求3所述的方法，其特征在于，在采用所述三次样条曲线对所述无碰路径点进行插值，得到所述空间冗余机械臂的所述无碰初始关节轨迹之后，所述方法还包括：
基于机械臂逆动力学确定所述无碰初始关节轨迹对应的关节力矩。
5.根据权利要求1所述的方法，其特征在于，对所述轨迹规划最优控制问题模型的约束条件进行凸化处理，包括：
基于速度阻尼法对所述避障约束的时间区间进行离散化处理，将所述空间冗余机械臂的连杆与障碍物的避障约束处理成关节角速度的线性不等式约束，并设置避障策略；
基于单位四元数对所述空间冗余机械臂的末端手眼相机的视线锥约束进行凸化处理，将所述视线锥约束处理成单位姿态四元数的正定二次型不等式约束；
基于二阶积分器和连续线性化对所述运动学约束、所述动力学约束进行凸化处理，得到凸化处理后的运动学约束与动力学约束。
6.根据权利要求1至5中任意一项所述的方法，其特征在于，根据所述无碰初始关节轨迹以及所述凸化处理后的轨迹规划最优控制问题模型，确定所述空间冗余机械臂的最优关节轨迹，包括：
将所述无碰初始关节轨迹输入所述凸化处理后的轨迹规划最优控制问题模型，由所述凸化处理后的轨迹规划最优控制问题模型输出所述空间冗余机械臂的所述最优关节轨迹。
7.一种空间冗余机械臂的运动规划装置，其特征在于，包括：
第一确定模块，用于确定所述空间冗余机械臂的无碰初始关节轨迹；
处理模块，用于建立所述空间冗余机械臂的轨迹规划最优控制问题模型，并对所述轨迹规划最优控制问题模型的约束条件进行凸化处理，得到凸化处理后的轨迹规划最优控制问题模型，其中，所述约束条件至少包括：运动学约束、动力学约束、避障约束和末端任务约束；
第二确定模块，用于根据所述无碰初始关节轨迹以及所述凸化处理后的轨迹规划最优控制问题模型，确定所述空间冗余机械臂的最优关节轨迹。
8.根据权利要求7所述的装置，其特征在于，所述第一确定模块包括：
规划单元，用于利用基于快速扩展随机树优化算法和三次样条曲线规划所述空间冗余机械臂从起始点到目标点的所述无碰初始关节轨迹。
9.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质包括存储的程序，其中，在所述程序运行时控制所述计算机可读存储介质所在设备执行权利要求1至6中任意一项所述的空间冗余机械臂的运动规划方法。
10.一种处理器，其特征在于，所述处理器用于运行程序，其中，所述程序运行时执行权利要求1至6中任意一项所述的空间冗余机械臂的运动规划方法。
CN202210525278.5A 2022-05-15 空间冗余机械臂的运动规划方法与装置 Active CN114932549B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210525278.5A CN114932549B (zh) 2022-05-15 空间冗余机械臂的运动规划方法与装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210525278.5A CN114932549B (zh) 2022-05-15 空间冗余机械臂的运动规划方法与装置

Publications (2)

Publication Number Publication Date
CN114932549A true CN114932549A (zh) 2022-08-23
CN114932549B CN114932549B (zh) 2024-05-31

Family

ID=

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116476080A (zh) * 2023-06-20 2023-07-25 西湖大学 一种基于几何可行性的空中自动抓取作业规划方法
CN116901063A (zh) * 2023-07-13 2023-10-20 长春通视光电技术股份有限公司 一种基于二次规划的冗余空间机械臂轨迹规划方法
CN117444943A (zh) * 2023-12-26 2024-01-26 济南二机床集团有限公司 一种混联机械手及机械手轨迹规划方法

Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1591209A2 (de) * 2004-04-30 2005-11-02 KUKA Roboter GmbH Verfahren zum Steuern einer Maschine, insbesondere eines Industrieroboters
US8204623B1 (en) * 2009-02-13 2012-06-19 Hrl Laboratories, Llc Planning approach for obstacle avoidance in complex environment using articulated redundant robot arm
US20120277907A1 (en) * 2011-04-28 2012-11-01 Waseda University Trajectory planning method, trajectory planning system and trajectory planning and control system
CN103009389A (zh) * 2012-11-30 2013-04-03 北京控制工程研究所 一种冗余空间机械臂在轨抓捕的轨迹规划方法
US8700307B1 (en) * 2013-03-04 2014-04-15 Mitsubishi Electric Research Laboratories, Inc. Method for determining trajectories manipulators to avoid obstacles
CN107351085A (zh) * 2017-08-21 2017-11-17 西北工业大学 一种基于多个控制点的空间机器人避撞方法
CN107877517A (zh) * 2017-11-16 2018-04-06 哈尔滨工业大学 基于CyberForce遥操作机械臂的运动映射方法
CN107966907A (zh) * 2017-11-30 2018-04-27 华南理工大学 一种应用于冗余度机械臂的障碍物躲避解决方法
US20190143517A1 (en) * 2017-11-14 2019-05-16 Arizona Board Of Regents On Behalf Of Arizona State University Systems and methods for collision-free trajectory planning in human-robot interaction through hand movement prediction from vision
CN110340890A (zh) * 2019-06-27 2019-10-18 北京控制工程研究所 一种空间机械臂全局无碰轨迹规划系统
CN110653805A (zh) * 2019-10-10 2020-01-07 西安科技大学 笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法
CN111251297A (zh) * 2020-02-20 2020-06-09 西北工业大学 一种基于随机采样的双臂空间机器人协调路径规划方法
WO2021065196A1 (en) * 2019-10-03 2021-04-08 Mitsubishi Electric Corporation Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints
US20210197377A1 (en) * 2019-12-26 2021-07-01 X Development Llc Robot plan online adjustment
CN113341731A (zh) * 2021-06-29 2021-09-03 西北工业大学 一种基于序列凸优化的空间机器人轨迹规划方法
CN113352319A (zh) * 2021-05-08 2021-09-07 上海工程技术大学 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法
CN113414761A (zh) * 2021-02-03 2021-09-21 中国人民解放军63920部队 一种冗余机械臂运动轨迹优化的方法

Patent Citations (17)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1591209A2 (de) * 2004-04-30 2005-11-02 KUKA Roboter GmbH Verfahren zum Steuern einer Maschine, insbesondere eines Industrieroboters
US8204623B1 (en) * 2009-02-13 2012-06-19 Hrl Laboratories, Llc Planning approach for obstacle avoidance in complex environment using articulated redundant robot arm
US20120277907A1 (en) * 2011-04-28 2012-11-01 Waseda University Trajectory planning method, trajectory planning system and trajectory planning and control system
CN103009389A (zh) * 2012-11-30 2013-04-03 北京控制工程研究所 一种冗余空间机械臂在轨抓捕的轨迹规划方法
US8700307B1 (en) * 2013-03-04 2014-04-15 Mitsubishi Electric Research Laboratories, Inc. Method for determining trajectories manipulators to avoid obstacles
CN107351085A (zh) * 2017-08-21 2017-11-17 西北工业大学 一种基于多个控制点的空间机器人避撞方法
US20190143517A1 (en) * 2017-11-14 2019-05-16 Arizona Board Of Regents On Behalf Of Arizona State University Systems and methods for collision-free trajectory planning in human-robot interaction through hand movement prediction from vision
CN107877517A (zh) * 2017-11-16 2018-04-06 哈尔滨工业大学 基于CyberForce遥操作机械臂的运动映射方法
CN107966907A (zh) * 2017-11-30 2018-04-27 华南理工大学 一种应用于冗余度机械臂的障碍物躲避解决方法
CN110340890A (zh) * 2019-06-27 2019-10-18 北京控制工程研究所 一种空间机械臂全局无碰轨迹规划系统
WO2021065196A1 (en) * 2019-10-03 2021-04-08 Mitsubishi Electric Corporation Method and system for trajectory optimization for nonlinear robotic systems with geometric constraints
CN110653805A (zh) * 2019-10-10 2020-01-07 西安科技大学 笛卡尔空间下的七自由度冗余机械臂任务约束路径规划方法
US20210197377A1 (en) * 2019-12-26 2021-07-01 X Development Llc Robot plan online adjustment
CN111251297A (zh) * 2020-02-20 2020-06-09 西北工业大学 一种基于随机采样的双臂空间机器人协调路径规划方法
CN113414761A (zh) * 2021-02-03 2021-09-21 中国人民解放军63920部队 一种冗余机械臂运动轨迹优化的方法
CN113352319A (zh) * 2021-05-08 2021-09-07 上海工程技术大学 基于改进快速扩展随机树的冗余机械臂避障轨迹规划方法
CN113341731A (zh) * 2021-06-29 2021-09-03 西北工业大学 一种基于序列凸优化的空间机器人轨迹规划方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
朱战霞;靖飒;仲剑飞;王明明;: "基于碰撞检测的空间冗余机械臂避障路径规划", 西北工业大学学报, no. 01, 15 February 2020 (2020-02-15), pages 190 - 197 *
朱战霞;靖飒;仲剑飞;王明明;: "基于碰撞检测的空间冗余机械臂避障路径规划", 西北工业大学学报, no. 01, pages 190 - 197 *
韩伟一;王铮;黄思明;: "一类或决策模型及其应用", 数学的实践与认识, no. 20, 23 October 2008 (2008-10-23), pages 24 - 29 *
韩伟一;王铮;黄思明;: "一类或决策模型及其应用", 数学的实践与认识, no. 20, pages 24 - 29 *

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116476080A (zh) * 2023-06-20 2023-07-25 西湖大学 一种基于几何可行性的空中自动抓取作业规划方法
CN116476080B (zh) * 2023-06-20 2023-08-29 西湖大学 一种基于几何可行性的空中自动抓取作业规划方法
CN116901063A (zh) * 2023-07-13 2023-10-20 长春通视光电技术股份有限公司 一种基于二次规划的冗余空间机械臂轨迹规划方法
CN116901063B (zh) * 2023-07-13 2024-01-26 长春通视光电技术股份有限公司 一种基于二次规划的冗余空间机械臂轨迹规划方法
CN117444943A (zh) * 2023-12-26 2024-01-26 济南二机床集团有限公司 一种混联机械手及机械手轨迹规划方法
CN117444943B (zh) * 2023-12-26 2024-03-22 济南二机床集团有限公司 一种混联机械手及机械手轨迹规划方法

Similar Documents

Publication Publication Date Title
CN111538949B (zh) 冗余机器人逆运动学求解方法、装置和冗余机器人
CN107490965B (zh) 一种空间自由漂浮机械臂的多约束轨迹规划方法
Pan et al. Collision-free and smooth trajectory computation in cluttered environments
Xu et al. Dynamics modeling and analysis of a flexible-base space robot for capturing large flexible spacecraft
CN112605996B (zh) 一种面向冗余机械臂的无模型碰撞避免控制方法
Zhang et al. An obstacle avoidance algorithm for robot manipulators based on decision-making force
Rezende et al. Constructive time-varying vector fields for robot navigation
Xie et al. Obstacle avoidance and path planning for multi-joint manipulator in a space robot
Zhao et al. Minimum base disturbance control of free-floating space robot during visual servoing pre-capturing process
Joukhadar et al. Advanced UAVs nonlinear control systems and applications
Hua et al. A time-optimal trajectory planning strategy for an aircraft with a suspended payload via optimization and learning approaches
Peng et al. Modeling and analysis of the multiple dynamic coupling effects of a dual-arm space robotic system
CN114326810A (zh) 一种无人机在复杂动态环境下的避障方法
Xi Obstacle avoidance trajectory planning of redundant robots based on improved Bi-RRT
Chen et al. Learning real-time dynamic responsive gap-traversing policy for quadrotors with safety-aware exploration
Li et al. A model predictive obstacle avoidance method based on dynamic motion primitives and a Kalman filter
Zhou et al. RETRACTED: uncalibrated dynamic visual servoing via multivariate adaptive regression splines and improved incremental extreme learning machine
CN117369507A (zh) 一种自适应粒子群算法的无人机动态路径规划方法
Luo et al. Segmented hybrid motion-force control for a hyper-redundant space manipulator
CN114932549B (zh) 空间冗余机械臂的运动规划方法与装置
CN114932549A (zh) 空间冗余机械臂的运动规划方法与装置
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following
Liu et al. Trajectory prediction and visual localization of snake robot based on BiLSTM neural network
Bai et al. A fuzzy adaptive drogue control method in autonomous aerial refueling
CN112161626A (zh) 一种基于航路跟踪映射网络的高可飞性航路规划方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant