本发明涉及机器人控制，具体地说，是涉及一种核设施退役机器人避障轨迹规划方法。
背景技术：
1、核废物由于具有放射水平高、半衰周期长、生物毒性大等特点，其处理处置已成为世界性关注的问题，目前，业内对高放核废物治理的工艺路线采用的是“玻璃固化－地质处置”，高放核废料储运容器是第一道安全屏障。高放核废料储运容器的制造，可以填补业内空白，对业内核废物后处理工程的发展具有重要意义。
2、当前在核设施退役领域，通常采用人工远程遥控的方式进行机器人运动控制。然而，由于核设施退役场景的环境结构复杂，操作人员仅依赖摄像头传回的图像难以准确感知周围障碍物的距离，特别是在狭小的作业空间中，这显著提高了操作难度、降低了作业效率，并且容易导致操作失误对机器人造成损害。
技术实现思路
1、本发明的目的在于提供一种核设施退役机器人避障轨迹规划方法，旨在提升核设施退役作业的安全性和效率。
2、为实现上述目的，本发明采用的技术方案如下：
3、一种核设施退役机器人避障轨迹规划方法，包括以下步骤：
4、s1，通过固态激光雷达和双目相机采集环境数据，基于三维建图与定位算法，建立三维点云地图；
5、s2，通过机器人前部双目相机基于sgbm算法，确定障碍物到机器人当前质心的距离；
6、s3，以机器人的后轴中心为原点，建立以机器人运动方向为纵向并作为x轴横向为y轴的自身笛卡尔坐标系，基于自身笛卡尔坐标系顺时针方向求机器人的四个顶点坐标，并转换到全局笛卡尔坐标系下，提取所有点云的二维平面坐标，以两条线性方程为边界，筛选出在边界内的障碍物点云；
7、s4，计算所有障碍物点云到机器人的纵向距离，根据预设障碍物点云采样个数，保留纵向距离最近的预设采样个数的障碍物点云，作为此时的障碍物有效边界点云集合；然后基于三次样条曲线对有效边界点云进行拟合，生成当前时刻的障碍物有效边界曲线；
8、s5，基于lattice planner轨迹规划算法，通过撒点采样和五次多项式轨迹拟合，生成机器人局部候选轨迹；
9、s6，通过判断每条候选轨迹与当前障碍物有效边界曲线的关系，若存在相交则删除，反之，保留；
10、s7，基于代价最小原则，比较所有与当前障碍物有效边界曲线不相交的候选轨迹，确定局部最优运动轨迹；
11、s8，将运动轨迹的路径点位姿信息发布到控制层，将控制信号传输到伺服电机驱动器，控制液压油泵工作，进而使液压马达工作；
12、s9，在机器人进行避障的任意时刻，循环执行步骤s3-s8，动态求解出每一时刻的障碍物有效边界，从而规划出当前时刻的最佳运动轨迹，直到机器人绕过障碍物。
13、进一步地，在所述步骤s2中，障碍物到机器人当前质心的距离的计算公式如下：
14、dis(x)＝argmin(c(x,d))
15、＝d(x,d)+p1*min(c(x-1,d),c(x,d-1))+p2*min(c(x-1,d-1),c(x-1,d+1))
16、
17、distance(x)＝baseline*f/dis(x)
18、式中，dis(x)表示目标点x的视差值，argmin表示取使得c(x,d)最小的d值，c(x,d)表示目标点(x,d)表的代价，d(x,d)表示目标点(x,d)的视差值，p1和p2是平滑项的权重；distance(x)表示目标点x的距离，baseline表示双目相机的基线长度，f表示双目相机的焦距。
19、进一步地，在所述步骤s3中，由自身笛卡尔坐标系转换到全局笛卡尔坐标系下的变换关系为：
20、
21、式中，表示机器人在全局笛卡尔坐标下的位置，表示自身笛卡尔坐标系下机器人的顶点坐标，其中i＝1,2,3,4；θ表示机器人航向角，表示全局笛卡尔坐标系下对应的机器人的顶点坐标；其中，在全局笛卡尔坐标系下，分别将顶点以及顶点进行线性拟合，得到两条直线方程：
22、
23、r表示任意实数，j表示线性方程编号j＝1,2。
24、进一步地，在所述步骤s4中，生成得到的当前时刻的障碍物有效边界曲线为：
25、sτi(t)＝ai+bi(t-ti)+ci(t-ti)2+di(t-ti)3
26、式中，ai,bi,ci,di表示第i段曲线的样条系数，sτi(t)表示τ方向上t时刻对应的第i段曲线，τ表示x方向或y方向。
27、进一步地，在所述步骤s5中，生成得到的机器人局部候选轨迹为：
28、
29、式中：a0,a1,a2,a3,a4,,a5表示五次多项式系数，t为时间参数。
30、与现有技术相比，本发明具有以下有益效果：
31、(1)本发明提高了核退役机器人智能化水平，解决了目前核退役机器人在避障过程中需依赖人工通过监控视频远程操作时，机器人与周围环境的距离难以确定问题，不仅减轻了人工操纵的负担，也提高了避障的效率与准确性。
32、(2)本发明提出了障碍物边界动态生成方法，改进了传统避障方法中对障碍物边界描述不准确(往往大于实际障碍物边界)的问题，能够更准确描述障碍物边界，提升了避障性能，能够更好的适应核退役的复杂、狭小作业环境。
1.一种核设施退役机器人避障轨迹规划方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种核设施退役机器人避障轨迹规划方法，其特征在于，在所述步骤s2中，障碍物到机器人当前质心的距离的计算公式如下：
3.根据权利要求2所述的一种核设施退役机器人避障轨迹规划方法，其特征在于，在所述步骤s3中，由自身笛卡尔坐标系转换到全局笛卡尔坐标系下的变换关系为：
4.根据权利要求3所述的一种核设施退役机器人避障轨迹规划方法，其特征在于，在所述步骤s4中，生成得到的当前时刻的障碍物有效边界曲线为：
5.根据权利要求4所述的一种核设施退役机器人避障轨迹规划方法，其特征在于，在所述步骤s5中，生成得到的机器人局部候选轨迹为：
