CN103625475B

Movatterモバイル変換

Info

Publication number: CN103625475B
Application number: CN201310697908.8A
Authority: CN
Inventors: 李旭; 宋翔
Original assignee: Southeast University
Current assignee: Southeast University
Priority date: 2013-12-18
Filing date: 2013-12-18
Publication date: 2016-04-13
Anticipated expiration: 2033-12-18
Also published as: CN103625475A

Abstract

Translated fromChinese

本发明公开了一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法，本方法针对陆地行驶四轮车辆，建立符合其行驶特征的车辆动力学模型，进一步通过带遗忘因子的递归最小二乘方法实现对车辆侧倾角与俯仰角的实时、准确估计，且仅需低成本车载传感器，成本较低，所估计出的侧倾角与俯仰角信息可满足有关汽车组合导航与定位的需要。

The invention discloses a method for estimating vehicle roll angle and pitch angle based on recursive least squares. The method is aimed at four-wheeled vehicles on land, and establishes a vehicle dynamics model conforming to its driving characteristics, and further uses recursive least squares with forgetting factor The multiplication method realizes real-time and accurate estimation of the vehicle's roll angle and pitch angle, and only requires low-cost on-board sensors, and the cost is low. The estimated roll angle and pitch angle information can meet the needs of vehicle integrated navigation and positioning.

Description

Translated fromChinese

一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法A Method for Estimating Vehicle Roll Angle and Pitch Angle Based on Recursive Least Squares

技术领域technical field

本发明涉及一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法，其目的在于对汽车行驶的动力学过程进行适当的建模，并利用带遗忘因子的递归最小二乘方法获得车辆侧倾角与俯仰角估计值，这些估计值可用于车辆组合导航与定位，具有精度高、成本低、实时性好等显著优点，属于汽车定位导航领域。The present invention relates to a method for estimating vehicle roll angle and pitch angle based on recursive least squares. Estimated values of inclination and pitch angles, these estimated values can be used for vehicle integrated navigation and positioning, and have significant advantages such as high precision, low cost, and good real-time performance, and belong to the field of vehicle positioning and navigation.

背景技术Background technique

近年来，智能交通系统ITS（IntelligentTransportationSystems）在世界范围内得到了高度的重视和快速的发展，ITS功能有效发挥的一个重要基础是实现车辆的准确可靠定位。在车辆准确可靠定位的情况下，ITS可以更有效的诱导车辆，提高运行效率，改善行车安全，减少尾气排放。In recent years, intelligent transportation system ITS (Intelligent Transportation Systems) has been highly valued and developed rapidly all over the world. An important basis for the effective function of ITS is to realize accurate and reliable positioning of vehicles. In the case of accurate and reliable positioning of the vehicle, ITS can guide the vehicle more effectively, improve operating efficiency, improve driving safety, and reduce exhaust emissions.

目前，车辆导航定位领域应用最多的是GPS技术。但由于遮挡或多路径现象，GPS会出现定位不准甚至失效的问题，无法实现准确可靠的连续定位。为克服GPS的不足，车辆多传感器组合导航的研究引起了广泛的重视，即在GPS失效的情况下，利用惯导传感器或航位推算来进行推算定位，从而在GPS失效时获取较为准确的定位信息。At present, GPS technology is the most widely used in the field of vehicle navigation and positioning. However, due to occlusion or multi-path phenomena, GPS will have inaccurate or even invalid positioning problems, and cannot achieve accurate and reliable continuous positioning. In order to overcome the shortcomings of GPS, the research of vehicle multi-sensor integrated navigation has attracted extensive attention, that is, in the case of GPS failure, the inertial navigation sensor or dead reckoning is used to calculate the positioning, so as to obtain a more accurate positioning when the GPS fails information.

车辆在实际道路上行驶过程中，由于道路纵向和横向坡度的存在以及车辆悬架的运动，导致存在着一定的车辆侧倾角和俯仰角，虽然其值一般较小，但其对于推算定位的准确性所起的作用却难以忽略。在陆地车辆应用中，车辆的加速度往往远小于重力加速度，以至于较小的侧倾角和俯仰角可能导致在车体坐标系下测量纵向和侧向加速度时产生较大的误差，这些误差会累积导致推算速度和位置信息时产生较大的累积误差。因此，对侧倾角和俯仰角等姿态角进行准确的测量或估计是推算出准确的定位信息的重要保证，其准确性是影响影响车辆组合定位精度的一个重要因素。When the vehicle is running on the actual road, due to the existence of the longitudinal and lateral slope of the road and the movement of the vehicle suspension, there is a certain vehicle roll angle and pitch angle. The role played by sex is hard to ignore. In land vehicle applications, the acceleration of the vehicle is often much smaller than the acceleration of gravity, so that small roll and pitch angles can cause large errors in the measurement of longitudinal and lateral acceleration in the vehicle body coordinate system, and these errors will accumulate This results in a larger cumulative error when estimating velocity and position information. Therefore, accurate measurement or estimation of attitude angles such as roll angle and pitch angle is an important guarantee for calculating accurate positioning information, and its accuracy is an important factor affecting the positioning accuracy of vehicle assembly.

通常用来确定侧倾角和俯仰角等姿态角信息的方法是使用完整的六维惯性测量单元IMU（InertialMeasurementUnit），该IMU包括3个加速度计和3个角速度陀螺仪，车辆的姿态角信息可以通过六维IMU的捷联算法计算得出。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。考虑到许多车辆安装有电子稳定控制或横摆稳定控制系统，部分IMU信号可以通过车辆的CAN（ControllerAreaNetwork，控制器局域网络）总线较容易的获取，这些信号通常包括横摆角速度、纵向加速度与侧向加速度，为了有效降低成本，本专利即利用这些可获取的信息来估计车辆的侧倾角和俯仰角。The method usually used to determine attitude angle information such as roll angle and pitch angle is to use a complete six-dimensional inertial measurement unit IMU (InertialMeasurementUnit), which includes 3 accelerometers and 3 angular velocity gyroscopes. The attitude angle information of the vehicle can be passed Calculated by the strapdown algorithm of the six-dimensional IMU. However, six-dimensional IMUs are expensive, especially for the price of three gyroscopes. Considering that many vehicles are equipped with electronic stability control or yaw stability control systems, some IMU signals can be easily obtained through the CAN (ControllerAreaNetwork, Controller Area Network) bus of the vehicle. These signals usually include yaw rate, longitudinal acceleration and side In order to effectively reduce the cost, this patent utilizes these available information to estimate the roll angle and pitch angle of the vehicle.

理论上，如果车辆的初始状态已知且车辆的横摆角速度可获取，车辆的侧倾角和俯仰角可以通过数值积分方法计算。然而事实上，直接积分方法由于传感器误差和不可避免的数值运算误差，会导致较大的漂移，特别是在使用车载低成本MEMS（Micro-Electro-MechanicSystem，微机电系统）传感器时，因此，本发明并不采用直接积分方法，而是提出一种实时递归最小二乘（RecursiveLeastSquares,RLS）算法来对车辆的侧倾角和俯仰角进行估计。Theoretically, if the initial state of the vehicle is known and the yaw rate of the vehicle is available, the roll and pitch angles of the vehicle can be calculated by numerical integration methods. However, in fact, the direct integration method will cause large drift due to sensor errors and inevitable numerical calculation errors, especially when using low-cost MEMS (Micro-Electro-MechanicSystem, micro-electromechanical system) sensors on the vehicle. Therefore, this paper The invention does not adopt the direct integration method, but proposes a real-time recursive least squares (Recursive Least Squares, RLS) algorithm to estimate the roll angle and pitch angle of the vehicle.

发明内容Contents of the invention

本发明的目的是克服现有技术的不足，提出一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法，该方法精度高、成本低、实时性好，能够汽车定位导航的需求。The purpose of the present invention is to overcome the deficiencies in the prior art, and propose a vehicle roll angle and pitch angle estimation method based on recursive least squares, which has high precision, low cost, good real-time performance, and can meet the needs of vehicle positioning and navigation.

本发明采用的技术方案如下：一种基于递归最小二乘的车辆侧倾角与俯仰角估计方法，其特征在于：本发明针对陆地行驶四轮车辆，建立符合其行驶特征的车辆动力学模型，进一步通过带遗忘因子的递归最小二乘（RecursiveLeastSquares,RLS）方法实现对车辆侧倾角与俯仰角的实时、准确估计，且仅需低成本车载传感器，成本较低；具体步骤包括：The technical scheme adopted in the present invention is as follows: a method for estimating vehicle roll angle and pitch angle based on recursive least squares, characterized in that: the present invention aims at driving a four-wheeled vehicle on land, and establishes a vehicle dynamics model conforming to its driving characteristics, and further The real-time and accurate estimation of vehicle roll angle and pitch angle is realized through the recursive least squares (Recursive Least Squares, RLS) method with forgetting factor, and only low-cost on-board sensors are required, and the cost is low; the specific steps include:

1）建立汽车行驶过程的动力学模型1) Establish a dynamic model of the vehicle driving process

忽略地球旋转速度，假设车辆的俯仰角速度、侧倾角速度与垂向速度为零，则可建立车辆行驶过程的动力学方程为：Neglecting the rotation speed of the earth, assuming that the pitch angular velocity, roll angular velocity and vertical velocity of the vehicle are zero, the dynamic equation of the vehicle driving process can be established as:

$\begin{matrix} {\overset{. .}{v v}}_{x x} = = {a a}_{x x} + + {ω ω}_{z z} {v v}_{y the y} + + g g sin sin θ θ \\ {\overset{. .}{v v}}_{y the y} = = {a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} - - g g sin sin φ φ cos cos θ θ \end{matrix} - - - - - - ((11))$

式（1）中，v_x,v_y分别表示车辆的纵向速度和侧向速度，a_x,a_y分别表示车辆的纵向加速度和侧向加速度，ω_z表示车辆的横摆角速度，上述定义都是针对车体坐标系，g表示重力加速度，φ,θ分别表示车辆的侧倾角与俯仰角，上标志“·”表示微分，如表示对v_x的微分；In formula (1), v_x , v_y represent the longitudinal velocity and lateral velocity of the vehicle respectively, a_x , a_y represent the longitudinal acceleration and lateral acceleration of the vehicle respectively, ω_z represents the yaw rate of the vehicle, the above definitions are is for the vehicle body coordinate system, g represents the acceleration of gravity, φ, θ represent the roll angle and pitch angle of the vehicle respectively, and the upper mark "·" represents the differential, such as Indicates the differentiation of v_x ;

由（1）式可得From (1) can get

$\begin{matrix} θ θ = = arcsin arcsin ((\frac{{\overset{. .}{v v}}_{x x} - - {a a}_{x x} - - {ω ω}_{z z} {v v}_{y the y}}{g g})) \\ φ φ = = arcsin arcsin ((\frac{{a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} - - {\overset{. .}{v v}}_{y the y}}{g g cos cos θ θ})) \end{matrix} - - - - - - ((22))$

式（2）中，纵向车速的微分可通过纵向车速对时间求导获取，考虑到车辆正常行驶中，v_y与数值较小因而可以忽略，同时，考虑到在大部分路面情况下，车辆的侧倾角与俯仰角通常是小角度，即有arcsin(·)≈·，则式（2）可简化为：In formula (2), the differential of the longitudinal vehicle speed can be obtained by deriving the longitudinal vehicle speed with respect to time. Considering that the vehicle is running normally, v_y and The value is small so it can be ignored. At the same time, considering that in most road conditions, the roll angle and pitch angle of the vehicle are usually small angles, that is, arcsin(·)≈·, then formula (2) can be simplified as:

$\begin{matrix} θ θ = = arcsin arcsin ((\frac{{\overset{. .}{v v}}_{x x} - - {a a}_{x x}}{g g})) \approx \approx \frac{{\overset{. .}{v v}}_{x x} - - {a a}_{x x}}{g g} \\ φ φ = = arcsin arcsin ((\frac{{a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} - - {\overset{. .}{v v}}_{y the y}}{g g cos cos θ θ})) \approx \approx \frac{{a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x}}{g g cos cos θ θ} \end{matrix} - - - - - - ((33))$

2）所需车载传感器安装2) Required on-board sensor installation

由式（3）可知，只需测得车辆的纵向速度、纵向加速度、侧向加速度与横摆角速度，即可利用所建立并合理简化后的车辆行驶动力学方程，即式（3）来估计车辆的俯仰角和侧倾角；因此，仅需要两个低成本MEMS（Micro-Electro-MechanicSystem，微机电系统）加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求；From formula (3), it can be seen that only the longitudinal velocity, longitudinal acceleration, lateral acceleration and yaw rate of the vehicle are measured, and the established and reasonably simplified vehicle dynamics equation, namely formula (3), can be used to estimate The pitch angle and roll angle of the vehicle; therefore, only two low-cost MEMS (Micro-Electro-MechanicSystem, micro-electromechanical system) acceleration sensors, a low-cost MEMS gyroscope and a vehicle speed sensor are required to meet the measurement requirements;

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量侧向加速度，低成本MEMS陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车速传感器用于测量纵向车速，霍尔车速传感器或轮速传感器等传感器均可采用，在此不做限定，但要求车速测量精度误差小于0.05米/秒，在测得纵向车速信号后，将其对时间求导即可获取其微分；Two low-cost MEMS acceleration sensors are installed near the center of mass of the vehicle, one is along the longitudinal axis of the vehicle body coordinate system to measure longitudinal acceleration, and the other is along the horizontal axis of the vehicle body coordinate system to measure lateral acceleration. Low-cost MEMS gyro The instrument is also installed near the center of mass of the vehicle and installed along the vertical axis of the vehicle body coordinate system to measure the yaw rate. The vehicle speed sensor is used to measure the longitudinal vehicle speed. Sensors such as Hall speed sensors or wheel speed sensors can be used. There is no limitation, but the speed measurement accuracy error is required to be less than 0.05 m/s. After the longitudinal speed signal is measured, its differential can be obtained by deriving it with respect to time;

3）基于递归最小二乘的车辆侧倾角与俯仰角估计3) Estimation of vehicle roll angle and pitch angle based on recursive least squares

将式（3）表示为参数识别标准形式：Formula (3) is expressed as the standard form of parameter identification:

式（4）中，k表示离散时刻， $γ (k) = [\begin{matrix} \hat{θ} (k) \\ \hat{φ} (k) \end{matrix}]$ 表示待估参数矩阵，其中，与分别表示待估计的车辆俯仰角与侧倾角； $y (k) = [\begin{matrix} {\dot{v}}_{x_m} - a_{x_m} \\ a_{y_m} - ω_{z_m} v_{x_m} \end{matrix}]$ 表示系统输出矩阵，a_{x_m}、a_{y_m}与ω_{z_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、侧向加速度与横摆角速度，v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：In formula (4), k represents the discrete time, $γ (k) = [\begin{matrix} \hat{θ} (k) \\ \hat{φ} (k) \end{matrix}]$ Represents the parameter matrix to be estimated, where, and represent the pitch angle and roll angle of the vehicle to be estimated, respectively; $the y (k) = [\begin{matrix} {\dot{v}}_{x_m} - a_{x_m} \\ a_{the y_m} - ω_{z_m} v_{x_m} \end{matrix}]$ Represents the system output matrix, a_{x_m} , a_{y_m} and ω_{z_m} represent the longitudinal acceleration, lateral acceleration and yaw rate measured by the low-cost MEMS sensor respectively, v_{x_m} represents the vehicle longitudinal velocity obtained by the vehicle speed sensor, Indicates the differential of v_{x_m} , which is obtained from the time derivative of the longitudinal speed signal measured by the vehicle speed sensor, that is, at each discrete time k, there is:

${\overset{. .}{v v}}_{x x__m m} ((k k)) = = \frac{{v v}_{x x__m m} ((k k)) - - {v v}_{x x__m m} ((k k - - 11))}{dt dt} - - - - - - ((55))$

式（5）中，dt表示采样时间间隔，本发明中，dt=0.01（秒）；表示输入回归矩阵，本发明中上角标T表示对矩阵转置；则利用带遗忘因子的递归最小二乘（RecursiveLeastSquares,RLS）算法实时估计车辆侧倾角与横摆角的估计步骤如下：In formula (5), dt represents the sampling time interval, in the present invention, dt=0.01 (second); Represents the input regression matrix, and the superscript T represents the transposition of the matrix among the present invention; then utilize the recursive least squares (Recursive Least Squares, RLS) algorithm with forgetting factor to estimate the estimation steps of vehicle roll angle and yaw angle in real time as follows:

（1）计算系统输出矩阵y(k)，并计算输入回归矩阵(1) Calculate the system output matrix y(k), and calculate the input regression matrix

（2）计算增益矩阵K(k)；其中，方差矩阵参数λ为遗忘因子，能有效减少不再与模型相关的旧数据的影响，并防止协方差发散，通常取值范围在[0.9,1]，本发明取0.975；(2) Calculate the gain matrix K(k); Among them, the variance matrix The parameter λ is a forgetting factor, which can effectively reduce the influence of old data that is no longer relevant to the model, and prevent the divergence of covariance. Usually, the value range is [0.9,1], and the present invention takes 0.975;

（3）计算待估参数矩阵γ(k)；(3) Calculate the parameter matrix γ(k) to be estimated;

其中I为2×2单位矩阵，至此，可实时估计出车辆侧倾角与横摆角。Where I is a 2×2 unit matrix, so far, the vehicle roll angle and yaw angle can be estimated in real time.

本发明的优点及显著效果：Advantage of the present invention and remarkable effect:

（1）本发明提出了一种低成本、高精度、实时性好的车辆侧倾角和俯仰角估计方法，可用于车辆组合导航与定位领域对于车辆位置与速度信息进行准确推算需要；(1) The present invention proposes a low-cost, high-precision, and real-time vehicle roll angle and pitch angle estimation method, which can be used in the field of vehicle integrated navigation and positioning for accurate estimation of vehicle position and speed information;

（2）本发明是对车辆动力学模型根据车辆行驶特点进行合理简化，并利用递归最小二乘算法进行侧倾角和俯仰角的估计，保障了其估计精度与实时性；(2) The present invention rationally simplifies the vehicle dynamics model according to the driving characteristics of the vehicle, and uses the recursive least squares algorithm to estimate the roll angle and pitch angle, ensuring the estimation accuracy and real-time performance;

（3）本发明仅需量产车上车载传感器，具有成本低的优点，便于大规模推广。(3) The present invention only needs to mass-produce the vehicle-mounted sensor, which has the advantage of low cost and is convenient for large-scale promotion.

附图说明Description of drawings

图1是仿真工况1俯仰角估计结果；Fig. 1 is the estimation result of pitch angle in simulation condition 1;

图2是仿真工况2侧倾角估计结果；Fig. 2 is the estimation result of the roll angle of the simulation working condition 2;

具体实施方式detailed description

实施实例1Implementation example 1

车辆在实际道路上行驶过程中，由于道路纵向和横向坡度以及车辆悬架的运动，导致存在着一定的车辆侧倾角和俯仰角，虽然其值一般较小，但其对于推算定位的准确性所起的作用却难以忽略。在陆地车辆行驶过程中，车辆的加速度往往远小于重力加速度，以至于较小的侧倾角和俯仰角可能导致在车体坐标系下测量纵向和侧向加速度时产生较大的误差，这些误差会累积导致在推算速度和位置信息时产生较大的累积误差。因此，对侧倾角和俯仰角等姿态角进行准确的测量或估计是推算出准确的定位信息的重要保证，其准确性是影响影响车辆组合定位精度的一个重要因素。When the vehicle is running on the actual road, due to the longitudinal and lateral slope of the road and the movement of the vehicle suspension, there is a certain vehicle roll angle and pitch angle. Its role cannot be ignored. During the running of a land vehicle, the acceleration of the vehicle is often much smaller than the gravitational acceleration, so that the small roll angle and pitch angle may cause large errors in the measurement of the longitudinal and lateral acceleration in the vehicle body coordinate system, and these errors will Accumulation results in large cumulative errors in deriving velocity and position information. Therefore, accurate measurement or estimation of attitude angles such as roll angle and pitch angle is an important guarantee for deriving accurate positioning information, and its accuracy is an important factor affecting the positioning accuracy of vehicle assembly.

通常用来确定侧倾角和俯仰角等姿态角信息的方法是使用完整的六维惯性测量单元IMU（InertialMeasurementUnit），该IMU包括3个加速度计和3个角速度陀螺仪，利用IMU输出量和角度信息的微分之间的运动学关系，并忽略地球旋转速度，车辆动力学过程可建模为[此处可参考文献：H.EricTsenga,LiXu，DavorHrovat，Estimationoflandvehiclerollandpitchangles[J].VehicleSystemDynamics:InternationalJournalofVehicleMechanicsandMobility,2007,45(5):433-443.]：The method usually used to determine attitude angle information such as roll angle and pitch angle is to use a complete six-dimensional inertial measurement unit IMU (InertialMeasurementUnit), which includes 3 accelerometers and 3 angular velocity gyroscopes, using IMU output and angle information The kinematics relationship between the differentials, and ignoring the earth's rotation speed, the vehicle dynamics process can be modeled as [references here: H.EricTsenga, LiXu, DavorHrovat, Estimationoflandvehiclerollandpitchangles[J].VehicleSystemDynamics:InternationalJournalofVehicleMechanicsandMobility,2007,45 (5):433-443.]:

$\begin{matrix} \overset{. .}{φ φ} = = {ω ω}_{x x} + + (({ω ω}_{y the y} sin sin φ φ + + {ω ω}_{z z} cos cos φ φ)) tan the tan θ θ \\ \overset{. .}{θ θ} = = {ω ω}_{y the y} cos cos φ φ - - {ω ω}_{z z} sin sin φ φ \\ \overset{. .}{ψ ψ} = = (({ω ω}_{y the y} sin sin φ φ + + {ω ω}_{z z} cos cos φ φ)) / / cos cos θ θ \end{matrix} - - - - - - ((11))$

$\begin{matrix} {\overset{. .}{v v}}_{x x} = = {a a}_{x x} + + {ω ω}_{z z} {v v}_{y the y} - - {ω ω}_{y the y} {v v}_{z z} + + g g sin sin θ θ \\ {\overset{. .}{v v}}_{y the y} = = {a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} + + {ω ω}_{x x} {v v}_{z z} - - g g sin sin φ φ cos cos θ θ - - - - - - ((22)) \\ {\overset{. .}{v v}}_{z z} = = {a a}_{z z} + + {ω ω}_{y the y} {v v}_{x x} - - {ω ω}_{x x} {v v}_{y the y} - - g g cos cos φ φ cos cos θ θ \end{matrix}$

式中，ω_x,ω_y和ω_z分别表示围绕车体坐标系纵轴、横轴以及垂向轴的角速度，v_x,v_y和v_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的线速度，a_x,a_y和a_z分别表示沿车体坐标系纵轴、横轴以及垂向轴的加速度；φ,θ,和ψ分别表示侧倾、俯仰和横摆三个欧拉角；g表示重力加速度，本发明取9.78。In the formula, ω_x , ω_y and ω_z respectively represent the angular velocity around the vertical axis, horizontal axis and vertical axis of the vehicle body coordinate system, and v_x , v_y and v_z represent the angular velocity along the vertical axis and horizontal axis of the vehicle body coordinate system, respectively. And the linear velocity of the vertical axis, a_x , a_y and a_z respectively represent the acceleration along the longitudinal axis, transverse axis and vertical axis of the car body coordinate system; φ, θ, and ψ represent roll, pitch and yaw respectively Three Euler angles; g represents the acceleration of gravity, which is 9.78 in the present invention.

利用式（1）和（2），车辆的姿态角信息可以通过六维IMU的捷联算法计算得出，大量车辆定位文献中都有涉及。然而，六维IMU价格昂贵，特别是三个陀螺仪的价格。考虑到许多车辆安装有电子稳定控制或横摆稳定控制系统，部分IMU信号可以通过车辆的CAN（ControllerAreaNetwork，控制器局域网络）总线较容易的获取，这些信号通常包括横摆角速度、纵向加速度与侧向加速度；为了有效降低成本，本专利即研究如何利用这些可获取的信息而非利用完整的6维IMU来估计车辆的侧倾角和俯仰角。Using formulas (1) and (2), the attitude angle information of the vehicle can be calculated by the strapdown algorithm of the six-dimensional IMU, which is involved in a large number of vehicle positioning literatures. However, six-dimensional IMUs are expensive, especially for the price of three gyroscopes. Considering that many vehicles are equipped with electronic stability control or yaw stability control systems, some IMU signals can be easily obtained through the CAN (ControllerAreaNetwork, Controller Area Network) bus of the vehicle. These signals usually include yaw rate, longitudinal acceleration and side acceleration; in order to effectively reduce costs, this patent studies how to use these available information instead of using a complete 6-dimensional IMU to estimate the roll angle and pitch angle of the vehicle.

由式（1），可以看出为了估计出侧倾角和俯仰角，并不需要横摆角信息ψ。同时，由于车辆通常行驶的交通路面的纵向和横向坡度都较小，其横向坡度率与纵向坡度率通常都小于20%，（本发明即主要针对横向坡度率与纵向坡度率都小于20%的道路），车辆的侧倾角与俯仰角都是连续缓慢的变化，其相应的角速度值较小可忽略，且车辆垂向速度一般也较小，因此，可以合理的认为ω_x≈0,ω_y≈0,ν_z≈0.则式（1）和（2）可以简化为：From formula (1), it can be seen that in order to estimate the roll angle and pitch angle, the yaw angle information ψ is not needed. Simultaneously, because the longitudinal and transverse slopes of the traffic road surface that the vehicle usually travels on are all relatively small, its transverse gradient rate and longitudinal gradient rate are all less than 20% usually, (the present invention is mainly aimed at transverse gradient rate and longitudinal gradient rate all less than 20% Road), the roll angle and pitch angle of the vehicle are continuous and slow changes, the corresponding angular velocity is small and negligible, and the vertical velocity of the vehicle is generally small, so it is reasonable to think that ω_x ≈ 0, ω_y ≈0, ν_z ≈0. Then equations (1) and (2) can be simplified as:

$\begin{matrix} \overset{. .}{φ φ} = = {ω ω}_{z z} cos cos φ φ tan the tan θ θ \\ \overset{. .}{θ θ} = = - - {ω ω}_{z z} sin sin φ φ \end{matrix} - - - - - - ((33))$

$\begin{matrix} {\overset{. .}{v v}}_{x x} = = {a a}_{x x} + + {ω ω}_{z z} {v v}_{y the y} + + g g sin sin θ θ \\ {\overset{. .}{v v}}_{y the y} = = {a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} - - g g sin sin φ φ cos cos θ θ \end{matrix} - - - - - - ((44))$

根据式（3），理论上，如果车辆的初始状态已知且车辆的横摆角速度可获取，车辆的侧倾角和俯仰角可以通过数值积分方法计算。然而事实上，直接积分方法由于传感器误差和不可避免的数值运算误差，会导致较大的漂移，特别是使用低成本MEMS传感器时，因此，本发明并不采用直接积分方法，而是利用式（4），提出一种实时递归最小二乘（RecursiveLeastSquares,RLS）算法来对车辆的侧倾角和俯仰角进行估计。According to formula (3), in theory, if the initial state of the vehicle is known and the yaw rate of the vehicle is available, the roll angle and pitch angle of the vehicle can be calculated by numerical integration method. However, in fact, the direct integration method will cause larger drift due to sensor errors and inevitable numerical calculation errors, especially when low-cost MEMS sensors are used. Therefore, the present invention does not use the direct integration method, but uses the formula ( 4) A real-time recursive least squares (Recursive Least Squares, RLS) algorithm is proposed to estimate the roll angle and pitch angle of the vehicle.

递归最小二乘是对未知矢量的迭代算法，以模型误差的最小方差为目标，对于每个采样周期，使用已有采样数据通过反复迭代计算未知矢量，具有数据存储量小、算法简便的特点。Recursive least squares is an iterative algorithm for unknown vectors, with the goal of minimizing the variance of model errors. For each sampling period, the unknown vector is calculated through repeated iterations using existing sampling data. It has the characteristics of small data storage and simple algorithm.

由（4）式可得From (4) can get

$\begin{matrix} θ θ = = arcsin arcsin ((\frac{{\overset{. .}{v v}}_{x x} - - {a a}_{x x} - - {ω ω}_{z z} {v v}_{y the y}}{g g})) \\ φ φ = = arcsin arcsin ((\frac{{a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} - - {\overset{. .}{v v}}_{y the y}}{g g cos cos θ θ})) \end{matrix} - - - - - - ((55))$

式（5）中，纵向车速的微分可通过纵向车速对时间求导获取，考虑到车辆正常行驶中，v_y与数值较小因而可以忽略，同时，考虑到由于车辆通常行驶的交通路面的纵向和横向坡度都较小，其横向坡度率与纵向坡度率都小于20%，（本发明即主要针对横向坡度率与纵向坡度率都小于20%的道路），因此，车辆的侧倾角与俯仰角通常是小角度，即有arcsin(·)≈·，则式（5）可简化为：In formula (5), the differential of the longitudinal vehicle speed can be obtained by deriving the longitudinal vehicle speed with respect to time. Considering that the vehicle is running normally, v_y and Numerical value is little thereby can neglect, meanwhile, considering that because the longitudinal and lateral gradient of the traffic road surface that vehicle travels usually is all less, its transverse gradient rate and longitudinal gradient rate are all less than 20%, (the present invention mainly aims at transverse gradient rate and transverse gradient rate and The road with a longitudinal slope rate less than 20%), therefore, the roll angle and pitch angle of the vehicle are usually small angles, that is, arcsin(·)≈·, then formula (5) can be simplified as:

$\begin{matrix} θ θ = = arcsin arcsin ((\frac{{\overset{. .}{v v}}_{x x} - - {a a}_{x x}}{g g})) \approx \approx \frac{{\overset{. .}{v v}}_{x x} - - {a a}_{x x}}{g g} \\ φ φ = = arcsin arcsin ((\frac{{a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x} - - {\overset{. .}{v v}}_{y the y}}{g g cos cos θ θ})) \approx \approx \frac{{a a}_{y the y} - - {ω ω}_{z z} {v v}_{x x}}{g g cos cos θ θ} \end{matrix} - - - - - - ((66))$

由式（6）可知，只需测得车辆的纵向速度、纵向加速度、侧向加速度与横摆角速度，即可利用所建立并合理简化后的车辆行驶动力学方程（3）来估计车辆的俯仰角和侧倾角；因此，仅需要两个低成本MEMS（Micro-Electro-MechanicSystem，微机电系统）加速度传感器，一个低成本MEMS陀螺仪以及车速传感器即可满足测量要求，；From formula (6), it can be seen that only the longitudinal velocity, longitudinal acceleration, lateral acceleration and yaw rate of the vehicle are measured, and the established and reasonably simplified vehicle dynamics equation (3) can be used to estimate the pitch of the vehicle angle and roll angle; therefore, only two low-cost MEMS (Micro-Electro-MechanicSystem, micro-electromechanical systems) acceleration sensors, a low-cost MEMS gyroscope and a vehicle speed sensor are required to meet the measurement requirements;

其中两个低成本MEMS加速度传感器安装于车辆质心位置附近，一个沿车体坐标系纵轴，用以测量纵向加速度，一个沿车体坐标系横轴，用以测量侧向加速度，低成本MEMS陀螺仪也安装于车辆质心位置附近，沿车体坐标系垂向轴安装，用以测量横摆角速度，车速传感器用于测量纵向车速，霍尔车速传感器或轮速传感器等传感器均可采用，在此不做限定，但要求车速测量精度误差小于0.05米/秒，在测得纵向车速信号后，并将其对时间求导即可获取其微分；Two low-cost MEMS acceleration sensors are installed near the center of mass of the vehicle, one is along the longitudinal axis of the vehicle body coordinate system to measure longitudinal acceleration, and the other is along the horizontal axis of the vehicle body coordinate system to measure lateral acceleration. Low-cost MEMS gyro The instrument is also installed near the center of mass of the vehicle and installed along the vertical axis of the vehicle body coordinate system to measure the yaw rate. The vehicle speed sensor is used to measure the longitudinal vehicle speed. Sensors such as Hall speed sensors or wheel speed sensors can be used. There is no limitation, but the accuracy error of the vehicle speed measurement is required to be less than 0.05 m/s. After the longitudinal vehicle speed signal is measured, its differential can be obtained by deriving it with respect to time;

事实上，若车辆安装有电子稳定控制或横摆稳定控制系统，则这些信息可以通过车辆的CAN（ControllerAreaNetwork，控制器局域网络）总线获取。In fact, if the vehicle is equipped with an electronic stability control or yaw stability control system, the information can be obtained through the CAN (Controller Area Network, Controller Area Network) bus of the vehicle.

将式（6）表示为参数识别标准形式：Formula (6) is expressed as the standard form of parameter identification:

式（7）中，k表示离散时刻， $γ (k) = [\begin{matrix} \hat{θ} (k) \\ \hat{φ} (k) \end{matrix}]$ 表示待估参数矩阵，其中，与分别表示待估计的车辆俯仰角与侧倾角； $y (k) = [\begin{matrix} {\dot{v}}_{x_m} - a_{x_m} \\ a_{y_m} - ω_{z_m} v_{x_m} \end{matrix}]$ 表示系统输出矩阵，a_{x_m}、a_{y_m}与ω_{z_m}分别表示利用低成本MEMS传感器所测得的纵向加速度、侧向加速度与横摆角速度，v_{x_m}表示通过车速传感器所获取的车辆纵向速度，表示v_{x_m}的微分，由车速传感器所测得的纵向速度信号对时间求导来获取，即在每一离散时刻k，有：In formula (7), k represents the discrete time, $γ (k) = [\begin{matrix} \hat{θ} (k) \\ \hat{φ} (k) \end{matrix}]$ Represents the parameter matrix to be estimated, where, and represent the pitch angle and roll angle of the vehicle to be estimated, respectively; $the y (k) = [\begin{matrix} {\dot{v}}_{x_m} - a_{x_m} \\ a_{the y_m} - ω_{z_m} v_{x_m} \end{matrix}]$ Represents the system output matrix, a_{x_m} , a_{y_m} and ω_{z_m} represent the longitudinal acceleration, lateral acceleration and yaw rate measured by the low-cost MEMS sensor respectively, v_{x_m} represents the vehicle longitudinal velocity obtained by the vehicle speed sensor, Indicates the differential of v_{x_m} , which is obtained from the time derivative of the longitudinal speed signal measured by the vehicle speed sensor, that is, at each discrete time k, there is:

${\overset{. .}{v v}}_{x x__m m} ((k k)) = = \frac{{v v}_{x x__m m} ((k k)) - - {v v}_{x x__m m} ((k k - - 11))}{dt dt} - - - - - - ((88))$

式（8）中，dt表示采样时间间隔，本发明中，dt=0.01（秒）；表示输入回归矩阵，本发明中上角标^T表示对矩阵转置；则利用带遗忘因子的递归最小二乘（RecursiveLeastSquares,RLS）算法实时估计车辆侧倾角与横摆角的估计步骤如下：In formula (8), dt represents the sampling time interval, in the present invention, dt=0.01 (second); Represents the input regression matrix, and the superscript^T represents the transposition of the matrix among the present invention; then utilize the recursive least squares (Recursive Least Squares, RLS) algorithm with forgetting factor to estimate the estimation steps of vehicle roll angle and yaw angle in real time as follows:

实施实例2Implementation example 2

为检验本发明提出的基于递归最小二乘的车辆侧倾角和俯仰角估计方法的实际效果，在专业的汽车动力学仿真软件CarSim上进行了仿真验证实验。In order to test the actual effect of the method for estimating the vehicle roll angle and pitch angle based on recursive least squares proposed by the present invention, a simulation verification experiment was carried out on the professional vehicle dynamics simulation software CarSim.

CarSim是由美国MSC(MechanicalSimulationCorporation)公司开发的专门针对车辆动力学的仿真软件，目前已被国际上众多的汽车制造商、零部件供应商所采用，被广泛地应用于现代汽车控制系统的商业开发，已成为汽车行业的标准软件，享有很高的声誉。Carsim内的车辆动力学模型是通过分别对汽车的车体、悬架、转向、制动等各子系统以及各个轮胎的高逼真建模来实现的，具有很高的自由度，能够提供非常接近实际的准确的车辆运行状态信息，因此，Carsim输出的车辆运行状态信息可作为车辆的参考输出。CarSim is a simulation software specially designed for vehicle dynamics developed by MSC (Mechanical Simulation Corporation) in the United States. It has been adopted by many international automobile manufacturers and parts suppliers, and is widely used in the commercial development of modern automobile control systems. , has become the standard software in the automotive industry and enjoys a high reputation. The vehicle dynamics model in Carsim is realized through the high-fidelity modeling of the car body, suspension, steering, braking and other subsystems, as well as each tire. It has a high degree of freedom and can provide a very close The actual and accurate vehicle running status information, therefore, the vehicle running status information output by Carsim can be used as the reference output of the vehicle.

为检验本发明提出的算法在车辆典型行驶工况下的估计效果，在仿真试验中设置了两个典型工况，工况中包含了车辆直线和曲线运动，道路坡度的变化和车速的变化，工况具体描述见表1。In order to check the estimation effect of the algorithm proposed by the present invention under the typical driving conditions of the vehicle, two typical working conditions are set in the simulation test, which includes the straight line and curve motion of the vehicle, the change of the road gradient and the change of the vehicle speed, See Table 1 for a detailed description of the working conditions.

表1两种典型仿真工况Table 1 Two typical simulation conditions

仿真所用车辆是一个前轮转向的四轮小型客车，获取车辆纵向车速采用轮速传感器，所需惯性传感器和轮速传感器的采样频率都为100Hz，对于基于MEMS的低成本惯性传感器，陀螺仪的零均值随机白噪声的标准差为0.2（度/秒），加速度传感器的零均值随机白噪声的标准差为0.1956（米/（秒×秒）），轮速传感器的测量噪声均为均值是0、标准差是0.05（米/秒）的高斯白噪声。仿真初值设置如下：方差矩阵初值 $P = [\begin{matrix} 10 & 0 \\ 0 & 10 \end{matrix}],$ 待估矩阵初值为 $γ (0) = [\begin{matrix} 0 \\ 0 \end{matrix}] .$ The vehicle used in the simulation is a four-wheeled passenger car with front-wheel steering. The wheel speed sensor is used to obtain the longitudinal speed of the vehicle. The sampling frequency of the inertial sensor and the wheel speed sensor is both 100Hz. For the MEMS-based low-cost inertial sensor, the gyroscope’s The standard deviation of the zero-mean random white noise is 0.2 (degrees/second), the standard deviation of the zero-mean random white noise of the acceleration sensor is 0.1956 (m/(second×second)), and the mean value of the measurement noise of the wheel speed sensor is 0 , the standard deviation is 0.05 (m/s) Gaussian white noise. The initial value of the simulation is set as follows: initial value of variance matrix $P = [\begin{matrix} 10 & 0 \\ 0 & 10 \end{matrix}],$ The initial value of the estimated matrix is $γ (0) = [\begin{matrix} 0 \\ 0 \end{matrix}] .$

表2和图1、图2给出了仿真实验的结果。表2列出了利用直测法和本发明方法推算车辆侧倾角和俯仰角的统计结果对比，表中的误差均是相对于Carsim输出的相应参考值而言的（如直测法的俯仰角误差就表示利用直测法推算的俯仰角相对于Carsim输出的俯仰角参考值的误差）。另外需指出的是，上述两种方法的具体含义如下：直测法是指利用惯性传感器输出值，通过实施实例1中式（6）直接推算得到俯仰角和侧倾角的方法；本发明方法是指利用本发明提出的递归最小二乘估计方法来推算车辆侧倾角和俯仰角的方法。Table 2 and Fig. 1 and Fig. 2 show the results of the simulation experiment. Table 2 has listed the statistical result comparison of utilizing direct measurement method and the method of the present invention to calculate vehicle roll angle and pitch angle, and the error in the table is all relative to the corresponding reference value of Carsim output (as the pitch angle of direct measurement method The error refers to the error of the pitch angle calculated by the direct measurement method relative to the reference value of the pitch angle output by Carsim). In addition, it should be pointed out that the specific meanings of the above two methods are as follows: the direct measurement method refers to the method of directly calculating the pitch angle and roll angle by implementing the formula (6) in Example 1 by using the output value of the inertial sensor; the method of the present invention refers to A method for estimating vehicle roll angle and pitch angle by using the recursive least square estimation method proposed by the invention.

表2两种方法推算侧倾角与俯仰角效果对比表（单位：deg）Table 2 Comparison of the effects of the two methods to calculate the roll angle and pitch angle (unit: deg)

图1给出了工况1中利用直测法和本发明方法估计的俯仰角的结果曲线（图中以Original灰色虚线表示直测法结果，以RLS黑色点线标示本发明方法估计结果），以及相应的Carsim的参考输出值（图中以Carsim实黑线标示），图2给出了工况2中利用直测法和本发明方法估计的侧倾角的结果曲线（图中以Original灰色虚线表示直测法结果，以RLS黑色点线标示本发明方法估计结果），以及相应的Carsim的参考输出值（图中以Carsim实黑线标示）。Fig. 1 provides the result curve of the pitch angle estimated by the direct measurement method and the method of the present invention in working condition 1 (in the figure, the result of the direct measurement method is represented by the Original gray dotted line, and the estimation result of the method of the present invention is marked by the RLS black dotted line), And the reference output value of corresponding Carsim (marked with the solid black line of Carsim in the figure), Fig. 2 has provided the result curve of the roll angle that utilizes direct measurement method and the method of the present invention to estimate in working condition 2 (in the figure with Original gray dotted line Indicates the result of the direct measurement method, the estimated result of the method of the present invention is marked with the RLS black dotted line), and the corresponding reference output value of Carsim (marked by the solid black line of Carsim in the figure).

由表2的对比以及图1～图2，可以看出本发明方法相对于直测法在侧倾角和俯仰角的推算方面精度有了大幅的提高。From the comparison of Table 2 and Figures 1 to 2, it can be seen that the method of the present invention has greatly improved accuracy in calculating the roll angle and pitch angle compared with the direct measurement method.