1.用于粮仓机器人平粮作业的路径规划及平粮作业控制方法,其特征在于包括以下步骤:步骤1:通过即时定位与地图构建slam技术构建粮仓的地图;
步骤2:通过激光雷达识别高度高于设定阈值的粮堆的位置坐标、几何形状和高度,将粮堆底面近似圆的半径进行膨胀;
步骤3:根据机器人所处的环境,通过人工势场法变种构建虚拟的“势场”,是在粮仓环境中构造一个人工势场,势场中包括斥力极和吸引极,不希望机器人进入的区域或障碍物定义为斥力极,目标及建议机器人进入的区域定义为引力极,类似于物理学中的电磁场,使得在该势场中的机器人受到其目标位姿引力场和周围斥力场的共同作用,朝目标前进;该算法的具体步骤如下:
1)构建引力势场,引力势场的函数是:
其中U(q)为引力势场的势能,k为正比例系数,ρ(q,q0)为一矢量,方向为从机器人指向粮堆,大小为机器人与粮堆间的欧式距离|q-q0|,ρ0为一常数,表示粮堆对机器人产生作用的最大影响范围,决定粮堆引力势场的因素是机器人与粮堆间的距离,当机器人未进入粮堆的影响范围时,其受到的势能值为零,在机器人进入粮堆的影响范围后,两者之间的距离越小,机器人受到的势能值就越大,距离越大,机器人受到的势能值就越小;
相应的引力公式为:
式中,F1是机器人受到的引力,方向为机器人指向粮堆;
2)创建边界斥力,在粮仓环境中默认无其他障碍物,故不设立障碍物斥力势场,只考虑设定粮仓边界斥力,限制机器人的运动,并适当考虑机器人速度对斥力的影响,相应的具体斥力公式如下:假设粮仓底面为正方形,以底面中心为原点建立直角坐标系,式中F2为斥力,η是常数,v为机器人速度,x为机器人横向坐标,y为机器人纵向坐标,a为粮仓底面宽度一半,b为粮仓底面长度一半,L为车长,w为车宽,e(x+y)与e-(x+y)表示随着横、纵坐标绝对值的增大,机器人受到的斥力呈指数增长;
3)根据人工势场法的基础原理,机器人在这两种势场的环境下运动时所受的人工势场中的势能是由多个势能矢量相加决定的,因此,机器人受到的合势场的合力表达式为:F=F1+F2
式中:F是机器人在势场中受到的合力;
步骤4:规划出一条路径,使机器人能够驶向粮堆;
步骤5:绕着该粮堆底部做螺旋线运动,直到机器人到达膨胀后底面圆的圆心;
步骤6:重复步骤2到步骤5直到所有粮堆低于给定的高度阈值。
2.根据权利要求1所述的用于粮仓机器人平粮作业的路径规划及平粮作业控制方法,其特征在于通过slam技术构建粮仓的地图,具体包括以下步骤:
1)将激光雷达、摄像头、陀螺仪、GPS安装在机器人上,用于捕捉周围环境的数据以及机器人的姿态信息;
2)将机器人放入粮仓环境中,机器人需要在未知环境中移动,从而收集环境数据;这些数据包括激光雷达的距离和方向信息;
3)收集到数据后,需要使用ros中自带的slam算法来处理数据,并生成环境地图;
4)在地图生成之后,通过数据清理、算法优化对地图进行更新和优化,以提高精度和准确性;
5)将生成的地图导入到机器人的控制系统中,以实现路径规划功能。
3.根据权利要求1所述的用于粮仓机器人平粮作业的路径规划及平粮作业控制方法,其特征在于所述的通过激光雷达识别粮堆的位置坐标、几何形状和高度,将粮堆底面近似圆的半径进行膨胀,具体包括以下步骤:
1)使用激光雷达对粮堆进行扫描,获取其位置坐标、几何形状和高度信息;
2)对粮堆底面进行近似圆形拟合,得到其半径;
3)在上一步基础上将半径膨胀0.2倍,即将原始半径乘以1.2;
4)将膨胀后的粮堆底面信息与原始的位置、几何形状和高度信息结合起来,得到完整的粮堆信息。
4.根据权利要求1所述的用于粮仓机器人平粮作业的路径规划及平粮作业控制方法,其特征在于所述的规划出一条路径,使机器人能够驶向粮堆,是根据引力势场函数求出各个粮堆的势能大小,筛选出势能最大的粮堆,然后用梯度下降法计算出最优路径,从当前点开始,沿着势场的负梯度方向前进,直到到达目标点或者遇到边界,其具体步骤如下:设车辆位置为(x,y),目标位置为(xg,yg),则引力势场的函数为式中:表示机器人与目标位置间的欧式距离;
求导后得到引力势场的负梯度为:
式中,-▽U(x,y)为负梯度,-U'x(x,y)和-U'y(x,y)为负梯度分量,和表示坐标基;其中,化简后得引力势场的负梯度为:
梯度下降算法的输出是路径规划点序列,将这些序列传送给机器人底层控制系统,控制机器人按照规划路径移动至目标粮堆。
5.根据权利要求1所述的用于粮仓机器人平粮作业的路径规划及平粮作业控制方法,其特征在于所述的绕着该粮堆底部做螺旋线运动,直到机器人到达膨胀后底面圆的圆心,具体步骤如下:假设目标圆心的坐标为(x0,y0),半径为r,定义一个参数t,表示当前的角度,使用下面的公式计算点的坐标:x=r·cos(t)
y=r·sin(t)
为了绕着圆的外围做螺旋线运动,循环更新t值,将t的变化速度逐渐加快,使t的增量相对于当前的t值成比例地增加,每隔固定时间T=0.1s就循环一次,具体公式如下:初始化:
Δt=0.01
k=0.1
循环更新:
Δt=Δt·(1+k·t)
t=t+Δt
r=r-0.5
式中,Δt是t的增量,k是固定的比例因子,每循环一次就得到一个新坐标(x,y),直到得到的坐标近似于圆心(x0,y0),循环结束,将坐标序列传送给底层控制系统,就能控制机器人做螺旋线运动,利用机器人行走机构与粮面的相互作用,在运动过程中即可将粮堆摊平。
6.根据权利要求1所述的用于粮仓机器人平粮作业的路径规划及平粮作业控制方法,其特征在于所述的步骤6中重复步骤2到步骤5直到所有粮堆低于给定的高度阈值,是指机器人在粮仓工作的过程中,激光雷达实时监测粮堆的高度,假设机器人监测到的粮堆相对于机器人底盘高度为h,激光雷达相对于机器人底盘高度为h0,机器人按照步骤2到步骤5重复工作,直到h≤h0时,则认为所有粮堆已被摊平,机器人停止工作。