Inspection trajectory planning method, device, electronic equipment and storage medium

By obtaining motion environment information and using trajectory planning models to constrain the operation trajectory of the inspection robot, the problem of low intelligence level of inspection robots in the existing technology is solved, autonomous line drops and obstacle avoidance are achieved, and patrol efficiency and intelligence are improved. level of transformation.

CN118051066BActive Publication Date: 2024-12-06INST OF AUTOMATION CHINESE ACAD OF SCI
View PDF 8 Cites 0 Cited by

Patent Information

Application Number
CN202410131491.7
Authority / Receiving Office
CN · China
Patent Type
Patents(China)
Current Assignee / Owner
Filing Date
2024-01-30
Publication Date
2024-12-06
Estimated Expiration
2044-01-30

AI Technical Summary

Technical Problem

The existing inspection robots have low intelligence level in power line inspections, lack systematic motion planning methods, making it difficult to automatically drop lines and avoid obstacles.

Method used

By obtaining motion environment information, including power line position, obstacle position position and patrol robot status, the trajectory planning model is used to constrain the operation trajectory of the patrol robot. The models include motion geometric sub-model, line fall sub-model, obstacle avoidance sub-model and patrol. The line sub-model realizes constraints on the line drop, obstacle avoidance and line patrol status of the inspection robot.

Benefits of technology

The inspection robot's independent line-down and obstacle avoidance capabilities have been improved, the inspection efficiency and intelligence level have been improved, and the inspection robot can inspect the power line safely and effectively.

✦ Generated by Eureka AI based on patent content.

Smart Images

  • Figure CN118051066B_ABST
    Figure CN118051066B_ABST
Patent Text Reader

Abstract

The present invention provides a patrol trajectory planning method, device, electronic device and storage medium, which are applied to the field of robotics technology. The method comprises: obtaining motion environment information, the motion environment information comprises power line posture information, obstacle posture information and patrol robot status information; planning the running trajectory of the patrol robot according to the motion environment information and a trajectory planning model; wherein the trajectory planning model is used to constrain the patrol trajectory of the patrol robot, and the trajectory planning model comprises a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model.
Need to check novelty before this filing date? Find Prior Art

Description

Technical Field

[0001] The present invention relates to the field of robotics technology, and in particular to an inspection trajectory planning method, device, electronic equipment and storage medium. Background Art

[0002] Usually, in order to ensure power safety, it is necessary to conduct regular inspections of transmission lines. With the cross-application and development of multidisciplinary technologies such as drone technology, image processing technology, and intelligent control technology, inspection robots have gradually replaced traditional manual inspection methods.

[0003] Since most transmission lines are laid out in the wild, they may be covered by debris, corroded, or even broken due to the influence of natural environment, mechanical stress, material aging and other factors. The inspection robots in the existing technology can only autonomously drop the line based on feedback control, lack systematic motion planning methods, and have a low level of intelligence. Summary of the invention

[0004] The present invention provides an inspection trajectory planning method, device, electronic equipment and storage medium, which are used to solve the problem of low intelligence level of inspection robots in the prior art.

[0005] The present invention provides a patrol trajectory planning method, comprising: acquiring motion environment information, the motion environment information comprising power line posture information, obstacle posture information and patrol robot status information; planning the operation trajectory of the patrol robot according to the motion environment information and a trajectory planning model; wherein the trajectory planning model is used to constrain the patrol trajectory of the patrol robot, and the trajectory planning model comprises a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line-patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between motion thrust and the patrol robot status information; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the patrol robot; the obstacle avoidance sub-model is used to constrain the motion distance between the patrol robot and the obstacle, and the line-patrol sub-model is used to constrain the motion distance and motion direction between the patrol robot and the power line.

[0006] According to the inspection trajectory planning method provided by the present invention, the power line posture information includes a position vector l of a known point on the power line s and direction vector l v The obstacle position information includes: the center point coordinates of the obstacle's circumscribed cube o w and diagonal radius δ, safety distance parameter ε; the inspection robot status information includes: inspection robot position p wb , Inspection robot posture q wb .

[0007] According to the inspection trajectory planning method provided by the present invention, the model constraint formula of the line-falling sub-model includes a first range constraint formula and a line-falling direction constraint formula;

[0008] The first range constraint formula is:

[0009] The falling line direction constraint formula is:

[0010] Among them, p′ represents the vertex of the first triangular face of the guide mechanism modeling model, m w represents the midpoint of the bottom line of the first triangle, c w represents the intersection of the electric line and the first triangular face, θ represents the vertex angle of the first triangular face; x b represents a first direction axis of the body coordinate system, the first direction axis is parallel to the extension direction of the electric line, and k(x) represents an additional activation function.

[0011] According to the inspection trajectory planning method provided by the present invention, the obstacle avoidance constraint formula of the obstacle avoidance sub-model is: oa (x)=||p wb -o w ||-δ-r-ε>0; wherein r represents the radius of the circumscribed sphere of the inspection robot.

[0012] According to a patrol trajectory planning method provided by the present invention, the model formula of the patrol line sub-model includes:

[0013] Line patrol distance formula:

[0014] Line patrol direction formula: a p2l =|q wb ⊙y b ·l v |;

[0015] Where A = -l v,y , B=l v,x , C = l v,y l s,x -l v,x l s,y , (p wb,x ,p wb,y ) represents the inspection robot on the two-dimensional plane x w -y w The projection point on b A second direction axis represents the body coordinate system, wherein the second direction axis is perpendicular to the extension direction of the electric force lines.

[0016] According to the inspection trajectory planning method provided by the present invention, the model constraint formula of the trajectory planning model also includes:

[0017]

[0018] in, represents the position error, represents the attitude error, represents the speed error, represents the angular velocity error, Q k represents the intermediate state weight coefficient matrix, Q N Represents the terminal state weight coefficient matrix.

[0019] The present invention also provides a patrol trajectory planning device, comprising: an acquisition module and a processing module; the acquisition module is used to acquire motion environment information, the motion environment information includes power line posture information, obstacle posture information and patrol robot status information; the processing module is used to plan the operation trajectory of the patrol robot according to the motion environment information and the trajectory planning model; wherein the trajectory planning model is used to constrain the patrol trajectory of the patrol robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the patrol robot status information; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the patrol robot; the obstacle avoidance sub-model is used to constrain the motion distance between the patrol robot and the obstacle, and the line patrol sub-model is used to constrain the motion distance and motion direction between the patrol robot and the power line.

[0020] According to the inspection trajectory planning device provided by the present invention, the power line posture information includes a position vector l of a known point on the power line s and direction vector l v The obstacle position information includes: the center point coordinates of the obstacle's circumscribed cube o w and diagonal radius δ, safety distance parameter ε; the inspection robot status information includes: inspection robot position p wb , Inspection robot posture q wb .

[0021] According to the inspection trajectory planning device provided by the present invention, the model constraint formula of the line-falling sub-model includes a first range constraint formula and a line-falling direction constraint formula;

[0022] The first range constraint formula is:

[0023] The falling line direction constraint formula is:

[0024] Among them, p′ represents the vertex of the first triangular face of the guide mechanism modeling model, m w represents the midpoint of the bottom line of the first triangle, c w represents the intersection of the electric line and the first triangular face, θ represents the vertex angle of the first triangular face; x b represents a first direction axis of the body coordinate system, the first direction axis is parallel to the extension direction of the electric line, and k(x) represents an additional activation function.

[0025] According to the inspection trajectory planning device provided by the present invention, the obstacle avoidance constraint formula of the obstacle avoidance sub-model is: oa (x)=||p wb -o w ||-δ-r-ε>0; wherein r represents the radius of the circumscribed sphere of the inspection robot.

[0026] According to a patrol trajectory planning method provided by the present invention, the model formula of the patrol line sub-model includes:

[0027] Line patrol distance formula:

[0028] Line patrol direction formula: a p2l =|q wb ⊙y b ·l v |;

[0029] Where A = -l v,y , B=l v,x , C = l v,y l s,x -l v,x l s,y , (p wb,x ,p wb,y ) represents the inspection robot on the two-dimensional plane x w -y w The projection point on b A second direction axis represents the body coordinate system, wherein the second direction axis is perpendicular to the extension direction of the electric force lines.

[0030] According to the inspection trajectory planning device provided by the present invention, the model constraint formula of the trajectory planning model also includes:

[0031]

[0032] in, represents the position error, represents the attitude error, represents the speed error, represents the angular velocity error, Q k represents the intermediate state weight coefficient matrix, Q N Represents the terminal state weight coefficient matrix.

[0033] The present invention also provides an electronic device, comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the program, the steps of any of the above-described inspection trajectory planning methods are implemented.

[0034] The present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the steps of any of the inspection trajectory planning methods described above.

[0035] The inspection trajectory planning method, device, electronic device and storage medium provided by the present invention can obtain motion environment information, and plan the operation trajectory of the inspection robot according to the motion environment information and the trajectory planning model. Through this scheme, the operation trajectory of the inspection robot can be determined according to the motion environment information and the trajectory planning model. Since the trajectory planning model is used to constrain the inspection trajectory of the inspection robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model, it is possible to constrain the three motion states of the inspection robot, namely, autonomous line-falling, obstacle avoidance and line patrol, thereby realizing the autonomous obstacle-crossing and line-falling capabilities of the inspection robot during power line inspection, thereby improving the inspection efficiency and intelligence level of the inspection robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] In order to more clearly illustrate the technical solutions in the present invention or the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0037] Figure 1 It is a flow chart of the inspection trajectory planning method provided by the present invention;

[0038] Figure 2 It is one of the modeling schematic diagrams of the inspection trajectory planning method provided by the present invention;

[0039] Figure 3 It is a functional image schematic diagram of the activation function provided by the present invention;

[0040] Figure 4 This is the second modeling schematic diagram of the inspection trajectory planning method provided by the present invention;

[0041] Figure 5This is an analysis chart of the trajectory planning and trajectory tracking data of the inspection robot;

[0042] Figure 6 This is an analysis chart of the trajectory planning and trajectory tracking data of the inspection robot's autonomous landing.

[0043] Figure 7 It is a structural schematic diagram of the inspection trajectory planning device provided by the present invention;

[0044] Figure 8 It is a structural schematic diagram of the electronic device provided by the present invention. DETAILED DESCRIPTION

[0045] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention will be clearly and completely described below in conjunction with the drawings of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0046] It should be noted that, in the embodiments of the present invention, words such as "exemplary" or "for example" are used to indicate examples, illustrations or descriptions. Any embodiment or design described as "exemplary" or "for example" in the embodiments of the present invention should not be interpreted as being more preferred or more advantageous than other embodiments or designs. Specifically, the use of words such as "exemplary" or "for example" is intended to present related concepts in a specific way.

[0047] It should be noted that, in this article, the terms "comprise", "include" or any other variants thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the presence of other identical elements in the process, method, article or device including the element. In addition, it should be pointed out that the scope of the method and device in the embodiment of the present invention is not limited to performing functions in the order shown or discussed, and may also include performing functions in a substantially simultaneous manner or in reverse order according to the functions involved, for example, the described method may be performed in an order different from that described, and various steps may also be added, omitted, or combined. In addition, the features described with reference to certain examples may be combined in other examples.

[0048] In order to clearly describe the technical solutions of the embodiments of the present invention, in the embodiments of the present invention, the words "first", "second", etc. are used to distinguish between the same items or similar items with basically the same functions and effects. Those skilled in the art can understand that the words "first", "second", etc. are not limiting the quantity and execution order.

[0049] The embodiments of the present invention describe some exemplary embodiments for the purpose of explanation. It should be understood that the present invention can be implemented in other ways that are not specifically shown in the drawings.

[0050] The above implementation is described in detail below with reference to specific embodiments and drawings.

[0051] like Figure 1 As shown, an embodiment of the present invention provides a patrol trajectory planning method, which can be applied to a patrol trajectory planning device. The patrol trajectory planning method may include S101-S102:

[0052] S101. The inspection trajectory planning device obtains motion environment information.

[0053] The above-mentioned motion environment information includes power line posture information, obstacle posture information and inspection robot status information.

[0054] Optionally, the power line posture information includes a position vector l of a known point on the power line s and direction vector l v ; The obstacle position information includes: the coordinates of the center point of the obstacle's circumscribed cube o w and diagonal radius δ, safety distance parameter ε; the inspection robot status information includes: the inspection robot position p wb , Inspection robot posture q wb .

[0055] Optionally, the power line posture information and obstacle posture information can be determined through an existing offline map, and the inspection robot status information can be obtained by fusing the posture data from the motion capture system or the global positioning system with the inertial measurement unit (IMU) data from Pixhawk by using an extended Kalman filter.

[0056] Optionally, the above-mentioned motion environment information can also be obtained by detecting a detection device installed on the inspection robot, and the detection device can be a laser radar, a binocular camera, an optoelectronic pod, etc.

[0057] It should be noted that the above-mentioned inspection robot is a hybrid inspection robot, which integrates flying function and crawling function. It can not only crawl and inspect on overhead power lines, but also fly over obstacles on the power lines.

[0058] S102: The inspection trajectory planning device plans the running trajectory of the inspection robot according to the motion environment information and the trajectory planning model.

[0059] Among them, the above-mentioned trajectory planning model is used to constrain the inspection trajectory of the inspection robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line-patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the state information of the inspection robot; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the inspection robot; the obstacle avoidance sub-model is used to constrain the movement distance between the inspection robot and the obstacle, and the line-patrol sub-model is used to constrain the movement distance and movement direction between the inspection robot and the power line.

[0060] Optionally, the model constraint formula of the above motion geometry sub-model is formula (1):

[0061]

[0062] in, represents the system state variable, p wb and q wb They represent the position and posture of the inspection robot relative to the world coordinate system, v wb and w b They represent the linear velocity of the body in the world coordinate system and the angular velocity of the body in the body coordinate system respectively. f and τ represent the normalized motion thrust and the derivative of the motion thrust respectively. Λ represents the skew-symmetric matrix related to the angular velocity.

[0063] It should be noted that the above-mentioned motion geometry sub-model is proposed based on a simplified UAV dynamics model. The motion geometry sub-model takes motion thrust and the body angular velocity of the inspection robot as input. When generating trajectories, due to the performance of the inspection robot's actuators, excessive instantaneous changes in thrust may lead to infeasible trajectories. To solve this problem, the motion thrust can be replaced with the derivative of the motion thrust as the input of the system. This enables the trajectory planning model to generate smoother and more stable trajectories and ensures that these trajectories can be effectively tracked by the inspection robot.

[0064] Alternatively, if Figure 2As shown, the modeling model of the inspection robot's guiding mechanism includes a first triangular facet and a second triangular facet; the model constraint formula of the line-falling sub-model may include a first range constraint formula, a second range constraint formula and a line-falling direction constraint formula; the first range constraint formula is used to constrain the intersection of the power line with the first triangular facet; the second range constraint formula is used to constrain the intersection of the power line with the second triangular facet; the line-falling direction constraint formula is used to constrain the line-falling movement direction of the inspection robot to be consistent with the inspection direction of the power line.

[0065] Specifically, continue to refer to Figure 2 ,For the autonomous obstacle crossing task, the motion planning only involves a segment of the power line, so the power line can be approximated by a straight line, which can be expressed as l s +σl v , σ is the straight line parameter.

[0066] Continue to refer Figure 2 , the guiding mechanism of the inspection robot can be modeled as a triangular prism with a first triangular face and a second triangular face of length l. The base of the first triangular face is ||ef||, the height is h, and the vertex angle is θ.

[0067] In the body coordinate system, the vertex coordinates of the first triangle are:

[0068]

[0069] In the world coordinate system, the vertex coordinates of the first triangle are:

[0070] p′ w =q wb ⊙p′ b +p wb , e w =q wb ⊙e b +p wb , f w =q wb ⊙f b +p wb ;

[0071] The operator ⊙ represents the multiplication between a quaternion and a vector, point m is the midpoint of the base ef, and n w = p′e w ×p′f w is the normal vector of the first triangle, c w = l s +λl v is the intersection of the electric line and the first triangular face, λ can be calculated according to p′c w ⊥ w Sure.

[0072] Ensuring that the inspection robot lands on the power line is equivalent to making the power line intersect with the first triangle face and the second triangle face. Figure 2 Taking the first triangle face as an example, we can limit the cosine value of angle γ to less than Make the electric lines intersect the first triangular face. Since the angle γ can be calculated based on p′m w and p′c w Determine, so we can get formula (2):

[0073]

[0074] However, when the initial position of the inspection robot is on the power line, a small positioning drift will make the constraint of formula (2) invalid. Moreover, the constraint of formula (2) also limits the initial position of the inspection robot when it is above the power line. Therefore, the constraint of formula (2) can be relaxed by adding an activation function related to the position of the inspection robot. When p′m w and p′c w In the reverse direction, formula (2) takes the minimum value Therefore, we can add To ensure that it always holds, we can get formula (3):

[0075]

[0076] in, is the coordinate of the target position in the world coordinate system, is the distance from the inspection robot to the target position, sigm(x) is the activation function, which is defined as: like Figure 3 As shown, this is the function image of the activation function.

[0077] After the above processing, the first range constraint formula containing the activation function can be finally obtained. The first range constraint formula can be expressed as formula (4):

[0078]

[0079] Among them, p′ represents the vertex of the first triangle, m w Indicates the midpoint of the bottom line of the first triangle, c w represents the intersection of the electric line and the first triangular face, and θ represents the vertex angle of the first triangular face.

[0080] It should be noted that when the distance between the inspection robot and the target position is much greater than d, the value of the activation function approaches 1, the additional term in formula (4) is activated, and the constraint does not work. When the distance between the inspection robot and the target position is close to or less than d, the value of the activation function approaches 0, the additional term in formula (4) becomes invalid, and the constraint takes effect. In this way, it can be ensured that the line-dropping constraint takes effect only when the inspection robot is close to the target position, so that the inspection robot can basically complete the adjustment of the yaw angle before reaching the target position, while avoiding the conflict between the initial value and the line-dropping constraint.

[0081] Optionally, the second range constraint formula can be expressed as formula (5):

[0082]

[0083] It should be noted that the construction process of formula (5) can refer to the construction process of the first range constraint formula, that is, formula (4), which will not be repeated here.

[0084] Furthermore, when the direction of the inspection robot's head is consistent with or opposite to the direction of the power line, the above constraints may be satisfied. Therefore, in the final stage of landing, it is necessary to limit the x-axis direction of the inspection robot to be consistent with the direction of the power line. This constraint can be achieved by limiting the cosine value of the angle between the x-axis of the body and the direction vector of the power line to be greater than 0, and the falling line direction constraint formula is obtained. The falling line direction constraint formula can be expressed as formula (6):

[0085]

[0086] Among them, x b represents the first direction axis of the body coordinate system, which is parallel to the extension direction of the electric line, and k(x) represents an additional activation function. The additional activation function is an additional term with an activation function similar to the above formula (3).

[0087] Optionally, the obstacle avoidance constraint formula of the above obstacle avoidance sub-model is formula (7):

[0088] h oa (x)=|p wb -o w |-δ-r-ε>0 (7)

[0089] Among them, r represents the radius of the circumscribed sphere of the inspection robot, δ represents the diagonal radius of the circumscribed cube of the obstacle, ε represents the safety distance parameter, and p wb Indicates the position of the inspection robot relative to the world coordinate system, o w Represents the center point of the circumscribed cube of the obstacle.

[0090] Specifically, continue to refer to Figure 2 In order to avoid the inspection robot from colliding with obstacles during the inspection process, the inspection robot can be modeled as a sphere with a radius of r, and the obstacles are represented by a circumscribed cube with a diagonal radius of δ. When the constraints of the above formula (7) are met, the inspection robot can be prevented from colliding with obstacles.

[0091] Optionally, the model formula of the above line patrol sub-model includes a line patrol distance formula and a line patrol direction formula. The line patrol distance formula can be expressed as formula (8), and the line patrol direction formula can be expressed as formula (9):

[0092]

[0093]

[0094] Where A = -l v,y , B=l v,x , C = l v,y l s,x -l v,x l s,y , (p wb,x ,p wb,y ) represents the inspection robot on the two-dimensional plane x w -y w The projection point on b A second direction axis represents the body coordinate system, wherein the second direction axis is perpendicular to the extension direction of the electric force lines.

[0095] Specifically, in order to ensure the inspection quality of the inspection robot during maneuvering, the inspection robot should be as close to the power line as possible in the horizontal direction and aligned with the power line direction to maximize the visibility of the power line. This goal can be achieved by processing the projection geometric relationship between the inspection robot and the power line on a plane.

[0096] like Figure 4 As shown, the electric lines are in the plane x w -y w The projection on can be approximated as a line segment, and the electric line L can be expressed as: Ax+By+C=0, where A=-l v,y , B=l v,x , C = l v,y l s,x -l v,x l s,y Inspection robot on plane x w -y w The projection on can be regarded as point P(p wb,x ,p wb,y ). In the plane x w -y wThe distance from the inspection robot to the power line can be expressed as d in the above formula (8): p2l .

[0097] Furthermore, from the plane x w -y w Analysis shows that aligning the inspection robot with the power line is equivalent to making the body's y b The axis is perpendicular to the electric field lines L. Therefore, by minimizing the body y b The dot product of the axis and the direction vector of the electric line can make the y b The axis is perpendicular to the electric line L, thus obtaining the line inspection direction formula, namely the above formula (9).

[0098] When the robot is aligned with the power line, formula (8) and formula (9) take the minimum value. By minimizing formula (8) and formula (9), the alignment level between the inspection robot and the power line during the maneuver can be optimized, thereby maximizing the visibility of the power line and making it easier for the inspection robot to land on the power line.

[0099] Based on the construction of the above sub-models, the trajectory planning problem of the inspection robot can be modeled as a nonlinear planning problem of the trajectory planning model. The complete trajectory is discretized into N nodes on the variable time range T, and each node is represented by the current state and input of the system. Under the condition of satisfying the model constraints of the trajectory planning model, the inspection trajectory planning device can determine the state and input at the discrete time point that minimizes the cost function.

[0100] The model constraint formula of the trajectory planning model can include the following formula (10.1)-formula (10.13):

[0101]

[0102] s.tx0=x init (10.2)

[0103] T min <T<T max (10.3)

[0104] x k+1 =f(x k ,u k ) (10.4)

[0105] 0<f k <f max (10.5)

[0106] τ min <τ k <τ max (10.6)

[0107] v min <v k <v max (10.7)

[0108] w min <w k <w max (10.8)

[0109]

[0110]

[0111]

[0112] 0<h dir (x k ) (10.12)

[0113] 0<h oa (x k ) (10.13)

[0114] The above formula (10.1) is the objective function, which takes into account a variety of performance indicators and consists of multiple items. At each time node, these items are assigned weight coefficients through the diagonal matrix Q. Among them, the first item of the objective function Represents the operating cost, the second term of the objective function represents the terminal cost, Q k represents the intermediate state weight coefficient matrix, Q N Represents the terminal state weight coefficient matrix.

[0115] It should be noted that the first In Smooth control input can be achieved by minimizing the derivative of the motion thrust and the body angular velocity, minimizing the above inspection distance formula d p2l And the above inspection direction formula a p2l The inspection robot can be aligned with the power lines to ensure visibility of the power lines during maneuvers. Represents the position error between the current position and the target position. The objective function minimizes this distance so that the inspection robot can reach the target position as soon as possible while satisfying the actuator constraints.

[0116] For example, the inspection distance formula d p2l And inspection direction formula a p2l The power line parameters in can be defined as: c =(0.5,0,0.3), l v =(1,0,0), A = -l v,y , B=lv,x , C = l v,y l s,x -l v,x l s,y , Q k =diag(0.01,30,30,30,30,30,30,300,30).

[0117] It should be noted that the second In The terminal state of the trajectory, that is, the last node, can be optimized by minimizing the position error between the terminal state and the target position. Attitude Error Speed ​​error and angular velocity error Realize precise landing of the inspection robot.

[0118] Exemplarily, the target state can be defined as: The terminal state weight coefficient matrix can be defined as: Q N =diag(3000,3000,3000,300,300,300,300,100,100,100,10,10,10).

[0119] The above formula (10.2) represents the initial value of the system state variable. The position p in the initial state x0 of the inspection robot is wb , Posture q wb , Linear speed v wb It can be acquired in real time by motion capture equipment.

[0120] The above formula (10.3) represents the constraint on the total maneuvering time. The upper and lower bounds of the maneuvering time can be set as: T min =15, T max =40.

[0121] The above formula (10.4) represents the discrete system state transfer equation.

[0122] The above formula (10.5) represents the constraint on the motion thrust of the actuator in the inspection robot. The above formula (10.6) represents the constraint on the inverse of the motion thrust of the actuator. The limits of the motion thrust and the derivative of the motion thrust can be set as: f max =40,τ min =-40, τ max =40.

[0123] The above formula (10.7) represents the constraint on the linear velocity of the inspection robot. The upper and lower bounds of the linear velocity can be set as: v min =(-0.5,-0.5,-0.5), vmax =(0.5,0.5,0.5).

[0124] The above formula (10.8) represents the constraints on the angular velocity of the inspection robot. The upper and lower bounds of the angular velocity can be set as: min =(-0.2,-0.2,-0.2), w max =(0.2,0.2,0.2).

[0125] The above formula (10.9) represents the constraint on the movement height of the inspection robot. The lower limit of the flight height can be set as: min =0.29.

[0126] The above formulas (10.10) and (10.11) represent the line-falling constraint of the inspection robot. The guiding mechanism parameters of the inspection robot are: l = 0.2, h = 0.15, θ = 0.3491, and the line-falling constraint It can be defined as: Among them, g(x k ) is defined as sigm(x) is defined as d=1, the target position is defined as

[0127] The above formula (10.12) represents the constraint on the line-falling direction of the inspection robot. Line-falling direction constraint h dir (x k ) can be defined as: Here, k(x) is defined as

[0128] The above formula (10.13) represents the obstacle avoidance constraint. The obstacle avoidance constraint is defined as h oa (x k )=||p wb -o w ||-δ-r-ε>0, where o w =(0.42,0,0.3), δ=0.235, r=0.25, ε=0.1.

[0129] The inspection trajectory planning device can solve the nonlinear programming problem of the above formula (10.1)-formula (10.13) through the ForcesPRO solver. Specifically, the total maneuvering time can be set between 3-15 seconds and discretized into N=51 nodes, and the nonlinear programming problem is solved by the dual interior point method. The constructed nonlinear programming problem usually converges between 20 and 70 iterations, and the calculation time is generally less than 50 milliseconds. After solving the discrete trajectory, the Runge-Kutta method can be used to interpolate between discrete points to increase the resolution of the trajectory. The generated high-resolution trajectory is then subjected to collision detection. If a collision is detected, the number of nodes N is increased and the above process is repeated until a collision-free trajectory is obtained. Finally, the inspection robot can perform trajectory tracking to achieve autonomous line-falling or autonomous obstacle-crossing operations.

[0130] like Figure 5 As shown in the figure, it is an analysis diagram of the trajectory planning and trajectory tracking data of the inspection robot. Figure 5 The top picture in the figure is a top view, and the bottom picture is a side view. From the top view, it can be seen that the planned trajectory can keep the inspection robot directly above the power line and aligned with the power line. From the side view, it can be seen that the inspection robot can cross obstacles. Combining the terminal states of the inspection robot in the top view and the side view, it can be seen that the robot can successfully land on the power line in the end.

[0131] like Figure 6 The following is an analysis chart of the trajectory planning and trajectory tracking data of the inspection robot's autonomous landing. Figure 5 The top picture is a top view, and the bottom picture is a side view. From the top view, it can be seen that the line inspection target allows the inspection robot to quickly align with the power line, and from the side view, it can be seen that the inspection robot can land on the power line without collision.

[0132] In an embodiment of the present invention, the running trajectory of the inspection robot can be determined based on the motion environment information and the trajectory planning model. Since the trajectory planning model is used to constrain the inspection trajectory of the inspection robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model, it is possible to constrain the three motion states of the inspection robot, namely, autonomous line-falling, obstacle avoidance and line patrol, thereby realizing the autonomous obstacle-crossing and line-falling capabilities of the inspection robot during power line inspection, thereby improving the inspection efficiency and intelligence level of the inspection robot.

[0133] The above mainly introduces the solution provided by the embodiment of the present invention from the perspective of the method. In order to achieve the above functions, it includes hardware structures and / or software modules corresponding to the execution of each function. Those skilled in the art should easily realize that, in combination with the units and algorithm steps of each example described in the embodiment disclosed in this article, the embodiment of the present invention can be implemented in the form of hardware or a combination of hardware and computer software. Whether a function is executed in the form of hardware or computer software driving hardware depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to exceed the scope of the present invention.

[0134] The inspection trajectory planning method provided in the embodiment of the present invention can be executed by the inspection trajectory planning device or the control module for inspection trajectory planning in the inspection trajectory planning device. In the embodiment of the present invention, the inspection trajectory planning device provided in the embodiment of the present invention is described by taking the inspection trajectory planning device executing the inspection trajectory planning method as an example.

[0135] It should be noted that the embodiment of the present invention can divide the inspection trajectory planning device into functional modules according to the above method example. For example, each functional module can be divided corresponding to each function, or two or more functions can be integrated into one processing module. The above integrated module can be implemented in the form of hardware or in the form of software functional modules. Optionally, the division of modules in the embodiment of the present invention is schematic and is only a logical functional division. There may be other division methods in actual implementation.

[0136] like Figure 7 As shown, an embodiment of the present invention provides a patrol trajectory planning device 700. The patrol trajectory planning device 700 includes: an acquisition module 701 and a processing module 702. The acquisition module 701 can be used to acquire motion environment information, and the motion environment information includes power line posture information, obstacle posture information and patrol robot status information; the processing module 702 can be used to plan the running trajectory of the patrol robot according to the motion environment information and the trajectory planning model; wherein the trajectory planning model is used to constrain the patrol trajectory of the patrol robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model; the motion geometry sub-model is used to constrain the change relationship between the motion thrust and the patrol robot status information; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the patrol robot; the obstacle avoidance sub-model is used to constrain the motion distance between the patrol robot and the obstacle, and the line patrol sub-model is used to constrain the motion distance and motion direction between the patrol robot and the power line.

[0137] Optionally, the power line posture information includes a position vector l of a known point on the power line s and direction vector l v The obstacle position information includes: the center point coordinates of the obstacle's circumscribed cube o w and diagonal radius δ, safety distance parameter ε; the inspection robot status information includes: inspection robot position p wb , Inspection robot posture q wb .

[0138] Optionally, the model constraint formula of the line-falling sub-model includes a first range constraint formula and a line-falling direction constraint formula;

[0139] The first range constraint formula is:

[0140] The falling line direction constraint formula is:

[0141] Among them, p′ represents the vertex of the first triangular face of the guide mechanism modeling model, m w represents the midpoint of the bottom line of the first triangle, c w represents the intersection of the electric line and the first triangular face, θ represents the vertex angle of the first triangular face; x b represents a first direction axis of the body coordinate system, the first direction axis is parallel to the extension direction of the electric line, and k(x) represents an additional activation function.

[0142] Optionally, the obstacle avoidance constraint formula of the obstacle avoidance sub-model is: oa (x)=||p wb -o w ||-δ-r-ε>0; wherein r represents the radius of the circumscribed sphere of the inspection robot.

[0143] Optionally, the model formula of the line patrol sub-model includes:

[0144] Line patrol distance formula:

[0145] Line patrol direction formula: a p2l =|q wb ⊙y b ·l v |;

[0146] Where A = -l v,y , B=l v,x , C = l v,y l s,x -l v,x l s,y , (p wb,x ,pwb,y ) represents the inspection robot on the two-dimensional plane x w -y w The projection point on b A second direction axis represents the body coordinate system, wherein the second direction axis is perpendicular to the extension direction of the electric force lines.

[0147] Optionally, the model constraint formula of the trajectory planning model also includes:

[0148]

[0149] in, represents the position error, represents the attitude error, represents the speed error, represents the angular velocity error, Q k represents the intermediate state weight coefficient matrix, Q N Represents the terminal state weight coefficient matrix.

[0150] In an embodiment of the present invention, the running trajectory of the inspection robot can be determined based on the motion environment information and the trajectory planning model. Since the trajectory planning model is used to constrain the inspection trajectory of the inspection robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model, it is possible to constrain the three motion states of the inspection robot, namely, autonomous line-falling, obstacle avoidance and line patrol, thereby realizing the autonomous obstacle-crossing and line-falling capabilities of the inspection robot during power line inspection, thereby improving the inspection efficiency and intelligence level of the inspection robot.

[0151] Figure 8 An example of a physical structure diagram of an electronic device is shown in FIG. Figure 8As shown, the electronic device may include: a processor (processor) 810, a communication interface (Communications Interface) 820, a memory (memory) 830 and a communication bus 840, wherein the processor 810, the communication interface 820, and the memory 830 communicate with each other through the communication bus 840. The processor 810 can call the logic instructions in the memory 830 to execute the inspection trajectory planning method, which includes: obtaining motion environment information, the motion environment information includes power line posture information, obstacle posture information and patrol robot status information; planning the operation trajectory of the patrol robot according to the motion environment information and the trajectory planning model; wherein the trajectory planning model is used to constrain the patrol trajectory of the patrol robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the patrol robot status information; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the patrol robot; the obstacle avoidance sub-model is used to constrain the movement distance between the patrol robot and the obstacle, and the line patrol sub-model is used to constrain the movement distance and movement direction between the patrol robot and the power line.

[0152] In addition, the logic instructions in the above-mentioned memory 830 can be implemented in the form of a software functional unit and can be stored in a computer-readable storage medium when it is sold or used as an independent product. Based on such an understanding, the technical solution of the present invention, in essence, or the part that contributes to the prior art or the part of the technical solution, can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the present invention. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (ROM, Read-Only Memory), random access memory (RAM, Random Access Memory), disk or optical disk and other media that can store program codes.

[0153] On the other hand, the present invention also provides a computer program product, which includes a computer program stored on a non-transitory computer-readable storage medium, and the computer program includes program instructions. When the program instructions are executed by a computer, the computer can execute the inspection trajectory planning method provided by the above-mentioned methods, and the method includes: obtaining motion environment information, the motion environment information includes power line posture information, obstacle posture information and patrol robot status information; planning the operation trajectory of the patrol robot according to the motion environment information and the trajectory planning model; wherein the trajectory planning model is used to constrain the inspection trajectory of the patrol robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the state information of the patrol robot; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the patrol robot; the obstacle avoidance sub-model is used to constrain the motion distance between the patrol robot and the obstacle, and the line patrol sub-model is used to constrain the motion distance and motion direction between the patrol robot and the power line.

[0154] On the other hand, the present invention also provides a non-transitory computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, is implemented to execute the above-mentioned inspection trajectory planning methods, the method comprising: obtaining motion environment information, the motion environment information comprising power line posture information, obstacle posture information and inspection robot status information; planning the operation trajectory of the inspection robot according to the motion environment information and the trajectory planning model; wherein the trajectory planning model is used to constrain the inspection trajectory of the inspection robot, the trajectory planning model comprises a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line-patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the inspection robot status information; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the inspection robot; the obstacle avoidance sub-model is used to constrain the motion distance between the inspection robot and the obstacle, and the line-patrol sub-model is used to constrain the motion distance and motion direction between the inspection robot and the power line.

[0155] The device embodiments described above are merely illustrative, wherein the units described as separate components may or may not be physically separated, and the components displayed as units may or may not be physical units, that is, they may be located in one place, or they may be distributed on multiple network units. Some or all of the modules may be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative labor.

[0156] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0157] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit it. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or make equivalent replacements for some of the technical features therein. However, these modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A patrol trajectory planning method, It is characterized in that include: Acquire motion environment information, wherein the motion environment information includes power line posture information, obstacle posture information, and inspection robot status information; Planning the running trajectory of the inspection robot according to the motion environment information and the trajectory planning model; Among them, the trajectory planning model is used to constrain the inspection trajectory of the inspection robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line-patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the state information of the inspection robot; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the inspection robot; the obstacle avoidance sub-model is used to constrain the movement distance between the inspection robot and the obstacle, and the line-patrol sub-model is used to constrain the movement distance and movement direction between the inspection robot and the power line; The power line posture information includes a position vector l of a known point on the power line s and direction vector l v ; The obstacle position information includes: the coordinates of the center point of the circumscribed cube of the obstacle w and diagonal radius δ, safety distance parameter ε; The inspection robot status information includes: the inspection robot position p wb , Inspection robot posture q wb ; The model constraint formula of the line-falling sub-model includes a first range constraint formula and a line-falling direction constraint formula; The first range constraint formula is: The falling line direction constraint formula is: Among them, p′ represents the vertex of the first triangular face of the guide mechanism modeling model, m w represents the midpoint of the bottom line of the first triangle, c w represents the intersection of the electric line and the first triangular face, θ represents the vertex angle of the first triangular face; x b represents a first direction axis of the body coordinate system, the first direction axis is parallel to the extension direction of the electric line, and k(x) represents an additional activation function.

2. The inspection trajectory planning method according to claim 1, It is characterized in that The obstacle avoidance constraint formula of the obstacle avoidance sub-model is: oa (x)=||p wb -o w ||-δ-r-ε>0; Wherein, r represents the radius of the circumscribed sphere of the inspection robot.

3. The inspection trajectory planning method according to claim 1, It is characterized in that The model formula of the line patrol sub-model includes: Line patrol distance formula: Line patrol direction formula: a p2l =|q wb ⊙y b l v |; Where A = -l v,y , B=l v,x , C = l v,y l s,x -l v,x l s,y , (p wb,x ,p wb,y ) represents the inspection robot on the two-dimensional plane x w -y w The projection point on b A second direction axis represents the body coordinate system, wherein the second direction axis is perpendicular to the extension direction of the electric force lines.

4. The inspection trajectory planning method according to claim 3, It is characterized in that The model constraint formula of the trajectory planning model also includes: in, represents the position error, represents the attitude error, represents the speed error, represents the angular velocity error, Q k represents the intermediate state weight coefficient matrix, Q N Represents the terminal state weight coefficient matrix.

5. A patrol trajectory planning device, It is characterized in that include: Acquisition module and processing module; The acquisition module is used to acquire motion environment information, wherein the motion environment information includes power line posture information, obstacle posture information and inspection robot status information; The processing module is used to plan the running trajectory of the inspection robot according to the motion environment information and the trajectory planning model; Among them, the trajectory planning model is used to constrain the inspection trajectory of the inspection robot, and the trajectory planning model includes a motion geometry sub-model, a line-falling sub-model, an obstacle avoidance sub-model and a line-patrol sub-model; the motion geometry sub-model is used to constrain the changing relationship between the motion thrust and the state information of the inspection robot; the line-falling sub-model is used to constrain the line-falling range and line-falling direction of the inspection robot; the obstacle avoidance sub-model is used to constrain the movement distance between the inspection robot and the obstacle, and the line-patrol sub-model is used to constrain the movement distance and movement direction between the inspection robot and the power line; The power line posture information includes a position vector l of a known point on the power line s and direction vector l v ; The obstacle position information includes: the coordinates of the center point of the circumscribed cube of the obstacle w and diagonal radius δ, safety distance parameter ε; The inspection robot status information includes: the inspection robot position p wb , Inspection robot posture q wb ; The model constraint formula of the line-falling sub-model includes a first range constraint formula and a line-falling direction constraint formula; The first range constraint formula is: The falling line direction constraint formula is: Among them, p′ represents the vertex of the first triangular face of the guide mechanism modeling model, m w represents the midpoint of the bottom line of the first triangle, c w represents the intersection of the electric line and the first triangular face, θ represents the vertex angle of the first triangular face; x b represents a first direction axis of the body coordinate system, the first direction axis is parallel to the extension direction of the electric line, and k(x) represents an additional activation function.

6. An electronic device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, It is characterized in that When the processor executes the program, the steps in the inspection trajectory planning method according to any one of claims 1 to 4 are implemented.

7. A non-transitory computer-readable storage medium having a computer program stored thereon, It is characterized in that When the computer program is executed by a processor, the steps in the inspection trajectory planning method according to any one of claims 1 to 4 are implemented.

Citation Information

Patent Citations

  • Quadrotor drone electric tower inspection method based on movement learning

    CN106774389A

  • Rapid wire falling control method of flying-away type inspection robot for overhead transmission cable

    CN113253751A

  • Method for dynamically adjusting route of routing inspection e-dog

    CN116360456A

  • Power line patrol unmanned aerial vehicle path planning method based on DWA algorithm, storage medium and device

    CN116627168A

  • Path planning method and device for unmanned aerial vehicle to inspect power transmission line, and storage medium

    CN116755463A