A collaborative control method for an amphibious unmanned platform vehicle

Abstract

A method for collaborative control of an amphibious unmanned platform vehicle belongs to the technical field of amphibious unmanned platforms and solves the problem of the lack of collaborative control methods for vehicles under different working conditions in the existing technology. The method includes: constructing a system thread, a state machine thread, a timer thread, and an emergency stop monitoring thread; the system thread is used to receive control instructions, heartbeat signals, and status information, send the control instructions and status information to the state machine thread, and send the heartbeat signal to the timer thread and the emergency stop monitoring thread; the emergency stop monitoring thread and the timer thread are used to determine whether to trigger an emergency stop using a first method and a second method based on the heartbeat signal respectively; the state machine thread is used to send status information to the autonomous domain; based on the control instruction, it is determined whether to switch the working condition, and if so, the working condition switching control is executed, otherwise the autonomous control corresponding to the historical working condition is performed; the working conditions include water working conditions, land working conditions, and water-land working conditions. Collaborative control of actuators under different working conditions of the amphibious unmanned platform is achieved.

Description

Technical Field

[0001] The present invention relates to the technical field of amphibious unmanned platforms, and in particular to a whole-vehicle collaborative control method for an amphibious unmanned platform. Background Art

[0002] Amphibious unmanned platforms must operate in a variety of different operating conditions, including on land, water, and water. They carry two sets of relatively independent yet coupled actuators, one on land and one on water. Each operating condition places significant demands on the control of each actuator. Managing the actuators appropriately is crucial to the stable operation of the amphibious unmanned platform. Existing solutions primarily focus on controlling a single operating condition, failing to fully consider the interdependencies and differences between the various operating conditions of an amphibious platform, and thus failing to achieve overall coordination between the actuators. Summary of the Invention

[0003] In view of the above analysis, an embodiment of the present invention aims to provide a whole-vehicle collaborative control method for an amphibious unmanned platform, so as to solve the problem of the existing lack of a whole-vehicle collaborative control method for an amphibious unmanned platform under different working conditions.

[0004] In one aspect, an embodiment of the present invention provides a method for coordinated control of an amphibious unmanned platform vehicle, comprising the following steps:

[0005] Build system threads, state machine threads, timer threads and emergency stop monitoring threads;

[0006] The system thread is used to receive control instructions, heartbeat signals and status information of the actuator, send the control instructions and status information to the state machine thread, and send the heartbeat signal of the actuator to the timer thread and the emergency stop monitoring thread;

[0007] The emergency stop monitoring thread is used to determine whether to trigger an emergency stop in a first manner according to the heartbeat signals of the executor, the system thread, the state machine thread and the timer thread;

[0008] The timer thread is used to determine whether to trigger an emergency stop in a second manner according to the heartbeat signals of the executor, the system thread, the state machine thread and the emergency stop monitoring thread;

[0009] The state machine thread is used to send status information to the autonomous domain; determine whether to switch the working condition according to the control instruction, and if so, execute the working condition switching control; otherwise, perform autonomous control corresponding to the historical working condition; the working conditions include water working conditions, land working conditions and water-land working conditions.

[0010] Based on the further improvement of the above method, the state machine performs the working mode switching control in the following manner:

[0011] Determine whether the current switching conditions are met based on the previous working condition and the target working condition. If not, trigger an emergency stop. Otherwise, determine the current switching type based on the previous working condition and the target working condition.

[0012] Adjust the slide's retraction and extension angle according to the switching type;

[0013] Whether the switching is successful is determined based on the switching type. If successful, the autonomous control corresponding to the target working condition is executed; otherwise, the autonomous control corresponding to the historical working condition is executed.

[0014] Based on the further improvement of the above method, the retraction angle of the slide is adjusted according to the switching type, including:

[0015] If the switching type is from land-water working condition to water working condition, the target angle of water working condition is directly assigned to the bow upper slide, bow lower slide and tail slide;

[0016] If the switching type is from water-land working condition to land working condition, from land working condition to water-land working condition, or from water working condition to water-land working condition, then:

[0017] S21, determining whether the first upper slide has been adjusted to the target angle corresponding to the target working condition at the current moment, if so, the first step flag position is set to true, otherwise it is set to false; determining whether the first lower slide has been retracted to the target angle corresponding to the target working condition at the current moment, if so, the second step flag position is set to true, otherwise it is set to false;

[0018] S22. If the flags of the first and second steps are both false, the upper slide is assigned an intermediate angle corresponding to the target working condition, and the lower slide continues to be assigned the angle value fed back by the current sensor. The process returns to step S21. Otherwise, the process proceeds to step S23.

[0019] S23. If the first step flag is true and the second step flag is false, the upper slide is assigned an intermediate angle corresponding to the target working condition, and the lower slide is assigned a target angle corresponding to the target working condition, and the process returns to step S21; otherwise, the process proceeds to step S24.

[0020] S24. If the flags of the first and second steps are both true, the target angle corresponding to the target working condition is assigned to the first upper slide, the target angle corresponding to the target working condition is assigned to the first lower slide, and the target angle corresponding to the target working condition is assigned to the tail slide. Otherwise, an emergency stop is triggered.

[0021] Based on the further improvement of the above method, the following method is used to determine whether the switching is successful:

[0022] If the switching type is from land to water, from water to land, or from water to water, then determine whether the clutch is engaged, the shutters are closed, the vehicle posture adjustment state is suspended, and the front upper slide, front lower slide, and tail slide have all reached the target angle. If so, the switching is successful, the current working condition is set as the target working condition, and the working condition switching flag is set to false. Otherwise, the working condition switching flag is set to true.

[0023] If the switching type is switching from water and land working conditions to land working conditions, then determine whether the clutch is engaged, whether the vehicle posture adjustment state is in suspension state, and whether the front upper slide, front lower slide and tail slide have all reached the target angle. If all are yes, the switching is successful, the current working condition is set to the target working condition, and the working condition switching flag is set to false. Otherwise, the working condition switching flag is set to true.

[0024] Based on the further improvement of the above method, the state machine adopts the following method to perform autonomous control corresponding to the water working conditions:

[0025] Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise:

[0026] Set the parking command to not park; send the actuator status information to the autonomous domain;

[0027] If the left and right water gate control instructions in the received control instructions are invalid, the left and right water gate actions are stopped; if the left and right water gate control instructions are open, the water maneuvering control enable is set to enable mode, the water maneuvering control mode is set to direct action mode, and the left and right water gates are opened; if the left and right water gate control instructions are closed, the left and right water gates are closed;

[0028] If the control skateboard angle in the received control instruction is enabled, the water maneuvering control mode is set to the target angle instruction mode, and the skateboard is controlled according to the skateboard angle value in the control instruction.

[0029] Based on the further improvement of the above method, the state machine performs autonomous control corresponding to the onshore working conditions in the following manner:

[0030] Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise:

[0031] If the desired speed in the control command is 0, the parking command is set to the parking state, the gear is set to neutral, and the desired speed and the desired curvature on the water are both set to 0;

[0032] If the desired speed in the control command is greater than 0, the parking command is set to not park, and if the land clutch is disengaged, the gear position is set to neutral, and the desired speed and curvature are set to 0; if the land clutch is engaged, the gear position is set to forward gear;

[0033] If the expected speed is less than 0, the parking command is set to no parking and the gear is set to reverse;

[0034] The parking command, gear position, desired speed, autonomous speed limit, desired land curvature, and desired acceleration are sent to the chassis actuator.

[0035] Based on the further improvement of the above method, the state machine adopts the following method to perform autonomous control corresponding to water and land working conditions:

[0036] Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise:

[0037] Determine the desired engine speed based on the water flag in the control command;

[0038] If the desired speed is greater than 0, open the left and right water gates and set the clutch control mode to automatic; if both the land clutch and the water clutch are not engaged, set the gear to neutral, the engine desired speed to idle, and the parking command to park; if both the land clutch and the water clutch are engaged, set the gear to forward gear and the parking command to not park;

[0039] If the desired speed is less than 0, the left and right water gates are closed, the parking command is set to park, the clutch control mode is set to manual mode, both the land clutch and the water clutch are disconnected, and the gear is set to neutral;

[0040] If the desired speed is equal to 0, open the left and right water gates, set the accelerator pedal position to 0, set the parking command to park, set the clutch control mode to manual mode, disconnect the land clutch and the water clutch, and set the gear to neutral;

[0041] controlling an accelerator pedal according to a calculated accelerator pedal position;

[0042] If the water mark position is on water, segmented steering control is performed according to the expected curvature on water; otherwise, segmented steering control is performed according to the expected curvature on land.

[0043] Based on the further improvement of the above method, the accelerator pedal is calculated in the following way:

[0044] Calculating the speed difference between the desired engine speed and the actual engine speed at the current moment;

[0045] If the speed difference is greater than or equal to 0, the current accelerator pedal position = the current speed difference × the proportional coefficient + the last set accelerator pedal position;

[0046] If the speed difference is less than 0, the current accelerator pedal position = the last set accelerator pedal position - the current speed difference × the proportional coefficient.

[0047] Based on the further improvement of the above method, the timer thread determines whether to trigger an emergency stop in the following way according to the heartbeat signals of the executor, state machine thread and emergency stop monitoring thread:

[0048] S11. For each heartbeat signal, periodically extract a set of heartbeat data; store the first data of the set of heartbeat data in the first position of the first judgment array, set the current position index to zero; and set the current traversal index to one;

[0049] S12, determine whether the data at the current traversal index in the group of heartbeat data is equal to the data at the current traversal index position in the first judgment array, if equal, execute step S13; if not equal, increment the current position index by one, store the value at the current traversal index in the group of heartbeat data at the current position index of the first judgment array, determine whether the current position index is equal to a preset threshold, if so, execute step S14, otherwise execute step S13;

[0050] S13, the current traversal index is increased by one. If the current traversal index is equal to the length of the heartbeat data group, step S14 is executed; otherwise, step S12 is executed;

[0051] S14. If the current position index is less than the preset threshold, an emergency stop is triggered; otherwise, it is determined whether the numbers in the first judgment array are not equal to each other; if not, an emergency stop is triggered.

[0052] Based on the further improvement of the above method, the emergency stop monitoring thread uses the following method to determine whether to trigger an emergency stop based on the heartbeat signals of the executor, state machine thread and timer thread:

[0053] For each heartbeat signal, it is regularly determined whether the current heartbeat data is equal to the previous heartbeat data. If they are equal and the time difference between the current moment and the moment when the previous heartbeat data is received is greater than a first threshold, an emergency stop is triggered.

[0054] Compared with the prior art, the present invention obtains control instructions, status information and heartbeat signals of each actuator through the system thread, sends the control instructions and status information to the state machine thread, and then feeds back the status information to the autonomous domain through the state machine thread, so as to facilitate accurate operation planning of the autonomous domain. The state machine thread determines whether to switch the working condition. If so, it executes the working condition switching control. If not, it executes the autonomous control under each working condition, thereby performing process control on the operation of the amphibious unmanned platform to ensure driving safety during the working condition switching process and within each working condition. The heartbeat signals of each system thread and actuator are monitored in two different ways through the emergency stop monitoring thread and the timer thread, and an abnormality is detected in time to trigger an emergency stop, thereby ensuring the safety of the amphibious unmanned platform. At the same time, the threads are interlocked, and an abnormal heartbeat of one thread also triggers an emergency stop, thereby further improving the safety of the amphibious unmanned platform.

[0055] In the present invention, the above-mentioned technical solutions can be combined with each other to achieve more preferred combinations. Other features and advantages of the present invention will be described in the following description, and some advantages will become apparent from the description or be learned through practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the contents particularly pointed out in the description and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] The accompanying drawings are only used for the purpose of illustrating specific embodiments and are not to be considered as limiting the present invention. Like reference symbols denote like components throughout the accompanying drawings.

[0057] Figure 1 This is a data flow diagram of a method for collaborative control of an amphibious unmanned platform vehicle according to an embodiment of the present invention;

[0058] Figure 2 This is a flow chart of the working condition switching control according to an embodiment of the present invention;

[0059] Figure 3 This is a flow chart of water and land working condition control according to an embodiment of the present invention.

[0060] Reference numerals:

[0061] 1-State machine thread. DETAILED DESCRIPTION

[0062] The preferred embodiments of the present invention will be described in detail below in conjunction with the accompanying drawings, wherein the accompanying drawings constitute a part of this application and are used together with the embodiments of the present invention to illustrate the principles of the present invention, and are not used to limit the scope of the present invention.

[0063] A specific embodiment of the present invention discloses a method for coordinated control of an amphibious unmanned platform vehicle, comprising the following steps:

[0064] Build system threads, state machine threads, timer threads and emergency stop monitoring threads;

[0065] The system thread is used to receive control instructions, heartbeat signals and status information of the actuator, send the control instructions and status information to the state machine thread, and send the heartbeat signal of the actuator to the timer thread and the emergency stop monitoring thread;

[0066] The emergency stop monitoring thread is used to determine whether to trigger an emergency stop in a first manner according to the heartbeat signals of the executor, the system thread, the state machine thread and the timer thread;

[0067] The timer thread is used to determine whether to trigger an emergency stop in a second manner according to the heartbeat signals of the executor, the system thread, the state machine thread and the emergency stop monitoring thread;

[0068] The state machine thread is used to send state information to the autonomous domain; determine whether to switch the working condition according to the control instruction, and if so, execute the working condition switching control; otherwise, perform autonomous control corresponding to the historical working condition.

[0069] It should be noted that the actuators of the amphibious unmanned platform, namely the chassis actuators, include the engine, gearbox, left and right water gates, left and right rudders, suspension upgrades, and front and rear skids. The control commands received by the system thread include those issued by the autonomous domain.

[0070] The autonomous domain, or the autonomous planning module of the amphibious unmanned platform, acquires environmental information through a sensor network. Based on the platform's mission objectives, state information, and environmental information, it plans driving strategies using intelligent algorithms such as machine learning, reinforcement learning, and deep learning. It then sends control commands to the collaborative control terminal to guide the unmanned platform toward the planned target.

[0071] The control instructions issued by the autonomous domain include expected speed, expected curvature on land, expected curvature on water, expected acceleration, target working condition for working condition switching, historical working conditions, water flag position, front upper skateboard angle, front lower skateboard angle, tail skateboard angle, left and right water gate control instructions (invalid, open, closed), control skateboard angle enable, etc.

[0072] When implemented, data flows like Figure 1 shown.

[0073] Compared with the existing technology, the amphibious unmanned platform vehicle collaborative control method provided in this embodiment obtains control instructions, status information and heartbeat signals of each actuator through the system thread, and sends the control instructions and status information to the state machine thread, so that the status information is fed back to the autonomous domain through the state machine thread, which facilitates the autonomous domain to accurately perform operation planning. The state machine thread determines whether to switch the working condition. If so, it executes the working condition switching control. If not, it executes the autonomous control under each working condition, thereby performing process control on the operation of the amphibious unmanned platform to ensure driving safety during the working condition switching process and within each working condition. The heartbeat signals of each system thread and actuator are monitored in two different ways through the emergency stop monitoring thread and the timer thread, and an abnormality is detected in time to trigger an emergency stop, thereby ensuring the safety of the amphibious unmanned platform. At the same time, the threads are interlocked, and an abnormal heartbeat of one thread also triggers an emergency stop, thereby further improving the safety of the amphibious unmanned platform.

[0074] During implementation, four independent threads are first constructed: system thread, timer thread, emergency stop monitoring thread and state machine thread.

[0075] The system thread is used to obtain control instructions from the autonomous domain (amphibious unmanned platform autonomous planning module), obtain the actuator status information and heartbeat signal from the chassis actuator, and send the heartbeat signal to the timer thread and emergency stop monitoring thread for emergency stop monitoring, and send the control instructions and status information to the state machine thread for corresponding control processing.

[0076] Status information includes engine accelerator pedal position, skateboard angle, vehicle posture adjustment status, clutch status, shutter status, etc. The heartbeat signal is used to monitor whether the actuator is operating normally. If the heartbeat signal determines that the operation is abnormal, an emergency stop will be triggered to protect the amphibious unmanned platform.

[0077] The system thread sends the heartbeat signal to the timer thread and the emergency stop monitoring thread for heartbeat signal monitoring to determine whether an emergency stop is triggered. The timer thread and the emergency stop monitoring thread use two different monitoring methods to more accurately monitor the operating status. At the same time, the timer thread and the emergency stop monitoring thread also monitor the heartbeat signals of the system thread and the state machine thread. In addition, the timer thread and the emergency stop monitoring thread monitor each other's heartbeat signals. That is, the timer thread monitors the heartbeat signals of each executor, system thread, state machine thread, and emergency stop monitoring thread to determine whether an emergency stop is triggered. Any abnormality in the heartbeat signal will trigger an emergency stop; the emergency stop monitoring thread monitors the heartbeat signals of each executor, system thread, state machine thread, and timer thread to determine whether an emergency stop is triggered. Any abnormality in the heartbeat signal will trigger an emergency stop.

[0078] It should be noted that, under normal circumstances, the heartbeat signal of each executor and each thread should start from 0 and increase by one each time it is executed until it exceeds a threshold, for example, exceeds 255, and then returns to 0, that is, the heartbeat signal is constantly changing.

[0079] Specifically, the emergency stop monitoring thread determines whether to trigger an emergency stop in the following manner based on the heartbeat signals of the executor, state machine thread, and timer thread:

[0080] For each heartbeat signal, it is regularly determined whether the current heartbeat data is equal to the previous heartbeat data. If they are equal and the time difference between the current moment and the moment when the previous heartbeat data is received is greater than a first threshold, an emergency stop is triggered.

[0081] Because the heartbeat signal may jump back and forth, the second method is used to determine whether to trigger an emergency stop through the timer thread. The emergency stop monitoring thread and the timer thread are redundant and complementary to each other, allowing for a more comprehensive and accurate determination of the amphibious unmanned platform's status and improving operational reliability.

[0082] Specifically, the timer thread determines whether to trigger an emergency stop based on the heartbeat signals of the executor, system thread, state machine thread, and emergency stop monitoring thread in the following way:

[0083] S11. For each heartbeat signal, periodically extract a set of heartbeat data; store the first data of the set of heartbeat data in the first position of the first judgment array, set the current position index to zero; and set the current traversal index to one;

[0084] S12, determine whether the data at the current traversal index in the group of heartbeat data is equal to the data at the current traversal index position in the first judgment array, if equal, execute step S13; if not equal, increment the current position index by one, store the value at the current traversal index in the group of heartbeat data at the current position index of the first judgment array, determine whether the current position index is equal to a preset threshold, if so, execute step S14, otherwise execute step S13;

[0085] S13, the current traversal index is increased by one. If the current traversal index is equal to the length of the heartbeat data group, step S14 is executed; otherwise, step S12 is executed;

[0086] S14. If the current position index is less than the preset threshold, an emergency stop is triggered; otherwise, it is determined whether the numbers in the first judgment array are not equal to each other; if not, an emergency stop is triggered.

[0087] During implementation, for example, if the sampling period of the heartbeat signal is 100 milliseconds, the timer thread makes a judgment every 3 seconds, that is, it takes a set of heartbeat data Hb for each heartbeat signal every 3 seconds. i For example, among 30 heartbeat data, find out whether there are k unequal numbers, where k can be 3, to determine whether the heartbeat signal is abnormal.

[0088] First, the first judgment array is currently empty, the current position index P index =0, current traversal index L index =1, the first data of the heartbeat data group is stored in the first position of the first judgment array.

[0089] Determine heartbeat data Hb i The current traversal index L in index The heartbeat data and the first judgment array current position index P index Are the heartbeat data at the same? If not, then L index +1, if the current traversal index is equal to the length of the heartbeat data group (that is, the heartbeat data Hb has not been traversed yet, i ), then continue to traverse the heartbeat data Hb i If the next data is not equal, P index +1, heartbeat data Hb i The current traversal index L in index The heartbeat data is stored in the current position index P of the first judgment array indexIf the number of stored data in the first judgment array does not meet the requirement, then step S13 continues to search for the next unequal heartbeat signal. If the number requirement is met, then it is determined whether the numbers in the first judgment array are unequal. If not, an emergency stop is triggered. i If the number of items stored in the first judgment array still does not meet the requirements, it is considered an abnormality and an emergency stop is triggered.

[0090] During implementation, the emergency stop flag can be used to indicate whether an emergency stop has been triggered. If the emergency stop flag is true, it indicates that an emergency stop has been triggered; if it is false, it indicates that an emergency stop has not been triggered. The emergency stop flag is initially set to false by default. The emergency stop monitoring thread and timer thread will set the emergency stop flag to true when they determine that an emergency has been triggered.

[0091] The system thread sends the received control instructions and status information to the state machine thread, which then sends the status information to the autonomous domain. Based on the control instructions, the system determines whether to switch operating modes. If so, the system executes the switching control. Otherwise, the system performs autonomous control corresponding to the historical operating mode. The historical operating mode is the most recent recorded operating mode, i.e., the operating mode executed by the unmanned vehicle at the last moment.

[0092] During implementation, the state machine thread first executes a heartbeat auto-increment instruction, starting from 0 and increasing by 1 each time until the heartbeat exceeds 255, at which point it returns to 0. It then sends the received status information of each actuator to the autonomous domain, which uses this information to make the next planning step. This status information includes chassis speed, chassis curvature, left and right driving wheel speeds, front upper and lower slide angles, rear slide angles, clutch control, and gear position.

[0093] Before determining whether to switch operating modes, the state machine thread first executes the shutter protection program. Specifically, if the historical operating mode is onshore, and the shutter state is closed, the shutter opening instruction is executed; otherwise, the shutter automatic control instruction is executed. If the historical operating mode is any other, the shutter closing instruction is executed; otherwise, the shutter automatic control instruction is executed.

[0094] Then determine whether the historical operating conditions and the target operating conditions are the same. If they are the same, there is no need to switch the operating conditions. If the historical operating conditions are onshore conditions, the autonomous control corresponding to the onshore conditions will be executed. If the historical operating conditions are onshore and offshore conditions, the autonomous control corresponding to the onshore and offshore conditions will be executed. If the historical operating conditions are onshore conditions, the autonomous control corresponding to the offshore conditions will be executed.

[0095] If the historical operating conditions are different from the target operating conditions, operating condition switching control needs to be executed.

[0096] Specifically, such as Figure 2 As shown, the state machine performs operating mode switching control in the following manner:

[0097] Determine whether the current switching conditions are met based on historical operating conditions and target operating conditions. If not, trigger an emergency stop. Otherwise, determine the current switching type based on historical operating conditions and target operating conditions.

[0098] Adjust the slide's retraction and extension angle according to the switching type;

[0099] Whether the switching is successful is determined based on the switching type. If successful, the autonomous control corresponding to the target working condition is executed; otherwise, the autonomous control corresponding to the historical working condition is executed.

[0100] During implementation, the operating mode switch must meet one of the following rules: 1. The target operating mode is onshore, and the historical operating mode is land-water mode; 2. The target operating mode is onshore mode, and the historical operating mode is land-water mode; 3. The target operating mode is onshore mode, and the historical operating mode is either onshore or onshore mode. If the switching rules are not met, an emergency stop is triggered.

[0101] The switching types obtained according to the historical working conditions and the target working conditions include switching from water-land working conditions to water working conditions, switching from water-land working conditions to land conditions, switching from land conditions to water-land conditions, and switching from water working conditions to water-land conditions.

[0102] Specifically, the retraction and extension angles of the slide are adjusted according to the switching type, including:

[0103] If the switching type is from land-water working condition to water working condition, the target angle of water working condition is directly assigned to the bow upper slide, bow lower slide and tail slide;

[0104] If the switching type is from water-land working condition to land working condition, from land working condition to water-land working condition, or from water working condition to water-land working condition, then:

[0105] S21, determining whether the first upper slide has been adjusted to the target angle corresponding to the target working condition at the current moment, if so, the first step flag position is set to true, otherwise it is set to false; determining whether the first lower slide has been retracted to the target angle corresponding to the target working condition at the current moment, if so, the second step flag position is set to true, otherwise it is set to false;

[0106] S22. If the flags of the first and second steps are both false, the upper slide is assigned an intermediate angle corresponding to the target working condition, and the lower slide continues to be assigned the angle value fed back by the current sensor. The process returns to step S21. Otherwise, the process proceeds to step S23.

[0107] S23. If the first step flag is true and the second step flag is false, the upper slide is assigned an intermediate angle corresponding to the target working condition, and the lower slide is assigned a target angle corresponding to the target working condition, and the process returns to step S21; otherwise, the process proceeds to step S24.

[0108] S24. If the flags of the first and second steps are both true, the target angle corresponding to the target working condition is assigned to the first upper slide, the target angle corresponding to the target working condition is assigned to the first lower slide, and the target angle corresponding to the target working condition is assigned to the tail slide. Otherwise, an emergency stop is triggered.

[0109] When switching from land to water working condition, the target angle of the water working condition can be directly assigned to the bow upper slide, bow lower slide and tail slide.

[0110] If the switching type is from water-land working condition to land working condition, from land working condition to water-land working condition, or from water working condition to water-land working condition, the values ​​need to be assigned step by step.

[0111] During implementation, when switching from water-land operating conditions to land-land operating conditions, the target angle of the upper front skid plate is 33 degrees, the middle angle is 70 degrees, the target angle of the lower front skid plate is 27 degrees, and the target angle of the tail skid plate is 90 degrees. When switching from land operating conditions or water-land operating conditions to water-land conditions, the target angle of the upper front skid plate is 180 degrees, the middle angle is 70 degrees, the target angle of the lower front skid plate is 22 degrees, and the target angle of the tail skid plate is 90 degrees.

[0112] After processing the slide retraction and extension angles, determine whether the switch is successful. Specifically include:

[0113] If the switching type is from land to water, from water to land, or from water to water, then determine whether the clutch is engaged, the shutters are closed, the vehicle posture adjustment state is suspended, and the front upper slide, front lower slide, and tail slide have all reached the target angle. If so, the switching is successful, the current working condition is set as the target working condition, and the working condition switching flag is set to false. Otherwise, the working condition switching flag is set to true.

[0114] If the switching type is switching from water and land working conditions to land working conditions, then determine whether the clutch is engaged, whether the vehicle posture adjustment state is in suspension state, and whether the front upper slide, front lower slide and tail slide have all reached the target angle. If all are yes, the switching is successful, the current working condition is set to the target working condition, and the working condition switching flag is set to false. Otherwise, the working condition switching flag is set to true.

[0115] If the switch is successful, the autonomous control corresponding to the current working condition is executed, that is, the autonomous control corresponding to the target working condition is executed. Otherwise, the autonomous control corresponding to the historical working condition is executed.

[0116] If the target working condition is an overwater condition, autonomous control corresponding to the overwater condition is performed.

[0117] Specifically, the following methods are used to perform autonomous control corresponding to the water working conditions:

[0118] Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise:

[0119] Set the parking command to not park; send the actuator status information to the autonomous domain;

[0120] If the left and right water gate control instructions in the received control instructions are invalid, the left and right water gate actions are stopped; if the left and right water gate control instructions are open, the water maneuvering control enable is set to enable mode, the water maneuvering control mode is set to direct action mode, and the left and right water gates are opened; if the left and right water gate control instructions are closed, the left and right water gates are closed;

[0121] If the control skateboard angle in the received control instruction is enabled, the water maneuvering control mode is set to the target angle instruction mode, and the skateboard is controlled according to the skateboard angle value in the control instruction.

[0122] During autonomous control corresponding to the water working condition, it is first determined whether an emergency stop is triggered. If triggered, the emergency stop control according to the above-mentioned water working condition is processed.

[0123] If an emergency stop is not triggered, general control options are executed: the parking command is set to "no parking," sending a "no parking" command to the actuator and feeding actuator status information back to the autonomous domain. During autonomous water planning, status information fed back to the autonomous domain includes heading angle, desired heading angle, engine speed, bow upper slide angle, tail slide angle, and left and right rudder angles. The desired heading angle is the desired curvature on the water sent from the autonomous domain. The left and right sluice gates are then controlled according to the left and right sluice gate control commands in the control commands, and the slide angle is controlled according to the slide control commands in the control commands.

[0124] If the target operating condition is an onshore operating condition, autonomous control corresponding to the onshore operating condition is performed.

[0125] Specifically, autonomous control corresponding to onshore working conditions is performed in the following manner:

[0126] Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise:

[0127] If the desired speed in the control command is 0, the parking command is set to the parking state, the gear is set to neutral, and the desired speed and the desired curvature on the water are both set to 0;

[0128] If the desired speed in the control command is greater than 0, the parking command is set to not park, and if the land clutch is disengaged, the gear position is set to neutral, and the desired speed and curvature are set to 0; if the land clutch is engaged, the gear position is set to forward gear;

[0129] If the expected speed is less than 0, the parking command is set to no parking and the gear is set to reverse;

[0130] The parking command, gear position, desired speed, autonomous speed limit, desired land curvature, and desired acceleration are sent to the chassis actuator.

[0131] During autonomous control corresponding to onshore working conditions, it is first determined whether an emergency stop is triggered. If triggered, the emergency stop control according to the above-mentioned onshore working conditions is processed.

[0132] If no emergency stop is triggered, general control items are executed based on the desired speed: If the desired speed received from the autonomous domain is 0, the vehicle is currently in parking mode. The parking command is set to parking mode, the gear is set to neutral, and the desired speed and curvature fed back to the chassis actuator are both set to 0.

[0133] If the desired speed is greater than 0, the vehicle is in forward overland mode, and the parking command is set to "no parking." If the land clutch is disengaged, the gear is set to neutral, and the desired speed and curvature reported to the chassis are set to 0. If the land clutch is engaged, the gear is set to forward, and the desired speed, autonomous speed limit, desired curvature on land, and desired acceleration reported to the chassis are the values ​​in the received control command.

[0134] If the desired speed is less than 0, the vehicle is currently in reverse mode. The parking command is set to "no parking" and the gear is set to reverse. The remaining feedback variables are set to the same values ​​as in the forward mode. Specifically, the desired speed, autonomous speed limit, desired curvature on land, and desired acceleration fed back to the chassis are the same values ​​as in the received control command.

[0135] The chassis operation is controlled by sending parking commands, gear position, desired speed, autonomous speed limit, desired land curvature, and desired acceleration to the chassis actuators.

[0136] If the target working condition is water and land working condition, autonomous control corresponding to the water and land working condition is performed.

[0137] Specifically, such as Figure 3 As shown, the following methods are used for autonomous control corresponding to water and land working conditions:

[0138] Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise:

[0139] Determine the desired engine speed based on the water flag in the control command;

[0140] If the desired speed is greater than 0, open the left and right water gates and set the clutch control mode to automatic; if both the land clutch and the water clutch are not engaged, set the gear to neutral, the engine desired speed to idle, and the parking command to park; if both the land clutch and the water clutch are engaged, set the gear to forward gear and the parking command to not park;

[0141] If the desired speed is less than 0, the left and right water gates are closed, the parking command is set to park, the clutch control mode is set to manual mode, both the land clutch and the water clutch are disconnected, and the gear is set to neutral;

[0142] If the desired speed is equal to 0, open the left and right water gates, set the accelerator pedal position to 0, set the parking command to park, set the clutch control mode to manual mode, disconnect the land clutch and the water clutch, and set the gear to neutral;

[0143] controlling an accelerator pedal according to a calculated accelerator pedal position;

[0144] If the water mark position is on water, segmented steering control is performed according to the expected curvature on water; otherwise, segmented steering control is performed according to the expected curvature on land.

[0145] It should be noted that idle speed is 800 rpm. The water flag controls whether the engine is controlled according to the water coefficient or the land coefficient in both water and land conditions. If the water flag sent by the autonomous domain is 1, the desired speed conversion factor uses the water coefficient, and the desired engine speed is equal to the desired speed multiplied by the water conversion factor. Otherwise, the land conversion factor is used, and the desired engine speed is equal to the desired speed multiplied by the land conversion factor.

[0146] Specifically, the accelerator pedal is calculated in the following way:

[0147] Calculating the speed difference between the desired engine speed and the actual engine speed at the current moment;

[0148] If the speed difference is greater than or equal to 0, the current accelerator pedal position = the current speed difference × the proportional coefficient + the last set accelerator pedal position;

[0149] If the speed difference is less than 0, the current accelerator pedal position = the last set accelerator pedal position - the current speed difference × the proportional coefficient.

[0150] In practice, the scaling factor can be 0.01. If the calculated throttle pedal position is greater than 800, the throttle pedal position is set to 800 to prevent sudden wide-open throttle conditions. If the calculated throttle pedal position is less than 0, the throttle pedal position is set to 0 to prevent negative values.

[0151] Perform segmented steering control based on the desired curvature on the water, including:

[0152] When the expected curvature on the water is greater than 0.5, open the left water gate, open the right rudder, and close the left rudder;

[0153] When the desired curvature on the water is less than -0.5, open the right water gate, open the left rudder, and close the right rudder;

[0154] When the expected curvature on the water is greater than or equal to 0.02 and less than or equal to 0.5, open the right rudder and close the left rudder;

[0155] When the expected curvature on the water is greater than or equal to -0.5 and less than or equal to -0.02, open the left rudder and close the right rudder;

[0156] When the desired curvature on the water is greater than -0.02 and less than 0.02, turn off the left and right rudders.

[0157] Perform segmented steering control based on the desired curvature on land, including:

[0158] The desired steering wheel angle is obtained by multiplying the desired curvature on land by the land coefficient;

[0159] When the value of the desired steering wheel angle is greater than or equal to 500 and less than or equal to 6000, or less than or equal to -500 and greater than or equal to -6000, the steering angle sent to the chassis actuator is the value of the desired steering wheel angle;

[0160] When the desired steering wheel angle is 0, the steering angle sent to the chassis actuator is 0;

[0161] When the expected steering wheel angle is greater than 0 and less than 500, the steering angle sent to the chassis actuator is 500; when the expected steering wheel angle is greater than -500 and less than 0, the steering angle sent to the chassis actuator is -500; when the expected steering wheel angle is greater than 6000 or less than -6000, the steering angle sent to the chassis actuator is 6000 or -6000.

[0162] It should be noted that the execution process of the state machine thread is in a while loop, that is, after executing the autonomous control corresponding to the water working condition, the autonomous control corresponding to the land working condition, or the autonomous control corresponding to the water and land working condition, it returns to the starting position of the state machine thread, that is, the position where the heartbeat is automatically incremented.

[0163] It should be noted that the state information sent by the state machine thread to the autonomous domain and the instructions sent to the executor are forwarded through the timer thread.

[0164] Specifically, the emergency stop control is performed in the following manner;

[0165] If the current working condition is on land or in water, the accelerator pedal position, brake pedal position and direction angle signal and the water maneuvering enable are all set to zero, and the emergency stop signal is fed back to the transmission module;

[0166] If the current working condition is on water, the brake pedal position, direction angle signal and water maneuvering enable are all set to zero; the water emergency switch is turned on and the gear position is set to zero; the accelerator pedal position is controlled in a step-by-step manner; and the emergency stop signal is fed back to the transmission module.

[0167] During implementation, the accelerator pedal position is controlled in a step-by-step manner when operating above water. If the current accelerator pedal position is greater than 800, the accelerator pedal position is maintained at 800 for 2 seconds. If the current accelerator pedal position is between 500 and 800, the accelerator pedal position is maintained at 500 for 3 seconds. In other cases, the accelerator pedal position is directly set to 0. The accelerator pedal position is gradually reduced until it reaches 0, allowing the unmanned platform to come to a stable emergency stop.

[0168] In other working conditions, the accelerator pedal position, brake pedal position, steering angle signal and water maneuvering enable are directly set to zero.

[0169] After executing the emergency stop control, the emergency stop control can only be released through other modes, thereby improving the safety of the amphibious unmanned platform.

[0170] Those skilled in the art will appreciate that all or part of the process steps of the above-described embodiments can be implemented by instructing related hardware through a computer program, and the program can be stored in a computer-readable storage medium, such as a magnetic disk, an optical disk, a read-only memory, or a random access memory.

[0171] The above description is only a preferred specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily thought of by any technician familiar with this technical field within the technical scope disclosed by the present invention should be covered by the scope of protection of the present invention.

Claims

1. A method for coordinated control of an amphibious unmanned platform vehicle, characterized in that: The following steps are involved: Build system threads, state machine threads, timer threads and emergency stop monitoring threads; The system thread is used to receive control instructions, heartbeat signals and status information of the actuator, send the control instructions and status information to the state machine thread, and send the heartbeat signal of the actuator to the timer thread and the emergency stop monitoring thread; The emergency stop monitoring thread is used to determine whether to trigger an emergency stop based on the heartbeat signals of the executor, system thread, state machine thread and timer thread; The timer thread is used to determine whether to trigger an emergency stop based on the heartbeat signals of the executor, system thread, state machine thread and emergency stop monitoring thread; The state machine thread is used to send state information to the autonomous domain; Determine whether to switch the working condition according to the control instruction, and if so, execute the working condition switching control; otherwise, perform autonomous control corresponding to the historical working condition; the working conditions include water working conditions, land working conditions, and water-land working conditions; The autonomous domain is the autonomous planning module of the amphibious unmanned platform; The timer thread determines whether to trigger an emergency stop based on the heartbeat signals of the executor, state machine thread, and emergency stop monitoring thread in the following manner: S11. For each heartbeat signal, periodically extract a set of heartbeat data; store the first data of the set of heartbeat data in the first position of the first judgment array, and set the current position index to zero; Set the current traversal index to one; S12, determining whether the data at the current traversal index in the group of heartbeat data is equal to the data at the current traversal index position in the first judgment array, if equal, executing step S13; If they are not equal, the current position index is incremented by one, and the value at the current traversal index in the group of heartbeat data is stored at the current position index of the first judgment array, and it is determined whether the current position index is equal to the preset threshold. If so, step S14 is executed, otherwise step S13 is executed; S13, the current traversal index is increased by one. If the current traversal index is equal to the length of the heartbeat data group, step S14 is executed; otherwise, step S12 is executed; S14. If the current position index is less than the preset threshold, an emergency stop is triggered; otherwise, it is determined whether the numbers in the first judgment array are not equal to each other; if not, an emergency stop is triggered; The emergency stop monitoring thread determines whether to trigger an emergency stop in the following manner based on the heartbeat signals of the executor, state machine thread, and timer thread: For each heartbeat signal, it is regularly determined whether the current heartbeat data is equal to the previous heartbeat data. If they are equal and the time difference between the current moment and the moment when the previous heartbeat data is received is greater than a first threshold, an emergency stop is triggered.

2. The amphibious unmanned platform vehicle collaborative control method according to claim 1 is characterized in that: The state machine performs the working condition switching control in the following manner: Determine whether the current switching conditions are met based on the previous working condition and the target working condition. If not, trigger an emergency stop. Otherwise, determine the current switching type based on the previous working condition and the target working condition. Adjust the slide's retraction and extension angle according to the switching type; Determine whether the switching is successful based on the switching type. If successful, execute the autonomous control corresponding to the target working condition; Otherwise, autonomous control corresponding to historical working conditions is executed.

3. The amphibious unmanned platform vehicle collaborative control method according to claim 2 is characterized in that: Adjust the slide's retraction and extension angles based on the switch type, including: If the switching type is from land-water working condition to water working condition, the target angle of water working condition is directly assigned to the bow upper slide, bow lower slide and tail slide; If the switching type is from water-land working condition to land working condition, from land working condition to water-land working condition, or from water working condition to water-land working condition, then: S21, determining whether the first upper slide has been adjusted to the target angle corresponding to the target working condition at the current moment, if so, the first step flag position is set to true, otherwise it is set to false; determining whether the first lower slide has been retracted to the target angle corresponding to the target working condition at the current moment, if so, the second step flag position is set to true, otherwise it is set to false; S22. If the flags of the first and second steps are both false, the upper slide is assigned an intermediate angle corresponding to the target working condition, and the lower slide continues to be assigned the angle value fed back by the current sensor. The process returns to step S21. Otherwise, the process proceeds to step S23. S23. If the first step flag is true and the second step flag is false, the upper slide is assigned an intermediate angle corresponding to the target working condition, and the lower slide is assigned a target angle corresponding to the target working condition, and the process returns to step S21; otherwise, the process proceeds to step S24. S24. If the flags of the first and second steps are both true, the target angle corresponding to the target working condition is assigned to the first upper slide, the target angle corresponding to the target working condition is assigned to the first lower slide, and the target angle corresponding to the target working condition is assigned to the tail slide. Otherwise, an emergency stop is triggered.

4. The amphibious unmanned platform vehicle collaborative control method according to claim 2 is characterized in that: Use the following methods to determine whether the switch is successful: If the switching type is from land to water, from water to land, or from water to water, then determine whether the clutch is engaged, the shutters are closed, the vehicle posture adjustment state is suspended, and the front upper slide, front lower slide, and tail slide have all reached the target angle. If so, the switching is successful, the current working condition is set as the target working condition, and the working condition switching flag is set to false. Otherwise, the working condition switching flag is set to true. If the switching type is switching from water and land working conditions to land working conditions, then determine whether the clutch is engaged, whether the vehicle posture adjustment state is in suspension state, and whether the front upper slide, front lower slide and tail slide have all reached the target angle. If all are yes, the switching is successful, the current working condition is set to the target working condition, and the working condition switching flag is set to false. Otherwise, the working condition switching flag is set to true.

5. The amphibious unmanned platform vehicle collaborative control method according to claim 1 is characterized in that: The state machine performs autonomous control corresponding to the above-water working conditions in the following manner: Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise: Set the parking command to not park; send the actuator status information to the autonomous domain; If the left and right water gate control instructions in the received control instructions are invalid, the left and right water gate actions are stopped; if the left and right water gate control instructions are open, the water maneuvering control enable is set to enable mode, the water maneuvering control mode is set to direct action mode, and the left and right water gates are opened; if the left and right water gate control instructions are closed, the left and right water gates are closed; If the control skateboard angle in the received control instruction is enabled, the water maneuvering control mode is set to the target angle instruction mode, and the skateboard is controlled according to the skateboard angle value in the control instruction.

6. The amphibious unmanned platform vehicle collaborative control method according to claim 1 is characterized in that: The state machine performs autonomous control corresponding to onshore operating conditions in the following manner: Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise: If the desired speed in the control command is 0, the parking command is set to the parking state, the gear is set to neutral, and the desired speed and the desired curvature on the water are both set to 0; If the desired speed in the control command is greater than 0, the parking command is set to not park, and if the land clutch is disengaged, the gear position is set to neutral, and the desired speed and curvature are set to 0; if the land clutch is engaged, the gear position is set to forward gear; If the expected speed is less than 0, the parking command is set to no parking and the gear is set to reverse; The parking command, gear position, desired speed, autonomous speed limit, desired land curvature, and desired acceleration are sent to the chassis actuator.

7. The amphibious unmanned platform vehicle collaborative control method according to claim 1 is characterized in that: The state machine performs autonomous control corresponding to water and land conditions in the following manner: Determine whether the emergency stop is currently triggered. If so, execute the emergency stop control. Otherwise: Determine the desired engine speed based on the water flag in the control command; If the desired speed is greater than 0, open the left and right water gates and set the clutch control mode to automatic; if both the land clutch and the water clutch are not engaged, set the gear to neutral, the engine desired speed to idle, and the parking command to park; if both the land clutch and the water clutch are engaged, set the gear to forward gear and the parking command to not park; If the desired speed is less than 0, the left and right water gates are closed, the parking command is set to park, the clutch control mode is set to manual mode, both the land clutch and the water clutch are disconnected, and the gear is set to neutral; If the desired speed is equal to 0, open the left and right water gates, set the accelerator pedal position to 0, set the parking command to park, set the clutch control mode to manual mode, disconnect the land clutch and the water clutch, and set the gear to neutral; controlling an accelerator pedal according to a calculated accelerator pedal position; If the water mark position is on water, segmented steering control is performed according to the expected curvature on water; otherwise, segmented steering control is performed according to the expected curvature on land.

8. The amphibious unmanned platform vehicle collaborative control method according to claim 7 is characterized in that: The accelerator pedal is calculated as follows: Calculating the speed difference between the desired engine speed and the actual engine speed at the current moment; If the speed difference is greater than or equal to 0, the current accelerator pedal position = the current speed difference × the proportional coefficient + the last set accelerator pedal position; If the speed difference is less than 0, the current accelerator pedal position = the last set accelerator pedal position - the current speed difference × the proportional coefficient.

Images