CN110489807B - Local accurate positioning method for swing arm suspension structure inspection device - Google Patents
Local accurate positioning method for swing arm suspension structure inspection device Download PDFInfo
- Publication number
- CN110489807B CN110489807B CN201910668424.8A CN201910668424A CN110489807B CN 110489807 B CN110489807 B CN 110489807B CN 201910668424 A CN201910668424 A CN 201910668424A CN 110489807 B CN110489807 B CN 110489807B
- Authority
- CN
- China
- Prior art keywords
- wheel
- rocker arm
- wheels
- component
- coordinate system
- 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.)
- Active
Links
Landscapes
- Length Measuring Devices With Unspecified Measuring Means (AREA)
- Vehicle Body Suspensions (AREA)
Abstract
A local accurate positioning method of a patrol device with a rocker arm suspension structure comprises the steps of firstly calculating theoretical instantaneous body speed of the patrol device under the action of six wheels, then calculating theoretical instantaneous steering angular speed of the patrol device under the action of six wheels, and finally calculating position estimation information of the patrol device with slip compensation. Compared with the prior art, the invention can better reflect the influence of the terrain change on the structure of the inspection device, effectively reduce the calculation error caused by sliding to the positioning of the whole device through indirect calculation and compensation, can be suitable for various complex terrains, has good walking performance and strong practicability.
Description
Technical Field
The invention relates to a local accurate positioning method of a swing arm suspension structure inspection device, which can be applied to local accurate positioning of a roaming vehicle with a swing arm suspension type chassis structure, steering control by adopting steering wheels and an angular velocity measurement sensor.
Background
The local positioning of the patrol machine is also called relative position determination, and the task is that in a navigation coordinate system taking a certain starting point as an origin, three-axis coordinates of the patrol machine in the motion process are determined according to measurement data of a sensor of the patrol machine.
For a moving vehicle moving at a low speed, because the accuracy of obtaining position information by integrating by adopting an accelerometer is poor, the inertial navigation system cannot be generally used for local positioning, and a positioning method based on a wheel train odometer is widely used.
The wheel speed output of each wheel is weighted and integrated to obtain the movement speed of the patrol device, and the movement mileage and the position change are obtained by dead reckoning. The conventional wheel train odometer positioning method of the vehicle only needs to consider the situation when in a plane, namely, the estimation of the speed of the whole vehicle is completed through the relative geometrical positional relationship between the planes of each wheel and the center of the whole vehicle. For a patrol machine of a rocker arm suspension chassis structure, the application environment is unstructured complex relief terrain, and if only two-dimensional plane motion is considered, larger errors can be generated in local positioning. In addition, slippage generated by movement on soft terrain can bring uncertainty to movement of the patrol device, and the positioning result is greatly affected.
Disclosure of Invention
The invention aims to overcome the defects of the prior art, and provides a local accurate positioning method of a swing arm suspension structure inspection device, which introduces swing arm joint measurement information and whole steering angular velocity measurement information, so that the influence of terrain change on the inspection device structure can be better reflected under non-planar terrain, and through indirect calculation and compensation, the calculation error brought by slippage to the whole inspection device positioning is effectively reduced, and the method is applicable to various complex terrains, has good walking performance and strong practicability.
The above object of the present invention is mainly achieved by the following technical solutions:
the utility model provides a local accurate positioning method of a rocking arm suspension structure inspection device, inspection device's train includes six wheels altogether of two front wheels, two rear wheels and two well wheels, and each wheel all possesses steering capability, and six wheels are on the coplanar or not on the coplanar for six wheels are on any topography and all land, and the concretely realization step is as follows:
step one, calculating theoretical instant body speed of the inspection device under the action of six rounds, wherein the specific method comprises the following steps:
(1) Establishing an inspection device vehicle body coordinate system OXYZ, wherein the advancing direction of the inspection device vehicle body is the X direction, and the Y direction and the Z direction are directions pointed by any two vertical vectors in a plane perpendicular to the X direction and meet the right-hand rule;
(2) Calculating theoretical instant body speed of inspection device under action of ith single wheel
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; />Is the rotational angular velocity of the i-th wheel; j (J) iθv For the speed jacobian of the ith wheel, one can describe +.>And->Linear transformation relation of J iθv ＝[J iθvx J iθvy J iθvz ] T Wherein: j (J) iθvx Is J iθv Component in X direction, J iθvy Is J iθv Component in Y direction, J iθvz Is J iθv A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(3) Calculating theoretical instant body speed of patrol device under six-wheel action
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction;
calculating theoretical instantaneous steering angular velocity of the inspection device under the action of six wheels, wherein the specific method comprises the following steps:
(1) Calculating the position of the ith wheel center in the coordinate system OXYZ of the vehicle body
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(2) Calculating theoretical instantaneous speed of ith wheel center under vehicle body coordinate system OXYZ
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(3) Calculating theoretical instantaneous steering angular velocity of patrol device
Calculating position estimation information of the inspection device with slip compensation, wherein the specific method comprises the following steps:
(1) Calculating instantaneous slip compensation coefficient k s ：
Wherein: omega zE For measuring the obtained true steering angular velocity k of the inspection device s0 Default slip compensation coefficient, k for inspection device min To compensate for the lower limit of slip, k max To compensate for the upper limit of slip, ω th To calculate the slip-compensated steering angle speed threshold, k min And k max Can be combined with the test or analysis result of the mechanical property of the soil to determine that k should be generally adopted min <k max ≤1，ω th For zero removal protection, it can be a non-negative small value, LIMIT (a, B, C) indicates that if a is less than B, the value of B is taken, if a is greater than C, the value of C is taken, otherwise the value of a is taken;
(2)calculating a patrol device speed estimation in the vehicle body coordinate system OXYZ
(3) Calculating a patrol machine speed estimation in a navigational coordinate system
Wherein the navigation coordinate system is a coordinate system fixedly connected with the terrain environment and is different from the vehicle body coordinate system, the origin position and the triaxial direction of the coordinate system are not changed along with the movement of the patrol instrument,is a conversion matrix from a car body coordinate system to a navigation coordinate system.
(4) Integration to obtain the position estimation P of the patrol device under the navigation coordinate system E ：
Wherein P is EL And estimating the position of the patrol device in the previous sampling period, wherein deltat is the sampling period, namely the calculation period of the local accurate positioning of the invention.
The inspection device wheel train comprises a differential mechanism, two main rocker arms, two auxiliary rocker arms, two front wheels, two rear wheels and two middle wheels, wherein the two main rocker arms are connected with an inspection device vehicle body through the differential mechanism, the differential mechanism is fixedly connected to the inspection device vehicle body, the rotation angles of the left main rocker arm and the right main rocker arm relative to the differential mechanism are equal in size and opposite in direction, each main rocker arm is respectively connected with one front wheel and one auxiliary rocker arm, and each auxiliary rocker arm is respectively connected with one middle wheel and one rear wheel;
calculating the Jacobian J of the speed of the ith wheel in step (2) of step (one) iθv The specific method of (2) is as follows:
for both front wheels, i=1, 2; the formula is as follows:
wherein: beta i Is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ，δ i For the steering angle of the two front wheels, R W Wheel radius for two front wheels;
for two middle wheels, i=3, 4; the formula is as follows:
wherein:β i is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ，ρ 1 、ρ 2 Is the rotation angle delta of the auxiliary rocker arm relative to the main rocker arm i For the steering angle of the two middle wheels, R W Wheel radius for two middle wheels;
for both rear wheels, i=5, 6; the formula is as follows:
wherein:β i is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ，ρ 1 、ρ 2 For turning the auxiliary rocker arm relative to the main rocker armAngle delta i For the steering angle of the two rear wheels, R W Is the wheel radius of the two rear wheels.
Calculating the position of each wheel in the vehicle body coordinate system OXYZ in the step (1)The specific method of (2) is as follows:
for both front wheels, i=1, 2; the formula is as follows:
wherein: l (L) m2 Is the horizontal distance between the front wheel axle and the main rocker arm rotating shaft, l x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l di Represents the horizontal distance from the geometric center of the differential mechanism to the wheel centers of the left and right wheels, d a1 R is the vertical distance between the main rocker arm rotating shaft and the front wheel shaft W Is the wheel radius of the two front wheels.
For two middle wheels, i=3, 4; the formula is as follows:
wherein: l (L) r2 L is the horizontal distance between the middle wheel shaft and the rotating shaft of the auxiliary rocker arm m1 Is the horizontal distance of the main rocker arm and the auxiliary rocker arm x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l d(i-2) Represents the horizontal distance from the geometric center of the differential mechanism to the wheel centers of the left and right wheels, d a2 D is the vertical distance between the rotating shaft of the auxiliary rocker arm and the middle wheel shaft r R is the vertical distance between the main rocker arm and the auxiliary rocker arm W Is the wheel radius of the two middle wheels.
For both rear wheels, i=5, 6; the formula is as follows:
wherein: l (L) r1 L is the horizontal distance between the rear axle and the rotating shaft of the auxiliary rocker arm m1 Is the horizontal distance of the main rocker arm and the auxiliary rocker arm x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l d(i-4) Represents the distance from the geometric center of the differential mechanism to the wheel centers of the left and right wheels, d a3 D is the vertical distance between the rotating shaft of the auxiliary rocker arm and the rear wheel shaft r R is the vertical distance between the main rocker arm and the auxiliary rocker arm W Is the wheel radius of the two rear wheels.
Compared with the prior art, the invention has the following beneficial effects:
(1) According to the invention, on the basis of the design thought of the positioning method of the traditional vehicle based on the wheel train odometer, by introducing the rocker arm joint rotation angle measurement information, the negative influence of the topography fluctuation on local positioning can be reduced to the greatest extent under non-planar topography.
(2) The invention utilizes the steering angular velocity measurement information, and through indirect calculation and compensation, the calculation error brought by slippage to the positioning of the whole device is effectively reduced.
(3) The local positioning method is also suitable for local positioning of a roaming vehicle or a wheeled mobile robot with a similar deformable chassis structure, has a wider application range, is suitable for various complex terrains, has good walking performance and strong practicability.
Drawings
FIG. 1 is a schematic illustration of a conventional planar six-wheel steering vehicle corner assignment;
FIG. 2 is a schematic diagram of the gear train of the inspection device of the present invention;
FIG. 3 is a schematic diagram of geometric parameters of the inspection machine train of the present invention.
Detailed Description
The invention is described in further detail below with reference to the attached drawings and to specific embodiments:
the inspection device is provided with a six-wheel steering and six-wheel driving rocker arm suspension chassis structure, and can be passively adapted to natural relief topography, thus belonging to a wheel type mobile robot with incomplete constraint. The invention discloses a patrol device wheel train which comprises six wheels: two front wheels, two rear wheels and two middle wheels, each wheel having steering capability, and six wheels being on the same plane or not.
If the wheels cannot be guaranteed to be coplanar in the movement process of the inspection device, if the wheels are not on the same plane, the positioning method of the wheel train odometer must be adjusted by considering the three-dimensional space relation, and the position change of the wheels relative to the whole device, which is generated when the rocker arm suspension chassis structure passively adapts to the terrain, is substituted into an algorithm.
Ideally, as shown in fig. 1, through reasonably distributing the turning angles of all steering wheels, when the steering wheels are in a coordinated motion state, the expected motion speed of all wheels on the terrain is basically consistent with the actual speed, but due to the complex mechanical relationship of the contact of the wheels, longitudinal slippage is often generated in the motion process, the local positioning of a patrol device is adversely affected, and the motion slippage is generally not measurable and cannot be accurately measured.
The invention relates to a local accurate positioning method of a swing arm suspension structure inspection device, which specifically comprises the following steps:
step one, calculating theoretical instant body speed of the inspection device under the action of six rounds, wherein the specific method comprises the following steps:
(1) Establishing an inspection device vehicle body coordinate system OXYZ, wherein the advancing direction of the inspection device vehicle body is the X direction, and the Y direction and the Z direction are directions pointed by any two vertical vectors in a plane perpendicular to the X direction and meet the right-hand rule;
(2) Calculating theoretical instant body speed of inspection device under action of ith single wheel
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; />Is the rotational angular velocity of the i-th wheel; j (J) iθv For the speed jacobian of the ith wheel, one can describe +.>And->Linear transformation relation of J iθv ＝[J iθvx J iθvy J iθvz ] T Wherein: j (J) iθvx Is J iθv Component in X direction, J iθvy Is J iθv Component in Y direction, J iθvz Is J iθv A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(3) Calculating theoretical instant body speed of patrol device under six-wheel action
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction;
calculating theoretical instantaneous steering angular velocity of the inspection device under the action of six wheels, wherein the specific method comprises the following steps:
(1) Calculating the position of the ith wheel center in the coordinate system OXYZ of the vehicle body
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(2) Calculating theoretical instantaneous speed of ith wheel center under vehicle body coordinate system OXYZ
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(3) Calculating theoretical instantaneous steering angular velocity of patrol device
Calculating position estimation information of the inspection device with slip compensation, wherein the specific method comprises the following steps:
(1) Calculating instantaneous slip compensation coefficient k s ：
Wherein: omega zE For measuring the obtained true steering angular velocity k of the inspection device s0 Default slip compensation coefficient, k for inspection device min To compensate for the lower limit of slip, k max To compensate for the upper limit of slip, ω th To calculate the slip-compensated steering angle speed threshold, k min And k max Can be combined with the test or analysis result of the mechanical property of the soil to determine that k should be generally adopted min <k max ≤1，ω th For zero removal protection, it can be a non-negative small value, LIMIT (a, B, C) indicates that if a is less than B, the value of B is taken, if a is greater than C, the value of C is taken, otherwise the value of a is taken;
(2) Calculating a patrol device speed estimation in the vehicle body coordinate system OXYZ
(3) Calculating a patrol machine speed estimation in a navigational coordinate system
Wherein the navigation coordinate systemFor being fixedly connected with a coordinate system of a terrain environment, the three-axis directions are respectively directed to the north, east and ground directions,the calculation formula of the conversion matrix from the vehicle body coordinate system to the navigation coordinate system is as follows:
wherein the method comprises the steps ofEuler angles of a vehicle body coordinate system described by adopting 3-2-1 turn sequence relative to a navigation coordinate system are respectively a rolling gesture, a pitching gesture and a yawing gesture.
(4) Integration to obtain the position estimation P of the patrol device under the navigation coordinate system E ：
Wherein P is EL The patrol position estimate for the previous sample period, Δt, is the sample period.
The invention provides a gear train structure of an inspection machine, which comprises a differential mechanism, two main rocker arms and two auxiliary rocker arms, wherein the two main rocker arms are connected with an inspection machine body through the differential mechanism, the differential mechanism is fixedly connected with the body, the differential mechanism is used for enabling the rotation angles of the left main rocker arm and the right main rocker arm relative to the differential mechanism to be equal in size and opposite in direction, each main rocker arm is respectively connected with a front wheel and one auxiliary rocker arm, and each auxiliary rocker arm is respectively connected with a middle wheel and a rear wheel. In fig. 3, a schematic representation of one side (divided into left and right sides) of the inspection device train is shown, which comprises a main rocker arm, an auxiliary rocker arm, a front wheel, a middle wheel and a rear wheel, the other side is completely symmetrical, and the definition of the related geometric structural parameters is shown in fig. 3.
Establishing a vehicle body coordinate system OXYZ of the patrol device, wherein the center of a bottom plate of the vehicle body of the patrol device is used as a circle center O, the advancing direction of the vehicle body of the patrol device on the plane of the bottom plate is used as an X direction, the direction which is perpendicular to the X direction and is directed to the right side of the patrol device on the plane of the bottom plate is used as a Y direction, the Z direction is perpendicular to the plane of the bottom plate and meets the right hand rule with the X, Y direction, and the steering wheel rotation angle is defined to turn right to be positive and turn left to be negative.
For both front wheels, i=1, 2; the formula is as follows:
wherein: beta i I.e. beta 1 、β 2 Is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ；δ i For steering angle, R W Is the radius of the wheel; d, d a1 Is the vertical distance between the main rocker arm rotating shaft and the front wheel axle, l x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l m2 Is the horizontal distance between the front wheel axle and the main rocker arm rotating shaft, l di I.e. l d1 、l d2 The distance from the geometric center of the differential mechanism to the centers of the left and right wheels is shown.
For two middle wheels, i=3, 4; the formula is as follows:
wherein:i.e. < ->Respectively->ρ 1 、ρ 2 The rotation angle of the auxiliary rocker arm relative to the main rocker arm; d, d a2 Is the vertical distance between the rotating shaft of the auxiliary rocker arm and the middle wheel shaft, l m1 Is the horizontal distance of the main rocker arm and the auxiliary rocker arm r2 D is the horizontal distance between the middle wheel shaft and the rotating shaft of the auxiliary rocker arm r Is the vertical distance of the main rocker arm and the auxiliary rocker arm d(i-2) I.e. l d1 、l d2 The distance from the geometric center of the differential mechanism to the centers of the left and right wheels is shown.
For both rear wheels, i=5, 6; the formula is as follows:
wherein:i.e. < ->Respectively->d a3 Is the vertical distance between the rotating shaft of the auxiliary rocker arm and the rear wheel shaft, l r1 L is the horizontal distance between the rear axle and the rotating shaft of the auxiliary rocker arm d(i-4) I.e. l d1 、l d2 The distance from the geometric center of the differential mechanism to the centers of the left and right wheels is shown.
In this embodiment, k is taken when calculating the slip compensation coefficient s0 Is 0.9, k min Is 0.8, k max 1.0, omega th 0.005rad/s, i.e.:
when the absolute value of the theoretical steering angular velocity is smaller than 0.005rad/s, the slip compensation coefficient is taken as 0.9, otherwise, the slip compensation coefficient is taken as a value between 0.8 and 1.0 according to the calculation result.
The method is also suitable for the wheel train coordination control of the roaming vehicle or the wheel type mobile robot with the similar deformable chassis structure.
What is not described in detail in the present specification belongs to the known technology of those skilled in the art.
Claims (5)
1. A local accurate positioning method of a swing arm suspension structure inspection device is characterized by comprising the following specific implementation steps:
calculating theoretical instantaneous body speed of the patrol device under the action of six wheels, wherein the wheel train of the patrol device comprises six wheels, namely two front wheels, two rear wheels and two middle wheels, each wheel has steering capability, and the six wheels are on the same plane or not on the same plane, so that the six wheels land on any terrain;
calculating theoretical instantaneous steering angular speed of the inspection device under the action of six wheels;
(21) Calculating the position of the ith wheel center in the coordinate system OXYZ of the vehicle body
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(22) Calculating theoretical instantaneous speed of ith wheel center under vehicle body coordinate system OXYZ
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; i=1, 2, 3, 4, 5, 6；/>Theoretical instantaneous body speed of the inspection device under the action of the ith single wheel, < +.>Theoretical instantaneous body speed for a cruiser under six-wheel action;
(23) Calculating theoretical instantaneous steering angular velocity of patrol device
Calculating position estimation information of the patrol device with slip compensation:
(31) Calculating instantaneous slip compensation coefficient k s ：
Wherein: omega zE For measuring the obtained true steering angular velocity k of the inspection device s0 Default slip compensation coefficient, k for inspection device min To compensate for the lower limit of slip, k max To compensate for the upper limit of slip, ω th To calculate the slip-compensated steering angle speed threshold, k min And k max Can be combined with the test or analysis result of the mechanical property of the soil to determine k min ＜k max ≤1，ω th For zero removal protection, LIMIT (a, B, C) indicates that if a is less than B, the value of B is taken, if a is greater than C, the value of C is taken, otherwise the value of a is taken;
(32) Calculating a patrol device speed estimation in the vehicle body coordinate system OXYZ
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction;
(33) Calculating a patrol machine speed estimation in a navigational coordinate system
Wherein the navigation coordinate system is a coordinate system fixedly connected with the terrain environment, the origin position and the triaxial direction of the coordinate system are not changed along with the movement of the patrol device,the method comprises the steps of converting a vehicle body coordinate system into a navigation coordinate system;
(34) Integration to obtain the position estimation P of the patrol device under the navigation coordinate system E ：
Wherein P is EL And estimating the position of the patrol device in the previous sampling period, wherein deltat is the sampling period corresponding to the calculation period of the local accurate positioning.
2. The method for locally and precisely positioning the swing arm suspension structure inspection machine according to claim 1, wherein the method comprises the following steps: the specific method for calculating the theoretical instantaneous body speed of the patrol device under six-wheel action in the step (one) is as follows:
(1) Establishing an inspection device vehicle body coordinate system OXYZ, wherein the advancing direction of the inspection device vehicle body is the X direction, and the Y direction and the Z direction are directions pointed by any two vertical vectors in a plane perpendicular to the X direction and meet the right-hand rule;
(2) Calculating theoretical instant body speed of inspection device under action of ith single wheel
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction; />Is the rotational angular velocity of the i-th wheel; j (J) iθv Description of ith wheel +.>And->Velocity jacobian, J of linear conversion relation iθv ＝[J iθvx J iθvy J iθvz ] T Wherein: j (J) iθvx Is J iθv Component in X direction, J iθvy Is J iθv Component in Y direction, J iθvz Is J iθv A component in the Z direction; i=1, 2, 3, 4, 5, 6;
(3) Calculating theoretical instant body speed of patrol device under six-wheel action
Wherein:is->Component in the X direction, < >>Is->Component in Y-direction, ++>Is->A component in the Z direction.
3. The method for locally and precisely positioning the swing arm suspension structure inspection machine according to claim 1, wherein the method comprises the following steps: the inspection device wheel train comprises a differential mechanism, two main rocker arms, two auxiliary rocker arms, two front wheels, two rear wheels and two middle wheels, wherein the two main rocker arms are connected with an inspection device body through the differential mechanism, the differential mechanism is fixedly connected to the inspection device body, the rotation angles of the left main rocker arm and the right main rocker arm relative to the differential mechanism are equal in size and opposite in direction, each main rocker arm is respectively connected with one front wheel and one auxiliary rocker arm, and each auxiliary rocker arm is respectively connected with one middle wheel and one rear wheel.
4. The method for locally and precisely positioning the swing arm suspension structure inspection machine according to claim 2, wherein the method comprises the following steps: calculating the Jacobian J of the speed of the ith wheel in the step (2) in the step (one) iθv The specific method of (2) is as follows:
for both front wheels i=1, 2, yielding
Wherein: beta i Is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ，δ i For the steering angle of the two front wheels, R W Wheel radius for two front wheels;
for both middle wheels i=3, 4, we get
Wherein:β i is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ，ρ 1 、ρ 2 Is the rotation angle delta of the auxiliary rocker arm relative to the main rocker arm i For the steering angle of the two middle wheels, R W Wheel radius for two middle wheels;
for both rear wheels i=5, 6, we get
Wherein:β i is the rotation angle of the main rocker arm relative to the differential mechanism and meets the requirement of beta 1 ＝-β 2 ，ρ 1 、ρ 2 Is the rotation angle delta of the auxiliary rocker arm relative to the main rocker arm i For the steering angle of the two rear wheels, R W Is the wheel radius of the two rear wheels.
5. The method for locally and precisely positioning the swing arm suspension structure inspection machine according to claim 1, wherein the method comprises the following steps: calculating the position of each wheel in the body coordinate system OXYZ in step (21) in the step (two)The specific method of (2) is as follows:
for both front wheels i=1, 2, yielding
Wherein: l (L) m2 Is the horizontal distance between the front wheel axle and the main rocker arm rotating shaft, l x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l di Represents the horizontal distance from the geometric center of the differential mechanism to the ith wheel center on the left and right sides, d a1 R is the vertical distance between the main rocker arm rotating shaft and the front wheel shaft W Wheel radius for two front wheels;
for both middle wheels i=3, 4, we get
Wherein: l (L) r2 L is the horizontal distance between the middle wheel shaft and the rotating shaft of the auxiliary rocker arm m1 Is the horizontal distance of the main rocker arm and the auxiliary rocker arm x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l d(i-2) Represents the horizontal distance from the geometric center of the differential mechanism to the ith wheel center on the left and right sides, d a2 D is the vertical distance between the rotating shaft of the auxiliary rocker arm and the middle wheel shaft r R is the vertical distance between the main rocker arm and the auxiliary rocker arm W Wheel radius for two middle wheels;
for both rear wheels, i=5, 6; the formula is as follows:
wherein: l (L) r1 L is the horizontal distance between the rear axle and the rotating shaft of the auxiliary rocker arm m1 Is the horizontal distance of the main rocker arm and the auxiliary rocker arm x 、l z Is the X-direction and Z-direction coordinates of the center of the differential mechanism under the vehicle body coordinate system of the inspection device, l d(i-4) Representing the differenceDistance d from geometric center of moving mechanism to ith wheel center on left and right sides a3 D is the vertical distance between the rotating shaft of the auxiliary rocker arm and the rear wheel shaft r R is the vertical distance between the main rocker arm and the auxiliary rocker arm W Is the wheel radius of the two rear wheels.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910668424.8A CN110489807B (en) | 2019-07-23 | 2019-07-23 | Local accurate positioning method for swing arm suspension structure inspection device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910668424.8A CN110489807B (en) | 2019-07-23 | 2019-07-23 | Local accurate positioning method for swing arm suspension structure inspection device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110489807A CN110489807A (en) | 2019-11-22 |
| CN110489807B true CN110489807B (en) | 2023-07-18 |
Family
ID=68547992
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910668424.8A Active CN110489807B (en) | 2019-07-23 | 2019-07-23 | Local accurate positioning method for swing arm suspension structure inspection device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110489807B (en) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111899303B (en) * | 2020-07-14 | 2021-07-13 | 中国人民解放军63920部队 | Novel feature matching and relative positioning method considering space inverse projection constraint |
| CN113734317A (en) * | 2021-09-18 | 2021-12-03 | 北京控制工程研究所 | Kinematic positioning method for swinging arm suspension frame structure inspection device |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN101173858A (en) * | 2007-07-03 | 2008-05-07 | 北京控制工程研究所 | Three-dimensional posture fixing and local locating method for lunar surface inspection prober |
| JP2008142841A (en) * | 2006-12-11 | 2008-06-26 | Toyota Motor Corp | Mobile robot |
| CN109374315A (en) * | 2018-12-18 | 2019-02-22 | 中国科学院自动化研究所 | Acquisition methods, control method, system, the device of the instantaneous slippage rate of wheel of vehicle |
-
2019
- 2019-07-23 CN CN201910668424.8A patent/CN110489807B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008142841A (en) * | 2006-12-11 | 2008-06-26 | Toyota Motor Corp | Mobile robot |
| CN101173858A (en) * | 2007-07-03 | 2008-05-07 | 北京控制工程研究所 | Three-dimensional posture fixing and local locating method for lunar surface inspection prober |
| CN109374315A (en) * | 2018-12-18 | 2019-02-22 | 中国科学院自动化研究所 | Acquisition methods, control method, system, the device of the instantaneous slippage rate of wheel of vehicle |
Non-Patent Citations (5)
| Title |
|---|
| Xiaokang Song etc .Kinematics-Based Velocity Estimation of Lunar Rovers.《Proceedings of the 2007 IEEE International Conference on Robotics and Biomimetics》.2007,全文. * |
| 徐辛超 ; 徐爱功 ; 刘少创 ; 马友青 ; 郑真真 ; .降落影像和导航影像的巡视探测器定位方法.导航定位学报.2017,(第02期),全文. * |
| 方海涛 ; 邓宗全 ; 陶建国 ; 董玉红 ; 禹鑫D ; .六圆锥轮式月球车运动控制系统的设计研究.机器人.2007,(第05期),全文. * |
| 李清毅 ; .巡视器对地指向受月表地形的影响分析.中国空间科学技术.2009,(第06期),全文. * |
| 陈建新, 邢琰, 滕宝毅, 毛晓艳, 刘祥, 贾永, 张晋, 王磊.嫦娥三号巡视器GNC 及地面试验技术.《中国科学》.2014,全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110489807A (en) | 2019-11-22 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109916431B (en) | Wheel encoder calibration algorithm for four-wheel mobile robot | |
| CN113819914B (en) | Map construction method and device | |
| CN109141410B (en) | Multi-sensor fusion positioning method for AGV (automatic guided vehicle) combined navigation | |
| CN108731670A (en) | Inertia/visual odometry combined navigation locating method based on measurement model optimization | |
| CN107063305B (en) | Method for correcting downhill suspended rear wheel odometer error by using inertial navigation and pressure sensors | |
| CN106568449B (en) | A kind of GNSS/INS Combinated navigation method of auto model auxiliary and constraint based on MEMS | |
| CN105172793A (en) | Pose estimation method for automatic driving automobile | |
| CN113819905B (en) | Mileage metering method and device based on multi-sensor fusion | |
| WO2020191977A1 (en) | Dead reckoning method of automatic parking positioning system | |
| CN108036797A (en) | Mileage projectional technique based on four motorized wheels and combination IMU | |
| CN104713555A (en) | Autonomous vehicle navigation method for assisting orientation by applying omnimax neutral point | |
| CN104977002A (en) | SINS/double OD-based inertial integrated navigation system and method | |
| CN110489807B (en) | Local accurate positioning method for swing arm suspension structure inspection device | |
| CN112097792A (en) | Ackerman model mobile robot odometer calibration method | |
| CN111024074A (en) | Inertial navigation speed error determination method based on recursive least square parameter identification | |
| CN115597535A (en) | High-speed magnetic suspension track irregularity detection system and method based on inertial navigation | |
| CN113008229B (en) | Distributed autonomous integrated navigation method based on low-cost vehicle-mounted sensor | |
| CN109033017B (en) | Vehicle roll angle and pitch angle estimation method under packet loss environment | |
| Zhang et al. | Learning end-to-end inertial-wheel odometry for vehicle ego-motion estimation | |
| CN113734317A (en) | Kinematic positioning method for swinging arm suspension frame structure inspection device | |
| CN113048987A (en) | Vehicle navigation system positioning method | |
| Guan et al. | Multi-sensor fusion vehicle positioning based on Kalman Filter | |
| CN211012986U (en) | Unmanned autonomous cruise vehicle navigation system based on inertial navigation technology | |
| CN109945857B (en) | Vehicle-mounted inertial positioning method and device for real estate field measurement | |
| CN116576849A (en) | Vehicle fusion positioning method and system based on GMM assistance |
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 |