Safety Behavior Detection Method and System for Ground Unmanned Equipment's Shooting During Autonomous Movement

Abstract

The present invention relates to a safety behavior detection method and system for a ground unmanned equipment to shoot during autonomous movement, belonging to the technical field of vehicle safety control, and solves the problem of the lack of safety detection for shooting during autonomous movement. It includes: according to the variable-scale envelope of the composite attitude of each trajectory point, setting the first mark of the trajectory point corresponding to the variable-scale envelope with the number of existing environmental point clouds less than or equal to the threshold as safe, otherwise setting it as risky; for the risky trajectory points, obtaining the bounding boxes of each sub-component of the ground unmanned equipment, if the number of environmental point clouds existing in any bounding box is greater than the threshold, modifying the first mark to dangerous; detecting whether the composite attitude of the trajectory points meeting the shooting conditions satisfies the recoil stillness condition and the recoil stability condition from the attitude and steering dimensions, if both are satisfied, setting the second mark as safe, otherwise setting it as dangerous; obtaining the safety behavior detection result according to the first mark and the second mark. The safety detection for shooting during movement is realized.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle safety control, and particularly to a method and system for detecting safety behaviors of a ground unmanned equipment during autonomous moving shooting. Background Art

[0002] In the modern combat environment with complex situations and ever-changing battle scenes, as a new type of combat equipment, military ground unmanned equipment can undertake more risky tasks such as reconnaissance, patrol, target locking, and fire strike compared with human soldiers, and has higher combat efficiency and safety.

[0003] In the fire strike task, the autonomous moving shooting of ground unmanned equipment can improve its combat efficiency and flexibility, which is a hot research direction of ground unmanned equipment. Most of the existing safety behavior detections for autonomous moving shooting are in the research and testing stage.

[0004] In practical applications, the autonomous moving shooting of ground unmanned equipment needs to be implemented in various complex environments. Unreasonable task load scheduling and the choice of shooting moments not only affect shooting accuracy, but also may pose great challenges to the safety of its own driving. Summary of the Invention

[0005] In view of the above analysis, embodiments of the present invention aim to provide a method and system for detecting safety behaviors of a ground unmanned equipment during autonomous moving shooting, so as to solve the problem that there is a lack of safety detection for the autonomous moving shooting of ground unmanned equipment.

[0006] On the one hand, embodiments of the present invention provide a method for detecting safety behaviors of a ground unmanned equipment during autonomous moving shooting, including the following steps:

[0007] Obtain the composite attitude of the ground unmanned equipment at each trajectory point according to the planned trajectory sequence and the expected scheduling sequence;

[0008] According to the variable-scale envelope under the composite attitude of each trajectory point, set the first mark of the trajectory point corresponding to the variable-scale envelope with the number of environmental point clouds less than or equal to the threshold as safe, otherwise set it as risky; for the risky trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment, and if the number of environmental point clouds in any bounding box is greater than the threshold, modify the first mark to dangerous;

[0009] Based on the expected scheduling sequence, detect whether the composite attitude of the trajectory points meeting the shooting conditions satisfies the recoil stillness condition and the recoil stability condition from the attitude and steering dimensions. If both conditions are satisfied, set the second mark of the trajectory point as safe, otherwise set it as dangerous;

[0010] Obtain the safety behavior detection results of each trajectory point according to the first mark and the second mark.

[0011] Based on further improvements to the above method, according to the planned trajectory sequence and the expected scheduling sequence, obtain the composite pose of the ground unmanned equipment at each trajectory point, including:

[0012] Based on the ground model fitted from the ground point cloud, calculate the pitch angle and roll angle of each trajectory point at each moment according to the position and orientation angle of the trajectory points in the planned trajectory sequence, and obtain the chassis pose;

[0013] According to the expected scheduling sequence, obtain the state of the mission payload at each moment, including the slewing angle of the turret in the mission payload, the pitch angle of the gun barrel in the mission payload, and whether it has the shooting condition;

[0014] Combine the chassis pose and the state of the mission payload at the same moment to obtain the composite pose of the ground unmanned equipment at each trajectory point.

[0015] Based on further improvements to the above method, based on the ground model fitted from the ground point cloud, calculate the pitch angle and roll angle of each trajectory point at each moment according to the position and orientation angle of the trajectory points in the planned trajectory sequence, and obtain the chassis pose, including:

[0016] Construct a grid map composed of rectangular units, and obtain the height of the chassis from the ground in each grid of the ground unmanned equipment according to the ground height in the ground model, and obtain the chassis height map;

[0017] Initialize the rotation matrix of the chassis pose. According to the ground model and the chassis height map, based on the position and orientation angle of each trajectory point, obtain the support polygon according to the contact grid between the ground unmanned equipment and the ground;

[0018] Calculate the rotation axis for the trajectory points with unstable poses according to the support polygon, and rotate the ground unmanned equipment around the rotation axis by a fixed angle, update the rotation matrix and the chassis height map, and re-obtain the support polygon until the pose of the trajectory point is stable, obtain the rotation matrix of the chassis pose, and resolve the pitch angle and roll angle of the ground unmanned equipment at the corresponding trajectory point to obtain the chassis pose.

[0019] Based on further improvements to the above method, the variable-scale envelope under the composite pose of each trajectory point is obtained through the following steps:

[0020] According to the slewing angle of the turret and the pitch angle of the gun barrel in the composite pose, calculate the maximum envelopes projected by the mission payload onto the transverse, longitudinal, and height directions of the chassis respectively as the expansion distances;

[0021] Based on the relative position relationship between the mission payload and the chassis, calculate the size of the variable-scale envelope according to the chassis size and the expansion distance.

[0022] Based on the further improvement of the above method, based on the relative position relationship between the mission payload and the chassis, according to the chassis size and the expansion distance, the size of the variable-scale envelope is calculated using the following formula:

[0023] ,

[0024] In the formula, , , are the length, width, and height of the variable-scale envelope respectively, H is the height of the chassis, is the slewing angle of the turret, is the distance from the rotation center of the mission payload to the front end of the ground unmanned equipment, is the distance from the rotation center of the mission payload to the rear end of the ground unmanned equipment, is the distance from the rotation center of the mission payload to the left end of the ground unmanned equipment, is the distance from the rotation center of the mission payload to the right end of the ground unmanned equipment, , , are the maximum envelopes of the mission payload projected onto the lateral, longitudinal, and height directions of the chassis respectively.

[0025] Based on the further improvement of the above method, the variable-scale envelopes with the number of existing environmental point clouds less than or equal to the threshold are obtained through the following steps:

[0026] For each variable-scale envelope, according to the position of the environmental point clouds, detect whether each environmental point cloud satisfies the six half-space constraints corresponding to the six planes of the variable-scale envelope, count the number of environmental point clouds that simultaneously satisfy the six half-space constraints, and compare with the threshold to obtain the variable-scale envelopes with the number of existing environmental point clouds less than or equal to the threshold.

[0027] Based on the further improvement of the above method, the bounding boxes of each sub-component of the ground unmanned equipment are obtained, including: according to the sizes of each sub-component, obtaining the sizes of each bounding box; constructing the transformation relationship between each sub-component in the ground unmanned equipment through the D-H parameter method, obtaining the coordinate systems of each bounding box according to the transformation relationship, and obtaining the bounding boxes of each sub-component of the ground unmanned equipment according to the bounding box coordinate systems, the center point coordinates of the bounding boxes, and the sizes of the bounding boxes.

[0028] Based on the further improvement of the above method, the recoil rest condition in the attitude dimension is that the component force of the gun recoil force on the ramp is less than or equal to the maximum frictional force provided by the ground; the recoil stability condition in the attitude dimension is that the sum of the recoil moment, the gravity moment, and the ground support moment on the ramp is greater than or equal to 0.

[0029] Based on the further improvement of the above method, the recoil stillness condition for the steering dimension is that the sum of the recoil component force and the centrifugal force of the gun during flat ground steering is less than or equal to the maximum frictional force provided by the ground; the recoil stability condition for the steering dimension is that the sum of the recoil moment, the gravity moment, the ground support moment, and the centrifugal moment during flat ground steering is greater than or equal to 0.

[0030] On the other hand, the embodiment of the present invention provides a safety behavior detection system for a ground unmanned equipment during autonomous traveling and shooting, including:

[0031] A composite attitude acquisition module, configured to acquire the composite attitude of the ground unmanned equipment at each trajectory point according to the planned trajectory sequence and the expected scheduling sequence;

[0032] A collision safety detection module, configured to set the first mark of the trajectory point corresponding to the variable-scale envelope with the number of environmental point clouds less than or equal to the threshold as safe according to the variable-scale envelope under the composite attitude of each trajectory point, otherwise set it as risky; for the risky trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment, and if the number of environmental point clouds in any bounding box is greater than the threshold, modify the first mark to dangerous;

[0033] A chassis stability detection module, configured to detect whether the composite attitude of the trajectory points meeting the shooting conditions satisfies the recoil stillness condition and the recoil stability condition from the attitude and steering dimensions based on the expected scheduling sequence. If both are satisfied, set the second mark of the trajectory point as safe, otherwise set it as dangerous;

[0034] A detection result feedback module, configured to obtain the safety behavior detection result of each trajectory point according to the first mark and the second mark.

[0035] Compared with the prior art, the present invention can at least achieve one of the following beneficial effects: By studying the collision between the gun and obstacles during the gun scheduling process and the influence of the gun recoil force on the stability of the ground unmanned equipment under the composite attitude, ensure the safety of the complete process of traveling and shooting, avoid the occurrence of accidents, and ensure the safety of personnel and equipment; accurately classify the safety of the autonomous planning result of the chassis and the scheduling planning of the gun, which is convenient for accurately guiding the adjustment of the autonomous planning result of the chassis and the scheduling planning of the gun to ensure the safety of the ground unmanned equipment.

[0036] In the present invention, the above technical solutions can also be combined with each other to achieve more preferred combination schemes. Other features and advantages of the present invention will be described in the subsequent specification, and some advantages can be made obvious from the specification, or understood by implementing the present invention. The objectives and other advantages of the present invention can be realized and obtained through the content specifically pointed out in the specification and the drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0037] The accompanying drawings are only for the purpose of showing specific embodiments and are not considered as limiting the present invention. Throughout the drawings, the same reference signs denote the same components;

[0038] Figure 1 It is a flowchart of a safety behavior detection method for a ground unmanned equipment during autonomous moving shooting in Embodiment 1 of the present invention;

[0039] Figures 2(a), 2(b), 2(c) and 2(d) are respectively schematic diagrams of different sizes of variable-scale envelopes in Embodiment 1 of the present invention;

[0040] Figure 3 It is a schematic diagram of the bounding boxes of three sub-components of the ground unmanned equipment in Embodiment 1 of the present invention;

[0041] Figures 4(a) and 4(b) are respectively schematic diagrams of the coordinate systems of the turret and the barrel in the mission payload in Embodiment 1 of the present invention;

[0042] Figure 5 It is a schematic diagram of the decomposition of the recoil force of the artillery in Embodiment 1 of the present invention;

[0043] Figure 6 It is a force analysis diagram of the ground unmanned equipment during moving shooting on a longitudinal slope in Embodiment 1 of the present invention;

[0044] Figure 7 It is a force analysis diagram of the ground unmanned equipment during moving shooting on a transverse slope in Embodiment 1 of the present invention;

[0045] Figure 8 It is a force analysis diagram of the ground unmanned equipment during moving shooting while turning on a flat ground in Embodiment 1 of the present invention;

[0046] Figure 9 It is a block diagram of a safety behavior detection system for a ground unmanned equipment during autonomous moving shooting in Embodiment 2 of the present invention. Specific Embodiments

[0047] The following will specifically describe the preferred embodiments of the present invention with reference to the accompanying drawings. The accompanying drawings form a part of this application and are used together with the embodiments of the present invention to explain the principle of the present invention, rather than to limit the scope of the present invention.

[0048] Embodiment 1

[0049] A specific embodiment of the present invention discloses a safety behavior detection method for a ground unmanned equipment during autonomous moving shooting, as Figure 1 shown, including the following steps:

[0050] S11. Obtain the composite attitude of the ground unmanned equipment at each trajectory point according to the planned trajectory sequence and the expected scheduling sequence;

[0051] S12. According to the variable-scale envelopes under the composite postures of each trajectory point, set the first mark of the trajectory point corresponding to the variable-scale envelope with the number of environmental point clouds less than or equal to the threshold to safe, otherwise set it to risky; for the risky trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment, and if the number of environmental point clouds in any bounding box is greater than the threshold, modify the first mark to dangerous;

[0052] S13. Based on the expected scheduling sequence, detect whether the composite postures of the trajectory points with shooting conditions meet the recoil rest condition and the recoil stability condition from the attitude and steering dimensions. If both conditions are met, set the second mark of the trajectory points to safe, otherwise set it to dangerous;

[0053] S14. According to the first mark and the second mark, obtain the safety behavior detection results of each trajectory point.

[0054] During implementation, by regularly obtaining the planned trajectory sequence and the expected scheduling sequence for a future period of time, iteratively execute steps S11 - S14 to achieve continuous safety behavior detection. Among them, steps S12 and S13 can be carried out synchronously without a sequence requirement. This embodiment aims at the potential combat safety threats caused by the recoil impact and composite postures of the artillery during the synchronous driving of autonomous driving and shooting on undulating roads and rugged terrains. It conducts collaborative analysis on the ground unmanned equipment based on the road - equipment - artillery, locates the strategy space state based on event-driven and multi-source information fusion during autonomous moving shooting, and synchronously issues and deploys it, sounding an alarm before the danger occurs to facilitate timely updating of the collaborative safety behavior control strategy.

[0055] It should be noted that the ground unmanned equipment mainly includes two parts: the chassis and the mission payload. The chassis is wheeled, and the mission payload installed on the chassis is set according to the actual mission to be executed. In this embodiment, the ground unmanned equipment needs to shoot during autonomous movement. Therefore, the mission payload includes a turret and a barrel. Next, steps S11 - S14 will be described separately.

[0056] In step S11, the planned trajectory sequence is to regularly obtain the planned route for a future period of time from the path planning module, including: the x-axis and y-axis coordinates and the expected speed of each trajectory point at each moment; according to the expected speeds of every two adjacent trajectory points, the expected acceleration and the orientation angle of each trajectory point can be obtained. The expected scheduling sequence is to regularly obtain the action sequence for a future period of time from the mission payload scheduling module, including: the slewing angle of the turret, the elevation angle of the barrel, and whether shooting conditions are met at each moment, where meeting the shooting conditions means that the ground unmanned equipment has aimed at the target and is ready to shoot at any time according to the received shooting command. Among the moments with shooting conditions in the received expected scheduling sequence, through the safety behavior detection of this embodiment, the ground unmanned equipment is controlled to shoot at a position where shooting conditions are met and it is safe.

[0057] It should be noted that in this embodiment, the point cloud collected by the lidar on the ground unmanned equipment is segmented and extracted to obtain the ground point cloud and the environmental point cloud. The ground point cloud includes the point clouds of various terrains, such as flat ground and ramps; the environmental point cloud is the obstacle that the ground unmanned equipment cannot cross in the environment, including trees, rocks, tents, and other vehicles, etc.

[0058] Furthermore, fitting the ground model according to the ground point cloud includes: using the cubic B-spline method, taking each ground point cloud as a control point to fit out a smooth and continuous surface, ensuring that the curvature is continuous; dividing the environmental map into a grid map composed of rectangular cells, and taking the average value of the ground point clouds in each cell as the height of the grid; fitting out the mathematical model of the ground according to the center coordinates and height of each grid to obtain the ground model.

[0059] According to the ground model , at the specified position , the ground height is expressed as .

[0060] Next, based on the ground model fitted from the ground point cloud, according to the positions and orientation angles of the trajectory points at each moment in the planned trajectory sequence, calculate the pitch angle and roll angle of each moment's trajectory point to obtain the chassis pose, including:

[0061] ①Construct a grid map composed of rectangular cells, and according to the ground height in the ground model, obtain the height of the chassis of the ground unmanned equipment from the ground in each grid to obtain the chassis height map.

[0062] The chassis height map is a grid map in the vehicle body coordinate system. According to the ground height in the ground model and the size of the ground unmanned equipment, calculate the height of the chassis of the unmanned equipment from the ground in each grid to obtain the chassis height map. According to the chassis height map , at the specified position , the height of the ground unmanned equipment from the ground is expressed as .

[0063] ②Initialize the rotation matrix of the chassis pose. According to the ground model and the chassis height map, based on the positions and orientation angles of the trajectory points at each moment, obtain the support polygon according to the contact grid of the ground unmanned equipment with the ground.

[0064] Specifically, initialize the rotation matrix with pitch angle and roll angle being 0, and set the height to , higher than the height of any ground point cloud, that is, place the ground unmanned equipment horizontally suspended at a position above the ground; then according to update the chassis height map the height value in it, i.e., add to each grid ; After specifying the position and orientation angle of the ground unmanned equipment, obtain the clearance between each point on the chassis of the ground unmanned equipment and the ground through the difference between the height corresponding to each grid in the chassis height map and the height corresponding to the ground model and obtain the minimum clearance from the ground ; Keep the current pitch and orientation angles of the platform, and lower the height of the ground unmanned equipment by to make the ground unmanned equipment contact the ground and update the chassis height map.

[0065] According to the following formula, obtain the grids less than or equal to the sum of the height at the corresponding position on the ground model and the contact increment to get the contact grids:

[0066] ,

[0067] wherein represents the contact increment, which is used to represent the trade-off between the accuracy of the contact grid estimation and the robustness of the height error caused by the existence of sensor noise.

[0068] The support polygon is the smallest convex hull that can enclose all the contact grids of the ground unmanned equipment. If there is only one contact grid, the support polygon is this one contact grid; if there are two contact grids, the support polygon is the line segment formed by these two grids; if there are more than two contact grids, use the Graham scan to perform convex hull detection on the contact grids to obtain the support polygon.

[0069] ③ Calculate the rotation axis for the trajectory points with unstable pose according to the support polygon, and rotate the ground unmanned equipment around the rotation axis by a fixed angle, update the rotation matrix and the chassis height map, re-obtain the contact point data and the support polygon until the pose of the trajectory point is stable, obtain the rotation matrix of the chassis pose, and resolve the pitch angle and roll angle of the ground unmanned equipment at the corresponding trajectory point to obtain the chassis pose.

[0070] Specifically, if the projection point of the centroid of the ground unmanned equipment on the chassis height map is inside the support polygon, the current trajectory point pose is stable; otherwise, the pose is unstable. Calculate the rotation axis according to the distance from the projection point to the support polygon, including: calculate the distance from the projection point to each point and each side of the support polygon. If the shortest distance corresponds to a point, the rotation axis is the line passing through this point and orthogonal to the line from this point to the projection point; otherwise, the rotation axis is the line where the side corresponding to the shortest distance is located. In order to accurately calculate the pose, the fixed angle for rotating the ground unmanned equipment around the rotation axis is a preset small angle. Update the rotation matrix and the chassis height map, and iteratively execute steps ② and ③ until the pose is stable. Exemplarily, the preset fixed angle of rotation is 0.1 rad.

[0071] Finally, through the inverse calculation method of the X-Y-Z fixed-axis attitude matrix, the pitch angle of the ground unmanned equipment is parsed from the rotation matrix at the time of pose stability. and the roll angle . Combining the position and orientation angle of the ground unmanned equipment in the planned trajectory sequence , and, according to the chassis height map, obtaining the height of the unmanned equipment at each trajectory point, the complete chassis pose data of the ground unmanned equipment at each trajectory point is obtained.

[0072] Meanwhile, according to the expected scheduling sequence, the states of the mission payloads at each moment are obtained, including the slewing angle of the turret in the mission payload, the pitch angle of the gun barrel in the mission payload, and whether the shooting condition is met; combining the chassis pose and the states of the mission payloads at the same moment, the composite attitude at each trajectory point is obtained.

[0073] Since the operation of the mission payload will change the safety dimensions of the entire ground unmanned equipment, step S12 performs a rough detection of the variable-scale envelope through step S121, and then performs a fine collision detection of sub-components on the trajectory points with risks through step S122 to improve the accuracy of safety detection.

[0074] S121. According to the variable-scale envelope of the ground unmanned equipment in the composite attitude at each trajectory point, set the first mark of the trajectory point corresponding to the variable-scale envelope without any environmental point cloud as safe, otherwise set it as risky.

[0075] It should be noted that first, according to the slewing angle of the turret and the pitch angle of the gun barrel in the composite attitude, the maximum envelopes of the mission payload projected onto the lateral, longitudinal, and height directions of the chassis are calculated respectively through the following formulas as the expansion distances, and the unit is meters:

[0076] ,

[0077] In the formula, , , are respectively the maximum envelopes of the mission payload projected onto the lateral, longitudinal, and height directions of the chassis; is the slewing angle of the turret in the mission payload, is the pitch angle of the gun barrel in the mission payload, is the length of the gun barrel in the mission payload.

[0078] Based on the relative position relationship between the mission payload and the chassis, according to the chassis dimensions and the expansion distances, the dimensions of the variable-scale envelope are calculated using the following formula:

[0079] ,

[0080] In the formula, , , are the length, width, and height of the variable-scale envelope, H is the height of the chassis, is the distance from the rotation center of the mission payload to the front end of the ground unmanned equipment, is the distance from the rotation center of the mission payload to the rear end of the ground unmanned equipment, is the distance from the rotation center of the mission payload to the left end of the ground unmanned equipment, is the distance from the rotation center of the mission payload to the right end of the ground unmanned equipment.

[0081] Exemplarily, Figures 2(a), 2(b), 2(c), and 2(d) are schematic diagrams of different sizes of the variable-scale envelope respectively.

[0082] Calculate the homogeneous transformation matrix of the variable-scale envelope center in the world coordinate system according to the variable-scale envelope size and the chassis pose :

[0083] ,

[0084] where, is the rotation matrix of the chassis pose, is the position vector of the ground unmanned equipment in the world coordinate system, is the position vector of the variable-scale envelope in the vehicle body coordinate system calculated according to the variable-scale envelope size.

[0085] The variable-scale envelope is actually a cuboid composed of six planes. When the combined pose of the chassis and the mission payload is safe, the variable-scale envelope does not collide with obstacles in the environment, that is, there is no environmental point cloud in the variable-scale envelope. The position of each environmental point cloud is determined by the spatial coordinates . When each environmental point cloud exists in the variable-scale envelope, the three coordinate values of the environmental point cloud simultaneously satisfy the six half-space constraints corresponding to the six planes of the variable-scale envelope. The formula is as follows:

[0086] ,

[0087] where, , and are the corresponding three axial rotation components in, , and are respectively , and transpose.

[0088] Considering the noise in the environmental point cloud, in this embodiment, a threshold is set according to the actual environmental conditions. For each variable-scale envelope, according to the position of the environmental point cloud, it is detected whether each environmental point cloud satisfies the six half-space constraints corresponding to the variable-scale envelope. If the six half-space constraints are satisfied simultaneously, it means that the environmental point cloud exists in the variable-scale envelope. The number of environmental point clouds that satisfy the six half-space constraints simultaneously is counted and compared with the threshold. If it is less than or equal to the threshold, it means that the trajectory point corresponding to the variable-scale envelope travels safely in the composite pose, and the rough detection in step S121 is continued for the next undetected trajectory point, and there is no need to perform the precise detection in step S122. If it is greater than the threshold, it means that there is a collision risk when the trajectory point corresponding to the variable-scale envelope travels in the composite pose, and step S122 is entered to perform the fine collision detection of the sub-components.

[0089] S122. For the risk trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment. If the number of environmental point clouds existing in any bounding box is greater than the threshold, modify the first flag to dangerous.

[0090] It should be noted that when driving autonomously on off-road surfaces, due to the rigid connection between the chassis and the mission payload, the pose transformation of the chassis will drive the movement of the rotating shaft of the turret. In this embodiment, the mission payload is further divided into the turret and the barrel, that is, the ground unmanned equipment is divided into three sub-components: the chassis, the turret, and the barrel. The bounding boxes of each sub-component of the ground unmanned equipment are as Figure 3 shown. The chassis is used as the No. 1 sub-component, the turret is used as the No. 2 sub-component, and the barrel is used as the No. 3 sub-component. The bounding box includes a coordinate system, a center point, and dimensions. Among them, the coordinate system determines the orientation of the bounding box, the center point determines the position of the bounding box, and the dimensions determine the size of the bounding box.

[0091] To further simplify the problem, the chassis is defined as the base, considering the pose transformation of the mission payload relative to the chassis, and the mission payload is represented as two rotating joints, as shown in FIGS. 4(a) and 4(b). The mission payload link is composed of 2 rotating joints and 1 rigid link (i.e., the base) in series. The center point of each bounding box is located on the joint axis, and the bounding box rotates around the joint axis with the rotation of the joint, ensuring that the composite pose of the ground unmanned equipment is always inside the bounding box during the movement process.

[0092] Furthermore, obtaining the bounding boxes of each sub-component of the ground unmanned equipment includes:

[0093] ① Obtain the dimensions of each bounding box according to the dimensions of each sub-component.

[0094] ② Construct the transformation relationship between each sub-component in the ground unmanned equipment by the D-H parameter method, and obtain the coordinate systems of each bounding box according to the transformation relationship.

[0095] Specifically, the homogeneous transformation matrix of the turret in the chassis coordinate system is calculated using the D-H parameter method , and the homogeneous transformation matrix of the barrel in the turret coordinate system .

[0096] The coordinate systems of each bounding box are obtained according to the transformation relationship through the following formula:

[0097] ,

[0098] In the formula, is the coordinate system of the bounding box of the k-th sub-component, is the homogeneous transformation matrix from the chassis to the world coordinate system, is the rotation matrix of the chassis pose, is the position vector of the ground unmanned equipment in the world coordinate system.

[0099] ③ According to the bounding box coordinate system, the coordinates of the center point of the bounding box, and the size of the bounding box, the bounding boxes of each sub-component of the ground unmanned equipment are obtained.

[0100] The center point of the bounding box determines the specific position of the bounding box. The coordinates of the center point of the bounding box are calculated according to the following formula:

[0101] ,

[0102] In the formula, are the coordinates of the center point of the bounding box of the k-th sub-component in the world coordinate system; are the coordinates of the center point of the bounding box of the k-th sub-component in the bounding box coordinate system of the k-th sub-component.

[0103] After obtaining the bounding boxes of each sub-component, refer to the method of detecting whether the environmental point cloud exists in the variable-scale envelope in step S121 to detect the number of environmental point clouds existing in the bounding box of each sub-component in turn. If it is less than or equal to the threshold, the first mark is still a risk. If it is greater than the threshold, it means that the composite pose of the ground unmanned equipment at this trajectory point will collide with obstacles in the environment, and the first mark is modified to dangerous.

[0104] It should be noted that the complex combat environment requires the ground unmanned equipment to be able to complete coordination with the barrel during the process of traveling in the cross-country environment. The barrel will have an impact on the steady driving of the chassis during the execution of tasks. This impact is mainly reflected in the instantaneous recoil force impact during the process of firing while moving. In severe cases, it will cause the chassis to slide and roll over. To ensure the steady safety of the chassis under the impact of the recoil force of the artillery, it is necessary to consider the safety strategy space in different scenarios.

[0105] During the process of the gun completing the shooting mission, the recoil part completes the recoil and recuperation movements along the axis of the barrel under the action of the resultant force in the barrel, the recoil mechanism force, and the recuperator force, and generates a recoil force. Step S13 respectively considers the recoil stillness under the recoil force condition and the recoil stability of the vehicle body attitude under the recoil moment condition from the attitude and steering dimensions through step S131 and step S132, and obtains the chassis steady-state safe shooting action space. The safe shooting action space reflects the feasible sections of shooting on the planned expected path and provides guidance for the gun shooting moment.

[0106] For the trajectory points with shooting conditions, according to the slewing angle of the turret at this trajectory point, the recoil force is decomposed on the two-dimensional plane, as Figure 5 shown. Set the x direction as the roll axis and the y direction as the pitch axis. Then, based on the slewing angle of the turret, the components of the recoil force and in the x and y directions are respectively expressed as:

[0107] .

[0108] S131. Detect whether the recoil stillness condition and the recoil stability condition are satisfied from the attitude dimension.

[0109] Since the attitude change on the ramp is relatively large, this step mainly considers the sliding and overturning situations that may occur during shooting while traveling on the ramp (longitudinal slope and cross slope). The recoil stillness condition is that the component of the gun recoil force on the ramp is less than or equal to the maximum friction force provided by the ground, which is used to ensure that when the instantaneous maximum gun recoil force is generated, the wheels of the chassis always remain in contact with the ground and do not slide horizontally; the recoil stability condition is that the sum of the recoil moment, the gravity moment, and the ground support moment on the ramp is greater than or equal to 0, ensuring that the wheels always remain in contact with the ground and do not overturn.

[0110] Specifically, as Figure 6 shown, when shooting while traveling on the longitudinal slope, the recoil stillness condition for keeping the unmanned ground equipment from sliding is as follows:

[0111] ,

[0112] In the formula, is the pitch angle of the cross slope where the chassis is located; is the friction coefficient of the cross slope where the chassis is located during shooting while traveling; is the maximum friction force provided by the ground in the x direction, is the pitch angle of the gun barrel relative to the chassis coordinate system during shooting while traveling.

[0113] As Figure 7As shown in the figure, when firing while moving on a cross slope, the conditions for the recoil stillness of the ground unmanned equipment without sliding are as follows:

[0114] ,

[0115] wherein, is the roll angle of the longitudinal slope where the chassis is located; is the friction coefficient of the longitudinal slope where the chassis is located during firing while moving; is the maximum friction force in the y direction provided by the ground.

[0116] Since the possible rollover occurs on the set of wheels that are most prone to rollover, in this embodiment, the rollover on the slope is calculated with the wheel group with a lower height as the rotation center, and the distance from the last wheel group to the centroid is used as the lever arm to calculate each moment.

[0117] Specifically, when firing while moving on a longitudinal slope, the conditions for the recoil stability of the ground unmanned equipment without rollover are as follows:

[0118] ,

[0119] wherein, is the recoil moment of the mission payload in the x direction, is the gravity moment of the ground unmanned equipment, is the ground support moment, is the vertical distance from the centroid of the mission payload to the centroid of the chassis, is the longitudinal distance from the centroid of the mission payload to the centroid of the chassis, is the vertical distance from the ground to the centroid of the chassis, is the horizontal distance from the i-th ground unmanned equipment to the vehicle centroid, is the minimum allowable support force of the ground on the tire at the i-th axle when the ground unmanned equipment is stable.

[0120] When firing while moving on a cross slope, the conditions for the recoil stability of the ground unmanned equipment without rollover are as follows:

[0121] ,

[0122] wherein, is the recoil moment of the mission payload in the y direction, B is the wheelbase, is the resultant force of the minimum ground support forces of the wheels on the j-th side.

[0123] It should be noted that and are preset values, which are obtained by establishing a multi-body dynamics model of the ground unmanned equipment and using offline simulations of the ground unmanned equipment firing while moving under different ramp angles and lateral accelerations in various working conditions, and statistically obtaining and The minimum value under steady-state driving.

[0124] S132. Detect whether the recoil stillness condition and the recoil stability condition are satisfied from the steering dimension.

[0125] During steering, different centrifugal forces will be encountered at different steering speeds and steering curvatures, and the centrifugal force will affect the recoil stillness and recoil stability of the chassis. The recoil stillness condition is that the sum of the recoil force component and the centrifugal force during flat-ground steering is less than or equal to the maximum frictional force provided by the ground, ensuring that the wheels of the ground unmanned equipment always remain in contact with the ground and do not slide horizontally during shooting; the recoil stability condition in the steering dimension is that the sum of the recoil moment, the gravity moment, the ground support moment, and the centrifugal moment during flat-ground steering is greater than or equal to 0, ensuring that the wheels always remain in contact with the ground and do not flip.

[0126] Specifically, during traveling and shooting during flat-ground steering, there is an additional centrifugal force, making it easier to deflect in the direction of the centrifugal force. The centrifugal force is calculated based on the speed and curvature. The recoil stillness condition for flat-ground steering is as follows:

[0127] ,

[0128] In the formula, is the centrifugal force generated when the ground unmanned equipment rotates, is the recoil force, v is the speed of the chassis during flat-ground steering, R is the radius of curvature of the chassis during flat-ground steering, is the friction coefficient of the flat ground where the chassis is located during traveling and shooting during flat-ground steering.

[0129] During the steering process, it must be ensured that the resultant moment is greater than or equal to 0. Among them, due to the vehicle body's own weight and possible overturning occurring on the wheel sets with smaller curvatures, therefore, in this step, the overturning during flat-ground driving takes the wheel sets with smaller curvatures as the rotation center to calculate each moment.

[0130] Specifically, as Figure 8 shown, the recoil stability condition for the ground unmanned equipment not to flip during flat-ground steering is as follows:

[0131] ,

[0132] In the formula, is the moment of the centrifugal force when the ground unmanned equipment rotates.

[0133] When the composite postures of the trajectory points with shooting conditions all satisfy the recoil stillness condition and the recoil stability condition in step S131 and step S132, set the second mark of the trajectory point to safe; otherwise, set it to dangerous.

[0134] The first mark of the trajectory point is obtained through step S12, and the second mark of the trajectory point is obtained through step S13. In the safety behavior detection result of step S14, both the first mark and the second mark can be reflected, or based on the levels of various marks, the higher-level mark can overwrite the lower-level mark to reflect the final mark.

[0135] Preferably, for the trajectory points that do not meet the shooting conditions, directly take the first mark as the safety behavior detection result; for the trajectory points that meet the shooting conditions, according to the order of danger > risk > safety, take the higher-level mark among the first mark and the second mark as the safety behavior detection result.

[0136] Compared with the prior art, the safety behavior detection method for autonomous shooting during movement of the ground unmanned equipment provided in this embodiment ensures the safety of the entire process of shooting during movement by studying the collision between the artillery and obstacles during the artillery scheduling process and the impact of the recoil force of the artillery on the stability of the ground unmanned equipment under the compound posture, avoids the occurrence of accidents, and guarantees the safety of personnel and equipment; accurately classifies the safety of the autonomous planning result of the chassis and the scheduling plan of the artillery, which is convenient for accurately guiding the adjustment of the autonomous planning result of the chassis and the scheduling plan of the artillery to ensure the safety of the ground unmanned equipment.

[0137] Embodiment 2

[0138] Another embodiment of the present invention discloses a safety behavior detection system for autonomous shooting during movement of a ground unmanned equipment, so as to implement the safety behavior detection method for autonomous shooting during movement of the ground unmanned equipment in Embodiment 1. As Figure 9 shown, the specific implementation manners of each module refer to the corresponding descriptions in Embodiment 1. The system includes:

[0139] The compound posture acquisition module 101 is used to acquire the compound posture of the ground unmanned equipment at each trajectory point according to the planned trajectory sequence and the expected scheduling sequence;

[0140] The collision safety detection module 102 is used to set the first mark of the trajectory point corresponding to the variable-scale envelope with the number of environmental point clouds less than or equal to the threshold as safe according to the variable-scale envelope under the compound posture of each trajectory point, otherwise set it as risky; for the risky trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment, and if the number of environmental point clouds in any bounding box is greater than the threshold, modify the first mark to dangerous;

[0141] The chassis stability detection module 103 is used to detect whether the compound posture of the trajectory points that meet the shooting conditions satisfies the recoil stillness condition and the recoil stability condition from the attitude and steering dimensions based on the expected scheduling sequence. If both are satisfied, set the second mark of the trajectory point as safe, otherwise set it as dangerous;

[0142] The detection result feedback module 104 is configured to obtain the safety behavior detection results of each trajectory point according to the first tag and the second tag.

[0143] Since the relevant parts of this embodiment and the foregoing safety behavior detection method for autonomous shooting during the movement of ground unmanned equipment can be mutually referenced, and to avoid repetition, they will not be elaborated here. Since the principles of the system embodiment and the method embodiment are the same, the system embodiment also has the corresponding technical effects of the method embodiment.

[0144] Those skilled in the art can understand that all or part of the processes for implementing the methods of the above embodiments can be completed by instructing relevant hardware through a computer program, and the program can be stored in a computer-readable storage medium. Among them, the computer-readable storage medium is a magnetic disk, an optical disk, a read-only memory, or a random access memory, etc.

[0145] The above is only a preferred specific embodiment of the present invention, but the protection scope of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by those skilled in the art within the technical scope disclosed by the present invention should be covered within the protection scope of the present invention.

Claims

1. A safety behavior detection method for autonomous shooting during the movement of ground unmanned equipment, characterized in that, Including the following steps: According to the planned trajectory sequence and the expected scheduling sequence, obtain the composite pose of the ground unmanned equipment at each trajectory point; According to the variable-scale envelope under the composite pose of each trajectory point, set the first mark of the trajectory point corresponding to the variable-scale envelope with the number of environmental point clouds less than or equal to the threshold to safe, otherwise set it to risky; For the risky trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment. If the number of environmental point clouds in any bounding box is greater than the threshold, modify the first mark to dangerous; Based on the expected scheduling sequence, detect from the attitude and steering dimensions whether the composite pose of the trajectory points meeting the shooting conditions satisfies the recoil rest condition and the recoil stability condition. If both are satisfied, set the second mark of the trajectory point to safe, otherwise set it to dangerous; the recoil rest condition in the attitude dimension is that the component of the gun recoil force on the ramp is less than or equal to the maximum friction force provided by the ground; the recoil stability condition in the attitude dimension is that the sum of the recoil moment, the gravity moment and the ground support moment on the ramp is greater than or equal to 0; the recoil rest condition in the steering dimension is that the sum of the component of the gun recoil force and the centrifugal force during flat turning is less than or equal to the maximum friction force provided by the ground; the recoil stability condition in the steering dimension is that the sum of the recoil moment, the gravity moment, the ground support moment and the centrifugal moment during flat turning is greater than or equal to 0; According to the first mark and the second mark, obtain the safety behavior detection results of each trajectory point.

2. The safety behavior detection method for autonomous shooting during the movement of ground unmanned equipment according to claim 1, characterized in that, The obtaining the composite pose of the ground unmanned equipment at each trajectory point according to the planned trajectory sequence and the expected scheduling sequence includes: Based on the ground model fitted from the ground point cloud, according to the position and orientation angle of each trajectory point in the planned trajectory sequence, calculate the pitch angle and roll angle of each trajectory point to obtain the chassis pose; According to the expected scheduling sequence, obtain the states of the mission payloads at each moment, including the slewing angle of the turret in the mission payload, the pitch angle of the gun barrel in the mission payload, and whether the shooting condition is met; Combine the chassis pose and the states of the mission payloads at the same moment to obtain the composite pose of the ground unmanned equipment at each trajectory point.

3. The safety behavior detection method for autonomous shooting during the movement of ground unmanned equipment according to claim 2, characterized in that, The calculating the pitch angle and roll angle of each trajectory point based on the ground model fitted from the ground point cloud, according to the position and orientation angle of each trajectory point in the planned trajectory sequence to obtain the chassis pose includes: Construct a grid map composed of rectangular units, and according to the ground height in the ground model, obtain the height of the chassis from the ground in each grid of the ground unmanned equipment to obtain the chassis height map; Initialize the rotation matrix of the chassis pose. According to the ground model and the chassis height map, based on the position and orientation angle of each trajectory point, obtain the support polygon according to the contact grid of the ground unmanned equipment with the ground; Calculate the rotation axis for the trajectory points with unstable poses according to the support polygon, and rotate the ground unmanned equipment around the rotation axis by a fixed angle, update the rotation matrix and the chassis height map, and re-obtain the support polygon until the pose of the trajectory point is stable, obtain the rotation matrix of the chassis pose, and resolve the pitch angle and roll angle of the ground unmanned equipment at the corresponding trajectory point to obtain the chassis pose.

4. The safety behavior detection method for autonomous shooting during movement of ground unmanned equipment according to claim 2, characterized in that The variable-scale envelope under the composite pose of each trajectory point is obtained through the following steps: According to the slewing angle of the turret and the elevation angle of the gun barrel in the composite attitude, calculate the maximum envelopes of the mission payload projected onto the lateral, longitudinal, and height directions of the chassis respectively, and use them as the expansion distances. Based on the relative position relationship between the mission payload and the chassis, calculate the size of the variable-scale envelope according to the chassis size and the expansion distance.

5. The safety behavior detection method for autonomous shooting during movement of ground unmanned equipment according to claim 4, characterized in that, Based on the relative position relationship between the mission payload and the chassis, calculate the size of the variable-scale envelope using the following formula according to the chassis size and the expansion distance: Wherein, L p , W p , H p are respectively the length, width and height of the variable-scale envelope, H is the height of the chassis, ψ g is the slewing angle of the turret, L f is the distance from the rotation center of the mission payload to the front end of the ground unmanned equipment, L b is the distance from the rotation center of the mission payload to the rear end of the ground unmanned equipment, L l is the distance from the rotation center of the mission payload to the left end of the ground unmanned equipment, L r is the distance from the rotation center of the mission payload to the right end of the ground unmanned equipment, W dis , L dis , H dis are respectively the maximum envelopes of the mission payload projected onto the lateral, longitudinal and height directions of the chassis.

6. The safety behavior detection method for autonomous shooting during movement of ground unmanned equipment according to claim 5, characterized in that, Obtain the variable-scale envelopes with the number of existing environmental point clouds less than or equal to the threshold through the following steps: For each variable-scale envelope, detect whether each environmental point cloud satisfies the six half-space constraints corresponding to the six planes of the variable-scale envelope according to the position of the environmental point cloud, count the number of environmental point clouds that simultaneously satisfy the six half-space constraints, and compare it with the threshold to obtain the variable-scale envelopes with the number of existing environmental point clouds less than or equal to the threshold.

7. The safety behavior detection method for autonomous shooting during movement of the ground unmanned equipment according to claim 2, wherein The obtaining of the bounding boxes of each sub-component of the ground unmanned equipment includes: obtaining the size of each bounding box according to the size of each sub-component; constructing the transformation relationship between each sub-component in the ground unmanned equipment by the D-H parameter method, obtaining the coordinate systems of each bounding box according to the transformation relationship, and obtaining the bounding boxes of each sub-component of the ground unmanned equipment according to the bounding box coordinate systems, the center point coordinates of the bounding boxes, and the sizes of the bounding boxes.

8. A safety behavior detection system for autonomous shooting during the movement of ground unmanned equipment, characterized in that, Including: A composite attitude acquisition module, configured to acquire the composite attitude of the ground unmanned equipment at each trajectory point according to the planned trajectory sequence and the expected scheduling sequence. A collision safety detection module, configured to set the first mark of the trajectory point corresponding to the variable-scale envelope with the number of existing environmental point clouds less than or equal to the threshold to safe according to the variable-scale envelope in the composite attitude of each trajectory point, otherwise set it to risky. For the risky trajectory points, obtain the bounding boxes of each sub-component of the ground unmanned equipment, and if the number of environmental point clouds existing in any bounding box is greater than the threshold, modify the first mark to dangerous. A chassis stability detection module, configured to detect whether the composite attitude of the trajectory points with shooting conditions satisfies the recoil rest condition and the recoil stability condition from the attitude and steering dimensions based on the expected scheduling sequence. If both are satisfied, set the second mark of the trajectory point to safe, otherwise set it to dangerous; the recoil rest condition in the attitude dimension is that the component force of the gun recoil force on the slope is less than or equal to the maximum friction force provided by the ground; the recoil stability condition in the attitude dimension is that the sum of the recoil moment, the gravity moment, and the ground support moment on the slope is greater than or equal to 0; the recoil rest condition in the steering dimension is that the sum of the component force of the gun recoil force and the centrifugal force during flat turning is less than or equal to the maximum friction force provided by the ground; the recoil stability condition in the steering dimension is that the sum of the recoil moment, the gravity moment, the ground support moment, and the centrifugal moment during flat turning is greater than or equal to 0. A detection result feedback module, configured to obtain the safety behavior detection results of each trajectory point according to the first mark and the second mark.

Images