本发明属于机器人路径规划领域，特别涉及一种基于随机自适应变步长rrt*的移动机器人路径规划方法。
背景技术：
1、随着科技行业的快速发展，机器人行业也日新月异。机器人不仅为人类带来极大便利,提高工作效率，也成为制造业转型升级的重要支撑。随着应用场景的扩大，机器人工作环境日益复杂。让机器人具备自主感知与路径规划能力，根据工作环境中的障碍物自主规划最优路径，安全高效完成各种任务目标是一项关键技术问题，这不仅能提升工作效率，也能更好保障作业安全，推动机器人在复杂环境下的应用。
2、目前主流的移动机器人路径规划算法主要包括人工市场法、遗传算法、粒子群算法及蚁群算法。这些算法可以在二维和三维空间中规划出无障碍物碰撞的平滑路径。然而，多关节移动机器人在移动时并不能单纯视为质点，除了考虑末端位置的路径规划还需要注意机器人各连杆在空间中的位置情况。因此，多关节移动机器人的路径规划是一个多维空间路径搜索问题。rapidly-exploring random trees(rrt)是由s.m.lavalle和j.j.kuffner提出的一种快速搜索算法，主要用来解决高维非线性系统的路径设计问题。它是一种在空间中随机采样生成搜索树的策略。由于采样点随机生成，搜索树是在各方向均匀生长的，这导致它在寻找路径时没有方向性，收敛速度较慢。rrt*算法是rrt算法的一种改进形式，通过引入代价函数，在生成新采样点时选择代价最小的路径点进行重新优化连接，使路径收敛到最优解。
3、在多维空间中使用rrt*算法搜索可行路径时，由于缺少目标指引，导致算法搜索效率较低。此外，多关节移动机器人各关节在多维空间中的平均生长步长很可能会导致机器人末端轨迹在三维空间中并不平滑。因此，设计一种高效且路径平滑的避障路径规划策略具有重要意义。
技术实现思路
1、为解决上述技术问题，本发明提出一种基于随机自适应变步长rrt*的移动机器人路径规划方法，该方法可以使多关节移动机器人在躲避障碍的的同时规划出一条平滑路径到达指定位置，并且在无障碍物碰撞时可以直接、迅速、平滑的抵达指定位置。
2、本发明采用的技术方案为：一种基于随机自适应变步长rrt*的移动机器人路径规划方法，包括：
3、s1、为rrt*算法初始化起始点θstart、目标点θgoal和固定步长ε。对多关节机器人建立相应的d-h参数表，计算机器人正运动学方程，计算结果如下：
4、
5、其中sθi＝sinθi，cθi＝cosθi，sαi-1＝sinαi-1，cαi-1＝cosαi-1，αi-1、ai-1、θi和di表示连杆i-1和连杆i之间的变换关系系数；rx(αi-1)表示绕x轴旋转αi-1角度，dx(ai-1)表示沿x轴移动ai-1，其余同理；tii-1表示连杆i-1到连杆i的齐次变换矩阵。因此末端位姿的变换矩阵可以表示为：
6、
7、其中，表示世界坐标系与连杆一之间的变换矩阵，t12表示连杆一与连杆二之间的变换矩阵，等同理；px,py,pz为机器人末端三维坐标；(nx,ny,nz)、(ox,oy,oz)和(ax,ay,az)表示末端坐标系在世界坐标系下的单位向量；
8、s2、在多维空间中采样生成新节点，在算法中加入目标点信息，得到改进节点生成公式：
9、
10、其中ε为固定步长，||θgoal-θnear||(||θrand-θnear||)代表多维关节空间中两组关节间距离，θnew是生成的新采样点关节向量，θnear是已产生节点中离θnew最近的节点，θgoal是目标节点关节向量，p是0到1之间的随机数，α表示随机点的选择概率(初始时α＝0，当新节点与障碍物碰撞时令α＝0.9)；
11、s3、引入带权值的曼哈顿距离来衡量多维关节空间中两组关节间距离，其表达式为：
12、
13、其中，θ1和θ2是多维关节空间中的两组关节向量，n是关节空间维度，λi为各维度的距离计算权重，且同时将权值系数引入关节向量差值中，其表达式为：
14、θ1-θ2＝[λ1(θ11-θ21)…λi(θ1i-θ2i)…λn(θ1n-θ2n)] (5)
15、步骤s3所述λi的计算过程为：
16、s31、在改进rrt*算法采样搜索时，λi在平均权值与自适应权值中随机选择，其表达式为：
17、
18、其中，n为机器人关节数，p是0到1之间的随机数，β表示自适应生长概率(初始时β＝1，当新节点与障碍物碰撞时令β＝0.4)；
19、s32、a1中的λadp根据采样点父节点与目标点之间的关节差距自适应计算各关节的距离计算权值，其表达式为：
20、
21、s33、为避免机器人起始姿态与目标姿态有部分关节角度相同导致关节锁死，设置权值最小值改进自适应权值计算，其表达式为：
22、
23、其中，σ为最小权值，当a2计算出的λi小于σ时，令其等于σ，并在所有小于σ的权重都重新赋值后，再对剩下的权重按比例分配，使得
24、s4、将s3中计算的带权值曼哈顿距离代入s2中得到新关节向量节点θnew，并将θnew代入s1中计算的正运动学方程，得到机器人各连杆和末端位姿在三维空间中的信息状态，若该节点与障碍物无碰撞，按照起始点到新节点路径最短原则在已产生的节点中选择新节点的父节点，并将该节点加入搜索树；若该节点与障碍物碰撞，删除该节点并令α＝0.9，β＝0.4。
25、s5、重复步骤s2至s4，直到最新产生的节点进入目标点阈值范围，连接新节点与目标点，至此算法结束并得到一条多关节移动机器人避障平滑运动路径。
26、本发明的有益效果：采用传统rrt*算法探索可行路径时会向许多无效空间进行探索，降低了算法效率，并且由于在机器人关节空间中不同关节的运动范围并不相同，平均分配权值的方法会导致关节在运动时出现振动；本发明在rrt*算法中加入目标点信息作为引导，提高了路径探索效率，并根据不同采样点到目标点的距离不同分配相应的自适应距离计算权值λi，避免算法陷入局部最优解的同时保证关节运动的平滑性，同时在路径无碰撞情况时快速规划平滑避障路径，减少了随机搜索算法在无障碍环境下的冗余计算量。
1.一种带目标引导的随机自适应变步长rrt*避障路径规划策略，包括如下步骤：
