Animal farm system comprising an autonomous agricultural vehicle and method for planning a trajectory for an autonomous agricultural vehicle

Abstract

An animal farm system comprising: an autonomous agricultural vehicle adapted to perform an animal related action related to at least one of cleaning up and providing feed in an agricultural operating 5 area, comprising at least one drive wheel under the control of a control device, for controlling the at least one wheel to move the vehicle in accordance with control signals that are based on a pre-planned trajectory and a vehicle communication device; a data processing system comprising a memory and a processor adapted for carrying out a method comprising:0 - receiving input defining an initial trajectory, forming a rough path, discretized as a list of instantaneous vehicle states defined by trajectory parameters comprising a vehicle position in the agricultural operating area and an associated velocity; - obtaining trajectory constraints the trajectory constraints comprising of5 vehicle driving limitations, navigation safety constraints, and a task objective; - calculating an optimized trajectory, based on the initial trajectory through an optimization-based method using a cost function comprising mathematical expressions based on signal temporal logic representing the trajectory 0 constraints to determine a level of optimization with respect to the trajectory constraints; and - outputting the optimized trajectory the data processing system further being adapted for being operably connectable to a user interface system and for sending the output optimized trajectory to the control 5 device via the vehicle communication device, to be stored as the pre-planned trajectory.

Description

[0001] ANIMAL FARM SYSTEM COMPRISING AN AUTONOMOUS AGRICULTURAL VEHICLE AND METHOD FOR PLANNING A TRAJECTORY FOR AN AUTONOMOUS AGRICULTURAL VEHICLE

[0002] FIELD

[0003]

[0001] The invention relates to an animal farm system comprising: an autonomous agricultural vehicle adapted to perform an animal related action related to at least one of cleaning up and providing feed in an agricultural operating area, wherein the vehicle comprises at least one drive wheel under the control of a control device, for controlling the at least one wheel to move the vehicle in accordance with control signals that are based on a pre-planned trajectory and a vehicle communication device; and a data processing system comprising a memory and a processor adapted for being operably connectable to a user interface system and for sending a trajectory to the control device via the vehicle communication device, to be stored as the pre-planned trajectory. The invention further relates to a computer-implemented method for planning a trajectory for use by an autonomous agricultural vehicle, a computer program and a data processing system for carrying out the method and a graphical user interface for use with the computer program or the data processing system.

[0004] BACKGROUND

[0005]

[0002] Agricultural autonomous vehicles are well known and increasingly used in the agricultural industry, as they offer farmers the possibility to carry out repetitious and intensive tasks in a reliable manner according to schedule and at minimal manpower. A particular subset of these vehicles are adapted for carrying out an animal related action, such as providing feed or cleaning a floor surface, in a livestock space, such as the Discovery® manure removal vehicle and the Vector® feeding vehicle, both of Lely Industries.

[0006]

[0003] Known autonomous agricultural vehicles, in particular vehicles for performing tasks directly related to the feeding livestock and / or the cleaning of their living space, are mostly still programmed in situ, i.e. while the vehicle is in the agricultural operating area it is going to be used. The space where the vehicle is going to be used depends on the specific vehicle and its tasks, and can be one or a combination of a barn, shed, farmyard or farm-land. The routes for these known autonomous agricultural vehicles are programmed as one or more trajectories, i.e. a path with time-information relating to positions along the path. Each trajectory is inputstep by step, wherein each step consists of giving an instruction to the vehicle affecting at least a driving speed and driving direction at a predetermined time and / or during a time-step, waiting for the vehicle to execute the instruction and then saving it. Any potential problems, such as the vehicle colliding with a structural item or exceeding operational limits, are checked visually and / or prevented manually before saving the instruction as part of the trajectory. This method of installing an autonomous agricultural vehicle allows for tailor-made installation, substantially independent of the particular type of vehicle and lay-out of the location and can therefore be reliably used for all types of autonomous agricultural vehicles.

[0007]

[0004] A downside of these methods of installation, however, is that installers need to be trained and experienced to plan optimal trajectories for the vehicle that ensure consistent reliable functioning of the vehicle, meeting various constraints dictated by the vehicle design, the particular space and the tasks the vehicle is to perform. Furthermore, these methods of installation even requires experienced installers to be on site for a long time, as the step-by-step programming of the route in situ with a vehicle can take more than a day. Moreover, the operation of the farm in the location where the vehicle is to be used is limited during the installation time. To ensure a safe working environment for the installer and accurate route planning, the space where route is planned for is preferably cleared of animals and other work activities.

[0008]

[0005] A goal of the invention is to make the installation of autonomous agricultural vehicles more efficient.

[0009] SUMMARY OF THE INVENTION

[0010]

[0006] According to a first aspect of the invention, this goal is achieved with an animal farm system comprising:

[0011] an autonomous agricultural vehicle adapted to perform an animal related action related to at least one of cleaning up and providing feed in an agricultural operating area, wherein the vehicle comprises at least one drive wheel under the control of a control device, for controlling the at least one wheel to move the vehicle in accordance with control signals that are based on a pre-planned trajectory and a vehicle communication device;

[0012] a data processing system comprising a memory and a processor adapted for carrying out a method comprising:

[0013] receiving input defining an initial trajectory, the initial trajectory forming a rough path, discretized as a list of instantaneous vehicle states defined bytrajectory parameters comprising a vehicle position in the agricultural operating area and an associated vehicle velocity;

[0014] obtaining trajectory constraints the trajectory constraints comprising of:

[0015] vehicle driving limitations, comprising a maximum driving speed and a minimum turning radius for the vehicle,

[0016] navigation safety constraints, comprising a minimum required clearance to a particular object in or boundary of the agricultural operating area, and

[0017] a task objective, defining a task for the vehicle to carry out with respect to the agricultural operating area;

[0018] calculating an optimized trajectory based on the initial trajectory through an optimization-based method using a cost function comprising mathematical expressions based on signal temporal logic representing the trajectory constraints to determine a level of optimization with respect to the trajectory constraints; and

[0019] outputting the optimized trajectory;

[0020] with the data processing system further being adapted for being operably connectable to a user interface system and for sending the output optimized trajectory to the control device via the vehicle communication device, to be stored as the pre-planned trajectory.

[0021]

[0007] The data processing system is adapted to execute the described method steps in a manner that allows trajectory planning to be performed substantially independently of the physical location where the vehicle is (or will be) deployed. Once the trajectory constraints and the input defining an initial trajectory are obtained, the system can compute an optimal trajectory that is feasible for execution within the agricultural operating area and store this trajectory in the vehicle’s control device for subsequent use. The optimization-based method employs a cost function to determine optimal parameters that minimize (or maximize) the cost function value. By incorporating the trajectory constraints into the cost function, the system automatically balances these constraints to ensure both feasibility for the vehicle and compliance wiht set task requirements. This approach simplifies trajectory programming and reduces reliance on highly experienced installers.

[0022]

[0008] The trajectory constraints may be considered high-level constraints, meaning broad, overarching limitations or requirements that guide trajectory planning and execution. Non-limiting examples include: a maximum (save) operating velocity, a maximum battery capacity limiting trajectory duration between charging cycles, tasksequencing requirements (e.g. task A must precede task B), and spatial constraints such as maintaining the vehicle within a designated area for a specified duration. While cost functions for autonomous mobile vehicles can be formulated using various logical frameworks, including temporal logic, signal temporal logic (stl), fuzzy logic, probabilistic logic, and rule-based logic, a significant advantage of STL lies in its ability to handle continuous signals (e.g., position and velocity) and enforce time-bounded constraints. STL enables these temporal and spatial constraints to be expressed as formals mathematical specifications using temporal operators and predicates. Furthermore, STL provides both Boolean semantics (true / false satisfaction) and quantitative semantics through robustness functions, which assign real-valued scores indicating how strongly a trajectory satisfies or violates a specification. This quantitative measure allows the cost function to incorporate all constraints in a unified and optimization-friendly manner, enabling the algorithm to maximize robustness and thereby find a trajectory that satisfies all constraints best.

[0023]

[0009] The obtaining of the trajectory constraints may be achieved by obtaining a file with pre-set constraints, such as for example factory settings, and / or be based on user input. For ease of use, at least a part of the trajectory constraints may be provided as user input via the user interface system using natural language, i.e. normal human speech, whereby the method comprises a step of translating the user input trajectory constraints into mathematical expressions based on signal temporal logic.

[0024]

[0010] Preferably, the autonomous agricultural vehicle is adapted to perform one or more of the tasks of removing manure, pushing feed or providing feed.

[0025]

[0011] According to a second aspect of the invention, a solution is provided through a computer-implemented method for trajectory planning for use by an autonomous agricultural vehicle adapted to perform an animal related action related to at least one of cleaning up and providing feed in an agricultural operating area, such as a livestock space, the method comprising:

[0026] receiving input defining an initial trajectory, the initial trajectory forming a rough path, discretized as a list of instantaneous vehicle states z_i defined by trajectory parameters comprising a vehicle position in the agricultural operating area and an associated vehicle velocity;

[0027] obtaining trajectory constraints the trajectory constraints comprising of: vehicle driving limitations, comprising a maximum driving speed and a minimum turning radius for the vehicle,

[0028] navigation safety constraints, comprising a minimum required clearance to aparticular object in or boundary of the agricultural operating area, and a task objective, defining a task for the vehicle to carry out with respect to the agricultural operating area;

[0029] calculating an optimized trajectory based on the initial trajectory through an optimization-based method using a cost function comprising mathematical expressions based on signal temporal logic representing the trajectory constraints to determine a level of optimization with respect to the trajectory constraints; and

[0030] outputting the optimized trajectory.

[0031]

[0012] The advantages of the method are the same as specified for the farm system according to the first aspect of the invention. The following embodiments hold for both the farm system and the method, unless specified otherwise.

[0032]

[0013] An embodiment comprises a further method step of forming a computational representation of the agricultural operating space, such as a graph or matrix, based on map data comprising coordinates relating to boundaries of the agricultural operating area, and wherein the step of receiving input defining an initial trajectory and / or the step of obtaining trajectory constraints comprise receiving user input in relation to the computational representation. The computational representation thus allows for user input being directly linked to map data, using the same referencing framework. Boundaries of and fixed objects in the agricultural operating area may be taken into consideration to some extent when defining the initial trajectory, as well as for defining trajectory constraints that are related to these boundaries and fixed objects. The initial trajectory then forms a rough path, discretized as a list of instantaneous vehicle states with respect to the computational representation.

[0033]

[0014] In an embodiment, the step of receiving input defining an initial trajectory comprises receiving user input defining a rough path, such as by drawing on a user interface, and discretizing the rough path into the discretized list of trajectory parameters. This is to be understood as the user input providing all information that define at least the positions for the initial trajectory. The time-series listing may autogenerate the velocity for each time step, for example using a predetermined initial velocity, such as a pre-set generic average velocity or assuming the discretized list is a time-series list of fixed time-intervals and calculating an associated speed depending on a delta between each two subsequent positions in the list. This particular embodiment does not necessarily rely on the method including the step of forming a computational representation based on map data, as the user input may simply directly result in a list or matrix of vehicle states. However, it may be considered practical to form somecombination of map data with a graphical representation that is made visual to a user and adapted to receive the user input in direct relation to the graphical representation nonetheless. This situation is particularly preferred when the system, or method, is adapted to also determine initial trajectories using a path planning algorithm to automatically generate a rough path based on user input.

[0034]

[0015] An embodiment comprises the additional or alternative step of receiving input defining an initial trajectory comprises receiving user input defining a start-goal position pair and automatically generating the initial trajectory between the start-goal position pair using a path planning algorithm based on a graph-based method or sampling-based method. Some non-limiting examples of path planning algorithms are A*, Dijkstra, probabilistic roadmap, rapidly exploring random tree. To successfully use any one of these algorithms for defining a rough path, the computational representation of the agricultural operating area, based on map data, is required.

[0035]

[0016] Similarly, another embodiment comprises the additional or alternative step of receiving input defining an initial trajectory comprises receiving user input defining a goal area and generating of the initial trajectory using a coverage path planning algorithm suitable to plan a route covering substantially the entire goal area. Coverage path planning algorithms mostly use a grid-based or graph-based approach to cover a selected goal area.

[0036]

[0017] In an embodiment, wherein the step of obtaining an optimized trajectory comprises iteratively:

[0037] evaluating the cost function;

[0038] calculating a gradient with respect to the trajectory parameters using backpropagation, to determine how the trajectory parameters are to be adjusted; and updating the trajectory using the determined trajectory parameter adjustments until a loss converges below a predefined threshold or a predefined maximum number of iterations is reached.

[0039] The gradient is a vector of partial derivatives that indicates the direction and rate of the steepest increase in the cost function. Using backpropagation means that the trajectory parameters are adjusted in the opposite direction of the gradient to minimize the cost function.

[0040]

[0018] In an embodiment, the step of obtaining trajectory constraints comprises the obtaining of parametrized trajectory constraints and trajectory constraint parameters independently from one another. This allows for parameters being defined and updated individually, making the method more suitable for use with input data from differentsources. For example, all of the trajectory constraints may be provided as user input, whereby the user provides input defining both the parametrized trajectory constraints and trajectory constraint parameters, or at least a part of the trajectory constraints is provided as pre-set constraints, automatically providing expression to a part of the trajectory constraint parameters and / or parametrized trajectory constraints. In this case, for example, at least a part of the trajectory constraint parameters may be provided in an algorithm using this method as fixed or initial settings based on factory-data for a particular vehicle model.

[0041]

[0019] In an embodiment, the method further comprises a step of obtaining a vehicle model representative of dynamic behavior of the autonomous agricultural vehicle, wherein a set of time-discretized equations relate a current state of the vehicle, comprising position and velocity, and control input to the vehicle which directly influences the velocity to an upcoming state of the vehicle. In this embodiment the cost function used to obtain the optimal trajectory consists of a first term, which evaluates how well the trajectory aligns with feasible vehicle behavior defined by the vehicle model, and a second term, which measures how well the trajectory satisfies the trajectory constraints. The vehicle model may replace the need for including certain vehicle behavior as complex trajectory constraints, whilst allowing for a better, vehicle specific, optimized trajectory calculation. The use of a vehicle model in addition to trajectory constraints also makes it easier to use the same set of trajectory constraints, tailored to a specific agricultural operational area, for different vehicles, such as for example when a particular vehicle is being replaced by an updated model.

[0042]

[0020] The cost function is adapted to account for the dynamic capabilities of the vehicle, ensuring the calculated optimized trajectory can be reliably performed by the vehicle. Hereto for the first term of the cost function, at each time step, a discrepancy may be determined between a predicted next vehicle state that is calculated based on a current vehicle state and the vehicle model and an actual next vehicle state, and the discrepancies may be aggregated across all time steps.

[0043]

[0021] Moreover, for the second term of the cost function, robustness values for each of the trajectory constraints may be computed at each time step and combined across all constraints and time steps. A robustness value is a quantitative measure of how well a time step satisfies or violates a given mathematical expression using STL to represent a trajectory constraint. A positive value indicates that the signal satisfies the STL formula, whereby a higher value indicates that the specification is satisfied more robustly. A negative value signals that the specification is violated, whereby a lowervalue is indicative of a more severe violation. Various known methods of calculating robustness values are known and may be used. In particular, when performing the method using an algorithm in Python3, one may opt to use the readily available STLCG toolbox, which uses computation graphs to calculate robustness. These graphs represent the STL formula and the signal, allowing efficient computation using techniques from machine learning.

[0044]

[0022] The combining across all constraints may include using predetermined individual weighing factors for each constraint, giving certain trajectory constraints a higher priority for the optimal trajectory to obey by then others.

[0045]

[0023] Moreover, the second term of the cost function may be scaled with respect to the first term of the cost function through multiplication by a scaling factor, wherein the scaling factor has a pre-determined value such that both the first term and second term have the same order of magnitude during a first time calculation of the loss. The use of the scaling factor thus ensures that both components of the calculated loss part from a same order of magnitude at the start of the trajectory optimization, preventing one from overpowering the other during the optimization. The value for the scaling factor depends on the particular vehicle model and set of trajectory constraints that are used. The value may be manually set, or for example be automatically chosen during a first calculation by the trajectory optimizer upon obtaining a particular set of trajectory constraints and vehicle model.

[0046]

[0024] In an embodiment, a method for calibrating at least one trajectory constraint parameter for at least one corresponding trajectory constraint is included, the method comprising:

[0047] obtaining vehicle data from a plurality of expert planned trajectories performed by the autonomous agricultural vehicle;

[0048] using the expert planned trajectories as input to define the initial trajectories; defining a trajectory constraint comprising a parametrized trajectory constraint and at least one candidate trajectory constraint parameter, the at least one candidate trajectory constraint corresponding to at least one trajectory constraint parameter to be calibrated;

[0049] calculating optimized trajectories for the initial trajectories through the optimization-based method using a cost function comprising the at least one trajectory constraint is as a mathematical expression using signal temporal logic to determine the level of optimization with respect to the trajectory constraint; and

[0050] refining the value of the at least one candidate constraint parameter to anupdated value at which a objective function evaluating the similarity between the optimized trajectories and the expert planned trajectories is minimal, using a derivative-free optimization method wherein the objective function outputs a total loss based on a loss calculated for each pair of expert planned trajectory and associated optimized trajectory using a metric that quantifies the discrepancy therebetween; and

[0051]

[0025] - using the optimal value for the at least one trajectory constraint parameter for the corresponding trajectory constraints in the method for trajectory planning.

[0052]

[0026] This method of calibrating trajectory constraint parameters is particularly useful for obtaining precise numerical values of trajectory constraints that are not known, e.g. do not follow from a design calculation or specification. For vehicles that have their trajectories planned such as described in the background, such unknown trajectory constraints tend to be determined by the installer during installation, and thus may vary per vehicle, even when of the same make and model, depending on the installer’s instantaneous assessment and experience. Using the proposed method, data from one or more vehicle trajectories that have been successfully installed by an installer may be used to obtain the precise numerical values, for use when using the trajectory planning method for obtaining new trajectories, such as for a new vehicle of the same make and model and / or in a new agricultural operational space. In particular, these method steps may be employed to determine accurate trajectory constraint parameters that are directly related to the vehicle dimensions and dynamics of a particular vehicle make and model, which may then be provided as factory settings, in the form of default or even fixed constraint parameters, in planning software based on the trajectory planning method and / or the farm system according to the first aspect of the invention. The expert planned trajectories may be trajectories that are actually planned in an agricultural operational space, or from trajectories that have been performed with one or more vehicles in a test-area, such as in the factory where the particular vehicle type is manufactured.

[0053]

[0027] During the calibration, the expert planned trajectories are used as input to define the initial trajectories. The input that may be used to define the initial trajectories corresponds to the input types indicated above for the trajectory planning method, i.e. define a start-goal pair, a goal area or define an entire rough path. The expert planned trajectories may be used accordingly, i.e. have their start-end positions used as start-goal pair, have an area surrounding the entire path selected or use the path of the expert planned trajectory as rough path for the initial trajectory.

[0028] Preferably, each of the at least one candidate constraint parameter is a normalized value, to ensure consistency in the optimization process, particularly when optimizing for a plurality of parameters at the same time. The normalization ensures that all parameters have a value between 0 and 1, such that all parameters contribute equally to the optimization process.

[0054]

[0029] The method may be used to obtain all trajectory constraint parameters, or only a part thereof. The task objective is most likely a specific user requirement that is at least partially not obtainable through the calibration method steps.

[0055]

[0030] Although an evolution algorithm may be used to refine the value for the at least one trajectory constraint parameter to an optimal value, preferably the derivative-free optimization method is a Bayesian method due to having a better sample efficiency.

[0056]

[0031] The Bayesian method may further comprise:

[0057] updating a surrogate model, which is preferably a Gaussian Process (GP) model, using the calculated loss for each trajectory pair to approximate a relationship between the at least one candidate constraint parameter and the objective function; and modelling of an acquisition function, preferably the logarithm of the Expected Improvement, to determine an updated value for the at least one candidate constraint parameter.

[0058]

[0032] According to a third aspect of the invention, a computer program is provided, comprising instructions which, when the program is executed by a computer, cause the computer to carry out the method of the second aspect of the invention.

[0059]

[0033] According to a fourth aspect of the invention, a data processing system is provided, comprising a memory and a processor adapted for carrying out the method of the second aspect of the invention.

[0060]

[0034] According to a fifth aspect of the invention, a graphical user interface is provided for use with the computer program according to claim the third aspect of the invention or the data processing system according to the fourth aspect of the invention, adapted for a user to input data for the initial trajectory forming a rough path and to input a trajectory constraint.

[0061] DRAWINGS

[0062]

[0035] The invention will be explained below with reference to the drawings, which show non-limiting exemplary embodiments of the invention, in which identical reference numerals indicate identical or similar components, and in which:

[0063]

[0036] Figure 1 conceptually illustrates application of embodiments of theinvention;

[0064]

[0037] Figure 2 shows a flowchart of method steps according to embodiments of the invention;

[0065]

[0038] Figure 3 schematically illustrates an embodiment of a trajectory planner module for calculating optimized trajectories;

[0066]

[0039] Figures 4 and 5 conceptually illustrate application of embodiments of the invention;

[0067]

[0040] Figures 6a and 6b conceptually illustrate application of embodiments of the invention;

[0068]

[0041] Figure 7 is a flowchart of method steps according to embodiments of the invention;

[0069]

[0042] Figure 8 schematically illustrates another embodiment of a trajectory planner module.

[0070] DETAILED DESCRIPTION

[0071]

[0043] In the present detailed description, various embodiments of the system and method according to the present invention are described with reference to an autonomous agricultural vehicle in the form of a manure removal vehicle of a particular design and an animal farm system for use in an agricultural operating area designed for milking animals. However, the present invention may equally be used with differently designed manure removal vehicles and autonomous vehicles adapted to perform some other animal-related action, such as, but not limited to, obtaining and / or providing feed. Some exemplary alternative types of autonomous vehicles are briefly introduced in the description of Figure 1. Moreover, the present invention may equally be used in animal farm systems designed for other animals, such as meat cattle. In such case, fewer, more or other animal related structures may be provided in the animal-related space and have to be accounted for during vehicle route planning and performance. Finally, the agricultural operating area may also consist or comprise of an outdoor area, such as a farm yard or field.

[0072]

[0044] Figure 1 conceptually illustrates application of embodiments of the invention. Figure 1 diagrammatically shows a part of an animal farm system, that is generally designated with reference numeral 1. It comprises an in the form of a barn 2 in which animals such as cows (not shown here) can move about freely. The barn has boundaries which are formed by walls 3 and animal related structures like a feed fence 4, a drinking trough 8, cubicles 9 and a milking system 10. Behind the feed fence 4 is afeed alley 5. In the present case, the feed fence 4 separates a barn part that is accessible for animals from the feed alley 5 that is not accessible for the animals. In the feed alley 5, a farmer or automated system, such as an autonomous feeding vehicle such as the Vector® of Lely Industries, may provide feed 6 such as roughage. In the depicted situation, an autonomous agricultural vehicle in the form of a feed pusher 7 vehicle, such as the Juno® of Lely Industries, is provided, that pushes tossed about feed toward the feed fence 4, so that the animals can reach it again. The feed pusher 7 thereto moves along the arrow shown, along a trajectory parallel to the feed fence 4. Even though the feed alley 5 cannot be reached by animals, it is still part of the barn 2. Another autonomous vehicle provided here is a manure removal vehicle 11, such as the Discovery® of Lely Industries. It is shown here at a charging device 12, for recharging its battery. Such position is a useful reference point for orientation and navigation and is commonly used as start and end position in the routes to be followed by vehicles, such as the diagrammatic route indicated with the line marked 13. This route is very simple and given only for illustrational purposes.

[0073]

[0045] An autonomous agricultural vehicle, such as any one of the ones shown in this conceptual illustration, will normally be provided with one or more routes, each consisting of one or more trajectories, which are both vehicle and task dependent. For example, a route for a manure vehicle may be planned in accordance with a particular cleaning requirement, for example for a specific area of the barn that is soiled most by the animals, optionally while steering clear of another specific area, such as for example near the feeding fence during feeding time. A route for a feeding vehicle may for example pass along the feeding fence while providing feed, but in order to do so, must first include passing a feed kitchen or other space for loading feed.

[0074]

[0046] Each of the trajectories consist of a path (a where) and some definition of vehicle dynamics determining how the path is driven. Thus, a trajectory may define a sequence of poses for the vehicle to be at and some operational input affecting dynamic behaviour of the vehicle, such as driving velocity, at any one of these poses. For reliable operation, it is important that each trajectory is planned in accordance with trajectory constraints that account for dynamic limitations of the vehicle as well as navigational constraints such as based on boundaries arising from the agricultural operating area, ensuring the vehicle can drive the route without getting lost and / or stuck. Moreover, the trajectory must comply with constraints that arise from the specific vehicle task and any user dependent requirements related thereto, such as for example spending a particular amount of time driving a particular section of the route to obtain a desired level ofcleanliness of the floor, or spending a particular amount of time at a particular position to achieve a particular load level of feed or battery charge.

[0075]

[0047] Figure 2 shows a flowchart of method steps according to embodiments of the invention, the method steps outlining a method 100 for obtaining optimized trajectories 100. The steps of the method 100 are shown to be:

[0076] - form a computational representation based on map data 101,

[0077] - obtain trajectory constraints 102,

[0078] - receive input defining an initial trajectory 103,

[0079] - transform trajectory constraints into a formal mathematical expression using STL 104, and

[0080] - optimize the initial trajectory through an optimization-based method using a cost function formulated with the STL trajectory constraints 105.

[0081]

[0048] In the step of forming a computational representation based on map data 101, map data is converted to a computational data set representing the actual agricultural operating area and forms a base for further steps of the method, such as providing input defining an initial trajectory, being carried out in reference to. The map data is map data of the space the agricultural vehicle is intended to operate in. The agricultural operating area may consist or comprise of an indoor space such as the barn depicted in Figure 1 and / or an outdoor space such as a farm’s field or a farm’s yard. The map data includes boundaries of the space relevant to the trajectory planning for the vehicle, such as for example walls, fences, doors and pillars, as well as relevant locations of vehicle related features such as charger location(s), fill location(s) and dump pit(s). The manner of obtaining the map data itself is not part of the invention. Various methods of obtaining map data are well known and include taking physical measurements using measuring tape or optical sensors, using verified building maps and using camera images. The computational representation may for example be a two-or three-dimensional graph and may use any coordinate system. A cartesian coordinate system is preferred due to being intuitive to understand for most users and will be used from hereon in the description. Nevertheless, it will occur to the skilled person that this is non-limiting to the scope of the invention, since the same principles can be applied without significant changes using another coordinate system. Moreover, it might occur to the skilled person that this step may be regarded as an optional step, since a computational representation may also be formed together with, i.e. as part of, the received input defining an initial trajectory and the obtaining of trajectory constraints. For example, the boundaries as presented in map data could be defined through one ormore trajectory constraints directly.

[0082]

[0049] In the step of obtaining trajectory constraints 102, limitations and needs that are to be accounted for while planning the trajectory of the vehicle are collected. The trajectory constraints may be divided into the following three categories: vehicle driving limitations <pi, navigation safety constraints q>2 and task objectives q>3. Vehicle driving limitations cpi comprise constraints directly related to the physical properties and / or dynamic capabilities of the vehicle for which the trajectory is being planned. The vehicle driving limitations include, but are not necessarily limited to, a maximum driving speed and a minimum turning radius. Additional vehicle driving limitations may be included, such as for example one of a maximum acceleration, a maximum turning rate, a minimum turning rate, a maximum range, a minimum width and a battery life. Navigation safety constraints q>2 comprise constraints directly related to the fixed boundaries of the agricultural operating area and any associated boundaries that may follow from its particular use and / or preferences by its owner. As such, the navigation safety constraints comprise minimum required clearances to particular objects in and boundaries of the agricultural operating area. The task objectives q>3 define one or more tasks for the vehicle to carry out with respect to the agricultural operating area, such as for example specify a time to spend at a charger location for proper charging or have a lower driving speed in a particular area for higher intensity cleaning or have a predetermined driving speed along in a particular section to ensure a predetermined food output. As can be understood from these examples, at least a part of the trajectory constraints are high level constraints, i.e. constraints that are broad, overarching limitations or requirements that guide the planning and implementation of the trajectory. Although all trajectory constraints may be obtained as manual input by a user, a part of the trajectory constraints may also be provided as default and / or fixed constraints such as based on the map data and / or based on a particular vehicle type and model selection for which the trajectory is being planned. In the latter case, the method may for example be implemented as vehicle and model dependent software that comes with some factory-set trajectory constraints, or as a more general software that allows a particular vehicle model being selected to obtain corresponding factory-set trajectory constraints. In any case, at least the task objectives q>3 may be expected to be provided as manual input by a user.

[0083]

[0050] In the step of receiving input defining an initial trajectory 103 comprises receiving some form of user input that defines at least a part of the initial trajectory, in particular a part of the path for the initial trajectory. The input for the path may be one of:inputting a goal-position, inputting a start-goal position pair, inputting a goal-area and inputting an entire initial path for the trajectory. The start position for the initial trajectory may be a standardized pre-selected position, such as for example the position of a charging station for the vehicle or may thus be provided as manual input. When the input is limited to a goal-position (with respect to a pre-selected start-position) or a startgoal position pair, an initial path between the start and goal position is auto-generated by a path planning algorithm. Preferably, the path planning algorithm used to generate an initial trajectory based on having a start-goal position pair for which at least the goal position is provided as input is based on a graph-based method or sampling-based method. Some non-limiting examples of such path planning algorithms are: A*, Dijkstra, probabilistic roadmap and rapidly exploring random tree. In case the input consists of or comprises a goal-area, the initial path inside the goal-area may be auto-generated using a coverage path planning algorithm suitable to plan a route covering substantially the entire defined area. Some non-limiting examples of coverage path planning algorithms are: Boustrophedon Decomposition and Grid-based Coverage. Any additional dynamic behavior for completing the initial trajectory, such as an indication of driving speed along the initial path, may additionally be provided as input, or be auto-generated based on a pre-set rule or value, such as for example be equal to the maximum driving speed or a percentage thereof.

[0084]

[0051] The step of receiving input defining an initial trajectory 103 is not necessarily to be performed in the indicated order, i.e. after the step of obtaining trajectory constraints 102, but may be performed at any time between the step of forming a computational representation 101 and the step of optimizing the initial trajectory 105.

[0085]

[0052] Any trajectory objectives manually input by a user will at least first be specified by the user in natural language, i.e. normal human speak. To ensure user-friendliness, it is preferred that the user can provide input in a manner that is as close as possible to natural language. To ensure a computer can then perform calculations based on the user-input, the method comprises the step of transforming trajectory constraints into a formal mathematical expression using STL (signal temporal logic) 104. For the present invention, signal temporal logic is specifically chosen, as it allows for intricate constraints being described over time and provides robustness metrics that quantify how well a system satisfies the specified constraints. The transforming may for example be performed in a translation-layer or module that is in data-communication between a user interface through which the user input defining trajectory constraints innatural language is received and the algorithm for optimizing a trajectory. Various manners of implementing a translation-layer or module are possible, ranging from providing a user interface with selection options for inputting trajectory constraints in natural language that are directly linked to mathematical formulations using signal temporal logic in the background, to using large language models (LLM) to accurately transform any form of natural language user input (including written and speech-to-text) into temporal logic.

[0086]

[0053] To illustrate the transformation from natural language to STL constraints, the following examples are provided:

[0087] A user may express the following vehicle driving limitations:

[0088] “ The vehicle must always drive at a speed lower than Umax and perform turns with a radius greater than rmin."

[0089] This is transformed into the following signal temporal logic constraint:

[0090] φ1= □(v < umax∧ r > rmin)

[0091] with Umax and rmin being fixed velocity and radius thresholds, v being the vehicle’s instantaneous velocity, and r being the radius of a turn being executed.

[0092] A user may also express the following navigation safety constraint:

[0093] “The vehicle must always maintain a distance of at least 5i from all type 1 clearance zones and at least 52 from all type 2 clearance zones. ”

[0094] wherein the type 1 and type 2 clearance zones are identified with respect to the map data, such as being particularly selected areas around certain fixed features in the agricultural operating area. The user input is transformed into the following STL constraint:

[0095]

[0096] □(Λi ∈ zone₁di> δ1∧ Λj ∈ zone₂dj> δ2)

[0097] with bi and 62 representing the distance thresholds for the type 1 and 2 clearance zones respectively, and di and dj respectively denoting the vehicle’s distance from the i-th type 1 zone and j-th type 2 zone.

[0098] And finally, a user may for example express the following task objective:

[0099] “The vehicle must first visit and stay in Area of Interest 1 (AOh) for ti time steps, and then eventually visit and stay in Area of Interest 2 (AO 12) for t2 time steps. ”Which is transformed into the following STL constraint:

[0100] <

[0101]

[0102] φ3= ◇ (□[0,t₁]μAOI₁) ∧ ((□[0,t₁]μAOI₁) 풰 ◇ (□[0,t₂]μAOI₂))

[0103] wherein the predicates μAOI₁and μAOI₂evaluate to true when the vehicle is within AOI₁ and AOI₂ respectively. The term ◇ (□[0,t₁]μAOI₁) ensures the vehicle eventually visits AOI₁ and stays here for t1 consecutive steps, while the 풰 (Until) operator enforces the temporal order, ensuring that AOI₂ is visited after the conditions for AOI₁ are met.

[0104] These exemplary trajectory constraints will be adhered to for the rest of the detailed description. It will occur to the skilled person that alternative or additional constraints may be used with the method, depending on the particular vehicle and agricultural operating area.

[0105]

[0054] In the final step, the initial trajectory is optimized through an optimizationbased method using a cost function formulated with the STL trajectory constraints 105.

[0106]

[0055] The cost function incorporates STL robustness metrics, i.e. a function that quantifies how well the trajectory satisfies or violates the specified STL trajectory constraints. The cost function is then minimized or maximized as an optimization problem, by adjusting the path and / or operational input affecting dynamic behaviour of the vehicle to find the optimal trajectory for the vehicle that also satisfies the STL constraints. Methods like gradient descent, convex optimization, or direct collocation can be employed to find the optimal trajectory that minimizes the cost function and satisfies the STL constraints.

[0107]

[0056] Figure 3 schematically illustrates a trajectory planner module 110 for calculating optimized trajectories. This trajectory planner module performs a method 100 as outlined by the flowchart of Figure 2. The trajectory planner module 110 receives an input 115 that is passed onto input modules 120 which feed into a trajectory optimizer module 140 that finally outputs optimized trajectories 150. The trajectory planner is structured into two primary components: the input modules 120 and the trajectory optimizer module 140. The input modules include the user-defined elements, while the trajectory optimizer integrates this information into an optimization process with the goal of outputting feasible trajectories. Through the modular design, the trajectory planner is easily adaptable to different vehicle models and different trajectory constraints, including types and amounts thereof.

[0108]

[0057] The input modules 120 consist of an initial trajectories module 122, an STL trajectory constraints module 125 and a vehicle model module 128.

[0109]

[0058] The initial trajectories module 122 is adapted for receiving user inputdefining at least one initial trajectory. The various types of input that may be provided for defining a trajectory have been described in reference to Figure 2. Where the input is provided with a goal position or area, the manual input may be transferred into sets of initial and final coordinates for each trajectory to be planned. The pairs may for example be represented as a set of tuples

[0110] $

[0111]

[0112] ~ {si—(xstart’ ystart’xgoal> ygoal)}^_1where m is the total amount of start-goal pairs, i.e. the total amount of trajectories for which the optimizer will calculate an optimum trajectory that satisfy the same set of trajectory constraints. The (coverage) path planning algorithm then generates an initial trajectory for each of the start-goal pairs. Each of the initial trajectories Zj is finally represented as a time-series matrix 풵 = {zi}mi=1, where each row corresponds to a time step, and the columns encode the vehicle states at that step. Such a matrix may for example use the vehicle’s position and velocity to encode the vehicle states:

[0113] ’x(t0) y(to) Vx(t0) Vytoy Wo)'

[0114] 풵 = [x(t₀) y(t₀) v_x(t₀) v_y(t₀); x(t₁) y(t₁) v_x(t₁) v_y(t₁); ... ; x(t_T) y(t_T) v_x(t_T) v_y(t_T)] = [z(t₀); z(t₁); ...; z(t_T)]

[0115] The skilled person will understand that a similar matrix may easily be generated for an initial trajectory that is based on manual input defining an entire initial path. It will also occur to the skilled person that the initial trajectories module 122 does not necessarily need to contain the (coverage) path planning algorithm, but that any such algorithm may also be included inside the trajectory optimizer module 140 or in another module that exists between the input modules 120 and the trajectory optimizer module 140.

[0116]

[0059] The STL trajectory constraints module 125 is adapted for obtaining the trajectory constraints and transforming them into STL trajectory constraints that together form the specification set φp, as described previously in reference to Figure 2. Preferably, the STL trajectory constraints are obtained as parametrized trajectory constraints φ = {φ1, φ2, φ3} and trajectory constraint parameters p ∈ 풫 by the input module, i.e. listed independently from one another. This allows for parameters being defined and updated individually, making the method suitable for use with input data from different sources, such as for example partly being provided as initial settings based on factory-data and being supplemented by user input. In this case, the user input may be provided as independent parametrized trajectory constraints and trajectory constraint parameters, or the user input may be separated into the two individual components by an algorithm that operates between a user interface and the input modules 120. A readily available algorithm for the parametrized specifications whichmay be implemented when scripting the trajectory planner module in Python3 is the STLCG toolbox, available via GitHub. It should be noted that, although the trajectory constraints are defined as constituting three types of constraints, these constraints may be defined using more than three mathematical specifications, i.e. the specification set φ can be extended to any n number of desired specifications.

[0117]

[0060] The vehicle model module 128 is presented here to represent the dynamic behavior of the vehicle, independently from the vehicle driving limitations that are defined as trajectory constraints. The dynamic behavior of the vehicle is defined as a set of equations that relate its current states and input to its upcoming states, denoted as: ℳ : (x, u) ↦ ẋ. Here, x represents the state vector (e.g., position, velocity), u denotes the control inputs (e.g., velocity commands, forces, torques), and ẋ describes the state derivatives or updates.

[0118]

[0061] As an example, the following (simplified) linear vehicle model, using a time-discretized state-space representation may be used to compute the vehicle state of the next time step as:

[0119] x(t + 1) = Ax(t) + Bu(t)

[0120] with

[0121] x(t)

[0122] y(t)

[0123] x(t) = u(t) = A = I B = Δt · I

[0124] uv_x(t), uv_y(t)

[0125] vy(t)

[0126] Here, the state vector x(t) represents the vehicle’s position (x(t), y(t)) and velocity (vx(t), vy(t)) along the x and y-axes of the computational representation. The control input vector u(t) consists of the commands (uv_x(t), uv_y(t)) that directly influence the vehicle’s velocity along each axis. A is the state transition matrix, which is the identity matrix I, indicating that the state updates are not influenced by any additional internal dynamics. Meanwhile, B is the control input matrix, where At is the time step size, which scales the effect of the control inputs on the state changes. As indicated, this is merely an exemplary vehicle model. It will occur to the skilled person that the vehicle model framework can accommodate more complex ℳ, such as those with non-linear dynamics or coupling between states as long as they are time-discretized and provide a stateinput mapping.

[0127]

[0062] The trajectory optimizer module 140 integrates the information from the input modules 120 into an optimization loop with the goal of generating trajectories that satisfy the given constraints.

[0063] The first step 142 performed by the trajectory optimizer involves evaluating a cost function

[0128]

[0129] 풥i(ℳ, Øp, zi) for each trajectory ziin the time-series matrix 풵. The cost function is defined by the vehicle model ℳ and the instantiated trajectory constraints Øp, which is the combination of STL trajectory constraint parameters p with the parametrized STL trajectory constraints φ as:

[0130] 풥i(ℳ, Øp, zi) = jsys_dynamics+ γ · jrobustness

[0131] The jsysdynamics term evaluates how well the generated trajectory zi aligns with the feasible behaviors defined by the vehicle model. At each time step t, the state information in z(t) is used together with the vehicle’s state-space equations to predict the next state z(t + 1). This predicted state is then compared to the actual trajectory state z(t + 1). The discrepancies across all time steps are aggregated to compute jsys dynamics’ promoting that the trajectory adheres to the vehicle’s dynamics model. The jrobustness term measures how well the trajectory zi satisfies the STL constraints 0P. Robustness values for the various constraints in 0Pare computed at each time step in Zi capturing the degree of satisfaction or violation of each of the trajectory constraints. These individual contributions are then the overall jrob stness ■ Additionally, this term is weighted by the hyperparameter y to control its relative importance in the optimization process. The value of y is chosen such that both components of the cost function depart from the same order of magnitude at the start of the trajectory optimization and may vary between particular cases that are optimized depending on the set of STL specification and the robot model.

[0132]

[0064] During a second step 144, once the loss is computed, backpropagation is used to calculate gradients with respect to each of the trajectory constraint parameters p. Each gradient is a vector of partial derivatives that indicates the direction and rate of the steepest increase in the cost function. These gradients inform on how to adjust the vehicle states and control inputs to reduce the loss. Normally parameters are adjusted in the opposite direction of the gradient to minimize the cost function.

[0133]

[0065] During the final step of the loop 148, the trajectory zi is updated using Adaptive Moment Estimation (Adam), and the process repeats: recalculating the loss 풥i, computing gradients and updating the parameters. The loop continues until the loss converges below a predefined threshold or the maximum number of iterations is reached. This is done for all trajectories in Z. The output 150 of the trajectory planner is a set of optimized trajectories, represented as 풵* = {zi*}mi=1

[0134]

[0066] Figures 4 and 5 each conceptually illustrate a visual representation of acomputational representation in the form of a graph 40, 40’, displaying a map 22’ based on map data comprising coordinates relating to boundaries 41, trajectory constraints that directly relate to the agricultural operating area 42, 43, 44, 45 and an initial trajectory 50, 50’. The computational graphs are each visualized as being two-dimensional cartesian coordinate systems, although the skilled person will understand that the computational graph, and map, may also be three-dimensional and / or use another known coordinate system. It will be appreciated that the map and boundaries depicted therein will be dependent of the particular agricultural operating area the trajectory is being planned for. To illustrate the concept, a fictitious map is used, which is the same in both Figure 4 and 5. The depicted exemplary trajectory constraints that directly relate to the agricultural operating area 42, 43, 44, 45 are a first and second area of interest 42, 43, and type 1 and type 2 clearance zones 44, 45. The skilled person will understand that, depending on the specific agricultural operating area, vehicle and user needs, fewer or more of such agricultural operating area dependent trajectory constraints will be defined, and moreover, that the constraints may consist or comprise of additional or alternative constraints.

[0135]

[0067] A particular optional additional constraint that is illustrated in Figure 4 is a set of grid points 48, which in the concept are presented as center-points of the two-dimensional cartesian grid. Grid points 48 may be included to at least a part of the computational graph 40, to promote the optimized trajectory for the vehicle comprising at least on part of substantially straight path-sections.

[0136]

[0068] In Figure 4, the initial trajectory 50 is generated between a start position 51 and a goal position 52. As described in reference to Figure 2, the start position may be based on user input or may be pre-selected. It will occur to the skilled person that an initial trajectory that is completely based on user-input may result in a similar-looking visual representation, where optionally one or both of the start position 51 and goal position 52 are not explicitly marked as such.

[0137]

[0069] In Figure 5, instead of a goal position, a goal-area 52’ is defined. The initial path inside the goal-area 50’ substantially covers the entire defined area and may be auto-generated using a coverage path planning algorithm. It will occur to the skilled person that a fully manually input initial trajectory may also consist of or comprise a path section similar to the depicted path covering the goal-area 52’. The goal-area 52’ is illustrated as being some distance from a start position 51. The distance between the start position 51 and the goal-area 52’ may be treated as an independent trajectory from the selected goal-area 52’. In this case a predetermined position, for example positionof the goal-area 52’ that is closest to the start position 51 or a user-selected goal position, may be used as a goal position for the trajectory that is to be planned between the start-position and the goal-area 52’ and as the start position for the coverage trajectory inside the goal area 52’.

[0138]

[0070] Figures 6a and 6b further conceptually illustrate application of embodiments of the invention. Figure 6a shows the manure removal vehicle 11 and Figure 2b shows an external communication device 20.

[0139]

[0071] The manure removal vehicle 11 comprises a first drive wheel 19a, a second drive wheel 19b, a control unit 14 with a memory (not shown), a drive sensor system 15 and a vehicle communication device 18. The vehicle 11 can move manure across the floor with a scraping device and / or collect the manure, for example using a vacuum system. Thereto, it moves along at least one pre-planned trajectory, such as the schematically indicated route 13 in Fig. 1, under the control of the control device 14, which operates each of the drive wheels 19a, 19b of the vehicle 11 individually and in accordance with the pre-planned route. The control device 14 is operably connected to the drive sensor system 15 and the vehicle communication device 18. The vehicle 11 has a normal forward driving direction F as indicated.

[0140]

[0072] The drive sensor system 15 is adapted to collect vehicle driving data, comprising a sensor for monitoring motor-speeds of a drive motor actuating a drive wheel and / or a directly proportional value such as motor-current and wheel revolutions. Further, the drive sensor system 15 comprises a system adapted for monitoring a relative orientation and / or position, absolute or with respect to a previous orientation and / or position. The controller is adapted to use the data collected from the drive sensor system 15 to generate control inputs corresponding to the planned trajectory.

[0141]

[0073] The external communication device 20 is shown as consisting of a processor 25 and a user interface system 35 with a display screen 21 for providing a graphical user interface. The user interface system 35 is shown as displaying a map 22 as well as having buttons 24. The map 22 is based on map data comprising coordinates relating to boundaries of the agricultural operating area the vehicle is intended to operate in. In addition to the map 22. The processor 25 has a memory (not shown) storing instructions, which, when executed, cause the processor to carry out the method for obtaining optimized trajectories and / or a method for determining parameter for use as trajectory boundary constraints in the method for obtaining optimized trajectories further described with reference to Figures 7 and 8 below.

[0142]

[0074] The user interface system 35 and processor 25 may be comprised in asingle device, such as a computer. Alternatively, the processor 25 may be comprised in a device or system that is operably connectable to the user interface system 35, preferably in a wireless manner. For example, the processor 25 may be comprised in a server, that is either located on the farm itself or remotely and for example set up to provide a shared service such as via cloud. In a further alternative, the processor 25 may be comprised in the vehicle 11. The operably connectable user interface system 35 may be a computer, tablet or smartphone, communicating with the processor 25 via an app, which may be web-based. It will be obvious to the skilled person that the display screen, regardless of the external communication device being a computer, a tablet or a smartphone, may be a touchscreen. As such, the displayed buttons 24 may be provided as physical buttons and / or touch-screen buttons that are part of the graphical user interface.

[0143]

[0075] The user interface system 35 is adapted for a user to input data for defining an initial trajectory for the vehicle to follow, and to define trajectory constraints. The initial trajectory and at least a part of the trajectory constraints may be defined with respect to the map data that is visualized to the user via the graphical user interface 35. The user input may be provided via any of the aforementioned buttons and / or directly drawn on the displayed map. The map 22 displayed via the graphical user interface preferably is a visualization of the computational representation used to perform the method for obtaining optimized trajectories, and as such may also visualize at least the user input defining the initial trajectory, preferably the entire initial trajectory, and at least the trajectory constraints directly related to the agricultural operating area.

[0144]

[0076] The external communication device 20 can communicate with the vehicle 11 through the vehicle communication device 18, such as to send trajectory information. The trajectory information may be an optimized trajectory that is to be stored in the memory of the vehicle and for the controller to actuate the vehicle accordingly. The trajectory information may also consist of user input defining an initial trajectory and trajectory constraints, for generating an optimized trajectory onboard the vehicle.

[0145]

[0077] Figure 7 is a flowchart of method steps according to embodiments of the invention, outlining a method for calibrating at least one trajectory constraint parameter. The method enables parameter values being obtained without requiring complicated calculations and / or guesswork. Moreover, the trajectory parameter values obtained via this method may be closer to actual limit values than may be obtained otherwise, such as through design calculations and based on component limitations. The method 200 has the steps of:- obtaining vehicle data from expert planned trajectories 201;

[0146] - using the expert planned trajectories as input for defining initial trajectories 202; - defining a trajectory constraint comprising at least one candidate trajectory constraint parameter 203;

[0147] - calculating optimized trajectories for the initial trajectories 204;

[0148] - refining the at least one candidate trajectory constraint parameter based on an evaluation of similarity between the optimized trajectories and the expert planned trajectories 205; and

[0149] - optionally updating the candidate trajectory constraint parameter 206 in the step of defining a trajectory constraint 203 before repeating the steps thereafter.

[0150]

[0078] In the step of obtaining vehicle data from expert planned trajectories 201, datasets of trajectories as actually performed by the vehicle are collected. The datasets comprise data collected from the drive sensor system of the vehicle, while the respective trajectory driven by the vehicle is a trajectory as defined by a user, who is preferably an experienced installer.

[0151]

[0079] In the step of using the expert planned trajectories as input for defining initial trajectories 202, at least a part of the path from each of the expert planned trajectories is used as input to define an associated initial trajectory. The input may be substantially the same as indicated for step 103 of Figure 2, i.e. using the start-goal pairs or area as defined for the expert planned trajectories. Alternatively, the entire path from the expert planned trajectory may be used as input.

[0152]

[0080] In the step of defining a trajectory constraint comprising at least one candidate trajectory constraint parameter 203, one or more trajectory constraints are defined which were obeyed by the expert planned trajectories, and which depend on one or more trajectory constraint parameters that are to be calibrated. The trajectory constraints are preferably defined in a parametrized manner, such that any changes of the values for the trajectory constraint parameters can be made with more ease. The manner of defining a trajectory constraint substantially correspond to the steps 102 and 104 as described in reference to Figure 2. Although the method may be used to obtain all trajectory constraint parameters to be used for all future trajectory planning of a particular vehicle make and model, it may be preferred and of particular use to obtain any vehicle specific trajectory constraint parameters. In particular, the method 201 may be employed to determine accurate trajectory constraint parameters that are directly related to the vehicle dimensions and dynamics of a particular vehicle make and model, which may then be provided as factory settings, in the form of default or even fixedconstraint parameters, in planning software based on the trajectory planning method as outlined in Figure 2. Some non-limiting examples of such trajectory constraint parameters that are directly related to the vehicle dimensions and dynamics are: maximum driving speed, maximum acceleration, minimum turning radius and a minimum required passage width / distance.

[0153]

[0081] In the step of calculating optimized trajectories for the initial trajectories 204, the initial trajectories are optimized through an optimization-based method using a cost function formulated with the STL trajectory constraints, as also described in step 105 with reference to Figure 2.

[0154]

[0082] In the step of refining the at least one candidate trajectory constraint parameter based on an evaluation of similarity between the optimized trajectories and the expert planned trajectories 205, a derivative-free optimization method is used wherein an objective function outputs a total loss based on a loss calculated for each pair of expert planned trajectory and associated optimized trajectory using a metric that quantifies the discrepancy therebetween. The objective function is a second cost function. The optimization method uses computed loss values to approximate a relationship between the at least one candidate trajectory constraint parameter and the objective function in a surrogate model. Based on this surrogate model, an acquisition function determines the next (refined) candidate trajectory constraint parameter(s).

[0155]

[0083] In the step of updating the candidate trajectory constraint parameter 206 in the step of defining a trajectory constraint 203 before repeating the steps thereafter the refined value(s) are used to replace the previous values for the candidate trajectory constraint parameter(s). The step of updating the candidate trajectory constraint parameter 206 is marked as optional, since once the next (refined) candidate trajectory constraint parameter(s) determined by the acquisition function are the same for a predetermined number of consecutive iterations, the candidate trajectory constraint parameter(s) are determined to have the optimal value(s) and the steps are not repeated anymore. The predetermined number of consecutive iterations may be as low as 1, but is preferred to be at least 2 to prevent local maximum or minimum values being mistakenly identified as optimal values, with an upper bound for the predetermined number of consecutive iterations being based on practical considerations such as algorithm run-time.

[0156]

[0084] Figure 8 schematically illustrates another embodiment of a trajectory planner module 110’. This embodiment differs from the trajectory planner module 110 previously described in reference to Figure 3 in that a trajectory constraint parametercalibration module 160 is included, such that the module may be used to perform the method as described above in reference to Figure 7. Hereto, the input module 120’ is depicted in a slightly adjusted form, specifying that the STL trajectory constraints module 125 is adapted for obtaining the STL trajectory constraints are obtained as parametrized trajectory constraints <p = {p^ (p2,(p3] and trajectory constraint parameters p e J>. This makes the aforementioned updating of the candidate parameters easier. The expert trajectories as described in reference to Figure 7 are indicated here as input 215.

[0157]

[0085] The trajectory constraint parameter calibration module 160 as depicted uses Bayesian Optimization (BO) and thereto consists of or comprises an algorithm for performing a trajectory similarity evaluation 161, a surrogate model fitting 162, an acquisition function computation 163. The trajectory constraint parameter calibration module 160 forms a loop in the trajectory planner module, adapted to have first and second inputs from the STL trajectory constraints module 125 and the output 150 from the trajectory optimizer module 140 respectively, and to have output to the STL trajectory constraints module 125.

[0158]

[0086] The optimization process takes place over multiple iterations. At each iteration, the trajectory constraint parameter calibration module 160 takes in three inputs: the fixed expert trajectory set Zex, the current optimized trajectory set Z*, and the candidate trajectory constraint parameter set p*, of which the latter two are updated at every iteration.

[0159]

[0087] The two trajectory inputs Z*, Zexare submitted to the trajectory similarity evaluation 161, wherein the objective function outputs a total loss based on a loss calculated for each pair of expert planned trajectory and associated optimized trajectory. The difference may for example be evaluated using the Frechet distance for all trajectory pairs, resulting in the following cost function that is evaluated:

[0160] m m

[0161] L(Z', Zex)' = maxte [0,T]||zt*(t) -

[0162]

[0163] i = l i=l

[0164] The Frechet distance cost function has the advantage of having computational efficiency but is limited to pairwise trajectory comparisons. To better capture any broader differences between expert and non-expert trajectories, a more sophisticated cost function that better reflects distributional differences between trajectory sets may be used to enable a more effective use of available expert data. Non-limiting examples of such cost functions are Hausdorff Distance and Dynamic Time Warping (DTW).

[0165]

[0088] Then, the surrogate model fitting 162 is performed, using the candidatetrajectory constraint parameter set p* and the computed loss values to approximate the relationship between p* and the cost function in a surrogate model. The surrogate model is preferably a Gaussian Process (GP), which is the most commonly used type of surrogate models in Bayesian optimization do to their flexibility and ability to provide uncertainty estimates. However, the use of other known surrogate model types, such as for example Random Forests, Parzen Estimators, Bayesian Neural Networks Polynomial Chaos Expansion and Kriging, may also be considered feasible. The surrogate model provides a probabilistic model for the loss over the parameter space.

[0166]

[0089] Based on this model an acquisition function 163 is computed, quantifying the potential benefit of sampling a particular value for each candidate parameter, and used to select the next candidate parameter set p*. The acquisition function itself is optimized to find the next candidate parameter set p*. Types of acquisition functions that may be used are the logarithm of the Expected Improvement (El), the probability of improvement (PI) and upper confidence bound (UCB), with a preference for using El due to being comparatively simple to implement. The next candidate parameter set p* is then passed back to the input module 120’ for replacing the previously selected candidate parameter set p* and being used to generate a new set of optimized trajectories Z*.

[0167]

[0090] A readily available framework for generating an algorithm for the trajectory constraint parameter calibration module 160 as described in reference to Figure 8 in Python3 is the “botorch” Python library.

[0168]

[0091] The present invention has been described above with reference to a number of exemplary embodiments as shown in the drawings. It will be clear to a person skilled in the art that the scope of the invention is not limited to these examples, nor to the explicitly mentioned alternatives, but that a number of further variations and modifications thereof are possible without departing from the scope of the invention as defined in the attached claims.

Claims

CLAIMS1. An animal farm system comprising:an autonomous agricultural vehicle adapted to perform an animal related action related to at least one of cleaning up and providing feed in an agricultural operating area, wherein the vehicle comprises at least one drive wheel (19a, 19b) under the control of a control device (14), for controlling the at least one wheel to move the vehicle in accordance with control signals that are based on a pre-planned trajectory and a vehicle communication device;a data processing system comprising a memory and a processor adapted for carrying out a method comprising:receiving input defining an initial trajectory, defined as z =the initial trajectory forming a rough path, discretized as a list of instantaneous vehicle states Zi defined by trajectory parameters comprising a vehicle position in the agricultural operating area and an associated vehicle velocity;obtaining trajectory constraints the trajectory constraints comprising of:• vehicle driving limitations <pi, comprising a maximum driving speed and a minimum turning radius for the vehicle,• navigation safety constraints q>2, comprising a minimum required clearance to a particular object in or boundary of the agricultural operating area, and• a task objective q>3, defining a task for the vehicle to carry out with respect to the agricultural operating area;calculating an optimized trajectory, defined as z* =based on the initial trajectory through an optimization-based method using a cost function comprising mathematical expressions based on signal temporal logic representing the trajectory constraints to determine a level of optimization with respect to the trajectory constraints; andoutputting the optimized trajectorythe data processing system further being adapted for being operably connectable to a user interface system (35) and for sending the output optimized trajectory to the control device via the vehicle communication device (14), to be stored as the pre-planned trajectory.

2. The system according to claim 1, wherein the user interface system (35) is adapted for receiving trajectory constraints expressed in natural language and whereinthe user interface system or the data processing system comprises instructions for translating the trajectory constraints in natural language to trajectory constraints expressed in mathematical equations using signal temporal logic.

3. System according to any one of the preceding claims, wherein the autonomous agricultural vehicle is adapted to perform one or more of the tasks of removing manure, pushing feed or providing feed.

4. System according to any one of claims 1 -3, further comprising a step of forming a computational representation, such as a graph or matrix, based on map data comprising coordinates relating to boundaries of the agricultural operating area, and wherein the step of receiving input defining an initial trajectory and / or the step of obtaining trajectory constraints comprise receiving user input in relation to the computational representation.

5. System according to any one of claims 1 - 4, wherein the step of obtaining an optimized trajectory comprises iteratively:evaluating the cost function, which is preferably defined ascalculating a gradient with respect to the trajectory parameters using backpropagation, to determine how the trajectory parameters are to be adjusted; and updating the trajectory using the determined trajectory parameter adjustments until a loss converges below a predefined threshold or a predefined maximum number of iterations is reached.

6. System according to any one of claims 1 - 5, wherein the step of obtaining trajectory constraints comprises the obtaining of parametrized trajectory constraints and trajectory constraint parameters independently from one another.

7. System according to any one of claims 1 to 6, further comprising:obtaining a vehicle model representative of dynamic behavior of the autonomous agricultural vehicle, wherein a set of time-discretized equations relate a current state of the vehicle, comprising position and velocity, and control input to the vehicle which directly influences the velocity to an upcoming state of the vehicle defined asM: (, iz) i— > %;and wherein the cost function used to obtain the optimal trajectory consists of a first term, which evaluates how well the trajectory aligns with feasible vehicle behavior defined by the vehicle model, and a second term, which measures how well the trajectory satisfies the trajectory constraints.

8. System according to claim 7, wherein for the first term of the cost function, at each time step, a discrepancy is determined between a predicted next vehicle state,defined as z(t + i), that is calculated based on a current vehicle state z(t) and the vehicle model (M) and an actual next vehicle state, defined as z(t + i), and the discrepancies are aggregated across all time steps.

9. System according to any one of claims 7 - 8, wherein for the second term of the cost function, robustness values for each of the trajectory constraints are computed at each time step and combined across all constraints and time steps.

10. System according to claim 9, where the combining across all constraints includes using predetermined individual weighing factors for each constraint.

11. System according to any one of claims 7 - 10, wherein the second term of the cost function is scaled with respect to the first term of the cost function through multiplication by a scaling factor, the scaling factor having a pre-determined value such that both the first term and second term have the same order of magnitude during a first time calculation of the loss.

12. System according to any one of the preceding claims, wherein the step of receiving input defining an initial trajectory comprises receiving user input defining a start-goal position pair and automatically generating the initial trajectory between the start-goal position pair using a path planning algorithm based on a graph-based method or sampling-based method.

13. System according to any one of the preceding claims, wherein the step of receiving input defining an initial trajectory comprises receiving user input defining a goal area and generating of the initial trajectory using a coverage path planning algorithm suitable to plan a route covering substantially the entire goal area.

14. System according to any one of the preceding claims, wherein the step of receiving input defining an initial trajectory comprises receiving user input defining a rough path, such as by drawing on a user interface, and discretizing the rough path into the discretized list of trajectory parameters.

15. A computer-implemented method for trajectory planning for use by an autonomous agricultural vehicle adapted to perform an animal related action related to at least one of cleaning up and providing feed in an agricultural operating area, such as a livestock space, the method comprising:receiving input defining an initial trajectory, defined as z =the initial trajectory forming a rough path, discretized as a list of instantaneous vehicle statesdefined by trajectory parameters comprising a vehicle position in the agricultural operating area and an associated vehicle velocity;obtaining trajectory constraints the trajectory constraints comprising of:• vehicle driving limitations <pi, comprising a maximum driving speed and a minimum turning radius for the vehicle,• navigation safety constraints q>2, comprising a minimum required clearance to a particular object in or boundary of the agricultural operating area, and• a task objective q>3, defining a task for the vehicle to carry out with respect to the agricultural operating area;calculating an optimized trajectory, defined as Z‘ =based on the initial trajectory through an optimization-based method using a cost function comprising mathematical expressions based on signal temporal logic representing the trajectory constraints to determine a level of optimization with respect to the trajectory constraints; andoutputting the optimized trajectory.

16. The computer-implemented method according to claim 15, further comprising one or more of the steps or features according to claims 3 - 14 as indicated for the method that the data processing system of the system of claim 1 is adapted to carry out.

17. Computer-implemented method for calibrating at least one trajectory constraint parameter for at least one corresponding trajectory constraint, the method comprising:obtaining vehicle data from a plurality of expert planned trajectories, defined as zex= and performed by the autonomous agricultural vehicle;using the expert planned trajectories as input to define the initial trajectories; defining a trajectory constraint comprising a parametrized trajectory constraint and at least one candidate trajectory constraint parameter p*, the at least one candidate trajectory constraint p* corresponding to at least one trajectory constraint parameter to be calibrated;calculating optimized trajectories, defined as z* =for the initial trajectories through the optimization-based method using a cost function comprising the at least one trajectory constraint is as a mathematical expression using signal temporal logic to determine the level of optimization with respect to the trajectory constraint; and refining the value of the at least one candidate constraint parameter p* to an updated value at which a objective function, expressed as T( *,e%), evaluating the similarity between the optimized trajectories and the expert planned trajectories is minimal, using a derivative-free optimization method wherein the objective function outputs a total loss based on a loss calculated for each pair of expert planned trajectory and associated optimized trajectory using a metric that quantifies the discrepancytherebetween; andusing the optimal value for the at least one trajectory constraint parameter for the corresponding trajectory constraints in the method according to any one of claims 15 -16 or the method of the system according to any one of claims 1 - 14.

18. Method according to claim 17, wherein each of the at least one candidate constraint parameter p‘ is a normalized value.

19. Method according to claim 17 or 18, wherein the at least one trajectory constraint is a vehicle driving limitation cpi and / or a navigation safety constraint q>2.

20. The method according to any one of claims 17 to 19, wherein the derivative-free optimization method is a Bayesian method.

21. The method according to claim 20, wherein the Bayesian method further comprises:updating a surrogate model, which is preferably a Gaussian Process (GP) model, using the calculated loss for each trajectory pair to approximate a relationship between the at least one candidate constraint parameter p* and the objective function;modelling of an acquisition function, preferably the logarithm of the Expected Improvement, to determine an updated value for the at least one candidate constraint parameter p*.

22. A computer program comprising instructions which, when the program is executed by a computer, cause the computer to carry out the method of any one of the method claims 15 - 21.

23. A data processing system comprising a memory and a processor adapted for carrying out the method of any one of claims 15 - 21.

24. A graphical user interface for use with the computer program according to claim 22 or the data processing system according to claim 23, adapted for a user to input data for the initial trajectory forming a rough path and to input a trajectory constraint.

Images