Multi-mode path planning system and method for space station manipulator adapting to complex tasks

Abstract

The present invention discloses a multi-mode path planning system and method for a space station manipulator adapted to complex tasks. The system comprises: an instruction processing module, a planning algorithm module and a basic calculation module; the instruction processing module receives the control instruction input by the operating console, parses the instruction format and data information, and outputs the parsed data information to the planning algorithm module as a calculation input; the planning algorithm module implements the calculation of multiple motion mode planning algorithms; receives the data parameters output by the instruction processing module, selects the corresponding motion mode, and outputs the expected joint angle and expected angular velocity sequence data to the basic calculation module; the basic calculation module implements the basic calculation function, realizes the conversion between the terminal posture / speed and the joint angle / speed, and outputs the expected joint angle and expected angular velocity sequence data detected without collision to the joint controller. The present invention solves the problem of safe and fast path planning under complex condition constraints.

Description

Technical Field

[0001] The present invention belongs to the technical field of robots and relates to a multi-mode path planning system and method for a space station manipulator. Technical Background

[0002] The robotic arm is one of the major key technologies of my country's manned space station project. The robotic arm of the space station is used for many tasks such as space station assembly and construction, maintenance and repair, assisting astronauts' extravehicular activities, and supporting space applications. The space station has many equipment on the surface of the cabin, a large solar wing with a large rotation envelope, and a wide range of measurement and control antenna beams. The robotic arm needs to avoid the equipment and antenna beam range during movement; the robotic arm of the space station also needs to complete dynamic tracking, precise capture, and auxiliary transfer of large hovering cabins. In order to achieve full coverage of the surface of the space station, the robotic arm has a "crawling" function and can move between multiple "footprints" arranged on the surface of the cabin (for details, see 201910653256.5 "A foldable target adapter suitable for large tolerance capture"). The above mission requirements result in the unique characteristics of the space station robotic arm, such as a large range of motion, narrow space, and a wide variety of missions. The single-mode path planning method cannot meet the current mission requirements.

[0003] The patents and articles on the robot path planning methods currently disclosed mainly focus on the single path planning theory method itself. The invention with publication number CN113843791A (a robot multi-point motion trajectory planning method) proposes a terminal continuous trajectory planning method suitable for multi-point constraints, focusing on solving the problem of continuous smoothness of the robot motion joint angle and speed. The invention patent with publication number CN113858205A (a seven-axis redundant robot obstacle avoidance algorithm based on improved RRT*) proposes a method for searching for a collision-free asymptotic optimal path between the starting point and the desired target point using the RRT* algorithm. This method combines the heuristic strategy with the RRT* algorithm, which improves the planning speed to a certain extent, but it still requires online real-time collision detection and post-collision path optimization, which has high requirements on computer performance and there is a risk of collision at a close safety distance.

[0004] Based on the above background, the current path planning method cannot adapt to the multi-task path planning requirements of the complex environment of the space station robotic arm. It is of great significance to study efficient and safe path planning methods under the constraints of strict task time constraints, narrow motion space, complex task types and links. Summary of the invention

[0005] The technical problem to be solved by the present invention is: the present invention provides a multi-mode path planning system and method for a space station robotic arm that is adaptable to complex tasks, and solves the problem of safe and fast path planning under constraints such as limited computing resources, strict time constraints, narrow motion space, and complex task types and links.

[0006] The technical problem to be solved by the present invention is: a multi-mode path planning system for a space station manipulator adapted to complex tasks, comprising: an instruction processing module, a planning algorithm module and a basic calculation module;

[0007] Instruction processing module: receives control instructions input from the console, parses instruction format and data information, and outputs the parsed data information to the planning algorithm module as calculation input; the data information includes pre-programmed instruction sequence number, target point expected posture, joint expected angle, joint speed / acceleration setting, terminal speed / acceleration setting, visual servoing target point posture;

[0008] Planning algorithm module: realizes the calculation of planning algorithms for multiple motion modes; receives the data parameters output by the instruction processing module, selects the corresponding motion mode, and outputs the expected joint angle and expected angular velocity sequence data to the basic calculation module; the motion modes include pre-programmed motion mode, single joint position motion mode, multi-joint linkage motion mode, terminal linear motion mode, terminal handle motion mode, and visual servo motion mode;

[0009] Basic calculation module: realizes basic calculation functions, realizes conversion between end posture / velocity and joint angle / velocity, and outputs the expected joint angle and expected angular velocity sequence data for collision-free detection to the joint controller; the basic calculation includes forward kinematics calculation, inverse kinematics calculation, construction of speed continuous change displacement curve, and collision detection calculation.

[0010] Furthermore, the forward kinematics calculation calculates the position and posture of the end of the manipulator according to the DH parameters and joint angles of the spatial manipulator.

[0011] Furthermore, the inverse kinematics calculation calculates the speed of each joint of the robotic arm based on the DH parameters and the terminal speed of the spatial robotic arm.

[0012] Furthermore, the displacement curve with continuously changing construction speed includes:

[0013] The robot arm's running path is planned according to the trapezoidal method. According to the starting time, end time, acceleration time, maximum speed and displacement corresponding to the end point, the function expression of each segment of the displacement curve with continuously changing speed and time as the independent variable is obtained.

[0014] Furthermore, the collision detection calculation includes: a ground algorithm and an on-orbit algorithm;

[0015] On-orbit algorithm: Dynamic collision detection using simplified geometric models is adopted. The space station cabin and the robot arm body are simplified into multiple outer envelope cylindrical geometric models. The distance between each body is calculated based on the actual angle of the joint to achieve collision detection. The collision mark, collision parts and distance between parts are output.

[0016] Ground algorithm: The scattered point scanning static space interference method is used to establish the outer envelope model of the space station cabin and the robotic arm body through the scattered point scanning method. The distance between all discrete points is traversed and calculated according to the actual angle of the joint to realize collision detection, and the collision mark, specific collision point and the distance between each point on the component are output.

[0017] Furthermore, in the pre-programmed motion mode, the central controller CPU pre-stores the expected angle and expected angular velocity information of all joints. After the central controller CPU receives the pre-programmed control instruction issued by the operating platform, the CPU reads the pre-stored data of the corresponding address and sends it to each joint controller according to the control cycle output to control the output trajectory of each joint;

[0018] In the pre-programmed motion mode, the pre-stored expected joint angle and angular velocity are sent once in one control cycle.

[0019] Furthermore, the single joint position mode includes:

[0020] After selecting a single joint, the position of the single joint is planned, and the joint planning angle and angular velocity instructions are output to the basic calculation module. After verification that there is no collision, they are sent to the joint controller until they are in place;

[0021] The position planning of the single joint adopts a trapezoidal joint speed planning of first accelerating, then uniform speed and finally decelerating to obtain the desired joint angular velocity; according to the method of constructing a displacement curve with continuously changing speed in the basic algorithm module, the joint angle command sequence is calculated until it is in place and the planning is completed.

[0022] Furthermore, the multi-joint linkage motion mode includes:

[0023] After selecting the target angle of each joint, position planning is performed in the joint space, and the angular velocity and angle command sequence are output to the basic calculation module. After verification that there is no collision, it is sent to the joint controller until it runs into place; each joint plans the movement at the same time, first calculates the required rotation angle of each joint, and obtains the maximum joint rotation deviation. The movement time is calculated using the maximum joint angle deviation and the maximum set speed, and the maximum movement speed of the remaining joints is recalculated based on the movement time to ensure the synchronization of the joint movements.

[0024] Furthermore, the terminal linear motion mode includes:

[0025] Terminal linear velocity planning: According to the straight-line distance between the initial terminal posture and the target terminal posture of the robot arm, the maximum speed and acceleration time, the terminal position and velocity at each moment are calculated;

[0026] Angular velocity planning: Based on the deviation between the initial and final postures, the maximum angular velocity, and the acceleration time, the expected posture and angular velocity at each moment are calculated.

[0027] Furthermore, the end handle motion mode includes:

[0028] According to the task requirements, the terminal moving speed in the desired direction in the current coordinate system is given through the handle; after the coordinate system transformation, the terminal moving speed in the space station base coordinate system is obtained, and the angular velocity of each joint movement is obtained by inverse kinematics calculation; until the handle input speed command is zero, the movement ends.

[0029] Furthermore, the visual servoing motion mode includes:

[0030] The target posture information is measured in real time by the camera on the wrist of the robot arm, and the target posture information is provided to the planning algorithm module. The robot arm is moved to the target point autonomously through the visual servo algorithm to achieve tracking of the moving target in space.

[0031] The visual servo algorithm comprises:

[0032] (a) Calculate the position coordinate difference D of the target point coordinate system W relative to the terminal coordinate system T in the terminal coordinate system T oe And calculate the distance d between the end and the target point v ;

[0033] (b) Calculate the next displacement S end =vel*dt*D oe / d v , where vel is the set end-of-arm motion speed, dt is the motion control cycle, and d v is the distance between the end point and the target point;

[0034] (c) According to the end displacement S end , calculate the two sets of joint angle values ​​θ corresponding to the start and end points by the inverse kinematic solution now and θ next , where θ now is the current joint angle value, θ next is the joint angle value at the beginning of the next cycle;

[0035] (d) Determine the distance d between the end and the target point at this time vWhether it is within the given error range, and whether the difference ΔEul between the terminal attitude Euler angle and the target attitude is within the given error range, if so, the tracking ends; otherwise, return to step (a) to continue tracking until the conditions are met.

[0036] The space station manipulator multi-mode path planning method using the above-mentioned space station manipulator multi-mode path planning system adapted to complex tasks includes:

[0037] Receive control instructions input from the console and parse the instruction format and data information; the data information includes pre-programmed instruction serial number, target point desired posture, joint desired angle, joint speed / acceleration setting, terminal speed / acceleration setting, and visual servo target point posture;

[0038] According to the analyzed data information, a corresponding motion mode is selected to output the expected joint angle and expected angular velocity sequence data; the motion modes include pre-programmed motion mode, single joint position motion mode, multi-joint linkage motion mode, terminal linear motion mode, terminal handle motion mode, and visual servo motion mode;

[0039] The expected joint angle and expected angular velocity sequence data detected without collision are output to the joint controller to control the joint movement.

[0040] The advantages of the present invention compared with the prior art are:

[0041] The present invention adopts a variety of robot arm motion path planning modes, selects the appropriate path planning mode according to the characteristics of each link of the task, and uses a combination of multiple modes to solve the problem of safe and fast path planning under the constraints of limited computing resources of the space station robot arm, strict time constraints, narrow motion space, complex task types and links, etc. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] Figure 1 This is the module composition diagram of the robot arm motion path planning system;

[0043] Figure 2 This is the coordinate system definition diagram for the 7-DOF robot;

[0044] Figure 3 It is the speed change curve diagram;

[0045] Figure 4 is the linear interpolation displacement curve;

[0046] Figure 5 A collision detection strategy diagram for combining tasks;

[0047] Figure 6 It is the running trajectory diagram of the end of the robot arm;

[0048] Figure 7 It is the terminal velocity curve with parabolic transition;

[0049] Figure 8 This is the schematic diagram of the visual servo mode. DETAILED DESCRIPTION

[0050] The present invention is described with reference to the accompanying drawings.

[0051] The present invention discloses a multi-mode path planning system and method for a space station manipulator that is adaptable to complex tasks. The system solves the problem of safe and fast path planning under the constraints of task types and complex links by combining multiple path planning modes. The system solves the problem of reliable path planning for a space station manipulator with limited onboard computer capabilities and extremely high safety requirements by combining an on-orbit simplified collision detection algorithm and a ground-based fine collision detection algorithm.

[0052] like Figure 1 As shown, a multi-mode path planning system for a space station manipulator adapted to complex tasks includes: an instruction processing module, a planning algorithm module and a basic calculation module;

[0053] Instruction processing module: receives control instructions input from the console, parses instruction format and data information, and outputs the parsed data information to the planning algorithm module as calculation input; the data information includes pre-programmed instruction sequence number, target point expected posture, joint expected angle, joint speed / acceleration setting, terminal speed / acceleration setting, visual servoing target point posture;

[0054] Planning algorithm module: realizes the calculation of planning algorithms for multiple motion modes; receives the data parameters output by the instruction processing module, selects the corresponding motion mode, and outputs the expected joint angle and expected angular velocity sequence data to the basic calculation module; the motion modes include pre-programmed motion mode, single joint position motion mode, multi-joint linkage motion mode, terminal linear motion mode, terminal handle motion mode, and visual servo motion mode;

[0055] Basic calculation module: realizes basic calculation functions, realizes conversion between end posture / velocity and joint angle / velocity, and outputs the expected joint angle and expected angular velocity sequence data for collision-free detection to the joint controller; the basic calculation includes forward kinematics calculation, inverse kinematics calculation, construction of speed continuous change displacement curve, and collision detection calculation.

[0056] The forward kinematics calculation calculates the position and posture of the end of the manipulator according to the DH parameters and joint angles of the spatial manipulator.

[0057] The inverse kinematics calculation calculates the speed of each joint of the robotic arm based on the DH parameters and the terminal speed of the spatial robotic arm.

[0058] The displacement curve with continuously changing construction speed includes:

[0059] The robot arm's running path is planned according to the trapezoidal method. According to the starting time, end time, acceleration time, maximum speed and displacement corresponding to the end point, the function expression of each segment of the displacement curve with continuously changing speed and time as the independent variable is obtained.

[0060] The collision detection calculation includes: a ground algorithm and an on-orbit algorithm;

[0061] On-orbit algorithm: Dynamic collision detection using simplified geometric models is adopted. The space station cabin and the robot arm body are simplified into multiple outer envelope cylindrical geometric models. The distance between each body is calculated based on the actual angle of the joint to achieve collision detection. The collision mark, collision parts and distance between parts are output.

[0062] Ground algorithm: The scattered point scanning static space interference method is used to establish the outer envelope model of the space station cabin and the robotic arm body through the scattered point scanning method. The distance between all discrete points is traversed and calculated according to the actual angle of the joint to realize collision detection, and the collision mark, specific collision point and the distance between each point on the component are output.

[0063] The pre-programmed motion mode pre-stores the expected angle and expected angular velocity information of all joints in the central controller CPU. After the central controller CPU receives the pre-programmed control instruction issued by the operating platform, the CPU reads the pre-stored data of the corresponding address and sends it to each joint controller according to the control cycle output to control the output trajectory of each joint;

[0064] In the pre-programmed motion mode, the pre-stored expected joint angle and angular velocity are sent once in one control cycle.

[0065] The single joint position mode comprises:

[0066] After selecting a single joint, the position of the single joint is planned, and the joint planning angle and angular velocity instructions are output to the basic calculation module. After verification that there is no collision, they are sent to the joint controller until they are in place;

[0067] The position planning of the single joint adopts a trapezoidal joint speed planning of first accelerating, then uniform speed and finally decelerating to obtain the desired joint angular velocity; according to the method of constructing a displacement curve with continuously changing speed in the basic algorithm module, the joint angle command sequence is calculated until it is in place and the planning is completed.

[0068] The multi-joint linkage motion mode includes:

[0069] After selecting the target angle of each joint, position planning is performed in the joint space, and the angular velocity and angle command sequence are output to the basic calculation module. After verification that there is no collision, it is sent to the joint controller until it runs into place; each joint plans the movement at the same time, first calculates the required rotation angle of each joint, and obtains the maximum joint rotation deviation. The movement time is calculated using the maximum joint angle deviation and the maximum set speed, and the maximum movement speed of the remaining joints is recalculated based on the movement time to ensure the synchronization of the joint movements.

[0070] The terminal linear motion mode includes:

[0071] Terminal linear velocity planning: According to the straight-line distance between the initial terminal posture and the target terminal posture of the robot arm, the maximum speed and acceleration time, the terminal position and speed at each moment are calculated;

[0072] Angular velocity planning: Based on the deviation between the initial and final postures, the maximum angular velocity, and the acceleration time, the expected posture and angular velocity at each moment are calculated.

[0073] The end handle motion mode includes:

[0074] According to the task requirements, the terminal moving speed in the desired direction in the current coordinate system is given through the handle; after the coordinate system changes, the terminal moving speed in the space station base coordinate system is obtained, and the angular velocity of each joint movement is obtained by inverse kinematics calculation; until the handle input speed command is zero, the movement ends.

[0075] The visual servo motion mode includes:

[0076] The target posture information is measured in real time by the camera on the wrist of the robot arm, and the target posture information is provided to the planning algorithm module. The robot arm is moved to the target point autonomously through the visual servo algorithm to achieve tracking of the moving target in space.

[0077] The visual servo algorithm comprises:

[0078] (a) Calculate the position coordinate difference D of the target point coordinate system W relative to the terminal coordinate system T in the terminal coordinate system T oe And calculate the distance d between the end and the target point v ;

[0079] (b) Calculate the next displacement S end =vel*dt*D oe / d v , where vel is the set end-of-arm motion speed, dt is the motion control cycle, and d v is the distance between the end point and the target point;

[0080] (c) According to the end displacement S end, calculate the two sets of joint angle values ​​θ corresponding to the start and end points by the inverse kinematic solution now and θ next , where θ now is the current joint angle value, θ next is the joint angle value at the beginning of the next cycle;

[0081] (d) Determine the distance d between the end and the target point at this time v Whether it is within the given error range, and whether the difference ΔEul between the terminal attitude Euler angle and the target attitude is within the given error range, if so, the tracking ends; otherwise, return to step (a) to continue tracking until the conditions are met.

[0082] The space station manipulator multi-mode path planning method using the above-mentioned space station manipulator multi-mode path planning system adapted to complex tasks includes:

[0083] Receive control instructions input from the console and parse the instruction format and data information; the data information includes pre-programmed instruction serial number, target point desired posture, joint desired angle, joint speed / acceleration setting, terminal speed / acceleration setting, and visual servo target point posture;

[0084] According to the analyzed data information, a corresponding motion mode is selected to output the expected joint angle and expected angular velocity sequence data; the motion modes include pre-programmed motion mode, single joint position motion mode, multi-joint linkage motion mode, terminal linear motion mode, terminal handle motion mode, and visual servo motion mode;

[0085] The expected joint angle and expected angular velocity sequence data detected without collision are output to the joint controller to control the joint movement.

[0086] Example:

[0087] The present invention aims to realize a multi-mode path planning method for a space station manipulator that is adaptable to multiple tasks. The hardware carrier for the manipulator motion path planning is a central controller, which is implemented by a CPU and an FPGA. The CPU implements functions such as instruction processing, mode switching, and information management, and the FPGA focuses on implementing path planning algorithm calculation. The main modules of the multi-mode path planning system for a space station manipulator that is adaptable to complex tasks are as follows: Figure 1As shown, it includes an instruction processing module, a planning algorithm module and a basic calculation module, wherein the instruction processing module receives the control instruction input by the operating console, and parses the instruction format and data information, the data information includes the pre-programmed instruction sequence number, the expected posture of the target point, the expected angle of the joint, the joint speed / acceleration setting, the terminal speed / acceleration setting, and the visual servo target point posture, and the parsed data information is output to the planning algorithm module as a calculation input; the planning algorithm module realizes the planning algorithm calculation of various motion modes such as pre-programming, single joint position, multi-joint linkage, terminal straight line, terminal handle, visual servo, etc., and outputs the expected joint angle and expected angular velocity sequence data to the basic calculation module; the basic calculation module realizes the basic calculation functions such as forward kinematics calculation, inverse kinematics calculation, construction of speed continuous change displacement curve, collision detection calculation, etc., and finally outputs the expected joint angle and expected angular velocity sequence data detected without collision to the joint controller.

[0088] (1) Basic computing module

[0089] The basic calculation module mainly includes functions such as forward kinematics calculation of the robot arm, inverse kinematics calculation, collision detection calculation, and construction of speed continuous change displacement curve, to achieve the conversion between end posture / speed and joint angle / speed. For the specific conversion methods between posture representation methods such as posture rotation matrix, Euler angle, posture quaternion, etc., please refer to textbooks such as "Robot Dynamics and Control".

[0090] 1) Positive kinematics calculation

[0091] The forward kinematics calculation of the robot arm is the mapping from the robot arm joint space to the end operation space. When the DH parameters and joint angles of the space robot arm are known, the position and posture of the end of the robot arm can be calculated. Figure 2 The figure shows the coordinate system of the 7-DOF serial robot arm, including the root installation coordinate system F0 and the joint coordinate systems F1 to F7.

[0092] The position and posture description symbols between the robot coordinate systems are defined as follows, where i = 0, 1, 2, ..., 7, j = 0, 1, 2, ..., 7:

[0093] C i，j Represents the coordinate system F j Relative to coordinate system F i The rotation matrix of

[0094] Indicates that under the reference of coordinate system F0, coordinate system F j Relative to coordinate system F i The position vector of

[0095] From this, the end position p of the seven-degree-of-freedom manipulator at a given joint angle can be calculated:0，7 and the attitude rotation matrix C 0，7 .

[0096] p 0，7 =p 0，1 +C 0，1 p 1，2 +C 0，1 C 1，2 p 2，3 +C 0，1 C 1，2 C 2，3 p 3，4 +...C 0，1 C 1，2 C 2，3 C 3，4 p 4，5 +C 0，1 C 1，2 C 2，3 C 3 ，4 C 4，5 p 5，6 +...C 0，1 C 1，2 C 2，3 C 3，4 C 4，5 C 5，6 p 6，7

[0097] C 0，7 =C 0，1 C 1，2 C 2，3 C 3，4 C 4，5 C 5，6 C 6，7 .

[0098] 2) Inverse kinematics calculation

[0099] The inverse kinematic solution of the robot arm is to map the velocity of the end of the robot arm in the workspace into the corresponding joint angular velocity. e When , the speed of each joint of the robot arm can be obtained by calculation

[0100] In the fixed base control mode, the robot end speed vω e =[v e ,ω e ] T Angular velocity of the robot joint The following relationship exists:

[0101]

[0102] Where J is the Jacobian matrix of the robot. e ,ω e are the terminal linear velocity and terminal angular velocity respectively.

[0103] The above formula can be used to obtain the inverse kinematic solution of the robot arm velocity level in fixed base mode:

[0104]

[0105] Among them, J + is the generalized inverse of the Jacobian matrix.

[0106] J + =J T ·(J.J T ) -1 .

[0107] 3) Construct a displacement curve with continuously changing speed

[0108] Define the current time as t and the current displacement of the robot as f d .

[0109] From the initial point P0 coordinates P 0，ξ and the end point P f Coordinate P f，ξ The calculated straight line distance between the first and the last end is:

[0110]

[0111] Plan its running path according to the trapezoidal method (speed continuous), and set the starting and ending times to 0 and t f , the acceleration time is t s The transition point between the acceleration and deceleration curves is t s ,t f -t s , the maximum speed is v m , the displacement corresponding to the end point is d f .

[0112] Since the curve is symmetrical, the acceleration and deceleration time are the same. Figure 3 shown.

[0113] d f = dist,

[0114] Assume t a =t f -t s ,

[0115] v m =d f / t a ,

[0116] Based on the above known variables, other variables and curve functions are calculated. Figure 4 The functional expression of each segment of the displacement curve is:

[0117]

[0118] 4) Collision detection calculation

[0119] Considering the computing resource limitations of the onboard computer and the actual mission requirements, the space station manipulator collision detection strategy is as follows: Figure 5 As shown in the figure, the robot collision detection algorithm includes two algorithms: ground algorithm and on-orbit algorithm. On-orbit dynamic collision detection using simplified geometric models is used to simplify the space station cabin and the robot body into multiple outer envelope cylindrical geometric models. The distance between each body is calculated according to the actual angle of the joint to achieve collision detection. The collision mark, collision parts and distance between parts can be output. On the ground, the scatter point scanning static space interference method is used to establish an accurate outer envelope model of the space station cabin and the robot body through the scatter point scanning method. The distance between all discrete points is calculated according to the actual angle of the joint to achieve collision detection. The collision mark, specific collision points and distance between each point on the part can be accurately output. Among them, the calculation results of the on-orbit algorithm are relatively conservative, with a small amount of calculation. It is mainly used for on-orbit monitoring purposes to prevent the risk of collision caused by abnormal movement of the robot arm. The calculation results of the ground algorithm are accurate, but the calculation amount is large. It is mainly used to verify the planned path on the ground and can predict the minimum distance of the robot arm movement process. At the same time, during the actual movement of the robot arm, collision detection can be indirectly achieved through the monitoring images of the camera on the space station and the measurement values ​​of the robot arm joint and end torque sensor.

[0120] (2) Planning algorithm module

[0121] The planning algorithm module includes pre-programmed motion modes, single-joint position motion modes, multi-joint linkage motion modes, terminal linear motion modes, terminal handle motion modes, and visual servo motion modes. The planning algorithm module receives the data parameters output by the instruction processing module, calls the corresponding sub-mode program according to the selected mode, and outputs the planned joint expected angles and angular velocities.

[0122] 1) Pre-programmed sports modes

[0123] The pre-programmed motion mode does not require on-track planning. The central controller CPU pre-stores the expected angle and expected angular velocity information of all joints. After the central controller CPU receives the pre-programmed control instructions issued by the operating platform, the CPU reads the pre-stored data at the corresponding address and sends it to each joint controller according to the control cycle output to control the output trajectory of each joint.

[0124] In the pre-programmed motion mode, the pre-stored expected joint angle and angular velocity need to be sent once within one control cycle.

[0125] 2) Single joint position movement mode

[0126] In single joint position mode, after selecting a single joint, the central controller FPGA will plan the position of the single joint, and output the joint planning angle and angular velocity instructions to the basic calculation module first, and then send them to the joint controller after verification of no collision until they are in place. In order to meet the actual joint motion characteristics, the following are adopted: Figure 3 The trapezoidal joint velocity plan shown is first accelerated, then constant speed and finally decelerated.

[0127] Define the current time t, control period t0, initial joint angle θ int , planning the current joint angle θ now , target joint angle θ des , the expected angular velocity ω d , expected angular acceleration a, joint angular velocity command sequence ω i .but

[0128] Joint rotation distance:

[0129] dist=|θ des -θ int |

[0130] Acceleration time:

[0131] t s =ω d / a

[0132] Total planning time for a single joint:

[0133] t f =dist / ω d +t s

[0134] Joint rotation deviation:

[0135] θ d =θ des -θ int

[0136] Then call the "Construct displacement curve with continuously changing speed" module to obtain the joint angle command sequence.

[0137] 3) Multi-joint linkage motion mode

[0138] The multi-joint linkage planning is that after the target joint angle is selected, the central controller FPGA performs position planning in the joint space, and first outputs the angular velocity command sequence to the basic calculation module, and then sends it to the joint controller after verification of no collision until it runs in place. The difference from the single-joint position motion mode is that the 7 joints plan the motion at the same time. The planning principle of each joint in the multi-joint linkage mode is the same as the single-joint position planning. In order to ensure the synchronous motion of the 7 joints, the required rotation angles of the 7 joints are first calculated to obtain the maximum joint rotation deviation, and the motion time is calculated using the maximum joint angle deviation. The maximum motion speed of the other 6 joints is recalculated based on the motion time, thereby ensuring the synchronization of the 7 joints.

[0139] 4) Terminal linear motion mode

[0140] The linear planning of the space manipulator is to make the manipulator move along a straight line from the initial end position to the specified end position. Assume that the initial end position of the manipulator Pe0 = [P e0 , ψ e0 ] and the final posture Pef=[P ef , ψ ef ], then the end trajectory of the straight line planning of the end of the robot arm is as follows Figure 6 shown.

[0141] Terminal line speed planning:

[0142] First, the length d of the straight line distance between the start and end points is calculated from the coordinates of the initial point and the end point. f .

[0143] Assume that the running path is planned according to the trapezoidal method with parabola transition arc, and set the starting and ending times to 0 and t z , acceleration is a, then let t s =t z / (1+d f / a), the transition point between the acceleration and deceleration curves is at time t a / 3, 2t a / 3, t z -2t a / 3 and t z -t a / 3, at the transition point t a / 3(or t z -t a / 3)、2t a / 3(or t z -2t a / 3) are v1 and v2, and the maximum speed is v m The length of the straight line from the beginning to the end is d f , t a is the acceleration time, and its running trajectory is as follows Figure 7shown.

[0144] For general planning, d f is a known quantity. We can assume that t z ,t a and v m If any two variables in are known, find the other variables and curve functions. For ease of calculation, let t z and t a is known, since the speed curve is symmetrical, so t s =t z -2t a According to the definition, the functions of curves 1 to 6 are: f1(t) = a1t 2 +b1t, f2(t)=a2t+b2, f3(t)=a3t 2 +b3t+c3,f4(t)=a4t 2 +b4t+c4,f5(t)=a5t+b5,f6(t)=a6t 2 +b6t+c6. In order to ensure the continuity of speed and acceleration, the speed values ​​of each curve at the transition point are equal, and the slope of the curve, that is, the acceleration value, is also equal. a ,t a +t s and t z At this moment, the slope, i.e. the acceleration, is zero. Under the above conditions, we first calculate the acceleration curves 1 to 3 and write a set of equations. According to the following set of equations, we can solve the function coefficients of the acceleration curves 1 to 3, the transition point speeds v1, v2 and the maximum speed v m , and the coefficients of each curve segment. a1~a6, b1~b6 are all coefficients.

[0145]

[0146] Angular velocity planning:

[0147] First, the attitude pointing deviation of the head and the end is calculated and converted into the axis-angle relationship represented by the quaternion. Then, the trapezoidal method with parabolic transition arc is also used to plan the angular displacement of each axis of the end of the robot arm, and then the planned angular velocity of the end in each control cycle can be obtained.

[0148] From this, the linear velocity and angular velocity of the planned operation of the end of the robot arm can be obtained.

[0149] 5) End handle motion mode

[0150] According to the mission requirements, the astronauts use the handle to give the terminal moving speed in the desired direction in the current coordinate system. After the coordinate system changes, the terminal moving speed in the base coordinate system of the space station is obtained, and then the inverse kinematics calculation is used to obtain the angular velocity of each joint; until the handle input speed command is zero, the movement ends.

[0151] Assume that the terminal velocity is v e , the terminal angular velocity is ω e , after inverse kinematics calculation, the joint angular velocity can be obtained:

[0152]

[0153] Among them, J + is the generalized inverse of the Jacobian matrix.

[0154] 6) Visual servo motion mode

[0155] The visual servo mode is a planning and control mode in which the robot arm autonomously moves from the starting point to the target point under the guidance of visual posture measurement. This planning mode is mainly used to capture moving targets in a space environment. The camera gives the target's posture information in real time and provides this information to the central controller. The central controller FPGA calls the visual servo algorithm to make the robot arm autonomously move to the target point, thereby realizing the tracking and capture of moving targets in space. The principle of the visual servo algorithm is as follows Figure 8 shown.

[0156] Let the base coordinate system be I( I O- I X I y I Z), the end coordinate system of the robot arm is T( T O- T X T Y T Z), the target point coordinate system is W( W O- W X W Y W Z).

[0157] (a) Calculate the position coordinate difference D of the target point coordinate system W relative to the terminal coordinate system T in the terminal coordinate system T oe And calculate the distance d between the end and the target point v .

[0158] (b) Calculate the next displacement S end =vel*dt*D oe / d v , where vel is the set end-of-arm motion speed, dt is the motion control cycle, and d v is the distance between the end point and the target point.

[0159] (c) According to the end displacement S end , calculate the two sets of joint angle values ​​θ corresponding to the start and end points by the inverse kinematic solution now ,θ next , where θ now is the current joint angle value, θ next The joint angle value at the beginning of the next cycle.

[0160] (d) Determine the distance d between the end and the target point at this time v Whether it is within the given error range, and whether the difference ΔEul between the terminal attitude Euler angle and the target attitude is within the given error range, if so, the tracking ends; otherwise, return to step (a) to continue tracking until the conditions are met.

[0161] Parts of the present invention that are not described in detail belong to the well-known technologies of those skilled in the art.

Claims

1. A multi-mode path planning system for a space station manipulator adapted to complex tasks, characterized in that: include: Instruction processing module, planning algorithm module and basic calculation module; Instruction processing module: receives control instructions input from the console, parses instruction format and data information, and outputs the parsed data information to the planning algorithm module as calculation input; the data information includes pre-programmed instruction sequence number, target point expected posture, joint expected angle, joint speed / acceleration setting, terminal speed / acceleration setting, visual servoing target point posture; Planning algorithm module: realizes the calculation of planning algorithms for multiple motion modes; receives the data parameters output by the instruction processing module, selects the corresponding motion mode, and outputs the expected joint angle and expected angular velocity sequence data to the basic calculation module; the motion modes include pre-programmed motion mode, single joint position motion mode, multi-joint linkage motion mode, terminal linear motion mode, terminal handle motion mode, and visual servo motion mode; Basic calculation module: realizes basic calculation functions, realizes the conversion between end posture / speed and joint angle / speed, and outputs the expected joint angle and expected angular velocity sequence data for collision-free detection to the joint controller; the basic calculation includes forward kinematics calculation, inverse kinematics calculation, construction of speed continuous change displacement curve, and collision detection calculation; The single joint position motion mode includes: After selecting a single joint, the position of the single joint is planned, and the joint planning angle and angular velocity instructions are output to the basic calculation module. After verification that there is no collision, they are sent to the joint controller until they are in place; The position planning of the single joint adopts a trapezoidal joint speed planning of first accelerating, then uniform speed and finally decelerating to obtain the desired joint angular velocity; according to the method of constructing a displacement curve with continuously changing speed in the basic algorithm module, a joint angle command sequence is calculated until it is in place and the planning is completed; The multi-joint linkage motion mode includes: After selecting the target angle of each joint, the position planning of the joint space is carried out, and the angular velocity and angle command sequence are output to the basic calculation module. After verification that there is no collision, it is sent to the joint controller until it runs in place; each joint plans the movement at the same time, first calculates the required rotation angle of each joint, obtains the maximum joint rotation deviation, calculates the movement time using the maximum joint angle deviation and the maximum set speed, and recalculates the maximum movement speed of the remaining joints based on the movement time to ensure the synchronization of the movement of each joint; The visual servo motion mode includes: The target posture information is measured in real time by the camera on the wrist of the robot arm, and the target posture information is provided to the planning algorithm module. The robot arm is moved to the target point autonomously through the visual servo algorithm to achieve tracking of the moving target in space. The visual servo algorithm comprises: (a) Calculate the position coordinate difference D of the target point coordinate system W relative to the terminal coordinate system T in the terminal coordinate system T oe And calculate the distance d between the end and the target point v ; (b) Calculate the next displacement S end =vel*dt*D oe / d v , where vel is the set end-of-arm motion speed, dt is the motion control cycle, and d v is the distance between the end point and the target point; (c) According to the end displacement S end , calculate the two sets of joint angle values ​​θ corresponding to the start and end points by the inverse kinematic solution now and θ next , where θ now is the current joint angle value, θ next is the joint angle value at the beginning of the next cycle; (d) Determine the distance d between the end and the target point at this time v Whether it is within the given error range, and whether the difference ΔEul between the terminal attitude Euler angle and the target attitude is within the given error range, if so, the tracking ends; otherwise, return to step (a) to continue tracking until the conditions are met.

2. A multi-mode path planning system for a space station manipulator adapted to complex tasks according to claim 1, characterized in that: The forward kinematics calculation calculates the position and posture of the end of the manipulator according to the DH parameters and joint angles of the spatial manipulator.

3. The multi-mode path planning system for a space station manipulator adapted to complex tasks according to claim 1, characterized in that: The inverse kinematics calculation calculates the speed of each joint of the robotic arm based on the DH parameters and the terminal speed of the spatial robotic arm.

4. According to claim 1, a multi-mode path planning system for a space station manipulator adapted to complex tasks is characterized in that: The displacement curve with continuously changing construction speed includes: The robot arm's running path is planned according to the trapezoidal method. According to the starting time, end time, acceleration time, maximum speed and displacement corresponding to the end point, the function expression of each segment of the displacement curve with continuously changing speed and time as the independent variable is obtained.

5. The multi-mode path planning system for a space station manipulator adapted to complex tasks according to claim 1, characterized in that: The collision detection calculation includes: a ground algorithm and an on-orbit algorithm; On-orbit algorithm: Dynamic collision detection using simplified geometric models is adopted. The space station cabin and the robot arm body are simplified into multiple outer envelope cylindrical geometric models. The distance between each body is calculated based on the actual angle of the joint to achieve collision detection. The collision mark, collision parts and distance between parts are output. Ground algorithm: The scattered point scanning static space interference method is used to establish the outer envelope model of the space station cabin and the robotic arm body through the scattered point scanning method. The distance between all discrete points is traversed and calculated according to the actual angle of the joint to realize collision detection, and the collision mark, specific collision point and the distance between each point on the component are output.

6. The multi-mode path planning system for a space station manipulator adapted to complex tasks according to claim 1, characterized in that: The pre-programmed motion mode pre-stores the expected angle and expected angular velocity information of all joints in the central controller CPU. After the central controller CPU receives the pre-programmed control instruction issued by the operating platform, the CPU reads the pre-stored data of the corresponding address and sends it to each joint controller according to the control cycle output to control the output trajectory of each joint; In the pre-programmed motion mode, the pre-stored expected joint angle and angular velocity are sent once in one control cycle.

7. The multi-mode path planning system for a space station manipulator adapted to complex tasks according to claim 1, characterized in that: The terminal linear motion mode includes: Terminal linear velocity planning: According to the straight-line distance between the initial terminal posture and the target terminal posture of the robot arm, the maximum speed and acceleration time, the terminal position and velocity at each moment are calculated; Angular velocity planning: Based on the deviation between the initial and final postures, the maximum angular velocity, and the acceleration time, the expected posture and angular velocity at each moment are calculated.

8. The multi-mode path planning system for a space station manipulator adapted to complex tasks according to claim 1, characterized in that: The end handle motion mode includes: According to the task requirements, the terminal moving speed in the desired direction in the current coordinate system is given through the handle; after the coordinate system transformation, the terminal moving speed in the space station base coordinate system is obtained, and the angular velocity of each joint movement is obtained by inverse kinematics calculation; until the handle input speed command is zero, the movement ends.

9. A space station manipulator multi-mode path planning method using the space station manipulator multi-mode path planning system adapted to complex tasks as claimed in any one of claims 1 to 8, characterized in that: include: Receive control instructions input from the console and parse the instruction format and data information; the data information includes pre-programmed instruction serial number, target point desired posture, joint desired angle, joint speed / acceleration setting, terminal speed / acceleration setting, and visual servo target point posture; According to the analyzed data information, a corresponding motion mode is selected to output the expected joint angle and expected angular velocity sequence data; the motion modes include pre-programmed motion mode, single joint position motion mode, multi-joint linkage motion mode, terminal linear motion mode, terminal handle motion mode, and visual servo motion mode; The expected joint angle and expected angular velocity sequence data detected without collision are output to the joint controller to control the joint movement.

Images