RU2821391C1 - Wheel odometry sensors calibration method - Google Patents

Abstract

FIELD: measuring equipment.

SUBSTANCE: invention relates to computer engineering for determination of calibration coefficients of steering angle sensor and speed sensor installed on autonomous vehicle. Wheel odometry sensors calibration method consists in the fact that location data are recorded, autonomous vehicle movement track line is formed on a virtual map, at each point of the track, performing a polynomial regression of the trajectory, determining the velocity vector, the local curvature at the point and the radius of curvature by differentiating the polynomial-regressed trajectory, velocity vector modulus is compared with the readings of the speed sensor, data are accumulated, the desired calibration coefficient of the speed sensor is determined, at the same time, at the point of the autonomous vehicle movement track line, the readings of the wheel angle sensor and the autonomous vehicle speed are monitored, the values of the wheel angle and autonomous vehicle speed sensors are obtained, polynomial regression of the set of accumulated points is carried out and a polynomial is calculated, the coefficients of which are calibration.

EFFECT: higher accuracy of calibration of steering angle sensor and speed sensor.

1 cl, 2 dwg

Description

Изобретение относится к области вычислительной техники и может быть использовано для определения калибровочных коэффициентов датчика угла поворота управляемых колес и датчика скорости, устанавливаемых на автономном транспортном средстве.The invention relates to the field of computer technology and can be used to determine the calibration coefficients of the steering angle sensor and speed sensor installed on an autonomous vehicle.

Колесные роботы оснащаются комплексом сенсоров, обеспечивающих возможность локализации робота в пространстве, детектирование целевой траектории и возможных препятствий. Wheeled robots are equipped with a set of sensors that provide the ability to localize the robot in space, detect the target trajectory and possible obstacles.

Сенсоры, такие как датчик угла поворота управляемых колес и датчик скорости, в процессе движения колесных роботов регистрируют информацию в собственной системе координат и в собственных единицах измерения. Для приведения показаний сенсоров к общей системе координат и к единой системе физических единиц необходимо проведение процедур калибровки сенсоров.Sensors, such as the steering angle sensor and speed sensor, record information in their own coordinate system and in their own units of measurement as wheeled robots move. To bring sensor readings to a common coordinate system and to a unified system of physical units, it is necessary to carry out sensor calibration procedures.

Из указанной области техники известны следующие технические решения.The following technical solutions are known from this field of technology.

Также известен способ калибровки параметров шасси для автоматизированного управляемого транспортного средства (АУТС) по патенту № CN108982116, включающий в себя следующие этапы: получают данные об ориентации, когда АУТС неоднократно выполняет движение между несколькими точками калибровки, при этом данные об ориентации представляют отношения положения между АУТС и несколькими точками калибровки. Данные об ориентации содержат данные, полученные после того, как АУТС достигает любая точка калибровки, данные, полученные после поворота АУТС на заданный угол во всех или части точек калибровки, и данные, полученные АУТС перед движением, в начальной точке калибровки каждого движения цели. На основе данных об ориентации калибруются параметры шасси АУТС, при этом параметры шасси включают коэффициент радиуса колеса и/или коэффициент межосевого расстояния.Also known is a method for calibrating chassis parameters for an automated guided vehicle (AGV) according to patent No. CN108982116, which includes the following steps: obtaining orientation data when the AGV repeatedly moves between several calibration points, wherein the orientation data represents position relationships between the AVVs and several calibration points. Orientation data includes data received after the AUV reaches any calibration point, data obtained after the AUV has rotated through a given angle at all or part of the calibration points, and data received by the AUV before movement, at the initial calibration point of each target movement. Based on the orientation data, the AUTS chassis parameters are calibrated, and the chassis parameters include the wheel radius coefficient and/or the axle distance coefficient.

Данный вид калибровки выполняется на производстве и требует наличия специально оборудованной площадки. Также данный способ не позволяет калибровать датчик угла поворота колес.This type of calibration is performed in production and requires a specially equipped site. Also, this method does not allow you to calibrate the wheel angle sensor.

Кроме того, известен способ автоматической калибровки датчика угла наклона сельскохозяйственной техники и положения электрического рулевого колеса (патент № CN108762323). Указанное изобретение предусматривает систему автоматической калибровки положения электрического рулевого колеса, включающую устройство управления навигацией, устройство сенсорного дисплея, рулевое колесо с моторным управлением и устройство датчика угла поворота. Устройство управления навигацией используется для приема дифференциального сигнала и передачи информации о местоположении и соответствующих данных на устройство сенсорного дисплея посредством обработки. Рулевое колесо с электроприводом содержит электродвигатель рулевого колеса и приводное устройство. Приводное устройство управляет двигателем рулевого колеса, чтобы он вращался относительно друг друга, чтобы приводить во вращение колеса. Устройство углового кодера используется для обратной передачи данных об угле поворота переднего колеса в устройство управления навигацией. После автоматической калибровки сельскохозяйственной техники можно управлять транспортным средством для выполнения рулевого управления с помощью рулевого колеса с электрическим управлением. In addition, a method for automatically calibrating the tilt angle sensor of agricultural machinery and the position of an electric steering wheel is known (patent No. CN108762323). This invention provides an automatic position calibration system for an electric steering wheel, including a navigation control device, a touch display device, a motorized steering wheel and a steering angle sensor device. The navigation control device is used to receive the differential signal and transmit position information and related data to the touch display device through processing. The power steering wheel includes an electric steering wheel motor and a drive device. The drive device controls the steering wheel motor to rotate relative to each other to drive the wheels. The angle encoder device is used to feed back the front wheel steering angle data to the navigation control device. After automatic calibration of agricultural machinery, the vehicle can be controlled to perform steering operation using the electrically controlled steering wheel.

Недостаток указанного способа заключается в том, что в реальных механических системах угол поворота рулевого колеса не является однозначно связанным с углом поворота колес, руль может иметь люфт. Это может снижать точность оценки угла поворота колес на основе информации по положению руля.The disadvantage of this method is that in real mechanical systems the angle of rotation of the steering wheel is not uniquely related to the angle of rotation of the wheels; the steering wheel may have play. This may reduce the accuracy of wheel steering angle estimation based on steering wheel position information.

Известен способ оценки угла поворота шины на основе трека транспортного средства по патенту CN113682375, при котором:There is a known method for estimating the steering angle of a tire based on the vehicle track according to patent CN113682375, in which:

- устанавливают антенну GNSS на кузов транспортного средства, а энкодер устанавливают на двигатель рулевого колеса транспортного средства; - install the GNSS antenna on the vehicle body, and the encoder is installed on the vehicle steering wheel motor;

- курсовой угол транспортного средства рассчитывается по координатам положения GNSS; - the heading angle of the vehicle is calculated using GNSS position coordinates;

- значение разности курсовых углов и угловая скорость рыскания кузова транспортного средства получаются в соответствии с курсовым углом, полученным путем вычисления; - the value of the difference in heading angles and the yaw rate of the vehicle body are obtained in accordance with the heading angle obtained by calculation;

- абсолютный угол поворота шины рассчитывается по кинематике кузова автомобиля; - the absolute steering angle of the tire is calculated based on the kinematics of the car body;

- считывается числовое значение энкодера на двигателе рулевого колеса транспортного средства, и числовое значение вычисляется для получения соответствующего угла поворота энкодера;- the numerical value of the encoder on the steering wheel motor of the vehicle is read, and the numerical value is calculated to obtain the corresponding rotation angle of the encoder;

- когда скорость изменения угла поворота шины превышает установленное пороговое значение, угол поворота шины оценивается с использованием алгоритма фильтрации Калмана.- When the tire steering angle change rate exceeds a set threshold, the tire steering angle is estimated using the Kalman filtering algorithm.

В рамках указанного способа расчет курсового угла транспортного средства производится на основе локальной линейной аппроксимации как арктангенс deltaNord/deltaEast. При этом измерение координат GNSS сопровождается зашумлением, что может приводить к неустойчивому вычислению в данной формуле и к невысокой точности оценки курсового угла и разности курсовых углов, на которых базируется основной расчет. Within the framework of this method, the calculation of the heading angle of the vehicle is carried out on the basis of a local linear approximation as the arctangent deltaNord/deltaEast. In this case, the measurement of GNSS coordinates is accompanied by noise, which can lead to unstable calculations in this formula and to low accuracy in estimating the heading angle and the difference in heading angles on which the main calculation is based.

Указанное техническое решение наиболее близко по технической сущности к заявленному изобретению и может выступать в качестве прототипа.The specified technical solution is closest in technical essence to the claimed invention and can act as a prototype.

Технический результат заявленного изобретения заключается в повышении точности осуществления калибровки датчика угла поворота управляемых колес и датчика скорости.The technical result of the claimed invention is to increase the accuracy of calibration of the steering angle sensor and the speed sensor.

Технический результат достигается за счет осуществления способа калибровки сенсоров колесной одометрии, при котором в процессе движения автономного транспортного средства (АТС) посредством сенсора навигационной системы регистрируют данные о его местоположении, по полученным данным формируют трек-линию движения АТС на виртуальной карте, отличающегося тем, что в каждой точке трека производят полиномиальную регрессию траектории, определяют вектор скорости, локальную кривизну в точке и радиус кривизны посредством дифференцирования полиномиально-регрессированной траектории, модуль вектора скорости сопоставляют с показаниями датчика скорости, производят накопление данных и определяют искомый калибровочный коэффициент датчика скорости, одновременно в точке трек-линии движения АТС отслеживают показания датчика поворота угла колеса и скорости АТС, с учетом модели АТС кривизну траектории сопоставляют с расстоянием между колесными базами и углом поворота управляющих колес, в результате чего получают значения датчиков поворота угла колеса и скорости АТС, производят полиномиальную регрессию множества накопленных точек и вычисляют полином, коэффициенты которого являются калибровочными.The technical result is achieved by implementing a method for calibrating wheel odometry sensors, in which, during the movement of an autonomous vehicle (AV), data on its location is recorded using the navigation system sensor, and based on the data obtained, a track line of the vehicle's movement is formed on a virtual map, characterized in that at each point of the track, a polynomial regression of the trajectory is performed, the velocity vector, local curvature at the point and the radius of curvature are determined by differentiating the polynomial-regressed trajectory, the module of the velocity vector is compared with the readings of the speed sensor, data is accumulated and the desired calibration coefficient of the speed sensor is determined, simultaneously at the point track lines of vehicle movement track the readings of the wheel angle and vehicle speed sensors, taking into account the vehicle model, the curvature of the trajectory is compared with the distance between the wheelbases and the steering angle of the steering wheels, as a result of which the values of the wheel angle and vehicle speed sensors are obtained, and a polynomial regression of the set is performed accumulated points and calculate a polynomial whose coefficients are calibration.

На фиг.1 представлена схема расположения сенсоров колесной одометрии на автономном транспортном средстве, где 1 – сенсор навигационной системы, выполненный в виде антенны GNSS, 2 – датчик угла поворота управляемых колес, 3 – датчик скорости, 4 – траектория, зарегистрированная в процессе движения АТС. Figure 1 shows a diagram of the location of wheel odometry sensors on an autonomous vehicle, where 1 is a navigation system sensor made in the form of a GNSS antenna, 2 is a steering angle sensor, 3 is a speed sensor, 4 is a trajectory recorded while the vehicle is moving. .

На фиг.2 представлена конструктивная схема взаимного расположения датчика угла поворота (2) и колеса автономного транспортного средства (робота), где 5 – вал колеса Lk, 6 – рулевая трапеция Rk. Figure 2 shows a design diagram of the relative position of the rotation angle sensor (2) and the wheel of an autonomous vehicle (robot), where 5 is the wheel shaft Lk, 6 is the steering linkage Rk.

На фиг.2 показано, каким образом устанавливаются две дополнительные штанги Rd и Ld с шарнирным соединением, в результате чего образуется четырехугольник со сторонами Rk, Lk, Rd, Ld. При этом, датчик угла поворота (2) измеряет угол между ребрами Lk и Rd – β, а угол поворота колеса равен углу между ребрами Lk и Rk – α. Между углом поворота колеса и углом датчика существует тригонометрическая зависимость, которая в общем случае может быть приближена кубической интерполяцией.Figure 2 shows how two additional rods Rd and Ld are installed with a hinged connection, resulting in the formation of a quadrangle with sides Rk, Lk, Rd, Ld. In this case, the rotation angle sensor (2) measures the angle between the ribs Lk and Rd – β, and the angle of rotation of the wheel is equal to the angle between the ribs Lk and Rk – α. There is a trigonometric relationship between the wheel rotation angle and the sensor angle, which in general can be approximated by cubic interpolation.

Способ калибровки сенсоров колесной одометрии, в предпочтительном варианте, реализуется следующим образом (фиг.1, фиг.2).The method for calibrating wheel odometry sensors, in the preferred embodiment, is implemented as follows (Fig. 1, Fig. 2).

Для реализации предлагаемого способа датчик угла поворота колес (2), связанный с колесом автономного транспортного средства шарнирной передачей, в совокупности с точкой крепления к колесу и центром поворота колеса на оси образуют четырехугольник (фиг.2).To implement the proposed method, the wheel angle sensor (2), connected to the wheel of an autonomous vehicle by a hinge gear, together with the attachment point to the wheel and the center of rotation of the wheel on the axis, form a quadrangle (Fig. 2).

Также во многих моделях АТС присутствует датчик скорости (3), показания которого могут транслироваться через CAN шину устройства. Но датчик скорости определяет скорость по скорости вращения вала и может иметь погрешность, в зависимости от того, какие шины используются на АТС. Таким образом, значение датчика скорости также может калиброваться введением поправочного коэффициента.Also, many PBX models have a speed sensor (3), the readings of which can be transmitted via the device’s CAN bus. But the speed sensor determines the speed by the speed of rotation of the shaft and may have an error, depending on what tires are used on the vehicle. Thus, the speed sensor value can also be calibrated by introducing a correction factor.

В процессе движения автономного транспортного средства (АТС) посредством сенсора навигационной системы, выполненного в виде антенны GNSS (1), установленной на его кузове, регистрируют данные о местоположении АТС в геодезических единицах измерения – широту и долготу, а также может передаваться высота над уровнем моря (в зависимости от модели используемого сенсора). С учетом того, что движение осуществляется по условно плоской поверхности, высота может игнорироваться.During the movement of an autonomous vehicle (AV), a navigation system sensor made in the form of a GNSS antenna (1) installed on its body records data on the location of the vehicle in geodetic units of measurement - latitude and longitude, and altitude above sea level can also be transmitted (depending on the model of the sensor used). Taking into account the fact that the movement is carried out on a relatively flat surface, the height can be ignored.

Параметры местоположения АТС, такие как широта, долгота, а также опционно, высота, полученные от GNSS сенсора в конкретный момент времени, поступают в вычислительный блок с периодичностью работы GNSS сенсора.The location parameters of the vehicle, such as latitude, longitude, and optionally altitude, received from the GNSS sensor at a specific point in time, enter the computing unit with the frequency of operation of the GNSS sensor.

По мере поступления широты и долготы координаты преобразуются в локальную метровую систему координат XY с помощью проекции Меркатора с использованием той или иной модели геоида, известной из уровня техники. Таким образом, внутри вычислительного блока формируется последовательность записей вида <X, Y, T>, то есть последовательность координат движения (x, y), привязанных к времени t. As latitude and longitude coordinates are received, they are converted to a local XY meter coordinate system using a Mercator projection using one or another geoid model known in the art. Thus, inside the computing unit, a sequence of records of the form <X, Y, T> is formed, that is, a sequence of motion coordinates (x, y) tied to time t.

Последовательность координат <X, Y, T> обозначим как трек-линия движения, которая представляет собой аппроксимацию движения АТС.We denote the sequence of coordinates <X, Y, T> as a track line of movement, which is an approximation of the movement of the vehicle.

Затем, в каждой точке полученного трека движения производят полиномиальную регрессию траектории, в виде полиномиальных зависимостей вида:Then, at each point of the resulting movement track, a polynomial regression of the trajectory is performed, in the form of polynomial dependencies of the form:

Х(t) = P3(t), Y(t) = Q3(t); где P3(t) и Q3(t) – полиномы третьей степени от t. X(t) = P3(t), Y(t) = Q3(t); where P3(t) and Q3(t) are third-degree polynomials in t.

Указанные полиномы задаются коэффициентами при 0, 1, 2, 3 степени, то есть имеют вид X(t) = p0 + p1·t + p2·t² + p3·t³; Y(t) = q0 + q1·t + q2·t² + q3·t³;These polynomials are specified by coefficients of 0, 1, 2, 3 degrees, that is, they have the form X(t) = p0 + p1·t + p2·t ² + p3·t ³ ; Y(t) = q0 + q1 t + q2 t ² + q3 t ³ ;

Если в очередной точке ломаной X_iY_i траектория локально может быть приближена как полиномы <x(t), y(t)> = <P3(t), Q3(t) >, то кривизна траектории может быть рассчитана следующим образом:If at the next point of the broken line X _i Y _i the trajectory can be locally approximated as polynomials <x(t), y(t)> = <P3(t), Q3(t) >, then the curvature of the trajectory can be calculated as follows:

и которая может быть представлена в 3х-мерном случае как:and which can be represented in the 3-dimensional case as:

Приведенная формула расчета кривизны траектории параметрически заданной траектории является известным результатом математического анализа и представлена, в частности, в онлайн-версии учебника Л.Д. Кудрявцева «Краткий курс математического анализа» по адресу http://nuclphys.sinp.msu.ru/mathan/p1/m1802.html. The given formula for calculating the curvature of the trajectory of a parametrically specified trajectory is a well-known result of mathematical analysis and is presented, in particular, in the online version of the textbook by L.D. Kudryavtsev “Short Course in Mathematical Analysis” at http://nuclphys.sinp.msu.ru/mathan/p1/m1802.html.

В формуле используются следующие обозначения: The formula uses the following notation:

r' – первая производная траектории; r'' – вторая производная траектории; x', y', z' – производные координатных функций; x', y'', z'' – вторые производные координатных функций.r' – first derivative of the trajectory; r'' – second derivative of the trajectory; x', y', z' – derivatives of coordinate functions; x', y'', z'' – second derivatives of coordinate functions.

Если в рамках метода игнорируются колебания высоты z (то есть осуществляется движение по плоской площадке), то z' и z'' равны нулю и остается не нулевой третий член, для которого можно рассчитать значения производных x', y', x'', y'' как производные полиномов P3(t), Q3(t). If within the framework of the method, fluctuations in height z are ignored (that is, movement is carried out on a flat surface), then z' and z'' are equal to zero and a non-zero third term remains, for which the values of the derivatives x', y', x'' can be calculated, y'' as derivatives of the polynomials P3(t), Q3(t).

Таким образом, после полиномиальной регрессии траекторий в каждой точке трек-линии движения можно оценить кривизну.Thus, after polynomial regression of trajectories at each point of the track line of movement, the curvature can be estimated.

Также, определяют вектор скорости как производную полиномов x' (t), y' (t) по времени, сопоставляют с показаниями датчика скорости, производят накопление данных о местоположении АТС и кривизне движения в вычислительном блоке, а затем определяют искомый коэффициент датчика скорости следующим образом:Also, the speed vector is determined as the derivative of the polynomials x' (t), y' (t) with respect to time, compared with the readings of the speed sensor, data on the location of the vehicle and the curvature of movement are accumulated in the computing unit, and then the required coefficient of the speed sensor is determined as follows :

| V_GNSS |= | <x'(t), y' (t)> | = k_speed*fb_speed,| V _GNSS |= | <x'(t), y'(t)> | = k_speed*fb_speed,

где k_speed – искомый калибровочный коэффициент пропорциональности;where k_speed is the desired calibration proportionality coefficient;

fb_speed – скорость АТС, полученная по показаниям датчика скорости.fb_speed – vehicle speed obtained from speed sensor readings.

Вместе с тем, посредством датчика угла поворота управляемых колес (2) регистрируют угол поворота в условных единицах, производят накопление статистики кривизны траектории движения АТС в памяти вычислительного блока, определяют величину угла поворота колеса в зависимости от кинематической модели АТС, в частности модель с передними управляющими колесами, модель с задними управляющими колесами или шарнирно-сочлененная модель АТС.At the same time, using the steering angle sensor (2), the steering angle is recorded in conventional units, statistics of the curvature of the vehicle's motion trajectory are accumulated in the memory of the computing unit, and the wheel rotation angle is determined depending on the kinematic model of the vehicle, in particular a model with front steering wheels, a model with rear steering wheels or an articulated ATC model.

В рамках кинематической модели движения кривизна траектории колесного робота связана уравнениями с углом поворота колес, расстоянием между колесными осями и другими параметрами – в зависимости от модели транспортного средства. В частности, для колесного робота с двумя параллельными колесными осями и передними управляющими колесами (например, обычный автомобиль) зависимость угла поворота колес и кривизны траектории в контексте велосипедной модели выражается как Within the framework of the kinematic model of movement, the curvature of the trajectory of a wheeled robot is related by equations to the angle of rotation of the wheels, the distance between the wheel axles and other parameters, depending on the vehicle model. In particular, for a wheeled robot with two parallel wheel axles and front steering wheels (for example, a conventional car), the relationship between wheel angle and path curvature in the context of a bicycle model is expressed as

angle = atan (axle_base * curvature);angle = atan(axle_base * curve);

где axle_base – расстояние между колесными базами;where axle_base is the distance between the wheelbases;

curvature – радиус кривизны траектории.curvature – radius of curvature of the trajectory.

По результатам проведения вышеуказанных этапов предлагаемого способа получают два распределения – угла поворота датчика fb(t) и угла поворота управляемых колес angle(t).Based on the results of the above stages of the proposed method, two distributions are obtained - the angle of rotation of the sensor fb(t) and the angle of rotation of the steering wheels angle(t).

С учетом геометрии шарнира привода сенсора, который образует четырехугольник, можно определить тригонометрическую зависимость между углом поворота колеса и углом поворота датчика (фиг.2). Taking into account the geometry of the sensor drive joint, which forms a quadrangle, it is possible to determine the trigonometric relationship between the angle of rotation of the wheel and the angle of rotation of the sensor (Fig. 2).

Как показывают расчеты, при малых углах зависимость носит линейный характер, при увеличении углов зависимость начинает носить характер, близкий к кубической параболе.As calculations show, at small angles the dependence is linear; as the angles increase, the dependence begins to have a character close to a cubic parabola.

Аппроксимация зависимости fb(angle) в виде кубической зависимости в данной задаче является часто используемым приемом. В таком случае ответом калибровки являются коэффициенты кубического полинома k₀, k₁, k₂, k₃ при соответствующих степенях fb, которые могут быть получены методом полиномиальной регрессии, что позволяет рассчитать угол поворота колес angle в зависимости от значений датчика угла поворота fb и далее оценивать траекторию движения колесного робота.Approximating the fb(angle) dependence as a cubic dependence in this problem is a frequently used technique. In this case, the calibration response is the coefficients of the cubic polynomial k ₀ , k ₁ , k ₂ , k ₃ at the corresponding degrees fb, which can be obtained by the polynomial regression method, which allows you to calculate the angle of rotation of the wheels angle depending on the values of the steering angle sensor fb and further evaluate the trajectory of a wheeled robot.

Claims (1)

Способ калибровки сенсоров колесной одометрии, при котором в процессе движения автономного транспортного средства (АТС) посредством сенсора навигационной системы регистрируют данные о его местоположении, по полученным данным формируют трек-линию движения АТС на виртуальной карте, отличающийся тем, что в каждой точке трека производят полиномиальную регрессию траектории, определяют вектор скорости, локальную кривизну в точке и радиус кривизны посредством дифференцирования полиномиально-регрессированной траектории, модуль вектора скорости сопоставляют с показаниями датчика скорости, производят накопление данных и определяют искомый калибровочный коэффициент датчика скорости, одновременно в точке трек-линии движения АТС отслеживают показания датчика поворота угла колеса и скорости АТС, получают значения датчиков поворота угла колеса и скорости АТС, производят полиномиальную регрессию множества накопленных точек и вычисляют полином, коэффициенты которого являются калибровочными.A method for calibrating wheel odometry sensors, in which, while an autonomous vehicle (AV) is moving, data about its location is recorded using a navigation system sensor, and based on the data obtained, a track line of the vehicle’s movement is formed on a virtual map, characterized in that at each point of the track a polynomial regression of the trajectory, determine the speed vector, local curvature at the point and the radius of curvature by differentiating the polynomial-regressed trajectory, the module of the speed vector is compared with the readings of the speed sensor, data is accumulated and the desired calibration coefficient of the speed sensor is determined, at the same time the movement of the vehicle is monitored at the point on the track line readings of the wheel angle and vehicle speed sensors, the values of the wheel angle and vehicle speed sensors are obtained, a polynomial regression of the set of accumulated points is performed and a polynomial is calculated, the coefficients of which are calibration.