本发明属于机械臂轨迹规划领域，具体地说是一种带有底盘误差的机械臂路径规划方法。
背景技术：
机械臂轨迹规划是指计算出一条合适的运动轨迹，实现机械臂避障并指导完成相应的作业任务，其主要评价指标有安全可靠、路径最短、时间最少等。在现场作业中，机械臂面临现场工作环境狭窄、空间结构复杂等问题。
机械臂轨迹规划方法众多，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)
其中，p表示0～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)
其中，p表示0～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所示，本发明可以在仿真环境下指导机械臂规划一条从初始位置运动到摆放工件的货架的指定位置的路径。
需要说明的是，以上仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换，但是凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
1.一种带有底盘误差的机械臂路径规划方法，包括以下步骤：
获取移动底盘在运动空间中实际位置与目标位置的误差值，并依据误差值，重构构型空间，并对起始位置和目标位置进行修正；
初始化起始点和目标点，创建以修正后的起始点为根节点的随机树，随机树包括父节点和子节点；
调用具备偏向概率采样的随机函数，在构形空间中产生一个随机点xrand；
遍历随机树的所有子节点，搜索与随机点距离最近的节点xnear；
生成新节点xnew，调用回归过滤机制，将不符合要求的节点删除，不扩展到随机树中，将符合要求的节点扩展到随机树中；
调用随机点边界扩展机制，更新边界节点的状态信息，确定边界节点的可扩展性；
确定机械臂在工作空间中的位姿，对新节点进行碰撞检测；
扩展并更新随机树；
检查是否满足终止条件，获取机械臂规划路径。
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为修正后在空间中的映射位置。
3.根据权利要求2所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述创建的随机树具体为：
随机树以起始点为根节点，以指定步长ε为父节点与子节点间的固定距离，父节点与子节点连线方向任意，父节点可扩展任意数量的子节点，随机树的所有节点都位于构形空间的自由空间中。
4.根据权利要求3所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述生成随机点的具备偏向概率采样的随机函数具体为：
p＝rand(1)
其中，p表示0～1之间生成的随机数，
5.根据权利要求4所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述生成的新节点具体为：
在随机点xrand和最近节点xnear连线方向上，以最近节点xnear为父节点，根据指定步长ε，生成新节点xnew。
6.根据权利要求5所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述回归过滤机制具体为：
对于新节点xnew，与该新节点xnew直接相连的节点为xnear，距离为步长ε，当新节点xnew与随机树中所有节点的最短距离小于步长ε时，即与新节点xnew距离最近的节点不为xnear，则当前xnew节点是回归的，定义为回归节点，将该回归节点直接过滤，不扩展到随机树中,当新节点与随机树中所有节点的最短距离等于或大于步长ε时，该新节点符合要求，将该新节点扩展到随机树中。
7.根据权利要求6所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述随机点边界扩展机制具体为：
边界节点是指其扩展的节点有一定概率与障碍物发生碰撞的节点；
为每一个新扩展的节点定义一个状态值δ，δ值随着随机树的扩展不断更迭，新扩展的节点状态值δ初始为0；
定义当前有m条边与边界节点连接，若扩展的子节点与障碍物边界碰撞，若边界节点的状态值为0则将δ值更新为1/2，否则将δ值增加1/(m 1)；若扩展的子节点与障碍物边界不发生碰撞，则该节点的状态值δ将减少1/(m 1)，若减少后的状态值δ小于0，则δ值更新为0；
定义边界节点在扩展过程中与边界发生碰撞的概率阈值为cp，如果状态值δ>cp，则该边界节点将不再扩展任何子节点；如果状态值δ≤cp，则将该节点扩展到随机树中。
8.根据权利要求7所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述对新节点进行碰撞检测具体为：
根据已知的机械臂在构形空间中各个关节的旋转角度(θ1,θ2,…,θn)，执行机械臂的运动学正解，求解机械臂末端在笛卡尔坐标系中的位置，检测机械臂末端和机械臂本体是否与工作工件中的障碍物发生碰撞。
9.根据权利要求8所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述扩展并更新随机树具体为：
对于满足碰撞检测的新随机节点，将该新随机节点扩展到随机树中，并记录当前机械臂各关节的角度信息；初始化当前新节点的状态信息，根据随机点边界扩展机制更新与该新节点连接的父节点的状态信息。
10.根据权利要求9所述的带有底盘误差的机械臂路径规划方法，其特征在于，所述终止条件具体为：
在满足约束要求的新节点扩展到随机树后，判断该新节点与目标点间的距离，若距离小于步长ε，且新节点与目标点的连线不穿过障碍物，则终止随机树的扩展过程；否则继续扩展随机树。
技术总结