高兴华,王从永,崔晓越

(北华大学 机械工程学院,吉林 吉林 1320012)

对老人或病人、失能及半失能人的生活照顾是社会主要问题之一[1]。辅助智能装置在一定程度上赋予老人或病人生活自理能力,对于减轻社会和家庭的养老负担有重要帮助。因此,研制能够协助老年人完成坐、躺、立、蹲、行体位变化、室内外自主导航与定位和多模态输入的多体位自主导航智能轮椅,对提高患者的康复质量、生活质量、减轻社会负担具有重要的实际意义。路径规划是多体位轮椅自主定位导航的重要环节之一。2007年,中北大学学者利用双向Dijkstra算法,结合路径规划算法在车辆定位导航系统中的实际应用,给出该算法的实际应用并进行分析。2015年西北工业大学学者应用A*算法进行离线规划生成预规划航路,预规划航路作为参考选择局部目标点进行在线规划;2021年,海南大学学者对A*算法的子节点扩展优化与oyd算法优化路径,解决转角和转折节点过多的问题,使规划路径更加平稳及合理。常见的几种路径搜索算法中A*算法是当前主流的一种路径搜索算法,它是基于评估函数的启发式路径搜索算法,相比Dijkstra算法A*算法规划的路径更快、更短,但往往会出现搜索空间变大、路径冗余及转弯半径大等问题,因此需对A*算法改进以减小路径规划的范围和转弯半径,从而提高路径规划的效率。

1 多体位变换椅结构与控制系统

1.1 多体位变换椅结构

多体位变换椅机械结构如图1所示。主要结构包括:底盘机构、驱动机构、雷达扫描机构、升降机构、座椅翻转机构、小腿板翻转机构和坐板翻转机构等。

图1 体位自主导航智能轮椅机械结构方案示意

本轮椅使用丝杠传动机构及其他辅助机构配合完成高坐姿、坐姿和低坐姿的位置变换;采用背板翻转机构及其他机构辅助解决座椅与床之间的姿态转换;采用丝杠传动机构、背板翻转机构和小腿翻转机构完成坐姿与立姿的转换;采用后驱差速机构进行移动以确保在姿态变换时,重心偏移不会导致智能轮椅发生偏移。可实现坐、行、蹲、立和躺五种姿态,如图2所示。

图2 多体位自主导航智能轮椅总体位姿变换

1.2 控制系统

为实现室内外可灵活移动,该智能轮椅设置差速行走机构,动力轮的编码器实时传输速度等信息。由于使用群体的特殊性,为方便老年人操作,多体位轮椅设有操纵杆控制、语音交互、无线遥控以及人机交互界面,最大限度地为使用对象降低难度,做到对老年人与残疾人极度人性化。为了实现无人驾驶,智能轮椅基于ROS平台建立智能轮椅的运动模型,运用Move_base框架对高精度二维激光雷达与MPU-6050六轴姿态传感器的数据进行处理,从而实现SLAM定位和建图[2-3]。

控制主体由上位机系统与下位机系统组成[4-6]。上位机系统负责处理系统综合数据并发布指令,下位机系统主要完成底层驱动;上位机系统与下位机系统通过串口通信,下位机与遥控采用无线传输进行通信。其中,上位机为Mini工控机,主要负责将控制指令信息发送至下位机,进而驱动控制系统,并实时显示智能轮椅的状态,同时,接受来自驱动控制系统的状态信息;上位机通过串口接收雷达的信息,实现建图、导航及避障信息的处理。下位机的处理器使用STM32芯片,主要负责接收和发送硬件驱动的信息,为驱动控制系统提供控制信号,实时监测并且记录系统状态。操作遥控的处理器使用STM32芯片,主要是给下位机发送控制指令,控制原理如图3所示。

图3 控制系统原理结构图

2 自主导航算法及改进

2.1 自主导航总体框架

多体位变换轮椅导航框架的核心是感知层、规划层和控制层,信息由上层向下传递,共同实现多体位轮椅的导航。导航框架如图4所示。

图4 导航框架

上位机系统接收雷达扫描的数据和姿态传感器的数据输入到感知层,感知层提取外部环境信息,高精度定位并做出任务规划,Move_base对感知层传来的数据进行规划处理后发送给规划层,规划层对行为、运动进行规划,采用改进A*算法并融合正则表达进行限制性路径规划将规划的路径航迹点等运动信息送至控制层,进而对底盘运动进行控制。导航控制中融合全局路径规划技术、局部路径规划技术、自动避障技术等重要技术,其导航控制能力将直接影响智能轮椅的安全性和有效性。

2.2 基于DWA算法的局部路径规划

动态窗口(Dynamic Window Approach,DWA)[7-9]算法是一种局部路径规划算法,它考虑到加速度的影响,开创一个速度矢量空间(υ,ω),其中线速度υ和角速度ω是在设定T时间内的可能值,从该空间选取多组速度用于模拟轮椅在接下来的时间内的运动轨迹,然后对每一个轨迹进行评价,选取最优的轨迹对应的速度对轮椅进行驱动[10-12]。DWA算法主要包括两个过程:建立速度矢量空间和对运动轨迹进行评价。

假设智能轮椅存在速度矢量空间,则Vm的取值为

Vm={v∈[vmin,vmax],ω∈[ωmin,ωmax] ,

(1)

其中vmin和vmax分别为预先设置的轮椅的最小线速度和最大线速度;ωmin和ωmax分别为轮椅的最小角速度和最大角速度。设轮椅的最大线加速度为vmax,最大线减速度为vmin,最大角加速度为ωmax,最大角减速度为ωmin,设模拟周期T内,轮椅可能达到的速度矢量空间Vn为

Vn={(v,ω)·v∈[vo-vminT,vo+voT]∧ω∈[ωo-ωminT,ωo+ωoT]} ,

(2)

其中,vo和ωo分别表示当前的线速度和角速度。Vn为一个以速度对(vo,ωo)为中心的矩形窗口,窗口中的速度矢量表示当前模拟周期内可达的速度矢量,窗口外的速度矢量均为不可达。为了避免和障碍物发生碰撞,在最大减速能力下,有如下速度矢量空间:

(3)

其中distant(v,ω)表示智能轮椅与速度对(v,ω)前向模拟的运动轨迹上最近的障碍物的距离。综上所述,可以得到最终需要搜索的速度矢量空间:

Vr=Vm∩Vn∩Vmax,

(4)

现已得到速度矢量空间,下一步是计算速度矢量空间中各个速度矢量的评价值,DWA算法评价函数表达式为

Assess(v,ω)=σ[α·heading(v,ω)+β·distant(v,ω)+γ·velocity(v,ω)] ,

(5)

其中,α、β、γ均为权重系数,可根据实际需要来进行设定。heading(v,ω)用来评价在当前速度矢量下,θ表示智能轮椅到达模拟轨迹终点时的姿态角和目标姿态角之间的差值。heading(v,ω)的表达式为

(6)

可以看出,θ值越小则heading(v,ω)函数输出值越大。distant(v,ω)的含义与上文相同,其表达式为

(7)

其中,l为多体位轮椅与模拟轨迹上最近的障碍物之间的距离;L为障碍物判定的阈值,即距离大于L时,可将此障碍物忽略。velocity(v,ω)用来评价下一时刻智能轮椅的速度大小,表达式为

(8)

可以看出,速度v的值越大velocity(v,ω)函数值越大。从评价函数的各个表达式可以看出,DWA算法是一种以目标点为导向的局部路径规划算法,力求尽可能快速地移动到目标点,在实际应用中,可以结合实际需求来设置权重系数,以达到最优的实时避障效果。

2.3 栅格构建地图

本设计选择地图构建领域应用最广泛的栅格地图法,来实现全局路径规划算法路径搜索,栅格地图路径规划如图5所示。根据智能轮椅的实际工作环境,将地图分割成大小一致的二维网格图,“0”代表自由栅格,表示当前网格没有被障碍物占领,在网格图中用白色填充;而“1”代表占用栅格,表示当前网格被障碍物占领,在网格图中用黑色填充。

2.4 传统A*算法

Dijkstra算法在二维平面上从初始节点向四周蔓延,直到蔓延到目标节点。在向四周蔓延时不会因为目标节点而改变蔓延方向和范围,对四周的行为是一致的,没有盲区地进行遍历,如图6所示。

图6 Dijkstra算法示意图

A*算法基于评估函数的启发式路径搜索算法,从当前坐标开始,采用向下搜索的方式搜索最小代价点,并把搜索到的最小代价点更新为当前位置坐标继续搜索,直到最小代价点达到目的坐标点[13-14]。搜索公式可显示为

f(n)=g(n)+h(n) ,

(9)

其中,f(n)代表代价评估函数,它的含义是评估坐标起点到目的坐标的距离评估;g(n)表示的是坐标起点与当前节点之间的实际距离;h(n)为启发函数,代表的是当前任意节点到目的坐标点的最优距离估计。

g(n)可以通过以下公式计算得出:

(10)

h(n)是算法搜索过程中的距离评估,其约束条件为

h(n)≤cos*(n,ngoal)

(11)

其中cos*(n,ngoal)是栅格节点n到目标节点的最优距离。

以d(n)表示状态n到目标状态的距离,那么h(n)的选取大致有如下三种情况:

第一,如果h(n)

第二,如果h(n)=d(n),即距离估计h(n)等于最短距离,那么搜索将严格沿着最短路径进行,此时的搜索效率是最高的;

第三,如果h(n)>d(n),搜索的点数少,搜索范围小,效率高,但不能保证得到最优解。

传统A*算法的不足点:

1.重复寻找节点,使寻路的效率下降;

2.启发函数h(n)代价值小于实际值,搜索范围变大、路径重复;

3.轮椅视为质点遇见障碍物时精度不够甚至穿模。

2.5 A*算法改进理论

在寻找最优路径时,要对f(n)或h(n)的取值充分考虑,h(n)作为启发函数,作用是估计代价,通过把估计出来的距离与实际值相比较,可以得出,当估计距离和真实距离差越小,则路径搜索所达到的效果就越好。

假设在栅格地图中当前的节点为n(xo,yo),目标节点为goal(xgoal,ygoal),则两节点间的欧几里得距离为Do、曼哈顿距离Dm和对角线距离Dd公式如下所示:

(12)

Dm=|xo-xgoal|+|yo-ygoal| ,

(13)

Dd=max(|xo-xgoal|,|yo-ygoal|) 。

(14)

栅格地图中,搜索路径的节点连接方式包括任意点连接、八节点连接和四节点连接,如图7所示。

图7 栅格节点连接关系示意图

依据欧几里得函数在任意节点连接方式中有很大的作用,用它作为启发函数,为了克服求解过程中计算量较大的问题,选用欧几里得公式,如式(15)所示;此外,根据节点连接个数的不同选择不同的评估函数,如针对8节点连接选用切比雪夫距离,4节点的连接选用曼哈顿距离。

(15)

在以上的基础上通过创建表OPEN记录已被考察的节点,创建CLOSED表记录已经被检测过的节点,通过表P记录当前节点的父节点,同时,A*算法在执行过程中还需要对上述表格进行维护。

基于改进的A*算法多体位轮椅路径规划流程如图8所示。

图8 A*算法改进流程图

多体位变换轮椅基于改进后A*算法的路径规划过程为

1.建立数据表OPEN表与CLOSE表,将起点S写入OPEN表中,将障碍点写入CLOSE表中。

2.计算OPEN表中最小节点值f并将其纳入CLOSE表中,若OPEN表为空则直接结束。

3.判断当前节点m是否为目标节点,若是目标节点则根据m的前向路径生成最优路径;若不是则扩展目标节点m,生成后继节点n。

4.在OPEN表中建立从后继节点n返回到m的指针,计算f(n)=g(n)+h(n)。

5.对于生成的后继节点判断表OPEN中是否已含有节点n,若无节点n则将m放入表OPEN中;若有节点n则比较不同的前向指针f(n)值大小,保留较小的f(n)值。

6.更新g(n),f(n)以及后继节点n的前向指针。

7.将表OPEN中f(n)的数值由大到小排列,并返回第2步。

3 A*算法改进MATLAB仿真及验证

3.1.1 传统A*算法路径规划

采用栅格法构建地图,利用传统A*算法规划路径。在传统A*算法中启发函数h(n)一般使用曼哈顿距离定义,则启发函数:

Dm=|xo-xgoal|+|yo-ygoal| ,

(16)

那么多体位变换椅行驶到n点的代价函数为

F(n)=g(n)+Dm=|x0-xgoal|+|yo-ygoal| 。

(17)

3.1.2 仿真结果与分析

基于MATLAB进行仿真模拟,仿真环境为30 m*30 m,采用曼哈顿距离作为启发函数对算法进行仿真,结果如图9所示。

(a)

从仿真结果上看传统A*算法可以规划出一条从起点到终点的多折线路径。在路径规划时,拐点与拐点优先以对角线规划,直到对角线碰到障碍物,之后紧贴障碍物以直线规划。此算法存在搜索空间变大、紧贴障碍物,路径冗余及转弯半径大的问题。针对上述传统A*算法的不足,通过对启发函数的优化可提升路径规划性能。

3.2 改进A*算法仿真与分析

为验证A*算法的可行性,选用MATLAB进行仿真模拟,仿真环境为30 m*30 m,任意节点采用欧式距离函数作为启发函数,8节点连接选用切比雪夫距离,4节点的连接选用曼哈顿距离作为启发函数对算法进行仿真验证[15]。在构建地图中小车从起始点出发,到达目的地后由改进A*算法规划的路径如图10所示,而实际运行轨迹如图11所示。

图10 改进A*算法模拟运动轨迹

图11 实际运行轨迹

从仿真结果可以看出A*算法模拟轨迹较短,经拟合平滑处理后在拐点处转弯半径小,折线段优化为弧线段,较符合实际情况,由此可见对改进后的A*算法是可行的。

4 结 论

本文通过对多体位姿态变换智能轮椅的自主导航算法研究,在ROS操作系统下由高精度激光雷达与姿态传感器,在Move base导航框架下构建地图并实现定位,针对A*算法中启发函数h(n)代价值小于实际值,导致搜索空间变大、路径冗余的问题,采用不同节点代价函数选取不同公式对A*算法进行改进。针对重复搜索节点,使寻路效率下降问题,通过建立数据表中的OPEN表与CLOSE表记录被考察过的节点,改进传统A*算法中重复搜索节点,使寻路效率下降问题;通过建立数据表OPEN表与CLOSE表记录被考察过的节点,增强方向性,提高了算法的效率。采用MATLAB进行仿真验证,对小车规划路径进行拟合处理,使得优化之后的路径更加平滑,小车最终实现从起点至终点的路径规划,实现机器人SLAM功能。在改进A*算法下对全局路径进行规划,利用DWA算法对局部路径进行规划以达到最优的实时避障效果。