CN111251306B - 一种带有底盘误差的机械臂路径规划方法 - Google Patents
一种带有底盘误差的机械臂路径规划方法 Download PDFInfo
- Publication number
- CN111251306B CN111251306B CN202010191628.XA CN202010191628A CN111251306B CN 111251306 B CN111251306 B CN 111251306B CN 202010191628 A CN202010191628 A CN 202010191628A CN 111251306 B CN111251306 B CN 111251306B
- Authority
- CN
- China
- Prior art keywords
- node
- random
- new
- mechanical arm
- nodes
- 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.)
- Active
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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种带有底盘误差的移动机械臂路径规划方法,包括以下步骤:获取移动底盘在运动空间中实际位置与目标位置的误差值,依据误差值,重构构型空间,并对起始位置和目标位置进行修正,创建以修正后的起始点为根节点的随机树,随机树包括父节点和子节点;在构形空间中产生一个随机点xrand;搜索与随机点距离最近的节点xnear;生成新节点xnew,调用回归过滤机制;调用随机点边界扩展机制,更新边界节点的状态信息,确定边界节点的可扩展性;确定机械臂在工作空间中的位姿,对新节点进行碰撞检测;扩展并更新随机树;检查是否满足终止条件,获取机械臂规划路径。本发明可有效减少机械臂轨迹规划时间,适用于复杂空间场景的机械臂路径规划。
Description
技术领域
本发明属于机械臂轨迹规划领域,具体地说是一种带有底盘误差的机械臂路径规划方法。
背景技术
机械臂轨迹规划是指计算出一条合适的运动轨迹,实现机械臂避障并指导完成相应的作业任务,其主要评价指标有安全可靠、路径最短、时间最少等。在现场作业中,机械臂面临现场工作环境狭窄、空间结构复杂等问题。
机械臂轨迹规划方法众多,RRT算法是机械臂轨迹规划领域使用最广泛的方法之一,尤其适用于关节空间中的路径规划。RRT算法是一种采用增量方式增长的随机采样算法,具有概率完备性,但在对未知领域进行搜索时覆盖面大、搜索面广,导致时间代价大、规划路径不是最优等问题。当前,基于RRT算法的研究大多集于在算法本身,改进算法在不同程度上提高了算法的计算效率。但是,算法改进并没有充分研究特定的复杂作业环境和算法计算过程特征,比如狭窄通道和陷于局部最小值等问题。
对于通用的移动机械臂,由于有移动载体的存在,使得在运动过程会存在较大的停靠误差,这对于机械臂的规划带来很多不确定性。同时由于机械臂的规划属于高纬度空间的规划,在知道停靠的误差情况行直接的补偿会导致运算量极大,为了提高运算效率,并让补偿方法更为直接,需要进行进一步考虑。
发明内容
本发明要解决的技术问题是提供一种带有底盘误差的机械臂路径规划方法。
为了解决上述技术问题,本发明采取以下技术方案:
一种带有底盘误差的机械臂路径规划方法,包括以下步骤:
根据定位算法,得出移动底盘在运动空间中实际位置与目标位置的误差值,并依据误差值,重构构型空间,并对起始位置和目标位置进行修正:
e(x,y,θ)=E(x,y,θ)
其中E(*)为环境匹配函数,e(*)为误差函数,
Ce(x,y,θ)←C(x,y,θ)∩C(e(x,y,θ))
pe(x,y,θ)←p(x,y,θ)∩Ce(x,y,θ)
其中C为构型空间表达,Ce为考虑误差之后的空间重构,p为试教点位置,pe为修正后在空间中的映射位置。
初始化起始点和目标点,创建以修正后的起始点为根节点的随机树,随机树包括父节点和子节点;
调用具备偏向概率采样的随机函数,在构形空间中产生一个随机点xrand;
遍历随机树的所有子节点,搜索与随机点距离最近的节点xnear;
生成新节点xnew,调用回归过滤机制,将不符合要求的节点删除,不扩展到随机树中,将符合要求的节点扩展到随机树中;
调用随机点边界扩展机制,更新边界节点的状态信息,确定边界节点的可扩展性;
确定机械臂在工作空间中的位姿,对新节点进行碰撞检测;
扩展并更新随机树;
检查是否满足终止条件,获取机械臂规划路径。
所述获取机械臂规划路径具体为:
从目标点出发,在随机树中搜索与当前节点直接相连的节点,即父节点,不断向上回溯,直至回溯到随机树根节点,即起始点,回溯的路径即为机械臂所规划的路径。
所述创建的随机树具体为:
随机树以起始点为根节点,以指定步长ε为父节点与子节点间的固定距离,父节点与子节点连线方向任意,父节点可扩展任意数量的子节点,随机树的所有节点都位于构形空间的自由空间中。
所述生成随机点的具备偏向概率采样的随机函数具体为:
p=rand (1)
所述生成的新节点具体为:
在随机点xrand和最近节点xnear连线方向上,以最近节点xnear为父节点,根据指定步长ε,生成新节点xnew。
所述回归过滤机制具体为:
对于新节点xnew,与该新节点xnew直接相连的节点为xnear,距离为步长ε,当新节点xnew与随机树中所有节点的最短距离小于步长ε时,即与新节点xnew距离最近的节点不为xnear,则当前xnew节点是回归的,定义为回归节点,将该回归节点直接过滤,不扩展到随机树中,当新节点与随机树中所有节点的最短距离等于或大于步长ε时,该新节点符合要求,将该新节点扩展到随机树中。
所述随机点边界扩展机制具体为:
边界节点是指其扩展的节点有一定概率与障碍物发生碰撞的节点;
为每一个新扩展的节点定义一个状态值δ,δ值随着随机树的扩展不断更迭,新扩展的节点状态值δ初始为0;
定义当前有m条边与该节点连接,若扩展的子节点与障碍物边界碰撞,则该节点的状态值δ为0,δ值更新为1/2,否则将δ值增加1/(m+1);若扩展的子节点与障碍物边界不发生碰撞,则该节点的状态值δ将减少1/(m+1),若减少后的状态值δ小于0,则δ值更新为0;
定义边界节点在扩展过程中与边界发生碰撞的概率阈值为CP,如果状态值δ>CP,则该边界节点将不再扩展任何子节点;如果状态值δ≤CP,则将该节点扩展到随机树中。
所述对新节点进行碰撞检测具体为:
根据已知的机械臂在构形空间中各个关节的旋转角度(θ1,θ2,…,θn),执行机械臂的运动学正解,求解机械臂末端在笛卡尔坐标系中的位置,检测机械臂末端和机械臂本体是否与工作工件中的障碍物发生碰撞。
所述扩展并更新随机树具体为:
对于满足碰撞检测的新随机节点,将该新随机节点扩展到随机树中,并记录当前机械臂各关节的角度信息;初始化当前新节点的状态信息,根据随机点边界扩展机制更新与该新节点连接的父节点的状态信息。
所述终止条件具体为:
在满足约束要求的新节点扩展到随机树后,判断该新节点与目标点间的距离,若距离小于步长ε,且新节点与目标点的连线不穿过障碍物,则终止随机树的扩展过程;否则继续扩展随机树。
所述获取机械臂规划路径具体为:
本发明具有以下有益效果:
引入了回归过滤,显著减少了随机树节点的数量,减少了机械臂试探次数,提升了算法的计算效率,避免了算法陷入局部最小值的困境;机械臂在轨迹规划过程中,碰撞检测是非常耗时但又不可避免的过程,引入了随机点边界扩展机制,使机械臂尽量远离障碍物,明显降低了碰撞检测次数,可有效减少机械臂轨迹规划时间。
附图说明
附图1为本发明流程示意图;
附图2为本发明方法获得的路径图;
附图3位本发明应用于六轴机械臂的仿真路径图。
具体实施方式
为能进一步了解本发明的特征、技术手段以及所达到的具体目的、功能,下面结合附图与具体实施方式对本发明作进一步详细描述。
如附图1所示,本发明揭示了一种带有底盘误差的机械臂路径规划方法,包括以下步骤:
S1,根据基于粒子滤波的定位算法,得出移动底盘在运动空间中实际位置与目标位置的误差值,并依据误差值,重构构型空间,并对起始位置和目标位置进行修正:
e(x,y,θ)=E(x,y,θ)
其中E(*)为环境匹配函数,e(*)为误差函数。
Ce(x,y,θ)←C(x,y,θ)∩C(e(x,y,θ))
pe(x,y,θ)←p(x,y,θ)∩Ce(x,y,θ)
其中C为构型空间表达,Ce为考虑误差之后的空间重构,p为试教点位置,pe为修正后在空间中的映射位置。
S2,初始化起始点和目标点,创建以修正起始点为根节点的随机树,随机树包括父节点和子节点。随机树以起始点为根节点,以指定步长ε为父节点与子节点间的固定距离,父节点与子节点连线方向任意,父节点可扩展任意数量的子节点,随机树的所有节点都位于构形空间的自由空间中。
S3,调用具备偏向概率采样的随机函数,在构形空间中产生一个随机点xrand。生成随机点的具备偏向概率采样的随机函数具体为:
p=rand(1)
S4,遍历随机树的所有子节点,比较随机节点与随机树子节点的欧几里得距离,搜索与随机点距离最近的节点xnear。
S5,生成新节点xnew,调用回归过滤机制,将不符合要求的节点删除,不扩展到随机树中,将符合要求的节点扩展到随机树中。具体为在随机点xrand和最近节点xnear连线方向上,以最近节点xnear为父节点,根据指定步长ε,生成新节点xnew。
回归过滤机制具体为:
对于新节点xnew,与该新节点xnew直接相连的节点为xnear,距离为步长ε,当新节点xnew与随机树中所有节点的最短距离小于步长ε时,即与新节点xnew距离最近的节点不为xnear,则当前xnew节点是回归的,定义为回归节点,将该回归节点直接过滤,不扩展到随机树中,当新节点与随机树中所有节点的最短距离等于或大于步长ε时,该新节点符合要求,将该新节点扩展到随机树中。
S6,调用随机点边界扩展机制,更新边界节点的状态信息,确定边界节点的可扩展性。随机点边界扩展机制具体为:
边界节点是指其扩展的节点有一定概率与障碍物发生碰撞的节点;
为每一个新扩展的节点定义一个状态值δ,δ值随着随机树的扩展不断更迭,新扩展的节点状态值δ初始为0;
定义当前有m条边与该节点连接,若扩展的子节点与障碍物边界碰撞,则该节点的状态值δ为0,δ值更新为1/2,否则将δ值增加1/(m+1);若扩展的子节点与障碍物边界不发生碰撞,则该节点的状态值δ将减少1/(m+1),若减少后的状态值δ小于0,则δ值更新为0;
定义边界节点在扩展过程中与边界发生碰撞的概率阈值为CP,如果状态值δ>CP,则该边界节点将不再扩展任何子节点;如果状态值δ≤CP,则将该节点扩展到随机树中。
S7,确定机械臂在工作空间中的位姿,对新节点进行碰撞检测。
碰撞检测具体为:根据已知的机械臂在构形空间中各个关节的旋转角度(θ1,θ2,…,θn),执行机械臂的运动学正解,求解机械臂末端在笛卡尔坐标系中的位置,检测机械臂末端和机械臂本体是否与工作工件中的障碍物发生碰撞。机械臂的运动学正解,为公知技术,在此不再详细赘述。
S8,扩展并更新随机树。具体为:对于满足碰撞检测的新随机节点,将该新随机节点扩展到随机树中,并记录当前机械臂各关节的角度信息;初始化当前新节点的状态信息,根据随机点边界扩展机制更新与该新节点连接的父节点的状态信息。
S9,检查是否满足终止条件,获取机械臂规划路径。
所述终止条件具体为:在满足约束要求的新节点扩展到随机树后,判断该新节点与目标点间的距离,若距离小于步长ε,且新节点与目标点的连线不穿过障碍物,则终止随机树的扩展过程;否则继续扩展随机树。该约束要求即为随机点边界扩展机制。
所述获取机械臂规划路径具体为:从目标点出发,在随机树中搜索与当前节点直接相连的节点,即父节点,不断向上回溯,直至回溯到随机树根节点,即起始点,回溯的路径即为机械臂所规划的路径。
本发明实施方便,计算效率高,明显降低了机械臂路径规划时间。如图2所示,在复杂的迷宫地图环境下,本方法能够快速地规划处一条完整、连续、无碰撞的可行运动轨迹,避免了现有算法在复杂地图环境中规划路径时陷于局部最小值和规划时间长的困境。如图3所示,本发明可以在仿真环境下指导机械臂规划一条从初始位置运动到摆放工件的货架的指定位置的路径。
需要说明的是,以上仅为本发明的优选实施例而已,并不用于限制本发明,尽管参照实施例对本发明进行了详细的说明,对于本领域的技术人员来说,其依然可以对前述实施例所记载的技术方案进行修改,或者对其中部分技术特征进行等同替换,但是凡在本发明的精神和原则之内,所作的任何修改、等同替换、改进等,均应包含在本发明的保护范围之内。
Claims (7)
1.一种带有底盘误差的机械臂路径规划方法,包括以下步骤:
获取移动底盘在运动空间中实际位置与目标位置的误差值,并依据误差值,重构构型空间,并对起始位置和目标位置进行修正;
初始化起始位置和目标位置,创建以修正后的起始位置为根节点的随机树,随机树包括父节点和子节点;
调用具备偏向概率采样的随机函数,在构形空间中产生一个随机点xrand;
遍历随机树的所有子节点,搜索与随机点距离最近的节点xnear;
生成新节点xnew,调用回归过滤机制,将不符合要求的节点删除,不扩展到随机树中,将符合要求的节点扩展到随机树中;
调用随机点边界扩展机制,更新边界节点的状态信息,确定边界节点的可扩展性;
确定机械臂在工作空间中的位姿,对新节点进行碰撞检测;
扩展并更新随机树;
检查是否满足终止条件,获取机械臂规划路径;
所述回归过滤机制具体为:
对于新节点xnew,与该新节点xnew直接相连的节点为xnear,距离为步长ε,当新节点xnew与随机树中所有节点的最短距离小于步长ε时,即与新节点xnew距离最近的节点不为xnear,则当前xnew节点是回归的,定义为回归节点,将该回归节点直接过滤,不扩展到随机树中,当新节点与随机树中所有节点的最短距离等于或大于步长ε时,该新节点符合要求,将该新节点扩展到随机树中;
所述随机点边界扩展机制具体为:
边界节点是指其扩展的节点有一定概率与障碍物发生碰撞的节点;
为每一个新扩展的节点定义一个状态值δ,δ值随着随机树的扩展不断更迭,新扩展的节点状态值δ初始为0;
定义当前有m条边与边界节点连接,若扩展的子节点与障碍物边界碰撞,若边界节点的状态值为0则将δ值更新为1/2,否则将δ值增加1/(m+1);若扩展的子节点与障碍物边界不发生碰撞,则该节点的状态值δ将减少1/(m+1),若减少后的状态值δ小于0,则δ值更新为0;
定义边界节点在扩展过程中与边界发生碰撞的概率阈值为CP,如果状态值δ>CP,则该边界节点将不再扩展任何子节点;如果状态值δ≤CP,则将该节点扩展到随机树中;
所述终止条件具体为:
将新节点扩展到随机树后,判断该新节点与目标点间的距离,若距离小于步长ε,且新节点与目标点的连线不穿过障碍物,则终止随机树的扩展过程;否则继续扩展随机树。
2.根据权利要求1所述的带有底盘误差的机械臂路径规划方法,其特征在于,所述依据误差值,重构构型空间,具体为:
e(x,y,θ)=E(x,y,θ)
其中E(*)为环境匹配函数,e(*)为误差函数,
Ce(x,y,θ)←C(x,y,θ)∩C(e(x,y,θ))
pe(x,y,θ)←p(x,y,θ)∩Ce(x,y,θ)
其中C为构型空间,Ce为考虑误差之后的空间重构,p为示教点位置,pe为p修正后在空间中的映射位置。
3.根据权利要求2所述的带有底盘误差的机械臂路径规划方法,其特征在于,所述创建的随机树具体为:
随机树以起始点为根节点,以指定步长ε为父节点与子节点间的固定距离,父节点与子节点连线方向任意,父节点可扩展任意数量的子节点,随机树的所有节点都位于构形空间的自由空间中。
5.根据权利要求4所述的带有底盘误差的机械臂路径规划方法,其特征在于,所述生成的新节点具体为:
在随机点xrand和最近节点xnear连线方向上,以最近节点xnear为父节点,根据指定步长ε,生成新节点xnew。
6.根据权利要求5所述的带有底盘误差的机械臂路径规划方法,其特征在于,所述对新节点进行碰撞检测具体为:
根据已知的机械臂在构形空间中各个关节的旋转角度(θ1,θ2,...,θn),执行机械臂的运动学正解,求解机械臂末端在笛卡尔坐标系中的位置,检测机械臂末端和机械臂本体是否与工作工件中的障碍物发生碰撞。
7.根据权利要求6所述的带有底盘误差的机械臂路径规划方法,其特征在于,所述扩展并更新随机树具体为:
对于经过碰撞检测且满足扩展要求的新节点,将该新节点扩展到随机树中,并记录当前机械臂各关节的角度信息;初始化当前新节点的状态信息,根据随机点边界扩展机制更新与该新节点连接的父节点的状态信息。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010191628.XA CN111251306B (zh) | 2020-03-18 | 2020-03-18 | 一种带有底盘误差的机械臂路径规划方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202010191628.XA CN111251306B (zh) | 2020-03-18 | 2020-03-18 | 一种带有底盘误差的机械臂路径规划方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN111251306A CN111251306A (zh) | 2020-06-09 |
CN111251306B true CN111251306B (zh) | 2022-11-29 |
Family
ID=70946013
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202010191628.XA Active CN111251306B (zh) | 2020-03-18 | 2020-03-18 | 一种带有底盘误差的机械臂路径规划方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN111251306B (zh) |
Families Citing this family (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111761582B (zh) * | 2020-07-08 | 2021-05-18 | 浙江大学 | 一种基于随机采样的移动机械臂避障规划方法 |
CN111906765B (zh) * | 2020-07-31 | 2022-07-12 | 平安科技(深圳)有限公司 | 应用于路径规划的空间采样方法、装置、设备及介质 |
CN113119116B (zh) * | 2021-03-22 | 2023-01-31 | 深圳市优必选科技股份有限公司 | 一种机械臂运动规划方法、装置、可读存储介质及机械臂 |
CN113276109B (zh) * | 2021-04-21 | 2024-04-26 | 国网上海市电力公司 | 一种基于rrt算法的双机械臂解耦运动规划方法及系统 |
CN115674195B (zh) * | 2022-10-20 | 2024-06-04 | 四川大学 | 柔性机械臂在线臂形规划方法 |
Family Cites Families (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US8972057B1 (en) * | 2013-01-09 | 2015-03-03 | The Boeing Company | Systems and methods for generating a robotic path plan in a confined configuration space |
CN109571466B (zh) * | 2018-11-22 | 2021-01-26 | 浙江大学 | 一种基于快速随机搜索树的七自由度冗余机械臂动态避障路径规划方法 |
CN110497403A (zh) * | 2019-08-05 | 2019-11-26 | 上海大学 | 一种改进双向rrt算法的机械臂运动规划方法 |
CN110509279B (zh) * | 2019-09-06 | 2020-12-08 | 北京工业大学 | 一种仿人机械臂的运动路径规划方法及系统 |
-
2020
- 2020-03-18 CN CN202010191628.XA patent/CN111251306B/zh active Active
Also Published As
Publication number | Publication date |
---|---|
CN111251306A (zh) | 2020-06-09 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111251306B (zh) | 一种带有底盘误差的机械臂路径规划方法 | |
CN106569496B (zh) | 一种运动路径的规划方法 | |
CN111546347B (zh) | 一种适用于动态环境下的机械臂路径规划方法 | |
CN110509279B (zh) | 一种仿人机械臂的运动路径规划方法及系统 | |
CN107063280B (zh) | 一种基于控制采样的智能车辆路径规划系统及方法 | |
CN109542117B (zh) | 基于改进rrt的水下航行器滚动规划算法 | |
CN108638055B (zh) | 一种七自由度空间机械臂自主避障规划方法 | |
CN112223291B (zh) | 一种基于三维任务空间约束的机械臂避障方法及装置 | |
CN111761582B (zh) | 一种基于随机采样的移动机械臂避障规划方法 | |
CN109579854B (zh) | 基于快速扩展随机树的无人车避障方法 | |
CN114633258B (zh) | 一种隧道环境下机械臂运动轨迹的规划方法及相关装置 | |
CN112809665B (zh) | 一种基于改进rrt算法的机械臂运动规划方法 | |
CN112356033B (zh) | 一种融合低差异序列与rrt算法的机械臂路径规划方法 | |
CN113276109B (zh) | 一种基于rrt算法的双机械臂解耦运动规划方法及系统 | |
CN112549016A (zh) | 一种机械臂运动规划方法 | |
CN113771035B (zh) | 基于rrt*算法的冗余多自由度机械臂避障路径优化方法 | |
CN110561419A (zh) | 臂型线约束柔性机器人轨迹规划方法及装置 | |
CN115958590A (zh) | 一种基于rrt的机械臂深框避障运动规划方法及装置 | |
CN113650011B (zh) | 一种机械臂拼接路径规划方法及装置 | |
CN113442170B (zh) | 一种对机械臂路径冗余节点的反向拆分计算的方法及系统 | |
CN114428499A (zh) | 一种融合Astar与DWA算法的移动小车路径规划方法 | |
CN117773911B (zh) | 一种复杂环境下工业机器人避障方法 | |
CN116852367A (zh) | 一种基于改进RRTstar的六轴机械臂避障路径规划方法 | |
CN116652932A (zh) | 一种基于改进式rrt*算法的机械臂自主避障方法 | |
CN113084797B (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 | ||
GR01 | Patent grant |