WO2017143631A1 - 一种水产养殖作业船自动导航路径跟踪方法 - Google Patents

一种水产养殖作业船自动导航路径跟踪方法 Download PDF

Info

Publication number
WO2017143631A1
WO2017143631A1 PCT/CN2016/075946 CN2016075946W WO2017143631A1 WO 2017143631 A1 WO2017143631 A1 WO 2017143631A1 CN 2016075946 W CN2016075946 W CN 2016075946W WO 2017143631 A1 WO2017143631 A1 WO 2017143631A1
Authority
WO
WIPO (PCT)
Prior art keywords
target
speed
point
path
ship
Prior art date
Application number
PCT/CN2016/075946
Other languages
English (en)
French (fr)
Inventor
赵德安
罗吉
孙月平
洪剑青
Original Assignee
江苏大学
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by 江苏大学 filed Critical 江苏大学
Publication of WO2017143631A1 publication Critical patent/WO2017143631A1/zh

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions

Definitions

  • the invention relates to the field of automatic navigation path tracking, in particular to the automatic navigation path tracking of aquaculture farming vessels.
  • the path tracking control method of the hull is the key link. Whether the hull can be controlled strictly according to the set path will directly affect the efficiency of the water grass cleaning and the uniformity of the bait delivery.
  • the efficient path tracking method can enhance the real-time performance of the navigation control system and improve the working efficiency of the aquaculture operation vessel.
  • the object of the present invention is to solve the problem that the existing agricultural machinery navigation control method is complicated and the path tracking effect is not ideal, so that the working ship can accurately sail according to the set path, improve the efficiency of the water grass cleaning and the uniformity of the feeding.
  • the aquaculture involved in the present invention is applicable to various aquatic products such as fish, shrimp, and crab.
  • the present invention solves the above problems, and the technical solution adopted includes the following steps:
  • the first step using the high-precision GPS system based on the RTK mode to collect the latitude and longitude coordinates of each corner point that can reflect the contour of the target work area, and obtain the target route and the speed of the work ship through the trajectory planning, and determine the initial target path;
  • Step 2 Through the high-precision GPS system based on RTK mode and the inertial navigation module, the current position, heading and speed of the working ship are obtained, and the navigation speed of the working vessel is set and the target path is determined.
  • the third step using the real-time interpolation method to calculate the tracking target point P of the working ship: according to the calculation formula of the vertical intersection point of the point to the straight line, the projection point of the current position of the working ship on the current target path is obtained;
  • the projection point is the starting point, along the mesh
  • the tracking target point P is obtained by extending a certain distance from the forward direction of the target path, wherein the distance value of the forward extension is determined by factors such as the hull size of the working vessel and the speed of navigation;
  • the fourth step comparing the pose and motion state data of the work ship with the tracking target point and the target path of the work ship, and obtaining the heading deviation and speed deviation of the work ship, and deriving the work ship by PD and PI control algorithms respectively.
  • the heading control amount and the speed control amount, and the two are combined to obtain the left and right paddle control amount of the working ship;
  • Step 5 Real-time navigation control of the work ship according to the obtained control amount to realize path tracking.
  • the trajectory planning is performed to obtain the target route and the speed of the work ship, and the initial target path is determined.
  • the specific steps are as follows:
  • Step 1.1 Calculate the distance ⁇ L 0 , L 1 , ..., L n ⁇ of the work ship and each corner point ⁇ J 0 , J 1 , ..., J n ⁇ which can reflect the contour of the target work area, and The nearest point is the starting point of the target route T 0 ;
  • Step 1.2 Based on the latitude and longitude coordinates of each corner point, the target route of the work ship is derived according to the shortest path principle:
  • the corresponding target point set is:
  • the x coordinate represents the longitude
  • the y coordinate represents the latitude
  • T N is the end point of the target route of the work ship
  • step 1.3 the target cruising speed V L and the target turning speed V S are set in conjunction with the target route.
  • step 1.4 it is determined that the path T 0 ⁇ T 1 is the target path at which the work ship starts.
  • the sailing speed of the working vessel is set and it is determined whether the target path switching needs to be performed.
  • the specific steps are as follows:
  • Step 2.1 calculate the distance D 1 and the target path workboat end T k + 1 in accordance with latitude and longitude coordinates of the current position Q workboat end of the path and the target is T k + 1;
  • Step 2.2 if the distance D 1 calculated in step 2.1 is greater than the deceleration radius R 1 , it is considered that the working vessel has not approached the end of the target path, and the sailing speed V 1 of the working vessel is set to be a larger cruising speed V L ; 1 is less than or equal to the deceleration radius R 1 , it is considered that the work ship has approached the end of the target path, and the sailing speed V 1 of the work ship is set to be a small turning speed V S ;
  • Step 2.3 if the distance D 1 calculated in step 2.1 is greater than the target radius R 2 , it is considered that the work ship has not reached the end of the target path, and the target path is not required to be switched; if D 1 is less than or equal to the target radius R 2 , then It is considered that the work ship has reached the end of the target path, and the current target path needs to be switched from T k ⁇ T k+1 to T k+1 ⁇ T k+2 ;
  • R 1 and R 2 are determined by factors such as the hull size of the work ship.
  • Step 3.1 Calculate the projection point of the current position Q(x 1 , y 1 ) of the work vessel on the current target path T K (x TK , y TK ) ⁇ T K+1 (x TK+1 , y TK+1 ) M(x 2 , y 2 ), the formula is:
  • the abscissa x represents longitude and the ordinate y represents latitude.
  • Step 3.2 Calculate the distance d between the point M of the projection point and the end point of the target path T k+1 from the formula of calculating the distance between the two points by two points of latitude and longitude coordinates.
  • the formula is:
  • 6378137 is the radius of the earth, and the unit is m.
  • Step 3.3 Calculate the coordinates of the point coordinates of P(x 3 , y 3 ) by extending the distance D 2 to P(x 3 , y 3 ) along the T k ⁇ T k+1 direction with the projection point M as the starting point. for:
  • the value of D 2 in the formula is determined by factors such as the hull size of the work vessel, the speed of navigation, and the like.
  • the pose and motion state data of the work vessel are compared with the tracking target point and the target path of the work ship, and the heading deviation and the speed deviation of the work ship are calculated, and the specific steps are as follows:
  • Step 4.1 Determination of heading deviation ⁇ :
  • the heading deviation ⁇ of the working vessel is the difference between the target heading ⁇ 1 of the working vessel and the current heading ⁇ 2 , and the formula is:
  • the current heading ⁇ 2 is acquired by the inertial navigation module, and the target heading ⁇ 1 is calculated according to the current latitude and longitude coordinate Q(x, y) of the working ship and the latitude and longitude coordinates P(x, y) of the tracking target point;
  • Step 4.2 Determination of the speed deviation ⁇ V:
  • the speed deviation ⁇ V of the work vessel is the difference between the target speed V 1 of the work vessel and the current speed V 2 , and the formula is:
  • the target speed V 1 is obtained by the second step, and the current speed V 2 is acquired by the inertial navigation module.
  • the heading control amount and the speed control amount of the work ship are derived by the PD and PI control algorithms, respectively, and the two are combined to obtain the final control amount of the work vessel, that is, the left and right open wheel speeds, and the specific steps are as follows: :
  • Step 4.a derive the heading control quantity by the PD control algorithm ⁇ * :
  • the heading control quantity at the i-th moment ⁇ * (i) is derived from the following formula:
  • ⁇ (i) and ⁇ (i-1) are the heading deviations at the i-th and i-1th moments respectively
  • K p is the proportional parameter
  • K d is the differential parameter
  • K p and K d parameters are all set according to the experimental results. ;
  • Step 4.b by the speed PI control algorithm calculate the control amount V *: Speed control amount at time i V * (i) calculated by the following equation derived:
  • ⁇ V(i) is the i-th speed deviation
  • K p is the proportional parameter
  • K i is the integral parameter
  • K p and K i parameters are all set on the spot according to the experimental effect
  • Step 4.c the combined heading control quantity ⁇ * and the speed control amount V * get the left and right paddle control amount of the working ship: the invention uses the speed control quantity V * as a reference, and the left and right paddle wheel control amounts are respectively added and subtracted To decouple, the control quantities of the left and right paddles are:
  • the left Ming wheel is:
  • the right wheel is:
  • the present invention has the following technical effects: Firstly, the high-precision GPS system based on the RTK mode collects the latitude and longitude coordinates of each corner point that can reflect the contour of the work area, and obtains the target route and the speed of the work ship through the trajectory planning.
  • the irregular work area is planned as a plurality of straight-line paths connected end to end, which reduces the difficulty of obtaining the target route of the work ship; then, the current position of the work ship is collected in real time through the high-precision GPS system based on the RTK mode and the inertial navigation module.
  • the pose and motion state data such as heading and speed are set according to the distance between the work ship and the current target path end point and determine whether the target path is required to be switched.
  • the work ship can be improved under the premise of ensuring the tracking accuracy of the work ship path.
  • the work efficiency; then the real-time interpolation method is used to calculate the tracking target point of the work ship, which can improve the sensitivity of the work ship to the heading deviation, so that the tracking trajectory of the work ship is closer to the target path; then the posture of the work ship will be And motion state data and tracking target points of the work ship Comparing the target path analysis, course deviation and velocity deviation operation of the ship, respectively PD, PI control algorithm calculate the heading of the ship and the control amount operation Air Speed control amount, and the two are combined to obtain the left and right paddle control amount of the working ship.
  • the speed control since the output of the speed control directly acts on the open wheel speed instead of the ship speed, the speed control adopts PI control, and the speed is eliminated through the integral link.
  • the real-time navigation control of the working ship is performed according to the obtained control amount to realize the path tracking.
  • the invention can reflect the heading error in real time, and the implementation method is simple and effective.
  • the tracking target point of the working ship is calculated online by real-time interpolation, and the limited number of corner points are refined into numerous tracking. The target point can significantly improve the effect of automatic navigation path tracking, and provide guarantee for efficient completion of water grass cleaning and uniform feeding operation.
  • Figure 1 is a structural diagram of aquaculture operation vessel
  • Figure 2 is a block diagram showing the principle structure of the automatic navigation control system of the workboat
  • Figure 3 is a schematic view of the outline of the work area
  • Figure 4 is a schematic view showing the outline of the work area using a plurality of corner points
  • Figure 5 is a schematic diagram of calculating the distance between the work ship and each corner point
  • Figure 6 is a schematic diagram of setting a target route of a work ship
  • Figure 7 is a schematic diagram showing the setting of the navigation speed and the target path switching of the working ship
  • FIG. 8 is a schematic diagram of calculating a tracking point of a work ship based on a real-time interpolation method
  • Figure 9 is a track diagram of the automatic navigation path tracking of the workboat.
  • Figure 10 is a graph showing the tracking error of the automatic navigation path of the workboat.
  • the structure of the aquaculture work ship used in this embodiment is shown in Fig. 1.
  • the ship includes a hull 1, a water grass cutting device 2 at the front end of the hull 1, a water grass collecting device 3, a grass collecting bin 4 in the middle of the hull 1, and a water grass flattening device 5.
  • the paddle wheel 6 on both sides of the middle portion of the hull 1 and the console 7 and the feeding machine 8 at the rear end of the hull 1.
  • the hull 1 is equipped with Trimble's high-precision GPS system, SBG IG-500A MEMS high-precision inertial navigation module, ARM9 main control board and paddle motor controller to form the automatic navigation control system of the work ship, among them, GPS system
  • the serial port is connected with the inertial navigation module, and the GPS system and the inertial navigation module are connected to the ARM9 main controller through the serial port.
  • the structural block diagram of the principle is shown in Figure 2.
  • the high-precision GPS system consists of two parts: the base station and the mobile station.
  • the base station and the mobile station transmit data through the radio station, and the positioning accuracy can reach centimeter level.
  • the ARM9 main control board completes data acquisition, data processing and path tracking algorithm. And control command output and other functions, the program is written in the Windows 7 environment using the ADS1.2 development platform.
  • Step 1 Set the target route and speed of the workboat and determine the starting target path
  • the invention uses a high-precision GPS system based on the RTK mode to collect latitude and longitude coordinates of each corner point that can reflect the contour of the target work area, obtains the target route and the speed of the work ship through the trajectory planning, and determines the initial target path. Since the target work area of the aquaculture work ship is mostly irregular, the present invention first describes the target work area by using a plurality of corner points ⁇ J 0 , J 1 , ..., J n ⁇ which can reflect the outline of the target work area. Then calculate the formula for the distance between two points based on the known latitude and longitude of two points:
  • the travel route of the work ship is derived according to the shortest path principle:
  • the corresponding target point set is:
  • the x coordinate represents the longitude
  • the y coordinate represents the latitude
  • T N is the end of the target path of the work ship.
  • the target cruising speed V L and the target turning speed V S are set in combination with the target route, wherein when the value of V L is too large, the problem of incomplete cleaning of the water grass and insufficient feeding amount may occur, and the value is too small. Reduce the efficiency of the work boat. When the V S value is too large, it will increase the difficulty of turning the work boat. When it is too small, it will increase the turning time and reduce the working efficiency of the working ship. Setting the appropriate cruising speed V L and the turning speed V S can ensure the accuracy of the tracking path of the working ship. Under the premise, improve the working efficiency of the work boat.
  • the irregular work area is planned as a plurality of straight-line paths connected end to end, which reduces the difficulty of obtaining the target route of the work ship compared with the existing technology.
  • Step 2 Set the sailing speed of the working boat and determine if the target path switching is required.
  • the invention collects the position and motion state data of the current position, heading and speed of the working ship through the high-precision GPS system and the inertial navigation module based on the RTK mode, sets the navigation speed of the working ship and determines whether the target path switching is needed.
  • the target route of the work ship is composed of a plurality of straight target paths connected end to end, and the end point of the target path is the turning point of the route.
  • it is necessary to set the sailing speed of the work ship in real time and update the current target path of the work ship, as shown in FIG. 7 .
  • the Q(x, y) point is the current position of the work ship
  • T k ⁇ T k+1 is the current target path
  • T k+1 ⁇ T k+2 is the next target path. Since the work ship has a large inertia when navigating in the water, in order to prevent the work ship from rushing out of the set target route, it is necessary to reduce the ship speed and switch the target path in advance.
  • the main controller continuously calculates the distance D 1 between the current position of the work ship and the end point of the target path while the work ship approaches the end point of the target path. When D 1 is greater than the deceleration radius R 1 , the work ship is considered not to approach the end of the target path.
  • the values of R 1 and R 2 are determined by factors such as the hull size of the work ship. If the R 1 value is too large, the work boat will decelerate prematurely and reduce the working efficiency of the working ship; if the R 1 value is too small, the work ship will decelerate too late, causing the working ship to rush out of the target path. If the R 2 value is too large or too small, the work ship will switch the target path too early or too late, increasing the turning radius of the work boat. Setting the appropriate R 1 and R 2 can improve the working efficiency of the working vessel while ensuring the accuracy of the tracking path of the working vessel.
  • Step 3 Use the real-time interpolation method to get the tracking target point of the work ship.
  • the invention uses the real-time interpolation method to calculate the tracking target point of the work ship in real time, and the calculation diagram thereof is shown in FIG. 8.
  • T K (x TK , y TK ), T K+1 (x TK+1 , y TK+1 ) is the starting and ending point of the current target path.
  • the abscissa x represents longitude
  • the ordinate y represents latitude
  • the target path direction is T k ⁇ T k+1 . If the work ship directly uses the T k+1 point as the tracking target point, the travel route Q ⁇ T k+1 will largely deviate from the target path T k ⁇ T k+1 .
  • the vertical line from the Q point to the path T k ⁇ T k+1 intersects at the point M(x 2 , y 2 ). According to the calculation formula of the vertical intersection point from the point to the line, the coordinates of the M point can be obtained as:
  • 6378137m is the radius of the earth, and the unit of d is m.
  • the value of D 2 is determined by factors such as the hull size and navigation speed of the work ship. If the value of D 2 is too large, the calculated tracking target point is far from the working ship, and it is difficult when the work ship deviates from the target path. Callback; if the value of D 2 is too small, it will cause the work ship to be too sensitive to the change of heading, causing the system to oscillate.
  • the main controller will automatically navigate the workboat with point P as the tracking target point.
  • the tracking target point also moves forward to P' until it reaches the T K+1 point.
  • This method can reflect the heading error in real time, and refines a limited number of corner points into an infinite number of tracking target points, which can make the navigation path of the work ship more close to the set path, and can significantly improve the effect of the automatic navigation path tracking of the working ship. .
  • Step 4 Double closed loop control of heading and speed
  • the invention compares the position and motion state data of the work ship with the tracking target point and the target path of the work ship, and obtains the heading deviation and speed deviation of the work ship; and derives the heading control of the work ship through the PD and PI control algorithms respectively.
  • the amount and the speed control amount are combined and the two are combined to obtain the left and right paddle control amount of the working ship.
  • the invention adopts a bright wheel which can be reversed on both sides as a driving device, and designs a corresponding heading closed loop and a speed closed loop decoupling method to ensure that the two can not interfere with each other.
  • Implement decoupling control The current heading ⁇ 2 is acquired by the inertial navigation module, and the target heading ⁇ 1 is calculated according to the current latitude and longitude coordinate Q(x, y) of the working ship and the latitude and longitude coordinates P(x, y) of the tracking target point; the target speed V 1 is determined by the Obtained in 2 steps, the current speed V 2 is acquired by the inertial navigation module.
  • K p and K d are the ratio and differential coefficient of the PD controller, and ⁇ is the difference between the target heading ⁇ 1 and the current heading ⁇ 2 , ie
  • the adjustment amount V * of the speed closed loop is obtained.
  • K p , K i are the proportional and integral parameters of the speed PI controller, and ⁇ V is the difference between the target speed V 1 and the current speed V 2 , ie
  • the main antenna of the GPS device is placed at the midpoint of the left and right paddle lines to realize the decoupling control of the heading closed loop and the speed closed loop.
  • the left and right paddle speeds are with
  • the work boat will be turned to the original point with the main antenna as the center point.
  • the speed value of the GPS device output is 0, that is, the heading adjustment does not affect the speed; when the speed of the left and right paddles is simultaneously
  • V * the work ship will keep the current course forward or backward.
  • the change of the course of the work ship is 0, that is, the speed adjustment will not affect the heading.
  • the speed adjustment amount V * is used as a reference, and the left and right paddle speeds are respectively added and subtracted. To decouple, the speeds of the left and right paddles are:
  • the ARM9 main controller sets the speed of the left and right paddle wheels according to the values of U 1 and U 2 , and flexibly controls the hull to perform acceleration, deceleration, left turn or right turn, and control the hull to sail according to the set route.
  • Step 5 Perform real-time navigation control on the work ship according to the obtained control amount to realize path tracking.
  • the target work area of the aquaculture work ship is set to the irregular shape shown in FIG. 3, and four target work areas are set according to the shape characteristics of the target work area.
  • the corner points of the contour ⁇ J 0 , J 1 , J 2 , J 3 ⁇ , the original curve path is decomposed into a plurality of end-to-end linear paths as shown in FIG. 4, and the latitude and longitude coordinates of the corner points are collected using a high-precision GPS system.
  • the distances ⁇ L 0 , L 1 , L 2 , L 3 ⁇ of the current position Q(x, y) of the work boat from the respective corner points are respectively calculated, wherein Lat represents longitude, Lng represents latitude, and unit of L is m.
  • the corner point closest to the work boat is taken as the starting point T 0 of the travel route, and its schematic diagram is as shown in FIG. 5.
  • the travel route of the work boat is planned, and the travel route of the work ship is:
  • the corresponding travel route data of the work ship is:
  • the irregular work area is planned as a plurality of straight-line paths connected end to end, which reduces the difficulty in obtaining the travel route data of the work ship compared with the existing technology.
  • the sailing speed of the working vessel is set and it is judged whether or not the route switching is required.
  • the values of R 1 and R 2 should be determined by factors such as the hull size of the working vessel.
  • the real-time interpolation method is used to obtain the tracking target point of the working ship, and the value of the forward reach distance D 2 is set to 2.5 m, which can be quickly called back when the work vessel deviates from the target path and does not cause the system. Shock.
  • the automatic navigation path tracking trajectory diagram of the work ship in this embodiment is shown in FIG. 9, and the error curve during operation is as shown in FIG. It can be seen from the experimental results that the work ship is not easy to deviate from the target path at the turning point, and the callback will be quickly adjusted when the hull is deviated.
  • the maximum error of the deviation from the target path when turning and straightening is 0.36m and 0.09m respectively. It can well meet the requirements of clearing and evenly feeding the crab in the crab pond.

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Navigation (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

一种水产养殖作业船自动导航路径跟踪方法。包括：第一步：采集反映作业区轮廓的角点的位置信息，经轨迹规划，得到作业船的目标航线和航速；第二步：采集作业船实时的位置、航向和航速等数据，设定作业船的航行速度并判断是否需要切换目标路径；第三步：采用实时插点的方法计算出作业船的跟踪目标点；第四步：将作业船的位姿和运动状态数据与跟踪目标点及目标路径进行对比分析，得到航向与航速偏差，通过PD、PI算法得出航向与航速控制量，最终得到左右明轮控制量；第五步：根据最终的控制量对作业船进行实时导航控制。该方法能显著提高自动导航路径跟踪的效果，为水草清理和均匀投饵作业的高效完成提供了保障。

Description

一种水产养殖作业船自动导航路径跟踪方法 技术领域
本发明涉及自动导航路径跟踪领域，特别是河蟹养殖用水产养殖作业船的自动导航路径跟踪。
背景技术
我国是河蟹养殖大国，而河蟹养殖过程中主要存在着水草清理和均匀投饵两大难题。水草是河蟹喜食的饵料，同时也是河蟹的栖身场所，水草可以通过光合作用增氧，还可以吸收水中的营养盐，改善水体质量，防止水体富营养化。然而，若不及时对水草进行清理，露出水面的水草会阻碍空气中的氧气进入水体，腐烂的水草会过多地消耗水中的溶氧，导致池塘底质、水质的恶化；同时，由于河蟹的活动范围有限，只能在自身附近区域觅食，饵料的投放不均会导致局部饵料过多或过少，饵料过多时会导致浪费、增加养殖的成本，饵料过少时又容易引起河蟹因抢食而相互残杀。因此，用于水草清理或均匀投饵作业的水产养殖自动作业船具有重要的实用价值。水产养殖作业船导航控制系统中，船体的路径跟踪控制方法是其中的关键环节，能否控制船体严格按照设定路径航行会直接地影响到水草清理的效率和饵料投放的均匀度，一种简单、高效的路径跟踪方法可以增强导航控制系统的实时性，提高水产养殖作业船的工作效率。
目前已有一些自动导航路径跟踪方法，但都存在着一些不足，如申请号为200910082462.1的专利“农业机械的导航方法、导航装置和农业机械”根据农业机械的两轮运动学模型以及最优控制理论中的最小时间问题计算获得农业机械的预期路径信息，其计算量大、实时性不高；申请号为200710029018.4的专利“一种农业机械的导航控制方法”需要通过测量多个等间距的目标点来确定行进路线并通过查表的方法进行导航跟踪，实际操作过程复杂，灵活性差，很难达到较高的精度。
发明内容
本发明的目的是为了解决现有的农业机械导航控制方法复杂、路径跟踪效果不理想的问题，使作业船能精确按照设定路径航行，提高水草清理的效率以及投饵的均匀度。本发明所涉及的水产养殖适用于各种鱼、虾、蟹等水产品。
本发明为解决上述问题，采用的技术方案包含以下步骤：
第一步：使用基于RTK模式的高精度GPS系统采集能反映目标作业区轮廓的各个角点的经纬度坐标，经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径；
第二步：通过基于RTK模式的高精度GPS系统以及惯导模块采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，设定作业船的航行速度并判断是否需要进行目标路径切换；
第三步：采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目 标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度等因素确定；
第四步：将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船的左右明轮控制量；
第五步：根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。
进一步，所述第一步中经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径，具体步骤如下：
步骤1.1，计算作业船与能反映目标作业区轮廓的各个角点{J0,J1,...,Jn}的距离{L0,L1,...,Ln}，并将距离最近的点作为目标航线的起点T0；
步骤1.2，以各个角点的经纬度坐标为基准，按照路径最短原则推算出作业船的目标航线：
T0→T1→...→Tk-1→TK→TK+1→...→TN
对应的目标点集合为：
{T0,T1,...,Tk-1,TK,TK+1,...,TN}
其经纬度坐标为：
{(xT0,yT0),(xT1,yT1),...,(xTk-1,yTk-1),(xTk,yTk),(xTk+1,yTk+1),...,(xTn,yTn)}
其中x坐标表示经度，y坐标表示纬度，TN为作业船目标航线的终点；
步骤1.3，结合目标航线设定作业船的目标巡航速度VL和目标转弯速度VS。
步骤1.4，确定路径T0→T1为作业船起始的目标路径。
进一步，所述第二步中设定作业船的航行速度并判断是否需要进行目标路径切换，具体步骤如下：
步骤2.1，根据作业船当前位置Q及目标路径终点Tk+1的经纬度坐标计算出作业船与目标路径终点Tk+1的距离D1；
步骤2.2，若步骤2.1中计算得到的距离D1大于减速半径R1，则认为作业船还未逼近目标路径终点，设定作业船的航行速度V1为较大的巡航速度VL；若D1小于或等于减速半径R1，则认为作业船已逼近目标路径终点，设定作业船的航行速度V1为较小的转弯速度VS；
步骤2.3，若步骤2.1中计算得到的距离D1大于目标半径R2，则认为作业船还未到达目标路径终点，不需要进行目标路径的切换；若D1小于或等于目标半径R2，则认为作业船已到达目标路径终点，需要将当前的目标路径由Tk→Tk+1切换成Tk+1→Tk+2；
其中，R1、R2的值由作业船的船体尺寸等因素确定。
进一步，所述第三步的具体过程如下：
步骤3.1，计算作业船的当前位置Q(x1,y1)在当前目标路径TK(xTK,yTK)→TK+1(xTK+1,yTK+1)上的投影点M(x2,y2)，计算的公式为：
Figure PCTCN2016075946-appb-000001
Figure PCTCN2016075946-appb-000002
其中，横坐标x表示经度，纵坐标y表示纬度。
步骤3.2，由两点经纬度坐标计算两点距离的公式得到投影点M点到目标路径终点Tk+1点的距离d，计算的公式为：
Figure PCTCN2016075946-appb-000003
其中，6378137为地球半径，单位为m。
步骤3.3，以投影点M点作为起始点沿着Tk→Tk+1方向前伸距离D2至P(x3,y3)点，P(x3,y3)点坐标的计算公式为：
Figure PCTCN2016075946-appb-000004
Figure PCTCN2016075946-appb-000005
式中D2的值由作业船的船体尺寸、航行速度等因素确定。
进一步，所述第四步中，将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，计算得到作业船的航向偏差与速度偏差，具体步骤如下：
步骤4.1，航向偏差ΔΨ的确定：作业船的航向偏差ΔΨ为作业船的目标航向Ψ1与当前航向Ψ2的差值，计算的公式为：
ΔΨ＝Ψ1-Ψ2
其中，当前航向Ψ2由惯导模块采集得到，目标航向Ψ1根据作业船当前的经纬度坐标Q(x,y)与跟踪目标点的经纬度坐标P(x,y)计算得到；
步骤4.2，航速偏差ΔV的确定：作业船的航速偏差ΔV为作业船目标速度V1与当前速度V2的差值，计算的公式为：
ΔV(k)＝V1-V2
目标速度V1由第二步得到，当前速度V2由惯导模块采集得到。
进一步，所述第四步中，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船最终的控制量即左右明轮速度，具体步骤如下：
步骤4.a，通过PD控制算法推算出航向控制量Ψ*：第i时刻的航向控制量Ψ*(i)由以下公式推算得出：
Ψ*(i)＝Kp×ΔΨ(i)+Kd×(ΔΨ(i)-ΔΨ(i-1))
其中，ΔΨ(i)与ΔΨ(i-1)分别为第i、i-1时刻的航向偏差，Kp为比例参数，Kd为微分参数，Kp和Kd参数均根据实验效果现场整定；
步骤4.b，通过PI控制算法推算出航速控制量V*：第i时刻的航速控制量V*(i)由以下公式推算得出：
Figure PCTCN2016075946-appb-000006
其中，ΔV(i)为第i的航速偏差，Kp为比例参数，Ki为积分参数，Kp和Ki参数均根据实验效果现场整定；
步骤4.c，融合航向控制量Ψ*与航速控制量V*得到作业船左右明轮控制量：本发明以速度控制量V*为基准，左右明轮控制量分别加上和减去
Figure PCTCN2016075946-appb-000007
来进行解耦，得到左右明轮的控制量分别为：
左明轮为：
Figure PCTCN2016075946-appb-000008
右明轮为：
Figure PCTCN2016075946-appb-000009
本发明采用上述技术后，具有以下技术效果：本发明首先通过基于RTK模式的高精度GPS系统采集能反映作业区轮廓的各个角点的经纬度坐标，经轨迹规划得到作业船的目标航线和航速，将不规则的作业区域规划为多个首尾相连的直线路径，降低了作业船目标航线获取的难度；然后通过基于RTK模式的高精度GPS系统以及惯导模块实时地采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，根据作业船与当前目标路径终点的距离设置作业船的航行速度并判断是否需要进行目标路径切换，可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率；接着采用实时插点的方式计算得到作业船的跟踪目标点，可提高作业船对于航向偏差的敏感度，使作业船的跟踪轨迹更加地逼近目标路径；接着将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差，分别通过PD、PI控制算法推算出作业船的航向控制量及航 速控制量，并将两者融合得到作业船的左右明轮控制量，其中，由于航速控制的输出量直接作用于明轮转速而不是船速，航速控制采用PI控制，通过积分环节以消除航速的稳态误差，航向控制采用PD控制，通过微分环节以增大阻尼系数、保持船舶的稳定；最后，根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。与现有方法相比，本发明可以实时地反映出航向的误差，实现方法简单、有效，通过实时插点的方式在线计算出作业船的跟踪目标点，将有限个角点细化成无数个跟踪目标点，能显著提高自动导航路径跟踪的效果，为水草清理和均匀投饵作业的高效完成提供保障。
附图说明
图1为水产养殖作业船结构图；
图2为作业船自动导航控制系统的原理结构框图；
图3为作业区轮廓示意图；
图4为使用多个角点描述作业区轮廓示意图；
图5为计算作业船与各个角点距离示意图；
图6为设定作业船目标航线示意图；
图7为作业船设定航行速度及目标路径切换示意图；
图8为基于实时插点方法计算作业船跟踪目标点示意图；
图9为作业船自动导航路径跟踪轨迹图；
图10为作业船自动导航路径跟踪误差曲线图。
具体实施方式
下面将结合实施例对本发明做进一步的详细描述，但本发明的实施方式不限于此。
本实施例采用的水产养殖作业船的结构如图1所示，船上包括船体1，船体1前端的水草切割装置2和水草收集装置3，船体1中部的集草仓4、水草铺平装置5，船体1中部两侧的明轮6，以及船体1后端的操作台7和投饵机8。船体1上装载了Trimble公司的高精度GPS系统、SBG IG-500A MEMS高精度惯导模块、ARM9的主控制板和明轮电机控制器等设备，构成作业船自动导航控制系统，其中，GPS系统与惯导模块采用串口连接，GPS系统、惯导模块均通过串口与ARM9主控制器连接。其原理结构框图如图2所示。
高精度GPS系统由基站和移动站两个部分组成RTK测量模式，基站和移动站通过无线电台进行数据传输，其定位精度可达厘米级；ARM9主控制板完成数据采集、数据处理、路径跟踪算法及控制指令输出等功能，其程序是采用ADS1.2开发平台在Windows 7环境下编写完成的。
具体实施步骤如下：
第1步.设定作业船的目标航线和航速，并确定起始目标路径
本发明使用基于RTK模式的高精度GPS系统采集能反映目标作业区轮廓的各个角点的经纬度坐标，经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径。由于水产养殖作业船的目标作业区域多为不规则的形状，本发明首先使用多个能反映目标作业区域轮廓的角点{J0,J1,...,Jn}来描述目标作业区域；然后根据已知两点的经纬度计算两点间距离的公式：
Figure PCTCN2016075946-appb-000010
分别计算出作业船的当前位置Q(x,y)同各个角点{J0,J1,...,Jn}的距离{L0,L1,...,Ln}，并将距离最近的点作为行进路线的起点T0。其中，Lat表示经度，Lng表示纬度，L的单位为m。
接着，以各个角点的经纬度坐标为基准，按照路径最短原则推算出作业船的行进路线：
T0→T1→...→Tk-1→TK→TK+1→...→TN
对应的目标点集合为：
{T0,T1,...,Tk-1,TK,TK+1,...,TN}
其经纬度坐标为：
{(xT0,yT0),(xT1,yT1),...,(xTk-1,yTk-1),(xTk,yTk),(xTk+1,yTk+1),...,(xTn,yTn)}
其中x坐标表示经度，y坐标表示纬度，TN为作业船目标航线的终点。
接着，结合目标航线设定作业船的目标巡航速度VL和目标转弯速度VS，其中，VL值过大时会导致水草清理不彻底以及投饵量不足等问题，其值过小时又会降低作业船的作业效率。VS值过大时会增加作业船转弯的难度，过小时又会增加转弯时间，降低作业船的工作效率，设置合适的巡航速度VL和转弯速度VS可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率。
最终，确定路径T0→T1为作业船起始的目标路径。
这种方式将不规则的作业区域规划为多个首尾相连的直线路径，与现有的技术相比，降低了作业船目标航线获取的难度。
第2步.设定作业船航行速度并判断是否需要进行目标路径切换
本发明通过基于RTK模式的高精度GPS系统以及惯导模块采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，设定作业船的航行速度并判断是否需要进行目标路径切换。本发明中作业船的目标航线由多条首尾相连的直线目标路径组成，目标路径的终点即为路线的转弯点。为了保证作业船自动导航的精度，需要实时地设定作业船的航行速度并更新作业船当前的目标路径，其示意图如图7所示。
Q(x,y)点为作业船的当前位置，Tk→Tk+1为当前的目标路径，Tk+1→Tk+2为下一条目标路径。由于作业船在水中航行时具有很大的惯性，为防止作业船冲出设定的目标航线，必须降低船速并提前进行 目标路径的切换。主控制器在作业船逼近目标路径终点的过程中不断地计算作业船当前位置与目标路径终点的距离D1，当D1大于减速半径R1时，则认为作业船还未逼近目标路径终点，设定作业船的航行速度V1为较大的巡航速度VL；若D1小于或等于减速半径R1，则认为作业船已逼近目标路径终点，设定作业船的航行速度V1为较小的转弯速度VS；若D1大于目标半径R2，则认为作业船还未到达目标路径终点，不需要进行目标路径的切换；若D1小于或等于目标半径R2，则认为作业船已到达目标路径终点，需要将当前的目标路径由Tk→Tk+1切换成Tk+1→Tk+2。
其中，R1、R2的值由作业船的船体尺寸等因素确定。R1值过大时会导致作业船过早减速，降低作业船的工作效率；R1值过小时又会导致作业船减速太晚，使得作业船冲出目标路径。R2值过大或过小时会导致作业船过早或过晚地切换目标路径，增大作业船的转弯半径。设置合适的R1、R2可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率。
第3步.采用实时插点的方式得到作业船的跟踪目标点
本发明采用实时插点的方法实时地计算出作业船的跟踪目标点，其计算示意图如图8所示。
Q(x1,y1)为船体的当前位置坐标，TK(xTK,yTK)、TK+1(xTK+1,yTK+1)为当前目标路径的起止点。其中，横坐标x表示经度，纵坐标y表示纬度，目标路径方向为Tk→Tk+1。若此时作业船直接将Tk+1点作为跟踪目标点，其行走路线Q→Tk+1将会较大地偏离目标路径Tk→Tk+1。作Q点到路径Tk→Tk+1的垂线相交于M(x2,y2)点，根据点到直线的垂直交点计算公式可得M点的坐标为：
Figure PCTCN2016075946-appb-000011
Figure PCTCN2016075946-appb-000012
其中QM为作业船偏离目标路径的距离，由经纬度坐标计算两点距离的公式可得M点到点Tk+1的距离d为：
Figure PCTCN2016075946-appb-000013
式中，6378137m为地球半径，d的单位为m。
以M点作为起始点沿着Tk→Tk+1方向前伸距离D2至P点，可求得P(x3,y3)点的坐标为：
Figure PCTCN2016075946-appb-000014
Figure PCTCN2016075946-appb-000015
其中，D2的值由作业船的船体尺寸、航行速度等因素确定，D2的值过大时会导致计算得出的跟 踪目标点距离作业船较远，当作业船偏离目标路径时很难回调；D2的值过小时又会导致作业船对于航向的变化过于敏感，造成系统的震荡。
接着，主控制器将会以点P作为跟踪目标点对作业船进行自动导航控制。当船体由Q运动至Q′时，跟踪目标点也同时向前运动至P′，直至到达TK+1点为止。
这种方法能够实时地反映出航向的误差，将有限个角点细化成无数个跟踪目标点，可以使作业船的航行轨迹更加地逼近设定路径，能显著提高作业船自动导航路径跟踪的效果。
第4步.航向、航速双闭环控制
本发明将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差；分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船的左右明轮控制量。
为了减小水产养殖作业船的转弯半径，本发明采用两侧均可正反转的明轮作为驱动装置，并设计了相应的航向闭环、航速闭环解耦方法，保证两者能够互不干扰，实现解耦控制。当前航向Ψ2由惯导模块采集得到，目标航向Ψ1根据作业船当前的经纬度坐标Q(x,y)与跟踪目标点的经纬度坐标P(x,y)计算得到；目标速度V1由第2步得到，当前速度V2由惯导模块采集得到。
首先，根据公式：
Ψ*(k)＝Kp×ΔΨ(k)+Kd×(ΔΨ(k)-ΔΨ(k-1))
得到航向闭环的调整量Ψ*。式中，Kp、Kd为PD控制器的比例、微分系数，ΔΨ为目标航向Ψ1与当前航向Ψ2的差值，即
ΔΨ(k)＝Ψ1(k)-Ψ2(k)
接着，根据公式：
Figure PCTCN2016075946-appb-000016
得到航速闭环的调整量V*。式中，Kp、Ki为航速PI控制器的比例、积分参数，ΔV为目标航速V1和当前航速V2的差值，即
ΔV(k)＝V1(k)-V2(k)
本实施例将GPS设备的主天线放置在左右明轮连线的中点处以实现航向闭环和航速闭环的解耦控制。当左右明轮速度分别为
Figure PCTCN2016075946-appb-000017
和
Figure PCTCN2016075946-appb-000018
时，作业船将会以主天线为中心点原地转向，此时由于主 天线没有发生位移，GPS设备输出的速度值为0，即航向调整不会影响到航速；当左右明轮的速度同时为V*时，作业船将保持当前航向前进或者后退，此时作业船的航向变化量为0，即航速调整不会影响到航向。本实施例以速度调整量V*为基准，左右明轮速度分别加上和减去
Figure PCTCN2016075946-appb-000019
来进行解耦，得到左右明轮的速度分别为：
左明轮：
Figure PCTCN2016075946-appb-000020
右明轮：
Figure PCTCN2016075946-appb-000021
ARM9主控制器根据U1及U2的值设定左右明轮的转速，灵活控制船体进行加速、减速、左转或右转等动作，控制船体按照设定航线航行。
第5步.根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。
具体实施例
上述第1步规划作业船的目标航线和航速中，设定水产养殖作业船的目标作业区域为图3所示的不规则的形状，根据目标作业区域的形状特征设置4个能反映目标作业区域轮廓的角点{J0,J1,J2,J3}，将原始的曲线路径分解为图4所示的多个首尾相连的直线路径，使用高精度GPS系统采集这些角点的经纬度坐标分别为(119.51700896，32.19759716)、(119.51744361，32.19757820)、(119.51740220，32.19784642)、(119.51698746，32.19785174)，并将其以二维数组的形式储存在主控制器之中。接着，由已知两点的经纬度计算两点间距离的公式：
Figure PCTCN2016075946-appb-000022
分别计算出作业船的当前位置Q(x,y)同各个角点的距离{L0,L1,L2,L3}，其中，Lat表示经度，Lng表示纬度，L的单位为m。接着将距离作业船最近的角点作为行进路线的起点T0，其示意图如图5所示。按照路径最短的原则对作业船的行进路线进行规划，得到作业船的行进路线为：
T0→T1→T2→T3
作业船相应的行进路线数据为：
{(xT0,yT0),(xT1,yT1),(xT2,yT2),(xT3,yT3)}
最后，结合目标路径设定目标巡航速度为：VL＝0.8m.s-1，目标转弯速度为：VS＝0.35m.s-1。
其示意图如图6所示。
这种方式将不规则的作业区域规划为多个首尾相连的直线路径，与现有的技术相比，降低了作业船行进路线数据获取的难度。
上述第2步中设定作业船航行速度并判断是否需要路径切换中，R1、R2的值应由作业船的船体尺寸等因素确定。在本实施例中，考虑到水产养殖作业船的船体尺寸(长×宽×高)分别为4.0m×1.6m×1.2m，设置R1＝3m、R2＝1.5m，可以在保证作业船路径跟踪精度的前提下提高作业船的工作效率。
上述第3步采用实时插点的方式得到作业船的跟踪目标点中，设定前伸距离D2的值为2.5m，在作业船偏离目标路径时可以很快地回调且不会造成系统的震荡。
本实施例中作业船的自动导航路径跟踪轨迹图如图9所示，运行过程中的误差曲线如图10所示。由实验结果可以看到：作业船在转弯点处不易偏离目标路径，并且当船体出现偏差时会很快地进行回调，其在转弯与直行时偏离目标路径的最大误差分别为0.36m和0.09m，能很好地满足蟹塘内水草清理及均匀投饵的要求。
在本说明书的描述中，参考术语“一个实施例”、“一些实施例”、“示意性实施例”、“示例”、“具体示例”、或“一些示例”等的描述意指结合该实施例或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施例或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施例或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施例或示例中以合适的方式结合。
尽管已经示出和描述了本发明的实施例，本领域的普通技术人员可以理解：在不脱离本发明的原理和宗旨的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由权利要求及其等同物限定。

Claims (6)

  1. 一种水产养殖作业船自动导航路径跟踪方法，其特征在于，包括以下步骤：
    第一步：使用基于RTK模式的高精度GPS系统采集能反映目标作业区轮廓的各个角点的经纬度坐标，经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径；
    第二步：通过基于RTK模式的高精度GPS系统以及惯导模块采集得到作业船当前的位置、航向和航速等位姿和运动状态数据，设定作业船的航行速度并判断是否需要进行目标路径切换；
    第三步：采用实时插点的方法计算得到作业船的跟踪目标点P：根据点到直线的垂直交点的计算公式，得到作业船的当前位置在当前目标路径上的投影点；然后以上述得到的投影点为起点，沿着目标路径前进方向前伸一定距离得到作业船的跟踪目标点P，其中前伸的距离值由作业船的船体尺寸、航行速度等因素确定；
    第四步：将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，得到作业船的航向偏差与速度偏差，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船的左右明轮控制量；
    第五步：根据得到的控制量对作业船进行实时导航控制，实现路径的跟踪。
  2. 如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第一步中经轨迹规划，得到作业船的目标航线和航速，并确定起始目标路径，具体步骤如下：
    步骤1.1，计算作业船与能反映目标作业区轮廓的各个角点{J0,J1,...,Jn}的距离{L0,L1,...,Ln}，并将距离最近的点作为目标航线的起点T0；
    步骤1.2，以各个角点的经纬度坐标为基准，按照路径最短原则推算出作业船的目标航线：
    T0→T1→...→Tk-1→TK→TK+1→...→TN
    对应的目标点集合为：
    {T0,T1,...,Tk-1,TK,TK+1,...,TN}
    其经纬度坐标为：
    {(xT0,yT0),(xT1,yT1),...,(xTk-1,yTk-1),(xTk,yTk),(xTk+1,yTk+1),...,(xTn,yTn)}
    其中x坐标表示经度，y坐标表示纬度，TN为作业船目标航线的终点；
    步骤1.3，结合目标航线设定作业船的目标巡航速度VL和目标转弯速度VS。
    步骤1.4，确定路径T0→T1为作业船起始的目标路径。
  3. 如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第二步中设定作业船的航行速度并判断是否需要进行目标路径切换，具体步骤如下：
    步骤2.1，根据作业船当前位置Q及目标路径终点Tk+1的经纬度坐标计算出作业船与目标路径终点Tk+1的距离D1；
    步骤2.2，若步骤2.1中计算得到的距离D1大于减速半径R1，则认为作业船还未逼近目标路径终点，设定作业船的航行速度V1为较大的巡航速度VL；若D1小于或等于减速半径R1，则认为作业船已逼近目标路径终点，设定作业船的航行速度V1为较小的转弯速度VS；
    步骤2.3，若步骤2.1中计算得到的距离D1大于目标半径R2，则认为作业船还未到达目标路径终 点，不需要进行目标路径的切换；若D1小于或等于目标半径R2，则认为作业船已到达目标路径终点，需要将当前的目标路径由Tk→Tk+1切换成Tk+1→Tk+2；
    其中，R1、R2的值由作业船的船体尺寸等因素确定。
  4. 如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第三步的具体过程如下：
    步骤3.1，计算作业船的当前位置Q(x1,y1)在当前目标路径TK(xTK,yTK)→TK+1(xTK+1,yTK+1)上的投影点M(x2,y2)，计算的公式为：
    Figure PCTCN2016075946-appb-100001
    Figure PCTCN2016075946-appb-100002
    其中，横坐标x表示经度，纵坐标y表示纬度。
    步骤3.2，由两点经纬度坐标计算两点距离的公式得到投影点M点到目标路径终点Tk+1点的距离d，计算的公式为：
    Figure PCTCN2016075946-appb-100003
    其中，6378137为地球半径，单位为m。
    步骤3.3，以投影点M点作为起始点沿着Tk→Tk+1方向前伸距离D2至P(x3,y3)点，P(x3,y3)点坐标的计算公式为：
    Figure PCTCN2016075946-appb-100004
    Figure PCTCN2016075946-appb-100005
    式中D2的值由作业船的船体尺寸、航行速度等因素确定。
  5. 如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第四步中，将作业船的位姿和运动状态数据与作业船的跟踪目标点及目标路径进行对比分析，计算得到作业船的航向偏差与速度偏差，具体步骤如下：
    步骤4.1，航向偏差ΔΨ的确定：作业船的航向偏差ΔΨ为作业船的目标航向Ψ1与当前航向Ψ2的差值，计算的公式为：
    ΔΨ＝Ψ1-Ψ2
    其中，当前航向Ψ2由惯导模块采集得到，目标航向Ψ1根据作业船当前的经纬度坐标Q(x,y)与跟踪目标点的经纬度坐标P(x,y)计算得到；
    步骤4.2，航速偏差ΔV的确定：作业船的航速偏差ΔV为作业船目标速度V1与当前速度V2的差值，计算的公式为：
    ΔV(k)＝V1-V2
    目标速度V1由第二步得到，当前速度V2由惯导模块采集得到。
  6. 如权利要求1所述的水产养殖作业船自动导航路径跟踪方法，其特征在于：所述第四步中，分别通过PD、PI控制算法推算出作业船的航向控制量和航速控制量，并将两者融合得到作业船最终的控制量即左右明轮速度，具体步骤如下：
    步骤4.a，通过PD控制算法推算出航向控制量Ψ*：第i时刻的航向控制量Ψ*(i)由以下公式推算得出：
    Ψ*(i)＝Kp×ΔΨ(i)+Kd×(ΔΨ(i)-ΔΨ(i-1))
    其中，ΔΨ(i)与ΔΨ(i-1)分别为第i、i-1时刻的航向偏差，Kp为比例参数，Kd为微分参数，Kp和Kd参数均根据实验效果现场整定；
    步骤4.b，通过PI控制算法推算出航速控制量V*：第i时刻的航速控制量V*(i)由以下公式推算得出：
    Figure PCTCN2016075946-appb-100006
    其中，ΔV(i)为第i的航速偏差，Kp为比例参数，Ki为积分参数，Kp和Ki参数均根据实验效果现场整定；
    步骤4.c，融合航向控制量Ψ*与航速控制量V*得到作业船左右明轮控制量：本发明以速度控制量V*为基准，左右明轮控制量分别加上和减去
    Figure PCTCN2016075946-appb-100007
    来进行解耦，得到左右明轮的控制量分别为：
    左明轮为：
    Figure PCTCN2016075946-appb-100008
    右明轮为：
    Figure PCTCN2016075946-appb-100009
PCT/CN2016/075946 2016-02-22 2016-03-09 一种水产养殖作业船自动导航路径跟踪方法 WO2017143631A1 (zh)

Applications Claiming Priority (2)

Application Number Priority Date Filing Date Title
CN201610095261.5A CN105629976B (zh) 2016-02-22 2016-02-22 一种水产养殖作业船自动导航路径跟踪方法
CN201610095261.5 2016-02-22

Publications (1)

Publication Number Publication Date
WO2017143631A1 true WO2017143631A1 (zh) 2017-08-31

Family

ID=56045030

Family Applications (1)

Application Number Title Priority Date Filing Date
PCT/CN2016/075946 WO2017143631A1 (zh) 2016-02-22 2016-03-09 一种水产养殖作业船自动导航路径跟踪方法

Country Status (2)

Country Link
CN (1) CN105629976B (zh)
WO (1) WO2017143631A1 (zh)

Cited By (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109840358A (zh) * 2019-01-09 2019-06-04 上海海洋大学 一种基于航迹时域差分的航迹分段方法
CN110610134A (zh) * 2019-08-13 2019-12-24 上海大学 一种无人艇自主对接方法
CN110888447A (zh) * 2019-12-23 2020-03-17 西安电子科技大学 基于非线性pd双闭环控制的四旋翼3d路径跟踪方法
CN111460623A (zh) * 2020-03-06 2020-07-28 杭州吉翱世联土地勘测规划设计有限公司 一种测绘作业数据实时采集处理方法及系统
CN111949750A (zh) * 2020-07-30 2020-11-17 智慧航海（青岛）科技有限公司 一种船舶轨迹模型建立及异常轨迹检测方法
CN112068550A (zh) * 2020-08-11 2020-12-11 闽江学院 一种船舶航向跟踪控制的方法
CN113762125A (zh) * 2021-08-31 2021-12-07 华中科技大学 一种基于高分四号影像分析的运动舰船航速航向获取方法
CN114089758A (zh) * 2021-11-18 2022-02-25 上海联适导航技术股份有限公司 一种基于WebGIS的无人插秧作业路径规划方法
CN114217543A (zh) * 2021-10-11 2022-03-22 江苏大学 一种基于单抛盘模型的全自动投饵船均匀投饵的控制方法
CN116520834A (zh) * 2023-04-24 2023-08-01 安徽建筑大学 一种低能耗的无人船巡航方法
CN116736864A (zh) * 2023-07-12 2023-09-12 北鲲睿航科技(上海)有限公司 一种自主航行控制方法及系统
CN116736864B (zh) * 2023-07-12 2024-05-31 北鲲睿航科技(上海)有限公司 一种自主航行控制方法及系统

Families Citing this family (18)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106601030B (zh) * 2016-12-08 2019-03-26 中国交通通信信息中心 一种船舶智能过闸辅助航行系统
CN106708062B (zh) * 2017-03-10 2020-10-09 信阳农林学院 一种智能施药船的行驶控制方法
CN106950960B (zh) * 2017-04-12 2020-02-18 大连理工大学 一种湖库作业船对偶变向侧桨在线自动控制方法
CN108733057A (zh) * 2018-05-23 2018-11-02 江苏大学 一种基于自抗扰技术的水产养殖明轮船航向控制方法
CN109116856B (zh) * 2018-09-28 2021-04-20 上海海事大学 一种基于扰动观测器的欠驱动船舶路径跟踪控制方法
CN110673598B (zh) * 2019-09-29 2022-10-25 哈尔滨工程大学 一种水面无人艇智能路径跟踪控制方法
CN110986943B (zh) * 2019-11-01 2023-03-28 智慧航海（青岛）科技有限公司 一种船舶航行路径智能优化方法及装置
CN110908376B (zh) * 2019-11-26 2020-12-18 江苏大学 一种抗风力的自动投饵船精准投饵方法
CN111176292B (zh) * 2020-01-13 2022-11-22 天津工业大学 一种基于侧边距的波浪滑翔器路径跟踪控制方法
CN111522336B (zh) * 2020-03-26 2021-05-25 江苏大学 一种池塘水草自动清理船的作业路径规划及控制方法
CN111427350B (zh) * 2020-03-30 2022-11-18 江苏大学 一种基于麦克纳姆轮的自主移动式增氧机的控制方法
CN111309036A (zh) * 2020-05-14 2020-06-19 江苏博人文化科技有限公司 一种基于自动驾驶船的空间信息测量系统
CN111552309B (zh) * 2020-05-21 2021-04-20 上海海洋大学 船舶巡边控制方法
CN111830968B (zh) * 2020-06-12 2024-02-06 江苏大学 一种多功能莼菜无人作业船及其导航控制方法
CN111766873B (zh) * 2020-06-12 2023-12-05 广州极飞科技股份有限公司 作业设备的引导方法及相关装置
CN112269376B (zh) * 2020-09-14 2022-11-18 江苏大学 一种移动式增氧机的作业路径规划及控制方法
CN113110470A (zh) * 2021-04-23 2021-07-13 安徽工业大学 一种明轮无人增氧船的控制系统与航向控制算法
CN114495031A (zh) * 2022-03-31 2022-05-13 青岛海信网络科技股份有限公司 一种车牌信息校正方法、设备及装置

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011055512A1 (ja) * 2009-11-04 2011-05-12 川崎重工業株式会社 操船制御方法及び操船制御システム
CN103576555A (zh) * 2013-11-14 2014-02-12 哈尔滨工程大学 一种动力定位船舶循迹导引控制方法
CN104850122A (zh) * 2015-05-13 2015-08-19 上海海事大学 基于可变船长比的抵抗侧风无人水面艇直线路径跟踪方法
CN104950882A (zh) * 2014-09-24 2015-09-30 广东工业大学 全局一致渐进路径跟踪引导控制方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP5419622B2 (ja) * 2009-10-01 2014-02-19 古野電気株式会社 船舶表示装置
DE102010028251A1 (de) * 2010-04-27 2011-10-27 Robert Bosch Gmbh Verfahren zum Erkennen eines Arbeitsbereichs sowie ein Gerät hierfür
CN103461257B (zh) * 2013-09-13 2014-11-26 中国水产科学研究院渔业机械仪器研究所 一种养殖池塘自动投饲系统
CN205027907U (zh) * 2015-09-16 2016-02-10 佛山市中渔科技有限公司 一种基于雷达和gps定位的喂虾机
CN105204506A (zh) * 2015-09-24 2015-12-30 哈尔滨工程大学 一种动力定位船舶铺管循迹导引方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2011055512A1 (ja) * 2009-11-04 2011-05-12 川崎重工業株式会社 操船制御方法及び操船制御システム
CN103576555A (zh) * 2013-11-14 2014-02-12 哈尔滨工程大学 一种动力定位船舶循迹导引控制方法
CN104950882A (zh) * 2014-09-24 2015-09-30 广东工业大学 全局一致渐进路径跟踪引导控制方法
CN104850122A (zh) * 2015-05-13 2015-08-19 上海海事大学 基于可变船长比的抵抗侧风无人水面艇直线路径跟踪方法

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109840358B (zh) * 2019-01-09 2023-04-07 上海海洋大学 一种基于航迹时域差分的航迹分段方法
CN109840358A (zh) * 2019-01-09 2019-06-04 上海海洋大学 一种基于航迹时域差分的航迹分段方法
CN110610134A (zh) * 2019-08-13 2019-12-24 上海大学 一种无人艇自主对接方法
CN110610134B (zh) * 2019-08-13 2022-12-20 上海大学 一种无人艇自主对接方法
CN110888447B (zh) * 2019-12-23 2021-09-03 西安电子科技大学 基于非线性pd双闭环控制的四旋翼3d路径跟踪方法
CN110888447A (zh) * 2019-12-23 2020-03-17 西安电子科技大学 基于非线性pd双闭环控制的四旋翼3d路径跟踪方法
CN111460623A (zh) * 2020-03-06 2020-07-28 杭州吉翱世联土地勘测规划设计有限公司 一种测绘作业数据实时采集处理方法及系统
CN111460623B (zh) * 2020-03-06 2023-07-14 浙江吉翱空间规划科技有限公司 一种测绘作业数据实时采集处理方法及系统
CN111949750B (zh) * 2020-07-30 2024-06-11 智慧航海（青岛）科技有限公司 一种船舶轨迹模型建立及异常轨迹检测方法
CN111949750A (zh) * 2020-07-30 2020-11-17 智慧航海（青岛）科技有限公司 一种船舶轨迹模型建立及异常轨迹检测方法
CN112068550A (zh) * 2020-08-11 2020-12-11 闽江学院 一种船舶航向跟踪控制的方法
CN113762125A (zh) * 2021-08-31 2021-12-07 华中科技大学 一种基于高分四号影像分析的运动舰船航速航向获取方法
CN113762125B (zh) * 2021-08-31 2024-06-07 华中科技大学 一种基于高分四号影像分析的运动舰船航速航向获取方法
CN114217543A (zh) * 2021-10-11 2022-03-22 江苏大学 一种基于单抛盘模型的全自动投饵船均匀投饵的控制方法
CN114089758B (zh) * 2021-11-18 2022-11-04 上海联适导航技术股份有限公司 一种基于WebGIS的无人插秧作业路径规划方法
CN114089758A (zh) * 2021-11-18 2022-02-25 上海联适导航技术股份有限公司 一种基于WebGIS的无人插秧作业路径规划方法
CN116520834B (zh) * 2023-04-24 2024-05-28 安徽建筑大学 一种低能耗的无人船巡航方法
CN116520834A (zh) * 2023-04-24 2023-08-01 安徽建筑大学 一种低能耗的无人船巡航方法
CN116736864A (zh) * 2023-07-12 2023-09-12 北鲲睿航科技(上海)有限公司 一种自主航行控制方法及系统
CN116736864B (zh) * 2023-07-12 2024-05-31 北鲲睿航科技(上海)有限公司 一种自主航行控制方法及系统

Also Published As

Publication number Publication date
CN105629976B (zh) 2018-06-01
CN105629976A (zh) 2016-06-01

Similar Documents

Publication Publication Date Title
WO2017143631A1 (zh) 一种水产养殖作业船自动导航路径跟踪方法
CN108681321B (zh) 一种无人船协同编队的水下探测方法
US20190208695A1 (en) Path Planning for Area Coverage
CN103869824B (zh) 基于生物触角模型的多机器人水下目标搜寻方法及装置
CN108536140B (zh) 一种无人船自主导航系统及方法
CN104881045A (zh) 嵌入式视觉引导下仿生机器鱼三维追踪控制方法
CN107607093B (zh) 一种基于无人艇的湖泊动态库容的监测方法
CN109960262B (zh) 一种基于几何法的无人艇动态避障方法和系统
CN114281083B (zh) 一种基于混合路径规划自主导航的无人船水质监测物联网控制系统及方法
CN111522336B (zh) 一种池塘水草自动清理船的作业路径规划及控制方法
CN110286672A (zh) 机器人及其导航控制方法、导航控制装置以及存储介质
CN111930119B (zh) 流速自适应的无人船自主规划路径和运动精确跟踪方法
CN106643719A (zh) 一种智能割草车的路径规划算法
CN112947431B (zh) 一种基于强化学习的无人船路径跟踪方法
CN111348161A (zh) 一种应用于海洋牧场的资源环境监测系统及其操作方法
CN107526087A (zh) 一种获取水下3d断层图像的方法及系统
CN108762280A (zh) 一种考虑海洋环流影响的uuv基于能量消耗优化的远程航海路径规划方法
CN111830968B (zh) 一种多功能莼菜无人作业船及其导航控制方法
Plonski et al. Environment exploration in sensing automation for habitat monitoring
JP2000080673A (ja) 浚渫船向け経路計画法
Liu et al. Unmanned airboat technology and applications in environment and agriculture
CN106708062B (zh) 一种智能施药船的行驶控制方法
CN116907452A (zh) 基于集群auv数据驱动的亚中尺度涡观测方法
CN115657683B (zh) 一种可用于巡检作业任务的无人无缆潜水器实时避障方法
CN114942643B (zh) 一种usv无人艇路径规划模型的构建方法及应用

Legal Events

Date Code Title Description
NENP Non-entry into the national phase

Ref country code: DE

121 Ep: the epo has been informed by wipo that ep was designated in this application

Ref document number: 16891085

Country of ref document: EP

Kind code of ref document: A1

122 Ep: pct application non-entry in european phase

Ref document number: 16891085

Country of ref document: EP

Kind code of ref document: A1