CN116276966A - 基于二次规划的移动操作机器人全身反应规划控制方法 - Google Patents

基于二次规划的移动操作机器人全身反应规划控制方法 Download PDF

Info

Publication number
CN116276966A
CN116276966A CN202310029943.6A CN202310029943A CN116276966A CN 116276966 A CN116276966 A CN 116276966A CN 202310029943 A CN202310029943 A CN 202310029943A CN 116276966 A CN116276966 A CN 116276966A
Authority
CN
China
Prior art keywords
control
joint
whole body
quadratic programming
robot
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.)
Pending
Application number
CN202310029943.6A
Other languages
English (en)
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.)
Daimeng Shenzhen Robot Technology Co ltd
Original Assignee
Daimeng Shenzhen Robot Technology Co ltd
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 Daimeng Shenzhen Robot Technology Co ltd filed Critical Daimeng Shenzhen Robot Technology Co ltd
Priority to CN202310029943.6A priority Critical patent/CN116276966A/zh
Publication of CN116276966A publication Critical patent/CN116276966A/zh
Pending legal-status Critical Current

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
    • 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/1607—Calculation of inertia, jacobian matrixes and inverses
    • 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/1628—Programme controls characterised by the control loop
    • 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/1628—Programme controls characterised by the control loop
    • B25J9/163—Programme controls characterised by the control loop learning, adaptive, model based, rule based expert control
    • 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
    • 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
    • B25J9/1666—Avoiding collision or forbidden zones
    • 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/1694—Programme controls characterised by use of sensors other than normal servo-feedback from position, speed or acceleration sensors, perception control, multi-sensor controlled systems, sensor fusion
    • B25J9/1697—Vision controlled systems
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
    • Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
    • Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明公开了基于二次规划的移动操作机器人全身反应规划控制方法，首先，本发明提出了结合视觉信息的末端轨迹重规划方法，用B样条曲线参数化末端轨迹并根据环境信息修正B样条曲线的控制点实现了在未来的一定时域内对参考轨迹的修正。随后，提出了基于二次规划的反应性优化控制器进行全身控制，它结合了控制屏障函数推导的安全性约束和可操作性代价实现了移动操作臂的安全流畅协同运动。此外，通过松弛变量的引入以及对其的惩罚，允许了反应性控制器的瞬时误差并同时惩罚了偏离目标值的大小，提高了追踪的适应性和精准性，本发明所提出的算法在不确定环境下提高操作流畅性安全性和精确性的潜力。

Description

基于二次规划的移动操作机器人全身反应规划控制方法
技术领域
本发明涉及机械臂技术领域，具体为基于二次规划的移动操作机器人全身反应规划控制方法。
背景技术
为了解决分体控制的局限性，近年来，全身整体控制逐渐成为高维复合机器人系统控制的另一热门研究方向。
其中基于采样的整体控制方法在机器人构型空间里按一定概率密度函数采样，为从底盘到上层执行机构的每个关节生成参考轨迹并输入底层控制器。基于采样的路径规划方法追溯到上世纪90年代，斯坦福大学的Kavraki等人提出的概率路线图(probabilityroadmap)[1]通过在机器人构形空间随机采样来构建可行的路线图，并用搜索算法在高维空间中找到到目标点的可行路线。并依阿华州立大学的 LaValle等人提出了经典的RRT随机搜索路径算法[2]，用以生成无碰撞的机器人状态轨迹。为解决基于采样的方法普遍离线作用的限制。斯坦福大学的Starek等人提出了 BFMT^*算法[3]，将快速匹配树算法扩展为了基于采样的双向搜索并证明了其渐进优化性。麻省理工学院的Otte等人提出了RRTX算法，在机器人状态空间直接构建图和搜索树，并根据动态环境和机器人的运动对其进行在线修改和重建，由此产生的运动规划符合机器人运动学约束并且适应机器人运动过程中环境的动态变化，这一方法比其他流行RRT变式的计算复杂度更低[4]。然而基于采样的全身整体规划控制不仅局限于高维空间中计算量的指数增长，同时也需要预先知道环境障碍物等信息。
基于模型预测控制的方法是另一热门的全身整体控制研究方向，这一方法的目标是找到序列的控制策略来在滚动时间范围内优化一个给定的目标函数。苏黎世联邦理工的Hutter于2019年提出了球形平衡底盘移动操作臂的全身模型预测控制框架[5]，用以高效完成末端轨迹追踪等任务。他们在2021年扩展了该工作，通过在模型中引入末端交互动力学来实现末端负载的同时精准末端轨迹。这一方法通过协调全身所有自由度，在提高复合机器人末端控制目标的同时遵从于运动学、动力学及与环境交互过程中产生的各种约束。
然而，上述方法要求对系统及环境模型的精准建模，在复杂的有不确定性的场景下，面临着控制精度低、鲁棒性差等问题。在实际场景的应用中，结合感知信息的整体控制模块的计算频率很难满足实时性要求。同时，如何结合实时传感信息，在不确定性下实现高精度轨迹跟踪的难题也亟待解决。
发明内容
本发明的目的在于提供基于二次规划的移动操作机器人全身反应规划控制方法，以解决上述背景技术中提出的问题。
为了解决上述技术问题，本发明提供如下技术方案：基于二次规划的移动操作机器人全身反应规划控制方法，包括以下几个部分：
S1、基于B样条曲线优化的末端轨迹在线重规划；
S2、根据起点和终点生成末端初始轨迹；
S3、设计基于二次规划的反应性全身控制器；
包含以下组成部分：末端轨迹生成及在线重规划模块、环境处理模块、反应性优化控制器、机器人。
根据上述技术方案，上述步骤S1中，基于B样条曲线优化的末端轨迹在线重规划的具体方法为，将末端目标轨迹拉到一个存在不碰撞解的位置，保证后续的反应控制器能够在利用全身所有自由度追踪末端轨迹时同时满足全身所有位置不发生碰撞的约束。
根据上述技术方案，上述步骤S2中，给定了阶数之后，B-样条曲线的形状由其控制点决定且其与贝塞尔曲线相比的一大优点是通过更改控制点位置来更改局部曲线形状，而贝塞尔曲线则动一点则动整体，首先根据起点和终点生成末端初始轨迹如下：
Figure 625821DEST_PATH_IMAGE001
Figure 552189DEST_PATH_IMAGE002
和
Figure 366561DEST_PATH_IMAGE003
分别为起点和终点末端位姿的SE(3)表达，
Figure 985762DEST_PATH_IMAGE004
是末端旋量 (twist)的矩阵表达方式，表达了单位时间内从起点位姿到终点位姿需要的空间速度，这一 方法得到的末端初始轨迹类似于图一中的蓝色虚线，看起来像是条螺旋运动曲线。
根据上述技术方案，上述步骤S2中，在
Figure 781024DEST_PATH_IMAGE005
上均匀采样生成
Figure 979924DEST_PATH_IMAGE006
个控制点的集合
Figure 179961DEST_PATH_IMAGE007
作为控制点的初值，B样条曲线的阶次为
Figure 970062DEST_PATH_IMAGE008
，优化控制点集合去掉首尾的子集
Figure 724392DEST_PATH_IMAGE009
，前后
Figure 258141DEST_PATH_IMAGE008
个控制点会决定边界条件，构建B样条优化问题:
Figure 312685DEST_PATH_IMAGE010
目标函数由四部分组成，其中：
Figure 8108DEST_PATH_IMAGE011
这项代价是顺滑性代价，惩罚三个相邻控制点之间的连续性偏差，防止突变，保证轨迹丝滑度；
Figure 515313DEST_PATH_IMAGE012
碰撞代价中
Figure 587174DEST_PATH_IMAGE013
函数是映射到控制点
Figure 496225DEST_PATH_IMAGE014
与其最近的障碍物的距离，
Figure 628129DEST_PATH_IMAGE015
是距离阈值常 数，通过求解这样的一个优化问题，实现了将机械臂末端轨迹修正到一个可行合适的位置。
根据上述技术方案，上述步骤S3中，首先引入虚拟关节和松弛变量来增广整个机 器人到末端旋量的运动学雅可比矩阵，虚拟关节体现了在当前位置底盘具有的瞬时运动能 力，全向底盘具有三个虚拟关节，而具有非完整约束的差速底盘则只有x方向平动和绕z方 向转动这两个瞬时速度，定义松弛变量
Figure 622629DEST_PATH_IMAGE016
，定义增广雅可比矩阵：
Figure 498182DEST_PATH_IMAGE017
Figure 527317DEST_PATH_IMAGE018
其中
Figure 564544DEST_PATH_IMAGE019
是将在底盘系b中三个虚拟关节的速度旋量雅可比矩阵转换到末端系e 的6*6的转换矩阵；
Figure 46341DEST_PATH_IMAGE020
是映射到末端空间速度旋量的机械臂雅可比矩阵；
Figure 460004DEST_PATH_IMAGE021
是一个6*6的 单位矩阵，用来将松弛变量映射到末端空间速度旋量
Figure 346576DEST_PATH_IMAGE022
:
Figure 554704DEST_PATH_IMAGE023
决策变量
Figure 523797DEST_PATH_IMAGE024
和
Figure 741152DEST_PATH_IMAGE025
分别是底盘虚拟关节速度和机械臂各关节速度，以此构建二 次规划器等式约束，既将底盘运动能力考虑了进去又通过松弛变量的引入允许了追踪末端 空间速度
Figure 213721DEST_PATH_IMAGE022
的瞬时误差，提高了鲁棒性和适应性。
根据上述技术方案，上述步骤S3中，利用上述增广等式约束，我们提出来基于二次规划的安全反应性控制器：
Figure 858329DEST_PATH_IMAGE026
Figure 314718DEST_PATH_IMAGE027
Figure 70185DEST_PATH_IMAGE028
Figure 662840DEST_PATH_IMAGE029
对该二次规划器的各项进行解释说明：
代价函数中的二次项
Figure 478349DEST_PATH_IMAGE030
对广义控制量
Figure 156455DEST_PATH_IMAGE031
进行惩罚，不仅控制了底盘和机械 臂n维速度输出的大小，也同时限制了松弛变量的大小，通过调节（n+6）*（n+6）的对角矩阵Q 值各项大小均衡底盘和机械臂对追踪的贡献力度、末端追踪的精度，松弛变量越小，精度越 高，代价函数中线性项
Figure 450033DEST_PATH_IMAGE032
则包含了机械臂的可操作性代价，其具体推导表达式如下：
首先单位关节角速度满足:
Figure 162775DEST_PATH_IMAGE033
;
由
Figure 149185DEST_PATH_IMAGE034
代入上式可得:
Figure 314587DEST_PATH_IMAGE035
其中q代表机械臂末端在任务空间中的最少参数表达，
Figure 146277DEST_PATH_IMAGE036
，
Figure 713525DEST_PATH_IMAGE037
；
上式是一个在m维空间中的椭圆，这个椭圆的体积正比于A矩阵特征值的 连乘:
Figure 870836DEST_PATH_IMAGE038
；
同时，这一椭圆的体积越小，代表此时机械臂将越有可能失去某一方向的运动能 力，即陷入奇异点，所以该椭圆的体积是衡量机械臂可操作性的一个量化指标，对
Figure 257955DEST_PATH_IMAGE039
求时间 的导数：
Figure 167704DEST_PATH_IMAGE040
所以，代价函数中的c向量写为：
Figure 589458DEST_PATH_IMAGE041
Figure 917672DEST_PATH_IMAGE042
可操作性代价保证了移动操作臂整体运行过程中机械臂远离有可能产生奇异点从而失去某些运动能力的位姿；
不等式约束中的
Figure 792087DEST_PATH_IMAGE043
是控制屏障函数项，这个不等式约束是用来保 证移动操作臂在运动过程中全身避障的特性，即安全性约束，是控制屏障函数作用的示意 图，控制屏障函数约束类似于一个安全过滤器，保证了反应性控制器瞬时输出的关节速度 能让机器人维持在原本所在的安全的构型空间S中。
根据上述技术方案，上述步骤S3中，控制屏障函数满足定理:如果h是一个安全集 合S上的控制屏障函数，那么采取满足:
Figure 231158DEST_PATH_IMAGE044
的控制信号u使系统的状态q维持 在安全集合S中，控制信号即为各个关节角速度，系统的状态q包括底盘和机械臂当前的构 型。
根据上述技术方案，上述步骤S3中，采用各个关节点与最近障碍物的欧几里得距 离构建控制屏障函数：对于某一个关节点: 定义该关节的前向运动学
Figure 507419DEST_PATH_IMAGE045
是从关节状态映 射到该关节空间位置x,y,z的映射函数；
定义控制屏障函数h(q):
Figure 6533DEST_PATH_IMAGE046
其中O代表与该关节距离最近的障碍物的空间位置[Ox,Oy,Oz], 安全集合：
Figure 633824DEST_PATH_IMAGE047
即保证了障碍物和关节处的两个球形之间的距离大于一定阈值，根据定义，为保证安全集合S的前向不变性(即系统的状态q能够始终保持在这个安全集合内):
Figure 611007DEST_PATH_IMAGE048
Figure 7353DEST_PATH_IMAGE049
Figure 411790DEST_PATH_IMAGE050
其中J(q)是前向运动学F对关节角度q的导数，即当前构型下的雅可比矩阵，按照定理，如果满足：
Figure 995218DEST_PATH_IMAGE051
则该控制量x保证S的前向不变性，这也就是反应性二次规划优化器中的控制屏障函数项的解释和具体实现。
与现有技术相比，本发明所达到的有益效果是：本发明，第一，本发明提出一种移动操作臂根据视觉反馈环境信息的末端在线局部重规划算法，能够保证始终提供给后续反应性控制器一个可行合理的末端追踪目标。第二，提出了基于二次规划的反应性控制器，将传统的机械臂运动模型扩展为了附加底盘运动状态和松弛变量的增广雅可比矩阵，以此构建的等式约束既满足整体运动学特性，能同时输出底盘和机械臂的控制信号达到整体控制效果，又具有一定的瞬时容错性能，提高算法的鲁棒性适应性的同时，通过调节惩罚松弛变量的力度保证了追踪的精准度。第三，在整体控制算法中通过控制屏障函数约束和可操作性代价，提高了移动协作机器人的工作效率及安全性，大幅提高了移动协作机器人在工业应用中的工作效率和应对环境变化的实时响应性能。
附图说明
附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：
图1是本发明的整体模块结构示意图；
图2是本发明的B样条曲线及其控制点示意图；
图3是本发明的B样条优化原理示意图；
图4是本发明的移动操作臂虚拟关节和坐标系示意图；
图5是本发明的控制屏障函数作用示意图；
图6是本发明的整体算法框架图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
请参阅图1-图6，本发明提供技术方案：基于二次规划的移动操作机器人全身反应规划控制方法，包括以下几个部分：
S1、基于B样条曲线优化的末端轨迹在线重规划；
S2、根据起点和终点生成末端初始轨迹；
S3、设计基于二次规划的反应性全身控制器；
包含以下组成部分：末端轨迹生成及在线重规划模块、环境处理模块、反应性优化控制器、机器人；
上述步骤S1中，基于B样条曲线优化的末端轨迹在线重规划的具体方法为，将末端目标轨迹拉到一个存在不碰撞解的位置，保证后续的反应控制器能够在利用全身所有自由度追踪末端轨迹时同时满足全身所有位置不发生碰撞的约束；
上述步骤S2中，给定了阶数之后，B-样条曲线的形状由其控制点决定且其与贝塞尔曲线相比的一大优点是通过更改控制点位置来更改局部曲线形状，而贝塞尔曲线则动一点则动整体，首先根据起点和终点生成末端初始轨迹如下：
Figure 41671DEST_PATH_IMAGE052
；
Figure 292524DEST_PATH_IMAGE002
和
Figure 133441DEST_PATH_IMAGE003
分别为起点和终点末端位姿的SE(3)表达，
Figure 469745DEST_PATH_IMAGE004
是末端旋量 (twist)的矩阵表达方式，表达了单位时间内从起点位姿到终点位姿需要的空间速度，这一 方法得到的末端初始轨迹类似于图一中的蓝色虚线，看起来像是条螺旋运动曲线；
上述步骤S2中，在
Figure 788730DEST_PATH_IMAGE005
上均匀采样生成
Figure 159669DEST_PATH_IMAGE006
个控制点的集合
Figure 171487DEST_PATH_IMAGE007
作为控制 点的初值，B样条曲线的阶次为
Figure 995087DEST_PATH_IMAGE008
，优化控制点集合去掉首尾的子集
Figure 117764DEST_PATH_IMAGE009
，前后
Figure 611717DEST_PATH_IMAGE008
个控制点会决定边界条件，构建B样条优化问题:
Figure 528858DEST_PATH_IMAGE053
目标函数由四部分组成，其中：
Figure 839753DEST_PATH_IMAGE054
这项代价是顺滑性代价，惩罚三个相邻控制点之间的连续性偏差，防止突变，保证轨迹丝滑度；
Figure 500542DEST_PATH_IMAGE055
碰撞代价中
Figure 580493DEST_PATH_IMAGE013
函数是映射到控制点
Figure 934114DEST_PATH_IMAGE014
与其最近的障碍物的距离，
Figure 466727DEST_PATH_IMAGE015
是距离阈值常 数，通过求解这样的一个优化问题，实现了将机械臂末端轨迹修正到一个可行合适的位置；
上述步骤S3中，首先引入虚拟关节和松弛变量来增广整个机器人到末端旋量的运 动学雅可比矩阵，虚拟关节体现了在当前位置底盘具有的瞬时运动能力，全向底盘具有三 个虚拟关节，而具有非完整约束的差速底盘则只有x方向平动和绕z方向转动这两个瞬时速 度，定义松弛变量
Figure 931206DEST_PATH_IMAGE016
，定义增广雅可比矩阵：
Figure 131243DEST_PATH_IMAGE056
其中
Figure 390186DEST_PATH_IMAGE019
是将在底盘系b中三个虚拟关节的速度旋量雅可比矩阵转换到末端系e 的6*6的转换矩阵；
Figure 410095DEST_PATH_IMAGE020
是映射到末端空间速度旋量的机械臂雅可比矩阵；
Figure 943845DEST_PATH_IMAGE021
是一个6*6的 单位矩阵，用来将松弛变量映射到末端空间速度旋量
Figure 732809DEST_PATH_IMAGE022
:
Figure 428233DEST_PATH_IMAGE057
决策变量
Figure 935437DEST_PATH_IMAGE024
和
Figure 741719DEST_PATH_IMAGE025
分别是底盘虚拟关节速度和机械臂各关节速度，以此构建二 次规划器等式约束，既将底盘运动能力考虑了进去又通过松弛变量的引入允许了追踪末端 空间速度
Figure 650769DEST_PATH_IMAGE022
的瞬时误差，提高了鲁棒性和适应性；
上述步骤S3中，利用上述增广等式约束，我们提出来基于二次规划的安全反应性控制器：
Figure 517094DEST_PATH_IMAGE058
对该二次规划器的各项进行解释说明：
代价函数中的二次项
Figure 246016DEST_PATH_IMAGE030
对广义控制量
Figure 121568DEST_PATH_IMAGE031
进行惩罚，不仅控制了底盘和机械 臂n维速度输出的大小，也同时限制了松弛变量的大小，通过调节（n+6）*（n+6）的对角矩阵Q 值各项大小均衡底盘和机械臂对追踪的贡献力度、末端追踪的精度，松弛变量越小，精度越 高，代价函数中线性项
Figure 885125DEST_PATH_IMAGE032
则包含了机械臂的可操作性代价，其具体推导表达式如下：
首先单位关节角速度满足:
Figure 922351DEST_PATH_IMAGE033
;
由
Figure 138568DEST_PATH_IMAGE034
代入上式可得:
Figure 283723DEST_PATH_IMAGE059
其中q代表机械臂末端在任务空间中的最少参数表达，
Figure 167366DEST_PATH_IMAGE036
，
Figure 375493DEST_PATH_IMAGE037
；
上式是一个在m维空间中的椭圆，这个椭圆的体积正比于A矩阵特征值的 连乘:
Figure 79007DEST_PATH_IMAGE038
；
同时，这一椭圆的体积越小，代表此时机械臂将越有可能失去某一方向的运动能 力，即陷入奇异点，所以该椭圆的体积是衡量机械臂可操作性的一个量化指标，对
Figure 296362DEST_PATH_IMAGE039
求时间 的导数：
Figure 768931DEST_PATH_IMAGE060
所以，代价函数中的c向量写为：
Figure 147960DEST_PATH_IMAGE061
,
Figure 338770DEST_PATH_IMAGE062
可操作性代价保证了移动操作臂整体运行过程中机械臂远离有可能产生奇异点从而失去某些运动能力的位姿；
不等式约束中的
Figure 94236DEST_PATH_IMAGE043
是控制屏障函数项，这个不等式约束是用来保 证移动操作臂在运动过程中全身避障的特性，即安全性约束，是控制屏障函数作用的示意 图，控制屏障函数约束类似于一个安全过滤器，保证了反应性控制器瞬时输出的关节速度 能让机器人维持在原本所在的安全的构型空间S中；
上述步骤S3中，控制屏障函数满足定理:如果h是一个安全集合S上的控制屏障函 数，那么采取满足:
Figure 952471DEST_PATH_IMAGE044
的控制信号u使系统的状态q维持在安全集合S中，控 制信号即为各个关节角速度，系统的状态q包括底盘和机械臂当前的构型；
实施例1：
上述步骤S3中，采用各个关节点与最近障碍物的欧几里得距离构建控制屏障函 数：对于某一个关节点: 定义该关节的前向运动学
Figure 236822DEST_PATH_IMAGE045
是从关节状态映射到该关节空间位 置x,y,z的映射函数；
定义控制屏障函数h(q):
Figure 914928DEST_PATH_IMAGE063
其中O代表与该关节距离最近的障碍物的空间位置[Ox,Oy,Oz], 安全集合：
Figure 208506DEST_PATH_IMAGE064
即保证了障碍物和关节处的两个球形之间的距离大于一定阈值，根据定义，为保证安全集合S的前向不变性(即系统的状态q能够始终保持在这个安全集合内):
Figure 921247DEST_PATH_IMAGE065
Figure 642078DEST_PATH_IMAGE066
Figure 541901DEST_PATH_IMAGE067
其中J(q)是前向运动学F对关节角度q的导数，即当前构型下的雅可比矩阵，按照定理，如果满足：
Figure 639170DEST_PATH_IMAGE068
则该控制量x保证S的前向不变性，这也就是反应性二次规划优化器中的控制屏障函数项的解释和具体实现。
实施例2：
本发明提出的反应性全身控制器能在追踪末端轨迹的同时保证机身的安全性和机械臂的可操作性，这一控制器产生的运动具有连续顺畅性、高环境适应性，高鲁棒性和精确度。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (8)

1.基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：包括以下几个部分：
S1、基于B样条曲线优化的末端轨迹在线重规划；
S2、根据起点和终点生成末端初始轨迹；
S3、设计基于二次规划的反应性全身控制器；
包含以下组成部分：末端轨迹生成及在线重规划模块、环境处理模块、反应性优化控制器、机器人。
2.根据权利要求1所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S1中，基于B样条曲线优化的末端轨迹在线重规划的具体方法为，将末端目标轨迹拉到一个存在不碰撞解的位置，保证后续的反应控制器能够在利用全身所有自由度追踪末端轨迹时同时满足全身所有位置不发生碰撞的约束。
3.根据权利要求2所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S2中，给定了阶数之后，B-样条曲线的形状由其控制点决定且其与贝塞尔曲线相比的一大优点是通过更改控制点位置来更改局部曲线形状，而贝塞尔曲线则动一点则动整体，首先根据起点和终点生成末端初始轨迹如下：
Figure 657900DEST_PATH_IMAGE001
；
Figure 899526DEST_PATH_IMAGE002
和/>
Figure 971387DEST_PATH_IMAGE003
分别为起点和终点末端位姿的SE(3)表达，/>
Figure 880437DEST_PATH_IMAGE004
是末端旋量(twist)的矩阵表达方式，表达了单位时间内从起点位姿到终点位姿需要的空间速度。
4.根据权利要求3所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S2中，在
Figure 481183DEST_PATH_IMAGE005
上均匀采样生成/>
Figure 741263DEST_PATH_IMAGE006
个控制点的集合/>
Figure 351236DEST_PATH_IMAGE007
作为控制点的初值，B样条曲线的阶次为/>
Figure 114792DEST_PATH_IMAGE008
，优化控制点集合去掉首尾的子集/>
Figure 152018DEST_PATH_IMAGE009
，前后
Figure 899395DEST_PATH_IMAGE008
个控制点会决定边界条件，构建B样条优化问题:
Figure 47479DEST_PATH_IMAGE010
目标函数由四部分组成，其中：
Figure 931122DEST_PATH_IMAGE011
这项代价是顺滑性代价，惩罚三个相邻控制点之间的连续性偏差，防止突变，保证轨迹丝滑度；
Figure 139249DEST_PATH_IMAGE012
碰撞代价中
Figure 108342DEST_PATH_IMAGE013
函数是映射到控制点/>
Figure 60117DEST_PATH_IMAGE014
与其最近的障碍物的距离，/>
Figure 63846DEST_PATH_IMAGE015
是距离阈值常数。
5.根据权利要求4所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S3中，首先引入虚拟关节和松弛变量来增广整个机器人到末端旋量的运动学雅可比矩阵，虚拟关节体现了在当前位置底盘具有的瞬时运动能力，全向底盘具有三个虚拟关节，而具有非完整约束的差速底盘则只有x方向平动和绕z方向转动这两个瞬时速度，定义松弛变量
Figure 442874DEST_PATH_IMAGE016
，定义增广雅可比矩阵：
Figure 633684DEST_PATH_IMAGE017
；
Figure 392080DEST_PATH_IMAGE018
；
其中
Figure 984736DEST_PATH_IMAGE019
是将在底盘系b中三个虚拟关节的速度旋量雅可比矩阵转换到末端系e的6*6的转换矩阵；/>
Figure 800245DEST_PATH_IMAGE020
是映射到末端空间速度旋量的机械臂雅可比矩阵；/>
Figure 743930DEST_PATH_IMAGE021
是一个6*6的单位矩阵，用来将松弛变量映射到末端空间速度旋量/>
Figure 37508DEST_PATH_IMAGE022
:
Figure 750249DEST_PATH_IMAGE023
决策变量
Figure 736660DEST_PATH_IMAGE024
和/>
Figure 902062DEST_PATH_IMAGE025
分别是底盘虚拟关节速度和机械臂各关节速度。
6.根据权利要求5所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S3中，利用上述增广等式约束，我们提出来基于二次规划的安全反应性控制器：
Figure 999331DEST_PATH_IMAGE026
Figure 300999DEST_PATH_IMAGE027
b/>
Figure 458311DEST_PATH_IMAGE022
对每个关节i：
Figure 111009DEST_PATH_IMAGE028
Figure 746390DEST_PATH_IMAGE029
对该二次规划器的各项进行解释说明：
代价函数中的二次项
Figure 433723DEST_PATH_IMAGE030
对广义控制量/>
Figure 496357DEST_PATH_IMAGE031
进行惩罚，不仅控制了底盘和机械臂n维速度输出的大小，也同时限制了松弛变量的大小，通过调节（n+6）*（n+6）的对角矩阵Q值各项大小均衡底盘和机械臂对追踪的贡献力度、末端追踪的精度，松弛变量越小，精度越高，代价函数中线性项/>
Figure 636352DEST_PATH_IMAGE032
则包含了机械臂的可操作性代价，其具体推导表达式如下：
首先单位关节角速度满足:
Figure 75423DEST_PATH_IMAGE033
;
由
Figure 617263DEST_PATH_IMAGE034
代入上式可得:
Figure 116377DEST_PATH_IMAGE035
其中q代表机械臂末端在任务空间中的最少参数表达，
Figure 475159DEST_PATH_IMAGE036
，/>
Figure 452342DEST_PATH_IMAGE037
；
上式是一个在m维空间中的椭圆，这个椭圆的体积正比于A矩阵特征值的 连乘:
Figure 848689DEST_PATH_IMAGE038
；
同时，这一椭圆的体积越小，代表此时机械臂将越有可能失去某一方向的运动能力，即陷入奇异点，所以该椭圆的体积是衡量机械臂可操作性的一个量化指标，对
Figure 253125DEST_PATH_IMAGE039
求时间的导数：
Figure 367712DEST_PATH_IMAGE040
所以，代价函数中的c向量写为：
Figure 883007DEST_PATH_IMAGE041
,
Figure 399439DEST_PATH_IMAGE042
可操作性代价保证了移动操作臂整体运行过程中机械臂远离有可能产生奇异点从而失去某些运动能力的位姿；
不等式约束中的
Figure 974776DEST_PATH_IMAGE043
是控制屏障函数项，这个不等式约束是用来保证移动操作臂在运动过程中全身避障的特性，即安全性约束，是控制屏障函数作用的示意图，控制屏障函数约束类似于一个安全过滤器，保证了反应性控制器瞬时输出的关节速度能让机器人维持在原本所在的安全的构型空间S中。
7.根据权利要求6所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S3中，控制屏障函数满足定理:如果h是一个安全集合S上的控制屏障函数，那么采取满足:
Figure 576659DEST_PATH_IMAGE044
的控制信号u使系统的状态q维持在安全集合S中，控制信号即为各个关节角速度，系统的状态q包括底盘和机械臂当前的构型。
8.根据权利要求7所述的基于二次规划的移动操作机器人全身反应规划控制方法，其特征在于：上述步骤S3中，采用各个关节点与最近障碍物的欧几里得距离构建控制屏障函数：对于某一个关节点: 定义该关节的前向运动学
Figure 161224DEST_PATH_IMAGE045
是从关节状态映射到该关节空间位置x,y,z的映射函数；
定义控制屏障函数h(q):
Figure 532163DEST_PATH_IMAGE046
其中O代表与该关节距离最近的障碍物的空间位置[Ox,Oy,Oz], 安全集合：
Figure 278402DEST_PATH_IMAGE047
即保证了障碍物和关节处的两个球形之间的距离大于一定阈值，根据定义，为保证安全集合S的前向不变性(即系统的状态q能够始终保持在这个安全集合内):
Figure 102001DEST_PATH_IMAGE048
Figure 224678DEST_PATH_IMAGE049
Figure 184544DEST_PATH_IMAGE050
其中J(q)是前向运动学F对关节角度q的导数，即当前构型下的雅可比矩阵，按照定理，如果满足：
Figure 367263DEST_PATH_IMAGE051
则该控制量x保证S的前向不变性，这也就是反应性二次规划优化器中的控制屏障函数项的解释和具体实现。
CN202310029943.6A 2023-01-09 2023-01-09 基于二次规划的移动操作机器人全身反应规划控制方法 Pending CN116276966A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310029943.6A CN116276966A (zh) 2023-01-09 2023-01-09 基于二次规划的移动操作机器人全身反应规划控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310029943.6A CN116276966A (zh) 2023-01-09 2023-01-09 基于二次规划的移动操作机器人全身反应规划控制方法

Publications (1)

Publication Number Publication Date
CN116276966A true CN116276966A (zh) 2023-06-23

Family

ID=86782407

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310029943.6A Pending CN116276966A (zh) 2023-01-09 2023-01-09 基于二次规划的移动操作机器人全身反应规划控制方法

Country Status (1)

Country Link
CN (1) CN116276966A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116540553A (zh) * 2023-07-05 2023-08-04 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种基于强化学习的移动机器人安全运动方法
CN116985142A (zh) * 2023-09-25 2023-11-03 北京航空航天大学 一种机器人运动规划方法、装置和机器人
CN117572810A (zh) * 2024-01-16 2024-02-20 大连海事大学 一种基于控制障碍函数的机械臂安全协同控制系统

Cited By (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116540553A (zh) * 2023-07-05 2023-08-04 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种基于强化学习的移动机器人安全运动方法
CN116540553B (zh) * 2023-07-05 2023-08-25 哈尔滨工业大学(深圳)(哈尔滨工业大学深圳科技创新研究院) 一种基于强化学习的移动机器人安全运动方法
CN116985142A (zh) * 2023-09-25 2023-11-03 北京航空航天大学 一种机器人运动规划方法、装置和机器人
CN116985142B (zh) * 2023-09-25 2023-12-08 北京航空航天大学 一种机器人运动规划方法、装置和机器人
CN117572810A (zh) * 2024-01-16 2024-02-20 大连海事大学 一种基于控制障碍函数的机械臂安全协同控制系统
CN117572810B (zh) * 2024-01-16 2024-05-10 大连海事大学 一种基于控制障碍函数的机械臂安全协同控制系统

Similar Documents

Publication Publication Date Title
CN107490965B (zh) 一种空间自由漂浮机械臂的多约束轨迹规划方法
Zhang et al. Real-time kinematic control for redundant manipulators in a time-varying environment: Multiple-dynamic obstacle avoidance and fast tracking of a moving object
Bhardwaj et al. Storm: An integrated framework for fast joint-space model-predictive control for reactive manipulation
CN116276966A (zh) 基于二次规划的移动操作机器人全身反应规划控制方法
CN112757306B (zh) 一种机械臂逆解多解选择和时间最优轨迹规划算法
Wen et al. Elman fuzzy adaptive control for obstacle avoidance of mobile robots using hybrid force/position incorporation
Wang et al. Virtual-goal-guided RRT for visual servoing of mobile robots with FOV constraint
Xie et al. Obstacle avoidance and path planning for multi-joint manipulator in a space robot
Figueredo et al. Switching strategy for flexible task execution using the cooperative dual task-space framework
Atiyah et al. An overview: On path planning optimization criteria and mobile robot navigation
Wang et al. Path planning of a 6-DOF measuring robot with a direction guidance RRT method
Lv et al. Direct trajectory planning method based on IEPSO and fuzzy rewards and punishment theory for multi-degree-of freedom manipulators
Kennel-Maushart et al. Multi-arm payload manipulation via mixed reality
Lembono et al. Probabilistic iterative LQR for short time horizon MPC
Pham Trajectory planning
CN113867157B (zh) 一种控制补偿的最优轨迹规划方法、设备及存储设备
Malik Trajectory Generation for a Multibody Robotic System: Modern Methods Based on Product of Exponentials
Keshmiri et al. Performance comparison of various navigation guidance methods in interception of a moving object by a serial manipulator considering its kinematic and dynamic limits
Hu et al. Trajectory planning method of 6-DOF modular manipulator based on polynomial interpolation
Vahrenkamp et al. Planning and execution of grasping motions on a humanoid robot
Kalaycioglu et al. Coordinated Motion and Force Control of Multi-Rover Robotics System with Mecanum Wheels
Debrouwere et al. Time-optimal path following for robots with object collision avoidance using Lagrangian duality
CN114063621A (zh) 一种轮式机器人编队跟踪与避障控制方法
Jiang et al. Globally perceived obstacle avoidance for robots based on virtual twin and deep reinforcement learning
Sheng et al. Event-Potential Method for Mobile Robot Motion Planning and Control

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