Outdoor operation electric machine positioning system based on Beidou technology

Abstract

The invention relates to the technical field of electric power tool positioning, in particular to an outdoor operation electric power tool positioning system based on the Beidou technology. The method comprises the following steps: generating an expected driving track point sequence of a machine tool by analyzing a CAN bus signal of the machine tool and combining a kinematics equation set and a road surface attachment correction coefficient; a dynamic early warning distance is calculated by fusing the running speed of a machine tool and the relative radial speed of an obstacle, and collision risk prospective judgment based on a future motion relation and strong correlation among alarm prompt, a spatial position and an alarm source are realized by comparing an expected running track point sequence with an expected position of the obstacle. And the safety and the early warning accuracy of the electric machine tool in an outdoor complex operation environment are effectively improved.

Description

Technical Field

[0001] This invention relates to the field of power equipment positioning technology, specifically to an outdoor power equipment positioning system based on BeiDou technology. Background Technology

[0002] In outdoor operations within the power industry, the safe operation of large mobile equipment is crucial. When equipment approaches or accidentally touches live equipment, power poles, or other critical structures during operation, it can easily lead to serious safety accidents. Furthermore, in areas with relatively high pedestrian traffic, it can also pose a threat to pedestrian safety. Therefore, accurate and reliable real-time positioning and safety warnings for equipment are core elements in ensuring operational safety.

[0003] Existing technologies, such as Chinese invention patent publication number CN119247421A, disclose a real-time positioning and safety early warning method and system for power operation equipment. This method calculates the position and orientation of the equipment by deploying positioning terminals at its head and tail. However, the positioning parameters are relatively simple, and the system mainly relies on satellite positioning signals, which are easily affected by obstruction and interference in complex outdoor environments, leading to positioning point drift and affecting the reliability of subsequent risk assessment.

[0004] Furthermore, the aforementioned existing technologies do not take into account the real-time location and movement of obstacles around the equipment, making it difficult to perceive dynamic changes in the environment. Simultaneously, they also fail to consider the real-time operating status of the equipment itself, resulting in a lack of foresight in risk assessment and delayed warnings. Moreover, the generated alarm prompts lack a strong correlation with spatial location and alarm source, hindering operators from quickly locating risk sources and taking countermeasures. Summary of the Invention

[0005] The purpose of this invention is to overcome the shortcomings of the prior art and solve the problems of delayed early warning information and lack of strong correlation between alarm prompts and specific spatial locations and alarm sources during outdoor operation of power equipment, due to the single dimension and insufficient stability of positioning information and its failure to be combined with the real-time environment and equipment operation status.

[0006] The technical solution adopted by the present invention to solve its technical problem is: an outdoor power machinery positioning system based on Beidou technology, including: a fusion positioning module, used to obtain head coordinates, tail coordinates, three-axis acceleration and angular velocity according to the placement of the head and tail of the machinery into the BDS terminal and the centroid inertial measurement component, and output the three-dimensional coordinates and heading angle of the machinery.

[0007] The environmental perception module is used to output the three-dimensional coordinates of obstacles and their relative radial velocity based on the three-dimensional coordinates and heading angle of the equipment, combined with the point cloud data of the lidar on the top of the equipment.

[0008] The trajectory prediction module is used to analyze the CAN bus signal of the machine to obtain the steering wheel angle and wheel pulse frequency, calculate the current driving speed, and generate the expected driving trajectory point sequence by combining the heading angle.

[0009] The risk assessment module is used to determine the dynamic warning distance by integrating the current driving speed and the relative radial speed; calculate the Euclidean distance between each obstacle and each trajectory point to obtain a distance set; if the minimum value in the distance set is less than or equal to the dynamic warning distance, a positioning warning command is generated.

[0010] The positioning alarm module is used to send positioning warning commands to the alarm terminal of the equipment and upload them to the remote monitoring platform.

[0011] Compared with the prior art, the present invention has the following beneficial effects: 1. In addition to obtaining the BeiDou coordinates of the head and tail of the machine, the present invention also introduces the three-axis acceleration and angular velocity obtained by the centroid inertial measurement unit. Data fusion is performed by Kalman filtering, which effectively suppresses the cumulative error and heading angle drift of inertial navigation, so that the output three-dimensional coordinates and the corrected heading angle remain continuous and stable, ensuring the reliability of subsequent risk assessment.

[0012] 2. This invention collects point cloud data using a lidar on top of the machine, outputs the three-dimensional coordinates of obstacles after clustering, and calculates the radial velocity of obstacles relative to the machine based on continuous multi-frame point cloud data, thereby realizing the identification and tracking of static and dynamic obstacles in the work area.

[0013] 3. This invention generates a sequence of expected travel trajectory points for the equipment by analyzing the CAN bus signal of the equipment and combining the kinematic equations with the road surface adhesion correction coefficient. By fusing the equipment's travel speed with the relative radial velocity of the obstacle, the dynamic warning distance is calculated. By comparing the expected travel trajectory point sequence with the expected position of the obstacle, a forward-looking judgment of collision risk based on future motion relationships is achieved, as well as a strong correlation between alarm prompts, spatial location, and alarm source. This effectively improves the safety and warning accuracy of electric equipment in complex outdoor working environments. Attached Figure Description

[0014] To more clearly illustrate the technical solutions of the embodiments of the present invention, the accompanying drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0015] Figure 1 This is a schematic diagram of the system module connections of the present invention.

[0016] Figure 2 This is a schematic diagram illustrating the process of determining the relative radial velocity of an obstacle according to the present invention.

[0017] Figure 3 This is a schematic diagram illustrating the current driving speed of the computer in this invention.

[0018] Figure 4 This is a schematic diagram of the process for generating the expected driving trajectory point sequence according to the present invention. Detailed Implementation

[0019] Various exemplary embodiments of the present invention will now be described in detail with reference to the accompanying drawings. It should be noted that, unless otherwise specifically stated, the relative arrangement, numerical expressions, and values ​​of the components and steps set forth in these embodiments do not limit the scope of the invention. Furthermore, it should be understood that, for ease of description, the dimensions of the various parts shown in the drawings are not drawn to actual scale.

[0020] The following description of at least one exemplary embodiment is merely illustrative and is in no way intended to limit the invention or its application or use. Techniques, methods, and apparatus known to those skilled in the art may not be discussed in detail, but where appropriate, such techniques, methods, and apparatus should be considered part of the specification.

[0021] In all examples shown and discussed herein, any specific values ​​should be interpreted as merely exemplary and not as limitations. Therefore, other examples of exemplary embodiments may have different values.

[0022] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0023] The following description, in conjunction with the accompanying drawings, details a specific scheme for an outdoor power equipment positioning system based on BeiDou technology provided by this invention.

[0024] Please see Figure 1 The diagram shows a module connection diagram of an outdoor power equipment positioning system based on Beidou technology provided by the present invention, which specifically includes: a fusion positioning module, an environmental perception module, a trajectory prediction module, a risk assessment module, and a positioning alarm module.

[0025] The fusion positioning module is connected to the environmental perception module and the trajectory prediction module, respectively; both the environmental perception module and the trajectory prediction module are connected to the risk assessment module; and the risk assessment module is connected to the positioning alarm module.

[0026] The fusion positioning module is used to obtain the head coordinates, tail coordinates, three-axis acceleration and angular velocity based on the placement of the head and tail of the implement into the BDS terminal and the center of mass inertial measurement component, and outputs the implement's three-dimensional coordinates and heading angle.

[0027] In this invention, the BDS terminal refers to the BeiDou satellite navigation system terminal, which achieves absolute positioning by receiving BeiDou satellite signals and outputs longitude, latitude, and elevation coordinates.

[0028] Specifically, the process for determining the aircraft's three-dimensional coordinates and heading angle is as follows: First, initial positioning is performed using dual BDS terminals at the head and tail. The average value of the head coordinates and tail coordinates is calculated and used as the aircraft's initial position. Simultaneously, the azimuth angle of the vector pointing from the tail to the head in the horizontal plane is calculated and used as the initial heading angle.

[0029] The horizontal plane refers to the plane parallel to the east-north plane of the ENU coordinate system. The initial heading angle is the angle between the projection of the tail-to-head vector onto this plane and the true north direction.

[0030] However, in environments where satellite signals are blocked, such as tunnels and forested areas, the BDS terminal signal may lose lock or the signal-to-noise ratio may decrease, potentially causing positioning interruptions or reduced accuracy. Therefore, this invention also introduces an inertial measurement unit for assisted navigation, enabling continuous position and attitude calculation.

[0031] An inertial measurement unit (IMU) is mounted near the machine's center of mass to reduce attitude errors caused by non-rigid deformation of the machine. The IMU includes a three-axis accelerometer and a three-axis gyroscope to collect the machine's acceleration and angular velocity during motion.

[0032] After obtaining the initial position and initial heading angle, the aircraft attitude is updated using an attitude update algorithm based on the acquired three-axis acceleration and angular velocity. Aircraft attitude refers to the pitch angle, roll angle, and heading angle of the aircraft in three-dimensional space, which represent the rotation angles of the aircraft about its horizontal axis, longitudinal axis, and vertical axis, respectively.

[0033] This invention exemplarily employs a quaternion pose update algorithm, with the specific update formula as follows: .

[0034] in, This represents the sampling time number, with values ​​ranging from 0, 1, 2, ... ; This represents the total number of samples.

[0035] Sampling period ( ); Angular velocity vector measured by a three-axis gyroscope ( ); The magnitude of the angular velocity vector ( ); It is a unit vector in the direction of angular velocity.

[0036] Indicates the first The attitude quaternion at each sampling time; This represents the updated value obtained after integrating the angular velocity. The attitude quaternion at the sampling time.

[0037] The updated attitude quaternion can be used to obtain the corresponding Euler angles using the quaternion-Euler angle conversion formula. The specific conversion formula is as follows: .

[0038] in, For heading angle ( ); , , , These are the four components of the attitude quaternion.

[0039] Then, a rotation matrix is ​​constructed from the body coordinate system to the global navigation coordinate system using Euler angles. The three-axis accelerations in the body coordinate system are multiplied by the rotation matrix to obtain the accelerations in the navigation coordinate system.

[0040] Then, the acceleration due to gravity is subtracted from this acceleration to obtain the pure motion acceleration. The pure motion acceleration is then integrated to obtain the inertial velocity of the machine. Finally, the inertial position is obtained by integrating the inertial velocity.

[0041] Considering that inertial navigation inherently suffers from accumulated errors and heading angle drift, this invention addresses these issues by inputting the initial position, inertial position, inertial velocity, initial heading angle, and updated heading angle into a Kalman filter.

[0042] The Kalman filter is used as the optimal estimation algorithm here, with position, velocity, attitude quaternions, and the zero bias of the three-axis accelerometer and three-axis gyroscope in the global navigation coordinate system as state variables, and the head-to-tail coordinate difference position provided by the BDS terminal as the measurement input. The filter runs once every 100 milliseconds, first extrapolating the state based on the inertial measurement unit data, then calculating the Kalman gain using the residuals observed by the BDS terminal, and correcting the cumulative error of the inertial extrapolation and the heading angle drift error in real time.

[0043] Finally, the fusion positioning module outputs three-dimensional coordinates based on the machine's center of mass and a corrected heading angle. This provides a reliable data reference for the subsequent environmental perception module.

[0044] Please see Figure 2 The environmental perception module is used to output the three-dimensional coordinates of the obstacle and its relative radial velocity based on the three-dimensional coordinates and heading angle of the machine, combined with the point cloud data of the lidar on the top of the machine.

[0045] The lidar is mounted on top of the machine, and its scanning plane is parallel to the horizontal plane. In this invention, Hesai is exemplarily used. Its scanning frequency is 10 That is, the LiDAR scanning cycle is 0.1 seconds.

[0046] The process of determining the three-dimensional coordinates of the obstacle is as follows: First, a local coordinate system is established with the machine's center of mass as the origin and the heading direction indicated by the heading angle as the forward direction.

[0047] Next, the point cloud data acquired by the lidar is preprocessed by removing ground point clouds and statistical filtering for noise reduction. Then, Euclidean clustering algorithm is used for further cluster analysis.

[0048] The clustering analysis process is as follows: Based on the size characteristics of typical obstacles such as pedestrians and equipment in the point cloud, a neighborhood radius of 0.3 meters is set to effectively separate different obstacles. If the Euclidean distance between two points is less than 0.3 meters, they are considered the same obstacle. All points are traversed through a depth-first search to form several point cloud clusters, and each point cloud cluster corresponds to a potential obstacle.

[0049] For each point cloud cluster, each point needs to be transformed from the lidar coordinate system to the local coordinate system, and then to the global navigation coordinate system.

[0050] The lidar coordinate system is a right-handed coordinate system with the lidar's own installation position as the origin and a fixed orientation; the global navigation coordinate system adopts the ENU coordinate system, and the origin can be set as a fixed reference point in the work area, such as the center or starting point of the work area.

[0051] Subsequently, the three-dimensional geometric center of the point cloud cluster in the global navigation coordinate system is calculated, that is, the arithmetic mean of the coordinates of all points in the cluster is taken, and this is used as the three-dimensional coordinates of the corresponding obstacle.

[0052] Because static and dynamic obstacles pose different safety threats to machinery, three-dimensional coordinates alone are insufficient to assess dynamic risk; the obstacle's motion trend must also be obtained. Relative radial velocity reflects the relative motion trend between the obstacle and the machinery and is a key dynamic parameter for determining the likelihood of a collision.

[0053] Based on this, the present invention further obtains the relative radial velocity of the obstacle. The specific process is as follows: obtaining the three-dimensional coordinate sequence of each point cloud cluster in at least three consecutive lidar scanning cycles.

[0054] Then, the displacement vector between adjacent moments is calculated based on the three-dimensional coordinate sequence, and the ratio is calculated by combining the time interval to obtain the velocity vector of the obstacle.

[0055] To assess the proximity of the obstacle to the machine, the motion velocity vector is projected onto the direction of the line connecting the current position of the machine to the current position of the obstacle to obtain the first velocity component.

[0056] Simultaneously, the motion velocity vector of the machine is calculated based on the changes in its three-dimensional coordinates at adjacent moments, and projected onto the same connecting line direction to obtain the second velocity component.

[0057] Finally, the difference between the first and second velocity components is taken as the relative radial velocity of the obstacle relative to the machine. A negative relative radial velocity indicates that the obstacle and the machine are approaching each other; a positive relative radial velocity indicates that the obstacle and the machine are moving away from each other or are relatively stationary.

[0058] If a valid point cloud cluster of the same obstacle is not acquired within three consecutive scan cycles, the obstacle is determined to have left the monitoring range or is a false detection. In this case, its relative radial velocity is set to zero, and the extrapolation time window for its expected position is extended to a maximum of six scan cycles. If there are no updates thereafter, it is removed from the obstacle list.

[0059] While relative radial velocity reflects the proximity of the obstacle to the machine, accurate assessment of collision risk requires prediction of the machine's future trajectory. Therefore, this invention introduces a sequence of expected travel trajectory points.

[0060] The trajectory prediction module is used to parse the CAN bus signal of the machine to obtain the steering wheel angle and wheel pulse frequency, calculate the current driving speed accordingly, and generate the expected driving trajectory point sequence by combining the heading angle.

[0061] Among them, the CAN bus is a commonly used serial communication bus in industrial vehicles, used to transmit real-time data between sensors and controllers.

[0062] The specific parsing process is as follows: connect to the CAN bus of the machine, listen to and read the data frames on it in real time; according to the preset CAN protocol, parse the steering wheel angle sensor value from the data frame as the steering wheel angle.

[0063] Simultaneously, the wheel speed pulse count value of each wheel is analyzed, and the real-time pulse frequency of each wheel is calculated based on the difference between two consecutive count values ​​and the time interval.

[0064] The preset CAN protocol refers to the CAN protocol provided by the equipment manufacturer, which defines the position, length, and physical value conversion rules of key signals such as steering wheel angle and wheel speed pulse in the data frame.

[0065] For example, for In the data frame, bytes 3-4 represent the pulse count of the left front wheel, and bytes 5-6 represent the steering wheel angle.

[0066] While acquiring the steering wheel angle and wheel pulse frequency, the number of pulses and the distance traveled per wheel rotation are extracted from the wheel speed sensor parameters; the distance traveled is the wheel's rolling circumference. Then, the pulse frequency of each wheel is divided by the corresponding number of pulses, and multiplied by the distance traveled to obtain the linear velocity of each wheel.

[0067] Please see Figure 3 Next, the machine is determined to be either going straight or turning based on the steering wheel angle, and its current speed is calculated accordingly.

[0068] Before making a judgment, a steering threshold needs to be set. For example, the steering threshold is set to 0.087. If the machine is traveling on narrow bends or in high-precision working areas, the steering threshold can be appropriately reduced to 0.052. To improve the sensitivity of steering state recognition; in wide straight roads or high-speed driving scenarios, the steering threshold can be appropriately increased to 0.140. This avoids misinterpreting slight steering wheel movements as steering.

[0069] The specific judgment and calculation process is as follows: when the absolute value of the steering wheel angle is less than or equal to the preset steering threshold, the machine is determined to be in a straight-line state. At this time, the difference in speed between the left and right wheels is mainly caused by uneven road surface or slippage. The average value of the linear speed of each wheel is taken as the longitudinal driving speed, which can suppress noise. The lateral driving speed can be ignored and is set to zero in this invention.

[0070] When the absolute value of the steering wheel angle exceeds a preset steering threshold, the machine is determined to be in a steering state. Because the wheel trajectory is an arc during steering, the difference in wheel speed between the left and right sides is significant. If the average is still taken, the actual speed will be seriously underestimated. Therefore, a system of kinematic equations is established based on the linear velocity of the wheels, the steering wheel angle, the wheelbase of the axle, and the distance from the center of gravity to the rear axle, and the longitudinal and lateral travel speeds are obtained by solving the equations.

[0071] The kinematic equations are as follows: .

[0072] in, Longitudinal travel speed ( ); Lateral travel speed ( ); The average linear velocity of the front wheel ( It is obtained by taking the arithmetic mean of the linear velocities of the left and right front wheels; The average linear velocity of the rear wheel ( The value is obtained by taking the arithmetic mean of the linear velocities of the left and right rear wheels.

[0073] The centroid sideslip angle ( ), ; Steering wheel angle ( ); wheelbase ( ), The distance from the center of mass to the rear axle ( Both are inherent structural parameters of the equipment and can be obtained from the vehicle's technical manual.

[0074] Finally, the longitudinal and lateral travel speeds are vector-synthesized to obtain the machine's current travel speed.

[0075] Please see Figure 4 Furthermore, after obtaining the current driving speed, the trajectory prediction module will generate a sequence of expected driving trajectory points based on the current driving speed and heading angle.

[0076] The specific generation process is as follows: the current three-dimensional coordinates, heading angle, and travel speed of the equipment are used as the initial state. At the same time, the prediction duration is divided into multiple consecutive time intervals with a fixed time step.

[0077] The fixed time step is 0.1 seconds, synchronized with the LiDAR scanning frequency. The prediction duration can be set to 3 seconds for example; however, if the machine's travel speed is high, such as greater than 5... The prediction time can be appropriately extended to 4 seconds to cover a longer braking distance; if the work area has dense obstacles or low visibility, the prediction time can be appropriately shortened to 2 seconds to improve the real-time performance of the warning.

[0078] Then, a recursive calculation is performed for each time interval: the current travel speed is multiplied by the time step to obtain the predicted distance the machine will travel in the direction indicated by the current heading angle. Then, starting from the current three-dimensional coordinates, the predicted distance is moved forward in the direction indicated by the current heading angle to obtain the predicted position for the next time interval.

[0079] After generating the predicted position, it is also necessary to determine whether the equipment is in a turning state in order to update the heading angle and avoid the predicted trajectory deviating significantly from the actual path.

[0080] Therefore, if the equipment is traveling straight, its heading is stable, so the heading angle remains unchanged and no update is needed. However, if the equipment is turning, its heading is constantly changing, and ignoring this change will cause the predicted trajectory to deviate significantly from the actual path.

[0081] Therefore, it is necessary to calculate the change in heading angle based on the steering wheel angle, longitudinal driving speed, and wheelbase, and then superimpose it with the current heading angle to obtain the heading angle for the next time interval.

[0082] The formula for calculating the change in heading angle is as follows: .

[0083] in, The change in heading angle ( ); Longitudinal travel speed ( ); wheelbase ( ); For time step ( ); Steering wheel angle ( ); Steering ratio is the ratio of the steering wheel angle to the actual steering angle of the front wheels, and it can be obtained from the technical parameters provided by the vehicle manufacturer. Indicates the front wheel steering angle ( ), which is the actual front wheel deflection angle.

[0084] It should be noted that only longitudinal speed is used in the calculation of heading angle change because, in vehicle kinematics, lateral speed contributes little to the heading angle change and its influence can usually be ignored.

[0085] Finally, the coordinates of each predicted location are arranged in chronological order to form a sequence of the machine's expected travel trajectory points.

[0086] Considering that on road surfaces with low coefficients of friction, such as wet, sandy, gravel, or icy surfaces,

[0087] Slippage is prone to occur when machinery turns. This invention introduces a road surface adhesion correction coefficient to correct the change in heading angle, which can reduce the deviation of trajectory prediction on low-adhesion road surfaces and improve the consistency between the predicted trajectory and the actual path.

[0088] The specific correction process is as follows: divide the difference in linear velocity between the left and right wheels by the wheel track of the implement to obtain the actual yaw rate of the implement.

[0089] At the same time, according to the steering wheel angle With longitudinal driving speed Combined with wheelbase With steering ratio The theoretical yaw rate of the computer tool The unit is .

[0090] The specific calculation formula is as follows: .

[0091] Then, the ratio of the actual yaw rate to the theoretical yaw rate is used as the road surface adhesion correction factor. The road surface adhesion correction factor characterizes the degree of influence of the current road surface adhesion conditions on the steering response of the implement.

[0092] On surfaces with a low coefficient of friction, the actual yaw rate of the equipment will be less than the theoretical yaw rate, resulting in a road adhesion correction factor of less than 1. On ideal surfaces, the road adhesion correction factor is close to 1. Excessively high adhesion surfaces, such as those with extremely strong grip, can lead to a factor greater than 1, but such surfaces are rare in practice. Therefore, the road adhesion correction factor is limited to... Within the specified range, avoid excessively widening the bend.

[0093] Finally, the road surface adhesion correction coefficient is multiplied by the change in heading angle to obtain the corrected change in heading angle, which more accurately reflects the actual trend of heading change of the equipment on the road surface with a low friction coefficient.

[0094] It should be noted that in typical outdoor operating scenarios for power equipment, obstacles such as live poles, fixed equipment, opposing moving equipment, and vehicles are usually located in front of or along the expected trajectory of the equipment. Their relative motion with the equipment is mainly manifested as an approach along the direction of the equipment's flight, with a significant negative relative radial velocity.

[0095] Purely lateral obstacle crossings are rare in regulated operating areas and are generally controlled through warning zones and manual observation, with lateral risks being far lower than radial risks. Introducing complete three-dimensional relative velocity vector analysis would significantly increase the computational complexity of point cloud processing, trajectory prediction, and risk assessment, making it difficult to meet the requirements for real-time and reliable early warning.

[0096] Therefore, this invention focuses on the core dynamic parameter of relative radial velocity and determines the dynamic warning distance based on it.

[0097] The risk determination module is used to determine the dynamic warning distance by integrating the current driving speed and the relative radial speed; calculate the Euclidean distance between each obstacle and each trajectory point to obtain a distance set; if the minimum value in the distance set is less than or equal to the dynamic warning distance, a positioning warning command is generated.

[0098] The process of determining the dynamic warning distance is as follows: if the relative radial velocity is negative, it means that the obstacle and the machine are getting close to each other, and there is a risk of collision.

[0099] Based on this, the absolute value of the current driving speed and the absolute value of the relative radial speed are added together as a scalar to obtain the comprehensive approach speed.

[0100] Then, the overall approach rate is multiplied by the preset safe reaction time, and then the preset safety margin is added to obtain the dynamic warning distance.

[0101] In this invention, the preset safety reaction time is 1.5 seconds, which covers the time from the issuance of the warning to the driver's effective braking. Of course, it can also be adjusted according to the driver's average reaction speed or the complexity of the work area; for example, when the machine is in automatic driving mode, the preset safety reaction time can be shortened to 1.0 second; if there are relatively many pedestrians in the work area, the preset safety reaction time is extended to 2.0 seconds regardless of the driving mode, in order to improve safety.

[0102] The default safety margin is set at 1.0 meter. However, if the obstacle is a rigid wall, which is incompressible and has serious consequences from a collision, the default safety margin should be increased to 1.5 meters; if the obstacle is flexible vegetation, which is compressible and poses a lower risk, the default safety margin can be reduced to 0.5 meters.

[0103] If the relative radial velocity is positive, it means that the obstacle and the machine are far apart or relatively stationary. In this case, the risk of collision is extremely low, and the dynamic warning distance calculation is not triggered, thus avoiding unnecessary interference alarms.

[0104] After calculating the dynamic warning distance, a further specific risk assessment is conducted. First, for each trajectory point in the expected driving trajectory point sequence, the corresponding future time is determined based on their arrangement order and time step.

[0105] Then, based on the current three-dimensional coordinates and relative radial velocity of each obstacle, its expected position at each future moment is calculated.

[0106] The specific calculation formula is as follows: .

[0107] in, This represents the time offset of the future moment relative to the present. Its value range is related to the prediction duration; for example, if the prediction duration is 3 seconds, then the value range is... ; This represents the index number in the sequence of trajectory points, with values ​​ranging from 0, 1, 2, ... ; This represents the total number of trajectory points.

[0108] For obstacles in future moments The expected location; This represents the current position of the obstacle; all positions are represented by three-dimensional coordinate vectors, which can be directly substituted into the formula for calculation.

[0109] Relative radial velocity ( ); This is the unit direction vector of the machine pointing towards the obstacle, which is obtained by normalizing the coordinate difference between the current position of the machine and the current position of the obstacle.

[0110] Then, the Euclidean distance between each trajectory point and the expected position of each obstacle is calculated to form a distance set.

[0111] If the minimum value in the distance set is less than or equal to the dynamic warning distance, it indicates that there is a point on the expected driving trajectory whose distance to the expected position of the obstacle is below the safety boundary, and there is a risk of collision.

[0112] Therefore, the risk assessment module generates a location warning command. This command is not simply an alarm; it includes at least the identifier of the obstacle that triggered the warning, its three-dimensional coordinates, relative radial velocity, minimum Euclidean distance, dynamic warning distance, and estimated collision time.

[0113] The estimated collision time is derived from the minimum Euclidean distance and the relative radial velocity. If the relative radial velocity is negative, the estimated collision time is equal to the absolute value of the minimum Euclidean distance divided by the relative radial velocity; if the relative radial velocity is greater than or equal to zero, the estimated collision time is set to infinity or marked as no collision risk.

[0114] The markers are used to distinguish multiple obstacles; the location of obstacles can be displayed graphically on the alarm terminal using three-dimensional coordinates, making it easy for operators to perceive them intuitively.

[0115] The magnitude of the negative value of the relative radial velocity directly reflects the approach rate. The more negative the value, the higher the urgency. It can be used for sound or color-coded alarms.

[0116] The minimum Euclidean distance and dynamic warning distance are also used for graded alarms. For example, when the minimum Euclidean distance is greater than the dynamic warning distance, there is no alarm; when the minimum Euclidean distance is less than 80% of the dynamic warning distance, a red level 1 alarm is triggered; when the minimum Euclidean distance is between 80% and 100% of the dynamic warning distance, a yellow level 2 alarm is triggered.

[0117] The estimated collision time provides operators with a clear response window, allowing them to assess remaining reaction time and take appropriate action. Both tiered alarms and estimated collision times are highlighted on the alarm terminal.

[0118] Finally, the positioning alarm module sends positioning warning commands to the equipment's alarm terminals, such as the vehicle-mounted display screen and buzzer. Simultaneously, the positioning warning commands are uploaded to the remote monitoring platform. This completes a closed loop from risk perception to warning triggering, effectively ensuring the operational safety of power equipment in complex outdoor environments.

[0119] The above embodiments can be implemented, in whole or in part, by software, hardware, firmware, or any other combination thereof. When implemented using software, the above embodiments can be implemented, in whole or in part, in the form of a computer program product.

[0120] Those skilled in the art will recognize that the modules and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0121] In addition, the functional modules in the various embodiments of the present invention can be integrated into one processing module, or each module can exist physically separately, or two or more modules can be integrated into one module.

[0122] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

[0123] Finally, the above description is only a preferred embodiment of the present invention and is not intended to limit the present invention. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of the present invention should be included within the protection scope of the present invention.

Claims

1. An outdoor work power implement positioning system based on Beidou technology, characterized in that, The application relates to a positioning and warning system for agricultural machinery, comprising: a fusion positioning module for obtaining head coordinates, tail coordinates, three-axis acceleration and angular velocity from a BDS terminal at the head and tail of the agricultural machinery and a centroid inertial measurement assembly, and outputting three-dimensional coordinates and a heading angle of the agricultural machinery; an environment perception module for outputting three-dimensional coordinates and a relative radial velocity of an obstacle based on the three-dimensional coordinates and the heading angle of the agricultural machinery and combining point cloud data of a laser radar on the top of the agricultural machinery; a trajectory prediction module for analyzing CAN bus signals of the agricultural machinery to obtain a steering wheel turning angle and a wheel pulse frequency, calculating a current driving speed based on the steering wheel turning angle and the wheel pulse frequency, and generating an expected driving trajectory point sequence based on the heading angle; a risk judgment module for determining a dynamic early warning distance based on the current driving speed and the relative radial velocity; calculating Euclidean distances between each obstacle and each trajectory point to obtain a distance set; generating a positioning early warning instruction if a minimum value in the distance set is less than or equal to the dynamic early warning distance; a positioning warning module for issuing the positioning early warning instruction to a warning terminal of the agricultural machinery and uploading the positioning early warning instruction to a remote monitoring platform.

2. The outdoor working power implement positioning system based on Beidou technology according to claim 1, characterized in that, The determination process of the three-dimensional coordinates and the heading angle of the agricultural machinery comprises the following steps: calculating an average value of the head coordinates and the tail coordinates as an initial position; calculating an azimuth angle of a vector from the tail to the head in a horizontal plane as an initial heading angle; updating a body posture based on the three-axis acceleration and the angular velocity through a posture updating algorithm based on the initial position and the initial heading angle, and converting three-axis acceleration in a body coordinate system into a navigation coordinate system for integration to obtain an inertial position, an inertial velocity and an updated heading angle of the agricultural machinery; inputting the initial position, the inertial position, the inertial velocity, the initial heading angle and the updated heading angle into a Kalman filter to correct cumulative errors of inertial calculation and heading angle drift errors in real time, and outputting three-dimensional coordinates and a corrected heading angle with the centroid of the agricultural machinery as a reference.

3. The outdoor working power machine positioning system based on Beidou technology according to claim 1, characterized in that, The determination process of the three-dimensional coordinates of the obstacle comprises the following steps: establishing a local coordinate system with the centroid of the agricultural machinery as an origin and a heading direction indicated by the heading angle as a forward direction; preprocessing and clustering analysis are performed on the point cloud data of the laser radar to segment point cloud clusters corresponding to different obstacles; converting each point in the point cloud cluster from a laser radar coordinate system to a global navigation coordinate system through the local coordinate system; calculating a three-dimensional geometric center of the point cloud cluster in the global navigation coordinate system as the three-dimensional coordinates of the corresponding obstacle.

4. The outdoor working power machine positioning system based on Beidou technology according to claim 1, characterized in that, The determination process of the relative radial velocity of the obstacle comprises the following steps: obtaining a three-dimensional coordinate sequence of each point cloud cluster in a plurality of continuous laser radar scanning periods; calculating displacement vectors between adjacent time instants based on the three-dimensional coordinate sequence, and obtaining a motion velocity vector of the obstacle based on a time interval; projecting the motion velocity vector onto a line direction from the current position of the agricultural machinery to the current position of the obstacle to obtain a first velocity component; calculating a motion velocity vector of the agricultural machinery based on a three-dimensional coordinate change of the agricultural machinery at adjacent time instants, and projecting the motion velocity vector onto the same line direction to obtain a second velocity component; taking a difference between the first velocity component and the second velocity component as the relative radial velocity of the obstacle relative to the agricultural machinery.

5. The outdoor working power machine positioning system based on Beidou technology according to claim 1, characterized in that, The analysis of the CAN bus signals of the agricultural machinery to obtain the steering wheel turning angle and the wheel pulse frequency comprises the following steps: According to the preset CAN protocol, a steering wheel rotation angle sensor value is parsed from the CAN bus data frame as a steering wheel rotation angle; meanwhile, wheel speed pulse count values of each wheel are parsed, and real-time pulse frequencies of each wheel are calculated according to the difference between the count values of two continuous readings and the time interval.

6. The outdoor working power machine positioning system based on Beidou technology according to claim 1, characterized in that, The calculation process of the current driving speed is as follows: According to the wheel speed sensor parameters, the number of pulses corresponding to each rotation of the wheel and the distance traveled by the wheel are determined; The pulse frequency of each wheel is divided by the corresponding number of pulses, and then multiplied by the distance traveled to obtain the linear speed; When the absolute value of the steering wheel rotation angle is less than or equal to a preset steering threshold, it is determined that the machine is in a straight driving state, and the average value of the linear speeds of the wheels is taken as the longitudinal driving speed, and the lateral driving speed is set to zero; Otherwise, it is determined that the machine is in a steering state, and a kinematics equation set is established based on the linear speeds of the wheels, the steering wheel rotation angle, the wheelbase of the axle, and the distance from the center of mass to the rear axle, and the longitudinal driving speed and the lateral driving speed are obtained by solving the equation set; The longitudinal driving speed and the lateral driving speed are combined to obtain the current driving speed of the machine.

7. The outdoor working power machine positioning system based on Beidou technology according to claim 1, characterized in that, The generation process of the expected driving trajectory point sequence is as follows: The current three-dimensional coordinates, the heading angle, and the driving speed of the machine are taken as the initial state; a fixed time step is used to divide the prediction time into multiple continuous time intervals; Recursive calculation is performed for each time interval: if the machine is in a straight driving state, the heading angle is kept unchanged; if the machine is in a steering state, the change amount of the heading angle is calculated based on the steering wheel rotation angle, the longitudinal driving speed, and the wheelbase, and the change amount is added to the current heading angle to obtain the heading angle of the next time interval; The current driving speed is multiplied by the time step to obtain the predicted distance of the machine moving forward along the direction indicated by the current heading angle; The predicted distance is added to the current three-dimensional coordinates to obtain the predicted position of the next time interval along the direction indicated by the current heading angle; The coordinates of each predicted position are arranged in time sequence to form the expected driving trajectory point sequence of the machine.

8. The outdoor working power implement positioning system based on Beidou technology according to claim 7, characterized in that, The trajectory prediction module further includes a correction of the change amount of the heading angle, specifically: The difference between the linear speeds of the left and right wheels is divided by the wheelbase of the machine to obtain the actual yaw rate; The theoretical yaw rate is calculated based on the steering wheel rotation angle and the longitudinal driving speed, combined with the wheelbase and the steering transmission ratio; The ratio of the actual yaw rate to the theoretical yaw rate is taken as the road adhesion correction coefficient, and the corrected change amount of the heading angle is obtained by multiplying the correction coefficient by the change amount of the heading angle.

9. The outdoor working power tool positioning system based on Beidou technology according to claim 6, characterized in that, The determination process of the dynamic warning distance is as follows: If the relative radial velocity is negative, indicating that the obstacle and the machine are approaching each other, the absolute values of the current driving speed and the relative radial velocity are added to obtain the comprehensive approach rate; The comprehensive approach rate is multiplied by a preset safety reaction time, and a preset safety margin is added to obtain the dynamic warning distance; If the relative radial velocity is positive, indicating that the obstacle and the machine are moving away from each other or are relatively stationary, the dynamic warning distance calculation is not triggered.

10. The outdoor working power tool positioning system based on Beidou technology according to claim 9, characterized in that, The generation process of the positioning warning instruction is as follows: For each trajectory point in the expected driving trajectory point sequence, the corresponding future time is determined according to the arrangement order and the time step; According to the current three-dimensional coordinates and the relative radial velocities of the obstacles, the expected positions of the obstacles at each future time are calculated; Euclidean distances between each trajectory point and the expected positions of the obstacles are calculated to form a distance set; If the minimum value in the distance set is less than or equal to the dynamic warning distance, a positioning warning instruction is generated; The positioning warning instruction at least includes the identification, the three-dimensional coordinates, the relative radial velocity, the minimum Euclidean distance and the dynamic warning distance of the obstacle triggering the warning, and the predicted collision time calculated based on the minimum Euclidean distance and the relative radial velocity.

Images