Search

CN-121657687-B - Unmanned ship docking automatic hooking control system, method, equipment and medium

CN121657687BCN 121657687 BCN121657687 BCN 121657687BCN-121657687-B

Abstract

The invention discloses a system, a method, equipment and a medium for controlling automatic hooking of unmanned ships in docking, which relate to the technical field of automatic control of unmanned ships and comprise the steps of establishing a wharf coordinate system frame based on wharf control point coordinates acquired by a surveying instrument, and (3) carrying out calibration delay calculation through a UWB anchor node fixed on the wharf side, acquiring the coordinate calculation heading of the unmanned ship through a dual-antenna RTK, converting the coordinate calculation heading into a wharf heading, and simultaneously recording the rigid bias from the UWB ranging end to the geometric center of the hook. According to the method, the plane coordinates of the UWB ranging end under the wharf coordinate system are solved by using the linearization circular equation, geometric residual calculation and maximum ranging error constraint are introduced, closed loop from multi-anchor redundant ranging to positioning reliability screening is realized, the problem that the traditional global coordinate path planning is not matched with the physical space is solved, and the errors in the dynamic berthing and complex reflection environments are reduced.

Inventors

  • MA WEI
  • ZHAO JIUCHENG
  • Diao Yanyu

Assignees

  • 天津国兴海洋能源工程有限公司

Dates

Publication Date
20260512
Application Date
20260206

Claims (10)

  1. 1. An unmanned ship docking automatic hook control system, comprising: the UWB calibration module is used for establishing a wharf coordinate system frame based on wharf control point coordinates acquired by a surveying instrument, performing calibration delay calculation through a wharf side fixed UWB anchor node, acquiring a coordinate calculation heading of the unmanned ship through a dual-antenna RTK, converting the heading into a wharf system heading, simultaneously recording the rigid bias from a UWB ranging end to a geometric center of a hook, calculating a three-dimensional distance according to real-time round trip delay, response delay and calibration delay by using a UWB ranging technology, deriving a plane distance based on the anchor node height and the ranging end height, further solving the plane coordinate of the UWB ranging end in the wharf coordinate system through a linearization round equation, and calculating a geometric residual error to perform positioning effective identification; The attitude planning module converts and translates the rigid bias to a ranging end position through a rotation matrix formed by the wharf system heading on the basis of effective positioning to obtain a measurement consistent coordinate of a hook center, calculates relative displacement with a target hanging point coordinate, decomposes the measurement consistent coordinate into a longitudinal error and a transverse error along a wharf coordinate system, processes the errors of the wharf system heading and the target heading through angle normalization to form a relative attitude set, discretizes and plans the relative attitude set, calculates planning steps based on the allowable closed maximum speed and a sampling period of the unmanned ship, and generates an attitude stack containing each step of target position; the safety constraint module establishes a far-end and near-end transverse allowable boundary according to unmanned ship acquisition parameters, dynamically divides constraint grades according to the relation between the longitudinal distance of each step in the gesture stack and the length of a wharf funnel, and determines an applicable allowable boundary; And the gesture verification module is used for verifying each step of the gesture stack, executing saturation cutting operation aiming at transverse errors, forming a control interface data set based on the corrected gesture stack and the current actual measured gesture of the unmanned ship, and executing position control by using the control interface data set through the position controller.
  2. 2. The unmanned aerial vehicle docking automatic hooking control system of claim 1, wherein the calculating the three-dimensional distance according to the real-time round trip delay, the response delay and the calibration delay, deriving the plane distance based on the anchor node height and the ranging end height, solving the plane coordinate of the UWB ranging end in the wharf coordinate system through the linearization round equation, calculating the geometric residual error for positioning effective identification comprises, Acquiring control point coordinates of a wharf, fixed UWB anchor nodes and on-board sensing data of an unmanned ship wharf through a surveying instrument; calculating the azimuth angle of the wharf axis according to the coordinates of the double control points of the wharf edge; the origin coordinates of the two control points are selected to be x-axis through the direction of the coordinates of the origin points to the coordinates of the other control point, and the left direction is y-axis, so as to construct a wharf coordinate system; calculating the calibration delay of each anchor point according to the three-dimensional coordinates of the three fixed UWB anchor nodes, the response delay of the surveying instrument and the round trip delay acquired at the known reference points; Calculating a heading based on two antenna coordinates given by the unmanned ship through the dual-antenna RTK, and recording the rigid bias of the unmanned ship from the geometric center of the hook and the height of the UWB ranging end according to the on-board UWB ranging end of the unmanned ship; calculating a three-dimensional distance according to the real-time round trip delay, the response delay and the calibration delay, and calculating a plane distance according to the height of the UWB anchor node and the height of the UWB ranging end; And constructing a linearization round equation from the UWB ranging end to the UWB anchor node based on the position of the UWB anchor node in the wharf coordinate system and the plane distance, solving and calculating the coordinate of the UWB ranging end in the wharf coordinate system, calculating a geometric residual error, judging that the positioning is effective if the calculated geometric residual error is smaller than or equal to the maximum ranging error according to the maximum ranging error of the UWB ranging end, and outputting the plane coordinate and the geometric residual error of the UWB ranging end as effective identifications.
  3. 3. The unmanned aerial vehicle docking automatic hooking control system of claim 2, wherein the wharf-oriented coordinate system is decomposed into a longitudinal error and a transverse error, the errors of wharf-oriented heading and target heading are processed through angle normalization to form a relative pose set, the relative pose set is subjected to discretization planning, the number of planning steps is calculated based on the maximum allowable closing speed and the sampling period of the unmanned aerial vehicle, and a pose stack containing each step of target position is generated, and the unmanned aerial vehicle docking automatic hooking control system comprises, After the positioning is judged to be effective, coordinate transformation and error decomposition are carried out based on the wharf system heading, the plane coordinates of the UWB ranging end, the rigid bias of the geometric center of the hook and the coordinates of the target hanging point, a rotation matrix is formed through the wharf system heading, the rigid bias of the geometric center of the hook is combined with the wharf system heading, and the combination is translated to the position of the UWB ranging end, so that the measurement consistent coordinates of the center of the hook are obtained; Calculating a relative displacement vector according to the difference value of the measured consistent coordinates and the target hanging point coordinates, and carrying out error decomposition along the transverse and longitudinal directions of the wharf according to the relative displacement vector to calculate a transverse error and a longitudinal error; Calculating a heading error of the wharf system and the target heading through angle normalization, and forming a relative pose set by the longitudinal error, the transverse error and the heading error; Discretizing according to the relative pose set, and performing parameterization and interpolation construction through the allowable closing maximum speed and sampling period of the unmanned ship to generate a pose stack; Defining that the longitudinal propulsion of the unmanned ship in each sampling period does not exceed the upper limit of speed, and calculating the planning step number; For evenly converging longitudinal errors in the planning steps, calculating longitudinal steps and normalization progress, calculating longitudinal/transverse target amounts and heading target amounts of different planning steps, and calculating target positions of each step by combining a wharf coordinate system and target hanging point coordinates; and forming a gesture stack according to the target position and the heading target quantity of each step.
  4. 4. The unmanned aerial vehicle docking automatic hook control system of claim 3, wherein the dynamically grading constraints based on the longitudinal distance of each step in the gesture stack versus dock funnel length, determining the applicable tolerance boundaries comprises, Based on the unmanned ship and the wharf, respectively acquiring the width of the ship body, the minimum transverse clearance of the wharf, the transverse measurement error limit, the inner width of a wharf funnel inlet and the axial length of the funnel; establishing a transverse allowable boundary of a far end and a near end for the gesture stack; Based on the transverse allowable boundaries of the far end and the near end, constraint grades are divided according to the distance, the constraint grades are divided according to the longitudinal distance of each step in the gesture stack and the length of the wharf funnel, if the longitudinal distance of each step is larger than the length of the wharf funnel, the allowable boundary is defined as the far-end allowable boundary, and if the longitudinal distance of each step is smaller than or equal to the length of the wharf funnel, the allowable boundary is defined as the near-end allowable boundary.
  5. 5. The unmanned aerial vehicle docking automatic hook control system of claim 4, wherein each step of the gesture stack is verified, performing a saturation clipping operation for lateral errors comprises, For each step of the unmanned ship based on the gesture stack, an allowable boundary is defined and saturation clipping is performed for lateral errors of out-of-range steps.
  6. 6. The unmanned aerial vehicle docking automatic hook control system of claim 5, wherein the forming a control interface data set based on the corrected attitude stack and the current measured attitude of the unmanned aerial vehicle comprises, Forming a current actual measurement gesture based on the actual measurement position and the heading of the unmanned ship, and calculating errors and constraint parameters of a gesture stack subjected to saturation cutting and current actual measurement gesture data of the unmanned ship; And determining the current trackable position and heading error, and defining the current longitudinal step distance and the maximum allowable closing speed of the current unmanned ship as a real-time boundary to form a control interface data set.
  7. 7. The unmanned aerial vehicle docking automatic hook control system of claim 6, wherein the performing position control by the position controller using the control interface data set comprises, And according to the control interface data set, performing position control through the current trackable position and the current heading error through a position controller of the unmanned ship, and performing speed and acceleration limitation through the current longitudinal step and the allowable closing maximum speed of the front unmanned ship.
  8. 8. An unmanned ship docking automatic hooking control method based on the unmanned ship docking automatic hooking control system of any one of claims 1-7 is characterized by comprising the following steps, Establishing a wharf coordinate system frame based on wharf control point coordinates acquired by a surveying instrument, performing calibration delay calculation through a wharf side fixed UWB anchor node, acquiring a coordinate calculation heading of an unmanned ship through a dual-antenna RTK, converting the coordinate calculation heading into the wharf system heading, simultaneously recording rigid bias from a UWB ranging end to a geometric center of a hook, calculating a three-dimensional distance according to real-time round trip delay, response delay and calibration delay by using a UWB ranging technology, deriving a plane distance based on the anchor node height and the ranging end height, solving the plane coordinate of the UWB ranging end in the wharf coordinate system through a linearization round equation, and calculating a geometric residual error to perform positioning effective identification; On the basis of effective positioning, converting and translating rigid bias to a distance measuring end position through a rotation matrix formed by a wharf system heading, obtaining a measurement consistent coordinate of a hook center, calculating relative displacement with a target hanging point coordinate, decomposing the coordinate into a longitudinal error and a transverse error along the wharf coordinate system, meanwhile, processing errors of the wharf system heading and the target heading through angle normalization to form a relative pose set, carrying out discretization planning on the relative pose set, calculating planning steps based on maximum allowable closing speed and sampling period of an unmanned ship, and generating a pose stack containing target positions of each step; Establishing a transverse allowable boundary of a far end and a near end according to unmanned ship acquisition parameters, dynamically dividing constraint grades according to the relation between the longitudinal distance of each step in a gesture stack and the length of a wharf funnel, and determining an applicable allowable boundary; and checking each step of the gesture stack, performing saturation cutting operation aiming at transverse errors, forming a control interface data set based on the corrected gesture stack and the current measured gesture of the unmanned ship, and performing position control by using the control interface data set through a position controller.
  9. 9. A computer device comprising a memory and a processor, the memory storing a computer program, wherein the processor, when executing the computer program, performs the steps of the unmanned aerial vehicle docking automatic hook control method of claim 8.
  10. 10. A computer-readable storage medium, on which a computer program is stored, characterized in that the computer program, when executed by a processor, implements the steps of the unmanned aerial vehicle docking automatic hook control method of claim 8.

Description

Unmanned ship docking automatic hooking control system, method, equipment and medium Technical Field The invention relates to the technical field of unmanned ship automatic control, in particular to an unmanned ship docking automatic hook control system, method, equipment and medium. Background The unmanned ship berthing is currently dependent on visual identification, laser radar or high-precision RTK positioning means, target identification and path planning are realized through image or point cloud matching, however, in a wharf environment, due to factors such as illumination change, water surface reflection, shielding, complex wharf structure and the like, the stability of visual and laser ranging is limited, so that target identification drifting, path deviation and end collision risks are easy to occur in the berthing process, the existing RTK or GNSS scheme can provide higher positioning precision, but the problems of multipath interference and shielding unlocking of signals exist in a harbor steel structure or shielding environment, and the relative pose of the unmanned ship relative to wharf target points is difficult to be directly reflected; Aiming at the problems, research has been attempted to realize the relative position measurement between a wharf and an unmanned ship by using Ultra Wide Band (UWB) ranging technology, but the conventional UWB positioning scheme is mostly based on a static scene or a two-dimensional positioning model, and system time delay calibration, altitude difference compensation and ranging residual error screening mechanisms between an anchor node and a ranging end are not fully considered, so that errors in dynamic berthing and complex reflection environments are unstable. Disclosure of Invention The present invention has been made in view of the above-described problems occurring in the prior art. Therefore, the invention provides an unmanned ship docking automatic hooking control system, method, equipment and medium, which solve the problems that the conventional UWB positioning scheme is based on a static scene or a two-dimensional positioning model, the system time delay calibration, the height difference compensation and the ranging residual error screening mechanism between an anchor node and a ranging end are not fully considered, so that errors in a dynamic berthing and complex reflection environment are unstable, in addition, the conventional path planning algorithm is based on a global coordinate system for path optimization, the geometric characteristics of a wharf and the physical constraint of the unmanned ship are not combined, the planned path and the actual physical space are not matched easily, and the terminal alignment precision and the berthing success rate are affected. In order to solve the technical problems, the invention provides the following technical scheme: In a first aspect, the present invention provides an unmanned aerial vehicle docking automatic hook control system comprising: the UWB calibration module is used for establishing a wharf coordinate system frame based on wharf control point coordinates acquired by a surveying instrument, performing calibration delay calculation through a wharf side fixed UWB anchor node, acquiring a coordinate calculation heading of the unmanned ship through a dual-antenna RTK, converting the heading into a wharf system heading, simultaneously recording the rigid bias from a UWB ranging end to a geometric center of a hook, calculating a three-dimensional distance according to real-time round trip delay, response delay and calibration delay by using a UWB ranging technology, deriving a plane distance based on the anchor node height and the ranging end height, further solving the plane coordinate of the UWB ranging end in the wharf coordinate system through a linearization round equation, and calculating a geometric residual error to perform positioning effective identification; The attitude planning module converts and translates the rigid bias to a ranging end position through a rotation matrix formed by the wharf system heading on the basis of effective positioning to obtain a measurement consistent coordinate of a hook center, calculates relative displacement with a target hanging point coordinate, decomposes the measurement consistent coordinate into a longitudinal error and a transverse error along a wharf coordinate system, processes the errors of the wharf system heading and the target heading through angle normalization to form a relative attitude set, discretizes and plans the relative attitude set, calculates planning steps based on the allowable closed maximum speed and a sampling period of the unmanned ship, and generates an attitude stack containing each step of target position; the safety constraint module establishes a far-end and near-end transverse allowable boundary according to unmanned ship acquisition parameters, dynamically divides constraint grades according to the relation between