一种基于惯导与激光雷达测量的机器人切线避障方法与流程

本发明涉及机器人领域，具体而言，涉及一种基于惯导与激光雷达测量的机器人切线避障方法。

背景技术：

随着科学技术的不断进步和制造工艺的提高，机器人在军事、运输、救援、商业、娱乐、农业等领域的应用越来越广泛。机器人在执行任务的过程中，合理规划路径、有效躲避障碍物、提高工作效率是前提。因此，机器人路径规划研究成为当今主要研究热点之一。机器人路径规划研究算法主要有经典路径规划算法、智能路径规划算法，其中人工势场法作为一种简单高效的经典路径规划方法，得到广泛的研究与应用。人工势场法在1985年由khatib首次提出，并将之应用于机械臂的避碰问题。s.akishita等人在改进该方法后，将其应用于机器人的路径规划，该方法的优点是在提高时效性的同时，有效降低了路径规划的难度，但该路径规划方法仍然存在极小值和目标点不可达等问题。基于上述原因，本发明提出了一种采用惯导与激光雷达对自身位置与障碍物进行测量，并采用切线避障、威胁因子叠加合成总速度的方法，避免了极小值与目标点不可达问题，从而实现了机器人的自动避障与路线规划。

需要说明的是，在上述背景技术部分发明的信息仅用于加强对本发明的背景的理解，因此可以包括不构成对本领域普通技术人员已知的现有技术的信息。

技术实现要素：

本发明的目的在于提供一种基于惯导与激光雷达测量的机器人切线避障方法，进而至少在一定程度上克服由于相关技术的限制和缺陷导致的极小值问题与不大到达问题。

根据本发明的一个方面，提供一种基于惯导与激光雷达测量的机器人切线避障方法，包括以下步骤：

步骤s10，在避障机器人上安装imu-800型光纤陀螺惯性测量单元，对避障机器人的水平运动加速度进行测量，并进行两次积分，得到避障机器人的实时位置坐标与实时速度信息；

步骤s20，在避障机器人上安装rplidara3型激光雷达，测量障碍物的角度与距离信息；

步骤s30，根据所述安装的rplidara3型激光雷达对避障机器人周边范围内的障碍物进行实时测量并记录障碍物位置信息，以及进行坐标系转换得到障碍物的实时坐标；

步骤s40，根据rplidara3型激光雷达提供的障碍物边缘信息，估算障碍物的半径，并设置规避区域半径；

步骤s50，设置避障机器人的期望速度为v，根据rplidara3型激光雷达提供的障碍物质心位置极坐标信息与规避区域半径，判断是否进入规避区；

步骤s60，根据所述的需要规避的障碍物，按照切线法最短距离逃逸的原则，解算每个障碍物的理想规避速度方向；

步骤s70，根据机器人离障碍物边缘的距离，解算每个障碍物的威胁距离，然后根据威胁距离，求解每个障碍物的威胁因子，最后根据威胁因子的叠加，解算总的理想规避速度方向；

步骤s80，根据所述的总的理想规避速度方向与期望速度大小，进行理想位置解算，得到机器人进行自动避障的自动生成轨迹，从而完成整个机器人的自动轨迹规划与避障任务。

在本发明的一种示例实施例中，安装imu-800型光纤陀螺惯性测量单元，对避障机器人的水平运动加速度进行测量，并进行两次积分，得到避障机器人的实时位置坐标与实时速度信息包括：

vx＝∫axdt；

vy＝∫aydt；

xr＝∫vxdt；

yr＝∫vydt；

其中ax为采用imu-800型光纤陀螺惯性测量单元得到的避障机器人的x向加速度，ay为y向加速度，dt表示对时间信号进行积分，vx为积分得到的避障机器人x向速度，vy为积分得到的避障机器人y向速度，xr、yr为速度积分得到避障机器人的实时x向位置坐标与实时y向位置坐标。

在本发明的一种示例实施例中，安装rplidara3型激光雷达对避障机器人周边范围内的障碍物进行实时测量并记录障碍物位置信息，以及进行坐标系转换得到障碍物的实时坐标并估算障碍物的半径，设置规避区域半径包括：

xi＝ricos(θi)；

yi＝risin(θi)；

xri＝xr+xi；

yri＝yr+yi；

rai＝risin(|θim-θix|/2)；

rbi＝kirai；

其中(ri,θi)为采用rplidara3型激光雷达测量得到的第i个障碍物质心位置极坐标信息，1≤i≤n，n为障碍物个数。(xi,yi)为第i个障碍物相对避障机器人位置的直角坐标，(xri,yri)为第i个障碍物的实时坐标。xr、yr为速度积分得到避障机器人的实时x向位置坐标与实时y向位置坐标。θim与θix分别为rplidara3型激光雷达提供的第i个障碍物的角度最大值与最小值。rai为第i个障碍物的半径的估计值。rbi为第i个障碍物的规避区域半径。ki为第i个障碍物的规避因子，1＜ki＜2，其详细设计见后文案例实施。

在本发明的一种示例实施例中，根据所述的需要规避的障碍物，按照切线法最短距离逃逸的原则，解算每个障碍物的理想规避速度方向包括：

δy＝yt-yr；

δx＝xt-xr；

δyi＝yri-yr；

δxi＝xri-xr；

其中pi为每个障碍物的规避标志，pi＝0表示避障机器人距离障碍物较远，无需进行规避。pi＝1表示避障机器人距离障碍物较近，需要进行规避。(xr,yr)为imu-800型光纤陀螺惯性测量单元提供的避障机器人的实时位置坐标，(xt,yt)为目标位置的坐标，θ0即为无需避障情况下的机器人期望速度方向。θgi为考虑需要避障情况下，具体的第i个障碍物影响下，机器人的理想规避速度方向，其沿着机器人与障碍物连线为半径，以障碍物为圆心的园的切线方向，且保证与θ0的方向夹角小于90度。θai为机器人与障碍物连线的方向。

在本发明的一种示例实施例中，根据机器人离障碍物边缘的距离，解算每个障碍物的威胁距离，然后根据威胁距离，求解每个障碍物的威胁因子，最后根据威胁因子的叠加，解算总的理想规避速度方向包含：

rci＝ri-rai；

其中(ri,θi)为rplidara3型激光雷达测量得到的第i个障碍物的质心位置极坐标信息，rai为障碍物半径的估计值，rci为第i个障碍物的威胁距离，βi为第i个障碍物的威胁因子，θ为叠加求解得到的总的理想规避速度方向。

在本发明的一种示例实施例中，根据所述的总的理想规避速度方向与期望速度大小，进行理想位置解算，得到机器人进行自动避障的自动生成轨迹包括：

vix＝vcos(θ)；

viy＝vsin(θ)；

xir＝∫vixdt；

yir＝∫viydt；

其中v为机器人的期望速度大小，θ为所述的机器人的总的理想规避速度方向，vix为避障机器人x向的期望速度，viy为避障机器人y向的期望速度，xir为避障机器人x向期望位置，yir为避障机器人y向的期望位置，dt表示对时间信号的积分。自此，完整地实现了机器人自动避障与轨迹生成。

有益效果

本发明提供的一种基于惯导与激光雷达测量的机器人切线避障方法，能够通过惯性测量单元与激光雷达的测量，实现对障碍物的自动规避与路径规划。其优点在于通过切线避障与最短距离的原理，并采用威胁因子直接叠加速度方向的方法，即可得到多个障碍物综合影响下的自动避障航路规划。其相比与传统的通过速度求导得到加速度与力，再对力进行合成的引力场叠加的方法，具有逻辑更为简单清晰，计算简便的优点，而且能够避免极小值与目标不可到达的问题。

应当理解的是，以上的一般描述和后文的细节描述仅是示例性和解释性的，并不能限制本发明。

附图说明

此处的附图被并入说明书中并构成本说明书的一部分，示出了符合本发明的实施例，并与说明书一起用于解释本发明的原理。显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1是本发明提供的一种基于惯导与激光雷达测量的机器人切线避障方法的流程图；

图2是本发明实施例所提供方法的imu-800型光纤陀螺惯性测量单元放大图像；

图3是本发明实施例所提供方法的rplidara3型激光雷达正面图；

图4是本发明实施例所提供方法的rplidara3型激光雷达障碍物探测效果图；

图5是本发明实施例所提供方法的六个障碍物位置坐标曲线(无单位)；

图6是本发明实施例所提供方法的第1个障碍物的规避因子曲线(无单位)；

图7是本发明实施例所提供方法的第2个障碍物的规避因子曲线(无单位)；

图8是本发明实施例所提供方法的第3个障碍物的规避因子曲线(无单位)；

图9是本发明实施例所提供方法的第4个障碍物的规避因子曲线(无单位)；

图10是本发明实施例所提供方法的第5个障碍物的规避因子曲线(无单位)；

图11是本发明实施例所提供方法的第6个障碍物的规避因子曲线(无单位)；

图12是本发明实施例所提供方法的第1个障碍物的理想规避速度方向(无单位)；

图13是本发明实施例所提供方法的第2个障碍物的理想规避速度方向(无单位)；

图14是本发明实施例所提供方法的第3个障碍物的理想规避速度方向(无单位)；

图15是本发明实施例所提供方法的第4个障碍物的理想规避速度方向(无单位)；

图16是本发明实施例所提供方法的第5个障碍物的理想规避速度方向(无单位)；

图17是本发明实施例所提供方法的第6个障碍物的理想规避速度方向(无单位)；

图18是本发明实施例所提供方法的总的理想规避速度方向曲线(无单位)；

图19是本发明实施例所提供方法的机器人进行自动避障的自动生成轨迹图。

具体实施方式

现在将参考附图基础上更全面地描述示例实施方式。然而，示例实施方式能够以多种形式实施，且不应被理解为限于在此阐述的范例；相反，提供这些实施方式使得本发明将更加全面和完整，并将示例实施方式的构思全面地传达给本领域的技术人员。所描述的特征、结构或特性可以以任何合适的方式结合在一个或更多实施方式中。在下面的描述中，提供许多具体细节从而给出对本发明的实施方式的充分理解。然而，本领域技术人员将意识到，可以实践本发明的技术方案而省略所述特定细节中的一个或更多，或者可以采用其它的方法、组元、装置、步骤等。在其它情况下，不详细示出或描述公知技术方案以避免喧宾夺主而使得本发明的各方面变得模糊。

本发明提供了一种基于惯导与激光雷达测量的机器人切线避障方法，其通过惯性测量单元与激光雷达对机器人与障碍物的位置进行解算与测量，并根据沿着机器人与障碍物切线方向最短距离逃逸的原则进行障碍物理想速度规避方向角解算，然后按照距离解算威胁因子，并按照威胁因子对多个障碍物的理想速度规避方向进行综合叠加，得到最终的总的速度规避方向，从而实现机器人避障的轨迹自动生成。

下面，将结合附图对本发明的一种基于惯导与激光雷达测量的机器人切线避障方法进行进一步的解释以及说明。参考图1所示，该一种基于惯导与激光雷达测量的机器人切线避障方法包括以下步骤：

步骤s10，在避障机器人上imu-800型光纤陀螺惯性测量单元，对避障机器人的水平运动加速度进行测量，并进行两次积分，得到避障机器人的实时位置坐标与实时速度信息。

具体的，首先在避障机器人上安装imu-800型光纤陀螺惯性测量单元，该产品如图2所示，其加速度测量精度可达到50μg。

其次，将imu-800型光纤陀螺惯性测量单元得到的避障机器人x向加速度记为ax，y向加速度记为ay，再进行积分，得到避障机器人x向速度记为vx，y向速度记为vy。其积分方式如下：

vx＝∫axdt；

vy＝∫aydt；

其中dt表示对时间信号进行积分。

最后，根据所示的避障机器人x向速度记为vx，y向速度记为vy再次进行积分，得到避障机器人的实时位置坐标，记作(xr,yr)，其积分计算方式如下：

xr＝∫vxdt；

yr＝∫vydt；

其中dt表示对时间信号进行积分。

步骤s20，在避障机器人上安装rplidara3型激光雷达，测量障碍物的角度与距离信息；

具体的，首先将rplidara3型激光雷达在地面进行初步调试，然后安装在避障机器人上，rplidara3型激光雷达如图3所示。rplidara3型激光雷达的技术指标能够涵盖25米范围的实用化距离探测，每秒16,000的采样频率，高达0.33°(15hz情况)的角度分辨率。其探测效果如图4所示。

步骤s30，根据所述安装的rplidara3型激光雷达对避障机器人周边范围内的障碍物进行实时测量并记录障碍物位置信息，以及进行坐标系转换得到障碍物的实时坐标。

具体的，首先设定根据所述安装的rplidara3型激光雷达，测量周边的障碍物一共有n个，第i个障碍物质心位置极坐标信息为(ri,θi)，其中1≤i≤n。

其次，将其质心位置极坐标信息转换为相对避障机器人的直角坐标信息，设第i个障碍物相对避障机器人位置的直角坐标记作(xi,yi)，则有

xi＝ricos(θi)；

yi＝risin(θi)；

然后，根据避障机器人的实时坐标解算障碍物的实时坐标。假设避障机器人的实时坐标为(xr,yr)，障碍物的实时坐标为(xri,yri)，则根据下式进行障碍物的实时坐标解算：

xri＝xr+xi；

yri＝yr+yi；

步骤s40，，根据rplidara3型激光雷达提供的障碍物边缘信息，估算障碍物的半径，并设置规避区域半径。

具体的，首先把rplidara3型激光雷达提供的第i个障碍物的角度最大值记作θim、最小值记作θix。

然后根据下式对每个障碍物的半径进行估计，设第i个障碍物的半径记作rai，其估算方式如下：

rai＝risin(|θim-θix|/2)；

最后，在上述障碍物半径的设置基础上，设置加权系数，得到规避区域半径。具体的，设第i个障碍物的规避区域半径记作rbi＝kirai，其中ki为第i个障碍物的规避因子，1＜ki＜2，其详细设计见后文案例实施。

步骤s50，设置避障机器人的期望速度为v，根据rplidara3型激光雷达提供的障碍物质心位置极坐标信息与规避区域半径，判断是否进入规避区。

首先，依次设置每个障碍物的规避标志，对第i个障碍物质的规避标志记作pi，且设置pi＝0。其中pi＝0表示避障机器人距离障碍物较远，无需进行规避。pi＝1表示避障机器人距离障碍物较近，需要进行规避。

其次，依次对每个障碍物进行判断是否需要进行规避。具体的，针对第i个障碍物，如果rplidara3型激光雷达提供的第i个障碍物质心位置极坐标信息(ri,θi)，满足如下条件，则更改规避标志，具体判断方式如下：

如果ri≤rbi，则pi＝1；

如果ri≥rbi，则pi＝0。

步骤s60，根据所述的需要规避的障碍物，按照切线法最短距离逃逸的原则，解算每个障碍物的理想规避速度方向。

具体的，首先解算无需避障情况下的机器人期望速度v的方向，具体的根据imu-800型光纤陀螺惯性测量单元提供的避障机器人的实时位置坐标(xr,yr)，以及目标位置的坐标(xt,yt)，按照如下方式进行解算：

δy＝yt-yr；

δx＝xt-xr；

其中为了便于解算，我们选取坐标系时，将机器人与目标的位置局限于第一象限，因此θ0在正负90度之间。θ0即为无需避障情况下的机器人期望速度v的方向。

再考虑需要避障情况下，具体的第i个障碍物影响下，机器人的理想规避速度方向，应该是沿着机器人与障碍物连线为半径，以障碍物为圆心的园的切线方向，且保证与θ0的方向夹角小于90度。

其次，求解机器人与障碍物连线的方向。具体的，根据imu-800型光纤陀螺惯性测量单元提供的避障机器人的实时位置坐标(xr,yr)，以及rplidara3型激光雷达测量并解算得到的障碍物的实时坐标为(xri,yri)，进行解算得到机器人与障碍物连线的方向，记作θai，其计算方式如下：

δyi＝yri-yr；

δxi＝xri-xr；

最后，按照如下方式，求解第i个障碍物影响下，机器人的理想规避速度方向，记作θgi，其计算方式如下：

步骤s70，根据机器人离障碍物边缘的距离，解算每个障碍物的威胁距离，然后根据威胁距离，求解每个障碍物的威胁因子，最后根据威胁因子的叠加，解算总的理想规避速度方向。

具体的，首先根据rplidara3型激光雷达测量得到的第i个障碍物的质心位置极坐标信息为(ri,θi)，以及障碍物的半径的解算值rai，求解第i个障碍物的威胁距离，记作rci，其计算方式如下：

rci＝ri-rai；

其次，根据平分原则，解算第i个障碍物的威胁因子，记作βi，其解算如下：

最后，根据威胁因子，叠加求解最终的总的理想规避速度方向，记作θ，其解算如下：

步骤s80，根据所述的总的理想规避速度方向与期望速度大小，进行理想位置解算，得到机器人进行自动避障的自动生成轨迹，从而完成整个机器人的自动轨迹规划与避障任务。

具体的，首先，根据所述机器人的总的理想规避速度方向θ与期望速度大小v，首先进行速度分解，得到避障机器人x向的期望速度，记为vix，避障机器人y向的期望速度，记为viy，其分解方式如下：

vix＝vcos(θ)；

viy＝vsin(θ)；

其次，根据所述的避障机器人x向的期望速度与y向的期望速度，进行积分，得到x向的期望位置与y向的期望位置，分别记作xir与yir，从而合成得到机器人的自动避障轨迹。具体的积分解算方式如下：

xir＝∫vixdt；

yir＝∫viydt；

从而通过x与y两方向的合成，得到自动避障的自动生成轨迹，dt为对时间信号的积分。自此，完整地实现了机器人自动避障与轨迹生成。

案例实施与计算机仿真模拟结果分析

为验证本发明所提供方法的正确性与有效性，特提供如下案例仿真进行模拟。

在步骤s10中，安装的imu-800型光纤陀螺惯性测量单元；在步骤s20中，在避障机器人上安装rplidara3型激光雷达，并对障碍物的位置信息进行实时测量，如前面实施方式所描述，在此不再重复。

在步骤s30与步骤s40，中，根据所述安装的rplidara3型激光雷达对避障机器人周边范围内的障碍物进行实时测量并记录障碍物位置信息，以及进行坐标系转换得到障碍物的实时坐标，并根据rplidara3型激光雷达提供的障碍物边缘信息，估算障碍物的半径得到五个障碍物的坐标位置如下图5所示。其中一共5个障碍物，其坐标信息如下：

bx＝[3000400023001500700300]；

by＝[3000400022001500700300]；

ra＝[500300300400100100]；

ki＝[1.31.51.51.31.21.5]；

其中bx为六个障碍物的x坐标信息，by为六个障碍物的y坐标信息。ra为为六个障碍物的半径估计值。ki为六个障碍物的规避因子信息。图中钻石状为机器人的起点，*型为机器人的目标位置。

在步骤s50，设置避障机器人的期望速度为v＝5m/s，根据rplidara3型激光雷达提供的障碍物质心位置极坐标信息，得到每个障碍物的规避因子曲线如图6至图11所示。由图10可知，第5个障碍物的规避因子为0，其原因在于可以直线无需改变方向也不会碰到障碍物。

在步骤s60中，根据所述的需要规避的障碍物，按照切线法最短距离逃逸的原则，解算每个障碍物的理想规避速度方向如图12至17所示。

在步骤s70中，按照所提供方法，解算总的理想规避速度方向如图18所示。

在步骤s80，根据所述的总的理想规避速度方向与期望速度大小，进行理想位置解算，得到机器人进行自动避障的自动生成轨迹如图19所示。

由上述自动生成的轨迹曲线可以看出本发明所提供的方法是正确有效的，而且硬件实施简单合理，软件算法计算量小，而且能够快速有效地对平面障碍物进行规避，因此具有很高的工程推广与应用价值。

本领域技术人员在考虑说明书及实践这类的发明后，将容易想到本发明的其他实施例。本申请旨在涵盖本发明的任何变型、用途或者适应性变化，这些变型、用途或者适应性变化遵循本发明的一般性原理并包括本发明未指明的本技术领域中的公知常识或惯用技术手段。说明书和实施例仅被视为示例性的，本发明的真正范围和精神由权利要求指出。