发明内容
鉴于上述现有技术的不足,本发明的目的在于提供一种基于扫刷误差的二自由度动力模型沿边清扫控制方法,旨在将减少算力,同时需要调节的参数简单直观,有利于复杂作业环境的调参适配,更好地完成无人清扫车的沿边作业。
本发明的技术方案如下:
一种基于扫刷误差的二自由度动力模型沿边清扫控制方法,所述方法包括:
获取车辆作业速度范围并建立扫刷误差模型;
根据所述车辆作业速度范围以及所述扫刷误差模型制作离线的线性二次调节器控制表;
获取清扫车的状态信息以及贴边清扫的运动轨迹;
根据所述清扫车的状态信息以及贴边清扫的运动轨迹计算匹配点以及投影点;
根据所述投影点以及当前车身位姿计算误差变化量;
根据车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
在一个实施例中,所述误差变化量为根据所述投影点以及当前车身位姿计算出的车辆前轮扫刷中心到所述投影点的横向误差的误差变化量和根据所述投影点以及当前车身位姿计算出的车辆质心到所述投影点的航向误差的误差变化量。
在一个实施例中,所述建立扫刷误差模型具体包括:
获取车辆质心到所述投影点的横向误差以及所述车辆质心到所述投影点的航向误差;
建立以车辆质心为控制点的二自由度模型;
根据前扫刷误差公式获取车辆的前轮扫刷中心到所述投影点的横向误差;
联立所述二自由度模型,推导得所述扫刷误差模型。
在一个实施例中,根据所述投影点以及当前车身位姿计算误差变化量之后,所述方法还包括:
判断所述横向误差的误差变化量是否异常,并判断所述航向误差的误差变化量是否异常。
在一个实施例中,所述判断所述横向误差的误差变化量是否异常具体包括:
设定一第一阈值,如若所述横向误差的误差变化量小于所设定的第一阈值,则认为定位程序正常,误差异常计数器置零,并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量;
如若所述横向误差的误差变化量大于所设定的第一阈值,则认为定位程序异常,误差异常计数器加一,并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
在一个实施例中,所述判断所述航向误差的误差变化量是否异常具体包括:
设定一第二阈值,如若所述航向误差的误差变化量小于所设定的第二阈值,则认为定位程序正常,误差异常计数器置零,并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量;
如若所述航向误差的误差变化量大于所设定的第二阈值,则认为定位程序异常,误差异常计数器加一,并根据车辆运动学模型以及上一帧车辆位姿计算当前车辆位姿。
在一个实施例中,所述的认为定位程序异常,误差异常计数器加一方法之后,所述方法还包括:
判断所述误差异常计数器的数值是否异常,同时设定一第三阈值,如若所述误差异常计数器的数值超过所设定的第三阈值,则控制车辆退出清扫作业模式,停车进入保护模式;
如若所述误差异常计数器的数值不超过所设定的第三阈值,则根据计算出的当前车辆位姿计算当前帧的横向误差及其导数和航向误差及其导数,并根据当前车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量。
在一个实施例中,所述根据所述车辆作业速度和所述离线的线性二次调节器控制表计算反馈控制量的方法之后,所述方法还包括:
计算前馈控制量,并获得当前前轮最优控制量,并将所述当前前轮最优控制量作为控制模块输出给底盘执行,所述当前前轮最优控制量为所述反馈控制量与所述前馈控制量之和。
在一个实施例中,所述获取清扫车的状态信息以及贴边清扫的运动轨迹,具体包括:
通过定位模块、底盘模块获取清扫车状态信息;
通过感知模块、规划模块获取贴边清扫的运动轨迹。
在一个实施例中,所述将所述当前前轮最优控制量作为控制模块输出给底盘执行的方法之后,所述方法还包括:
判断车辆是否结束作业;
若判定车辆结束作业,则停车进入保护模式;
若判定车辆不结束作业,则继续获取清扫车的状态信息以及贴边清扫的运动轨迹。
综上所述:本文提出的一种针对于清扫车清扫场景的控制方案,基于扫刷误差二自由度动力模型沿边清扫控制,适配清扫车的沿边清扫需求,将前扫刷中心作为控制点,建立误差模型,使用离线线性二次调节器控制表(LQR)控制计算,能够有效的减少算力,同时需要调节的参数简单直观,有利于复杂作业环境的调参适配,更好地完成无人清扫车的沿边作业。
实施方式
为使本发明的目的、技术方案及效果更加清楚、明确,以下对本发明进一步详细说明。应当理解,此处所描述的具体实施例仅仅用以解释本发明,并不用于限定本发明。以下结合附图对本发明实施例进行介绍。
本实施例提供的基于扫刷误差的二自由度动力模型沿边清扫控制方法,用于可移动的载体上,特别是无人驾驶清扫车的清扫场景中,为实现自动化程度更高,算力更少的沿边清扫控制效果,本发明基于扫刷误差二自由度动力模型的沿边清扫控制方案,适配沿边清扫需求,将前扫刷中心作为控制点建立误差模型,使用离线线性二次调节器控制表(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、本发明还为定位丢失提供一个保障方案,在定位丢失的时间里仍可正常作业,等待定位模块恢复,直至超时才会触发停车保护,增强了无人环卫作业***的鲁棒性。
综上,本发明公开的一种基于扫刷误差的二自由度动力模型沿边清扫控制方法,根据二自由度车辆动力模型、车辆质心误差、前扫刷中心误差建立沿边清扫的扫刷误差模型,根据模型以及作业速度制作离线线性二次型调节器控制表,计算反馈控制转向,并计算前馈控制转向,最终控制量为反馈控制转向与前馈控制转向之和,输出,结束控制周期。本发明提出的基于扫刷误差二自由度动力模型沿边清扫控制方案,适配沿边清扫需求将前扫刷中心作为控制点建立误差模型,使用离线线性二次调节器控制表,减少算力,同时需要调节的参数简单直观,有利于复杂作业环境的调参适配,贴边清扫更加顺畅,更好地完成无人清扫车的沿边作业。
应当理解的是,本发明的应用不限于上述的举例,对本领域普通技术人员来说,可以根据上述说明加以改进或变换,所有这些改进和变换都应属于本发明所附权利要求的保护范围。