CN113459109A - 机械臂路径规划方法、装置、电子设备及存储介质 - Google Patents
机械臂路径规划方法、装置、电子设备及存储介质 Download PDFInfo
- Publication number
- CN113459109A CN113459109A CN202111029472.6A CN202111029472A CN113459109A CN 113459109 A CN113459109 A CN 113459109A CN 202111029472 A CN202111029472 A CN 202111029472A CN 113459109 A CN113459109 A CN 113459109A
- Authority
- CN
- China
- Prior art keywords
- mechanical arm
- joint
- point pose
- pose
- coordinate system
- 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
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
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Manipulator (AREA)
Abstract
本发明涉及机械臂控制技术领域,提供了一种机械臂路径规划方法、装置、电子设备及存储介质,通过获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{‑θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长;从而可避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题,并保证机械臂运行的平稳性。
Description
技术领域
本发明涉及机械臂控制技术领域,尤其涉及一种机械臂路径规划方法、装置、电子设备及存储介质。
背景技术
机械臂路径规划技术是机械臂控制技术中的一个重要环节,进行机械臂路径规划的主要目的是使机械臂能够无碰撞地从指定的起点到达指定的终点。现有的人工规划路径的方法,通常仅能够满足简单工作环境的使用要求,自动化程度低。现有的基于空间坐标的自动规划路径的方法在规划路径的时候,每次规划出一个候选的路径点时,均需要进行逆运动学解算以验证是否发生碰撞、是否处于奇异点(无法明确的逆向转化为机器人各个关节轴的旋转角度的坐标点,即在机器人工作空间中的逆运动学无解的点)、是否具有多解状态等,计算量大,且规划得到的路径有时候只是基于坐标的角度看是平滑的,但在相邻路径点处机械臂关节轴的旋转角度变化大(即关节空间变化大),导致机械臂运行不平稳,存在安全隐患。
发明内容
鉴于上述现有技术的不足之处,本申请实施例的目的在于提供一种机械臂路径规划方法、装置、电子设备及存储介质,能够避免机械臂沿规划的路径运行时关节状态发生突变,从而保证机械臂运行的平稳性,并能够避免奇异解和多解问题。
第一方面,本申请实施例提供一种机械臂路径规划方法,包括步骤:
A1.获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;
A2.根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;
A3.以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;
其中,θ为各关节的预设的最小运动步长。
本申请实施例的机械臂路径规划方法,以机械臂的每个关节为一个智能体,用多智能体强化学习算法直接从关节空间进行路径规划,得到的路径点位置信息是关节坐标系的坐标,即直接得到机械臂各关节的角度,避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题,也避免了相邻路径点的机械臂关节角度出现突变的情况,从而保证机械臂运行的平稳性。
优选地,所述奖惩机制为:
若机械臂达到第二终点位姿,且不发生碰撞,则将奖励设置为N1,并标记为任务结束,结束回合;N1为预设值;
机械臂在达到第二终点位姿前,每一次仿真移动都有N2的惩罚;N2为预设值;
机械臂在达到第二终点位姿前,若一次仿真移动后,关节坐标系下的位姿与所述第二终点位姿的欧氏距离变小,则得到N3的奖励;
若机械臂在运动过程中与障碍物发生碰撞或机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合。
优选地,所述多智能体强化学习算法为多智能体深度确定性策略梯度算法。
优选地,所述碰撞检测规则为基于包围盒的碰撞检测算法的碰撞检测规则。
第二方面,本申请实施例提供一种机械臂路径规划装置,包括:
获取模块,用于获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;
计算模块,用于根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;
规划模块,用于以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;
其中,θ为各关节的预设的最小运动步长。
优选地,所述奖惩机制为:
若机械臂达到第二终点位姿,且不发生碰撞,则将奖励设置为N1,并标记为任务结束,结束回合;N1为预设值;
机械臂在达到第二终点位姿前,每一次仿真移动都有N2的惩罚;N2为预设值;
机械臂在达到第二终点位姿前,若一次仿真移动后,关节坐标系下的位姿与所述第二终点位姿的欧氏距离变小,则得到N3的奖励;
若机械臂在运动过程中与障碍物发生碰撞或机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合。
优选地,所述多智能体强化学习算法为多智能体深度确定性策略梯度算法。
优选地,所述碰撞检测规则为基于包围盒的碰撞检测算法的碰撞检测规则。
第三方面,本申请实施例提供一种电子设备,包括处理器和存储器,所述存储器中存储有计算机程序,所述处理器通过调用所述存储器中存储的所述计算机程序,用于执行如所述的机械臂路径规划方法的步骤。
第四方面,本申请实施例提供一种存储介质,其上存储有计算机程序,所述计算机程序被处理器执行时运行如所述的机械臂路径规划方法的步骤。
有益效果:
本申请实施例提供的机械臂路径规划方法、装置、电子设备及存储介质,通过获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长;从而,得到的路径点位置信息是关节坐标系的坐标,避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题,也避免了相邻路径点的机械臂关节角度出现突变的情况,从而保证机械臂运行的平稳性。
附图说明
图1为本申请实施例提供的机械臂路径规划方法的流程图。
图2为本申请实施例提供的机械臂路径规划装置的结构示意图。
图3为本申请实施例提供的电子设备的结构示意图。
具体实施方式
下面详细描述本发明的实施方式,所述实施方式的示例在附图中示出,其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的,仅用于解释本发明,而不能理解为对本发明的限制。
在本发明的描述中,需要理解的是,术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系,仅是为了便于描述本发明和简化描述,而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作,因此不能理解为对本发明的限制。此外,术语“第一”、“第二”仅用于描述目的,而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此,限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的描述中,“多个”的含义是两个或两个以上,除非另有明确具体的限定。
下文的公开提供的实施方式或例子用来实现本发明的不同结构。为了简化本发明的公开,下文中对特定例子的部件和设置进行描述。当然,它们仅仅为示例,并且目的不在于限制本发明。此外,本发明可以在不同例子中重复参考数字和/或参考字母,这种重复是为了简化和清楚的目的,其本身不指示所讨论各种实施方式和/或设置之间的关系。此外,本发明提供了的各种特定的工艺和材料的例子,但是本领域普通技术佩戴人员可以意识到其他工艺的应用和/或其他材料的使用。
请参阅图1,本申请实施例提供的一种机械臂路径规划方法,包括步骤:
A1.获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;
A2.根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;
A3.以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;
其中,θ为各关节的预设的最小运动步长。
该机械臂路径规划方法,以机械臂的每个关节为一个智能体,用多智能体强化学习算法直接从关节空间进行路径规划,得到的路径点位置信息是关节坐标系的坐标,即直接得到机械臂各关节的角度,无需如现有技术中通过路径点的空间坐标计算机械臂各关节的角度以控制机械臂运动,避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题;而且在规划过程中,各关节每一步只能正向转动θ角度、负向转动θ角度或不转动,即相邻的路径点之间,各关节的最大转动幅度是θ角度,避免了相邻路径点的机械臂关节角度出现突变的情况,从而保证机械臂运行的平稳性。使用该机械臂路径规划方法进行路径规划,不限制机械臂的关节个数,进行拓展时,只需要增加扩展智能体的个数即可,通用性强,拓展的成本小。
在实际应用中,一般是在仿真环境下执行该机械臂路径规划方法的步骤,在该仿真环境中构建机械臂的模型,并根据障碍物的位置构建障碍物模型,从而可应对各种复杂场景进行路径规划。
其中,空间坐标系下的第一起点位姿和第一终点位姿可分别表示为(x1,y1,z1,α1,β1,γ1)和(x2,y2,z2,α2,β2,γ2),x1、y1、z1分别为第一起点位姿的三个位置坐标,α1、β1、γ1分别为第一起点位姿的三个姿态角度,x2、y2、z2分别为第一终点位姿的三个位置坐标,α2、β2、γ2分别为第一终点位姿的三个姿态角度。
其中,关节坐标系下对应的第二起点位姿和第二终点位姿可分别表示为(a1_1,a2_1,…,ai_1,…,an_1)和(a1_2,a2_2,…,ai_2,…,an_2),ai_1为第二起点位姿中第i个关节的角度,ai_2为第二终点位姿中第i个关节的角度,n为机械臂关节的总数。对于机械臂在第二起点位姿和第二终点位姿之间的任意位置的位姿可表示为(a1,…,ai,…an),ai为第i个关节的角度。
其中,智能体的动作集{-θ,θ,0}中,θ代表的动作是关节正向(如顺时针方向)转动θ角度,-θ代表的动作是关节反向(如逆时针方向)转动θ角度,0代表的动作是关节不转动。其中,θ的值可根据需要的精度设置,θ的值越小,得到的路径越能够保证机械臂运动的平稳性和顺滑性。优选地,θ的值等于机械臂的分辨率(即机械臂关节能够实现的最小转动角度),从而得到的路径最优。
在一些优选实施方式中,所述奖惩机制为:
若机械臂达到第二终点位姿,且不发生碰撞,则将奖励设置为N1,并标记为任务结束,结束回合;N1为预设值;
机械臂在达到第二终点位姿前,每一次仿真移动都有N2的惩罚;N2为预设值;
机械臂在达到第二终点位姿前,若一次仿真移动后,关节坐标系下的位姿与所述第二终点位姿的欧氏距离变小,则得到N3的奖励;
若机械臂在运动过程中与障碍物发生碰撞或机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合。
其中,N1、N2、N3的值可根据实际需要设置;例如,N1=1,N2=-0.01,N3=0.02,但不限于此。
在一些实施方式中,所述多智能体强化学习算法可选择多智能体深度确定性策略梯度算法(MADDPG算法),其优点如下:相比于单智能体算法,能够应用于本方案中将每个关节看作单独智能体的情况,克服对每个智能体来说环境不断变换、单个智能体获取的信息不足的缺点;其次,MADDPG算法中分布式的actor和集中式critic的训练机制,分布式的actor保证了每个智能体可以拥有自己的奖励函数,使用的时候actor只需要局部信息就能运行,集中式critic指的是训练critic网络的时候使用了全局的信息,critic集中了环境中所有信息用以指导actor;此外,支持策略集合效果优化,利用所有策略的整体效果进行优化,以提高算法的稳定性。
在一些实施方式中,所述碰撞检测规则可选用基于包围盒的碰撞检测算法的碰撞检测规则。包围盒是指用规则的形状把场景中的物体包裹起来,以达到快速、简单检测多面体之间是否相交的目的。在进行碰撞检测的过程中先对物体的包围盒进行粗略检测,当两个几何体的包围盒相交时这两个物体才有可能相交;当包围盒不相交时,这两个物体一定不相交。现有的基于包围盒的碰撞检测算法主要包括:轴平行包围盒AABB算法,包围球算法,沿任意方向包围盒OBB算法,固定方向包围盒FDH算法等。综合计算复杂度和精度,优选采用包围球算法和OBB算法。包围球是进行碰撞检测的三维几何体的最小外接球,OBB 是最贴近三维几何体的平行六面体,一个物体的OBB 被定义为包含该三维碰撞体并且相对于坐标轴方向任意的正六面体。
预先建立好机械臂各关节以及环境障碍物的包围盒,其中机械臂关节的包围盒是OBB包围盒(此处,作为智能体的机械臂关节是包括动作执行部件和对应的臂杆的,因此每个关节接近于杆状,故采用OBB包围盒;机械臂与障碍物发生碰撞是指至少一个关节与障碍物发生碰撞,机械臂自身发生碰撞是指关节之间发生碰撞),环境障碍物的包围盒是包围球或OBB包围盒(具体根据环境障碍物的形状设置),步骤A3中,根据不同包围盒之间的碰撞规则判断是否发生机械臂关节之间的碰撞(相邻关节不需要判断)、机械臂关节与环境障碍物的碰撞。具体判断机械臂关节之间是否发生碰撞的步骤如下:
根据分离轴定理判断两个机械臂关节的OBB包围盒之间是否相交(通过分离轴定理判断两个OBB包围盒之间是否相交的具体方法为现有技术);
若相交,则判定所述两个机械臂关节发生碰撞。
具体地,判断机械臂关节与环境障碍物之间是否发生碰撞的步骤如下:
若所述环境障碍物的包围盒为OBB包围盒,则根据分离轴定理判断所述机械臂关节的OBB包围盒与所述环境障碍物的OBB包围盒是否相交,如果相交,则判定所述机械臂关节与所述环境障碍物发生碰撞;
若所述环境障碍物的包围盒为包围球,则判断所述环境障碍物的包围球球心与所述机械臂关节的OBB包围盒表面最近点(即离所述球心最近的点)之间的距离是否小于所述包围球的半径,如果小于,则判定所述机械臂关节与所述环境障碍物发生碰撞。
具体的构建包围盒和碰撞检测接口可以从一些开源的库例如FCL(FlexibleCollision Library)获取;传统的机械臂路径规划过程中通常只需要考虑机械臂与障碍物之间的碰撞情况,而本申请中,由于把每个关节做为一个智能体进行路径规划,有可能出现智能体之间发生碰撞的情况,即可能出现关节之间发生碰撞的情况,从而得到的路径是不合理的,因此,在奖惩机制中,若机械臂在运动过程中机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合,从而可确保最终得到的路径不会引起机械臂的关节之间发生碰撞。
具体地,步骤A3中利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径的具体过程为:在仿真环境中配置好机械臂参数,设定环境障碍物信息、输入起始点信息;每个关节对应一个智能体,设置好单个智能体的行动空间,根据单个关节的最小可移动角度确定智能体的动作集为{-θ,θ,0},不同关节的最小可移动角度不同因此θ数值可能不相同;状态由仿真环境观测机械臂给出,按照前述的奖惩机制和碰撞检测规则设置智能体的奖励函数;配置好以上参数后,开始在仿真环境中采集数据,每个回合至多走500步(实际情况要根据任务起始点和θ做出调整),初期让每个智能体直接采用随机策略在动作空间采样,不使用actor的策略,保证初期采样空间足够大,数据采集到达一定数量后例如1000条开始训练,训练时使用actor选择动作,并增加高斯噪声,越往后噪声比例和方差逐渐减小;每次得到更高的平均奖励后,保存当前的模型和参数到文件,记为最优模型,并根据平均奖励判断是否需要继续训练(当平均奖励收敛且小于预设的阈值则停止训练);停止训练后加载最优模型,输入起始点,环境障碍物、奖励函数(使用与训练时候相同的配置),输出并记录每个智能体每一步选择的动作,构成中间的路径点,直到机械臂到达终点或者终点位置极小领域内,这些点组成整个路径规划的结果。
由上可知,该机械臂路径规划方法,通过获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长;从而,得到的路径点位置信息是关节坐标系的坐标,避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题,也避免了相邻路径点的机械臂关节角度出现突变的情况,从而保证机械臂运行的平稳性。
请参考图2,本申请实施例提供一种机械臂路径规划装置,包括:
获取模块1,用于获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;
计算模块2,用于根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;
规划模块3,用于以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;
其中,θ为各关节的预设的最小运动步长。
其中,空间坐标系下的第一起点位姿和第一终点位姿可分别表示为(x1,y1,z1,α1,β1,γ1)和(x2,y2,z2,α2,β2,γ2),x1、y1、z1分别为第一起点位姿的三个位置坐标,α1、β1、γ1分别为第一起点位姿的三个姿态角度,x2、y2、z2分别为第一终点位姿的三个位置坐标,α2、β2、γ2分别为第一终点位姿的三个姿态角度。
其中,关节坐标系下对应的第二起点位姿和第二终点位姿可分别表示为(a1_1,a2_1,…,ai_1,…,an_1)和(a1_2,a2_2,…,ai_2,…,an_2),ai_1为第二起点位姿中第i个关节的角度,ai_2为第二终点位姿中第i个关节的角度,n为机械臂关节的总数。对于机械臂在第二起点位姿和第二终点位姿之间的任意位置的位姿可表示为(a1,…,ai,…an),ai为第i个关节的角度。
其中,智能体的动作集{-θ,θ,0}中,θ代表的动作是关节正向(如顺时针方向)转动θ角度,-θ代表的动作是关节反向(如逆时针方向)转动θ角度,0代表的动作是关节不转动。其中,θ的值可根据需要的精度设置,θ的值越小,得到的路径越能够保证机械臂运动的平稳性和顺滑性。优选地,θ的值等于机械臂的分辨率(即机械臂关节能够实现的最小转动角度),从而得到的路径最优。
在一些优选实施方式中,所述奖惩机制为:
若机械臂达到第二终点位姿,且不发生碰撞,则将奖励设置为N1,并标记为任务结束,结束回合;N1为预设值;
机械臂在达到第二终点位姿前,每一次仿真移动都有N2的惩罚;N2为预设值;
机械臂在达到第二终点位姿前,若一次仿真移动后,关节坐标系下的位姿与所述第二终点位姿的欧氏距离变小,则得到N3的奖励;
若机械臂在运动过程中与障碍物发生碰撞或机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合。
其中,N1、N2、N3的值可根据实际需要设置;例如,N1=1,N2=-0.01,N3=0.02,但不限于此。
在一些实施方式中,所述多智能体强化学习算法可选择多智能体深度确定性策略梯度算法(MADDPG算法),其优点如下:相比于单智能体算法,能够应用于本方案中将每个关节看作单独智能体的情况,克服对每个智能体来说环境不断变换、单个智能体获取的信息不足的缺点;其次,MADDPG算法中分布式的actor和集中式critic的训练机制,分布式的actor保证了每个智能体可以拥有自己的奖励函数,使用的时候actor只需要局部信息就能运行,集中式critic指的是训练critic网络的时候使用了全局的信息,critic集中了环境中所有信息用以指导actor;此外,支持策略集合效果优化,利用所有策略的整体效果进行优化,以提高算法的稳定性。
在一些实施方式中,所述碰撞检测规则可选用基于包围盒的碰撞检测算法的碰撞检测规则。包围盒是指用规则的形状把场景中的物体包裹起来,以达到快速、简单检测多面体之间是否相交的目的。在进行碰撞检测的过程中先对物体的包围盒进行粗略检测,当两个几何体的包围盒相交时这两个物体才有可能相交;当包围盒不相交时,这两个物体一定不相交。现有的基于包围盒的碰撞检测算法主要包括:轴平行包围盒AABB算法,包围球算法,沿任意方向包围盒OBB算法,固定方向包围盒FDH算法等。综合计算复杂度和精度,优选采用包围球算法和OBB算法。包围球是进行碰撞检测的三维几何体的最小外接球,OBB 是最贴近三维几何体的平行六面体,一个物体的OBB 被定义为包含该三维碰撞体并且相对于坐标轴方向任意的正六面体。
预先建立好机械臂各关节以及环境障碍物的包围盒,其中机械臂关节的包围盒是OBB包围盒,环境障碍物的包围盒是包围球或OBB包围盒(具体根据环境障碍物的形状设置),规划模块3根据不同包围盒之间的碰撞规则判断是否发生机械臂关节之间的碰撞(相邻关节不需要判断)、机械臂关节与环境障碍物的碰撞。具体判断机械臂关节之间是否发生碰撞的步骤如下:
根据分离轴定理判断两个机械臂关节的OBB包围盒之间是否相交(通过分离轴定理判断两个OBB包围盒之间是否相交的具体方法为现有技术);
若相交,则判定所述两个机械臂关节发生碰撞。
具体地,判断机械臂关节与环境障碍物之间是否发生碰撞的步骤如下:
若所述环境障碍物的包围盒为OBB包围盒,则根据分离轴定理判断所述机械臂关节的OBB包围盒与所述环境障碍物的OBB包围盒是否相交,如果相交,则判定所述机械臂关节与所述环境障碍物发生碰撞;
若所述环境障碍物的包围盒为包围球,则判断所述环境障碍物的包围球球心与所述机械臂关节的OBB包围盒表面最近点(即离所述球心最近的点)之间的距离是否小于所述包围球的半径,如果小于,则判定所述机械臂关节与所述环境障碍物发生碰撞。
具体的构建包围盒和碰撞检测接口可以从一些开源的库例如FCL(FlexibleCollision Library)获取。
具体地,规划模块3利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径的具体过程为:在仿真环境中配置好机械臂参数,设定环境障碍物信息、输入起始点信息;每个关节对应一个智能体,设置好单个智能体的行动空间,根据单个关节的最小可移动角度确定智能体的动作集为{-θ,θ,0},不同关节的最小可移动角度不同因此θ数值可能不相同;状态由仿真环境观测机械臂给出,按照前述的奖惩机制和碰撞检测规则设置智能体的奖励函数;配置好以上参数后,开始在仿真环境中采集数据,每个回合至多走500步(实际情况要根据任务起始点和θ做出调整),初期让每个智能体直接采用随机策略在动作空间采样,不使用actor的策略,保证初期采样空间足够大,数据采集到达一定数量后例如1000条开始训练,训练时使用actor选择动作,并增加高斯噪声,越往后噪声比例和方差逐渐减小;每次得到更高的平均奖励后,保存当前的模型和参数到文件,记为最优模型,并根据平均奖励判断是否需要继续训练(当平均奖励收敛且小于预设的阈值则停止训练);停止训练后加载最优模型,输入起始点,环境障碍物、奖励函数(使用与训练时候相同的配置),输出并记录每个智能体每一步选择的动作,构成中间的路径点,直到机械臂到达终点或者终点位置极小领域内,这些点组成整个路径规划的结果。
由上可知,该机械臂路径规划装置,通过获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长;从而,得到的路径点位置信息是关节坐标系的坐标,避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题,也避免了相邻路径点的机械臂关节角度出现突变的情况,从而保证机械臂运行的平稳性。
请参阅图3,本申请实施例还提供一种电子设备100,包括处理器101和存储器102,存储器102中存储有计算机程序,处理器101通过调用存储器102中存储的计算机程序,用于执行如上述的机械臂路径规划方法的步骤。
其中,处理器101与存储器102电性连接。处理器101是电子设备100的控制中心,利用各种接口和线路连接整个电子设备的各个部分,通过运行或调用存储在存储器102内的计算机程序,以及调用存储在存储器102内的数据,执行电子设备的各种功能和处理数据,从而对电子设备进行整体监控。
存储器102可用于存储计算机程序和数据。存储器102存储的计算机程序中包含有可在处理器中执行的指令。计算机程序可以组成各种功能模块。处理器101通过调用存储在存储器102的计算机程序,从而执行各种功能应用以及数据处理。
在本实施例中,电子设备100中的处理器101会按照如下的步骤,将一个或一个以上的计算机程序的进程对应的指令加载到存储器102中,并由处理器101来运行存储在存储器102中的计算机程序,从而实现各种功能:获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长。
由上可知,该电子设备,通过获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长;从而,得到的路径点位置信息是关节坐标系的坐标,避免由空间坐标通过逆运动学解算计算各关节角度而导致的奇异解和多解问题,也避免了相邻路径点的机械臂关节角度出现突变的情况,从而保证机械臂运行的平稳性。
本申请实施例还提供一种存储介质,其上存储有计算机程序,该计算机程序被处理器执行时运行如上述的机械臂路径规划方法的步骤,以实现以下功能:获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;其中,θ为各关节的预设的最小运动步长。
其中,存储介质可以由任何类型的易失性或非易失性存储设备或者它们的组合实现,如静态随机存取存储器(Static Random Access Memory, 简称SRAM),电可擦除可编程只读存储器(Electrically Erasable Programmable Read-Only Memory, 简称EEPROM),可擦除可编程只读存储器(Erasable Programmable Read Only Memory, 简称EPROM),可编程只读存储器(Programmable Red-Only Memory, 简称PROM),只读存储器(Read-OnlyMemory, 简称ROM),磁存储器,快闪存储器,磁盘或光盘。
综上所述,虽然本发明已以优选实施例揭露如上,但上述优选实施例并非用以限制本发明,本领域的普通技术佩戴人员,在不脱离本发明的精神和范围内,均可作各种更动与润饰,其方案与本发明实质上相同。
Claims (10)
1.一种机械臂路径规划方法,其特征在于,包括步骤:
A1.获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;
A2.根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;
A3.以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;
其中,θ为各关节的预设的最小运动步长。
2.根据权利要求1所述的机械臂路径规划方法,其特征在于,所述奖惩机制为:
若机械臂达到第二终点位姿,且不发生碰撞,则将奖励设置为N1,并标记为任务结束,结束回合;N1为预设值;
机械臂在达到第二终点位姿前,每一次仿真移动都有N2的惩罚;N2为预设值;
机械臂在达到第二终点位姿前,若一次仿真移动后,关节坐标系下的位姿与所述第二终点位姿的欧氏距离变小,则得到N3的奖励;
若机械臂在运动过程中与障碍物发生碰撞或机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合。
3.根据权利要求1所述的机械臂路径规划方法,其特征在于,所述多智能体强化学习算法为多智能体深度确定性策略梯度算法。
4.根据权利要求1所述的机械臂路径规划方法,其特征在于,所述碰撞检测规则为基于包围盒的碰撞检测算法的碰撞检测规则。
5.一种机械臂路径规划装置,其特征在于,包括:
获取模块,用于获取空间坐标系下的第一起点位姿和第一终点位姿,并获取障碍物位置信息;
计算模块,用于根据所述空间坐标系下的第一起点位姿和第一终点位姿,通过运动学逆解算计算关节坐标系下对应的第二起点位姿和第二终点位姿;
规划模块,用于以每个关节为一个智能体,以关节的角度为智能体的状态集,以{-θ,θ,0}为智能体的动作集,根据预设的碰撞检测规则和奖惩机制,利用多智能体强化学习算法生成从第二起点位姿到达第二终点位姿的路径;
其中,θ为各关节的预设的最小运动步长。
6.根据权利要求5所述的机械臂路径规划装置,其特征在于,所述奖惩机制为:
若机械臂达到第二终点位姿,且不发生碰撞,则将奖励设置为N1,并标记为任务结束,结束回合;N1为预设值;
机械臂在达到第二终点位姿前,每一次仿真移动都有N2的惩罚;N2为预设值;
机械臂在达到第二终点位姿前,若一次仿真移动后,关节坐标系下的位姿与所述第二终点位姿的欧氏距离变小,则得到N3的奖励;
若机械臂在运动过程中与障碍物发生碰撞或机械臂自身发生碰撞,则得到-N1的惩罚,并标记为任务结束,结束回合。
7.根据权利要求5所述的机械臂路径规划装置,其特征在于,所述多智能体强化学习算法为多智能体深度确定性策略梯度算法。
8.根据权利要求5所述的机械臂路径规划装置,其特征在于,所述碰撞检测规则为基于包围盒的碰撞检测算法的碰撞检测规则。
9.一种电子设备,其特征在于,包括处理器和存储器,所述存储器中存储有计算机程序,所述处理器通过调用所述存储器中存储的所述计算机程序,用于执行如权利要求1-4任一项所述的机械臂路径规划方法的步骤。
10.一种存储介质,其上存储有计算机程序,其特征在于,所述计算机程序被处理器执行时运行如权利要求1-4任一项所述的机械臂路径规划方法的步骤。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111029472.6A CN113459109B (zh) | 2021-09-03 | 2021-09-03 | 机械臂路径规划方法、装置、电子设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202111029472.6A CN113459109B (zh) | 2021-09-03 | 2021-09-03 | 机械臂路径规划方法、装置、电子设备及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113459109A true CN113459109A (zh) | 2021-10-01 |
CN113459109B CN113459109B (zh) | 2021-11-26 |
Family
ID=77867227
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202111029472.6A Active CN113459109B (zh) | 2021-09-03 | 2021-09-03 | 机械臂路径规划方法、装置、电子设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113459109B (zh) |
Cited By (6)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113988202A (zh) * | 2021-11-04 | 2022-01-28 | 季华实验室 | 一种基于深度学习的机械臂异常振动检测方法 |
CN114536342A (zh) * | 2022-03-23 | 2022-05-27 | 宁波睿达医疗器械有限公司 | 一种多臂系统及其臂间防撞控制方法 |
CN114800523A (zh) * | 2022-05-26 | 2022-07-29 | 江西省智能产业技术创新研究院 | 机械臂轨迹修正方法、系统、计算机及可读存储介质 |
CN115305980A (zh) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
CN115933688A (zh) * | 2022-12-28 | 2023-04-07 | 南京衍构科技有限公司 | 一种多机器人协同工作避障方法、系统、设备及存储介质 |
CN117680977A (zh) * | 2024-02-04 | 2024-03-12 | 季华实验室 | 机器人上料拼接对位方法、装置、设备及介质 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2018171663A (ja) * | 2017-03-31 | 2018-11-08 | ファナック株式会社 | 行動情報学習装置、ロボット制御システム及び行動情報学習方法 |
CN109906132A (zh) * | 2016-09-15 | 2019-06-18 | 谷歌有限责任公司 | 机器人操纵的深度强化学习 |
CN111785045A (zh) * | 2020-06-17 | 2020-10-16 | 南京理工大学 | 基于演员-评论家算法的分布式交通信号灯联合控制方法 |
CN111897327A (zh) * | 2020-07-14 | 2020-11-06 | 季华实验室 | 多移动机器人控制/分派模型获取方法、装置、电子设备 |
CN111923039A (zh) * | 2020-07-14 | 2020-11-13 | 西北工业大学 | 一种基于强化学习的冗余机械臂路径规划方法 |
CN112809682A (zh) * | 2021-01-27 | 2021-05-18 | 佛山科学技术学院 | 机械臂避障路径规划方法、系统及存储介质 |
CN113156980A (zh) * | 2021-05-28 | 2021-07-23 | 山东大学 | 一种基于深度强化学习的塔式起重机路径规划方法及系统 |
-
2021
- 2021-09-03 CN CN202111029472.6A patent/CN113459109B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN109906132A (zh) * | 2016-09-15 | 2019-06-18 | 谷歌有限责任公司 | 机器人操纵的深度强化学习 |
JP2018171663A (ja) * | 2017-03-31 | 2018-11-08 | ファナック株式会社 | 行動情報学習装置、ロボット制御システム及び行動情報学習方法 |
CN111785045A (zh) * | 2020-06-17 | 2020-10-16 | 南京理工大学 | 基于演员-评论家算法的分布式交通信号灯联合控制方法 |
CN111897327A (zh) * | 2020-07-14 | 2020-11-06 | 季华实验室 | 多移动机器人控制/分派模型获取方法、装置、电子设备 |
CN111923039A (zh) * | 2020-07-14 | 2020-11-13 | 西北工业大学 | 一种基于强化学习的冗余机械臂路径规划方法 |
CN112809682A (zh) * | 2021-01-27 | 2021-05-18 | 佛山科学技术学院 | 机械臂避障路径规划方法、系统及存储介质 |
CN113156980A (zh) * | 2021-05-28 | 2021-07-23 | 山东大学 | 一种基于深度强化学习的塔式起重机路径规划方法及系统 |
Non-Patent Citations (1)
Title |
---|
丘文波: "工业机械臂智能路径规划研究", 《中国优秀硕士学位论文全文数据库 信息科技辑》 * |
Cited By (10)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113988202A (zh) * | 2021-11-04 | 2022-01-28 | 季华实验室 | 一种基于深度学习的机械臂异常振动检测方法 |
CN114536342A (zh) * | 2022-03-23 | 2022-05-27 | 宁波睿达医疗器械有限公司 | 一种多臂系统及其臂间防撞控制方法 |
CN114800523A (zh) * | 2022-05-26 | 2022-07-29 | 江西省智能产业技术创新研究院 | 机械臂轨迹修正方法、系统、计算机及可读存储介质 |
CN114800523B (zh) * | 2022-05-26 | 2023-12-01 | 江西省智能产业技术创新研究院 | 机械臂轨迹修正方法、系统、计算机及可读存储介质 |
CN115305980A (zh) * | 2022-08-16 | 2022-11-08 | 湖南中联重科智能高空作业机械有限公司 | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
CN115305980B (zh) * | 2022-08-16 | 2023-09-15 | 湖南中联重科智能高空作业机械有限公司 | 用于控制折叠臂式臂架的方法、处理器、装置及工程设备 |
CN115933688A (zh) * | 2022-12-28 | 2023-04-07 | 南京衍构科技有限公司 | 一种多机器人协同工作避障方法、系统、设备及存储介质 |
CN115933688B (zh) * | 2022-12-28 | 2024-03-29 | 南京衍构科技有限公司 | 一种多机器人协同工作避障方法、系统、设备及存储介质 |
CN117680977A (zh) * | 2024-02-04 | 2024-03-12 | 季华实验室 | 机器人上料拼接对位方法、装置、设备及介质 |
CN117680977B (zh) * | 2024-02-04 | 2024-04-16 | 季华实验室 | 机器人上料拼接对位方法、装置、设备及介质 |
Also Published As
Publication number | Publication date |
---|---|
CN113459109B (zh) | 2021-11-26 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN113459109B (zh) | 机械臂路径规划方法、装置、电子设备及存储介质 | |
JP6378783B2 (ja) | アーム型のロボットの障害物自動回避方法及び制御装置 | |
CN108356819B (zh) | 基于改进a*算法的工业机械臂无碰撞路径规划方法 | |
CN109960880B (zh) | 一种基于机器学习的工业机器人避障路径规划方法 | |
US11745346B2 (en) | Motion planning of a robot storing a discretized environment on one or more processors and improved operation of same | |
CN111015656A (zh) | 一种机器人主动避障的控制方法、装置及存储介质 | |
CN112809682B (zh) | 机械臂避障路径规划方法、系统及存储介质 | |
CN108638055B (zh) | 一种七自由度空间机械臂自主避障规划方法 | |
CN110488806B (zh) | 用于求取期望设备轮廓的方法和系统 | |
US10919154B2 (en) | Interference determination method, interference determination system, and computer program | |
JP2019177436A (ja) | ロボット制御装置、ロボットの関節の角度を求める方法、プログラム | |
CN114414231A (zh) | 一种动态环境下机械手自主避障规划方法和系统 | |
CN115502961A (zh) | 基于人体手臂运动信息预测的人机在线避障方法及系统 | |
CN109807933B (zh) | 一种能力图点云更新方法、装置、设备及存储介质 | |
Li et al. | Navigation of mobile robots based on deep reinforcement learning: Reward function optimization and knowledge transfer | |
US10035264B1 (en) | Real time robot implementation of state machine | |
Ota et al. | Deep reactive planning in dynamic environments | |
CN112894817B (zh) | 一种任务空间下的机械臂运动规划方法 | |
CN116237950A (zh) | 基于分段运动规划策略的机器人末端精确控制方法及设备 | |
US20220143836A1 (en) | Computer-readable recording medium storing operation control program, operation control method, and operation control apparatus | |
Orthey et al. | Towards reactive whole-body motion planning in cluttered environments by precomputing feasible motion spaces | |
US11285604B2 (en) | Robot collision detection using cascading variational autoencoder | |
Cao et al. | A General Method for Autonomous Assembly of Arbitrary Parts in the Presence of Uncertainty | |
Muradi et al. | Sample-based motion planning for multi-robot systems | |
Huang et al. | CEASE: Collision-Evaluation-based Active Sense System for Collaborative Robotic Arms |
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 | ||
GR01 | Patent grant |