Coordinated motion control method for mobile manipulator in hilly and mountainous areas

Abstract

The present invention discloses a method for coordinated motion control of a mobile robot arm in hilly and mountainous areas, comprising the following steps: step one, designing a local path planner; step two, constructing a kinematic model of a mobile robot arm; step three, designing a state estimator; and step four, designing a vehicle-arm coordinated controller. The present invention discloses a method for coordinated motion control of a mobile robot arm in hilly and mountainous areas, which is used for standing tree pruning operations in timber forests, and realizes the transportation and movement of pruning machines in an artificial forest environment in hilly and mountainous areas. On the premise of obtaining a global planning path for a mobile robot in a certain forest area, the method can first perform local path planning for obstacles in the working area, plan the trajectories of a mobile chassis and a robot arm, and coordinately control the chassis and the robot arm to track their respective trajectories for fully automatic operation. The task of carrying and moving the pruning machine is completed, unmanned operation is realized, and the degree of automation of the robot is greatly improved.

Description

Technical Field

[0001] The invention relates to the technical field of forestry pruning equipment, and in particular to a coordinated motion control method for a mobile mechanical arm in hilly and mountainous areas. Background Art

[0002] In recent years, with the continuous improvement of the modernization level of agriculture and forestry, the production of agriculture and forestry has gradually changed from labor-intensive to technology-intensive, and the operation robots used in agriculture and forestry have been greatly developed. However, many forestry fields in my country, especially the field of ecological construction, are still at a semi-mechanized level and have not yet reached the full mechanization level. Due to the complex environment of forest areas, mobile robots have not yet been widely used, and the pruning of timber forests is still mainly manual, with low work efficiency.

[0003] At present, the pruning operation of timber forests mainly relies on manual pruning or the use of tree climbing pruning machines. The research on timber forest pruning machinery and equipment in my country is still in the testing stage, and there are few applicable models. The disadvantage of the tree climbing pruning machine is that it requires manual assistance from operators, who manually place the pruning machine on the standing tree and remotely control the pruning machine for pruning. This working mode lacks intelligence and automation.

[0004] The information disclosed in this background technology section is only intended to enhance the understanding of the overall background of the invention and should not be regarded as an acknowledgment or any form of suggestion that the information constitutes the prior art already known to a person skilled in the art. Summary of the invention

[0005] The object of the present invention is to provide a method for controlling the coordinated motion of a mobile robot arm in hilly and mountainous areas, thereby overcoming the shortcoming of low automation level of pruning in hilly and mountainous areas.

[0006] To achieve the above object, the present invention provides a method for controlling the coordinated motion of a mobile robot arm in hilly and mountainous areas, comprising the following steps:

[0007] Step 1: Design a local path planner: Based on the global reference path and obstacle information, an obstacle model is established, an artificial potential field is established, and local paths are planned for the mobile chassis and the robotic arm respectively. Finally, the local path planning is output to the vehicle-arm collaborative controller.

[0008] Step 2: construct a kinematic model of the mobile robot arm: construct a kinematic model of the mobile robot arm based on the motion variables of the mobile chassis and the robot arm;

[0009] Step 3: Design a state estimator: Use an extended Kalman filter to perform real-time optimal estimation of the lateral and longitudinal slip of the chassis;

[0010] Step 4: Design the vehicle-arm collaborative controller: The vehicle-arm collaborative controller receives the reference trajectory output by the local path planner in step 1, and uses the nonlinear model predictive control method to collaboratively control the chassis and the robotic arm; discretizes the kinematic model of the mobile robotic arm, and constructs constraints and objective functions; solves the optimization problem in each control cycle, applies the control input increment to the mobile robotic arm, and realizes trajectory tracking control of the mobile chassis and the robotic arm.

[0011] Preferably, in the above technical solution, the method of designing a local path planner in step 1 includes:

[0012] (1) Establishing obstacle model: Multi-line laser radar detects obstacles and obtains point clouds of obstacles. Then, AABB bounding box technology is used to envelop the obstacles with cylinders of different shapes, thereby replacing irregular obstacles for local path planning.

[0013] (2) Design of obstacle avoidance function: The obstacle avoidance function is realized by using the artificial potential field method, and the following gravitational field function is established:

[0014]

[0015] In the formula, k a is the gravitational coefficient; d goal is the distance from the control point to the target point; the gravitational field is divided into gravitational field and variable force field, d L is the variable field radius of the gravitational field. In the variable field, the gravitational force is related to d goal Related; In a constant force field, the magnitude of gravity remains constant;

[0016] Establish the following repulsive field function:

[0017]

[0018] In the formula, k r is the gravitational coefficient; d obs is the distance from the control point to the obstacle point; d r is the radius of the repulsive field.

[0019] (3) Local path planning.

[0020] Preferably, in the above technical solution, the local path planning in step (3) includes the following steps:

[0021] 1) Take several points at equal distances from the global reference path as the target point set for local path planning;

[0022] 2) For the mobile chassis, the obstacle model is expanded with the radius of the inscribed circle and circumscribed circle of the chassis motion center as the size, and the target points in the obstacle model are eliminated; then the gradient descent algorithm is used, starting from the current position, and moving forward along the negative gradient direction until the gradient reaches the position of zero, and the planning point sequence is output;

[0023] 3) For the robot arm, the obstacle model is expanded with the radius of the pruning machine size to remove the target points in the obstacle model; first calculate the joint space pose of each joint, then find a set of joint angles with the smallest combined potential energy, and determine whether there is a collision in the joint angle. If there is a collision in the robot arm connecting rod, find a suitable joint angle again, and move the robot arm in the direction with the smallest combined potential energy of each joint until it reaches the position of the target point, and output the planning point sequence;

[0024] 4) If the planning point falls into the state of potential field extreme value and oscillates, random perturbations are added at the local extreme value to jump out of the local extreme value.

[0025] Preferably, in the above technical solution, the method of step 2 of constructing a kinematic model of the mobile robotic arm includes:

[0026] (1) Mobile chassis kinematic modeling: The chassis steering system and speed controller are modeled using the delay and dynamic characteristics of the chassis steering system and speed control using first-order lag plus time delay:

[0027]

[0028] Where s is the time constant of the first-order lag; L is the time delay;

[0029] Taking the center of the rear axle of the chassis as the origin of its coordinate system, the kinematic model of the mobile chassis is as follows:

[0030]

[0031] Where (x, y) is the center coordinate of the rear axle of the chassis, is the heading angle of the chassis, δ is the front wheel steering angle, v is the speed of the center of the rear axle of the chassis, l is the wheelbase of the front and rear axles of the chassis, τ v , τ α are the time constants of chassis speed and steering angle, respectively;

[0032] (2) Kinematic modeling of the robot arm: The robot arm is a six-degree-of-freedom robot arm installed on the central axis of the mobile chassis. The six joints are all rotary joints, and their joint variables are [θ1θ2θ3θ4θ5θ6]. The DH parameter method is used to model it. The coordinates of the end of the robot arm in the world coordinate system are P tip as follows:

[0033]

[0034] x tip =x+d6[c5s1+cos(θ2+θ3+θ4)c1s5]-d4s1+l3c1cos(θ2+θ3)+l2c1c2+d5c1sin(θ2+θ3+θ4)+d bb

[0035] y tip =y+d4c1+l3s1cos(θ2+θ3)-d6c1c5+l2c2s1+d5sin(θ2+θ3+θ4)s1+d6cos(θ2+θ3+θ4)s1s5

[0036] z tip =z+d1-d5[sin(θ2+θ3)s4-cos(θ2+θ3)c4]-l3sin(θ2+θ3)-l2s2-d6s5[cos(θ2+θ3)s4+sin(θ2+θ3)c4]+h bb

[0037] In the formula, c i 、s i are cosθ i and sinθ i abbreviation of; l i is the length of the i-th link of the robot arm; d i is the connecting rod offset; (x, y, z) is the center coordinate of the rear axle of the chassis, d bb Distance between the base of the robot and the rear axle of the chassis, h bb The height difference between the base of the robot arm and the rear axle of the chassis;

[0038] Calculate the end velocity V of the robot arm tip as follows:

[0039]

[0040] Preferably, in the above technical solution, step 3 of designing a state estimator includes acquiring data using a sensor system of the mobile manipulator, combining the measurement signal of the accelerometer, using an extended Kalman filter to perform real-time optimal estimation of the chassis state, and simultaneously performing real-time correction of the inertial measurement device;

[0041] Among them, the sensing system of the mobile robotic arm includes an RTK-GPS system, an inertial measurement device and a multi-line laser radar. The RTK-GPS system is used to estimate the position of the chassis; the inertial measurement device is used to measure the three-axis attitude angle and acceleration of the chassis; and the multi-line laser radar is used to detect standing tree trunks and obstacles.

[0042] Preferably, in the above technical solution, the step of designing a state estimator in step 3 includes:

[0043] (1) Definition of slip rate: Longitudinal slip and sliding are manifested as errors in the wheel's directional velocity, while lateral slip is manifested as a velocity or lateral force in the vertical direction of the wheel. The definition of slip rate is as follows:

[0044]

[0045] Where r is the wheel radius, ω is the wheel rotation angular velocity, and v w is the wheel speed.

[0046] (2) Establish the state model of the extended Kalman filter: Assume that lateral slip occurs only on the front wheels and longitudinal slip occurs on the rear wheels; the slip of the front wheels acts on the steering actuator, and the slip of the rear wheels acts on the speed controller; the state model of the extended Kalman filter used for mobile chassis state estimation is as follows:

[0047]

[0048] In the formula, is the estimated value of the front wheel slip rate, is the estimated value of rear wheel slip rate.

[0049] Preferably, in the above technical solution, the model discretization in the vehicle-arm cooperative controller is designed in step 4 as follows: the kinematic model of the mobile mechanical arm is discretized, and the control system of the mobile mechanical arm is generally in the form of:

[0050]

[0051] The input is They are the speed and acceleration of the mobile chassis, the front wheel angle and the angular velocity of each joint of the robotic arm; the state quantities are:

[0052]

[0053] The state quantities include the rear axle center coordinates and speed of the mobile chassis, the chassis heading angle, roll angle, and the angles and angular velocities of each joint of the robotic arm;

[0054] Using Taylor series expansion for equation (4.1) and ignoring higher-order terms, the state increment is obtained as:

[0055]

[0056] Discretize equation (4.2):

[0057]

[0058] Among them, A k,t =I n+TA(t), T is the sampling time of the discrete system, A(t) is the system's response to the state variable The Jacobian matrix of k,t =TB(t), B(t) is the Jacobian matrix of the system for the state variable u.

[0059] Preferably, in the above technical solution, the design of constraint conditions in the vehicle-arm coordinated controller in step 4 includes:

[0060] (1) Steering constraints and speed constraints: The constraints of the mobile chassis include steering constraints, speed constraints, and tire slip angle constraints; the constraints of the robotic arm include angle constraints and speed constraints of each joint. The constraints are as follows:

[0061]

[0062] Among them, the extreme values ​​of the front wheel steering angle δ and the chassis speed v are obtained by the lateral and longitudinal tracking ability test of the chassis; the robot arm joint angle θ and the rotation speed The extreme value of is obtained from the performance test of the robot arm;

[0063] (2) Tire slip angle constraint: The tire slip angle α includes the left and right front wheel and left and right rear wheel tire slip angles, which are α fl , α fr , α rl , α rr , the calculation formula of the sideslip angle is as follows:

[0064]

[0065] Where (x, y) is the center coordinate of the rear axle of the chassis, is the heading angle of the chassis, δ is the front wheel steering angle; a and b are the distances from the front and rear axles to the center of mass respectively; c is the half wheelbase of the front and rear wheels; according to the tire slip angle characteristics, when the tire slip angle does not exceed 5°, the slip angle and the cornering force are in a linear relationship; therefore, the tire slip angle constraint is set as follows:

[0066] -5°-ε<α fl,fr,rl,rr <5°+ε (4.6)

[0067] In the formula, ε>0 is the relaxation factor. The relaxation factor is added to the tire slip angle constraint to avoid the situation where there is no feasible solution to the model predictive control optimization problem due to unreasonable setting of the tire slip angle extreme value;

[0068] (3) Chassis roll angle constraint: lateral displacement y of the mobile chassis ZMP The calculation formula is as follows:

[0069]

[0070] In the formula, h CG is the height of the chassis center of mass; φ is the chassis roll angle; is the heading angle of the chassis; I x is the moment of inertia of the chassis around the x-axis; ZMP To constrain:

[0071] -y ZMP,max ≤y ZMP ≤y ZMP,max (4.8)

[0072] Preferably, in the above technical solution, the objective function in the vehicle-arm collaborative controller of step 4 includes the tracking capability of the chassis to the reference trajectory, the tracking capability of the manipulator to the reference trajectory, and the motion stability of the chassis and the manipulator; the design of the objective function includes:

[0073]

[0074] Where, (t-1) is the last sampling time; N p Predict the time domain for the system; N c is the system control time domain; ΔU=[Δu(t)…Δu(t+N p -1)] T ; The first item represents the tracking capability requirement of the chassis and the manipulator for the desired path; the second item represents the motion stability requirement of the robot system; the third item represents the control requirement for the rotation angle of the manipulator joint; ρ∈R is the weighted coefficient of the relaxation factor;

[0075] Combining equations 4.1-4.9, we get the vehicle-arm coordination controller as follows:

[0076]

[0077] Preferably, in the above technical solution, the calculation and solution in the vehicle-arm coordinated controller in step 4 is as follows: After solving equation (4.10) in each control cycle, a series of control input increments ΔU in the control time domain are obtained: * =[Δu * (t),Δu * (t+1),…,Δu * (t+N p -1)] T , and its first element is used as the actual control input increment to act on the mobile robot u(t) = u(t-1) + Δu * (t); After entering the next control cycle, the above process is repeated to achieve trajectory tracking control of the mobile chassis and the robotic arm.

[0078] Compared with the prior art, the present invention has the following beneficial effects: The present invention provides a method for coordinated motion control of a mobile robot arm in hilly and mountainous areas. The method is used for standing tree pruning in timber forests, and realizes the transportation and movement of pruning machines in artificial forest environments in hilly and mountainous areas. On the premise of obtaining the global planning path of a mobile robot in a certain forest area, the method can first perform local path planning for obstacles in the working area, plan the trajectories of the mobile chassis and the robot arm, and coordinately control the chassis and the robot arm to track their respective trajectories for fully automatic operation. The task of carrying and moving the pruning machine is completed, unmanned operation is realized, and the degree of automation of the robot is greatly improved. BRIEF DESCRIPTION OF THE DRAWINGS

[0079] Figure 1 It is a technical roadmap of the coordinated motion control method of a mobile manipulator in hilly and mountainous areas according to the present invention;

[0080] Figure 2 is a schematic diagram of a mobile robot arm control system according to the method of the present invention;

[0081] Figure 3 Schematic diagram of a mobile robot control system according to the method of the present invention. DETAILED DESCRIPTION

[0082] The specific implementation modes of the present invention are described in detail below in conjunction with the accompanying drawings, but it should be understood that the protection scope of the present invention is not limited by the specific implementation modes.

[0083] Unless explicitly stated otherwise, throughout the specification and claims, the term “comprise” or variations such as “include” or “comprising”, etc., will be understood to include the stated elements or components but not to exclude other elements or components.

[0084] like Figures 1 to 3 As shown, according to a specific embodiment of the present invention, a method for controlling the coordinated motion of a mobile mechanical arm in a hilly mountainous area is provided.

[0085] Step 1: Local path planner design

[0086] Based on the global reference path and obstacle information, an obstacle model is established, an artificial potential field is created, and local paths are planned for the mobile chassis and the robotic arm respectively. Finally, the local path is output to the vehicle-arm collaborative controller.

[0087] The task of the mobile robot arm is to detect the forest environment and automatically carry and move the pruning machine to realize the automation of standing tree pruning operations. Figure 2As shown in the figure, the control system mainly includes a local path planner and a vehicle-arm collaborative controller. Based on the local path output by the local path planner, the vehicle-arm collaborative controller is used to control the mobile chassis and the robotic arm to move along the planned path, and the robotic arm grasps the pruning machine to reach the pruning position.

[0088] The purpose of designing a local path planner is to perform local path planning under the condition of satisfying constraints, realize the obstacle avoidance function and minimize the deviation from the global reference path, and finally output it to the vehicle-arm collaborative controller. The global reference path of the mobile chassis and the robotic arm is obtained by the mapping and positioning module and the path planning module. Since the present invention mainly focuses on the control of the chassis and the robotic arm, these aspects are not mentioned. The specific steps of the local path planner design are as follows:

[0089] 1) Obstacle model establishment. After the pruning robot completes the pruning of a tree, it uses a multi-line laser radar to detect obstacles while moving to the next tree, and obtains the point cloud of the obstacle. Then, the AABB bounding box technology is used to envelop the obstacle with cylinders of different shapes, thereby replacing irregular obstacles for local path planning.

[0090] 2) Obstacle avoidance function design. The obstacle avoidance function of the local path planner is implemented using the artificial potential field method. The following gravitational field function is established:

[0091]

[0092] In the formula, k a is the gravitational coefficient; d goal is the distance from the control point to the target point; the gravitational field is divided into gravitational field and variable force field, d L is the variable field radius of the gravitational field. In the variable field, the gravitational force is related to d goal Related: In a constant force field, the magnitude of gravity remains unchanged, preventing the problem of excessive gravity due to excessive distance.

[0093] Establish the following repulsive field function:

[0094]

[0095] In the formula, k r is the gravitational coefficient; d obs is the distance from the control point to the obstacle point; d r is the radius of the repulsive field. The purpose of setting the repulsive field function is to avoid the problem that the control point cannot enter the effective range of the target point when the target point is in the repulsive field.

[0096] 3) Local path planning. The process is as follows:

[0097] ① Take several points at equal distances from the global reference path as the target point set for local path planning.

[0098] ② For the mobile chassis, the obstacle model is expanded with the radius of the inscribed circle and circumscribed circle of the chassis motion center as the size, and the target points in the obstacle model are eliminated. Then the gradient descent algorithm is used, starting from the current position, and moving forward along the negative gradient direction until the gradient reaches the position of zero, and the planning point sequence is output.

[0099] ③ For the robot arm, the obstacle model is expanded with the radius of the pruning machine size, and the target points in the obstacle model are eliminated. First, the joint space pose of each joint is calculated, and then a set of joint angles with the smallest combined potential energy is found to determine whether there is a collision with the joint angle. If there is a collision with the robot arm link, a suitable joint angle is found again. The robot arm moves in the direction with the smallest combined potential energy of each joint until it reaches the target point, and outputs the planning point sequence.

[0100] ④ If the planning point falls into the state of potential field extreme value and oscillates, add random perturbations at the local extreme value to jump out of the local extreme value.

[0101] Step 2: Construction of kinematic model of mobile robot

[0102] The kinematic model of the mobile manipulator is constructed by the motion variables of the mobile chassis and the manipulator.

[0103] In the motion control of the mobile robot, the selected model has a substantial impact on the control results. The present invention uses the robot's kinematic model to derive the prediction model of the controller. The motion variables of the mobile chassis and the robot are as follows: Figure 3 The specific steps of kinematic modeling are as follows:

[0104] 1) Mobile chassis kinematic modeling. Due to the delay and dynamic characteristics of the chassis steering system and speed control, the chassis steering system and speed controller are modeled using first-order lag plus time delay:

[0105]

[0106] Where s is the time constant of the first-order lag; L is the time delay;

[0107] Taking the center of the rear axle of the chassis as the origin of its coordinate system, the kinematic model of the mobile chassis is as follows:

[0108]

[0109] Where (x, y) is the center coordinate of the rear axle of the chassis, is the heading angle of the chassis, δ is the front wheel steering angle, v is the speed of the center of the rear axle of the chassis, l is the wheelbase of the front and rear axles of the chassis, τ v , τ α are the time constants of chassis speed and steering angle, respectively;

[0110] 2) Kinematic modeling of the robot arm. The robot arm is a six-degree-of-freedom robot arm installed on the central axis of the mobile chassis. The six joints are all rotary joints, and their joint variables are [θ1θ2θ3θ4θ5θ6]. The DH parameter method is used to model it. The coordinates of the end of the robot arm in the world coordinate system are P tip as follows:

[0111]

[0112] x tip =x+d6[c5s1+cos(θ2+θ3+θ4)c1s5]-d4s1+l3c1cos(θ2+θ3)+l2c1c2+d5c1sin(θ2+θ3+θ4)+d bb

[0113] y tip =y+d4c1+l3s1cos(θ2+θ3)-d6c1c5+l2c2s1+d5sin(θ2+θ3+θ4)s1+d6cos(θ2+θ3+θ4)s1s5

[0114] z tip =z+d1-d5[sin(θ2+θ3)s4-cos(θ2+θ3)c4]-l3sin(θ2+θ3)-l2s2-d6s5[cos(θ2+θ3)s4+sin(θ2+θ3)c4]+h bb

[0115] In the formula, c i 、s i are cosθ i and sinθ i abbreviation of; l i is the length of the i-th link of the robot arm; d i is the connecting rod offset; (x, y, z) is the center coordinate of the rear axle of the chassis, d bb Distance between the base of the robot and the rear axle of the chassis, h bb The height difference between the base of the robot arm and the rear axle of the chassis;

[0116] Calculate the end velocity V of the robot arm tip as follows:

[0117]

[0118] Step 3: State Estimator Design

[0119] The extended Kalman filter is used to perform real-time optimal estimation of the lateral slip and longitudinal slip of the chassis.

[0120] Specifically, the sensing system of the mobile manipulator consists of an RTK-GPS system, an inertial measurement unit (IMU), and a multi-line laser radar. The RTK-GPS system is used to estimate the position of the chassis; the IMU is used to measure the three-axis attitude angle and acceleration of the chassis; the multi-line laser radar is used to detect standing tree trunks and obstacles. When the mobile chassis is running on complex terrains such as hilly mountains, the wheels will slip and slide with the soft soil. Therefore, combined with the measurement signal of the accelerometer, the extended Kalman filter is used to perform real-time optimal estimation of the chassis state, and at the same time, the IMU signal is corrected in real time. The specific steps of the state estimator design are as follows:

[0121] 1) Definition of slip rate. Longitudinal slip and sliding are manifested as errors in the wheel's directional speed, while lateral slip is manifested as a speed or lateral force in the vertical direction of the wheel. The definition of slip rate is as follows:

[0122]

[0123] Where r is the wheel radius, ω is the wheel rotation angular velocity, and v w is the wheel speed.

[0124] 2) Establishment of the state model of the extended Kalman filter. Assume that lateral slip only occurs on the front wheels and longitudinal slip occurs on the rear wheels. The slip of the front wheels acts on the steering actuator, and the slip of the rear wheels acts on the speed controller. The state model of the extended Kalman filter used for mobile chassis state estimation is as follows:

[0125]

[0126] In the formula, is the estimated value of the front wheel slip rate, is the estimated value of rear wheel slip rate.

[0127] Step 4: Design of vehicle-arm collaborative controller based on NMPC

[0128] The nonlinear model predictive control method is used to coordinate the control of the chassis and the robotic arm. The kinematic model of the mobile robotic arm is discretized to construct constraint conditions and objective functions. The optimization problem is solved in each control cycle, and the control input increment is applied to the mobile robotic arm to achieve trajectory tracking control of the mobile chassis and the robotic arm, and a vehicle-arm coordinated controller is designed.

[0129] The chassis and the manipulator of the mobile manipulator are controlled collaboratively using nonlinear model predictive control (NMPC) to ensure that the movement of the mobile chassis and the manipulator accurately tracks the reference trajectory output by the local path planner. The specific steps of the vehicle-arm collaborative controller design are as follows:

[0130] 1) Model discretization. According to equations 2.2 to 2.4, the general form of the control system of the mobile robot is:

[0131]

[0132] The input is They are the speed and acceleration of the mobile chassis, the front wheel angle and the angular velocity of each joint of the robotic arm; the state quantities are:

[0133]

[0134] The state quantities include the rear axle center coordinates and speed of the mobile chassis, the chassis heading angle, roll angle, and the angles and angular velocities of each joint of the robotic arm.

[0135] Using Taylor series expansion for equation (4.1) and ignoring higher-order terms, the state increment is obtained as:

[0136]

[0137] Discretize equation (4.2):

[0138]

[0139] Among them, A k,t =I n +TA(t), T is the sampling time of the discrete system, A(t) is the system's response to the state variable The Jacobian matrix of k,t =TB(t), B(t) is the Jacobian matrix of the system for the state variable u.

[0140] 2) Constraint design.

[0141] ① Steering constraints and speed constraints. The constraints of the mobile chassis include steering constraints, speed constraints, and tire side slip angle constraints. The constraints of the robot arm include angle constraints and speed constraints of each joint. The constraints are as follows:

[0142]

[0143] Among them, the extreme values ​​of the front wheel steering angle δ and the chassis speed v are obtained by the lateral and longitudinal tracking ability test of the chassis; the robot arm joint angle θ and the rotation speed The extreme value of is obtained from the performance test of the robotic arm.

[0144] ② Tire slip angle constraint: The tire slip angle α includes the left and right front wheel and left and right rear wheel tire slip angles, which are α fl , α fr , α rl , α rr , the calculation formula of the sideslip angle is as follows:

[0145]

[0146] Where (x, y) is the center coordinate of the rear axle of the chassis, is the heading angle of the chassis, δ is the front wheel steering angle; a and b are the distances from the front and rear axles to the center of mass respectively; c is the half wheelbase of the front and rear wheels; according to the tire slip angle characteristics, when the tire slip angle does not exceed 5°, the slip angle and the cornering force are in a linear relationship; therefore, the tire slip angle constraint is set as follows:

[0147] -5°-ε<α fl,fr,rl,rr <5°+ε (4.6)

[0148] In the formula, ε>0 is the relaxation factor. The relaxation factor is added to the tire slip angle constraint to avoid the situation where there is no feasible solution to the model predictive control optimization problem due to unreasonable setting of the tire slip angle extreme value;

[0149] ③ Chassis roll angle constraint. Since the actual forest environment is hilly and mountainous, when the chassis is driving on uneven ground and there is lateral acceleration, the zero moment point of the chassis will be offset relative to the lateral direction. When the zero moment point is offset outside the vehicle wheelbase, the vehicle will roll. The lateral offset y of the mobile chassis ZMP The calculation formula is as follows:

[0150]

[0151] In the formula, h CG is the height of the chassis center of mass; φ is the chassis roll angle; is the heading angle of the chassis; I x is the moment of inertia of the chassis around the x-axis; ZMP To constrain:

[0152] -y ZMP,max ≤y ZMP ≤y ZMP,max (4.8)

[0153] 3) Objective function design. The goal of the vehicle-arm collaborative controller is to ensure that the mobile chassis and the robotic arm can quickly and smoothly track the desired trajectory. Therefore, the objective function includes the chassis's tracking ability for the reference trajectory, the robotic arm's tracking ability for the reference trajectory, and the stability of the chassis and robotic arm's motion. The designed objective function is as follows:

[0154]

[0155] Where, (t-1) is the last sampling time; N p Predict the time domain for the system; N c is the system control time domain; ΔU=[Δu(t)…Δu(t+N p -1)] T ; The first item represents the tracking capability requirement of the chassis and the robot arm for the desired path; the second item represents the motion smoothness requirement of the robot system; the third item represents the control requirement for the rotation angle of the robot arm joint; ρ∈R is the weighted coefficient of the relaxation factor.

[0156] Combining equations 4.1-4.9, we get the vehicle-arm coordination controller as follows:

[0157]

[0158] 4) Calculation and solution. After solving equation (3.10) in each control cycle, a series of control input increments ΔU in the control time domain are obtained. * =[Δu * (t),Δu * (t+1),…,Δu * (t+N p -1)] T , and its first element is used as the actual control input increment to act on the mobile robot u(t) = u(t-1) + Δu * (t); After entering the next control cycle, the above process is repeated to achieve trajectory tracking control of the mobile chassis and the robotic arm.

[0159] The present invention discloses a method for controlling the coordinated motion of a mobile robot arm in hilly and mountainous areas. The method is used for pruning standing trees in timber forests, and realizes the transportation and movement of a pruning machine in an artificial forest environment in hilly and mountainous areas. On the premise of obtaining a global planning path for a mobile robot in a certain forest area, the method can firstly perform local path planning for obstacles in the working area, plan the trajectory of the mobile chassis and the robot arm, and coordinately control the chassis and the robot arm to track their respective trajectories for fully automatic operation. The task of carrying and moving the pruning machine is completed, unmanned operation is realized, and the degree of automation of the robot is greatly improved.

[0160] The foregoing description of specific exemplary embodiments of the present invention is for the purpose of illustration and demonstration. These descriptions are not intended to limit the present invention to the precise form disclosed, and it is clear that many changes and variations can be made based on the above teachings. The purpose of selecting and describing the exemplary embodiments is to explain the specific principles of the present invention and its practical application, so that those skilled in the art can realize and utilize various different exemplary embodiments of the present invention and various different selections and changes. The scope of the present invention is intended to be limited by the claims and their equivalents.

Claims

1. A method for coordinated motion control of a mobile robot arm in hilly and mountainous areas, characterized in that: The following steps are involved: Step 1: Design a local path planner: Based on the global reference path and obstacle information, an obstacle model is established, an artificial potential field is established, and local paths are planned for the mobile chassis and the robotic arm respectively. Finally, the local path planning is output to the vehicle-arm collaborative controller. Step 2: construct a kinematic model of the mobile robot arm: construct a kinematic model of the mobile robot arm based on the motion variables of the mobile chassis and the robot arm; Step 3: Design a state estimator: Use an extended Kalman filter to perform real-time optimal estimation of the lateral and longitudinal slip of the chassis; Step 4, designing a vehicle-arm collaborative controller: The vehicle-arm collaborative controller receives the reference trajectory output by the local path planner in step 1, and uses a nonlinear model predictive control method to collaboratively control the chassis and the robotic arm; discretizes the kinematic model of the mobile robotic arm, constructs constraint conditions and objective functions; solves the optimization problem in each control cycle, applies the control input increment to the mobile robotic arm, and realizes trajectory tracking control of the mobile chassis and the robotic arm; The method of designing a local path planner in step 1 includes: (1) Establishing obstacle model: Multi-line laser radar detects obstacles and obtains point clouds of obstacles. Then, AABB bounding box technology is used to envelop the obstacles with cylinders of different shapes, thereby replacing irregular obstacles for local path planning. (2) Design of obstacle avoidance function: The obstacle avoidance function is realized by using the artificial potential field method, and the following gravitational field function is established: In the formula, k a is the gravitational coefficient; d goal is the distance from the control point to the target point; the gravitational field is divided into gravitational field and variable force field, d L is the variable field radius of the gravitational field. In the variable field, the gravitational force is related to d goal Related; In a constant force field, the magnitude of gravity remains constant; Establish the following repulsive field function: In the formula, k r is the gravitational coefficient; d obs is the distance from the control point to the obstacle point; d r is the radius of the repulsive field; (3) Local path planning; Step 2: The method of constructing the kinematic model of the mobile robot includes: (1) Mobile chassis kinematic modeling: The chassis steering system and speed controller are modeled using the delay and dynamic characteristics of the chassis steering system and speed control using first-order lag plus time delay: Where s is the time constant of the first-order lag; L is the time delay; Taking the center of the rear axle of the chassis as the origin of its coordinate system, the kinematic model of the mobile chassis is as follows: Where (x, y) is the center coordinate of the rear axle of the chassis, is the heading angle of the chassis, δ is the front wheel steering angle, v is the speed of the center of the rear axle of the chassis, l is the wheelbase of the front and rear axles of the chassis, τ v , τ α are the time constants of chassis speed and steering angle, respectively; (2) Kinematic modeling of the robot arm: The robot arm is a six-degree-of-freedom robot arm installed on the central axis of the mobile chassis. The six joints are all rotary joints, and their joint variables are [θ1θ2θ3θ4θ5θ6]. The DH parameter method is used to model it. The coordinates of the end of the robot arm in the world coordinate system are P tip as follows: x tip =x+d6[c5s1+cos(θ2+θ3+θ4)c1s5]-d4s1+l3c1cos(θ2+θ3)+l2c1c2+d5c1sin(θ2+θ3+θ4)+d bb y tip =y+d4c1+l3s1cos(θ2+θ3)-d6c1c5+l2c2s1+d5sin(θ2+θ3+θ4)s1+d6cos(θ2+θ3+θ4)s1s5 z tip =z+d1-d5[sin(θ2+θ3)s4-cos(θ2+θ3)c4]-l3sin(θ2+θ3)-l2s2-d6s5[cos(θ2+θ3)s4+sin(θ2+θ3)c4]+h bb In the formula, c i 、s i are cosθ i and sinθ i abbreviation of; l i is the length of the i-th link of the robot arm; d i is the connecting rod offset; (x, y, z) is the center coordinate of the rear axle of the chassis, d bb Distance between the base of the robot and the rear axle of the chassis, h bb The height difference between the base of the robot arm and the rear axle of the chassis; Calculate the end velocity V of the robot arm tip as follows: Step 3: Designing a state estimator includes acquiring data from the sensor system of the mobile manipulator, combining the measurement signal of the accelerometer, and using an extended Kalman filter to perform real-time optimal estimation of the chassis state, while performing real-time correction of the inertial measurement device; Among them, the sensing system of the mobile robotic arm includes an RTK-GPS system, an inertial measurement device and a multi-line laser radar. The RTK-GPS system is used to estimate the position of the chassis; the inertial measurement device is used to measure the three-axis attitude angle and acceleration of the chassis; and the multi-line laser radar is used to detect standing tree trunks and obstacles.

2. The method for controlling coordinated motion of a mobile robot arm in hilly and mountainous areas according to claim 1, characterized in that: Step (3) of local path planning includes the following steps: 1) Take several points at equal distances from the global reference path as the target point set for local path planning; 2) For the mobile chassis, the obstacle model is expanded with the radius of the inscribed circle and circumscribed circle of the chassis motion center as the size, and the target points in the obstacle model are eliminated; then the gradient descent algorithm is used, starting from the current position, and moving forward along the negative gradient direction until the gradient reaches the position of zero, and the planning point sequence is output; 3) For the robot arm, the obstacle model is expanded with the radius of the pruning machine size to remove the target points in the obstacle model; first calculate the joint space pose of each joint, then find a set of joint angles with the smallest combined potential energy, and determine whether there is a collision in the joint angle. If there is a collision in the robot arm connecting rod, find a suitable joint angle again, and move the robot arm in the direction with the smallest combined potential energy of each joint until it reaches the position of the target point, and output the planning point sequence; 4) If the planning point falls into the state of potential field extreme value and oscillates, random perturbations are added at the local extreme value to jump out of the local extreme value.

3. The method for controlling coordinated motion of a mobile robot arm in hilly and mountainous areas according to claim 1, characterized in that: Step 3 The steps of designing the state estimator include: (1) Definition of slip rate: Longitudinal slip and sliding are manifested as errors in the wheel's directional velocity, while lateral slip is manifested as a velocity or lateral force in the vertical direction of the wheel. The definition of slip rate is as follows: Where r is the wheel radius, ω is the wheel rotation angular velocity, and v w is the wheel speed; (2) Establish the state model of the extended Kalman filter: Assume that lateral slip occurs only on the front wheels and longitudinal slip occurs on the rear wheels; the slip of the front wheels acts on the steering actuator, and the slip of the rear wheels acts on the speed controller; the state model of the extended Kalman filter used for mobile chassis state estimation is as follows: In the formula, is the estimated value of the front wheel slip rate, is the estimated value of rear wheel slip rate.

4. The method for controlling coordinated motion of a mobile robot arm in hilly and mountainous areas according to claim 1, characterized in that: Step 4: Discretize the model in the vehicle-arm collaborative controller: discretize the kinematic model of the mobile robotic arm. The general form of the control system of the mobile robotic arm is: The input is They are the speed and acceleration of the mobile chassis, the front wheel angle and the angular velocity of each joint of the robotic arm; the state quantities are: The state quantities include the rear axle center coordinates and speed of the mobile chassis, the chassis heading angle, roll angle, and the angles and angular velocities of each joint of the robotic arm; Using Taylor series expansion for equation (4.1) and ignoring higher-order terms, the state increment is obtained as: Discretize equation (4.2): Among them, A k,t =I n +TA(t), T is the sampling time of the discrete system, A(t) is the Jacobian matrix of the system for the state variable x; B k,t =TB(t), B(t) is the Jacobian matrix of the system for the state variable u.

5. The method for controlling coordinated motion of a mobile robot arm in hilly and mountainous areas according to claim 1, characterized in that: Step 4: Design the constraints in the vehicle-arm collaborative controller, including: (1) Steering constraints and speed constraints: The constraints of the mobile chassis include steering constraints, speed constraints, and tire slip angle constraints; the constraints of the robotic arm include angle constraints and speed constraints of each joint. The constraints are as follows: Among them, the extreme values ​​of the front wheel steering angle δ and the chassis speed v are obtained by the lateral and longitudinal tracking ability experimental test of the chassis; the robot arm joint angle θ and the rotation speed The extreme value of is obtained from the performance test of the robot arm; (2) Tire slip angle constraint: The tire slip angle α includes the slip angles of the left and right front wheels and the left and right rear wheels, which are α fl , α fr , α rl , α rr , the calculation formula of the sideslip angle is as follows: Where (x, y) is the center coordinate of the rear axle of the chassis, is the heading angle of the chassis, δ is the front wheel steering angle; a and b are the distances from the front and rear axles to the center of mass respectively; c is the half wheelbase of the front and rear wheels; according to the tire slip angle characteristics, when the tire slip angle does not exceed 5°, the slip angle and the cornering force are in a linear relationship; therefore, the tire slip angle constraint is set as follows: -5°-e<a fl,fr,rl,rr <5°+ε (4.6) In the formula, ε>0 is the relaxation factor; (3) Chassis roll angle constraint: The lateral offset y of the mobile chassis ZMP The calculation formula is as follows: In the formula, h CG is the height of the chassis center of mass; φ is the chassis roll angle; is the heading angle of the chassis; I x is the moment of inertia of the chassis around the x-axis; ZMP To constrain: -and ZMP,mix ≤y ZMP ≤y ZMP,max (4.8)。 6. The method for controlling coordinated motion of a mobile robot arm in hilly and mountainous areas according to claim 1, characterized in that: Step 4: Design the objective function of the vehicle-arm collaborative controller, which includes the tracking capability of the chassis to the reference trajectory, the tracking capability of the manipulator to the reference trajectory, and the motion stability of the chassis and the manipulator. The design of the objective function includes: Where, (t-1) is the last sampling time; N p Predict the time domain for the system; N c is the system control time domain; ΔU=[Δu(t)…Δu(t+N p -1)] T ; The first item represents the tracking capability requirement of the chassis and the manipulator for the desired path; the second item represents the motion stability requirement of the robot system; the third item represents the control requirement for the rotation angle of the manipulator joint; ρ∈R is the weighted coefficient of the relaxation factor; The vehicle-arm collaborative controller is obtained by integrating the discretization of the kinematic model, the constraints and the objective function as follows:

7. The method for controlling coordinated motion of a mobile robot arm in hilly and mountainous areas according to claim 6, characterized in that: Step 4: Design the vehicle-arm coordinated controller to calculate and solve: After solving equation (4.10) in each control cycle, a series of control input increments ΔU in the control time domain are obtained. * =[Δu * (t),Δu * (t+1),…,Δu * (t+N p -1)] T , and its first element is used as the actual control input increment to act on the mobile robot u(t) = u(t-1) + Δu * (t); After entering the next control cycle, the above process is repeated to achieve trajectory tracking control of the mobile chassis and the robotic arm.

Images