AGV relative navigation method and system

Abstract

The application discloses an AGV relative navigation method and system, and is applied to the technical field of AGV navigation. The method comprises the following steps: estimating the platform motion state of a floating carrier based on a wave kinematics model constraint state estimator; differentiating the vehicle body inertia data and the platform inertia data and combining a wheel speed meter to estimate the vehicle body relative motion state; performing relative pose motion compensation on the original point cloud frame according to the laser point scanning time, obtaining the non-distortion point cloud under the platform coordinate system, and matching the relative map to obtain the relative pose; correcting the relative pose by using a beacon network fixed to the floating carrier; and generating a navigation control instruction under the safety constraint of a moving base. The application solves the problems of the existing AGV absolute navigation, such as the divergence of positioning on the moving base and the failure of path tracking, and realizes high-precision relative navigation on the floating carrier.

Description

Technical Field

[0001] This invention relates to the field of AGV navigation technology, and more specifically to an AGV relative navigation method and system. Background Technology

[0002] AGVs have been widely deployed in manufacturing workshops, warehousing and logistics, and container terminals. Currently, mainstream navigation methods include magnetic strip / magnetic nail guidance, QR code visual positioning, laser SLAM, visual SLAM, and inertial / satellite integrated navigation. These positioning solutions share a common technical premise: the ground on which the AGV travels (factory floor, warehouse floor, dock floor) is approximately stationary relative to the Earth, the map in the world coordinate system does not change over time, and the absolute pose output directly corresponds to the motion control quantity. However, in ship and floating offshore platform scenarios, the deck and cabin floors continuously undergo six degrees of freedom motion with the waves. Currently, material transfer on decks / cabins in this field still mainly relies on manual labor and ordinary forklifts. Work on offshore platforms focuses on inspection robots for fixed jacket platforms; work on ships focuses on path planning research for prefabricated cabin transfer robots and engine room rescue robots in shipyard shore-side workshops. Existing AGV navigation solutions on floating platforms have the following problems: 1. Existing SLAM maps are defined in a geostationary coordinate system. Platform swaying causes continuous motion of the world, leading to map distortion, loop closure failure, positioning drift, and path tracking divergence. 2. Within a single-frame laser scan cycle (e.g., 100ms), the platform's roll angular velocity can reach several degrees per second, resulting in significant motion distortion of the scanned point cloud, matching degradation, and even loss of positioning. 3. The vehicle's inertial measurement unit simultaneously senses the platform's motion and the vehicle's own motion; these two are coupled, and the inertial data cannot be directly used to calculate the vehicle's pose relative to the deck. 4. Under conditions of high roll, AGVs face the risk of sideslip and overturning. Existing navigation systems do not incorporate platform motion into speed and path constraints. Therefore, providing a relative navigation method and system for AGVs is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0003] In view of this, the present invention provides an AGV relative navigation method and system, which switches the navigation reference system from a stationary coordinate system to a coordinate system fixed to the floating carrier, and outputs the high-precision relative pose of the vehicle body relative to the deck under the condition of a moving base, thereby performing AGV navigation.

[0004] To achieve the above objectives, the present invention provides the following technical solution: An AGV relative navigation method includes the following steps: S1. Obtain the platform inertial data output by the platform inertial measurement unit on the floating carrier, and filter and estimate the platform inertial data based on the state estimator constrained by the wave kinematics model to obtain the platform motion state of the floating carrier. S2. Obtain the vehicle body inertial data output by the vehicle body inertial measurement unit installed on the AGV body and the wheel speed data output by the wheel speed meter; perform differential processing on the vehicle body inertial data and the platform inertial data in a unified coordinate system, and estimate the relative motion state of the vehicle body relative to the floating carrier by combining the wheel speed data with the error state Kalman filter. S3. Obtain the original point cloud frame output by the vehicle body lidar. Determine the relative pose of the vehicle body at the scanning time corresponding to each laser point in the original point cloud frame based on the platform motion state and relative motion state. Perform motion compensation on each laser point according to the scanning time to obtain the distortion-free point cloud in the platform coordinate system. S4. Match the distortion-free point cloud with the pre-constructed relative map in the platform coordinate system, and use the matched pose measurement as the observation to update the error state Kalman filter to obtain the relative pose of the vehicle body. S5. Obtain the distance measurement measurements between multiple beacons in the beacon network on the floating carrier and the beacon tags installed on the AGV body. Based on the calibration coordinates of each beacon in the relative map, correct the relative pose to obtain the corrected relative pose. S6. Based on the platform's motion state and the corrected relative posture, navigation control commands are generated under the safety constraints of the moving base to control the AGV to travel on the floating carrier.

[0005] Optionally, the wave kinematics model in S1 is an autoregressive moving average model, and the model parameters are identified by the spectral characteristics of the platform inertial data; the state estimator is an extended Kalman filter, and the state vector includes roll angle, roll angular velocity, pitch angle, pitch angular velocity, yaw angle, yaw angular velocity, heave displacement, and heave velocity.

[0006] Optionally, S2 is as follows: Acquire the vehicle inertial data output by the vehicle inertial measurement unit installed on the AGV body and the wheel speed data output by the wheel speed meter; transform the angular velocity in the vehicle coordinate system output by the vehicle inertial measurement unit to the platform coordinate system: ; In the formula, Let ω be the angular velocity in the vehicle coordinate system. Let ω be the angular velocity in the platform coordinate system. Let be the rotation matrix from the vehicle coordinate system to the platform coordinate system. The angular velocity of the vehicle body relative to the platform; The ratio output from the vehicle's inertial measurement unit After deducting platform-related acceleration, Coriolis acceleration, lever arm compensation term, and gravitational component: ; In the formula, The specific force output by the platform's inertial measurement unit. For lever arm vector, The platform's angular velocity vector. The platform's angular acceleration vector. The relative velocity of the vehicle body with respect to the platform. The relative linear acceleration of the vehicle body with respect to the platform; for The relative velocity estimate is obtained by integration; the relative velocity estimate is then fused with the wheel speed meter mileage increment using an error state Kalman filter to estimate the relative motion state.

[0007] Optionally, S3 specifically refers to: Obtain the original point cloud frame output by the vehicle's LiDAR, and let the first... The reference time of the original point cloud frame is , within the frame The scanning time of each laser point is The motion compensation formula is: ; In the formula, For pose transformation composite operations, for Momentary laser point Coordinates in the vehicle coordinate system for Momentary laser point Coordinates in the vehicle coordinate system for Predicted relative pose at any given time. for The predicted relative pose at each scan time is obtained by interpolating the platform motion state and the relative motion state over time.

[0008] Optionally, S4 specifically refers to: Using the predicted relative pose at the reference time of the original point cloud frame as the initial pose, the distortion-free point cloud is matched with the relative map in the pre-constructed platform coordinate system by performing a normal distribution transformation to obtain the pose measurement value at the reference time. The pose measurement value is fed back to S2 as an observation to estimate the relative motion state of the vehicle body relative to the floating carrier. The result is fused with the predicted relative pose at the reference time to obtain the relative pose of the vehicle body.

[0009] Optionally, S5 corrects the relative pose based on the calibration coordinates of each beacon in the relative map, specifically as follows: No. The distance measurement equation for each beacon is: ; In the formula, For beacon tags and the first Distance values ​​between beacons The position of the vehicle body in the platform coordinate system. The heading angle of the vehicle body relative to the platform. This is the rotation matrix corresponding to the heading angle. This refers to the installation offset vector of the beacon tag in the vehicle coordinate system. For the first The coordinates of a beacon on a relative map. To reduce ranging noise, the relative pose correction is calculated using the weighted least squares method. The correction weights are determined based on the variance of each beacon ranging observation and the non-line-of-sight detection results. Non-line-of-sight detection is based on the first-path energy ratio of the impulse response of the ultra-wideband signal channel.

[0010] Optionally, in S6, navigation control commands are generated based on the platform's motion state and the corrected relative pose, under the safety constraints of the moving base. Specifically: Based on the roll angle amplitude during platform motion, graded speed limits are implemented: when the roll angle amplitude does not exceed the first threshold, the AGV is controlled to travel at the first speed limit; when the roll angle amplitude exceeds the first threshold but does not exceed the second threshold, the AGV is controlled to travel at the second speed limit, which is lower than the first speed limit, and lateral movement along the ship's width is prohibited; when the roll angle amplitude exceeds the second threshold but does not exceed the third threshold, the AGV is controlled to travel along the ship's length at the third speed limit, which is lower than the second speed limit; when the roll angle amplitude exceeds the third threshold, the AGV is controlled to brake and park, and the parking mechanism is locked. Based on the online estimates of the roll angle and deck friction coefficient during platform motion, and the AGV's cargo loading status, the maximum allowable lateral acceleration, maximum allowable speed, and minimum turning radius are calculated according to lateral force balance constraints and incorporated into the local path planning; whereby the lateral force balance constraints satisfy: ,in, For the lateral acceleration of the vehicle body, This is an online estimate of the deck friction coefficient. It is the acceleration due to gravity. This is the roll angle.

[0011] Optionally, S6 also includes: predicting the motion envelope of the floating carrier within a preset time period based on the platform's motion status; scheduling key operational tasks to be executed within a wave window where the motion envelope is stable, and executing cruise-type tasks according to the graded speed limit rules where the current roll angle amplitude is located.

[0012] An AGV relative navigation system, applying the above-mentioned AGV relative navigation method, includes: The platform inertial measurement unit is installed on the floating carrier and is used to acquire the platform inertial data of the floating carrier. The platform state estimation module is connected to the platform inertial measurement unit. Based on the wave kinematics model constraint, the state estimator filters and estimates the platform inertial data to obtain the platform motion state of the floating carrier. The vehicle perception module includes a lidar, an inertial measurement unit, and a wheel speed meter installed on the AGV body, which are used to acquire raw point cloud frames, vehicle inertial data, and wheel speed data, respectively. The beacon module includes multiple beacons mounted on a floating carrier, and beacon tags mounted on the AGV body for measuring distances with each beacon; The onboard computing module, connected to the platform inertial measurement unit, platform state estimation module, vehicle perception module, and beacon module, is used to generate navigation control commands for the AGV vehicle. The execution module, connected to the onboard computing module, is used to receive the navigation control commands output by the onboard computing module and drive the AGV to move on the floating carrier.

[0013] Optionally, the onboard computing module includes a relative state estimation unit, a point cloud compensation unit, a relative pose matching unit, a relative pose correction unit, and a navigation control unit. The relative state estimation unit estimates the relative motion state of the vehicle body relative to the floating carrier based on vehicle body inertial data, platform inertial data, and wheel speed data. The point cloud compensation unit determines the relative pose of the vehicle body at the scanning time corresponding to each laser point in the original point cloud frame, and performs motion compensation on each laser point according to the scanning time to obtain a distortion-free point cloud in the platform coordinate system. The relative pose matching unit matches the distortion-free point cloud with a pre-constructed relative map in the platform coordinate system to obtain the relative pose of the vehicle body. The relative pose correction unit corrects the relative pose based on the calibration coordinates of each beacon in the relative map to obtain the corrected relative pose. The navigation control unit generates navigation control commands based on the platform motion state and the corrected relative pose under the safety constraints of the moving base.

[0014] As can be seen from the above technical solutions, compared with the prior art, the present invention provides an AGV relative navigation method and system, which has the following beneficial effects: The present invention defines the map and positioning reference in the coordinate system of the floating carrier's motion, fundamentally avoiding the SLAM failure problem caused by the dynamic environment; it eliminates the matching degradation caused by point cloud distortion through point-by-point relative pose compensation; it maintains long-term stable relative positioning accuracy even under the condition that the metal compartment completely blocks satellite signals through multi-source fusion of differential inertia, wheel speed measurement and fixed beacon network; it incorporates the platform roll angle, deck friction coefficient and cargo status into speed, turning and parking constraints, effectively reducing the risk of sideslip and overturning; and it significantly improves the success rate and average waiting time of key operations such as docking with fixed equipment through wave window scheduling based on platform motion envelope prediction. Attached Figure Description

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

[0016] Figure 1 This is a flowchart of the AGV relative navigation method of the present invention; Figure 2 This is a schematic diagram of the AGV relative navigation system of the present invention. Detailed Implementation

[0017] The technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0018] This invention discloses a relative navigation method for AGVs, such as... Figure 1 As shown, it includes the following steps: S1. Obtain the platform inertial data output by the platform inertial measurement unit on the floating carrier, and filter and estimate the platform inertial data based on the state estimator constrained by the wave kinematics model to obtain the platform motion state of the floating carrier. S2. Obtain the vehicle body inertial data output by the vehicle body inertial measurement unit installed on the AGV body and the wheel speed data output by the wheel speed meter; perform differential processing on the vehicle body inertial data and the platform inertial data in a unified coordinate system, and estimate the relative motion state of the vehicle body relative to the floating carrier by combining the wheel speed data with the error state Kalman filter. S3. Obtain the original point cloud frame output by the vehicle body lidar. Determine the relative pose of the vehicle body at the scanning time corresponding to each laser point in the original point cloud frame based on the platform motion state and relative motion state. Perform motion compensation on each laser point according to the scanning time to obtain the distortion-free point cloud in the platform coordinate system. S4. Match the distortion-free point cloud with the pre-constructed relative map in the platform coordinate system, and use the matched pose measurement as the observation to update the error state Kalman filter to obtain the relative pose of the vehicle body. S5. Obtain the distance measurement measurements between multiple beacons in the beacon network on the floating carrier and the beacon tags installed on the AGV body. Based on the calibration coordinates of each beacon in the relative map, correct the relative pose to obtain the corrected relative pose. S6. Based on the platform's motion state and the corrected relative posture, navigation control commands are generated under the safety constraints of the moving base to control the AGV to travel on the floating carrier.

[0019] Furthermore, the wave kinematics model in S1 is an autoregressive moving average model, with model parameters identified from the spectral characteristics of the platform's inertial data. The state estimator is an extended Kalman filter, and the state vector includes roll angle, roll angular velocity, pitch angle, pitch angular velocity, yaw angle, yaw angular velocity, heave displacement, and heave velocity. The autoregressive moving average model serves as the state equation for the extended Kalman filter, constraining the wave kinematics model for state recursion.

[0020] In this embodiment, the origin of the platform coordinate system is taken as the deck reference point, the x-axis points along the ship's length towards the bow, the y-axis points towards the starboard side, and the z-axis is perpendicular to the deck and upwards; the platform coordinate system moves with the carrier in six degrees of freedom and is a non-inertial coordinate system; the origin of the vehicle coordinate system is the center of the vehicle body, the x-axis is along the direction of the vehicle's front, the y-axis points towards the left side of the vehicle body, and the z-axis is perpendicular to the deck and upwards; the relative pose of the vehicle body with respect to the platform is determined by the position of the origin of the vehicle body coordinate system in the platform coordinate system and the attitude angle of the vehicle body coordinate system relative to the platform coordinate system.

[0021] In this embodiment of the invention, the platform inertial measurement unit also incorporates a combined attitude and bearing reference system (AHRS), which integrates a triaxial magnetometer and provides a yaw reference after being calibrated with hard iron / soft iron.

[0022] Furthermore, S2 specifically refers to: Acquire the vehicle inertial data output by the vehicle inertial measurement unit installed on the AGV body and the wheel speed data output by the wheel speed meter; transform the angular velocity in the vehicle coordinate system output by the vehicle inertial measurement unit to the platform coordinate system: ; In the formula, Let ω be the angular velocity in the vehicle coordinate system. Let ω be the angular velocity in the platform coordinate system. Let be the rotation matrix from the vehicle coordinate system to the platform coordinate system. The angular velocity of the vehicle body relative to the platform; The ratio output from the vehicle's inertial measurement unit After deducting platform-related acceleration, Coriolis acceleration, lever arm compensation term, and gravitational component: ; In the formula, The specific force output by the platform's inertial measurement unit. For lever arm vector, The platform's angular velocity vector. The platform's angular acceleration vector. The relative velocity of the vehicle body with respect to the platform. The relative linear acceleration of the vehicle body with respect to the platform; for The relative velocity estimate is obtained by integration; the relative velocity estimate is then fused with the wheel speed meter mileage increment using an error-state Kalman filter to estimate the relative motion state. The state vector of the error-state Kalman filter is derived from the vehicle's position, velocity, heading angle relative to the platform, and the gyro and accelerometer zero biases of the vehicle's inertial measurement unit.

[0023] Furthermore, S3 specifically refers to: Obtain the original point cloud frame output by the vehicle's LiDAR, and let the first... The reference time of the original point cloud frame is , within the frame The scanning time of each laser point is The motion compensation formula is: ; In the formula, For pose transformation composite operations, for Momentary laser point Coordinates in the vehicle coordinate system for Momentary laser point Coordinates in the vehicle coordinate system for Predicted relative pose at any given time. for The predicted relative pose at each scan time is obtained by interpolating the platform motion state and the relative motion state over time.

[0024] Furthermore, S4 specifically refers to: Using the predicted relative pose of the original point cloud frame at the reference time as the initial pose, the distortion-free point cloud is matched with the pre-constructed relative map in the platform coordinate system using a normal distribution transformation to obtain the pose measurement value at the reference time. The pose measurement value is fed back as an observation to S2 to estimate the relative motion state of the vehicle body relative to the floating carrier. The result is fused with the predicted relative pose at the reference time to obtain the relative pose of the vehicle body. The pose measurement value is used to update the filter in the form of position and heading observations.

[0025] In this embodiment, the relative map is constructed based on the platform coordinate system. Map elements are taken from structural features such as bulkheads, coamings, hatch coamings, guide rails, supports, and passageway markers that are fixed to the floating platform. During the stationary period when the floating platform is stationary (during berthing or dock repair, when the platform's motion amplitude is below a preset limit), an initial relative map is established using conventional synchronous positioning and mapping methods. During the motion period when the floating platform is in motion, the relative map is incrementally updated using distortion-free point clouds. Before updating, dynamic objects not fixed to the floating platform, such as personnel, cargo, and tooling, are removed through semantic segmentation to avoid map contamination. The relative map is stored and loaded by compartment or deck partition. The calibration coordinates of each beacon in the relative map are obtained by calibrating the beacon installation position using a total station during the stationary period of the floating platform and written into the relative map storage.

[0026] Furthermore, S5 corrects the relative pose based on the calibration coordinates of each beacon in the relative map, specifically as follows: No. The distance measurement equation for each beacon is: ; In the formula, For beacon tags and the first Distance values ​​between beacons The position of the vehicle body in the platform coordinate system. The heading angle of the vehicle body relative to the platform. This is the rotation matrix corresponding to the heading angle. This refers to the installation offset vector of the beacon tag in the vehicle coordinate system. For the first The coordinates of a beacon on a relative map. To reduce ranging noise, the relative pose correction is calculated using the weighted least squares method. The correction weights are determined based on the variance of each beacon ranging observation and the non-line-of-sight detection results. Non-line-of-sight detection is based on the first-path energy ratio of the impulse response of the ultra-wideband signal channel.

[0027] In this embodiment, the relative pose correction is calculated using the weighted least squares method. : ; In the formula, Let be the Jacobian matrix of the ranging equations with respect to relative pose. For the ranging residual vector, The weight matrix is ​​denoted by the reciprocal of the variance of each beacon ranging observation. Non-line-of-sight detection is based on the first-path energy ratio of the impulse response of the ultra-wideband signal channel. Beacon observations with a first-path energy ratio lower than a preset threshold (e.g., 0.5) are eliminated or downweighted. The corrected values ​​are superimposed on the relative pose to obtain the corrected relative pose.

[0028] Furthermore, in S6, based on the platform's motion state and the corrected relative pose, navigation control commands are generated under the safety constraints of the moving base, specifically as follows: Based on the roll angle amplitude during platform motion, graded speed limits are implemented: when the roll angle amplitude does not exceed the first threshold, the AGV is controlled to travel at the first speed limit; when the roll angle amplitude exceeds the first threshold but does not exceed the second threshold, the AGV is controlled to travel at the second speed limit, which is lower than the first speed limit, and lateral movement along the ship's width is prohibited; when the roll angle amplitude exceeds the second threshold but does not exceed the third threshold, the AGV is controlled to travel along the ship's length at the third speed limit, which is lower than the second speed limit; when the roll angle amplitude exceeds the third threshold, the AGV is controlled to brake and park, and the parking mechanism is locked. Based on the online estimates of the roll angle and deck friction coefficient during platform motion, and the AGV's cargo loading status, the maximum allowable lateral acceleration, maximum allowable speed, and minimum turning radius are calculated according to lateral force balance constraints and incorporated into the local path planning; whereby the lateral force balance constraints satisfy: ,in, For the lateral acceleration of the vehicle body, This is an online estimate of the deck friction coefficient. It is the acceleration due to gravity. This is the roll angle.

[0029] In this embodiment, the first threshold is 5°, the second threshold is 10°, and the third threshold is 14°. The first speed limit is 1.2 m / s, the second speed limit is 0.6 m / s, and the third speed limit is 0.3 m / s. When the roll angle amplitude exceeds the third threshold or the heave acceleration exceeds the preset limit, the AGV is controlled to brake and park and the parking mechanism is locked.

[0030] Furthermore, S6 also includes: predicting the motion envelope of the floating carrier within a preset time period based on the platform's motion status; scheduling key operational tasks to be executed within a wave window where the motion envelope is stable, and executing cruise-type tasks according to the graded speed limit rules where the current roll angle amplitude is located.

[0031] In this embodiment, based on the platform's motion state, an autoregressive model is used to extrapolate and predict the motion envelope of the floating carrier within a preset future time period. When the predicted envelope satisfies that the roll angle amplitude does not exceed 3° and the duration is not less than 45s, the time period is determined to be a stable wave window. Key operational tasks (such as docking with fixed conveyor equipment, cargo handling, and cross-hatch transfer) are scheduled to be executed within the wave window, while cruise-type tasks are executed according to the graded speed limit rules where the current roll angle amplitude is located.

[0032] and Figure 1 Corresponding to the method described above, this embodiment of the invention also discloses an AGV relative navigation system, applying the aforementioned AGV relative navigation method, such as... Figure 2 As shown, it includes: The platform inertial measurement unit is installed on the floating carrier and is used to acquire the platform inertial data of the floating carrier. The platform state estimation module is connected to the platform inertial measurement unit. Based on the wave kinematics model constraint, the state estimator filters and estimates the platform inertial data to obtain the platform motion state of the floating carrier. The vehicle perception module includes a lidar, an inertial measurement unit, and a wheel speed meter installed on the AGV body, which are used to acquire raw point cloud frames, vehicle inertial data, and wheel speed data, respectively. The beacon module includes multiple beacons mounted on a floating carrier, and beacon tags mounted on the AGV body for measuring distances with each beacon; The onboard computing module, connected to the platform inertial measurement unit, platform state estimation module, vehicle perception module, and beacon module, is used to generate navigation control commands for the AGV vehicle. The execution module, connected to the onboard computing module, is used to receive the navigation control commands output by the onboard computing module and drive the AGV to move on the floating carrier.

[0033] Furthermore, the onboard computing module includes a relative state estimation unit, a point cloud compensation unit, a relative pose matching unit, a relative pose correction unit, and a navigation control unit. The relative state estimation unit estimates the relative motion state of the vehicle body relative to the floating carrier based on vehicle body inertial data, platform inertial data, and wheel speed data. The point cloud compensation unit determines the relative pose of the vehicle body at the scanning time corresponding to each laser point in the original point cloud frame, and performs motion compensation on each laser point according to the scanning time to obtain a distortion-free point cloud in the platform coordinate system. The relative pose matching unit matches the distortion-free point cloud with a pre-constructed relative map in the platform coordinate system to obtain the relative pose of the vehicle body. The relative pose correction unit corrects the relative pose based on the calibration coordinates of each beacon in the relative map to obtain the corrected relative pose. The navigation control unit generates navigation control commands based on the platform motion state and the corrected relative pose under the safety constraints of the moving base.

[0034] The various embodiments in this specification are described in a progressive manner, with each embodiment focusing on its differences from other embodiments. Similar or identical parts between embodiments can be referred to interchangeably. For the systems disclosed in the embodiments, since they correspond to the methods disclosed in the embodiments, the descriptions are relatively simple; relevant parts can be referred to the method section.

[0035] Various modifications to these embodiments will be readily apparent to those skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the invention. Therefore, the invention is not to be limited to the embodiments shown herein, but is to be accorded the widest scope consistent with the principles and novel features disclosed herein.

Claims

1. A relative navigation method for AGVs, characterized in that, Includes the following steps: S1. Obtain the platform inertial data output by the platform inertial measurement unit on the floating carrier, and filter and estimate the platform inertial data based on the state estimator constrained by the wave kinematics model to obtain the platform motion state of the floating carrier. S2. Obtain the vehicle body inertial data output by the vehicle body inertial measurement unit installed on the AGV body and the wheel speed data output by the wheel speed meter; perform differential processing on the vehicle body inertial data and the platform inertial data in a unified coordinate system, and estimate the relative motion state of the vehicle body relative to the floating carrier by combining the wheel speed data with the error state Kalman filter. S3. Obtain the original point cloud frame output by the vehicle body lidar. Determine the relative pose of the vehicle body at the scanning time corresponding to each laser point in the original point cloud frame based on the platform motion state and relative motion state. Perform motion compensation on each laser point according to the scanning time to obtain the distortion-free point cloud in the platform coordinate system. S4. Match the distortion-free point cloud with the pre-constructed relative map in the platform coordinate system, and use the matched pose measurement as the observation to update the error state Kalman filter to obtain the relative pose of the vehicle body. S5. Obtain the distance measurement measurements between multiple beacons in the beacon network on the floating carrier and the beacon tags installed on the AGV body. Based on the calibration coordinates of each beacon in the relative map, correct the relative pose to obtain the corrected relative pose. S6. Based on the platform's motion state and the corrected relative posture, navigation control commands are generated under the safety constraints of the moving base to control the AGV to travel on the floating carrier.

2. The AGV relative navigation method according to claim 1, characterized in that, The wave kinematics model in S1 is an autoregressive moving average model, and the model parameters are identified by the spectral characteristics of the platform inertial data. The state estimator is an extended Kalman filter, and the state vector includes roll angle, roll angular velocity, pitch angle, pitch angular velocity, yaw angle, yaw angular velocity, heave displacement, and heave velocity.

3. The AGV relative navigation method according to claim 1, characterized in that, S2 specifically refers to: Acquire the vehicle inertial data output by the vehicle inertial measurement unit installed on the AGV body and the wheel speed data output by the wheel speed meter; transform the angular velocity in the vehicle coordinate system output by the vehicle inertial measurement unit to the platform coordinate system: ； In the formula, Let ω be the angular velocity in the vehicle coordinate system. Let ω be the angular velocity in the platform coordinate system. Let be the rotation matrix from the vehicle coordinate system to the platform coordinate system. The angular velocity of the vehicle body relative to the platform; The ratio output from the vehicle's inertial measurement unit After deducting platform-related acceleration, Coriolis acceleration, lever arm compensation term, and gravitational component: ； In the formula, The specific force output by the platform's inertial measurement unit. For lever arm vector, The platform's angular velocity vector. The platform's angular acceleration vector. The relative velocity of the vehicle body with respect to the platform. The relative linear acceleration of the vehicle body with respect to the platform; for The relative velocity estimate is obtained by integration; the relative velocity estimate is then fused with the wheel speed meter mileage increment using an error state Kalman filter to estimate the relative motion state.

4. The AGV relative navigation method according to claim 1, characterized in that, S3 specifically refers to: Obtain the original point cloud frame output by the vehicle's LiDAR, and let the first... The reference time of the original point cloud frame is , within the frame The scanning time of each laser point is The motion compensation formula is: ； In the formula, For pose transformation composite operations, for Momentary laser point Coordinates in the vehicle coordinate system for Momentary laser point Coordinates in the vehicle coordinate system for Predicted relative pose at any given time. for The predicted relative pose at each scan time is obtained by interpolating the platform motion state and the relative motion state over time.

5. The AGV relative navigation method according to claim 1, characterized in that, S4 specifically refers to: Using the predicted relative pose at the reference time of the original point cloud frame as the initial pose, the distortion-free point cloud is matched with the relative map in the pre-constructed platform coordinate system by performing a normal distribution transformation to obtain the pose measurement value at the reference time. The pose measurement value is fed back to S2 as an observation to estimate the relative motion state of the vehicle body relative to the floating carrier. The result is fused with the predicted relative pose at the reference time to obtain the relative pose of the vehicle body.

6. The AGV relative navigation method according to claim 1, characterized in that, S5 corrects the relative pose based on the calibration coordinates of each beacon in the relative map, specifically as follows: No. The distance measurement equation for each beacon is: ； In the formula, For beacon tags and the first Distance values ​​between beacons The position of the vehicle body in the platform coordinate system. The heading angle of the vehicle body relative to the platform. This is the rotation matrix corresponding to the heading angle. This refers to the installation offset vector of the beacon tag in the vehicle coordinate system. For the first The coordinates of a beacon on a relative map. To reduce ranging noise, the relative pose correction is calculated using the weighted least squares method. The correction weights are determined based on the variance of each beacon ranging observation and the non-line-of-sight detection results. Non-line-of-sight detection is based on the first-path energy ratio of the impulse response of the ultra-wideband signal channel.

7. The AGV relative navigation method according to claim 1, characterized in that, In S6, navigation control commands are generated based on the platform's motion state and the corrected relative pose, under the safety constraints of the moving base. Specifically, these commands are: Based on the roll angle amplitude during platform motion, graded speed limits are implemented: when the roll angle amplitude does not exceed the first threshold, the AGV is controlled to travel at the first speed limit; when the roll angle amplitude exceeds the first threshold but does not exceed the second threshold, the AGV is controlled to travel at the second speed limit, which is lower than the first speed limit, and lateral movement along the ship's width is prohibited; when the roll angle amplitude exceeds the second threshold but does not exceed the third threshold, the AGV is controlled to travel along the ship's length at the third speed limit, which is lower than the second speed limit; when the roll angle amplitude exceeds the third threshold, the AGV is controlled to brake and park, and the parking mechanism is locked. Based on the online estimates of the roll angle and deck friction coefficient during platform motion, and the AGV's cargo loading status, the maximum allowable lateral acceleration, maximum allowable speed, and minimum turning radius are calculated according to lateral force balance constraints and incorporated into the local path planning; whereby the lateral force balance constraints satisfy: ,in, For the lateral acceleration of the vehicle body, This is an online estimate of the deck friction coefficient. It is the acceleration due to gravity. This is the roll angle.

8. The AGV relative navigation method according to claim 1, characterized in that, S6 also includes: predicting the motion envelope of the floating carrier within a preset time period based on the platform's motion status; scheduling key operational tasks to be executed within a wave window where the motion envelope is stable, and executing cruise-type tasks according to the graded speed limit rules where the current roll angle amplitude is located.

9. An AGV relative navigation system, characterized in that, An AGV relative navigation method according to any one of claims 1-8 includes: The platform inertial measurement unit is installed on the floating carrier and is used to acquire the platform inertial data of the floating carrier. The platform state estimation module is connected to the platform inertial measurement unit. Based on the wave kinematics model constraint, the state estimator filters and estimates the platform inertial data to obtain the platform motion state of the floating carrier. The vehicle perception module includes a lidar, an inertial measurement unit, and a wheel speed meter installed on the AGV body, which are used to acquire raw point cloud frames, vehicle inertial data, and wheel speed data, respectively. The beacon module includes multiple beacons mounted on a floating carrier, and beacon tags mounted on the AGV body for measuring distances with each beacon; The onboard computing module, connected to the platform inertial measurement unit, platform state estimation module, vehicle perception module, and beacon module, is used to generate navigation control commands for the AGV vehicle. The execution module, connected to the onboard computing module, is used to receive the navigation control commands output by the onboard computing module and drive the AGV to move on the floating carrier.

10. An AGV relative navigation system according to claim 9, characterized in that, The onboard computing module includes a relative state estimation unit, a point cloud compensation unit, a relative pose matching unit, a relative pose correction unit, and a navigation control unit. The relative state estimation unit estimates the relative motion state of the vehicle body relative to the floating carrier based on vehicle body inertial data, platform inertial data, and wheel speed data. The point cloud compensation unit determines the relative pose of the vehicle body at the scanning time corresponding to each laser point in the original point cloud frame, and performs motion compensation on each laser point according to the scanning time to obtain a distortion-free point cloud in the platform coordinate system. The relative pose matching unit matches the distortion-free point cloud with a pre-constructed relative map in the platform coordinate system to obtain the relative pose of the vehicle body. The relative pose correction unit corrects the relative pose based on the calibration coordinates of each beacon in the relative map to obtain the corrected relative pose. The navigation control unit generates navigation control commands based on the platform motion state and the corrected relative pose under the safety constraints of the moving base.

Images