CN116225004A - 一种六轮独立驱动独立转向机器人的避障方法 - Google Patents
一种六轮独立驱动独立转向机器人的避障方法 Download PDFInfo
- Publication number
- CN116225004A CN116225004A CN202310103879.1A CN202310103879A CN116225004A CN 116225004 A CN116225004 A CN 116225004A CN 202310103879 A CN202310103879 A CN 202310103879A CN 116225004 A CN116225004 A CN 116225004A
- Authority
- CN
- China
- Prior art keywords
- obstacle
- robot
- obstacle avoidance
- track
- distance
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 67
- 230000008569 process Effects 0.000 claims abstract description 33
- 230000003068 static effect Effects 0.000 claims abstract description 10
- 238000009499 grossing Methods 0.000 claims abstract description 4
- 230000033001 locomotion Effects 0.000 claims description 61
- 230000004888 barrier function Effects 0.000 claims description 16
- 230000001133 acceleration Effects 0.000 claims description 7
- 238000005457 optimization Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 7
- 238000004364 calculation method Methods 0.000 claims description 6
- 238000013459 approach Methods 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 claims description 5
- 230000002829 reductive effect Effects 0.000 claims description 5
- 230000009471 action Effects 0.000 claims description 4
- 230000000452 restraining effect Effects 0.000 claims description 4
- 101100039010 Caenorhabditis elegans dis-3 gene Proteins 0.000 claims description 3
- 101100030351 Schizosaccharomyces pombe (strain 972 / ATCC 24843) dis2 gene Proteins 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 claims description 3
- 238000011160 research Methods 0.000 description 12
- 238000005070 sampling Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000036961 partial effect Effects 0.000 description 2
- 239000004566 building material Substances 0.000 description 1
- 230000009194 climbing Effects 0.000 description 1
- 238000011217 control strategy Methods 0.000 description 1
- 230000007123 defense Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000009826 distribution Methods 0.000 description 1
- 238000009776 industrial production Methods 0.000 description 1
- 238000011835 investigation Methods 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 239000000463 material Substances 0.000 description 1
- 230000002265 prevention Effects 0.000 description 1
- 238000005096 rolling process Methods 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000007704 transition Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0219—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory ensuring the processing of the whole working surface
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
一种六轮独立驱动独立转向机器人的避障方法,包括求解避障路径和基于模糊决策的避障策略;当传感器检测到障碍物后,将障碍物信息发送到避障控制器和避障规划器,然后通过实际环境的约束求解最佳路径点,通过轨迹平滑和多项式拟合后得到最佳路径,将最佳路径离散为坐标点后输出,在输出坐标点后进行避障策略的选择,根据策略进行避障,整个避障过程结束。本发明解决了六轮独立驱动独立转向机器人在轨迹跟踪过程中遇到静态障碍物和动态障碍物情况下的避障问题,并且针对实际的情况和物理模型已在机器人上进行验证,此外,本发明还拓展了机器人在避障过程中的各种不同类情况和新的机器人模型,为其他避障方法提供了一定的参考。
Description
技术领域
本发明属于轮式移动机器人运动控制技术领域,具体涉及一种轮式机器人避障方法和避障策略。
背景技术
随着智能控制技术的发展,移动机器人在工业生产、军事国防和教学科研等方面有了越来越重要的应用。移动机器人主要分为履带式移动机器人、腿足式移动机器人和轮式移动机器人等,而轮式移动机器人相较于履带式和腿足式机器人机动灵活、结构简单,控制难度较低,并且具有更强的承载能力,因此轮式移动机器人的运动控制研究是未来智能机器人研究的重要方向之一。
在针对移动机器人运动控制的研究中,机器人的路径规划和轨迹跟踪是其中的重要问题。但是在规划和跟踪过程中,静态障碍物或者动态障碍物的出现是实际控制中非常容易出现的情况,而且自主避障能力是移动机器人智能化程度的重要指标,也是移动机器人在复杂环境中稳定、安全、高效完成任务的重要保证。因此,关于机器人智能避障的研究也一直是研究的热门问题之一。
六轮独立驱动独立转向机器人是一种新的轮式机器人模型,与传统轮式移动机器人相比,六轮独立驱动独立转向机器人单体负载能力、爬坡能力和驱动能力等方面都有了一定的进步。所以六轮独立驱动独立转向机器人在多种地形的适应性更强,对货物运输、救援物资、建筑材料等在特殊环境下的运输等有重要意义。除此外,六轮独立驱动独立转向机器人采用电动机驱动,大大减小噪声,还可根据工况调整驱动轮数量,提高续航能力。由于其负载能力的大幅度增强,可以在平台上加装机械臂等进行不同情况的任务处理。并且,由于其高灵活性和高机动性,可以适应多种极限工况,在抢险救灾、地质勘查和其他特种领域都有很高的应用性。
模糊决策是指在模糊环境下进行决策的数学理论和方法。严格地说,现实的决策大多是模糊决策。模糊决策的研究开始较晚,但涉及的面很广,还没有明确的范围。常用的模糊决策方法有模糊排序、模糊寻优和模糊对策等。模糊寻优主要指的是在给定方案集及各种目标函数和限制条件以后,寻求其中的最优方案。若目标函数或约束条件是模糊的,这时的最优化就称为模糊寻优。目标函数模糊化的一种途径是以模糊数作为目标函数值,通过模糊数的分析、运算来寻求条件极值。约束条件的模糊化是将约束定义成模糊集合。在线性规划中这样的推广导致模糊线性规划的研究,其结果是使普通的线性规划应用范围更广,能更加灵活地适应各种不同的情况。
避障策略的主要作用是当机器人通过传感器检测到环境中出现障碍物并生成避障路径后,根据环境中的障碍物信息进行实时避让。通常实际环境中的障碍物运动情况较为复杂,所以在生成避障路径后在未来每一时刻都需要进行调整。避障策略是在机器人智能避障过程中是非常重要的研究内容,合适策略的选择直接影响到机器人避障效果。
当前避障的研究主要针对在机器人前进方向上出现障碍物的情况,没有分析其他情况。因此,针对机器人运动过程中各个方向出现障碍物的情况进行分析,对生成合适的避障路径和有效的避障策略的研究在智能机器人避障研究方面有非常重要的意义。
发明内容
针对六轮独立驱动独立转向机器人在轨迹跟踪过程中的现有避障技术存在的问题,本发明提供一种六轮独立驱动独立转向机器人的避障方法,以求解避障路径和根据障碍物不同情况的避障策略,解决在运动过程中出现不同动/静态障碍物时的避障问题。
本发明六轮独立驱动独立转向机器人的避障方法,采用以下方案:
包括求解避障路径和基于模糊决策的避障策略;当传感器检测到障碍物后,将障碍物信息发送到避障控制器和避障规划器,然后通过实际环境的约束(距离约束、控制量约束等)求解最佳路径点,通过轨迹平滑和多项式拟合后得到最佳路径,将最佳路径离散为坐标点后输出,在输出坐标点后进行避障策略的选择,根据策略进行避障,整个避障过程结束。
一.所述求解避障路径的过程是:
在规划避障路径的过程中,根据机器人的尺寸和障碍物的尺寸进行膨胀处理,按照障碍物和机器人的必需安全裕度和障碍物的信息对障碍物进行膨胀,因为存在安全裕度,所以将机器人的膨胀范围也加入到此部分中(规划避障路径的过程中);考虑到存在障碍物较大、机器人可能从障碍物中间穿过的情况,对障碍物进行分割处理,在障碍物轮廓处设置障碍物分割点将进行分割处理;
在求解避障路径中,设计避障惩罚函数和设计偏差参考轨迹的代价函数;避障惩罚函数用于避障函数的求解和约束中,通过机器人质心坐标与障碍物分割点的距离偏差来调节函数的大小,距离越近,函数值越大;机器人偏离参考轨迹的代价函数作为求解的一个约束条件;
避障路径的求解是直接求得最佳路径点坐标,所以定义:
ξ=[xa ya]T,
其中,xa ya为避障路径坐标点。
为了提高避障的准确性,定义距离惩罚函数Dobs,在避障函数中加入软约束其中,K为极大的正整数,这是针对动态障碍物在机器人移动过程中突然靠近机器人的情况,机器人与障碍物距离较近时,软约束项较大,权重影响大;当机器人与障碍物距离较远时,软约束项较小,权重影响小;
根据上述惩罚项和约束,设计基于数学优化的方法的避障规划器,设计如下:
其中:P为权重矩阵;项为MPC的控制量,在避障规划器中的作用为约束避障路径,保证规划器求得的避障路径符合机器人运动学模型约束;避障规划器求解的结果ξ即为机器人的避障路径;Pobs为避障惩罚函数,Dobs距离惩罚函数。在约束条件中,/>为MPC控制量,保证结果符合机器人的运动模型,γ为偏差参考轨迹的代价函数。
对避障规划器求解的避障路径通过曲线拟合进行处理,曲线拟合将平滑后的轨迹点生成符合机器人约束(如机器人位置要求是连续的,横摆角要求是一阶连续曲线、加速度约束要求二阶连续曲线等)的轨迹点。
所述避障惩罚函数Pobs如下:
其中,pobs是避障惩罚函数权重系数,(xi,yi)是障碍物分割点坐标,∈是极小的正数,避免分母出现0的情况;
所述偏差参考轨迹的代价函数如下:
其中,xmin,xmax,ymin,ymax是全局地图坐标范围,xr,yr是参考轨迹坐标,xa,ya为避障路径坐标点。
所述距离惩罚函数为:
其中,xobs,yobs为障碍物质心坐标,xrob,yrob为机器人质心坐标,dobs为距离惩罚函数权重系数。
所述曲线拟合,选取n次多项式拟合作为拟合曲线,基于最小二乘法原理进行拟合,拟合函数形式如下:
X=a1xn+a2xn-1+a3xn-2+…+anx+an+1,
Y=b1yn+b2yn-1+b3yn-2+…+bny+bn+1,
其中,a=[a1,a2…an+1],b=[b1,b2…bn+1]分别是多项式X和Y系数;选取n=4,即拟合的阶数为4次,X和Y是拟合后的曲线坐标。
二.所述基于模糊决策的避障策略如下所述:
机器人避障策略的选取是对障碍物轨迹和机器人轨迹进行预测,预测是否会发生碰撞,然后进行策略选择;对于静态障碍物视为运动速度和加速度为0的情况,当超过安全距离阈值,进行避障轨迹规划,规划完成避障轨迹后再进行轨迹跟踪即可;
对于动态障碍物来说,若障碍物的移动轨迹为直线,通过阈值函数的计算结果,直接判断在一定时间后是否会发生碰撞,然后利用约束求解最优路径;若障碍物的移动轨迹为曲线,根据障碍物位移对时间的微分函数来预测一定时间后的轨迹,根据与机器人轨迹相交时的运动时间判断机器人在此时间下的运动情况,利用机器人与障碍物的质心距离判断是否发生碰撞,然后求解最优路径。
在机器人避障决策的选择中采用模糊决策中的非对称模型,将机器人的避障情况作为约束条件,机器人避障策略的选择作为目标函数;在非对称模型中,把接受约束作为先决条件,目标与约束二者的地位不是对称的;给定论域X上的目标函数f(x)和X上的约束条件模糊集合D,所谓在约束D之下极大化f的最优解m,就是X上的一个模糊子集,它具有隶属函数当等式右端的集合为空集时,μm(x)等于0,给定论域X即为所有情况的合集,机器人遇到的当前情况即为约束D,目标函数即为机器人的避障策略;
机器人通过将遇到障碍物时环境信息和障碍物信息作为约束条件,求解目标函数,选择合适的避障策略。
所述动态障碍物,具体分为以下三种情况:
(1)第一种情况,障碍物运动方向与机器人轨迹运动方向相同;
此时避障轨迹的规划是使机器人从障碍物的左侧或者右侧选取最优轨迹进行超越,超越后再回归到原轨迹进行跟踪;进行避障轨迹重规划后将新的轨迹点输出至轨迹跟踪控制器,根据MPC算法会计算机器人的控制量进行轨迹跟踪;但当机器人质心位置与障碍物质心位置处在同一水平线时,在速度项中加入速度反馈比例控制,使机器人进行明显的超车动作并尽快回归到原有轨迹,同时加入微分控制防止超调;
其中,vmpc是MPC轨迹跟踪控制器计算的机器人控制量速度,kp1和kd1是速度反馈比例控制和微分控制系数,e11是机器人避障轨迹距离原轨迹的偏差量,tk是机器人运动的当前时刻,td1max是机器人运动到d1max的位置时刻,td1min是d1min所示位置时刻;
(2)第二种情况,障碍物运动方向与机器人运动方向相交,且障碍物移动速度较快;
此种情况在于判断机器人跟踪轨迹和障碍物移动轨迹在相交点是否会发生碰撞,判断依据与机器人和障碍物的移动速度有关;若障碍物移动速度相对较快,机器人在到达碰撞点前进行适当减速并待障碍物越过相交点后从障碍物后方超越,然后再回归到原轨迹进行跟踪;机器人与障碍物的距离先小后大,距离较小的时候需要减小速度,在MPC控制量的基础上加入速度反馈比例控制,距离较大时恢复到MPC控制,同时加入微分控制防止跟踪原轨迹时超调;
其中,vmpc是MPC计算的机器人控制量速度,kp2和kd2是速度反馈比例控制和微分控制系数,e21是机器人避障轨迹距离原轨迹的偏差量,e2是机器人质心位置与障碍物质心位置的距离,ddis2是机器人质心与障碍物质心在x方向绝对距离,d2min和d2max分别是机器人质心与障碍物的距离阈值;
(3)第三种情况,障碍物运动方向与机器人运动方向相交,且障碍物移动速度较慢;
机器人在到达碰撞点前进行适当加速并在障碍物未越过相交点时从障碍物前方超越,然后再回归到原轨迹进行跟踪;机器人与障碍物的距离先小后大,距离较小的时候需要增加速度,在MPC控制量的基础上加入速度反馈比例控制,加快进行避障,距离较大的时恢复到MPC控制,同时加入微分控制防止跟踪原轨迹时超调;
其中,vmpc是MPC计算的机器人控制量速度,kp3和kd3是速度反馈比例控制和微分控制系数,e31是机器人避障轨迹距离原轨迹的偏差量,e3是机器人质心位置与障碍物质心位置的距离,ddis3是机器人质心与障碍物质心在x方向绝对距离,d3min和d3max分别是机器人质心与障碍物的距离阈值。
本发明提出的避障方法和避障策略,解决了六轮独立驱动独立转向机器人在轨迹跟踪过程中遇到静态障碍物和动态障碍物情况下的避障问题。并且针对实际的情况和物理模型,本发明所提出的发明内容已在机器人上进行验证。除此外,本发明还拓展了机器人在避障过程中的各种不同类情况和新的机器人模型,为其他避障方法提供了一定的参考。
与现有技术相比,本发明具有以下特点:
1.本发明提供了在多种障碍物情形下的避障情况,并提出了相对应的避障方法和避障策略。结合障碍物的运动情况,在规划完成避障路径后,通过机器人自身速度的调节和合适避障策略的选择综合控制进行避障,符合现实情况下遇到的各种复杂情形,提出的方案也可以解决各种不同类型的避障问题。
2.本发明使用的是六轮独立驱动独立转向模型。六轮独立驱动独立转向机器人由于其复杂的控制特性以及与二轮、四轮机器人相比更大的控制量运算等特点,在现阶段轮式移动机器人的研究中研究内容较少。在规划避障路径时由于六轮机器人体积较大,需要同时考虑机器人转向情况和路径情况进行综合控制。通过全局定位***和机器人各轮轴编码器判断机器人的运动状态,利用单轮速度和转向精细控制保证机器人运动的准确性。
3.采用模糊决策的方法对机器人的避障策略进行选择,将机器人遇到的各种类型的避障问题作为约束,机器人的避障策略作为目标函数,利用模糊寻优中的非对称模型解决在不同类型障碍物的问题下的避障问题,降低了运算难度但保证了避障可靠性。
附图说明
图1是本发明中的避障流程示意图。
图2是障碍物膨胀分割示意图。
图3是部分段轨迹拟合结果示意图。
图4是机器人同方向障碍物避障示意图。
图5是轨迹与机器人相交障碍物(速度较快)避障示意图。
图6是轨迹与机器人相交障碍物(速度较慢)避障示意图。
图7是前进方向碰撞示意图。
图8是轨迹相交方向碰撞示意图。
图9是六轮全向移动机器人运动学模型示意图。
图10是碰撞状态示意图。
图11是潜在碰撞状态示意图。
图12是安全状态示意图。
图13是避障效果图。
具体实施方式
本发明提出的六轮独立驱动独立转向机器人的避障路径求解方法和基于模糊决策的避障策略选择,将机器人避障与轨迹跟踪控制相结合,通过对机器人位置的跟踪反馈提高了避障精度,解决了在静态障碍物和动态障碍物环境下的避障问题。需要解释的是,本发明中障碍物信息的获取由深度相机和激光雷达传感器获得的点云数据后进行处理并发送至机器人,本发明主要针对获取障碍物信息后的避障过程,关于障碍物信息的获取不作详细描述。
除此外,本发明还提供了在移动机器人避障领域六轮独立驱动机器人这一新模型和三种避障问题这一新情形,可以以此为基础进一步探索移动机器人更多的避障类型的研究内容,具有很高的推广价值。
本发明提出的六轮独立驱动独立转向机器人的避障方法和避障策略,具体流程如图1所示,具体包括以下步骤。
一.建立六轮运动学模型
机器人在获取避障路径后需要对路径进行跟踪,轨迹跟踪过程是根据机器人的运动学模型计算最优控制序列。假设机器人是具有不可变形车轮的刚体,侧滑角β=0rad。本发明所用MPC轨迹跟踪控制器进行避障路径跟踪,直接控制量为中轮速度和左前轮转角,所以在运动学模型分析中以中轮速度和左前轮转角作为控制量。因此,在图9所示的坐标系中,机器人的运动学模型如下:
其中,表示机器人中心的位置和偏航角,[v δl]T为控制量输入,即机器人速度和左前轮转角。δl、δr分别为左前轮转角和右前轮转角,由于左后轮转角和右后轮转角大小与前轮相同,只是方向相反,在此不做赘述。L为前轮与中轮轴间距离(也为中轮与后轮轴间距离),D为机器人宽度。本发明对机器人宽度不做简化处理。根据角度关系,左右轮转角的关系为:
可由此推得,转弯半径和前轮偏转角关系为:
六轮全向移动机器人后轮采用与前轮转角大小相同,方向相反的偏转朝向,故后轮角度问题在此不做赘述。
机器人受到非完整约束,即机器人的轮子不会滑动,只有纯滚动运动,所以此约束通过以下等式描述:
作为轮式移动机器人,状态空间模型最适合进行模型预测。由以上可得,基于机器人运动学模型的状态空间表达式为:
此运动学模型为非线性模型,本发明所用模型预测控制算法为机器人轨迹跟踪控制器的底层算法,由模型预测控制进行规划跟踪后通过滑模控制算法进行误差补偿,所以为提高控制效率,需要将非线性运动学模型简化为线性模型进行处理,并进行离散化,然后进行预测控制。
通过泰勒一阶展开的方法对模型进行线性化处理,机器人运动过程中,期望达到的目标状态为参考状态,所以在参考状态处进行泰勒一阶展开:
用前向欧拉法在采样时间T内对公式离散化后可得:
其中,I为单位矩阵。以上为对机器人进行运动学建模后线性化和离散化后的结果,将作为MPC的模型输入。
二.障碍物轨迹预测
本发明关于障碍物轨迹的预测仅用于判别机器人与障碍物的状态,并不用于准确预测障碍物轨迹。
机器人在避碰的过程中,需要加入障碍物轨迹预测环节。若在未来一定时间内,机器人与障碍物不会出现碰撞窗内,则不需要进行避障;反之,需要根据机器人与障碍物的位置状态选择合适的避障策略。因此,将机器人与障碍物的位置情况分为碰撞状态、潜在碰撞状态和安全状态三种来进行碰撞预测,如图10、图11和图12所示。机器人的轨迹作为参考轨迹输入,主要是判断障碍物的轨迹情况。利用拉格朗日基本多项式和其截断误差公式可分析得到障碍物的运动预测计算公式为:
其中,[xkt,ykt]代表kt个采样时刻后预测的障碍物位置,[xk,yk]、[xk+1,yk+1]、[xk+2,yk+2]分别代表已经的tk时刻、tk+1时刻和tk+2时刻观测到的障碍物的位置。通过障碍物运动预测公式,可以根据已知时刻的障碍物位置预测未来障碍物的运动轨迹,从而进行碰撞判断,选择避障策略。
通过障碍物轨迹预测来判断机器人与障碍物状态,主要是利用在一定时间后机器人与障碍物的距离进行判断。若机器人与障碍物的距离小于安全裕度距离,则将此状态设置为碰撞状态,根据碰撞类型选取合适的避障策略;若机器人与障碍物的距离略大于安全裕度距离,则设置为潜在碰撞状态,在下一时刻继续进行障碍物轨迹预测和状态分析;若机器人与障碍物的距离远大于安全裕度,则设置为安全状态,不会认为机器人会与障碍物发生碰撞。
三.阈值函数进行判断
关于不同的避障类型设计不同的阈值函数进行判断是否会发生碰撞,针对第一种情况(碰撞状态)设计安全距离阈值函数进行判断,第二种和第三种情况(潜在碰撞状态和安全状态)设计碰撞阈值函数进行判断。
安全距离阈值函数:
针对障碍物运动方向与机器人运动方向相同的情况,设计安全距离阈值函数来判断是否需要规划无碰撞安全路径,在完成避障动作后机器人回归到原始轨迹继续进行轨迹跟踪,如图7所示,定义距离阈值函数如下:
其中,vc和vobs分别代表机器人运动速度和障碍物运动速度,ac和aobs分别代表机器人最大加速度和障碍物最大加速度,tc代表机器人控制器发出指令到底层运动模块反应的时间,ds1代表最小安全距离和机器人安全膨胀裕度。
碰撞阈值函数:
针对障碍物运动方向与机器人运动方向相交的情况,设计碰撞阈值函数来判断是否需要规划无碰撞安全路径。主要通过当前时刻和预测未来t个采样时间步长的机器人与障碍物质心距离来判断(第t时刻为障碍物运动到与机器人轨迹相交点的时刻),如图8所示。定义碰撞阈值函数如下:
Dth2=min(di(k),di(k+1),…di(k+t))+ds2
其中,di(k)表示k时刻机器人与障碍物质心距离,ds2表示机器人安全膨胀裕度。
四.求解避障路径
求解避障路径的过程是当传感器检测到障碍物后,将障碍物信息发送到避障控制器和避障规划器,然后通过实际环境的约束(距离约束、控制量约束等)求解最佳路径点,通过轨迹平滑和多项式拟合后得到最佳路径,将最佳路径离散为坐标点后输出,在输出坐标点后进行避障策略的选择(具体策略在下文阐述),根据策略进行避障,整个避障过程结束。避障全过程如图1所示。
在规划避障的路径的过程中,需要根据机器人的尺寸和障碍物的尺寸进行一定的膨胀处理,按照障碍物和机器人的必需安全裕度和障碍物的信息对障碍物进行膨胀,因为存在安全裕度,所以将机器人的膨胀范围也加入到此部分中,所以机器人不需要进行膨胀处理。考虑到可能存在障碍物较大、机器人可能从障碍物中间穿过的情况,对障碍物进行分割处理,如图2所示。
在求解避障路径中,需要设计避障惩罚函数。避障惩罚函数主要用于避障函数的求解和约束中。基本思路是通过机器人质心坐标与障碍物分割点的距离偏差来调节函数的大小,距离越近,函数值越大。设计避障惩罚函数Pobs如下:
其中,pobs是避障惩罚函数权重系数,(xi,yi)是障碍物分割点坐标,∈是极小的正数,避免分母出现0的情况。
除了需要避障惩罚函数外,在避障函数的求解中,还需要设计机器人偏离参考轨迹的代价作为求解的一个约束条件,设计偏差参考轨迹的代价函数如下:
其中,xmin,xmax,ymin,ymax是全局地图坐标范围,xr,yr是参考轨迹坐标,xa,ya为避障路径坐标点。
避障路径的求解是直接求得最佳路径点坐标,所以定义:
ξ=[xa ya]T,
其中,xa ya为避障路径坐标点。
为了提高避障的准确性,定义距离惩罚函数Dobs,在避障函数中加入软约束其中,K为极大的正整数,此项主要针对动态障碍物在机器人移动过程中突然靠近机器人的情况。机器人与障碍物距离较近时,软约束项较大,权重影响大;当机器人与障碍物距离较远时,软约束项较小,权重影响小。距离惩罚函数为:
其中,xobs,yobs为障碍物质心坐标,xrob,yrob为机器人质心坐标,dobs为距离惩罚函数权重系数。避障规划器的主要控制目标是实现对障碍物的避让,并尽量减小与参考路径的偏差。所以根据上述惩罚项和约束,设计了基于数学优化的方法的避障规划器设计如下:
在避障函数的约束条件中,第一项为机器人控制量的硬约束,结合约束条件,是求解的关键。根据避障惩罚函数和参考路径的偏离情况,求出在此范围内的最优路径。避障规划器的采样时间与轨迹跟踪控制器采样时间相同,且在规划完成避障路径后采用短预测步长的轨迹跟踪策略,具备更好精度的模型预测轨迹跟踪控制器完全满足在路径重规划的要求,所以在实时性方面可以满足机器人的控制需求。
避障主要是在障碍物周围确定避障路径区域,然后寻找该区域中具体求解路径的坐标,即为避障路径。
在避障控制器算法中,所求得的最优求解序列是有限时域内与参考点距离偏差最小,求得的路径点是以离散点的形式给出。随着障碍物的情况和参考轨迹情况的变化,规划的局部轨迹参考点的数量和位置也会随之变化。若直接将此轨迹输入轨迹跟踪控制器,一些冗余重复的轨迹点会增加轨迹跟踪控制器的运算负担,而且不同参考点的分布密度不同,轨迹跟踪控制器很难根据离散的参考点完成轨迹跟踪的任务。
综合以上情况,有必要对避障控制器所规划的避障路径进行处理,实现规划器和轨迹跟踪控制器的顺利对接。避障规划器求解的避障路径主要为离散点的形式,且不考虑路径的平滑性等要求,因此主要通过曲线拟合进行处理,曲线拟合可以将平滑后的轨迹点生成符合机器人约束(如机器人位置要求是连续的,横摆角要求是一阶连续曲线、加速度约束要求二阶连续曲线等)的轨迹点。
在曲线拟合方面,选取n次多项式拟合作为拟合曲线,基于最小二乘法原理进行拟合,拟合函数形式如下:
X=a1xn+a2xn-1+a3xn-2+…+anx+an+1
Y=b1yn+b2yn-1+b3yn-2+…+bny+bn+1
其中,a=[a1,a2…an+1],b=[b1,b2…bn+1]分别是多项式X和Y系数。本文中,选取n=4,即拟合的阶数为4次,X和Y是拟合后的曲线坐标。图3中是重规划轨迹部分段拟合结果,图3中*为避障规划器中轨迹重规划算法经曲线平滑后的离散轨迹点,曲线为拟合后的轨迹曲线。
五.避障策略的选择
本部分主要分析机器人面临不同避障情况的避障策略选择。
本发明所述的避障问题主要研究面向六轮全向移动机器人在轨迹跟踪过程中出现障碍物时避障轨迹的规划和跟踪。因此,机器人针对不同类型障碍物需要进行不同避障轨迹和策略的选择。
在规划轨迹的同时,需要设计一定的控制策略来保证机器人更好的完成避障行为。对静态障碍物来说,可以将其视为动态障碍物速度为0的情况,规划完成避障轨迹后再进行轨迹跟踪即可。对动态障碍物来说,可以分为三种情况:
(1)第一种情况,障碍物运动方向与机器人轨迹运动方向相同
如图4所示,此时避障轨迹的规划主要是使机器人从障碍物的左侧或者右侧选取最优轨迹进行超越,超越后再回归到原轨迹进行跟踪。进行避障轨迹重规划后将新的轨迹点输出至轨迹跟踪控制器,根据MPC算法会计算机器人的控制量进行轨迹跟踪。但当机器人质心位置与障碍物质心位置处在同一水平线时,在速度项中加入速度反馈比例控制,使机器人进行明显的超车动作并尽快回归到原有轨迹。同时,加入微分控制防止超调。
其中,vmpc是MPC轨迹跟踪控制器计算的机器人控制量速度,kp1和kd1是速度反馈比例控制和微分控制系数,e11是机器人避障轨迹距离原轨迹的偏差量,tk是机器人运动的当前时刻,td1max是机器人运动到图4所示的d1max的位置时刻,td1min是图4中d1min所示位置时刻。
(2)第二种情况,障碍物运动方向与机器人运动方向相交,且障碍物移动速度较快
如图5所示,此种情况主要难点是判断机器人跟踪轨迹和障碍物移动轨迹在相交点是否会发生碰撞,主要判断依据与机器人和障碍物的移动速度有关。若障碍物移动速度相对较快,机器人在到达碰撞点前进行适当减速并待障碍物越过相交点后从障碍物后方超越,然后再回归到原轨迹进行跟踪。机器人与障碍物的距离先小后大,距离较小的时候需要减小速度,在MPC控制量的基础上加入速度反馈比例控制,距离较大时恢复到MPC控制。同时,加入微分控制防止跟踪原轨迹时超调。
其中,vmpc是MPC计算的机器人控制量速度,kp2和kd2是速度反馈比例控制和微分控制系数,e21是机器人避障轨迹距离原轨迹的偏差量,e2是机器人质心位置与障碍物质心位置的距离,ddis2是机器人质心与障碍物质心在x方向绝对距离,d2min和d2max分别是机器人质心与障碍物的距离阈值。
(3)第三种情况,障碍物运动方向与机器人运动方向相交,且障碍物移动速度较慢
如图6所示,机器人在到达碰撞点前进行适当加速并在障碍物未越过相交点时从障碍物前方超越,然后再回归到原轨迹进行跟踪。机器人与障碍物的距离先小后大,距离较小的时候需要增加速度,在MPC控制量的基础上加入速度反馈比例控制,加快进行避障,距离较大时恢复到MPC控制。同时,加入微分控制防止跟踪原轨迹时超调。
其中,vmpc是MPC计算的机器人控制量速度,kp3和kd3是速度反馈比例控制和微分控制系数,e31是机器人避障轨迹距离原轨迹的偏差量,e3是机器人质心位置与障碍物质心位置的距离,ddis3是机器人质心与障碍物质心在x方向绝对距离,d3min和d3max分别是机器人质心与障碍物的距离阈值。
上述避障策略加入速度反馈比例微分控制的主要原因是避障轨迹和原有轨迹的MPC轨迹跟踪算法不同,所以需要在交界处加入微分控制进行速度过渡,保证跟踪的准确性。由于避障轨迹较短,所以MPC轨迹跟踪控制器中采用短预测步长和权重矩阵的策略,相较于普通轨迹的跟踪提高运算效率和避障的时效性。
机器人避障策略的选取主要是对障碍物轨迹和机器人轨迹进行预测,预测是否会发生碰撞,然后进行策略选择。针对静态障碍物而言,可以视为运动速度和加速度为0的情况,当超过安全距离阈值,进行避障轨迹规划。
对于动态障碍物来说,若障碍物的移动轨迹为直线,通过上述阈值函数的计算结果,直接判断在一定时间后是否会发生碰撞,然后利用约束求解最优路径;若障碍物的移动轨迹为曲线,根据障碍物位移对时间的微分函数来预测一定时间后的轨迹,根据与机器人轨迹相交时的运动时间判断机器人在此时间下的运动情况,利用机器人与障碍物的质心距离判断是否发生碰撞,然后求解最优路径。
在机器人避障控制中,本发明提供了严格和***的方法考虑机器人的安全约束,并在具有反馈控制器的闭环中使用。
在机器人避障决策的选择中采用模糊决策中的非对称模型,将机器人的避障情况作为约束条件,机器人避障策略的选择作为目标函数。在非对称模型中,把接受约束作为先决条件,目标与约束二者的地位不是对称的。给定论域X上的目标函数f(x)和X上的约束条件模糊集合D,所谓在约束D之下极大化f的最优解M,就是X上的一个模糊子集,它具有隶属函数当等式右端的集合为空集时,μM(x)等于0,给定论域X即为所有情况的合集,机器人遇到的当前情况即为约束D,目标函数即为机器人的避障策略。
机器人通过将遇到障碍物时环境信息和障碍物信息作为约束条件,求解目标函数,选择合适的避障策略。
六.机器人进行避障
在进行完避障路径的规划和避障策略的选择后进行避障,效果如图13所示。可以看出,机器人较好的解决了避障问题,在遇到障碍物的情况下实现了避障。
Claims (8)
1.一种六轮独立驱动独立转向机器人的避障方法,其特征是:包括求解避障路径和基于模糊决策的避障策略;当传感器检测到障碍物后,将障碍物信息发送到避障控制器和避障规划器,然后通过实际环境的约束求解最佳路径点,通过轨迹平滑和多项式拟合后得到最佳路径,将最佳路径离散为坐标点后输出,在输出坐标点后进行避障策略的选择,根据策略进行避障,整个避障过程结束。
2.根据权利要求1所述的六轮独立驱动独立转向机器人的避障方法,其特征是:所述求解避障路径的过程是:
在规划避障路径的过程中,根据机器人的尺寸和障碍物的尺寸进行膨胀处理,按照障碍物和机器人的必需安全裕度和障碍物的信息对障碍物进行膨胀,因为存在安全裕度,所以将机器人的膨胀范围也加入到此部分中;考虑到存在障碍物较大、机器人可能从障碍物中间穿过的情况,对障碍物进行分割处理,在障碍物轮廓处设置障碍物分割点将进行分割处理;
在求解避障路径中,设计避障惩罚函数和设计偏差参考轨迹的代价函数;避障惩罚函数用于避障函数的求解和约束中,通过机器人质心坐标与障碍物分割点的距离偏差来调节函数的大小,距离越近,函数值越大;机器人偏离参考轨迹的代价函数作为求解的一个约束条件;
避障路径的求解是直接求得最佳路径点坐标,所以定义:
ξ=[xa ya]T,
其中,xa ya为避障路径坐标点;
为了提高避障的准确性,定义距离惩罚函数Dobs,在避障函数中加入软约束其中,K为极大的正整数,这是针对动态障碍物在机器人移动过程中突然靠近机器人的情况,机器人与障碍物距离较近时,软约束项较大,权重影响大;当机器人与障碍物距离较远时,软约束项较小,权重影响小;
根据上述惩罚项和约束,设计基于数学优化的方法的避障规划器,设计如下:
其中:P为权重矩阵;项为MPC的控制量,在避障规划器中的作用为约束避障路径,保证规划器求得的避障路径符合机器人运动学模型约束;避障规划器求解的结果ξ即为机器人的避障路径;Pobs为避障惩罚函数,Dobs距离惩罚函数。在约束条件中,/>为MPC控制量,保证结果符合机器人的运动模型,γ为偏差参考轨迹的代价函数;
对避障规划器求解的避障路径通过曲线拟合进行处理,曲线拟合将平滑后的轨迹点生成符合机器人约束的轨迹点。
6.根据权利要求2所述的六轮独立驱动独立转向机器人的避障方法,其特征是:所述曲线拟合,选取n次多项式拟合作为拟合曲线,基于最小二乘法原理进行拟合,拟合函数形式如下:
X=a1xn+a2xn-1+a3xn-2+…+anx+an+1,
Y=b1yn+b2yn-1+b3yn-2+…+bny+bn+1,
其中,a=[a1,a2…an+1],b=[b1,b2…bn+1]分别是多项式X和Y系数;选取n=4,即拟合的阶数为4次,X和Y是拟合后的曲线坐标。
7.根据权利要求1所述的六轮独立驱动独立转向机器人的避障方法,其特征是:所述基于模糊决策的避障策略如下所述:
机器人避障策略的选取是对障碍物轨迹和机器人轨迹进行预测,预测是否会发生碰撞,然后进行策略选择;对于静态障碍物视为运动速度和加速度为0的情况,当超过安全距离阈值,进行避障轨迹规划,规划完成避障轨迹后再进行轨迹跟踪即可;
对于动态障碍物来说,若障碍物的移动轨迹为直线,通过阈值函数的计算结果,直接判断在一定时间后是否会发生碰撞,然后利用约束求解最优路径;若障碍物的移动轨迹为曲线,根据障碍物位移对时间的微分函数来预测一定时间后的轨迹,根据与机器人轨迹相交时的运动时间判断机器人在此时间下的运动情况,利用机器人与障碍物的质心距离判断是否发生碰撞,然后求解最优路径。
在机器人避障决策的选择中采用模糊决策中的非对称模型,将机器人的避障情况作为约束条件,机器人避障策略的选择作为目标函数;在非对称模型中,把接受约束作为先决条件,目标与约束二者的地位不是对称的;给定论域X上的目标函数f(x)和X上的约束条件模糊集合D,所谓在约束D之下极大化f的最优解m,就是X上的一个模糊子集,它具有隶属函数当等式右端的集合为空集时,μm(x)等于0,给定论域X即为所有情况的合集,机器人遇到的当前情况即为约束D,目标函数即为机器人的避障策略;
机器人通过将遇到障碍物时环境信息和障碍物信息作为约束条件,求解目标函数,选择合适的避障策略。
8.根据权利要求7所述的六轮独立驱动独立转向机器人的避障方法,其特征是:所述动态障碍物,具体分为以下三种情况:
(1)第一种情况,障碍物运动方向与机器人轨迹运动方向相同;
此时避障轨迹的规划是使机器人从障碍物的左侧或者右侧选取最优轨迹进行超越,超越后再回归到原轨迹进行跟踪;进行避障轨迹重规划后将新的轨迹点输出至轨迹跟踪控制器,根据MPC算法会计算机器人的控制量进行轨迹跟踪;但当机器人质心位置与障碍物质心位置处在同一水平线时,在速度项中加入速度反馈比例控制,使机器人进行明显的超车动作并尽快回归到原有轨迹,同时加入微分控制防止超调;
其中,vmpc是MPC轨迹跟踪控制器计算的机器人控制量速度,kp1和kd1是速度反馈比例控制和微分控制系数,e11是机器人避障轨迹距离原轨迹的偏差量,tk是机器人运动的当前时刻,td1max是机器人运动到d1max的位置时刻,td1min是d1min所示位置时刻;
(2)第二种情况,障碍物运动方向与机器人运动方向相交,且障碍物移动速度较快;
此种情况在于判断机器人跟踪轨迹和障碍物移动轨迹在相交点是否会发生碰撞,判断依据与机器人和障碍物的移动速度有关;若障碍物移动速度相对较快,机器人在到达碰撞点前进行适当减速并待障碍物越过相交点后从障碍物后方超越,然后再回归到原轨迹进行跟踪;机器人与障碍物的距离先小后大,距离较小的时候需要减小速度,在MPC控制量的基础上加入速度反馈比例控制,距离较大时恢复到MPC控制,同时加入微分控制防止跟踪原轨迹时超调;
其中,vmpc是MPC计算的机器人控制量速度,kp2和kd2是速度反馈比例控制和微分控制系数,e21是机器人避障轨迹距离原轨迹的偏差量,e2是机器人质心位置与障碍物质心位置的距离,ddis2是机器人质心与障碍物质心在x方向绝对距离,d2min和d2max分别是机器人质心与障碍物的距离阈值;
(3)第三种情况,障碍物运动方向与机器人运动方向相交,且障碍物移动速度较慢;
机器人在到达碰撞点前进行适当加速并在障碍物未越过相交点时从障碍物前方超越,然后再回归到原轨迹进行跟踪;机器人与障碍物的距离先小后大,距离较小的时候需要增加速度,在MPC控制量的基础上加入速度反馈比例控制,加快进行避障,距离较大的时恢复到MPC控制,同时加入微分控制防止跟踪原轨迹时超调;
其中,vmpc是MPC计算的机器人控制量速度,kp3和kd3是速度反馈比例控制和微分控制系数,e31是机器人避障轨迹距离原轨迹的偏差量,e3是机器人质心位置与障碍物质心位置的距离,ddis3是机器人质心与障碍物质心在x方向绝对距离,d3min和d3max分别是机器人质心与障碍物的距离阈值。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310103879.1A CN116225004A (zh) | 2023-02-13 | 2023-02-13 | 一种六轮独立驱动独立转向机器人的避障方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202310103879.1A CN116225004A (zh) | 2023-02-13 | 2023-02-13 | 一种六轮独立驱动独立转向机器人的避障方法 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN116225004A true CN116225004A (zh) | 2023-06-06 |
Family
ID=86576222
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202310103879.1A Pending CN116225004A (zh) | 2023-02-13 | 2023-02-13 | 一种六轮独立驱动独立转向机器人的避障方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN116225004A (zh) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117341683A (zh) * | 2023-12-04 | 2024-01-05 | 苏州观瑞汽车技术有限公司 | 一种基于多目标识别的车辆动态轨迹拟合避障方法及*** |
-
2023
- 2023-02-13 CN CN202310103879.1A patent/CN116225004A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN117341683A (zh) * | 2023-12-04 | 2024-01-05 | 苏州观瑞汽车技术有限公司 | 一种基于多目标识别的车辆动态轨迹拟合避障方法及*** |
CN117341683B (zh) * | 2023-12-04 | 2024-04-23 | 苏州观瑞汽车技术有限公司 | 一种基于多目标识别的车辆动态轨迹拟合避障方法及*** |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110377039B (zh) | 一种车辆避障轨迹规划与跟踪控制方法 | |
Shan et al. | A reinforcement learning-based adaptive path tracking approach for autonomous driving | |
Peng et al. | Leader–follower formation control of nonholonomic mobile robots based on a bioinspired neurodynamic based approach | |
CN109799814B (zh) | 基于运动学模型的履带无人车辆轨迹跟踪控制器设计方法 | |
Maalouf et al. | A higher level path tracking controller for a four-wheel differentially steered mobile robot | |
CN112622903B (zh) | 一种车辆跟随驾驶环境下自主车辆的纵向和横向控制方法 | |
CN108227491B (zh) | 一种基于滑模神经网络的智能车轨迹跟踪控制方法 | |
Awad et al. | Model predictive control with fuzzy logic switching for path tracking of autonomous vehicles | |
Shi et al. | Planning the trajectory of an autonomous wheel loader and tracking its trajectory via adaptive model predictive control | |
Ding et al. | Trajectory tracking of redundantly actuated mobile robot by MPC velocity control under steering strategy constraint | |
CN112937571B (zh) | 一种智能汽车轨迹跟踪控制方法及*** | |
Hegedüs et al. | Motion planning for highly automated road vehicles with a hybrid approach using nonlinear optimization and artificial neural networks | |
CN116225004A (zh) | 一种六轮独立驱动独立转向机器人的避障方法 | |
Mohamed et al. | Autonomous navigation of agvs in unknown cluttered environments: log-mppi control strategy | |
Lee et al. | Learning terrain-aware kinodynamic model for autonomous off-road rally driving with model predictive path integral control | |
Kim et al. | Vehicle path tracking control using pure pursuit with MPC-based look-ahead distance optimization | |
Jiang et al. | Learning based predictive error estimation and compensator design for autonomous vehicle path tracking | |
Akiba et al. | Optimal tracking control of two-wheeled mobile robots based on model predictive control | |
Bai et al. | Relative navigation control of articulated vehicle based on LTV-MPC | |
Zheng et al. | Model predictive control for intelligent vehicle lane change | |
Fnadi et al. | Local obstacle-skirting path planning for a fast Bi-steerable rover using bézier curves | |
Kalaria et al. | Delay-aware robust control for safe autonomous driving and racing | |
Su et al. | Double-Iterative Gaussian Process Regression for Modeling Error Compensation in Autonomous Racing | |
Zhai et al. | Local Trajectory Planning for Obstacle Avoidance of Unmanned Tracked Vehicles Based on Artificial Potential Field Method | |
Kim et al. | Hierarchical Motion Planning and Control Algorithm of Autonomous Racing Vehicles for Overtaking Maneuvers |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination |