多传感器数据融合下机器人路径分段规划仿真

王红梅，陈 冬

(1.山西工程技术学院大数据与信息工程科学系，山西 阳泉 045000；2.山西大同大学数学与计算机科学学院 山西 大同 037009)

1 引言

随着军事、工业、航空航天领域对使用机器人的需求日益增多，大力研发机器人技术[1]成为了热门课题。因机器人凭借较好的灵活性与独立性等优势，还被广泛应用于服务业、医疗业、日常生活等各个领域，使人类在生产生活等诸多方面都得到极大程度的改善与革新。为满足当今的应用需求，机器人既要能够很好地适应环境，还需要有理想的智能控制系统，而机器人的路径规划作为该研究方向的一个关键课题，受到众多相关学者的深入探讨。

比如，文献[2]张杰等人在研究水下群机器人路径规划算法形式化的过程中，构建出一种基于定理证明器HOL4的遗传算法形式化模型，该模型具有合理性与有效性；李靖等人[3]在灰狼优化算法中引入Logistic混沌映射进行优化，将其与执行大任务量监测任务的机器人相结合，实现大规模任务量的有效路径规划；李娟等人[4]针对蚁群系统的种群多样性与收敛速度问题，利用动态混沌算子构建动态混沌蚁群系统，应用于机器人路径规划中，加快规划效率。

影响机器人路径规划效果因素较多，上述文献方法极易造成时间与空间信息上的缺失与冗余，因此，本文引用多传感器数据融合技术，提出一种机器人路径分段规划方法。从多级别、多层次、多方面处理多个传感器数据的技术就叫做多传感器信息融合技术[5]，其应用领域有工业机器人、金融系统、船舶避碰、空中交通管制、军事战略等。该项技术可有效补充缺失信息，优化冗余数据，使环境得到极大程度认知，强化传感器性能，既提升机器人路径规划的适用范围，也降低传感器元件生成的累积误差，且在机器人移动定位、障碍物躲避、路径规划等重要环节中得到了理想的仿真效果；通过平滑处理规划路径，消除因路径生成较多折线与拐点而产生的锯齿效应，解决机器人碰撞问题；利用转弯策略，抑制机器人本体宽度影响，防止其突然变向、发生碰撞；在简化运算方面，分别采取了做正切圆、将机器人车轮宽度算入整体宽度中等策略。

2 机器人路径状态变量估计值分析

为提升机器人路径规划的适用范围，降低传感器元件生成的累积误差[6]，融合机器人超声波测距仪等多个传感器的测量数据。

(1)

假设机器人在[k，k+1]时段内未发生轨迹，测距量i的权重为wi，则利用加权最小二乘法[8]构建下列表达式

(2)

(3)

其中

(4)

(5)

(6)

(7)

(8)

(9)

式中，θ为机器人正前方方向与顺时针旋转90°方向所呈夹角，b为机器人履带宽度，ΔSl、ΔSr分别为左右两条履带的移动距离。

(10)

(11)

得到机器人运行轨道偏方差为

S′(k+1)=H(k+1)P(k+1|k)H(k+1)T+R(k+1)

(12)

(13)

P(k+1|k+1)=P(k+1|k)-

K(k+1)S′(k+1)K(k+1)-1

(14)

K(k+1)=p(k+1|k)H(k+1)TS′(k+1)-1

(15)

通过更新机器人运行路径状态，结合方差，对路径数据进行卡尔曼滤波[9]，得到滤波结果表达式为

(16)

(17)

(18)

给定输入量与测量值的协方差数据信息后，通过融合多传感器数据，得到机器人不同采样时刻的状态，获取其路径规划结果。

3 机器人路径分段规划

机器人在需要躲避路径障碍物时，会存在较大路径曲率的路段，应先减速慢行，确保平稳过弯，待经过该路段需加速移动一段距离，之后再匀速通行。因此，以路径轨迹曲率极值点[11]为依据，分段处理各段路径上的机器人移动速度。

3.1 平滑处理

为消除因路径生成较多折线与拐点而产生的锯齿效应，解决机器人碰撞问题，需平滑处理规划路径。根据规划路径各分段点，连接当前分段点与下一个相邻分段点，逐段展开平滑处理至全路径结束。具体执行流程描述如下：

1)根据多传感器数据融合过程中得到的机器人定位点，分析最终路径规划结果上各分段点；

2)假设当前分段点是下一个路径起始点，若存在当前分段点的子分段点，则进入下一流程；反之，则停止平滑处理；

3)遍历起始点与下一个当前分段点，若获取的返回值是真值，则进入下一流程；反之，则跳至第(5)步；

4)用下一个当前分段点赋值当前分段点，与此同时，去除之前的各当前分段点，返回第(2)步；

5)用当前分段点赋值起始点，用下一个当前分段点赋值当前分段点，返回第(2)步；

6)终止路径平滑处理。

平滑处理工程中，需按照固定间隔进行遍历、采样，依序处理各分段路径。

3.2 转弯策略

为抑制机器人本体宽度影响，防止突然变向、发生碰撞，设计出一种合理的转弯策略。假设机器人转弯半径是R，圆心是O，以起始点为参照的圆心方向是DO，起始点方向是Dstart，x、y表示各点对应横纵坐标，则当机器人右转时，用下列表达式描述圆心O的转动方向与位置

(19)

(20)

已知点Q是加速区域的结束点，则基于三角定理可得该点与下一分段点的间距d′、距离LQ与半径R所呈夹角θ′，计算公式如下所示

(21)

由此推导出下列点Q的转动方向与位置计算公式

(22)

当机器人向左转，将圆心O的方向加90°，点Q坐标求解过程中把φQ+θ′改为φQ-θ′。

当机器人在起始点P′1与目标点P′2的转弯方向均为顺时针方向时，则两点间距的长度LP′1P′2与斜率k′P′1P′2等于以两点为圆心的圆外切线AB长度LAB与斜率k′AB，即

(23)

式中，离开第一段弧的角度是θ″arc1，抵达第二段弧的角度是θ″arc2。

若机器人在起始点P′1与目标点P′2的转弯方向是相对的，离开时为顺时针，抵达是逆时针，则对以目标点为圆心的圆做正切圆，以降低运算复杂度，设正切圆圆心为点P′3，因此，基于三角形P′1P′2P′3，推导出下列方程组

(24)

式中，两目标点间距为LP′2P′3，∠P′1P′2P′3度数是θ′P′1P′2P′3，切线AB角度是θ′AB。根据得到的各分段路径离开与到达弧度，实现机器人移动路径的分段规划，避免发生碰撞，跌倒。

4 机器人路径分段规划仿真

4.1 机器人

选取由驱动控制模块、传感器模块、GPS(Global Positioning System，全球定位系统)定位模块以及软件控制模块组成的机器人作为实验对象，规划其移动路径。机器人具体参数信息如表1所示。

表1 实验用机器人参数统计表

开启软件程序，进入仿真界面，初始化处理机器人与路径规划环境，将其存储于数据库中，利用编程软件执行本文分段规划方法，待机器人抵达指定目标点后，终止路径规划，退出程序。方法运行时，各传感器持续读取环境信息，不停往数据库中存储机器人位置信息。

4.2 机器人定位精度分析

超声波传感器的采样时间是0.1秒，分别采用文献[2]、[3]、[4]方法以及本文方法，融合一定时间里多个传感器采集到的数据信息，整理得到表2。

表2 多传感器数据融合测量值统计表

结合上表2中数据可知，本文方法经过预测、观测以及估计等一系列步骤，不断更新状态变量与对应协方差，获取机器人不同采样时刻状态与最优位置估计，所以，对比文献方法的定位准确度具有显著优越性，误差均值不超过1厘米，机器人移动路线与理想的预测轨迹较为趋近。因此，引入多传感器数据融合为机器人路径规划奠定了良好的数据基础。

4.3 路径分段规划仿真

为简化运算复杂度，验证方法有效性，将机器人车轮宽度算入整体宽度中，并分别在简单障碍物与复杂障碍物两种环境里展开仿真。按照所得融合数据，规划出机器人移动路径，分段规划的最终路径结果如图1所示。

图1 不同环境下路径分段规划仿真效果

根据上图中得到的路径结果可以看出，机器人不论是在简单障碍物还是更接近实际环境的复杂障碍物环境中，都规划出了较为理想的路径，不仅有效躲避了环境中的障碍物，而且规划的路径也比较合理；从机器人移动状态而言，未出现任何晃动、跌倒趋势。这是因为本文方法根据路径轨迹曲率极值点，分段处理了各段路径上的机器人移动速度，通过平滑处理与转弯策略，有效实现了机器人移动路径的分段规划，防止了碰撞与跌倒的发生。

5 结论

基于给定的环境信息程度，一般将路径规划方法划分成两种：全局、局部。前者常用于障碍物信息已知、固定、可预见等情况下，而后者则通过传感器信息，在未知、动态的环境中，展开路径规划。将进一步提升机器人障碍物躲避与环境识别能力、实时性与灵活性作为下一个研究重点；应尝试引用AS-R机器人视觉系统，增加其在三维空间中自由运动的能力；应在完全未知的环境中仿真规划方法，拓展方法适用性；仅以单机器人为研究对象具有一定的局限性，需在今后的工作中，深入探索多机器人的路径分段规划策略；路径规划应用领域多种多样，应通过分析环境信息，采取对应的规划方法。