Swing arm detection device and method, electronic equipment and storage medium

Abstract

The invention provides a swing arm detection device and method, electronic equipment and a storage medium. The swing arm detection device comprises a vehicle body, swing arms on the two sides, swing arm wheels, a motor system, an identification system and a control system. The vehicle body is composed of a vehicle body, two main wheels and a detachable track assembly, the main wheels serve as main supporting parts, and the swing arm detection device can be switched into a wheel mode and a track mode according to different environment detection tasks; the swing arms on the two sides can rotate by 360 degrees and are connected with the main wheels on the two sides of the vehicle body respectively, the motor system controls the main wheels to rotate and controls the swing arms on the two sides to swing in the circumferential direction, the swing arm wheels are connected to the main wheels through swing arms, and sharp hook assemblies are installed on the sides, close to the swing arm wheels, of the swing arms. The recognition system is used for recognizing the type of an obstacle, and the control system is used for giving an instruction according to an obstacle recognition result and a detection result and controlling the swing arm detection device to advance, retreat, turn and climb over. The device provided by the invention can detect front obstacles, and also can detect and model the terrain by contacting with the ground.

Description

Technical Field

[0001] The present invention relates to the technical field of robots, and particularly to a swing arm detection method and a swing arm detection device. Background Art

[0002] For the detection and recognition of obstacles or terrain in front, radar or visual cameras are usually used. Radar determines the position and attributes of underground objects by sending detection signals and receiving the reflected signals, and gives accurate position and size information. For example, lidar scanning uses a laser sensor to obtain the three-dimensional shape and position information of underground objects. This technology can quickly and efficiently obtain the geometric features of underground obstacles, but has certain requirements for the transparency of underground media. It is also possible to use a visual camera and utilize image processing and computer vision technologies to identify obstacles and terrain, such as edge detection, feature extraction, deep learning, stereo vision, etc.

[0003] In contact detection, there are some devices such as surface roughness measuring instruments. Such devices use a probe with a standardized surface to contact the surface of the object to be measured, and measure the surface roughness of the object through the contact point of the probe. However, the scale is usually small. In addition, there is a ground flatness measuring instrument, which usually consists of a measuring host, a measuring wheel, and a control system, etc. The measuring wheel contacts the ground, and the measuring host obtains the elevation data of the ground through the measuring wheel, and then combines the control system to process and analyze the data to obtain the flatness of the ground.

[0004] When radar or a visual camera detects the ground or the front environment, there are often some problems. For example, when encountering items such as flowers, leaves, plastic bags, cloth strips, etc., which are scattered on the ground or located in the front, they are usually regarded as obstacles by radar or the camera. In fact, such items can be directly rolled over or passed through. It is very difficult to accurately identify them only by radar or the camera, and complex and precise algorithms are required. Currently, it is still a difficult point that the academic and industrial circles have not been able to solve well.

[0005] In similar contact detection, such as ground flatness measurement equipment, it can only simply measure the ground flatness. For terrains with large slope changes, such equipment cannot pass well, so it cannot measure the slope of such ground. And for some equipment that can measure the ground slope, it can only identify the road surface slope with low accuracy and cannot well identify the fine road surface changes. Therefore, the present invention can not only identify the fine road surface changes, but also can better identify and model large-scale slope changes. And in the existing contact detection, the applications are usually single, and can only perform obstacle collision detection or only perform terrain modeling detection. Summary of the Invention

[0006] In view of this, the present invention provides a swing arm detection device, method, electronic device and storage medium to solve the above problems.

[0007] The present invention provides a swing arm detection device, which is characterized in that it comprises: a vehicle body, swing arms on both sides, a swing arm wheel, a motor system, an identification system, and a control system; the vehicle body is composed of a vehicle body, two main wheels and a detachable crawler assembly, wherein the main wheels serve as main supporting components, and the swing arm detection device can switch to a wheel mode and a crawler mode according to different environmental detection tasks; the swing arms on both sides can rotate 360 degrees and are respectively connected to the main wheels on both sides of the vehicle body, the motor system controls the rotation of the main wheels and the circumferential swing of the swing arms on both sides, the swing arm wheels are connected to the main wheels through the swing arms, and a sharp hook assembly is installed on the side of the swing arm close to the swing arm wheel, wherein, when the swing arm detection device is in a mode of detecting obstacles in front, the upper and lower swing arms are staggered with each other, divided into an upper swing arm and a lower swing arm, which are respectively used to detect upper obstacles and lower obstacles; the identification system is used to identify the type of obstacles, and the control system is used to give instructions according to obstacle identification results and detection results, and control the swing arm detection device to move forward, backward, turn and climb over.

[0008] In another implementation of the present invention, the motor system is controlled by a magnetic field oriented control algorithm; wherein, during the process of detecting obstacles ahead, the magnetic field oriented control algorithm determines whether the obstacle is passable by setting the motor vector control output torque, and through judgment logic such as PID torque and position angle loop control; during ground detection, the magnetic field oriented control algorithm is used to obtain geometric information such as the angle change of the motor and the distance traveled by the vehicle, so as to calculate the contour of the terrain to be passed.

[0009] In another aspect of the present invention, the above-mentioned swing arm detection device is used to execute a swing arm detection method, which includes: contacting an obstacle with the swing arm of the swing arm detection device to obtain a first angle value read by an encoder; the swing arm detection device continues to move forward, the swing arm angle continues to change, and a second angle value of the swing arm is read by the encoder; the angle deviation value between the first angle value and the second angle value is calculated by a controller; whether the angle deviation value exceeds an angle threshold is determined, and if it does not exceed the angle threshold, the swing arm detection device is instructed to pass directly; if it exceeds the angle threshold, whether the height of the obstacle exceeds the height threshold is determined; if the height of the obstacle exceeds the height threshold, the swing arm detection device is instructed to bypass, and if it does not exceed the height threshold, the swing arm detection device is instructed to climb over the obstacle.

[0010] In another implementation of the present invention, the swing arm detection method further includes: contacting the ground with the swing arm of the swing arm detection device to obtain an initial angle value, where the swing arm contacting the ground moves together with the swing arm detection device; reading the angle change value and the moving distance value during the movement of the swing arm through an encoder; calculating based on the initial angle value, the angle change value, and the moving distance value during the movement of the swing arm to obtain the ground height change information; and modeling the ground based on the ground height change information to obtain a two-dimensional terrain model.

[0011] In another implementation of the present invention, the calculating based on the initial angle value, the angle change value, and the moving distance value during the movement of the swing arm to obtain the ground height change information includes: calculating based on the geometric relationship among the initial angle value, the angle change value, and the moving distance value during the movement of the swing arm to obtain the absolute slope angle; judging whether the swing arm detection device can cross the slope based on the absolute slope angle and the maximum angle that the swing arm detection device can cross; if the swing arm detection device cannot cross the slope, marking the slope as an obstacle; if the swing arm detection device can cross the slope, calculating based on the relationship between the absolute slope angle and the moving distance value to obtain the ground height change information.

[0012] In another implementation of the present invention, the maximum angle that the swing arm detection device can cross is obtained by calculating the angle based on the geometric relationship among the swing arm length, the wheel radius of the swing arm detection device, and the radius of the swing arm wheel. When the angle is greater than this angle, the swing arm detection device can directly cross, and when the angle is less than this angle, the swing arm detection device cannot cross.

[0013] On the other hand, the present invention provides an electronic device, including: a memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the steps of the swing arm detection method described in any one of the above are implemented.

[0014] On the other hand, the present invention provides a computer storage medium, on which a computer program is stored. When the computer program is executed by a processor, the steps in the swing arm detection method described in any one of the above are implemented.

[0015] The present invention designs a detection method based on swing arms fixed on both sides of the vehicle body. The device is mainly configured by a vehicle and struts connected to both sides of the vehicle body, and there are also strut wheels at the top of the struts; the method adopted can, through contact detection only with the FOC motor, the swing arm, and its swing arm wheels, well identify and judge whether such obstacles can be directly passed through and perform terrain modeling, with simple implementation and low cost. Description of the Drawings

[0016] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following will briefly introduce the accompanying drawings required for the description of the embodiments or the prior art. By reading the detailed description of the following embodiments, the advantages and benefits in the solutions will become clear to those skilled in the art. The accompanying drawings are only for the purpose of showing the preferred embodiments and are not considered to be a limitation of the present invention. In the accompany

[0017] In the figures:

[0018] Figure 1 Schematic diagram of the swing arm detection device according to an embodiment of the present invention.

[0019] Figure 2 Block diagram of the field-oriented control according to an embodiment of the present invention.

[0020] Figure 3 Block diagram of the SVPWM control according to an embodiment of the present invention.

[0021] Figure 4 Schematic diagram of the SVPWM space voltage according to an embodiment of the present invention.

[0022] Figure 5 Schematic diagram of the triangular wave comparison according to an embodiment of the present invention.

[0023] Figure 6 Flow chart of the detection of the front detection object according to an embodiment of the present invention.

[0024] Figure 7 Schematic diagram of the swing arm detection device for detecting the front obstacle according to an embodiment of the present invention.

[0025] Figure 8 Schematic diagram of the obstacle being directly passable according to an embodiment of the present invention.

[0026] Figure 9 Schematic diagram of the swing arm detection device for ground detection in the wheel mode according to an embodiment of the present invention.

[0027] Figure 10 Schematic diagram of the geometric relationship between the main wheel and the swing arm wheel according to an embodiment of the present invention.

[0028] Figure 11 Schematic diagram of the geometric relationship between the uphill, downhill, swing arm detection device and the horizontal straight line according to an embodiment of the present invention.

[0029] Figure 12 Schematic diagram of the geometric relationship between the slope with one side being the horizontal plane, the swing arm detection device and the horizontal straight line according to an embodiment of the present invention.

[0030] Figure 13Schematic diagram of the slope, swing arm detection device and horizontal straight line geometric relationship with both sides being inclined planes in an embodiment of the present invention.

[0031] Figure 14 Schematic diagram of the slope, swing arm detection device and horizontal straight line geometric relationship when the main wheel and the swing arm wheel of the present invention are not in the same plane.

[0032] Figure 15 Schematic diagram of the curved surface slope, swing arm detection device and horizontal straight line geometric relationship in an embodiment of the present invention.

[0033] Figure 16 Flowchart of two-dimensional terrain modeling in an embodiment of the present invention. Detailed implementation manners

[0034] For ease of understanding, before describing the specific embodiments of the present invention in detail, an exemplary description is first given of the prior art of the swing arm detection device, method, electronic device and storage medium of the present invention.

[0035] In the prior art, the FOC annotation mentioned in the text: FOC (Field-Oriented Control) is a control method used for AC motor drives, especially widely applied in induction motors and permanent magnet synchronous motors. Its main goal is to analogize the control of an AC motor to that of a DC motor, so as to make it have better performance and efficiency. The basic idea of FOC is to transform the control of a three-phase AC motor system into two independent control loops: one is the magnetic field orientation axis (also called the d-axis), and the other is the rotor position axis (also called the q-axis). In this way, the controller can independently control the magnetic field orientation of the motor and the position of the rotor in the magnetic field, thus achieving more precise control.

[0036] The Clarke transformation annotation mentioned in the text: The Clarke Transformation is a mathematical method for converting three-phase AC voltages or currents between the αβ coordinate system (also called the stationary coordinate system) and the dq coordinate system (also called the rotating coordinate system). It is often used in the control of AC motors, especially in field-oriented control (FOC), in order to convert three-phase electrical quantities into components on the DC axis (d-axis) and the rotating axis (q-axis), thereby achieving simpler and more precise control.

[0037] The Park transformation annotation mentioned in the text: The Park Transformation, also known as the dq transformation or rotation transformation, is a mathematical method for converting three-phase AC voltages or currents from the αβ coordinate system (stationary coordinate system) to the dq coordinate system (rotating coordinate system) for applications such as field-oriented control (FOC) of AC motors.

[0038] SVPWM annotation mentioned in

[0039] To enable those skilled in the art to better understand the technical solutions in the embodiments of the present invention, the technical solutions in the embodiments of the present invention will be clearly and detailedly described below in conjunction with the accompanying drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art shall fall within the scope protected by the embodiments of the present invention.

[0040] Figure 1 A swing arm detection device provided for the embodiments of the present invention, as Figure 1 shown, this embodiment mainly includes:

[0041] Vehicle body, swing arms on both sides, swing arm wheels, motor system, recognition system, control system.

[0042] The vehicle body is composed of a body 103, two main wheels 101 and 102, and detachable track assemblies 104 and 105. Among them, the main wheels are the main supporting components, and the swing arm detection device can be switched to a wheel mode and a track mode for different environmental detection tasks.

[0043] The swing arms 201 and 202 on both sides can rotate 360 degrees and are respectively connected to the main wheels on both sides of the vehicle body. The main wheels are controlled by the motor system to rotate, and the swing arms on both sides swing circumferentially. The swing arm wheels 301 and 302 are connected to the main wheels through the swing arms. The swing arms are provided with hook assemblies 401 and 402 on the side close to the swing arm wheels. Among them, when the swing arm detection device is in the mode of detecting obstacles in front, the upper and lower swing arms are staggered from each other, and are divided into an upper swing arm and a lower swing arm, which are respectively used to detect obstacles above and below.

[0044] The recognition system is used to identify the type of obstacles, and the control system is used to give instructions according to the obstacle recognition result and the detection result, and control the swing arm detection device to move forward, backward, turn and climb over.

[0045] Exemplarily, accurate acquisition and control of the motor angle are required for the basis of realizing various functions. Therefore, a servo motor and its encoder are required in terms of hardware, and the FOC control algorithm is used to control the motor.

[0046] Specifically, as Figure 2The shown FOC control block diagram. When the system is given a speed input, the speed PI module generates IQ and ID signals. The IQ and ID signals are signals in the DQ coordinate system with the motor rotor magnet as the reference. The DQ coordinate system rotates with the rotor. The IQ controls the motor rotation, and the ID signal affects the motor heating. The IQ and ID signals are input to the PI module, and after current loop control, they output U q and U d voltage signals. The voltage information is transformed through the Park inverse transformation to convert the DQ signals in the rotating DQ coordinate system into U α and U β signals, where θ is the electrical angle, obtained from the encoder:

[0047] U α = U d cosθ - U q sinθ

[0048] U β = U q cosθ + U d sinθ

[0049] The U α and U β signals are input into the SVPWM module. Finally, this module generates PWM drive signals with a phase difference of 120° to the inverter (three-phase full-bridge circuit), and then drives the motor to rotate.

[0050] In PID control, often the target value remains unchanged and the actual value follows the target value. Therefore, in the FOC current closed-loop control, it is necessary to first transform the three-phase current sine signals generated by the full-bridge circuit through the equal-amplitude Clarke transformation to transform the three-phase signals into i α and i β signals:

[0051] i α = i a

[0052]

[0053] The I α and I β signals are transformed through the Park transformation to generate IQ and ID signals:

[0054] i d = i α cosθ + i β sinθ

[0055] i q = -i α sinθ + i β cosθ

[0056] Participate in the loop control of the current PI. Similarly, the encoder reads the rotation position (angle) and rotation speed of the motor, and inputs these signals into the position PID module and the speed PI module respectively, constituting the position PID loop control and the speed PI loop control.

[0057] Figure 3 It is the program control block diagram of SVPWM. Since there are eight combinations of control switches in the three-phase bridge, eight space voltage vectors can be generated, including two zero vectors and six non-zero vectors. As shown in the figure, V1-V6 are non-zero vectors, and V0 and V7 are zero vectors.

[0058] Such as Figure 4 The principle of SVPWM is to generate a PWM waveform that can make the motor rotate to any angle by using only these eight vectors. First, judge the sector of the synthesized voltage through the input U α 、U β to judge the sector of the synthesized voltage. The motor is divided into six sectors. Define the following functions:

[0059] V ref1 =u β

[0060]

[0061]

[0062] If

[0063] V ref1 >0, A = 1

[0064] V ref2 >0, B = 1

[0065] V ref3 >0, C = 1

[0066] Let

[0067] N = 4C + 2B + A

[0068] Then N can represent the sector where the synthesized reference voltage vector is located at this time, and its corresponding relationship is shown in the following table:

[0069] N 3 1 5 4 6 2 Sector Ⅰ Ⅱ Ⅲ Ⅳ Ⅴ Ⅵ

[0070] Table 1

[0071] Define the X, Y, Z parameters:

[0072]

[0073]

[0074]

[0075] T first ,T second denotes the first and second adjacent non - zero vector durations, and T0 (T7) denotes the zero - vector duration, according to the judgment in the following table:

[0076]

[0077]

[0078] Table 2

[0079] Define the following three time variables:

[0080]

[0081]

[0082]

[0083] Define T cm1 、T cm2 、T cm3 as the switching times of the a, b, and c phases of the inverter switch respectively, then their values can be looked up in the following table:

[0084] N 1 2 3 4 5 6 <![CDATA[T cm1 > <![CDATA[T b > <![CDATA[T a > <![CDATA[T a > <![CDATA[T c > <![CDATA[T c > <![CDATA[T b > <![CDATA[T cm2 > <![CDATA[T a > <![CDATA[T c > <![CDATA[T b > <![CDATA[T b > <![CDATA[T a > <![CDATA[T c > <![CDATA[T cm3 > <![CDATA[T c > <![CDATA[T b > <![CDATA[T c > <![CDATA[T a > <![CDATA[T b > <![CDATA[T a >

[0085] Table 3

[0086] Find T cm1 、T cm2 、T cm3 After the switching point, at this time, define an isosceles triangle aligned with time, such as Figure 5 , whose height d = T s / 2. Thus, the time and the value of the triangle can be directly compared. For example, after T cm1 time, at this time, the value of the triangle is T cm1 , so at this time a is conducting. After T cm2 , the value of the triangle is T cm2 , and at this time b is conducting. Therefore, the figure can be interpreted as follows: if the value of the triangular wave read is greater than T cm1 , then at this time a is conducting, otherwise it is off. The same applies to b and c. Therefore, by selecting a suitable triangular wave for comparison, the conduction of a, b, and c can be controlled.

[0087] It should be understood that with the above - mentioned FOC motor control algorithm and calculation formula, the motor can be controlled to rotate to any position.

[0088] In another implementation of the present invention, the motor system is controlled by a field-oriented control algorithm. During the detection of an obstacle in front, the field-oriented control algorithm determines whether the obstacle can be passed through by setting the output torque of the motor vector control and using judgment logics such as PID torque and position angle loop control. During the ground detection, geometric information such as the angular change of the motor and the distance traveled by the vehicle can be obtained through the field-oriented control algorithm, and the contour of the terrain passed through can be calculated.

[0089] On the other hand of the present invention, the above-mentioned swing arm detection device is used to execute a swing arm detection method, and the method includes: contacting an obstacle with the swing arm of the swing arm detection device to obtain a first angle value read by an encoder; continuing to move forward the swing arm detection device, and the swing arm angle continuously changes, and a second angle value of the swing arm is read by the encoder; calculating an angle deviation value between the first angle value and the second angle value by a controller; judging whether the angle deviation value exceeds an angle threshold. If it does not exceed the angle threshold, it is indicated that the swing arm detection device can pass directly; if it exceeds the angle threshold, it is judged whether the height of the obstacle exceeds a height threshold; if the height of the obstacle exceeds the height threshold, it is indicated that the swing arm detection device bypasses through, and if it does not exceed the height threshold, it is indicated that the swing arm detection device climbs over the obstacle.

[0090] Exemplarily, as Figure 6 shown, the detection of an obstacle in front mainly includes the following steps: the swing arm contacts an obstacle; the obstacle is identified; the height of the obstacle is calculated to judge whether to pass directly, climb over or bypass the obstacle; a decision is made.

[0091] Specifically, as shown in (a) of Figure 7 , the upper swing arm contacts an obstacle. At this time, the included angle with the horizontal line read by the motor encoder is θ1. At this time, the swing arm receives a force F perpendicular to the obstacle. The force F is decomposed into a force F2 pointing to the center of the swing arm circle and a tangential force F1 with the radius of the swing arm length as the circle. It is set that the torque given by the lower swing arm motor is less than F1. Under the action of the force, the upper swing arm swings counterclockwise until Figure 7 the state shown in (b) of

[0092] At this time, the angle read by the motor encoder is θ2 and is recognized by the detection system, then it is regarded that there is an obstacle above the swing arm detection device and it cannot move forward. Similarly, when the lower swing arm contacts the detection object, due to the action of the resistance, when the encoder reads that the angle of the lower swing arm changes from θ3 to θ4, it will be judged that there is an obstacle below. Figure 8As shown, when encountering certain obstacles such as flowers, plants, leaves, plastic bags, etc., the camera cannot distinguish whether the obstacle is an immovable rigid body or a soft obstacle that can be directly crossed. At this time, when the swing arm contacts such an obstacle, since the motor is set with a certain torque, such an obstacle does not have enough reaction force to block the swing arm, and the angle change read by the motor encoder does not exceed the predetermined threshold. Therefore, the detection system regards it as an obstacle that can be crossed at this time, and the swing arm detection device will directly cross this part of the obstacle, as shown in Figure 8 as shown in (b) of

[0093] It should be understood that the strut has a certain angle with the horizontal. Set the threshold values of the angle and torque. The algorithm for detecting the front obstacle is realized by contacting the obstacle and making the angle change reach the threshold through the contact. The front obstacle recognition algorithm solves the problem of difficult camera recognition. The algorithm of the present invention is simple, the mechanism is simple, and the cost is low.

[0094] In another implementation manner of the present invention, as shown in Figure 16 The swing arm detection method further includes: contacting the ground with the swing arm of the swing arm detection device to obtain an initial angle value, wherein the swing arm contacting the ground moves together with the swing arm detection device; reading the angle change value and the moving distance value during the movement of the swing arm through the encoder; calculating based on the initial angle value, the angle change value and the moving distance value during the movement of the swing arm to obtain the height change information of the ground; modeling the ground based on the height change information of the ground to obtain a two-dimensional terrain model.

[0095] Exemplarily, as shown in Figure 9 At this time, the swing arm detection device is in the wheel mode. The swing arm wheel of the swing arm detection device is in close contact with the ground. As the ground undulates, the swing arm also undulates, thereby forming an angle change. According to the change of the geometric information of the swing arm of the swing arm detection device, the height change information of the ground can be calculated in real time, so as to model the ground.

[0096] Preferably, calculate the maximum angle that the swing arm wheel of the swing arm detection device can measure. As shown in Figure 10 is the geometric relationship diagram of the crawler wheel and the swing arm wheel. O1 is the center of the crawler wheel, O2 is the center of the swing arm wheel, O1O2 is the length of the strut. The line segment B1E is tangent to the circle O1, the line segment B2E is tangent to the circle O2, DF is perpendicular to C1C2 and O1O2. When θ takes the maximum angle, it is the maximum angle that the swing arm detection device wheel and the strut wheel can cross when contacting the ground. If the angle is less than this angle, the strut will be stuck and the strut wheel will be suspended.

[0097] According to Figure 10 it can be known that:

[0098] ∠B1A1E = ∠B1EF = ∠B1O1O2 = θ1

[0099] ∠B2A2E = ∠B2EF = ∠B2O1O2 = θ2

[0100] As Figure 10 Let the length of C1E be x, then:

[0101] A1B1 = O1B1 - O1A1

[0102]

[0103]

[0104] x cosθ1sinθ1 - O1C1 cos 2 θ1 - O1B1 sinθ1 + O1C1 = 0

[0105] A2B2 cosθ2 = E2A2

[0106] (O2B2 - O2A2)cosθ2 = C1C2 - x - A2C2

[0107]

[0108] 0 = O2C2 cos 2 θ2 - (C1C2 - x)cosθ2sinθ2 + O2B2 sinθ2 + O2C2

[0109] Assume the radius of wheel O1 is 12 cm, the radius of circle O2 is 5 cm, the length of the strut is 40 cm, and the width of the strut is 50 cm, that is, O1C1 and O2C2 are 25 cm. After arrangement, we get:

[0110] cosθ1sinθ1 - 25cos 2 θ1 - 120sinθ1 + 25 = 0

[0111] 25cos 2 θ2 - (400 - x)cosθ2sinθ2 + 50sinθ2 - 25 = 0

[0112] It is required to find the maximum value of θ = θ1 + θ2, where the value range of x is set as 120 < x < 350, and the intervals of θ1 and θ2 are [0:2p i , and a numerical solution method is used to find this solution.

[0113] The process is as follows. Select a value of x:

[0114] 1. Find θ1 and θ2.

[0115] 2. Select a step size to iterate over the value range of x and repeat the above process.

[0116] 3. Select the approximate value, reduce the step size and iterate again to select a more accurate result.

[0117] Finally, the value of x is obtained (from the assumed value above, it is calculated to be 266.70, the maximum value of θ1 + θ2, θs, is approximately 147.73°, (2.5783)).

[0118] Let the maximum angle between the main wheel of the swing arm detection device and the swing arm wheel that can continuously contact the ground be θ. max , (from the calculated value of θ assumed above max The value range of is from 360° to 147.73°), so when measuring the road surface with an angle greater than θ between two planes max it brings difficulties to the measurement of ground information.

[0119] Preferably, the following analyzes the road surface with an angle less than θ max and how to measure the ground information.

[0120] Figure 11 In (a) of, the right road surface is horizontal and it is an uphill section. Figure 11 In (c) of, the right road surface is also horizontal and it is a downhill section. The slope angles of both of them with the horizontal are θ, and the value range of θ is [0:θ max ), when θ is in [0:Π), it is uphill, and when θ is in [Π:θ max ), it is downhill. The schematic diagrams are as shown in Figure 11 In (a) and (c) of. During the uphill (downhill) process of the swing arm detection device, it travels from state 1 to state 2 and moves a distance of Δd. The angle between the strut and the horizontal direction in state 1 is β, and the angle between the strut and the horizontal direction in state 2 is α. All the above parameters can be measured from the motor encoder. Extract the geometric relationships of the schematic diagrams in Figure 11 In (a) and (c) to obtain Figure 11 In (b) and (d) of. l1 is the straight line corresponding to the slope, l0 is the horizontal straight line, P3P2 = P1P4, which is the radius of the wheel of the tracked vehicle and is perpendicular to the line l1. P1P2 is the distance that the tracked vehicle moves, which is Δd. P3P5 = P4P6 is the length of the strut, and P5P7 = P6P8 is the length of the strut wheel.

[0121] As shown in Figure 12 , define P5 as the origin O of the x - y coordinate. Figure 12 The points in are represented by P j (x j , y j ). The line segments P7P8, P1P2, P3P4, P3P5, P4P6, P5P6 are respectively located on the straight lines l0, l1, l2, l3, l4, l5.

[0122] From the geometric relationship, we know that l1 and l2 are parallel, l5 is parallel to l0, and is a horizontal straight line. Therefore, the horizontal angle θ of the slope is equal to Figure 12 (a)∠P4P9P5( Figure 12 (b)2Π-∠P3P4P9), that is, if we know the slope of l2, we can know the angle of the slope.

[0123] Preferably, the angle between the slope and the horizontal line is obtained through the following steps.

[0124] Point P5 is the origin:

[0125] (x5,y5)=(0,0)

[0126] It is known that the angle between l3 and the x-axis is α. According to the angle and the P5 coordinate, the linear equation of l3 is:

[0127] y=tan(π-α)x

[0128] Point P3 is on line segment l3, and the distance between P3 and P5 is the length of the strut L. The coordinates of P3 are obtained:

[0129]

[0130] y3=tanαx3

[0131] There are two solutions to the coordinates of P3. Since point P3 is to the left of the origin, x3<0, so one of the solutions can be discarded.

[0132] It is known that the angle between l4 and the horizontal is β. According to the coordinates and angles of P6, the straight line equation of l4 is:

[0133] y-y6=tan(π-β)(x-x6)

[0134]

[0135] Since P6 is on the x-axis, y6=0. By combining the above equations, we can get the expression of P4(x4,y4) with respect to x6. x4 has two solutions. Using the condition that P4 is to the left of P6, we can discard one solution. The length of the P3P6 line segment is Δd, that is:

[0136]

[0137] Point P3 can be solved from the above, and point P4 is only related to x6, so it can be solved to get x6.

[0138] From the solved coordinates of P3 and P4, we can get the slope k of l2:

[0139]

[0140] The angle between the slope and the horizontal plane can be known from the slope k:

[0141] θ = tan -1 k

[0142] For slopes that are not horizontal, as shown in (a) and (b) of Figure 13 , the blue line represents the running trajectory of the swing arm detection device on a slope with an angle of θ. At this time, the angle between the slope and the horizontal plane is γ. The black pattern indicates that the blue pattern is rotated counterclockwise by γ degrees so that the right side of the slope coincides with the horizontal. The β angle is the angle between the swing arm and the horizontal at time t for the swing arm detection device, and the α angle is at time t + 1 for the swing arm detection device. Then, the angle between the swing arm and the horizontal after rotation can be obtained as follows:

[0143] α' = α ± γ

[0144] β' = β ± γ

[0145] θ = θ' ± γ

[0146] Substitute the obtained α', β', and γ into the calculation of the θ angle on the previous horizontal road surface, that is, the current slope. It should be noted that γ is the slope angle of the slope calculated within the time period [t:t - 1].

[0147] Preferably, if the ground where the wheels of the swing arm detection device and the strut wheels are located are not directly connected, as shown in Figure 14 , l1 is the contact surface for the movement of the caterpillar wheels, l0 is the connected ground, and l-1 is the ground contacted during the movement of the strut wheels. If it is necessary to calculate the slope of the l1 ground, it is necessary to rely on the previous modeling to calculate the position of the strut wheels and the slope of the plane passed by the strut wheels, and finally the θ angle as shown in (b) of Figure 14 can be calculated.

[0148] Preferably, for non-flat terrains, as shown in Figure 15 , the distance Δd traveled by the swing arm detection device, and Δd' is the straight line of the required fitted surface. If the driving step length is made as small as possible, as compared in (a) and (b) of Figure 15 , then Δd' can be approximately equal to Δd, and thus the calculation method for the flat road surface mentioned above can be applied.

[0149] The strut wheels contact the terrain. As the vehicle moves, the strut wheels also move accordingly. At the same time, the strut wheels closely adhere to the ground. As the ground profile undulates, the angle of the strut also changes accordingly. Based on the front and rear angle changes and the traveled distance values, an algorithm for two-dimensional terrain modeling can be developed. The terrain modeling algorithm can adapt to terrains with large slope changes.

[0150] The technology provided by the present invention can be used in farmland, construction site inspection, field rescue, detection, military anti-terrorism and other scenarios. In some environments, when encountering flowers, trees, plastic bags, cloth strips and other items, the camera or radar will identify them as obstacles and regard them as impassable, but in fact the tracked vehicle can directly pass over such obstacles; when detecting obstacles in front, the swing arm is in the same direction as the direction of movement, and by contacting the obstacle, the swing arm forms a certain angle with the horizontal, and is set to the FOC motor vector control output torque, and the judgment logic such as PID torque and position angle loop control is used to judge whether the obstacle can pass; in addition, by continuously contacting the swing arm wheel with the ground, while the vehicle is constantly moving, the geometric information such as the angle change of the FOC motor and the distance traveled by the vehicle can be opened, and the contour of the terrain passed can be calculated, thereby modeling the terrain.

[0151] The present invention is a detection method based on a swing arm design fixed on both sides of the vehicle body. The device is mainly configured as a vehicle and a support rod connected to both sides of the vehicle body, and the top of the support rod is also provided with a support rod wheel; the method adopted only relies on the FOC motor and the swing arm and its swing arm wheel, and through contact detection, it can well identify and judge whether such obstacles can be directly passed through and terrain modeling is carried out, which is simple to implement and low cost.

[0152] In another implementation of the present invention, the calculation based on the initial angle value, angle change value and moving distance value during the movement of the swing arm to obtain the height change information of the ground includes: calculating based on the geometric relationship between the initial angle value, angle change value and moving distance value during the movement of the swing arm to obtain the absolute angle of the slope; judging whether the swing arm detection device can climb over the slope based on the absolute angle of the slope and the maximum angle that the swing arm detection device can climb over; if the swing arm detection device cannot climb over the slope, marking the slope as an obstacle; if the swing arm detection device can climb over the slope, calculating according to the relationship between the absolute angle of the slope and the moving distance value to obtain the height change information of the ground.

[0153] In another implementation of the present invention, the maximum angle that the swing arm detection device can climb over is obtained by calculating the angle based on the geometric relationship between the swing arm length, the wheel radius of the swing arm detection device and the radius of the swing arm wheel, wherein when the angle is greater than the angle, the swing arm detection device can directly climb over, and when the angle is less than the angle, the swing arm detection device cannot climb over.

[0154] Another aspect of the present invention provides an electronic device, which may include: a processor, a memory, a communication bus, and a communication interface.

[0155] in:

[0156] The processor, the memory, and the communication interface communicate with each other via a communication bus.

[0157] The communication interface is used to communicate with other electronic devices or servers.

[0158] The processor is used to execute a program, and specifically can execute the steps of any one of the swing arm detection methods in the above embodiments.

[0159] Specifically, the program may include program code, and the program code includes computer operation instructions.

[0160] The processor may be a central processing unit (CPU), or a specific integrated circuit (ASIC) (Application Specific Integrated Circuit), or one or more integrated circuits configured to implement the embodiments of the present application. One or more processors included in the intelligent device may be of the same type of processor, such as one or more CPUs; or may be of different types of processors, such as one or more CPUs and one or more ASICs.

[0161] The memory is used to store the program. The memory may include high-speed RAM memory, and may also include non-volatile memory, such as at least one disk memory.

[0162] The program is specifically used to cause the processor to execute to implement the steps of any one of the swing arm detection methods described in the embodiments. For the specific implementation of each step in the program, reference may be made to the steps and corresponding descriptions in the units of any one of the swing arm detection methods described above, which will not be elaborated here. Those skilled in the art can clearly understand that for the convenience and conciseness of description, the specific working processes of the devices and modules described above can refer to the corresponding process descriptions in the foregoing method embodiments.

[0163] On the other hand, the present invention also provides a non-transitory computer-readable storage medium storing computer instructions, wherein the computer instructions are used to cause a computer to execute the methods of the embodiments of the present application.

[0164] The method according to an embodiment of the present invention can be implemented in hardware, firmware, or be implemented as software or computer code that can be stored in a recording medium (such as a CD ROM, RAM, floppy disk, hard disk, or magneto-optical disk), or be implemented as computer code that is originally stored in a remote recording medium or a non-transitory machine-readable medium and downloaded via a network and will be stored in a local recording medium, so that the method described herein can be stored in such software processing on a recording medium using a general-purpose computer, a dedicated processor, or programmable or dedicated hardware (such as an ASIC or FPGA). It can be understood that a computer, a processor, a microprocessor controller, or programmable hardware includes a storage component (such as a RAM, a ROM, a flash memory, etc.) that can store or receive software or computer code, and when the software or computer code is accessed and executed by the computer, the processor, or the hardware, the method described herein is implemented. In addition, when a general-purpose computer accesses the code for implementing the method shown herein, the execution of the code converts the general-purpose computer into a dedicated computer for executing the method shown herein.

[0165] So far, specific embodiments of the present invention have been described. Other embodiments are within the scope of the appended claims. In some cases, the actions recited in the claims can be performed in a different order and still achieve the desired result. Additionally, the processes depicted in the figures do not necessarily require the particular order or sequential order shown to achieve the desired result. In certain embodiments, multitasking and parallel processing may be advantageous.

[0166] It should be noted that all directional indications (such as up, down, left, right, back...) in the embodiments of the present invention are only used to explain the relative positional relationship and movement conditions between components in a specific posture (as shown in the drawings). If the specific posture changes, the directional indications will change accordingly.

[0167] In the description of the present invention, the terms "first" and "second" are only used for convenience in describing different components or names, and cannot be construed as indicating or implying an order relationship, relative importance, or implicitly indicating the quantity of the indicated technical features. Thus, the features defined with "first" and "second" may explicitly or implicitly include at least one of such features.

[0168] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by those of ordinary skill in the technical field to which the present invention belongs. The terms used in the description of the present invention herein are only for the purpose of describing specific embodiments and are not intended to limit the present invention.

[0169] It should be noted that although the specific embodiments of the present invention have been described in detail with reference to the accompanying drawings, it should not be construed as a limitation on the protection scope of the present invention. Within the scope described in the claims, various modifications and deformations that can be made by those skilled in the art without creative efforts still fall within the protection scope of the present invention.

[0170] The examples of the embodiments of the present invention are intended to briefly illustrate the technical features of the embodiments of the present invention, so that those skilled in the art can intuitively understand the technical features of the embodiments of the present invention, and do not serve as an improper limitation on the embodiments of the present invention.

[0171] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them; although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that they can still modify the technical solutions recorded in the foregoing embodiments, or perform equivalent replacements on some of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. An arm detection device, characterized in that, include: Vehicle body, swing arms on both sides, swing arm wheels, motor system, identification system, control system; The vehicle body consists of a vehicle body, two main wheels and a detachable track assembly, wherein the main wheels serve as the main supporting components, and the swing arm detection device can be switched between wheel mode and track mode for different environmental detection tasks; The swing arms on both sides can rotate 360 degrees and are respectively connected to the main wheels on both sides of the vehicle body. The motor system controls the rotation of the main wheels and the circumferential swing of the swing arms on both sides. The swing arm wheels are connected to the main wheels through the swing arms. The swing arms are equipped with a sharp hook assembly on the side close to the swing arm wheels. When the swing arm detection device is in the mode of detecting obstacles in front, the upper and lower swing arms are staggered and divided into upper swing arms and lower swing arms, which are used to detect obstacles above and below respectively. The recognition system is used to identify the type of obstacles, and the control system is used to give instructions based on the obstacle recognition results and detection results to control the swing arm detection device to move forward, backward, turn and climb over.

2. The device according to claim 1, characterized in that The motor system is controlled by a field oriented control algorithm; Among them, during the process of detecting obstacles ahead, the magnetic field oriented control algorithm determines whether the obstacle is passable by setting the motor vector control output torque and judging logic such as PID torque and position angle loop control; During ground detection, the contour of the terrain can be calculated by obtaining geometric information such as the angle change of the motor and the distance traveled by the vehicle through a magnetic field-oriented control algorithm.

3. The device according to claim 1, wherein The device is used to perform a swing arm detection method, the method comprising: The swing arm of the swing arm detection device contacts the obstacle, and obtains a first angle value read by the encoder; The swing arm detection device continues to move forward, the swing arm angle continues to change, and the second angle value of the swing arm is read through the encoder; Calculating, by a controller, an angle deviation value between a first angle value and a second angle value; Determine whether the angle deviation value exceeds the angle threshold, and if it does not exceed the angle threshold, instruct the swing arm detection device to pass directly; If the angle threshold is exceeded, it is determined whether the obstacle height exceeds the height threshold; If the height of the obstacle exceeds the height threshold, the swing arm detection device is instructed to bypass it. If the height does not exceed the height threshold, the swing arm detection device is instructed to climb over the obstacle.

4. The method according to claim 3, characterized in that Also includes: The initial angle value is obtained by the swing arm of the swing arm detection device touching the ground, wherein the swing arm touching the ground moves together with the swing arm detection device; The encoder is used to read the angle change and distance of the swing arm during movement. The height change information of the ground is obtained by calculating the initial angle value, angle change value and moving distance value during the swing arm movement process; The ground is modeled based on the height change information of the ground to obtain a two-dimensional terrain model.

5. The method according to claim 4, characterized in that, The calculation based on the initial angle value, angle change value and moving distance value during the swing arm movement process to obtain the height change information of the ground includes: The absolute angle of the slope is obtained by calculating the geometric relationship between the initial angle value, the angle change value and the distance value moved during the movement of the swing arm; Based on the absolute angle of the slope and the maximum angle that the swing arm detection device can climb over, determining whether the swing arm detection device can climb over the slope; If the swing arm detection device cannot climb over the slope, the slope is marked as an obstacle; If the swing arm detection device can climb over the slope, calculations are performed based on the relationship between the absolute angle of the slope and the distance value of movement to obtain the height change information of the ground.

6. The method according to claim 5, characterized in that, The maximum angle that the swing arm detection device can climb over is obtained by calculating the angle based on the geometric relationship among the swing arm length, the wheel radius of the swing arm detection device, and the radius of the swing arm wheel. Among them, when the angle is greater than this angle, the swing arm detection device can directly climb over, and when the angle is less than this angle, the swing arm detection device cannot climb over.

7. An electronic device, characterized in that, Including: A memory, a processor, and a computer program stored on the memory and executable on the processor. When the processor executes the computer program, the steps of the swing arm detection method described in any one of claims 3 to 6 are implemented.

8. A computer storage medium, characterized in that, A computer program is stored on the computer storage medium. When the computer program is executed by the processor, the steps in the swing arm detection method described in any one of claims 3 to 6 are implemented.

Images