一种基于构型空间采样算法的机械臂运动规划方法

本发明涉及机器人，尤其涉及一种基于构型空间采样算法的机械臂运动规划方法。

背景技术：

1、机械臂工作空间中，经常存在一些障碍物影响机械臂的正常运动。传统的示教方法在复杂场景下对机械臂进行路径规划效率低，因此需要引入智能路径规划算法来提高机械臂的路径规划效率。

2、尽管常见的快速拓展随机树算法在机械臂路径规划中得到广泛应用，但该算法存在随机性强和无效搜索的问题，导致规划效率很低。

技术实现思路

1、本发明所要解决的技术问题是克服现有技术的不足，提出一种基于构型空间采样算法的机械臂运动规划方法，能够解决传统快速扩展随机树算法规划效率低，路径代价大的技术问题。

2、为达到上述目的，本发明是采用下述技术方案实现的：

3、第一方面，本发明提供了一种基于构型空间采样算法的机械臂运动规划方法，包括以下步骤：

4、步骤1，根据机械臂构型与几何参数进行运动学建模，建立运动学模型；

5、步骤2，初始化构型空间，基于建立的运动学模型，求解机械臂起始状态与目标状态对应的关节角，作为机械臂在构型空间中的起始节点与目标节点；

6、步骤3，通过蒙特卡洛法将笛卡尔空间中机械臂与障碍物的碰撞关系映射到构型空间；

7、步骤4，基于构型空间和机械臂在构型空间中的起始节点与目标节点，通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点进入目标节点的膨胀范围，生成无碰撞路径。

8、进一步的，所述步骤1中，根据机械臂构型与几何参数进行运动学建模，建立运动学模型，包括：

9、建立机械臂dh运动学模型，将相邻杆件之间的位姿变换关系分解成两次旋转与两次平移运动，然后连乘得到机械臂末端相对于基坐标系的变换矩阵，包括：

10、

11、

12、

13、

14、

15、

16、

17、其中，关节与连杆参数ai、αi、θi、di为dh参数。各个参数的定义与含义如下：

18、αi-1：将zi-1轴围绕xi-1轴旋转一定角度使得zi-1与zi轴平行，该角度定义为连杆扭转角αi；

19、ai-1：将zi-1轴沿xi-1轴移动一定距离使得zi-1与zi轴重合，该距离定义为连杆长度ai-1；

20、θi：将xi-1轴围绕zi旋转一定角度使得xi-1与xi轴平行，该角度定义为关节角θi；

21、di：将xi-1轴沿着zi移动一定距离使得xi-1与xi轴重合，该距离定义为连杆偏距di。

22、s、c是sin、cos的缩写。

23、rot(x,α)：绕x轴旋转α角；

24、trans(a,0,0)：沿x轴平移a。

25、其中，表示相邻杆件之间的位姿变换矩阵；

26、分别表示从基座到末端相邻杆件之间的位姿描述；

27、表示机械臂末端相对于底座基坐标系的描述；

28、r11、r12、r13、r21、r22、r23、r31、r32、r33、px、py、pz：分别对应于上述六个变换矩阵相乘后的结果，用于描述末端相对于基坐标系的位置与姿态。

29、进一步的，所述步骤2中，初始化构型空间，基于建立的运动学模型，求解机械臂起始状态与目标状态对应的关节角，作为机械臂在构型空间中的起始节点与目标节点，包括：

30、确定机械臂在笛卡尔空间中的起始位姿与目标位姿；

31、将机械臂起始位姿与目标位姿通过所建立的运动学模型逆解到构型空间，求得对应的起始关节角与目标关节角，作为构型空间中的起始节点与目标节点，构型空间中的每一个节点坐标对应机械臂六个关节角数值。

32、进一步的，所述步骤3中，通过蒙特卡洛法将笛卡尔空间中机械臂与障碍物的碰撞关系映射到构型空间，包括：

33、确定障碍物在笛卡尔空间中的位置，在笛卡尔空间通过蒙特卡洛法随机采集大量关节角样本并计算机械臂与障碍物的碰撞信息，然后将碰撞信息映射到机械臂构型空间。

34、进一步的，所述步骤4中，基于构型空间和机械臂在构型空间中的起始节点与目标节点，通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点进入目标节点的膨胀范围，生成无碰撞路径，包括：

35、步骤a：从起始节点qstart开始在构型空间中随机采样获得随机节点qrand，从距离随机节点最近的树节点qnearest出发向qrand方向经过一步长λ得到第一个新节点qnew1，判断当前路径是否发生碰撞，若未发生碰撞将该节点保存至节点树中；若发生碰撞，舍弃qnew1，重新采样；

36、步骤b：判断新节点qnew1是否进入目标节点qgoal的膨胀范围，进入膨胀范围后，直连qnew1与目标节点，完成规划；若未进入，以qnew1为圆心，r为检索半径对新节点进行父节点调整与区域重连，完成局部优化；

37、步骤c：计算新节点qnew1处的势场合力，沿着势场合力方向前进λ生成另一个新节点qnew2；

38、步骤d：判断另一个新节点qnew2是否进入目标节点qgoal的膨胀范围，若进入膨胀范围后，直连qnew2与目标节点，完成规划；若未进入，以qnew2为圆心，r为半径对新节点进行父节点调整与区域重连，完成局部优化；

39、步骤e：循环步骤b-d，直到生成的新节点进入目标节点qgoal的膨胀范围，输出一系列从起始节点到目标节点的节点序列，得到一条从qstart到qgoal的路径点，完成规划。

40、进一步的，所述随机节点qrand通过如下公式获得：

41、

42、

43、其中，p0表示初始偏置率，0＜p0＜1，pmin表示最小偏置率，0＜pmin＜1，且p0＞pmin，iter、itermax表示当前迭代次数与设置的最大迭代次数。

44、进一步的，所述采样步长λ通过如下公式获取：

45、

46、其中，iter为当前迭代次数；dmin为目标膨胀范围。

47、进一步的，所述目标节点qgoal的膨胀范围的确定方法包括：

48、通过计算目标节点至第i个障碍物之间的欧式距离di，减去第i个障碍物半径ri，取(di-ri)中最小值而得到膨胀范围，当有节点到达目标节点qgoal的膨胀范围内后，直连当前节点与目标节点。

49、进一步的，所述检索半径r通过如下公式进行计算：

50、

51、其中，nnode表示当前节点树中的节点数量。

52、进一步的，计算新节点qnew1处的势场合力，包括：

53、

54、

55、

56、其中，fnew1表示势场合力，表示势场引力，表示势场斥力；

57、fatt(q)表示q节点处的势场引力，frep(q)表示q节点处的势场引力，▽表示求梯度，ξ表示引力尺度因子，ρ(q,qgoal)表示当前节点与目标节点之间的距离，η为斥力尺度因子，ρ(q,qobs)表示物体和障碍物之间的距离，ρ0表示障碍物的影响范围。

58、与现有技术相比，本发明所达到的有益效果：

59、1、本发明通过采样策略进行节点拓展，并对拓展的新节点进行父节点重选与区域重连，循环执行节点拓展，直到拓展的新节点进入目标节点的膨胀范围，生成无碰撞路径，从而最小化不必要的随机搜索和提高规划效率。与传统方法相比，本方法能够更快速地找到可行路径，从而在复杂场景下提高机械臂的运动效率。这一技术的引入将大幅提高机械臂在各种应用中的适用性，为自动化工作流程和机器人应用提供更高的效率和灵活性。

60、2、利用高度智能化的路径规划算法，旨在最小化不必要的随机搜索和提高规划效率。与传统方法相比，该算法能够更快速地找到可行路径，从而在复杂场景下提高机械臂的运动效率。这一技术的引入将大幅提高机械臂在各种应用中的适用性，为自动化工作流程和机器人应用提供更高的效率和灵活性。

61、3、本发明提出了一种创新的路径规划技术，旨在在机械臂工作空间中更有效地应对障碍物。本方法在传统快速扩展随机树算法的基础上对节点扩展方式进行了改进与完善，具体的方法是结合人工势场法提出二次采样的扩展策略；

62、4、本方法对目标节点进行了膨胀处理，为完成路径搜索提供了更快速的判断；

63、5、本方法利用动态目标偏置、自适应步长以及动态检索为算法提供自适应搜索的特性。

64、6、本方法采用融合人工势场与快速扩展随机树算法，通过构建引力势场、进行二次采样、膨胀目标节点的策略引导节点扩展，生成无碰撞路径，能够解决传统快速扩展随机树算法规划效率低，路径代价大的技术问题。