避障方法、移动机器人及计算机可读存储介质
技术领域
本发明涉及导航领域,特别涉及一种避障方法、移动机器人及计算机可读存储介质。
背景技术
近年来,随着人工智能技术的进一步发展,移动机器人的应用越来越广泛,而自主移动机器人越来越成为科技界的研究热点,其中的导航问题是机器人实现自主移动的关键问题。移动机器人的自主导航是指机器人依靠自身携带的传感器在特定环境中,按时间路径最短或能耗最低等原则实现从起始位置到目标位置的无碰撞运动。
但是,即使根据起始位置和目标位置设定好预设轨迹,移动机器人在沿预设轨迹移动的过程中,仍然有可能遇到突然出现的障碍物挡住预设轨迹,因此,现有技术中提出了移动机器人的局部路径规划方法。
然而,本发明的发明人发现,现阶段常见的路径规划方法通常只能生成圆弧形状的路径,在障碍物排布复杂的环境下,路径规划的效果较差。
发明内容
本发明实施方式的目的在于提供一种避障方法、移动机器人及计算机可读存储介质,可以有效的提升路径规划的效果。
为解决上述技术问题,本发明的实施方式提供了一种避障方法,应用于移动机器人,包括:获取多条预测避障路径;对各条所述预测避障路径进行图优化,得到多条优化避障路径;评估各条所述优化避障路径的评估得分、并获取评估得分最高的优化避障路径作为目标路径;控制所述移动机器人沿所述目标路径移动。
本发明的实施方式还提供了一种移动机器人,包括:至少一个处理器;以及,与所述至少一个处理器通信连接的存储器;其中,所述存储器存储有可被所述至少一个处理器执行的指令,所述指令被所述至少一个处理器执行,以使所述至少一个处理器能够执行如前述的避障方法。
本发明的实施方式还提供了一种计算机可读存储介质,存储有计算机程序,所述计算机程序被处理器执行时实现前述的避障方法。
本发明实施方式相对于现有技术而言,在获取多条预测避障路径之后、评估各条预测避障路径的评估得分之前,对各条预测避障路径进行图优化,从而提升预测避障路径的质量,进而可以有效的提升路径规划的效果。此外,评估每条优化避障路径的评估得分,并获取评估得分最高的优化避障路径作为目标路径,从而可以得到最优的避障路径,进一步的提升路径规划的效果。
另外,所述对多条所述预测避障路径进行图优化,具体包括:根据各条所述预测避障路径、建立与所述预测避障路径一一对应的超图;根据梯度下降优化算法对各个所述超图进行图优化。
另外,所述建立与所述预测避障路径一一对应的超图,具体包括:对所述预测避障路径进行采样、得到多个采样点;根据所述采样点的位置、所述障碍物的位置、以及所述移动机器人在水平面上的正投影的外接圆半径,建立所述超图。
另外,所述建立超图,具体包括:根据所述采样点的位置、所述障碍物的位置、以及所述移动机器人在水平面上的正投影的外接圆半径,依据预设约束条件,建立所述超图。
另外,所述建立所述超图,具体包括:根据所述采样点的位置、所述障碍物的位置、所述移动机器人在水平面上的正投影的外接圆半径、以及卡尔曼车体的运动学模型,建立所述超图。
另外,所述获取多条预测避障路径,具体包括:根据所述移动机器人的当前速度和所述预测加速度,预测所述移动机器人每间隔预设时长所处的位置,得到多个预测位置;依次连接多个所述预测位置,形成所述预测避障路径。
另外,所述对所述预测避障路径进行采样、得到多个采样点,具体包括:获取所述预测避障路径中的全部预测位置,将所述预测位置作为所述采样点。将预测位置作为采样点,相对于随机采样,在进行梯度下降优化迭代后,其更容易得到全局的最优解,进一步的提升路径规划的效果。
另外,所述梯度下降优化算法包括列文伯格-马夸尔特算法。
附图说明
图1是本发明第一实施方式所提供的避障方法的程序流程图;
图2是本发明第一实施方式所提供的避障方法中获取预测避障路径的程序流程图;
图3是本发明第一实施方式所提供的避障方法中获取预测避障路径的示意图;
图4是本发明第二实施方式所提供的移动机器人的结构示意图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚,下面将结合附图对本发明的各实施方式进行详细的阐述。然而,本领域的普通技术人员可以理解,在本发明各实施方式中,为了使读者更好地理解本申请而提出了许多技术细节。但是,即使没有这些技术细节和基于以下各实施方式的种种变化和修改,也可以实现本申请所要求保护的技术方案。
本发明的第一实施方式涉及一种避障方法。具体流程如图1所示,包括以下步骤:
步骤S101:获取多条预测避障路径。
具体的,如图2所示,本步骤包括以下子步骤:
步骤S201:获取移动机器人的当前速度。
具体的,在本实施方式中,当前速度包括当前线速度和当前角速度。
步骤S202:获取移动机器人可达到的最大加速度。
具体的,在本实施方式中,最大加速度包括最大线加速度和最大角加速度。可以理解的是,最大加速度根据移动机器人驱动电机的物理性质决定。
步骤S203:根据预设规则获取多个小于或等于最大加速度的预测加速度。
具体的,预设加速度至少包括预设线加速度和预设角加速度。
进一步的,在本步骤中,预设规则为在移动机器人的加速度的可取值范围(即大于或等于零且小于或等于最大加速度)内按照预设间隔均匀采样,得到多个小于或等于最大加速度的预测加速度。可以理解的是,上述仅为本实施方式中预设规则的一种具体的举例说明,并不构成限定,在本发明的其它实施方式中,也可以是通过其它方法获取,如在加速度的可取值范围内随机取样等,在此不进行一一列举。
步骤S204:计算移动机器人以当前速度和各个预测加速度运动预设时长后所处的多个第一预测位置和多个预测速度。
具体的,在本步骤中,首先建立基于地图的绝对坐标系,根据移动机器人的当前线速度和预测线加速度求取预测线速度;根据移动机器人的当前角速度和预测角加速度求取预测角速度。
进一步的,如图3所示,1号位置为移动机器人的当前位置,2号位置为移动机器人根据各个预测加速度运动预设时长后所处的多个第一预测位置。
步骤S205:连接移动机器人的当前位置和各个第一预测位置,形成多条第一预测避障路径。
具体的,如图3所示,连接1号位置和各个2号位置,即可得到多条第一预测避障路径。
步骤S206:判断各个第一预测避障路径是否大于预设长度,若是,则执行步骤S207,若否,则执行步骤S208。
步骤S207:将第一预测避障路径作为预测避障路径。
步骤S208:根据预测速度计算、再次间隔预设时长后,移动机器人所处的第二预测位置。
具体的,如图3所示,预测完2号位置后,根据预测速度计算、再次间隔预设时长后,即总间隔两倍的预设时长后,移动机器人所处的第二预测位置,即图中的3号位置。
步骤S209:连接第一预测避障路径和第二预测位置,形成新的第一预测避障路径,并再次执行步骤S206。
具体的,如图3所示,连接各个第一预测避障路径中的第一预测位置(2号位置)和第二预测位置(3号位置),即可形成新的第一预测避障路径。
具体的,在本步骤中,连接第一预测避障路径和第二预测位置,形成新的第一预测避障路径。根据预设时长逐渐延长第一预测避障路径的长度,直至第一预测避障路径大于预设长度,从而避免预测避障路径长度过长而对预测避障路径的评分产生干扰,提升预测避障路径评分的准确性,并提升路径选择的质量。
步骤S102:对各条预测避障路径进行图优化,得到多条优化避障路径。
具体的,在本步骤中,首先根据各条预测避障路径、建立与预测避障路径一一对应的超图;然后根据梯度下降优化算法对各个超图进行图优化,得到多条优化避障路径。
进一步的,在本步骤中,在建立与预测避障路径一一对应的超图的过程中,首先对各条预测避障路径进行采样,得到多个采样点,然后根据采样点的位置、障碍物的位置、以及移动机器人在水平面上的正投影的外接圆半径,建立超图。可以理解的是,上述仅为本实施方式中的一种具体的举例说明,并不构成限定,在本发明的其它实施方式中,除根据采样点的位置、障碍物的位置、以及移动机器人在水平面上的正投影的外接圆半径外,还可以是根据其它的数据建立超图,例如卡尔曼车体的运动学模型等。即根据采样点的位置、障碍物的位置、移动机器人在水平面上的正投影的外接圆半径、以及卡尔曼车体的运动学模型,建立超图。
需要说明的是,在本实施方式中,在获取到采样点的位置、障碍物的位置、移动机器人在水平面上的正投影的外接圆半径、以及卡尔曼车体的运动学模型后,依据预设约束条件,建立超图。预设约束条件包括但不限于:
其中,其中,p
i、p
i+1、p
i+2为相邻的所述采样点,n为所述采样点的个数,dt
1为所述移动机器人自p
i采样点运动至p
i+1采样点的预测时长,dt
2为所述移动机器人自p
i+1采样点运动至p
i+2采样点的预测时长,acc为所述移动机器人自p
i采样点运动至p
i+2采样点过程中的平均加速度,acc
max为所述移动机器人的最大加速度,error
acc为所述平均加速度acc的代价函数,dist为所述预测避障路径与所述障碍物的最小距离,radius为所述移动机器人在水平面上的正投影的外接圆半径,k为预设常数,error
obs为所述障碍物的代价函数,vel为所述移动机器人自p
i采样点运动至p
i+1采样点的过程中的平均速度,vel
max为所述移动机器人的最大线速度,error
vel为所述平均速度vel的代价函数,error
viaDist为所述采样点的代价函数;p
diff为自p
i采样点指向p
i+1采样点的向量,
为所述向量p
diff的纵坐标,
为所述向量p
diff的横坐标,
和
分别为p
i采样点和p
i+1采样点处的向量角度,error
car为卡尔曼车体的运动学模型的代价函数。
优选的,在本实施方式中,采样点为步骤S101中所述的预测位置,即将预测位置作为采样点。与随机采样相比,将预测位置作为采样点,在进行梯度下降优化迭代后,其更容易得到全局的最优解,进一步的提升路径规划的效果。
进一步的,在本实施方式中,梯度下降优化算法为列文伯格-马夸尔特算法。可以理解的是,梯度下降优化算法为列文伯格-马夸尔特算法仅为本实施方式中的一种具体的举例说明,并不构成限定,在本发明的其它实施方式中,也可以是批量梯度下降、随机梯度下降或者小批量梯度下降等其它算法,在此不进行一一列举,具体可以根据实际需要进行灵活的设定。
步骤S103:评估各条优化避障路径的评估得分。
具体的,在本步骤中,获取各个预设轨迹与障碍物的最小距离,并根据最小距离获取各个预设轨迹对应的障碍得分,并根据障碍得分计算评估得分。其中障碍得分等于最小距离与预设障碍偏置系数的乘积,预设障碍偏置系数为大于零的常量。
优选的,在本实施方式中,还可以获取各个预测轨迹与目标位置的最小位置差值,根据所述最小位置差值获取位置得分,并根据位置得分和障碍得分共同计算评估得分。其中,位置得分等于最小位置差值与预设位置偏置系数的乘积,预设位置偏置系数为大于零的常量。
更优的,在本实施方式中,还可以获取各个预测轨迹与预设轨迹(即移动机器人检测到障碍物前的预设移动轨迹)的最小轨迹偏差,根据最小轨迹偏差获取轨迹得分,并根据位置得分、障碍得分和轨迹得分共同计算评估得分。其中,轨迹得分等于最小轨迹偏差与预设轨迹偏置系数的乘积,预设轨迹偏置系数为大于零的常量。
此外,在本实施方式中,还可以根据各个预测速度评估各个预测避障路径的速度得分,根据速度得分计算评估得分,并根据位置得分、障碍得分、轨迹得分和速度得分共同计算评估得分。其中,速度得分等于预测速度与当前速度的速度差值、与预设速度偏置系数的乘积,预设速度偏置系数为大于零的常量。
进一步的,在本实施方式中,还可以获取移动机器人沿当前路径行驶至目标位置时的姿态角作为目标姿态角。获取移动机器人沿各个预测避障路径移动至各个预测避障路径的终点位置时的姿态角作为预测姿态角。计算目标姿态角和预测姿态角的姿态角差值;将姿态角差值与预设姿态偏置系数的乘积作为姿态得分,预设姿态偏置系数为大于零的常量;根据位置得分、障碍得分、轨迹得分、速度得分和姿态得分共同计算评估得分。其中,姿态角即为移动机器人的朝向与绝对坐标系横轴正向的夹角。
可以理解的是,上述根据位置得分、障碍得分、轨迹得分、速度得分和姿态得分共同计算评估得分仅为本实施方式中的一种具体的举例说明,并不构成限定,在本发明的其他实施方式中,也可以是根据其中的任意一者或多者计算评估得分,或者是根据其它的参数计算评估得分,在此不进行一一列举。
其中,根据位置得分、障碍得分、轨迹得分、速度得分和姿态得分共同计算评估得分的公示表示如下:
其中,cost为预设路径的成本,costpathDis为轨迹得分、costgoalDis为位置得分、costoccdist为障碍得分、costvel为速度得分、costangle为姿态得分,pathBias为预设轨迹偏置系数、goalBias为预设位置偏置系数、occBias为预设障碍偏置系数、velBias为预设速度偏置系数、angleBias为预设姿态偏置系数,pathDis为各个预测轨迹与预设轨迹的最小轨迹偏差、goalDis为各个预测轨迹与目标位置的最小位置差值、occdist为各个预测轨迹与障碍物的最小距离,anglegloble为目标姿态角、angleplanner为预测姿态角,velmax为预测速度、vel当前速度。最后根据cost确定各个预测避障路径的评估得分,cost越低则评估得分越高。
步骤S104:获取评估得分最高的优化避障路径作为目标路径。
步骤S105:控制所述移动机器人沿目标路径移动。
具体的,在本步骤中,将目标路径对应的预测速度发送至移动机器人的控制器,控制器根据预测速度控制电机、使移动机器人沿目标路径移动。
与现有技术相比,本发明第一实施方式所提供的避障方法根据移动机器人的当前速度和多个预测加速度获取的多条预测避障路径的长度均大于预设长度,而预设长度大于或等于移动机器人与障碍物的距离,因此,多条预测避障路径均大于移动机器人与障碍物的距离,从而避免移动机器人停在障碍物前、而不绕过障碍物,使得移动机器人可以有效的绕过障碍物。此外,获取评估得分最高的预测避障路径作为目标路径,可以达到最优的行驶效果。此外,在进行预测避障路径的评估得分的过程中,加入速度得分和姿态得分进行评估,从而有效的提升了得分评估的准确性,进而提升路径选择的质量。
本发明第二实施方式涉及一种移动机器人,如图4所示,包括:至少一个处理器401;以及,与至少一个处理器401通信连接的存储器402;其中,存储器402存储有可被至少一个处理器401执行的指令,指令被至少一个处理器401执行,以使至少一个处理器401能够执行如上述的避障方法。
其中,存储器402和处理器401采用总线方式连接,总线可以包括任意数量的互联的总线和桥,总线将一个或多个处理器401和存储器402的各种电路连接在一起。总线还可以将诸如外围设备、稳压器和功率管理电路等之类的各种其他电路连接在一起,这些都是本领域所公知的,因此,本文不再对其进行进一步描述。总线接口在总线和收发机之间提供接口。收发机可以是一个元件,也可以是多个元件,比如多个接收器和发送器,提供用于在传输介质上与各种其他装置通信的单元。经处理器401处理的数据通过天线在无线介质上进行传输,进一步,天线还接收数据并将数据传送给处理器401。
处理器401负责管理总线和通常的处理,还可以提供各种功能,包括定时,外围接口,电压调节、电源管理以及其他控制功能。而存储器402可以被用于存储处理器401在执行操作时所使用的数据。
本发明第三实施方式涉及一种计算机可读存储介质,存储有计算机程序。计算机程序被处理器执行时实现上述方法实施例。
即,本领域技术人员可以理解,实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成,该程序存储在一个存储介质中,包括若干指令用以使得一个设备(可以是单片机,芯片等)或处理器(processor)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括:U盘、移动硬盘、只读存储器(ROM,Read-OnlyMemory)、随机存取存储器(RAM,Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
本领域的普通技术人员可以理解,上述各实施方式是实现本发明的具体实施例,而在实际应用中,可以在形式上和细节上对其作各种改变,而不偏离本发明的精神和范围。