阅读说明：本技术 环卫车及其车身姿态检测方法、车身稳定方法及对应系统 (Sanitation vehicle, vehicle body posture detection method thereof, vehicle body stabilization method and corresponding system ) 是由 李群峰 赵玉东 张冬 李利锋 翟锋涛 张增 于 2018-07-23 设计创作，主要内容包括：本发明涉及环卫车及其车身姿态检测方法、车身稳定方法及对应系统,首先建立车身三维坐标系,其中,X轴的方向为车头朝向方向,Y轴的方向为与车头朝向方向相垂直的车身横向方向,Z轴的方向为垂直于车身底盘平面的方向；然后检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度,以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；最后根据得到的各数据获取车身姿态,还能够根据车身姿态进行车身稳定。该姿态检测方式是自动检测方式,相较于人工凭借驾驶经验判断环卫车车身姿态的方式,可靠性得到很大的提升。(The invention relates to a sanitation vehicle, a vehicle body posture detection method, a vehicle body stabilizing method and a corresponding system thereof.A vehicle body three-dimensional coordinate system is established at first, wherein the direction of an X axis is the direction of the heading direction of a vehicle head, the direction of a Y axis is the transverse direction of the vehicle body which is vertical to the heading direction of the vehicle head, and the direction of a Z axis is the direction vertical to the plane of a chassis of the vehicle body; then detecting the acceleration of the vehicle body in the X-axis direction, the acceleration in the Y-axis direction and the acceleration in the Z-axis direction, and the roll angle of the vehicle body taking the X-axis as a rotating shaft, the pitch angle of the vehicle body taking the Y-axis as a rotating shaft and the yaw angle of the vehicle body taking the Z-axis as a rotating shaft; and finally, acquiring the posture of the vehicle body according to the obtained data, and stabilizing the vehicle body according to the posture of the vehicle body. The posture detection mode is an automatic detection mode, and compared with a mode of manually judging the posture of the sanitation vehicle body through the driving experience, the reliability is greatly improved.)

环卫车及其车身姿态检测方法、车身稳定方法及对应系统

技术领域

本发明涉及环卫车及其车身姿态检测方法、车身稳定方法及对应系统。

背景技术

环卫车是针对路面清洁的专用车辆。由于环卫车的工作特性，环卫车在工作过程中，有时候道路环境比较恶劣，比如：道路上可能会有突出异物，或者道路上有不知深浅的水沟，而且，环卫车的工作道路还可能会出现坡度比较陡的上坡或者下坡。环卫车在通过上述恶劣路面时，车身会出现一定程度的晃动，更有甚者，还可能使车辆侧翻。因此，环卫车亟需车身姿态判断策略或者车身稳定控制策略，以实时获知车身姿态以及根据车身姿态稳定车身。但是，目前的环卫车车身姿态判断策略或者车身稳定控制策略均为人为判断方式，驾驶员凭借驾驶经验人工判断车身姿态或者车身稳定情况，当然，这种人为判断方式的可靠性较低。

发明内容

本发明的目的是提供环卫车及其车身姿态检测方法和系统，用以解决人工凭借驾驶经验判断环卫车车身姿态的方式可靠性较差的问题。本发明同时提供环卫车及其车身稳定方法和系统，用以解决传统的环卫车车身稳定方式的可靠性较差的问题。

为实现上述目的，本发明包括以下技术方案。

一种环卫车车身姿态检测方法，包括以下步骤：

(1)建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

(2)检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

(3)根据得到的各数据获取车身姿态。

首先建立车身三维坐标系，然后根据建立的坐标系检测车身在各轴方向上的加速度以及与各轴相关的车身角度，最后根据得到的各数据获取车身姿态。通常来说，车身姿态具体体现在三维坐标系中三个轴对应的加速度以及角度数据，因此，根据得到的三维坐标系检测三个轴的数据信息能够准确得到车身姿态，所以，车身姿态检测精度得到很大地提升。而且，该检测方式是自动检测方式，相较于人工凭借驾驶经验判断环卫车车身姿态的方式，可靠性得到很大的提升，而且，检测结果没有迟滞性。

进一步地，步骤(2)和步骤(3)之间还包括以下步骤：采用卡尔曼滤波处理方式对检测得到的各数据进行滤波。

一种环卫车车身姿态检测系统，包括：

车身三维坐标系建立模块，用于建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

数据检测模块，用于检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

车身姿态获取模块，用于根据得到的各数据获取车身姿态。

进一步地，所述检测系统还包括卡尔曼滤波模块，用于采用卡尔曼滤波处理方式对数据检测模块检测得到的各数据进行滤波。

一种环卫车，包括一种环卫车车身姿态检测系统，所述检测系统包括：

车身三维坐标系建立模块，用于建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

数据检测模块，用于检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

车身姿态获取模块，用于根据得到的各数据获取车身姿态。

一种环卫车车身稳定方法，包括以下步骤：

(1)建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

(2)检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

(3)根据得到的各数据获取车身姿态；

(4)根据得到的车身姿态进行车身稳定控制。

首先建立车身三维坐标系，然后根据建立的坐标系检测车身在各轴方向上的加速度以及与各轴相关的车身角度，接着根据得到的各数据获取车身姿态，最后根据车身姿态进行车身稳定控制。通常来说，车身姿态具体体现在三维坐标系中三个轴对应的加速度以及角度数据，因此，根据得到的三维坐标系检测三个轴的数据信息能够准确得到车身姿态，所以，车身姿态检测精度得到很大地提升，进而提升车身稳定控制的可靠性。

进一步地，步骤(2)和步骤(3)之间还包括以下步骤：采用卡尔曼滤波处理方式对检测得到的各数据进行滤波。

一种环卫车车身稳定系统，包括：

车身三维坐标系建立模块，用于建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

数据检测模块，用于检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

车身姿态获取模块，用于根据得到的各数据获取车身姿态；

车身稳定模块，用于根据得到的车身姿态进行车身稳定控制。

进一步地，所述车身稳定系统还包括卡尔曼滤波模块，用于采用卡尔曼滤波处理方式对数据检测模块检测得到的各数据进行滤波。

一种环卫车，包括一种环卫车车身稳定系统，所述车身稳定系统包括：

车身三维坐标系建立模块，用于建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

数据检测模块，用于检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

车身姿态获取模块，用于根据得到的各数据获取车身姿态；

车身稳定模块，用于根据得到的车身姿态进行车身稳定控制。

附图说明

图1是飞机机身三维坐标系示意图；

图2是车身三维坐标系示意图；

图3是车身稳定控制过程示意图。

具体实施方式

环卫车实施例一

本实施例提供一种环卫车，比如新能源环卫车。该环卫车包括一种环卫车车身姿态检测系统，当然，还包括其他的组成部分，其他的组成部分不是本环卫车的保护重点，这里就不再具体说明。

该检测系统包括：

车身三维坐标系建立模块，用于建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

数据检测模块，用于检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

车身姿态获取模块，用于根据得到的各数据获取车身姿态。

因此，该检测系统中，通过各个组成部分的功能作用限定对应的组成部分，因此，该检测系统相当于一种包括以下步骤的环卫车车身姿态检测方法：

(1)建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

(2)检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

(3)根据得到的各数据获取车身姿态。

以下给出一种具体的实施方式来说明上述环卫车车身姿态检测系统，即环卫车车身姿态检测方法。

参照飞机机身三维坐标系建立车身三维坐标系，图1为飞机机身三维坐标系。如图2所示，车身三维坐标系包括X轴、Y轴和Z轴，其中，X轴的方向为车头朝向方向，那么将X轴称为翻滚轴X(ROLL)；Y轴的方向为与车头朝向方向相垂直的车身横向方向(即车身垂直横穿方向)，那么将Y轴称为俯仰轴Y(PITCH)；Z轴的方向为垂直于车身底盘平面的方向，那么将Z轴称为偏航轴Z(YAW)。

本实施例中，检测相关数据的数据检测模块以6轴陀螺仪为例，比如MPU60X0系列6轴陀螺仪。而6轴陀螺仪并不仅仅只有一种实现方式，而是有三种构成方式，分别是：6轴陀螺仪、9轴陀螺仪、3轴加速度计+3轴陀螺仪。当然，上述数据可通过对接单独的加速度计和陀螺仪来实现，亦可通过9轴集成式陀螺仪来实现，不过考虑到车身充满金属和高低压电路，9轴陀螺仪中的3轴磁场数据起不到作用，会造成检测数据不准确，因此，最优方案就是采用集成式6轴陀螺仪。

因此，6轴陀螺仪的功能是：检测车身在X轴方向上的加速度Gx、在Y轴方向上的加速度Gy和在Z轴方向上的加速度Gz，以及以X轴为转动轴的车身侧倾角度(即翻滚角roll)、以Y轴为转动轴的车身俯仰角度(即俯仰角pitch)和以Z轴为转动轴的车身横摆角度(即偏航角yaw)，如图2所示。其中，俯仰角pitch实时指示车身处于上坡还是下坡姿态，以及坡的角度，动态反应时间低于10ms，而且，配合加速度Gx可以判断得到是上坡还是下坡；翻滚角roll实时指示车身侧倾的角度，左倾还是右倾，以及倾斜角度；偏航角yaw反映车身转弯的幅度。

本实施例中，根据6轴陀螺仪的检测数据来检测车身姿态的车身姿态获取模块具体为整车控制器，即整车控制器用于获取车身姿态。那么，整车控制器上增加6轴陀螺仪的数据接口，6轴陀螺仪集成了3轴加速度计和3轴陀螺仪，通过IIC或者SPI接口与整车控制器上的相关数据接口进行对接，接收6轴陀螺仪的检测数据。因此，6轴陀螺仪不仅能够提供车身倾斜姿态，亦可提供车身加减速的加速度值，整车控制器进行车辆动态判断，尤其是刹车介入和急加速方面。

进一步地，6轴陀螺仪的检测数据先进行滤波处理，具体采用卡尔曼滤波处理方式对检测得到的各数据进行平滑滤波，确保采集的数据真实可靠反映车身状态，然后将滤波处理后的数据输出给整车控制器。经过卡尔曼滤波后的数据，包括翻滚角roll、俯仰角pitch、偏航角yaw及各个轴的加速度值，共同辅助作用于整车控制策略决策。

因此，翻滚角roll、俯仰角pitch、偏航角yaw及各个轴的加速度值能充分反映车身所处的环境及车身行驶状态，包括直线加速、后退、转弯等，整车控制器对6轴陀螺仪检测的翻滚角roll、俯仰角pitch、偏航角yaw及各个轴的加速度值进行分析，得到车身姿态。而且，在得到车身姿态之后，整车控制器结合这些数据以及油门和刹车踏板信号，做出整车控制策略决策，确保车身稳定。如图3所示，采用6轴陀螺仪配合整车控制器以及电机控制器，共同完成整车控制策略。

因此，该环卫车引入陀螺仪辅助，车身姿态判断不具有迟滞性，提升换挡策略的流畅度，避免整车产生顿挫感；而且，在车身侧倾方面，检测也不存在迟滞的情况。而且，环卫车根据6轴陀螺仪实现辅助整车控制策略，实时检测车辆姿态，确保整车控制器及时介入，通过主动控制保证车身稳定。不额外增加车辆成本，在现有整车控制器上增加6轴陀螺仪的数据采集，根据实时数据，借鉴四轴无人机飞控滤波算法，做优化的卡尔曼滤波处理，与整车控制策略协同，控制车辆动力输出，尤其适合用于四驱车辆。

以上给出了具体的实施方式，但本发明不局限于所描述的实施方式。本发明的基本思路在于上述基本方案，对本领域普通技术人员而言，根据本发明的教导，设计出各种变形的模型、公式、参数并不需要花费创造性劳动。在不脱离本发明的原理和精神的情况下对实施方式进行的变化、修改、替换和变型仍落入本发明的保护范围内。

环卫车车身姿态检测系统实施例

本实施例提供一种环卫车车身姿态检测系统，由于该检测系统在上述环卫车实施例一中已给出了详细地描述，这里就不再具体说明。

环卫车车身姿态检测方法实施例

本实施例提供一种环卫车车身姿态检测方法，包括以下步骤：

(1)建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

(2)检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

(3)根据得到的各数据获取车身姿态。

由于该检测方法的实施过程在上述环卫车实施例一中已给出了详细地描述，这里就不再具体说明。

环卫车实施例二

本实施例提供的环卫车包括环卫车车身稳定系统，该车身稳定系统相较于环卫车实施例一中的环卫车车身姿态检测系统，还包括车身稳定模块，用于根据得到的车身姿态进行车身稳定控制。因此，本实施例提供的环卫车是在环卫车实施例一的基础上再加入车身稳定模块。由于根据车身姿态进行车身稳定控制的实现过程属于现有技术，以下给出车身稳定控制的具体策略：

当检测到车身横摆角度yaw值突然变化时，比如突然增大(突然变大是指横摆角度yaw值的增大变化率大于设定值)时，可以降低扭矩输出，防止高速过弯导致侧翻；针对四驱系统，可以限制外侧轮扭矩，调整车身进行稳定；

当检测到车身侧倾角度roll值变化时，roll值越大，输出扭矩越小，防止侧倾情况下翻车；针对四驱系统，可以加大下方轮扭矩，或者减小上方轮扭矩，调整车身进行稳定；

当检测到车身俯仰角度pitch时，结合加速度Gx值的正负，判断是上坡还是下坡，从而做出增加扭矩或者减小扭矩的动力控制策略，其中，当上坡时，增加扭矩输出，下坡时，减小扭矩输出。

环卫车车身稳定系统实施例

本实施例提供一种环卫车车身稳定系统，由于该车身稳定系统在上述环卫车实施例二中已给出了详细地描述，这里就不再具体说明。

环卫车车身稳定方法实施例

本实施例提供一种环卫车车身稳定方法，包括以下步骤：

(1)建立车身三维坐标系，其中，X轴的方向为车头朝向方向，Y轴的方向为与车头朝向方向相垂直的车身横向方向，Z轴的方向为垂直于车身底盘平面的方向；

(2)检测车身在X轴方向上的加速度、在Y轴方向上的加速度和在Z轴方向上的加速度，以及以X轴为转动轴的车身侧倾角度、以Y轴为转动轴的车身俯仰角度和以Z轴为转动轴的车身横摆角度；

(3)根据得到的各数据获取车身姿态；

(4)根据得到的车身姿态进行车身稳定控制。

由于该车身稳定方法的实施过程在上述环卫车实施例二中已给出了详细地描述，这里就不再具体说明。