3D避障路径规划方法与流程

3d避障路径规划方法
技术领域
1.本发明涉及路径规划技术领域，具体涉及3d避障路径规划方法。


背景技术：

2.当下工厂自动化程度不断加深，工业自动化在工厂中的占有比例越来越大，工业机器人的使用量逐年增加，但是传统机器人应对不同的应用场景的灵活性不够，更换不同的场景都需要重新进行机器人的示教工作，这个过程重复性比较大，耗费了大量的时间。
3.同时，现有示教标定对机器人操作人员的要求较高，需要考虑障碍物信息以及要对机器人的位姿有着充分的了解，才能调试出比较满意的运动轨迹，技术要求高。


技术实现要素：

4.有鉴于此，本发明要解决的问题是当前传统机器人灵活性不够，基于上述问题提出了3d避障路径规划方法，使用此方法只需要示教出工艺中的起始点、目标点、障碍物在机器人坐标系中的位置以及障碍物的尺寸即可，就能协助机器人完成路径规划，该方法可以很大程度上节省时间成本，相比于传统方法示教的坐标点要少很多，同时还可以避免因人为失误未考虑到机器人与障碍物发生碰撞的情况。
5.为解决上述技术问题，本发明采用的技术方案是：3d避障路径规划方法，包括如下步骤：s1：输入起始点、目标点以及障碍物信息；s2：随机采样，依据采样修正策略得到各采样点信息；s3：通过各采样点进行轨迹规划生成轨迹点，得到目标路径；s4：根据轨迹点采取路径优化策略得到优化路径。
6.所述路径优化策略具体包括如下步骤：s41：构建以起始点和目标点为焦点、目标路径的长度一半作为长半轴的椭圆参数方程；s42：在椭圆内重复采样，返回至步骤s2，比较判断是否得到更短的目标路径，是则执行步骤s41，更新实际父节点、轨迹点目标集并得到更新后的目标路径作为优化路径，直至采样周期结束；否则将s3中的目标路径作为优化路径输出。
7.在本发明中，优选地，所述采样修正策略具体包括如下步骤：s21：获得起始点和各采样点的坐标信息，设置步进长度，将起始点作为当前采样点；s22：计算当前采样点与下一采样点的欧几里得距离，将欧几里得距离与步进长度进行比较；s23：判断欧几里得距离是否大于步进长度，若是则对采样点进行坐标变换，得到下一采样点的更新坐标信息；否则，保留下一采样点的坐标信息；s24：将下一采样点作为当前采样点，返回步骤s22，直至下一采样点与目标点满足
近似判定条件结束指令。
8.在本发明中，优选地，所述近似判定条件为下一采样点与目标点的欧几里得距离小于设定的目标点容错值。
9.在本发明中，优选地，所述轨迹规划具体包括如下步骤：s31：设定路径优化参数，以当前采样点为圆心，以路径优化参数为半径作圆，记录圆内所有采样点的编号及坐标信息；s32：将圆内所有采样点记为当前采样点的临时父节点；s33：分别获取临时父节点与起始点之间的若干组轨迹点集，得到每组轨迹点集的编号，计算每组轨迹点集的欧几里得距离之和；s34：比较得到欧几里得距离之和最小的轨迹点集，将该轨迹点集所对应的临时父节点记为实际父节点，轨迹点集记为轨迹点目标集，根据轨迹点目标集得到目标路径。
10.在本发明中，优选地，所述目标路径通过轨迹点目标集按起始点到当前采样点方向的顺序记录生成。
11.在本发明中，优选地，得到优化路径之后执行步骤s5：对优化路径依照运动学优化算法以消除轨迹点凸点。
12.在本发明中，优选地，所述运动学优化算法具体为采用埃尔米特算法对目标路径中要进行插值的坐标点进行扩展，扩展后对插值点进行插值计算，令加速度连续可导以使得目标路径形成平滑曲线。
13.在本发明中，优选地，所述障碍物信息通过机器人获取，所述障碍物信息包括障碍物尺寸以及在机器人坐标系中的位置，通过障碍物尺寸以及在机器人坐标系中的位置得到障碍物区域，结合采样点信息进行碰撞检测，使得所述目标路径绕过所述障碍物区域。
14.在本发明中，优选地，随机采样为在机器人运动空间内进行采样。
15.本发明具有的优点和积极效果是：本发明在输入原始信息之后通过在机器人运动范围内进行随机采样，并依据采样修正策略得到各采样点信息，采样修正策略设定有步进长度，将采样点与步进长度进行比较，即使采样点不在机器人运动范围内也能对采样点进行调整，将其转变为以步进长度为半径的圆内，从而有效防止采样点偏离机器人运行空间的距离过大；通过轨迹规划以及路径优化策略不断循环计算得到最终优化路径，在设定范围内得到目标路径的路径总长度较短；通过对优化路径依照运动学优化算法以消除轨迹点凸点，目标路径更加平滑，从而实现机器人运动过程更加平滑柔顺。
附图说明
16.附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。在附图中：图1是本发明的3d避障路径规划方法的整体流程示意图；图2是本发明的3d避障路径规划方法的采样修正策略的流程示意图；图3是本发明的3d避障路径规划方法的轨迹规划的流程示意图；图4是本发明的3d避障路径规划方法的路径优化策略的流程示意图；图5是本发明的3d避障路径规划方法的轨迹规划的原理示意图；图6是本发明的3d避障路径规划方法的路径优化策略的轨迹示意图；
图7是本发明的3d避障路径规划方法的路径优化策略的椭圆参数方程示意图；图8是本发明的3d避障路径规划方法的埃尔米特算法的对插值点进行扩展的示意图；图9是本发明的3d避障路径规划方法的埃尔米特算法的扩展前后路径的示意图。
具体实施方式
17.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
18.需要说明的是，当组件被称为“固定于”另一个组件，它可以直接在另一个组件上或者也可以存在居中的组件。当一个组件被认为是“连接”另一个组件，它可以是直接连接到另一个组件或者可能同时存在居中组件。当一个组件被认为是“设置于”另一个组件，它可以是直接设置在另一个组件上或者可能同时存在居中组件。本文所使用的术语“垂直的”、
ꢀ“ꢀ
水平的”、
“ꢀ
左”、
“ꢀ
右”以及类似的表述只是为了说明的目的。
19.除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。本文所使用的术语“及／或”包括一个或多个相关的所列项目的任意的和所有的组合。
20.如图1所示，本发明提供3d避障路径规划方法，包括如下步骤：s1：输入起始点、目标点以及障碍物信息；s2：随机采样，依据采样修正策略得到各采样点信息；s3：通过各采样点进行轨迹规划生成轨迹点，得到目标路径；s4：根据轨迹点采取路径优化策略得到优化路径。
21.如图4、图6和图7所示，路径优化策略具体包括如下步骤：s41：构建以起始点和目标点为焦点、目标路径的长度一半作为长半轴的椭圆参数方程；s42：在椭圆内重复采样，返回至步骤s2，通过步骤s3得到新的目标路径，将新的目标路径与上一目标路径进行比较判断，若得到更短的目标路径，则执行步骤s41，直至采样周期结束，并更新实际父节点、轨迹点目标集并得到更新后的目标路径作为优化路径，通过对规划出来的路径不断进行优化取值，使得在采样周期内能规划的路径为较优化的路径。由于仅仅知道椭球体的两个焦点距离不能唯一确定椭球体，第一次采样是在机器人运动空间进行采样，我们得出从起始点到目标点的一条轨迹，计算出该轨迹的欧几里得距离，再结合焦点，即可确定出椭球体的参数方程。通过不断迭代，每找到一条更短的路径，则用该路径的长度作为新的c
best
，c
best
表示椭圆的长轴，计算得到的椭圆面积会变小，由于采样点均选取于椭圆上或者椭圆内部，这样就能够缩小采样点的采样范围，从而提高采样的效率。如果根据椭圆内重复采样得到的目标路径长度大于或者等于上一目标路径的长度，则直接输出上一目标路径的实际父节点、轨迹点目标集及该目标路径，将其作为优化路径。其中，图6中示意的四边形图形均表示的是障碍物区域，障碍物信息通过机器人采集，根据障碍物信
息结合采样点信息进行碰撞检测，能协助机器人完成路径动态规划，该方法可以很大程度上节省时间成本，相比于传统方法示教的坐标点要少很多，同时还可以避免因人为失误未考虑到机器人与障碍物发生碰撞的情况。
22.如图2所示，在本实施例中，进一步地，采样修正策略具体包括如下步骤：s21：获得起始点和各采样点的坐标信息，设置步进长度，将起始点作为当前采样点；理论上步进长度可以设定为任意值，但实际上需要根据实际情况进行调整，设定合适的步进长度，步进长度设定值大，得到的轨迹的时间较短，但距离最优轨迹可能存在较大偏差。
23.s22：计算当前采样点与下一采样点的欧几里得距离，将欧几里得距离与步进长度进行比较；欧几里得距离是标量值，是两点的自然长度；s23：判断欧几里得距离是否大于步进长度，若是则对采样点进行坐标变换，得到下一采样点的更新坐标信息；否则，保留下一采样点的坐标信息；s24：将下一采样点作为当前采样点，返回步骤s22，直至下一采样点与目标点满足近似判定条件结束指令。
24.在本实施例中，通过采样得到第一个采样点a的坐标为（100，100），第二个采样点b的坐标为（1.1，1.1），我们令起始点坐标为o（0，0），设置步进长度为1，计算起始点o与采样点a之间的欧几里得距离为，由于大于1，则需要对第一采样点的坐标进行修正更新，以起始点为圆心，以步进长度为半径画圆，该圆与线段oa之间的交点作为采样点a的更新点a’，得到a’坐标，继续判断是否需要修正采样点b的坐标，此时使得a’为当前采样点，计算a’b的欧几里得距离为，由于0.618小于1，则说明无需对采样点b进行修正，令采样点b（1.1，1.1）为当前采样点继续验证下一采样点是否需要修正，直至判断得出下一采样点为目标点的近似点时则停止。
25.在本实施例中，进一步地，近似判定条件为下一采样点与目标点的欧几里得距离小于设定的目标点容错值。
26.如图3和图5所示，在本实施例中，进一步地，轨迹规划具体包括如下步骤：s31：设定路径优化参数，以当前采样点为圆心，以路径优化参数为半径作圆，记录圆内所有采样点的编号及坐标信息；s32：将圆内所有采样点记为当前采样点的临时父节点；s33：分别获取临时父节点与起始点之间的若干组轨迹点集，得到每组轨迹点集的编号，计算每组轨迹点集的欧几里得距离之和；s34：比较得到欧几里得距离之和最小的轨迹点集，将该轨迹点集所对应的临时父节点记为实际父节点，轨迹点集记为轨迹点目标集，根据轨迹点目标集得到目标路径。
27.在本实施例中，进一步地，目标路径通过轨迹点目标集按起始点到当前采样点方向的顺序记录生成。
28.具体结合图5对轨迹规划优化的工作过程进行解释说明，图中各个圆圈表示采样得到的点坐标，圆圈内的数字表示采样点的编号顺序，0表示起始点坐标，1为第一个采样点，以此类推。第九个采样点为最新的采样点，从左侧图可以看出，距离第九个采样点最近
的点为第六个采样点，二者的欧几里得距离为1，因而我们将点第六个采样点作为第九个采样点的最接近节点，以第九个采样点为圆心，设定路径优化参数，画出以第九个采样点为圆心、以路径优化参数为半径的圆，通过设定路径优化参数是为了圈定一个包含所有第九个采样点的临时父节点范围，从而便于找到第九个采样点的实际父节点，设定值越小，随着目标路径动态规划进程的不断推进更新父节点的可能性越小，在本实施例中包含在该圆内的所有采样点都为第九个采样点的临时父节点，也就是说，第四个采样点、第五个采样点、第六个采样点、第八个采样点都为第九个采样点的临时父节点，第九个采样点与这几个点的欧几里得距离分别为4、3、1、3，得到每组轨迹点集编号以及其分别对应的0
‑4‑
9、0
‑1‑5‑
9、0
‑4‑6‑
9、0
‑1‑5‑8‑
9四条轨迹，计算每组轨迹点集的的欧几里得距离之和分别为14、11、16、12，通过比较得到11的欧几里得距离之和最小，也就是说，第五个采样点为第九个采样点的实际父节点，包含“0、1、5、9”在内的轨迹点集为轨迹点目标集，将轨迹点目标集按起始点到当前第九个采样点方向顺序生成的“0
‑1‑5‑
9”这条轨迹为目标路径，后面随着当前采样点的持续更新，其实际父节点均通过该方法进行计算求解。在本实施例中，之所以不直接选取0
‑
9（两点之间线段最短），是因为要考虑障碍物和机器人位姿的问题，如果不考虑障碍物和机器人位姿问题，直接选择起点和终点的连线路径是最短的，这样就没必要使用算法，由于只有序号4、5、6、8这几个点在序号9为圆心的圆内，因此实际父节点只能从上述四个点中选取，之所以不选择0
‑1‑2‑3‑5‑
9等图5中未示出的路径，是因为在采样点序号9之前的采样点在执行该步骤时已经将以上未示出的路径舍掉了，随着路径规划的不断推进，距离较长的路径段被舍掉，因而在本实施例中的序号为5的点与序号为3的点之间没有连接。
29.如图8和图9所示，在本实施例中，进一步地，得到优化路径之后执行步骤s5：对优化路径依照运动学优化算法以消除轨迹点凸点，运动学优化算法具体为采用埃尔米特算法对目标路径中要进行插值的坐标点进行扩展，可根据路径的形态与实际情况选取不同的扩展方式，如循环扩展、镜像扩展等，闭合式的路径适用于循环扩展，分段式的路径适用于镜像扩展，图8中示出的为镜像扩展，其中a、b、c、d、e为原始点位，a’、b’、c’、d’、e’以及a
’’
、b
’’
、c
’’
、d
’’
、e
’’
为扩展的点位，将点位扩展完成之后，输入优化路径所涵盖的所有采样点的坐标、x、y方向上的速度、两点的差值时间、扩展模式以及扩展点数目，再对插值点进行插值计算使得目标路径形成平滑曲线。将采样计算出来的轨迹点进行运动学规划，运动学规划就是将采样进行加减速的规划，采用埃尔米特算法进行规划，对起始点之前进行扩展，对目标点之后进行扩展，使得原始点相对于扩展后的部分处于中间部分，参见图9示出的是循环扩展，可以明显看出扩展后的路径相较于扩展前的路径更加平滑柔顺，改进后的方法能够避免传统方法带来的启停端的轨迹凸点问题。
30.在本实施例中，进一步地，障碍物信息通过机器人获取，障碍物信息包括障碍物尺寸以及在机器人坐标系中的位置，通过障碍物尺寸以及在机器人坐标系中的位置得到障碍物区域，结合采样点信息进行碰撞检测，如果得到的采样点与该采样点的临时父节点之间的轨迹与障碍物区域存在交点，则需要舍弃该采样点，重新采样并进行碰撞计算，最终使得目标路径绕过障碍物区域。
31.在本实施例中，进一步地，随机采样为在机器人运动空间内进行采样。
32.本发明的工作原理和工作过程如下：首先，输入包括起始点、目标点以及障碍物信息在内的原始数据，在运动范围内进行随机采样，这里的运行范围指的是机器人的运动空
间范围，因为有设置步进长度，所以即使第一个采样点不在运动范围内也会进行处理，将其转变到以步进长度为半径的圆内，记录当前采样点，分别计算出当前采样点与已有的节点的距离，进而找到与该当前采样点最近的节点，计算出当前采样点与最近的节点的欧几里得距离，如果欧几里得距离大于设定的步进距离，则需要更新一下当前采样点的坐标值，使两个点的欧几里得距离小于等于步进长度，以新生成的节点为圆心，在设定半径范围内找出临时父节点，按照此方法进行循环计算，直到在设定范围内计算出轨迹。
33.本发明的采样的范围是以起始点与目标点之间的欧几里得距离作为椭球体的两个焦点的距离，在椭球体内进行采样，对于六轴机器人而言要在六维空间对机器人轨迹进行采样，此时采样需要在超椭球体内进行采样。超椭球体内采样是针对高纬度空间的采样而言的，如果只是二维空间采样，则在椭圆内采样即可；用意就是可以控制采样范围，提高算法的效率。
34.图6显示了采样范围的变化过程，以二维范围采样为例，以起始点和目标点为椭圆的左右焦点，将采样得到的第一条路径的长度的一半作为椭圆长半轴的长度，则根据长半轴的长度以及焦点之间距离的一半，即可得到椭圆的短半轴的距离，进而得到椭圆方程的所有参数，如图7所示。
35.该实施环境中包括终端设备、控制器、控制柜以及机器人，其中控制器具体形式为服务端设备，上位机软件可以配置在控制器中也可以在其他设备中,本发明实施例对此不进行具体的限定，终端设备的类型包括但不限于台式电脑、笔记本电脑或者示教器。本发明实施例采用的ethercat通信方式实现控制器与驱动器之间的通信。本发明的避障路径自动规划方法是在驱动器中的一个模块，所以所需设备与机器人控制器相同。
36.得到最终目标路径之后，将运动学规划出来的坐标值转换为机器人关节角度，并根据机器人的机械参数，转换为机器人中电机的脉冲值发送给电机，通过控制机器人沿目标路径运动。
37.以上对本发明的实施例进行了详细说明，但所述内容仅为本发明的较佳实施例，不能被认为用于限定本发明的实施范围。凡依本发明范围所作的均等变化与改进等，均应仍归属于本专利涵盖范围之内。