CN116578100A - 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法 - Google Patents

一种基于扫刷误差的二自由度动力模型沿边清扫控制方法 Download PDF

Info

Publication number
CN116578100A
CN116578100A CN202310849163.6A CN202310849163A CN116578100A CN 116578100 A CN116578100 A CN 116578100A CN 202310849163 A CN202310849163 A CN 202310849163A CN 116578100 A CN116578100 A CN 116578100A
Authority
CN
China
Prior art keywords
error
vehicle
sweeping
degree
control
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
CN202310849163.6A
Other languages
English (en)
Other versions
CN116578100B (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.)
City Lights Shenzhen Driverless Co ltd
Original Assignee
City Lights Shenzhen Driverless 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 City Lights Shenzhen Driverless Co ltd filed Critical City Lights Shenzhen Driverless Co ltd
Priority to CN202310849163.6A priority Critical patent/CN116578100B/zh
Publication of CN116578100A publication Critical patent/CN116578100A/zh
Application granted granted Critical
Publication of CN116578100B publication Critical patent/CN116578100B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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
    • 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/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process

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

一种基于扫刷误差的二自由度动力模型沿边清扫控制方法
技术领域
本发明涉及智能辅助驾驶技术领域，尤其涉及一种基于扫刷误差的二自由度动力模型沿边清扫控制方法。
背景技术
2022年，我国首次出现人口负增长，目前环卫领域中老龄化明显，环卫人员短缺问题已经越发突出。低速无人环卫车的出现不仅迎合了需求，在高温，严寒的恶劣天气下作业，也提高了环卫作业的安全。
相关技术中，无人驾驶清扫车技术的发展越来越受到各方的重视，无人驾驶清扫车作业中最常见也是最核心的一个场景就是贴边清扫，近年来随着无人驾驶技术的不断发展与普及，应用于特定环境内的小型低速无人清扫车成为无人驾驶最先落地的商业化产品，对于公园、校园、大型商场、工业园区、公共马路等场景的卫生清扫作业，由智能化无人清扫车替代人工清扫成为趋势，而“贴边”清扫是无人清扫车特定环境下完成高效率清扫目标的重要技术手段之一，如何保证清扫车可以精确的“贴边”清扫作业是无人驾驶清扫车落地运营的关键因素之一。
现有技术中，常见的无人环卫车控制方案，低算力有纯跟踪方案，PID控制方案，但它们对于车辆控制多输入多输出的系统存在参数整定困难，PID控制不能很好处理多变量问题，而模型预测控制方案算力高，参数个数多，调参困难。
因此，现有技术还有待提高。
发明内容
鉴于上述现有技术的不足，本发明的目的在于提供一种基于扫刷误差的二自由度动力模型沿边清扫控制方法，旨在将减少算力，同时需要调节的参数简单直观，有利于复杂作业环境的调参适配，更好地完成无人清扫车的沿边作业。
本发明的技术方案如下：
一种基于扫刷误差的二自由度动力模型沿边清扫控制方法，所述方法包括：
获取车辆作业速度范围并建立扫刷误差模型；
根据所述车辆作业速度范围以及所述扫刷误差模型制作离线的线性二次调节器控制表；
获取清扫车的状态信息以及贴边清扫的运动轨迹；
根据所述清扫车的状态信息以及贴边清扫的运动轨迹计算匹配点以及投影点；
根据所述投影点以及当前车身位姿计算误差变化量；
根据车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
在一个实施例中，所述误差变化量为根据所述投影点以及当前车身位姿计算出的车辆前轮扫刷中心到所述投影点的横向误差的误差变化量和根据所述投影点以及当前车身位姿计算出的车辆质心到所述投影点的航向误差的误差变化量。
在一个实施例中，所述建立扫刷误差模型具体包括：
获取车辆质心到所述投影点的横向误差以及所述车辆质心到所述投影点的航向误差；
建立以车辆质心为控制点的二自由度模型；
根据前扫刷误差公式获取车辆的前轮扫刷中心到所述投影点的横向误差；
联立所述二自由度模型，推导得所述扫刷误差模型。
在一个实施例中，根据所述投影点以及当前车身位姿计算误差变化量之后，所述方法还包括：
判断所述横向误差的误差变化量是否异常，并判断所述航向误差的误差变化量是否异常。
在一个实施例中，所述判断所述横向误差的误差变化量是否异常具体包括：
设定一第一阈值，如若所述横向误差的误差变化量小于所设定的第一阈值，则认为定位程序正常，误差异常计数器置零，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量；
如若所述横向误差的误差变化量大于所设定的第一阈值，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
在一个实施例中，所述判断所述航向误差的误差变化量是否异常具体包括：
设定一第二阈值，如若所述航向误差的误差变化量小于所设定的第二阈值，则认为定位程序正常，误差异常计数器置零，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量；
如若所述航向误差的误差变化量大于所设定的第二阈值，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
在一个实施例中，所述的认为定位程序异常，误差异常计数器加一方法之后，所述方法还包括：
判断所述误差异常计数器的数值是否异常，同时设定一第三阈值，如若所述误差异常计数器的数值超过所设定的第三阈值，则控制车辆退出清扫作业模式，停车进入保护模式；
如若所述误差异常计数器的数值不超过所设定的第三阈值，则根据计算出的当前车辆位姿计算当前帧的横向误差及其导数和航向误差及其导数，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
在一个实施例中，所述根据所述车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量的方法之后，所述方法还包括：
计算前馈控制量，并获得当前前轮最优控制量，并将所述当前前轮最优控制量作为控制模块输出给底盘执行，所述当前前轮最优控制量为所述反馈控制量与所述前馈控制量之和。
在一个实施例中，所述获取清扫车的状态信息以及贴边清扫的运动轨迹，具体包括：
通过定位模块、底盘模块获取清扫车状态信息；
通过感知模块、规划模块获取贴边清扫的运动轨迹。
在一个实施例中，所述将所述当前前轮最优控制量作为控制模块输出给底盘执行的方法之后，所述方法还包括：
判断车辆是否结束作业；
若判定车辆结束作业，则停车进入保护模式；
若判定车辆不结束作业，则继续获取清扫车的状态信息以及贴边清扫的运动轨迹。
综上所述：本文提出的一种针对于清扫车清扫场景的控制方案，基于扫刷误差二自由度动力模型沿边清扫控制，适配清扫车的沿边清扫需求，将前扫刷中心作为控制点，建立误差模型，使用离线线性二次调节器控制表（LQR）控制计算，能够有效的减少算力，同时需要调节的参数简单直观，有利于复杂作业环境的调参适配，更好地完成无人清扫车的沿边作业。
附图说明
下面将结合附图及实施例对本发明作进一步说明，附图中：
图1为本发明实施例提供的基于扫刷误差的二自由度动力模型沿边清扫控制方法的流程图；
图2为本发明实施例提供的基于扫刷误差的二自由度动力模型沿边清扫控制方法的车辆运动学模型示意图；
图3为本发明实施例提供的基于扫刷误差的二自由度动力模型沿边清扫控制方法的匹配点和投影点的示意图；
图4为本发明实施例提供的基于扫刷误差的二自由度动力模型沿边清扫控制方法的车辆贴边示意图。
实施方式
为使本发明的目的、技术方案及效果更加清楚、明确，以下对本发明进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。以下结合附图对本发明实施例进行介绍。
本实施例提供的基于扫刷误差的二自由度动力模型沿边清扫控制方法，用于可移动的载体上，特别是无人驾驶清扫车的清扫场景中，为实现自动化程度更高，算力更少的沿边清扫控制效果，本发明基于扫刷误差二自由度动力模型的沿边清扫控制方案，适配沿边清扫需求，将前扫刷中心作为控制点建立误差模型，使用离线线性二次调节器控制表（LQR），减少算力，同时需要调节的参数简单直观，有利于复杂作业环境的调参适配，更好地完成无人清扫车的沿边作业，本发明实施例中，默认坐标系为大地坐标系，请参阅图1-图4，该方法具体包括如下步骤：
S100、获取车辆作业速度范围并建立扫刷误差模型。
本实步骤中，首先在清扫车辆运行的过程中，获取车辆的运行作业速度范围，即车辆的纵向速度范围，本发明实施例中，优选车辆的运行作业速度范围为0~3m/s，根据公式，即可推导建立所述扫刷误差模型。
S200、根据所述车辆作业速度范围以及所述扫刷误差模型制作离线的线性二次调节器控制表。
所述线性二次调节器控制表即LQR，优选的，本实施例中，所述扫刷误差模型即前扫刷误差模型，并对所述车辆作业速度区间在0~3m/s，时间间隔为0.01m/s进行采样，将所述前扫刷误差模型的,/>以及误差代价权重进行矩阵Q，控制代价权重矩阵R带入代数黎卡提方程公式，求得离线LQR查找表/>。
S300、获取清扫车的状态信息以及贴边清扫的运动轨迹。
本实施例中，所述获取清扫车的状态信息包括整车质量m、车辆前轮的侧偏刚度、车辆后轮的侧偏刚度/>、车辆前轴中心到车辆质心的距离a，车辆后轴中心到车辆质心的距离b等。
具体的，本发明实施例中，通过定位模块、底盘模块获取清扫车状态信息：
；
。
通过感知模块、规划模块获取贴边清扫的运动轨迹。
其中，为笛卡尔坐标系下轨迹点的x坐标，/>为笛卡尔坐标系下轨迹点的y坐标;/> 为轨迹点的期望速度，/>为轨迹点的期望加速度；/>为轨迹点的航向角，/>为轨迹点的航向角速度；/>为轨迹点的曲率。
S400、根据所述清扫车的状态信息以及贴边清扫的运动轨迹计算匹配点以及投影点/>。
在步骤S300获取到车辆的状态信息以及贴边清扫的运动轨迹后，可根据欧氏距离，计算清扫车与轨迹的最近点，作为匹配点，确定所述匹配点/>后，在误差允许的范围内，近似匹配点/>到投影点/>的曲率不变，/>，在沿所述运动轨迹方向做所述匹配点/>的切线，在所述切线方向上距离车身最近的点即为投影点/>。
S500、根据所述投影点以及当前车身位姿计算误差变化量。
根据所述误差变化量，判断定位程序是否异常。
本发明另一实施例中，可以为定位丢失提供一个保障方案，若车辆在行驶过程中，失去定位，而定位丢失会造成计算出来的误差变化量异常，则短时间内，优选5-10个控制周期内，可以用上一帧的车辆状态估计当前车辆状态，代替定位的信息，重新计算所述误差变化量，这个时候计算出来的误差变化量是正常可以用的，等待定位恢复后，再使用定位信息去计算误差变化量。
S600、根据所述车辆作业速度范围和所述离线的线性二次调节器控制表LQR计算反馈控制量。
所述车辆作业速度范围为0-3m/s，根据当前的车辆作业车速v以及离线的线性二次调节器控制表LQR，可以计算出作业速度范围内的每个速度下，反馈控制量。
进一步的，本实施例中，所述误差变化量为根据所述投影点以及当前车身位姿计算出的所述车辆前轮扫刷中心到所述投影点/>的横向误差/>的误差变化量和根据所述投影点/>以及当前车身位姿计算出的所述车辆质心到所述投影点/>的航向误差/>的误差变化量。
本发明实施例可根据投影点以及当前车身位姿计算出当前帧的所述车辆前轮扫刷中心到所述投影点/>的横向误差/>，以及上一帧的横向误差/>，根据上一帧的横向误差/>以及当前帧的横向误差/>的差值/>，即可计算出横向误差的误差变化量。
进一步的，可根据所述投影点以及当前车身位姿计算出当前帧的所述车辆质心到所述投影点的航向误差/>，以及上一帧的航向误差/>，根据上一帧的航向误差/>以及当前帧的航向误差/>的差值/>，即可计算出航向误差的误差变化量/>。
再进一步的，在本实施例中，具体的，所述步骤S100中，建立扫刷误差模型具体包括：
S101、获取所述车辆质心到所述投影点的横向误差/>以及所述车辆质心到所述投影点/>的航向误差/>。
本步骤中，请参阅图3，由于本实施例近似匹配点到投影点/>的曲率不变，，即有/>，首先，根据下述公式计算清扫车质心与轨迹投影点/>的航向角/>，该航向角/>为匹配点/>的航向角加上匹配点/>到投影点/>弧长所对应的角度。近似匹配点/>到投影点/>曲率不变且大小等于匹配点/>的曲率，近似（/>）在匹配点/>上的投影大小等于弧长。根据弧长等于曲率乘以弧长所对应的角度，则有，
。
计算所述车辆质心到所述投影点的横向误差/>：
。
其中，为匹配点/>的法向量。
再计算所述车辆质心到所述投影点横向误差的一阶导数/>：
。
再计算所述车辆质心与所述投影点的航向误差：
。
再计算所述车辆质心与所述投影点航向误差的一阶导数：
。
S102、建立以车辆质心为控制点的二自由度模型。
本实步骤中，利用车辆动力学建模的方法，其中，二自由度是指车辆在平面上有两个自由度，即横向移动和纵向移动，本实施例将车辆质心作为控制点来进行模型设计和控制，可以帮助预测和优化车辆在不同行驶条件下的性能。
具体的，本发明利用如下公式建立以车辆质心为控制点的二自由度模型：
;
;
;
;
。
其中，为车辆前轮的侧偏刚度，/>为车辆后轮的侧偏刚度，m为整车质量，为车辆z轴转动惯量，/>为车辆前轴中心到质心的距离，/>为车辆后轴中心到质心的距离，/>为车辆纵向速度，/>为车辆质心到投影点的横向误差，/>为车辆质心到投影点的横向误差的一阶导数，/>为车辆质心到投影点的航向误差，/>为车辆质心到投影点的航向误差的一阶导数。
S103、根据前扫刷误差公式获取车辆的前轮扫刷中心到所述投影点的横向误差。
所述前扫刷误差公式如下：
;
。
进一步的，请参阅图4，本发明实施例中，是从底盘获取的车辆速度，计算所述车辆前轮扫刷中心到所述投影点的横向误差/>：
。
其中，为以车辆质心为原点的车身坐标系下车辆质心到前轮扫刷中心在x轴投影的大小，/>为以车辆质心为原点的车身坐标系下车辆质心到前轮扫刷中心在y轴投影的大小。
再计算所述车辆前轮扫刷中心到所述投影点横向误差的一阶导数：
。
S104、联立所述以车辆质心为控制点的二自由度模型，推导得所述扫刷误差模型/>。
A2=。
。
。
。
。
更进一步的，优选的，本发明实施例中，选择控制周期T为20ms，离散矩阵如下：
;
。
对工作速度区间0~3m/s，间隔0.01m/s进行采样，以及前扫刷误差模型的,/>以及误差代价权重矩阵Q，控制代价权重矩阵R带入公式，即可求得离线LQR查找表/>；;
。
更进一步的，本实施例中，所述步骤S500、根据所述投影点以及当前车身位姿计算误差变化量之后，所述方法还包括：
S501、判断所述横向误差的误差变化量/>是否异常，并判断所述航向误差的误差变化量/>是否异常。
判断两个误差变化量是否异常，只需判定其中任意一个的误差变化量异常即可判定定位程序异常。
更进一步的，所述判断所述横向误差的误差变化量/>是否异常具体包括：
设定一第一阈值，优选的，本发明实施例中所述第一阈值为15cm，如若所述横向误差的误差变化量/>小于所设定的第一阈值15cm，则认为定位程序正常，误差异常计数器置零，并根据当前所述车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量/>。
如若所述横向误差的误差变化量/>大于所设定的第一阈值15cm，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
更进一步的，所述判断所述航向误差的误差变化量/>是否异常具体包括：
设定一第二阈值，优选的，本发明实施例中所述第二阈值为10°,如若所述航向误差的误差变化量/>小于所设定的第二阈值10°，则认为定位程序正常，误差异常计数器置零，并根据当前所述车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量/>。
如若所述航向误差的误差变化量/>大于所设定的第二阈值10°，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
更进一步的，本发明的具体实施例中，所述上一帧车辆位姿，根据所述上一帧车辆位姿计算当前车辆位姿/>。
。
。
。
。
。
。
更进一步的，所述的认为定位程序异常，误差异常计数器加一的方法之后，所述方法还包括：
S502、判断所述误差异常计数器的数值是否异常，同时设定一第三阈值，优选的，本发明实施例中所述第三阈值为5次,如若所述误差异常计数器的数值超过设定的所述第三阈值5，则控制车辆退出清扫作业模式，停车进入保护模式；
如若所述误差异常计数器的数值不超过所设定的第三阈值5，则根据所述计算出的当前车辆位姿计算当前帧的横向误差及其导数和航向误差及其导数，并根据当前所述车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
更进一步的，所述步骤S600、根据所述车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量的方法之后，所述方法还包括：
S700、请参阅图2，计算前馈控制量，并获得当前前轮最优控制量/>，并将所述当前前轮最优控制量/>作为控制模块输出给底盘执行，所述当前前轮最优控制量/>为所述反馈控制量/>与所述前馈控制量/>之和。
具体的，根据下述公式，前馈控制量。
。
因此，所述当前前轮最优控制量，即前轮转角为：
。
具体的，底盘控制模块接收到当前前轮最优控制量的信号后，控制前轮的旋转角度，从而实现控制车辆的贴边清扫方式。
为了达到实时控制以及及时停车的效果，所述将所述当前前轮最优控制量作为控制模块输出给底盘执行的方法之后，所述方法还包括：
S800、判断车辆是否结束作业；
S801、若判定车辆结束作业，则停车进入保护模式；
S802、若判定车辆不结束作业，则继续步骤S300、获取清扫车的状态信息以及贴边清扫的运动轨迹；
以此循环，形成周期控制，从而实现了在控制周期内对于车辆贴边清扫模式的实时控制。
为了更好的理解本发明，将本发明的控制流程总结如下：
本发明首先根据二自由度车辆动力模型、车辆质心误差、前扫刷中心误差建立沿边清扫的扫刷误差模型，根据所述扫刷误差模型以及作业速度制作离线LQR（线性二次型调节器）控制表；在控制周期初始，获取车辆的运动轨迹以及车辆状态信息作为输入，计算匹配点与投影点；根据计算的投影点以及车辆位姿计算误差变化量。如误差变化量异常，进入保护模式；在保护模式下，当前控制周期会使用航机推算的位姿重新计算横向误差、航向误差，继续接下来的控制步骤，直至定位恢复正常或连续异常次数超过第三阈值，进入停车保护；如误差变化量正常，则根据离线LQR控制表计算反馈控制转向；再根据公式计算前馈控制转向，最终控制量为反馈控制转向与前馈控制转向之和，输出给底盘，结束控制周期。
与现有技术相比，本发明的有益效果：
1、本发明根据清扫车的实际需求，以扫刷为目标，建立其误差模型，使用线性二次调节器，直接控制扫刷对于路沿的贴边跟踪，直接调节Q矩阵中的横向误差权重和航向误差权重大小控制横向误差和航向误差的收敛速度以及超调，调参上快捷方便。
2、本发明根据速度，制作离线LQR查找表，避免迭代代数黎卡提方程求解，相比模型预测控制计算量大幅度的减少。
3、本发明使用投影点替代匹配点计算航向误差，对匹配点与匹配点之间进行近似插值。相比传统算法中使用匹配点作为跟踪位姿，避免了前后控制周期匹配点变化所导致的误差跳变，环卫车输出转向信号更加平滑，贴边清扫更加顺畅。
4、本发明还为定位丢失提供一个保障方案，在定位丢失的时间里仍可正常作业，等待定位模块恢复，直至超时才会触发停车保护，增强了无人环卫作业系统的鲁棒性。
综上，本发明公开的一种基于扫刷误差的二自由度动力模型沿边清扫控制方法，根据二自由度车辆动力模型、车辆质心误差、前扫刷中心误差建立沿边清扫的扫刷误差模型，根据模型以及作业速度制作离线线性二次型调节器控制表，计算反馈控制转向，并计算前馈控制转向，最终控制量为反馈控制转向与前馈控制转向之和，输出，结束控制周期。本发明提出的基于扫刷误差二自由度动力模型沿边清扫控制方案，适配沿边清扫需求将前扫刷中心作为控制点建立误差模型，使用离线线性二次调节器控制表，减少算力，同时需要调节的参数简单直观，有利于复杂作业环境的调参适配，贴边清扫更加顺畅，更好地完成无人清扫车的沿边作业。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (10)

1.一种基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述方法包括：
获取车辆作业速度范围并建立扫刷误差模型；
根据所述车辆作业速度范围以及所述扫刷误差模型制作离线的线性二次调节器控制表；
获取清扫车的状态信息以及贴边清扫的运动轨迹；
根据所述清扫车的状态信息以及贴边清扫的运动轨迹计算匹配点以及投影点；
根据所述投影点以及当前车身位姿计算误差变化量；
根据车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
2.根据权利要求1所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述误差变化量为根据所述投影点以及当前车身位姿计算出的车辆前轮扫刷中心到所述投影点的横向误差的误差变化量和根据所述投影点以及当前车身位姿计算出的车辆质心到所述投影点的航向误差的误差变化量。
3.根据权利要求1所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述建立扫刷误差模型具体包括：
获取车辆质心到所述投影点的横向误差以及车辆质心到所述投影点的航向误差；
建立以车辆质心为控制点的二自由度模型；
根据前扫刷误差公式获取车辆的前轮扫刷中心到所述投影点的横向误差；
联立所述二自由度模型，推导得所述扫刷误差模型。
4.根据权利要求2所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，根据所述投影点以及当前车身位姿计算误差变化量之后，所述方法还包括：
判断所述横向误差的误差变化量是否异常，并判断所述航向误差的误差变化量是否异常。
5.根据权利要求4所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述判断所述横向误差的误差变化量是否异常具体包括：
设定一第一阈值，如若所述横向误差的误差变化量小于所设定的第一阈值，则认为定位程序正常，误差异常计数器置零，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量；
如若所述横向误差的误差变化量大于所设定的第一阈值，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
6.根据权利要求4所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述判断所述航向误差的误差变化量是否异常具体包括：
设定一第二阈值，如若所述航向误差的误差变化量小于所设定的第二阈值，则认为定位程序正常，误差异常计数器置零，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量；
如若所述航向误差的误差变化量大于所设定的第二阈值，则认为定位程序异常，误差异常计数器加一，并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
7.根据权利要求5或6所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述的认为定位程序异常，误差异常计数器加一方法之后，所述方法还包括：
判断所述误差异常计数器的数值是否异常，同时设定一第三阈值，如若所述误差异常计数器的数值超过所设定的第三阈值，则控制车辆退出清扫作业模式，停车进入保护模式；
如若所述误差异常计数器的数值不超过所设定的第三阈值，则根据计算出的所述当前车辆位姿计算当前帧的横向误差及其导数和航向误差及其导数，并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
8.根据权利要求7所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量的方法之后，所述方法还包括：
计算前馈控制量，并获得当前前轮最优控制量，并将所述当前前轮最优控制量作为控制模块输出给底盘执行，所述当前前轮最优控制量为所述反馈控制量与所述前馈控制量之和。
9.根据权利要求1所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述获取清扫车的状态信息以及贴边清扫的运动轨迹，具体包括：
通过定位模块、底盘模块获取清扫车状态信息；
通过感知模块、规划模块获取贴边清扫的运动轨迹。
10.根据权利要求8所述的基于扫刷误差的二自由度动力模型沿边清扫控制方法，其特征在于，所述将所述当前前轮最优控制量作为控制模块输出给底盘执行的方法之后，所述方法还包括：
判断车辆是否结束作业；
若判定车辆结束作业，则停车进入保护模式；
若判定车辆不结束作业，则继续获取清扫车的状态信息以及贴边清扫的运动轨迹。
CN202310849163.6A 2023-07-12 2023-07-12 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法 Active CN116578100B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202310849163.6A CN116578100B (zh) 2023-07-12 2023-07-12 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202310849163.6A CN116578100B (zh) 2023-07-12 2023-07-12 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法

Publications (2)

Publication Number Publication Date
CN116578100A true CN116578100A (zh) 2023-08-11
CN116578100B CN116578100B (zh) 2023-11-21

Family

ID=87534467

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202310849163.6A Active CN116578100B (zh) 2023-07-12 2023-07-12 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法

Country Status (1)

Country Link
CN (1) CN116578100B (zh)

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110307836A (zh) * 2019-07-10 2019-10-08 北京智行者科技有限公司 一种用于无人清扫车辆贴边清扫的精确定位方法
CN110895334A (zh) * 2019-12-25 2020-03-20 广州赛特智能科技有限公司 基于激光雷达和gps融合虚拟墙无人清扫车校准装置及方法
CN113120001A (zh) * 2021-05-11 2021-07-16 雄狮汽车科技(南京)有限公司 自动驾驶车辆的横向控制方法、装置及车辆
CN113419420A (zh) * 2021-06-30 2021-09-21 东风悦享科技有限公司 一种车辆线性二自由度动力学模型循迹控制方法及系统
CN113753080A (zh) * 2021-08-31 2021-12-07 的卢技术有限公司 一种自动驾驶汽车横向运动自适应参数控制方法
CN115123219A (zh) * 2022-06-02 2022-09-30 东风柳州汽车有限公司 一种基于模糊pid的车道保持控制方法及装置
CN116080682A (zh) * 2022-12-30 2023-05-09 同济大学 一种面向复杂道路环境的自动驾驶车辆横向控制方法
CN116279410A (zh) * 2023-02-17 2023-06-23 中汽创智科技有限公司 航向误差补偿方法、系统、装置和存储介质

Patent Citations (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110307836A (zh) * 2019-07-10 2019-10-08 北京智行者科技有限公司 一种用于无人清扫车辆贴边清扫的精确定位方法
CN110895334A (zh) * 2019-12-25 2020-03-20 广州赛特智能科技有限公司 基于激光雷达和gps融合虚拟墙无人清扫车校准装置及方法
CN113120001A (zh) * 2021-05-11 2021-07-16 雄狮汽车科技(南京)有限公司 自动驾驶车辆的横向控制方法、装置及车辆
CN114655248A (zh) * 2021-05-11 2022-06-24 雄狮汽车科技(南京)有限公司 自动驾驶车辆的横向控制方法、装置及车辆
CN113419420A (zh) * 2021-06-30 2021-09-21 东风悦享科技有限公司 一种车辆线性二自由度动力学模型循迹控制方法及系统
CN113753080A (zh) * 2021-08-31 2021-12-07 的卢技术有限公司 一种自动驾驶汽车横向运动自适应参数控制方法
CN115123219A (zh) * 2022-06-02 2022-09-30 东风柳州汽车有限公司 一种基于模糊pid的车道保持控制方法及装置
CN116080682A (zh) * 2022-12-30 2023-05-09 同济大学 一种面向复杂道路环境的自动驾驶车辆横向控制方法
CN116279410A (zh) * 2023-02-17 2023-06-23 中汽创智科技有限公司 航向误差补偿方法、系统、装置和存储介质

Also Published As

Publication number Publication date
CN116578100B (zh) 2023-11-21

Similar Documents

Publication Publication Date Title
CN109795502B (zh) 智能电动汽车路径跟踪模型预测控制方法
Soetanto et al. Adaptive, non-singular path-following control of dynamic wheeled robots
CN110780594B (zh) 一种智能车的路径跟踪方法及系统
CN106372758B (zh) 一种辅助泊车系统的路径跟随方法及装置
Lapierre et al. Nonsingular path following control of a unicycle in the presence of parametric modelling uncertainties
CN114435470B (zh) 自动倒车控制方法、装置、车辆和存储介质
CN110626340B (zh) 基于灰狼算法的智能汽车路径跟踪控制系统及方法
CN112622895B (zh) 一种应用于自动驾驶的轨迹控制的预估控制方法
Yin et al. Trajectory tracking based on adaptive sliding mode control for agricultural tractor
Wang et al. Research on logistics autonomous mobile robot system
CN116552550A (zh) 基于参数不确定性和横摆稳定性的车辆轨迹跟踪控制系统
CN110687908A (zh) 基于ros的园区无人车轨迹生成系统及其控制方法
CN116048081A (zh) 一种考虑安全边界约束的自动驾驶车辆决策与规控方法
CN113553726B (zh) 一种冰雪环境下的主从博弈型人机协同转向控制方法
WO2022248399A1 (en) An adaptive path following algorithm for heavy-duty vehicles
CN116578100B (zh) 一种基于扫刷误差的二自由度动力模型沿边清扫控制方法
CN115993089B (zh) 基于pl-icp的在线四舵轮agv内外参标定方法
CN112379675B (zh) 环卫机器人的控制方法及系统、环卫机器人
CN117163050B (zh) 一种基于运动模型的预测控制算法
Zhang et al. Design of AGV Chassis Motion Control System Based on MWorks
CN114895556A (zh) 一种轮式机器人轨迹跟踪控制方法及计算机可读介质
Sasiadek et al. Path tracking of an autonomous LHD articulated vehicle
CN114326728B (zh) 高安全裕度的单agv智能车库路径跟踪控制系统及方法
CN117519133B (zh) 一种无人驾驶采棉机轨迹跟踪控制方法
CN117922548A (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