CN113211438A - 基于预瞄距离自适应的轮式机器人控制方法及控制系统 - Google Patents

基于预瞄距离自适应的轮式机器人控制方法及控制系统 Download PDF

Info

Publication number
CN113211438A
CN113211438A CN202110502894.4A CN202110502894A CN113211438A CN 113211438 A CN113211438 A CN 113211438A CN 202110502894 A CN202110502894 A CN 202110502894A CN 113211438 A CN113211438 A CN 113211438A
Authority
CN
China
Prior art keywords
robot
representing
longitudinal
coordinate system
preview
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.)
Granted
Application number
CN202110502894.4A
Other languages
English (en)
Other versions
CN113211438B (zh
Inventor
曾鹏云
崔翔
董熠鹏
彭维峰
魏传峰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
China Star Network Application Co Ltd
Original Assignee
Dongfanghong Satellite Mobile Communication Co Ltd
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Dongfanghong Satellite Mobile Communication Co Ltd filed Critical Dongfanghong Satellite Mobile Communication Co Ltd
Priority to CN202110502894.4A priority Critical patent/CN113211438B/zh
Publication of CN113211438A publication Critical patent/CN113211438A/zh
Application granted granted Critical
Publication of CN113211438B publication Critical patent/CN113211438B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J13/00—Controls for manipulators
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1661—Programme controls characterised by programming, planning systems for manipulators characterised by task planning, object-oriented languages
    • 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/60—Other road transportation technologies with climate change mitigation effect
    • Y02T10/72—Electric energy management in electromobility

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于预瞄距离自适应的轮式机器人控制方法及系统。控制方法包括：设置期望路径，重复执行以下步骤使机器人沿期望路径行驶：S1，基于当前路径曲率和当前纵向速度从参考表中获取最优预瞄距离，获得与最优预瞄距离对应的预瞄点；S2，基于预瞄点进行横向控制和/或纵向控制；横向控制为：获取机器人转向轮基于预瞄点的目标转角，控制机器人的转向轮按照所述目标转角偏转；纵向控制为：获得机器人纵向总的期望加速度，根据机器人纵向总的期望加速度获得机器人各车轮的目标力矩，控制各车轮按照各自的目标力矩转动。可自适应地根据实时的路径曲率和纵向速度快速从参考表中获得最优的预瞄距离，进行纵横向运动协同控制。

Description

基于预瞄距离自适应的轮式机器人控制方法及控制系统
技术领域
本发明涉及轮式机器人控制技术领域，特别是涉及一种基于预瞄距离自适应的轮式机器人控制方法及系统。
背景技术
轮式机器人作为机器人众多领域的一个重要分支，一直都是人类研究的热点。相比需要固定在某一位置的机器人，轮式机器人具有更强的灵活性，它可以方便地在人类难以到达的环境下作业，帮助人类探索未知的领域。例如，我国在2013年12月2日成功发射了嫦娥三号月球探测器，其中搭载的玉兔号也成为我国首辆月球探测车，为我国的航天事业做出了巨大贡献。此外，轮式机器人在物流行业、医疗领域、甚至军事领域都具备广泛应用潜力。
对于在各领域广泛应用的轮式机器人，对其进行准确的运动控制是完成各种任务的前提，其中主要包括横向控制和纵向控制两方面研究内容。横向控制主要研究机器人对给定路径的跟踪能力，纵向控制主要研究机器人如何准确跟踪以期望速度行驶，以路径跟踪精度、行驶安全性、机器人运动稳定性和控制器稳定性等作为主要性能指标。
从控制角度讲，轮式机器人运动控制器设计主要包含机器人动力学模型建立、跟踪偏差产生、控制量生成等几方面内容。目前应用较广的控制量生成方法主要包括经典控制方法(以PID算法为代表)、最优控制方法等。PID控制器具有设计简单，计算方便等优点，但其参数的获取往往倚重于试凑或经验估计，难以实现不同环境下的迁移应用。最优控制常常将机器人运动模型简化为线性定常系统，在设计时依赖精确的数学模型，因此当参数时变或存在外界扰动时，控制器稳定性和鲁棒性会受到影响。
此外，在机器人的纵横向控制中，预瞄距离的选取对跟踪精度、转向轻便性和机器人运动稳定性有较大影响。预瞄距离越短，跟踪精度越高，但是转向平稳性和稳定性则可能变差。因此，如何在轮式机器人运动过程中合理选择预瞄距离十分重要。
发明内容
本发明旨在至少解决现有技术中存在的技术问题，特别创新地提出了一种基于预瞄距离自适应的轮式机器人控制方法及系统。
为了实现本发明的上述目的，根据本发明的第一个方面，本发明提供了一种基于预瞄距离自适应的轮式机器人控制方法，包括：为机器人设置期望路径，重复执行以下步骤使机器人沿所述期望路径行驶：步骤S1，获取当前路径曲率和当前纵向速度，基于当前路径曲率和当前纵向速度从参考表中获取最优预瞄距离，获得与所述最优预瞄距离对应的预瞄点，预瞄距离为预瞄点在机器人坐标系下的纵轴坐标，所述参考表中罗列了路径曲率和纵向速度的不同取值组合对应的最优预瞄距离；步骤S2，基于所述预瞄点进行横向控制和/或纵向控制；所述横向控制为：获取机器人转向轮基于所述预瞄点的目标转角，控制机器人的转向轮按照所述目标转角偏转；所述纵向控制为：根据机器人当前纵向速度、预瞄点坐标和预瞄点的期望纵向速度获得机器人纵向总的期望加速度，根据机器人纵向总的期望加速度获得机器人各车轮的目标力矩，控制各车轮按照各自的目标力矩转动。
上述技术方案：该控制方法在轮式机器人行驶过程中，可自适应地根据实时的路径曲率和纵向速度快速从参考表中获得最优的预瞄距离，利用最优的预瞄距离进行横向和/或纵向运动控制，提高了转向平稳性、跟踪精度、运动稳定性，实现了机器人纵横向运动协同控制。
在本发明一种优选实施方式中，所述机器人的转向轮为前轮。
上述技术方案：便于转向控制。
在本发明一种优选实施方式中，所述参考表的建立方法包括：分别为预瞄距离、路径曲率和纵向速度设置取值范围，在路径曲率的取值范围和纵向速度的取值范围内构建路径曲率和纵向速度的多个不同的取值组合，通过第一方法获得每个取值组合对应的最优预瞄距离，所述第一方法包括：步骤S11，设置种群规模，将预瞄距离作为粒子的位置变量，将预瞄距离的取值范围作为粒子的搜索空间，初始化历史最优位置以及历史最优位置的初始值对应的适应度值，初始化全局最优位置以及全局最优位置的初始值对应的适应度值；设t为迭代次数，令t的初始值为1；通过随机函数初始化每个粒子的位置和速度；步骤S12，第t次迭代过程包括：计算每个粒子的适应度值，将全局最优位置更新为适应度值最小的粒子的位置；若适应度最小值小于历史最优值位置对应的适应度值，将历史最优位置更新为适应度值最小的粒子的位置，将更新后的历史最优位置与最小适应度值对应，否则不更新历史最优位置；更新每个粒子的位置和速度；步骤S13，若t≥T或者本次迭代的最小适应度值收敛至预设适应度阈值，则进入步骤S14，否则令t＝t+1，返回执行步骤S12；所述T为预设的最大迭代次数；步骤S14，迭代结束，将历史最优位置作为与取值组合对应的最优预瞄距离。
上述技术方案：由于预瞄距离、路径曲率和纵向运动速度三者之间很难用数学关系精确表示，采用了粒子群算法实现预瞄距离对路径曲率和运动速度的自适应寻优，保证路径跟踪精度，建立参考表，通过查表的方式能够快速获得最优预瞄距离，保证了机器人运动稳定性和控制器稳定性。
在本发明一种优选实施方式中，所述适应度值根据适应度函数获得，所述适应度函数为：
Figure BDA0003056288300000041
其中，ω1表示J1的权重系数，ω2表示J2的权重系数，ω3表示J3的权重系数，ω4表示J4的权重系数，J1表示机器人路径跟踪误差代价函数，J2表示方向误差代价函数，J3表示转向轻便性代价函数，J4表示侧向加速度代价函数；所述机器人路径跟踪误差代价函数
Figure BDA0003056288300000042
其中，Yref(t)表示t时刻期望路径在大地坐标系中对应的Y轴坐标，Yreal(t)表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动t时刻所在实际位置点在大地坐标系中对应的Y轴坐标，
Figure BDA0003056288300000043
为预设的路径偏差阈值，tn表示机器人以当前粒子的位置作为最优预瞄距离下按照步骤S2获得的目标转角和/或目标力矩运动至期望路径终点的时间；所述方向误差代价函数
Figure BDA0003056288300000044
其中，
Figure BDA0003056288300000045
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中的质心横摆角速度，
Figure BDA0003056288300000046
表示预设的机器人质心横摆角速度阈值，vx表示取值组合中的纵向速度；所述转向轻便性代价函数
Figure BDA0003056288300000051
其中，
Figure BDA0003056288300000052
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中前轮的转动角速度，
Figure BDA0003056288300000053
表示预设的机器人前轮转动角速度阈值；所述侧向加速度代价函数
Figure BDA0003056288300000054
其中，
Figure BDA0003056288300000055
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中的横向加速度，
Figure BDA0003056288300000056
表示预设的机器人横向加速度阈值。
上述技术方案：适应度函数能够建立起预瞄距离与路径曲率、纵向速度的对应关系，通过该适应度函数选出的粒子能够提高跟踪精度、机器人运动稳定性和控制器稳定性。
在本发明一种优选实施方式中，在所述步骤S2中，所述横向控制中获得前轮的目标转角的过程包括：步骤S21，建立机器人的单轴运动模型，所述单轴运动模型包含在机器人坐标系xoy下的三个自由度，三个自由度分别为沿纵向x轴的平动、沿横向y轴的平动和绕竖直方向的转动即横摆角；步骤S22，根据如下公式获得预瞄点在机器人坐标系xoy下坐标
Figure BDA0003056288300000057
Figure BDA0003056288300000058
其中，xe为预瞄点在机器人坐标系xoy下的纵轴坐标，定义为预瞄距离；ye表示预瞄点在机器人坐标系xoy下的横轴坐标，定义为横向预瞄误差；
Figure BDA0003056288300000059
表示机器人在机器人坐标系xoy下位于预瞄点的期望横摆角；
Figure BDA00030562883000000510
表示预瞄点P在大地坐标系XOY中的坐标，Xp、Yp、
Figure BDA00030562883000000511
分别表示预瞄点P在大地坐标系XOY中的X轴坐标、Y轴坐标、预瞄点P处切线与X轴的夹角；
Figure BDA00030562883000000613
表示当前机器人质心在大地坐标系XOY中的坐标，Xc、Yc、
Figure BDA0003056288300000061
分别表示当前机器人质心在大地坐标系XOY中的X轴坐标、Y轴坐标、横摆角；根据预瞄点在机器人坐标系xoy下坐标
Figure BDA0003056288300000062
和所述单轴运动模型获得机器人的单点视觉预瞄模型；步骤S23，结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角。
上述技术方案：通过对前轮转角的控制实现了机器人横向运动控制，使轮式机器人稳定跟踪期望路径所需的前轮转角，保证机器人行驶过程中具有较小的横向偏差。
在本发明一种优选实施方式中，在大地坐标系XOY下，所述机器人的单轴运动模型表示为：
Figure BDA0003056288300000063
其中，δf表示机器人的前轮转角；
Figure BDA0003056288300000064
表示机器人质心在大地坐标系XOY下的纵向的加速度；
Figure BDA0003056288300000065
表示机器人质心在大地坐标系XOY下的横向的加速度；
Figure BDA0003056288300000066
表示机器人质心在大地坐标系XOY下的横摆角加速度；变量
Figure BDA0003056288300000067
Figure BDA0003056288300000068
表示机器人质心在大地坐标系XOY下的横向速度，
Figure BDA0003056288300000069
表示机器人质心在大地坐标系XOY下的横摆角速度，m表示机器人质量，Clf表示前轮纵向刚度，Clr表示后轮的纵向刚度，sf表示前轮的滑移率，sr表示后轮的滑移率；变量
Figure BDA00030562883000000610
Ccf表示前轮侧偏刚度，a表示机器人质心距前轴的距离；变量
Figure BDA00030562883000000611
Ccr表示后轮侧偏刚度，b表示机器人质心距后轴的距离；变量
Figure BDA00030562883000000612
sf表示前轮滑移率；变量
Figure BDA0003056288300000071
Iz表示机器人绕z轴的转动惯量；变量
Figure BDA0003056288300000072
上述技术方案：该单轴运动模型仅用三个自由度能够准确完整地描述机器人的运动姿态。
在本发明一种优选实施方式中，所述机器人的单点视觉预瞄模型为：
Figure BDA0003056288300000073
其中，
Figure BDA0003056288300000074
表示在机器人坐标系中机器人的横向偏差变化率，
Figure BDA0003056288300000075
表示机器人坐标系中机器人质心在预瞄点处的横摆角速度，
Figure BDA0003056288300000076
表示当前机器人质心在大地坐标系的横摆角速度，vp表示在大地坐标系中预瞄点期望的纵向速度，ρp表示预瞄点的路径曲率，
Figure BDA0003056288300000077
表示大地坐标系下机器人质心处的横向速度。
上述技术方案：通过该单点视觉预瞄模型便于后续目标转角计算。
在本发明一种优选实施方式中，所述步骤S23结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角的过程具体包括：步骤S231，基于滑膜变结构理论获得控制对象X的状态方程为：
Figure BDA0003056288300000078
其中，
Figure BDA0003056288300000079
Figure BDA00030562883000000710
分别表示机器人驶向预瞄点过程中任意时间点在大地坐标系中机器人质心的纵向速度、横向速度、横摆角速度，
Figure BDA00030562883000000711
表示在机器人坐标系中的横向偏差变化率，
Figure BDA00030562883000000712
表示对X中各分量求导，得到
Figure BDA00030562883000000713
第一系数矩阵f(X)为：
Figure BDA0003056288300000081
Figure BDA0003056288300000082
表示预瞄点期望纵向加速度；第二系数矩阵B为：B＝[b1 b2 b3 b4 b5 b6]T；第三系数矩阵W为：W＝[0 0 0 0 0 vp]T；步骤S232，以横向预瞄误差及横向预瞄误差变化率作为控制目标，则滑模面可设计为：
Figure BDA0003056288300000083
其中，c表示滑膜参数；s表示滑膜；步骤S233，设当s→0时，滑膜趋近率
Figure BDA0003056288300000084
设置为等速趋近率，满足如下关系：
Figure BDA0003056288300000085
其中，g表示切换增益，
Figure BDA0003056288300000086
sat(·)表示饱和函数，Δ表示边界层厚度，转换系数q＝1/Δ；步骤S234，根据关系式
Figure BDA0003056288300000087
得到等式
Figure BDA0003056288300000088
根据等式
Figure BDA0003056288300000089
和控制对象X的状态方程得到机器人前轮的目标转角δd为：
Figure BDA00030562883000000810
上述技术方案：结合单点视觉预瞄模型和滑膜控制理论获得的目标转角能够更好的横向跟踪期望路径，在滑膜控制器中使用饱和函数能够消除滑模控制器的高频抖振。
在本发明一种优选实施方式中，所述纵向控制中获得车轮的目标力矩的过程包括：步骤A，获取机器人纵向总的期望加速度aref为：
Figure BDA00030562883000000811
其中，vc表示机器人当前在大地坐标系下的纵向速度，vp表示在大地坐标系下预瞄点处的期望纵向速度，tp是机器人保持当前纵向速度vc到达预瞄点在大地坐标系下的纵向坐标位置的时间；步骤B，根据机器人纵向总的期望加速度aref建立如下等式：
Figure BDA0003056288300000091
其中，Fl表示机器人总的期望纵向力，m表示机器人质量，α表示道路坡度，ρ表示空气密度，Cd表示空气阻力系数，Awind表示机器人迎风面面积；步骤C，每个车轮平均分配力矩，每个车轮的目标力矩为：
Figure BDA0003056288300000092
其中，N表示车轮总数，R表示车轮半径。
上述技术方案：基于加速度预瞄模型进行纵向速度控制，采用对各轮子进行独立平均力矩分配，便于实现轮式机器人的纵横向协同控制。
为了实现本发明的上述目的，根据本发明的第二个方面，本发明提供了一种基于预瞄距离自适应的轮式机器人控制系统，包括安装在轮式的机器人上用于检测机器人运动过程中的实时位置、姿态和速度的状态检测模块，以及车轮驱动模块和控制器；所述控制器分别与所述状态检测模块和所述车轮驱动模块连接；所述控制器按照本发明所述的基于预瞄距离自适应的轮式机器人控制方法控制机器人运动。
上述技术方案：该控制系统在轮式机器人行驶过程中，可自适应地根据实时的路径曲率和纵向速度快速从参考表中获得最优的预瞄距离，利用最优的预瞄距离进行横向和/或纵向运动控制，提高了转向平稳性、跟踪精度、运动稳定性，实现了机器人纵横向运动协同控制。
附图说明
图1是本发明一具体实施方式中基于预瞄距离自适应的轮式机器人控制方法的流程示意图；
图2是本发明一具体实施方式中机器人的单轴运动模型示意图；
图3是本发明一具体实施方式中单点视觉预瞄模型示意图；
图4是本发明一种应用场景中纵横向协同控制的流程示意图；
图5是本发明一具体实施方式中控制器内部示意图。
具体实施方式
下面详细描述本发明的实施例，所述实施例的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施例是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
在本发明的描述中，需要理解的是，术语“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。
在本发明的描述中，除非另有规定和限定，需要说明的是，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是机械连接或电连接，也可以是两个元件内部的连通，可以是直接相连，也可以通过中间媒介间接相连，对于本领域的普通技术人员而言，可以根据具体情况理解上述术语的具体含义。
本发明公开了一种基于预瞄距离自适应的轮式机器人控制方法，在一种优选实施方式中，如图1所示该控制方法包括：为机器人设置期望路径，重复执行以下步骤使机器人沿期望路径行驶：
步骤S1，获取当前路径曲率和当前纵向速度，基于当前路径曲率和当前纵向速度从参考表中获取最优预瞄距离，获得与最优预瞄距离对应的预瞄点，预瞄距离为预瞄点在机器人坐标系下的纵轴坐标，参考表中罗列了路径曲率和纵向速度的不同取值组合对应的最优预瞄距离；
步骤S2，基于预瞄点进行横向控制和/或纵向控制；
横向控制为：获取机器人转向轮基于预瞄点的目标转角，控制机器人的转向轮按照目标转角偏转；
纵向控制为：根据机器人当前纵向速度、预瞄点坐标和预瞄点的期望纵向速度获得机器人纵向总的期望加速度，根据机器人纵向总的期望加速度获得机器人各车轮的目标力矩，控制各车轮按照各自的目标力矩转动。
在本实施方式中，在步骤S2中，可以仅执行横向控制，或者仅执行纵向控制，或者横向控制和纵向控制同时执行实现横纵协同控制。
在本实施方式中，优选的，机器人的转向轮为前轮。优选的，机器人的各车轮采用平均分配转矩的方式获得目标力矩，这样便于快速稳定控制。
在本实施方式中，步骤S1中，基于当前路径曲率和当前纵向速度从参考表中获取最优预瞄距离可以是直接读取也可以采用插值法获得具体的最优预瞄距离数值。
在一种优选实施方式中，参考表的建立方法包括：分别为预瞄距离、路径曲率和纵向速度设置取值范围，在路径曲率的取值范围和纵向速度的取值范围内构建路径曲率和纵向速度的多个不同的取值组合，每个阻值组合对应了一组路径曲率值和纵向速度值，通过第一方法获得每个取值组合对应的最优预瞄距离，第一方法包括：
步骤S11，设置种群规模即粒子数量，将预瞄距离作为粒子的位置变量，将预瞄距离的取值范围作为粒子的搜索空间，初始化历史最优位置以及历史最优位置的初始值对应的适应度值，初始化全局最优位置以及全局最优位置的初始值对应的适应度值；优选的，可将历史最优位置的初始值对应的适应度值和全局最优位置的初始值对应的适应度值设置为0。设t为迭代次数，令t的初始值为1；通过随机函数初始化每个粒子的位置和速度；
步骤S12，第t次迭代过程包括：
计算每个粒子的适应度值，将全局最优位置更新为适应度值最小的粒子的位置；若适应度最小值小于历史最优值位置对应的适应度值，将历史最优位置更新为适应度值最小的粒子的位置，将更新后的历史最优位置与最小适应度值对应，否则不更新历史最优位置；更新每个粒子的位置和速度；粒子的位置和速度更新公式可采用现有的常规更新公式，如可参考网址https：//blog.csdn.net/weixin＿40679412/article/details/80571854中公开的更新公式，在此不再赘述。
步骤S13，若t≥T或者本次迭代的最小适应度值收敛至预设适应度阈值，则进入步骤S14，否则令t＝t+1，返回执行步骤S12；T为预设的最大迭代次数；
步骤S14，迭代结束，将历史最优位置作为与取值组合对应的最优预瞄距离。
在本实施方式中，判断本次迭代的最小适应度值是否收敛至预设适应度阈值的方法可以是：计算本次迭代的最小适应度值与适应度阈值的差值的绝对值，若绝对值与适应度阈值的比值小于等于预设比例阈值，就认为本次迭代的最小适应度值已收敛至预设适应度阈值，否则认为本次迭代的最小适应度值没有收敛至预设适应度阈值。
在一种优选实施方式中，适应度值根据适应度函数获得，适应度函数为：
Figure BDA0003056288300000131
其中，ω1表示J1的权重系数，ω2表示J2的权重系数，ω3表示J3的权重系数，ω4表示J4的权重系数。各权重系数标识对应的指标的相对重要程度，如若J1到J4同等重要，则ω1到ω4取相同值即可。J1表示机器人路径跟踪误差代价函数，J2表示方向误差代价函数，J3表示转向轻便性代价函数，J4表示侧向加速度代价函数；
机器人路径跟踪误差代价函数
Figure BDA0003056288300000132
其中，Yref(t)表示t时刻期望路径在大地坐标系中对应的Y轴坐标，Yreal(t)表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动t时刻所在实际位置点在大地坐标系中对应的Y轴坐标，
Figure BDA0003056288300000133
为预设的路径偏差阈值，tn表示机器人以当前粒子的位置作为最优预瞄距离下按照步骤S2获得的目标转角和/或目标力矩运动至期望路径终点的时间；方向误差代价函数
Figure BDA0003056288300000134
其中，
Figure BDA0003056288300000135
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中的质心横摆角速度，
Figure BDA0003056288300000136
表示预设的机器人质心横摆角速度阈值，vx表示取值组合中的纵向速度；转向轻便性代价函数
Figure BDA0003056288300000137
其中，
Figure BDA0003056288300000138
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中前轮的转动角速度，
Figure BDA0003056288300000141
表示预设的机器人前轮转动角速度阈值；侧向加速度代价函数
Figure BDA0003056288300000142
其中，
Figure BDA0003056288300000143
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中的横向加速度，
Figure BDA0003056288300000144
表示预设的机器人横向加速度阈值。
在本实施方式中，为期望路径事先给定的路径可以为任意曲线或直线。虽然适应度函数中没有存在路径曲率与适应度函数的显性关系，但是不同的路径曲率对应了不同的期望路径，则会导致按照步骤S2中的方法进行控制时，机器人路径跟踪误差代价函数J1、转向轻便性代价函数J3、方向误差代价函数J2、侧向加速度代价函数J4四个函数的函数值不同，因此路径曲率不同会改变适应度函数值。
在一种优选实施方式中，在步骤S2中，横向控制中获得前轮的目标转角的过程包括：
步骤S21，建立机器人的单轴运动模型，轮式机器人优选的为四轮机器人，如图2所示，单轴运动模型包含在大地坐标系XOY下的三个自由度，三个自由度分别为沿纵向X轴的平动、沿横向Y轴的平动和绕竖直方向的转动即横摆角；具体的，四轮驱动的轮式机器人单轴运动模型可表示为：
Figure BDA0003056288300000145
Figure BDA0003056288300000146
Figure BDA0003056288300000147
优选的，为简化数学推导，将上述模型表示中的控制量δf分离出来，得到：
在大地坐标系XOY下，机器人的单轴运动模型表示为：
Figure BDA0003056288300000151
其中，δf表示机器人的前轮转角；
Figure BDA0003056288300000152
表示机器人质心在大地坐标系XOY下的纵向的加速度；
Figure BDA0003056288300000153
表示机器人质心
Figure BDA0003056288300000154
的横向的加速度；
Figure BDA0003056288300000155
表示机器人质心
Figure BDA0003056288300000156
的横摆角加速度；变量
Figure BDA0003056288300000157
Figure BDA0003056288300000158
表示机器人质心在大地坐标系XOY下的横向速度，
Figure BDA0003056288300000159
表示机器人质心在大地坐标系XOY下的横摆角速度，m表示机器人质量，Clf表示前轮纵向刚度，Clr表示后轮的纵向刚度，sf表示前轮的滑移率，sr表示后轮的滑移率；变量
Figure BDA00030562883000001510
Ccf表示前轮侧偏刚度，a表示机器人质心距前轴的距离；变量
Figure BDA00030562883000001511
Ccr后轮侧偏刚度，b表示机器人质心距后轴的距离；变量
Figure BDA00030562883000001512
变量
Figure BDA00030562883000001513
Iz表示机器人绕z轴的转动惯量；变量
Figure BDA00030562883000001514
步骤S22，如图3所示，针对轮式机器人建立单点视觉预瞄模型，其中XOY为大地坐标系，xoy为机器人本体的坐标系，xoy中以机器人质心为坐标原点，P点和C点分别表示预瞄点和机器人当前位置点。
根据如下公式获得预瞄点在机器人坐标系xoy下坐标
Figure BDA00030562883000001515
Figure BDA0003056288300000161
其中，xe为预瞄点在机器人坐标系xoy下的纵轴坐标，定义为预瞄距离；ye表示预瞄点在机器人坐标系xoy下的横轴坐标，定义为横向预瞄误差；
Figure BDA0003056288300000162
表示机器人在机器人坐标系xoy下位于预瞄点的期望横摆角；
Figure BDA0003056288300000163
表示预瞄点P在大地坐标系XOY中的坐标，Xp、Yp、
Figure BDA0003056288300000164
分别表示预瞄点P在大地坐标系XOY中的X轴坐标、Y轴坐标、预瞄点P处切线与X轴的夹角；
Figure BDA0003056288300000165
表示当前机器人质心在大地坐标系XOY中的坐标，Xc、Yc、
Figure BDA0003056288300000166
分别表示当前机器人质心在大地坐标系XOY中的X轴坐标、Y轴坐标、横摆角；
根据预瞄点在机器人坐标系xoy下坐标
Figure BDA0003056288300000167
和单轴运动模型获得机器人的单点视觉预瞄模型；
优选的，机器人的单点视觉预瞄模型为：
Figure BDA0003056288300000168
其中，
Figure BDA0003056288300000169
表示在机器人坐标系中机器人的横向偏差变化率，
Figure BDA00030562883000001610
表示机器人坐标系中机器人质心在预瞄点处的横摆角速度，
Figure BDA00030562883000001611
表示当前机器人质心在大地坐标系的横摆角速度，vp表示在大地坐标系中预瞄点期望的纵向速度(由于一般机器人横向速度远小于纵向速度，因此这里将vp定义为纵向速度)，ρp表示预瞄点的路径曲率，
Figure BDA00030562883000001612
表示大地坐标系下机器人质心处的横向速度。
步骤S23，结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角。
在一种优选实施方式中，步骤S23结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角的过程具体包括：
步骤S231，基于滑膜变结构理论获得控制对象X的状态方程为：
Figure BDA0003056288300000171
其中，
Figure BDA0003056288300000172
Figure BDA0003056288300000173
分别表示机器人驶向预瞄点过程中任意时间点在大地坐标系中机器人质心的纵向速度、横向速度、横摆角速度，
Figure BDA0003056288300000174
表示在机器人坐标系中的横向偏差变化率，
Figure BDA0003056288300000175
表示对X中各分量求导，得到
Figure BDA0003056288300000176
第一系数矩阵f(X)为：
Figure BDA0003056288300000177
Figure BDA0003056288300000178
表示预瞄点期望纵向加速度；
第二系数矩阵B为：B＝[b1 b2 b3 b4 b5 b6]T；
第三系数矩阵W为：W＝[0 0 0 0 0 vp]T；
步骤S232，以横向预瞄误差及横向预瞄误差变化率作为控制目标，则滑模面可设计为：
Figure BDA0003056288300000179
其中，c表示滑膜参数；s表示滑膜；
步骤S233，设当s→0时，滑膜趋近率
Figure BDA00030562883000001710
设置为等速趋近率，满足如下关系：：
Figure BDA00030562883000001711
其中，g表示切换增益，
Figure BDA00030562883000001712
sat(·)表示饱和函数，Δ表示边界层厚度，转换系数q＝1/Δ；
步骤S234，根据关系式
Figure BDA0003056288300000181
得到等式
Figure BDA0003056288300000182
根据等式
Figure BDA0003056288300000183
和控制对象X的状态方程得到机器人前轮的目标转角δd为：
Figure BDA0003056288300000184
在本实施方式中，需要找到合适的控制量使s→0，这就需要人为设计控制器保证
Figure BDA0003056288300000185
成立，这样就引入了状态变量X。此时
Figure BDA0003056288300000186
解得s＝s(0)e-gt，当t→∞时，s→0。引入sat(·)函数仅仅是为了趋近过程更为平滑。
Figure BDA0003056288300000187
表示一个滑模面。当s＝0时，有
Figure BDA0003056288300000188
此时ye＝ye(0)e-ct，当t→∞时，ye→0。这样就达到的控制目的，即横向偏差收敛到0。因此，g和c需要为正数。
在本实施方式中，δd即为期望的前轮的目标转角，实际的前轮转角δf应按照δd变化。
滑模控制器的控制率为：
Figure BDA0003056288300000189
由状态方程可知：
Figure BDA00030562883000001810
将
Figure BDA00030562883000001811
和
Figure BDA00030562883000001812
代入公式(1)，得到：
Figure BDA00030562883000001813
在一种优选实施方式中，为实现轮式机器人纵横向运动协同控制，本发明设计了基于加速度预瞄模型的纵向速度跟踪控制器，纵向控制中获得车轮的目标力矩的过程包括：
步骤A，获取机器人纵向总的期望加速度aref为：
Figure BDA0003056288300000191
其中，vc表示机器人当前在大地坐标系下的纵向速度，即状态方程中的
Figure BDA0003056288300000192
vp表示在大地坐标系下预瞄点处的期望纵向速度，tp是机器人保持当前纵向速度vc到达预瞄点在大地坐标系下的纵向坐标位置的时间；机器人向前运动时，预瞄点也是在不断更新的，当横向偏差ye不大时，
Figure BDA0003056288300000193
步骤B，根据机器人纵向总的期望加速度aref建立如下等式：
Figure BDA0003056288300000194
其中，Fl表示机器人总的期望纵向力，m表示机器人质量，α表示道路坡度，ρ表示空气密度，Cd表示空气阻力系数，Awind表示机器人迎风面面积；
步骤C，为提高控制实时性，本发明采用力矩平均分配的方式进行速度跟踪控制，每个车轮平均分配力矩，每个车轮的目标力矩为：
Figure BDA0003056288300000195
其中，N表示车轮总数，R表示车轮半径。
本发明控制方法的一种应用场景中，为了实现对期望路径的跟踪控制，通过建立轮式机器人非线性动动学模型(单轴动力模型)和单点视觉预瞄模型，得到机器人纵横向运动控制模型，然后基于该非线性控制模型实现纵横向运动协同控制，具体流程示意图如图4所示，包括：
步骤一，获取轮式机器人在大地坐标系下的期望路径和期望速度信息。具体地，可以根据不同应用环境为轮式机器人规划一条平滑的运动轨迹，然后对轨迹进行插补得到一系列轨迹姿态点。
步骤二，获取轮式机器人当前的位姿信息和速度信息
Figure BDA0003056288300000201
具体地，可以在轮式机器人平台固定视觉传感器、毫米波雷达、三轴惯性测量单元等，以检测机器人运动过程中的实时位置、姿态、速度信息。
步骤三，获取期望路径上预瞄点在大地坐标系下的位姿信息
Figure BDA0003056288300000202
并基于单点视觉预瞄模型计算期望路径上预瞄点在机器人本体坐标系下的位姿信息
Figure BDA0003056288300000203
其中xe和ye分别为预瞄距离和横向偏差。
具体地，基于前述所获取的大地坐标系下的期望轨迹，此处可利用坐标变换的方式，计算一系列期望轨迹点在机器人本体坐标系下的坐标。
步骤四，计算能使轮式机器人稳定跟踪期望路径所需的前轮的目标转角δd。
具体地，通过对轮式机器人非线性动力学模型和视觉预瞄模型进行分析计算，利用李亚普洛夫稳定性判据设计基于滑膜变结构理论的机器人横向运动控制器。
步骤五，基于粒子群算法计算能针对不同路径曲率和速度自适应调整的预瞄距离。
具体地，采用粒子群算法计算考虑路径跟踪精度、机器人运动稳定性和控制器稳定性的最优预瞄距离，并制作预瞄距离随路径曲率和机器人速度变化的参考表，实际计算时查表获取最优预瞄距离。
步骤六，基于加速度预瞄计算机器人速度跟踪所需的期望转矩。
具体地，纵向控制器设计采用力矩平均分配的方式，获取基于加速度预瞄模型的机器人期望转矩T，实现轮式机器人的纵横向协同控制。
本发明还公开了一种基于预瞄距离自适应的轮式机器人控制系统，在一种优选实施方式中，该控制系统包括安装在轮式的机器人上用于检测机器人运动过程中的实时位置、姿态和速度的状态检测模块，以及车轮驱动模块和控制器；控制器分别与状态检测模块和车轮驱动模块连接；控制器按照上述基于预瞄距离自适应的轮式机器人控制方法控制机器人运动。
在本实施方式中，状态检测模块优选但不限于安装在机器人上的视觉传感器、毫米波雷达、三轴惯性测量单元。车轮驱动模块优选但不限于现有的轮毂电机驱动模块。
在本实施方式中，控制器通过建立轮式机器人非线性动动学模型和视觉预瞄模型，得到机器人纵横向运动控制模型，然后基于该非线性控制模型实现纵横向运动协同控制。
在本实施方式中，控制器的内部结构如图5所示，包括横向控制器、纵向控制器、期望轨路径规划模块、最优预瞄距离确定模块、视觉预瞄模型单元等。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (10)

1.一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，为机器人设置期望路径，重复执行以下步骤使机器人沿所述期望路径行驶：
步骤S1，获取当前路径曲率和当前纵向速度，基于当前路径曲率和当前纵向速度从参考表中获取最优预瞄距离，获得与所述最优预瞄距离对应的预瞄点，预瞄距离为预瞄点在机器人坐标系下的纵轴坐标，所述参考表中罗列了路径曲率和纵向速度的不同取值组合对应的最优预瞄距离；
步骤S2，基于所述预瞄点进行横向控制和/或纵向控制；
所述横向控制为：获取机器人转向轮基于所述预瞄点的目标转角，控制机器人的转向轮按照所述目标转角偏转；
所述纵向控制为：根据机器人当前纵向速度、预瞄点坐标和预瞄点的期望纵向速度获得机器人纵向总的期望加速度，根据机器人纵向总的期望加速度获得机器人各车轮的目标力矩，控制各车轮按照各自的目标力矩转动。
2.如权利要求1所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，所述机器人的转向轮为前轮。
3.如权利要求1或2所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，所述参考表的建立方法包括：
分别为预瞄距离、路径曲率和纵向速度设置取值范围，在路径曲率的取值范围和纵向速度的取值范围内构建路径曲率和纵向速度的多个不同的取值组合，通过第一方法获得每个取值组合对应的最优预瞄距离，所述第一方法包括：
步骤S11，设置种群规模，将预瞄距离作为粒子的位置变量，将预瞄距离的取值范围作为粒子的搜索空间，初始化历史最优位置以及历史最优位置的初始值对应的适应度值，初始化全局最优位置以及全局最优位置的初始值对应的适应度值；设t为迭代次数，令t的初始值为1；通过随机函数初始化每个粒子的位置和速度；
步骤S12，第t次迭代过程包括：
计算每个粒子的适应度值，将全局最优位置更新为适应度值最小的粒子的位置；若适应度最小值小于历史最优值位置对应的适应度值，将历史最优位置更新为适应度值最小的粒子的位置，将更新后的历史最优位置与最小适应度值对应，否则不更新历史最优位置；更新每个粒子的位置和速度；
步骤S13，若t≥T或者本次迭代的最小适应度值收敛至预设适应度阈值，则进入步骤S14，否则令t＝t+1，返回执行步骤S12；所述T为预设的最大迭代次数；
步骤S14，迭代结束，将历史最优位置作为与取值组合对应的最优预瞄距离。
4.如权利要求3所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，所述适应度值根据适应度函数获得，所述适应度函数为：
Figure FDA0003056288290000021
其中，ω1表示J1的权重系数，ω2表示J2的权重系数，ω3表示J3的权重系数，ω4表示J4的权重系数，，J1表示机器人路径跟踪误差代价函数，J2表示方向误差代价函数，J3表示转向轻便性代价函数，J4表示侧向加速度代价函数；
所述机器人路径跟踪误差代价函数
Figure FDA0003056288290000022
其中，Yref(t)表示t时刻期望路径在大地坐标系中对应的Y轴坐标，Yreal(t)表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动t时刻所在实际位置点在大地坐标系中对应的Y轴坐标，
Figure FDA0003056288290000031
为预设的路径偏差阈值，tn表示机器人以当前粒子的位置作为最优预瞄距离下按照步骤S2获得的目标转角和/或目标力矩运动至期望路径终点的时间；
所述方向误差代价函数
Figure FDA0003056288290000032
其中，
Figure FDA0003056288290000033
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中的质心横摆角速度，
Figure FDA0003056288290000034
表示预设的机器人质心横摆角速度阈值，vx表示取值组合中的纵向速度；
所述转向轻便性代价函数
Figure FDA0003056288290000035
其中，
Figure FDA0003056288290000036
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中前轮的转动角速度，
Figure FDA0003056288290000037
表示预设的机器人前轮转动角速度阈值；
所述侧向加速度代价函数
Figure FDA0003056288290000038
其中，
Figure FDA0003056288290000039
表示机器人以当前粒子的位置作为最优预瞄距离按照步骤S2获得的目标转角和/或目标力矩运动在t时刻在大地坐标系中的横向加速度，
Figure FDA00030562882900000310
表示预设的机器人横向加速度阈值。
5.如权利要求2所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，在所述步骤S2中，所述横向控制中获得前轮的目标转角的过程包括：
步骤S21，建立机器人的单轴运动模型，所述单轴运动模型包含在机器人坐标系xoy下的三个自由度，三个自由度分别为沿纵向x轴的平动、沿横向y轴的平动和绕竖直方向的转动即横摆角；
步骤S22，根据如下公式获得预瞄点在机器人坐标系xoy下坐标
Figure FDA0003056288290000041
Figure FDA0003056288290000042
其中，xe为预瞄点在机器人坐标系xoy下的纵轴坐标，定义为预瞄距离；ye表示预瞄点在机器人坐标系xoy下的横轴坐标，定义为横向预瞄误差；
Figure FDA0003056288290000043
表示机器人在机器人坐标系xoy下位于预瞄点的期望横摆角；
Figure FDA0003056288290000044
表示预瞄点P在大地坐标系XOY中的坐标，Xp、Yp、
Figure FDA0003056288290000045
分别表示预瞄点P在大地坐标系XOY中的X轴坐标、Y轴坐标、预瞄点P处切线与X轴的夹角；
Figure FDA0003056288290000046
表示当前机器人质心在大地坐标系XOY中的坐标，Xc、Yc、
Figure FDA0003056288290000047
分别表示当前机器人质心在大地坐标系XOY中的X轴坐标、Y轴坐标、横摆角；
根据预瞄点在机器人坐标系xoy下坐标
Figure FDA0003056288290000048
和所述单轴运动模型获得机器人的单点视觉预瞄模型；
步骤S23，结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角。
6.如权利要求5所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，在大地坐标系XOY下，所述机器人的单轴运动模型表示为：
Figure FDA0003056288290000049
其中，δf表示机器人的前轮转角；
Figure FDA00030562882900000410
表示机器人质心在大地坐标系XOY下的纵向的加速度；
Figure FDA00030562882900000411
表示机器人质心在大地坐标系XOY下的横向的加速度；
Figure FDA00030562882900000412
表示机器人质心在大地坐标系XOY下的横摆角加速度；变量
Figure FDA0003056288290000051
Figure FDA0003056288290000052
表示机器人质心在大地坐标系XOY下的横向速度，
Figure FDA0003056288290000053
表示机器人质心在大地坐标系XOY下的横摆角速度，m表示机器人质量，Clf表示前轮纵向刚度，Clr表示后轮的纵向刚度，sf表示前轮的滑移率，sr表示后轮的滑移率；变量
Figure FDA0003056288290000054
Ccf表示前轮侧偏刚度，a表示机器人质心距前轴的距离；变量
Figure FDA0003056288290000055
Ccr表示后轮侧偏刚度，b表示机器人质心距后轴的距离；变量
Figure FDA0003056288290000056
变量
Figure FDA0003056288290000057
Iz表示，机器人绕z轴的转动惯量；变量
Figure FDA0003056288290000058
7.如权利要求6所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，所述机器人的单点视觉预瞄模型为：
Figure FDA0003056288290000059
其中，
Figure FDA00030562882900000510
表示在机器人坐标系中机器人的横向偏差变化率，
Figure FDA00030562882900000511
表示机器人坐标系中机器人质心在预瞄点处的横摆角速度，
Figure FDA00030562882900000512
表示当前机器人质心在大地坐标系的横摆角速度，vp表示在大地坐标系中预瞄点期望的纵向速度，ρp表示预瞄点的路径曲率，
Figure FDA00030562882900000513
表示大地坐标系下机器人质心处的横向速度。
8.如权利要求7所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，所述步骤S23结合单点视觉预瞄模型和滑膜控制确定机器人前轮的目标转角的过程具体包括：
步骤S231，基于滑膜变结构理论获得控制对象X的状态方程为：
Figure FDA0003056288290000061
其中，
Figure FDA0003056288290000062
Figure FDA0003056288290000063
分别表示机器人驶向预瞄点过程中任意时间点在大地坐标系中机器人质心的纵向速度、横向速度、横摆角速度，
Figure FDA0003056288290000064
表示在机器人坐标系中的横向偏差变化率，
Figure FDA0003056288290000065
表示对X中各分量求导，得到
Figure FDA0003056288290000066
第一系数矩阵f(X)为：
Figure FDA0003056288290000067
Figure FDA0003056288290000068
表示预瞄点期望纵向加速度；
第二系数矩阵B为：B＝[b1 b2 b3 b4 b5 b6]T；
第三系数矩阵W为：W＝[0 0 0 0 0 vp]T；
步骤S232，以横向预瞄误差及横向预瞄误差变化率作为控制目标，则滑模面可设计为：
Figure FDA0003056288290000069
其中，c表示滑膜参数；s表示滑膜；
步骤S233，设当s→0时，滑膜趋近率
Figure FDA00030562882900000610
设置为等速趋近率，满足如下关系：
Figure FDA00030562882900000611
其中，g表示切换增益，
Figure FDA00030562882900000612
sat(·)表示饱和函数，Δ表示边界层厚度，转换系数q＝1/Δ；
步骤S234，根据关系式
Figure FDA0003056288290000071
得到等式
Figure FDA0003056288290000072
根据等式
Figure FDA0003056288290000073
和控制对象X的状态方程得到机器人前轮的目标转角δd为：
Figure FDA0003056288290000074
9.如权利要求1所述的一种基于预瞄距离自适应的轮式机器人控制方法，其特征在于，所述纵向控制中获得车轮的目标力矩的过程包括：
步骤A，获取机器人纵向总的期望加速度aref为：
Figure FDA0003056288290000075
其中，vc表示机器人当前在大地坐标系下的纵向速度，vp表示在大地坐标系下预瞄点处的期望纵向速度，tp是机器人保持当前纵向速度vc到达预瞄点在大地坐标系下的纵向坐标位置的时间；
步骤B，根据机器人纵向总的期望加速度aref建立如下等式：
Figure FDA0003056288290000076
其中，Fl表示机器人总的期望纵向力，m表示机器人质量，α表示道路坡度，ρ表示空气密度，Cd表示空气阻力系数，Awind表示机器人迎风面面积；
步骤C，每个车轮平均分配力矩，每个车轮的目标力矩为：
Figure FDA0003056288290000077
其中，N表示车轮总数，R表示车轮半径。
10.一种基于预瞄距离自适应的轮式机器人控制系统，其特征在于，包括安装在轮式的机器人上用于检测机器人运动过程中的实时位置、姿态和速度的状态检测模块，以及车轮驱动模块和控制器；所述控制器分别与所述状态检测模块和所述车轮驱动模块连接；所述控制器按照权利要求1－9之一所述的基于预瞄距离自适应的轮式机器人控制方法控制机器人运动。
CN202110502894.4A 2021-05-08 2021-05-08 基于预瞄距离自适应的轮式机器人控制方法及控制系统 Active CN113211438B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110502894.4A CN113211438B (zh) 2021-05-08 2021-05-08 基于预瞄距离自适应的轮式机器人控制方法及控制系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110502894.4A CN113211438B (zh) 2021-05-08 2021-05-08 基于预瞄距离自适应的轮式机器人控制方法及控制系统

Publications (2)

Publication Number Publication Date
CN113211438A true CN113211438A (zh) 2021-08-06
CN113211438B CN113211438B (zh) 2023-06-16

Family

ID=77094581

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110502894.4A Active CN113211438B (zh) 2021-05-08 2021-05-08 基于预瞄距离自适应的轮式机器人控制方法及控制系统

Country Status (1)

Country Link
CN (1) CN113211438B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113752262A (zh) * 2021-09-17 2021-12-07 上海傅利叶智能科技有限公司 用于机器人变阻尼柔顺控制的方法、装置和康复机器人
WO2023093594A1 (zh) * 2021-11-24 2023-06-01 泉州装备制造研究所 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置

Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120179349A1 (en) * 2009-09-30 2012-07-12 Hitachi Automotive Systems, Ltd. Vehicle Motion Control Device
CN106154831A (zh) * 2016-07-25 2016-11-23 厦门大学 一种基于学习法的智能汽车纵向神经滑模控制方法
CN106926840A (zh) * 2017-03-13 2017-07-07 北京理工大学 一种无人车辆极限动力学轨迹跟踪控制系统
CN107097785A (zh) * 2017-05-25 2017-08-29 江苏大学 一种预瞄距离自适应的智能车辆横向控制方法
CN109017760A (zh) * 2018-07-31 2018-12-18 大连民族大学 车辆期望轨迹跟踪方法、装置及滚动时域优化算法
CN109976321A (zh) * 2017-12-27 2019-07-05 湖南中车时代电动汽车股份有限公司 一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统
CN111258323A (zh) * 2020-03-30 2020-06-09 华南理工大学 一种智能车辆轨迹规划与跟踪的联合控制方法
CN112009499A (zh) * 2020-09-14 2020-12-01 北京罗克维尔斯科技有限公司 一种车辆自动驾驶控制方法及装置
US20200385018A1 (en) * 2018-03-23 2020-12-10 Mitsubishi Electric Corporation Path generation device and vehicle control system
CN112109732A (zh) * 2020-09-03 2020-12-22 南京理工大学 一种智能驾驶自适应曲线预瞄方法
CN112148016A (zh) * 2020-09-30 2020-12-29 深兰人工智能(深圳)有限公司 基于模型预测控制算法横纵向解耦的车辆控制方法和装置
CN112141101A (zh) * 2020-09-29 2020-12-29 合肥工业大学 一种基于cnn和lstm的预瞄安全路径的方法及系统
US20210041878A1 (en) * 2019-08-07 2021-02-11 Boston Dynamics, Inc. Navigating a Mobile Robot
CN112668093A (zh) * 2020-12-21 2021-04-16 西南交通大学 一种分布式驱动汽车全轮纵向力最优分配控制方法

Patent Citations (14)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20120179349A1 (en) * 2009-09-30 2012-07-12 Hitachi Automotive Systems, Ltd. Vehicle Motion Control Device
CN106154831A (zh) * 2016-07-25 2016-11-23 厦门大学 一种基于学习法的智能汽车纵向神经滑模控制方法
CN106926840A (zh) * 2017-03-13 2017-07-07 北京理工大学 一种无人车辆极限动力学轨迹跟踪控制系统
CN107097785A (zh) * 2017-05-25 2017-08-29 江苏大学 一种预瞄距离自适应的智能车辆横向控制方法
CN109976321A (zh) * 2017-12-27 2019-07-05 湖南中车时代电动汽车股份有限公司 一种用于智能驾驶系统的轨迹规划方法及智能驾驶系统
US20200385018A1 (en) * 2018-03-23 2020-12-10 Mitsubishi Electric Corporation Path generation device and vehicle control system
CN109017760A (zh) * 2018-07-31 2018-12-18 大连民族大学 车辆期望轨迹跟踪方法、装置及滚动时域优化算法
US20210041878A1 (en) * 2019-08-07 2021-02-11 Boston Dynamics, Inc. Navigating a Mobile Robot
CN111258323A (zh) * 2020-03-30 2020-06-09 华南理工大学 一种智能车辆轨迹规划与跟踪的联合控制方法
CN112109732A (zh) * 2020-09-03 2020-12-22 南京理工大学 一种智能驾驶自适应曲线预瞄方法
CN112009499A (zh) * 2020-09-14 2020-12-01 北京罗克维尔斯科技有限公司 一种车辆自动驾驶控制方法及装置
CN112141101A (zh) * 2020-09-29 2020-12-29 合肥工业大学 一种基于cnn和lstm的预瞄安全路径的方法及系统
CN112148016A (zh) * 2020-09-30 2020-12-29 深兰人工智能(深圳)有限公司 基于模型预测控制算法横纵向解耦的车辆控制方法和装置
CN112668093A (zh) * 2020-12-21 2021-04-16 西南交通大学 一种分布式驱动汽车全轮纵向力最优分配控制方法

Non-Patent Citations (9)

* Cited by examiner, † Cited by third party
Title
PENGYUN ZENG: "A Dynamic Trajectory Planning for Automatic", 《SAE INTERNATIONAL》, 14 April 2020 (2020-04-14) *
刘春辉, 北京理工大学出版社 *
刘春辉, 北京理工大学出版社, pages: 9 - 12 *
曾鹏云: "智能汽车主动避障轨迹优化及自适应协同控制研究", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》 *
曾鹏云: "智能汽车主动避障轨迹优化及自适应协同控制研究", 《中国优秀博硕士学位论文全文数据库(硕士)工程科技Ⅱ辑》, 15 April 2022 (2022-04-15) *
蒋一辰: "融合自适应曲线预瞄的驾驶机器人转向操纵粒子群优化滑模控制", 《西安交通大学学报》 *
蒋一辰: "融合自适应曲线预瞄的驾驶机器人转向操纵粒子群优化滑模控制", 《西安交通大学学报》, 31 March 2021 (2021-03-31) *
赵治国: "无人驾驶车辆路径跟踪控制预瞄距离自适应优", 《机械工程学报》 *
赵治国: "无人驾驶车辆路径跟踪控制预瞄距离自适应优", 《机械工程学报》, 31 December 2018 (2018-12-31), pages 166 - 173 *

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113752262A (zh) * 2021-09-17 2021-12-07 上海傅利叶智能科技有限公司 用于机器人变阻尼柔顺控制的方法、装置和康复机器人
CN113752262B (zh) * 2021-09-17 2022-10-21 上海傅利叶智能科技有限公司 用于机器人变阻尼柔顺控制的方法、装置和康复机器人
WO2023093594A1 (zh) * 2021-11-24 2023-06-01 泉州装备制造研究所 一种误差矫正的地坪磨抛机器人轨迹纠偏方法及装置

Also Published As

Publication number Publication date
CN113211438B (zh) 2023-06-16

Similar Documents

Publication Publication Date Title
CN109795502B (zh) 智能电动汽车路径跟踪模型预测控制方法
CN111273551B (zh) 双舵轮自动导引车轨迹追踪控制方法及系统
CN109733395B (zh) 一种基于可拓优度评价的自动驾驶汽车横向协调控制方法
CN108732921B (zh) 一种自动驾驶汽车横向可拓预瞄切换控制方法
CN108646763A (zh) 一种自主行驶轨迹跟踪控制方法
CN113211438B (zh) 基于预瞄距离自适应的轮式机器人控制方法及控制系统
CN111752150B (zh) 一种轮足机器人四轮协同控制方法
CN112230651A (zh) 一种基于分层控制理论的分布式无人车路径跟踪控制方法
CN113408062A (zh) 自动驾驶全工况路面自适应mpc轨迹跟踪控制及评价方法
CN111158376A (zh) 松软崎岖地形中摇杆摇臂式星球车轨迹跟踪协调控制方法
CN109292018A (zh) 基于同轴式轮腿结构的四轮转向轨迹跟踪控制方法
Qinpeng et al. Path tracking control of wheeled mobile robot based on improved pure pursuit algorithm
Shen et al. A lateral control method for wheel-footed robot based on sliding mode control and steering prediction
Ghani et al. Comparative assessment for two wheels inverted pendulum mobile robot using robust control
Li et al. Anti-disturbance path-following control for snake robots with spiral motion
Arab et al. Motion control of autonomous aggressive vehicle maneuvers
Cheng et al. A game theoretical chassis domain approach to trajectory tracking for automated vehicles
Oliver et al. Fuzzy control to simulate 4 autonomous navigation behaviors in a differential-drive mobile robot
CN112882460A (zh) 基于模型预测控制的无人驾驶车辆动态避障轨迹规划方法
Zhang et al. Adaptive path tracking for unmanned ground vehicle
Nomura et al. Stable traveling control considering slip of wheels in two-wheel mobile robot
Ran et al. Adaptive point stabilization control of two-wheel robot with parameter uncertainties based on human-simulated intelligent backstepping method
Jiang et al. Modeling and LQR control of a multi-DOF two-wheeled robot
Yazar et al. Path Tracking Methods Employed in Lane Centering Systems
CN114896694B (zh) 一种基于两点预瞄的路径跟踪控制方法

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
GR01 Patent grant
GR01 Patent grant
CP03 Change of name, title or address
CP03 Change of name, title or address

Address after: 401120 39-1, Building 1, No. 64, Middle Huangshan Avenue, Yubei District, Chongqing

Patentee after: China Star Network Application Co.,Ltd.

Address before: 618 Liangjiang Avenue, Longxing Town, Yubei District, Chongqing

Patentee before: Dongfanghong Satellite Mobile Communication Co.,Ltd.