CN115284293A - Space station mechanical arm multi-mode path planning system and method suitable for complex tasks - Google Patents

Abstract

The invention discloses a multi-mode path planning system and a method for a mechanical arm of a space station, which are suitable for complex tasks, wherein the system comprises the following steps: the system comprises an instruction processing module, a planning algorithm module and a basic computing module; the instruction processing module receives a control instruction input by the console, analyzes the instruction format and the data information, and outputs the analyzed data information to the planning algorithm module as calculation input; the planning algorithm module realizes the calculation of a planning algorithm of a plurality of motion modes; receiving data parameters output by the instruction processing module, selecting a corresponding motion mode, and outputting expected joint angle and expected angular velocity sequence data to the basic calculation module; and the basic calculation module realizes a basic calculation function, realizes conversion between the terminal pose/speed and the joint angle/speed, and outputs the detected collision-free expected joint angle and expected angular speed sequence data to the joint controller. The invention solves the problem of safe and rapid path planning under the constraint of complex conditions.

Description

Multi-mode path planning system and method for space station mechanical arm suitable for complex tasks

Technical Field

The invention belongs to the technical field of robots, and relates to a multi-mode path planning system and method for a mechanical arm of a space station.

Technical Field

The mechanical arm is one of important key technologies of manned space station engineering in China, and the space station mechanical arm is used for numerous tasks such as space station assembly construction, maintenance, auxiliary spaceman out-of-cabin activity, space application support and the like. The space station cabin body surface equipment is more, the large solar wing has large rotating envelope, the measurement and control antenna beam range is wide, and the equipment and the antenna beam range need to be avoided in the mechanical arm movement process; the space station mechanical arm also needs to complete dynamic tracking, accurate capture and auxiliary transfer of the large hovering cabin section. In order to realize full coverage of the surface of the space station, the mechanical arm has a crawling function and can move among a plurality of foot prints arranged on the surface of the cabin body (see 201910653256.5' a foldable target adapter suitable for large-tolerance capture for details). Due to the task requirements, the space station mechanical arm has the unique characteristics of large movement range, narrow accessible space, various tasks and the like. The path planning method in a single mode cannot meet the current task requirements.

The patent and article of the related method for planning the path of the mechanical arm disclosed at present mainly focus on a single path planning theoretical method, and the invention of the publication number CN113843791A (a method for planning the multipoint motion path of the mechanical arm) provides a method for planning the continuous path of a tail end suitable for multipoint constraint, and focuses on solving the problem of continuous and smooth angles and speeds of the motion joint of the mechanical arm. The invention patent of publication No. CN113858205A (a seven-axis redundant manipulator obstacle avoidance algorithm based on improved RRT) provides a method for searching a collision-free progressive optimal path between a starting point and an expected target point by using an RRT algorithm, and the method combines a heuristic strategy with the RRT algorithm, thereby improving the planning speed to a certain extent, but essentially needs to perform collision detection and path optimization after collision in real time on line, has high requirements on the performance of a computer, and has the risk of collision at a safe distance.

Based on the above background, the current path planning method cannot adapt to the multi-task path planning requirement in the complex environment of the mechanical arm of the space station, and has a very important significance in researching the efficient safe path planning method under the constraints of severe task time constraint, narrow motion space, complex task types and links and the like.

Disclosure of Invention

The technical problem to be solved by the invention is as follows: the invention provides a multi-mode path planning system and a multi-mode path planning method for a mechanical arm of a space station, which are suitable for complex tasks and solve the problems of safe and rapid path planning under the constraints of limited computing resources, severe time constraint, narrow motion space, complex task types and links and the like.

The technical problem to be solved by the invention is as follows: a multi-mode path planning system for a mechanical arm of a space station, which is suitable for complex tasks, comprises: the system comprises an instruction processing module, a planning algorithm module and a basic computing module;

the instruction processing module: receiving a control instruction input by an operation console, analyzing the instruction format and the data information, and outputting the analyzed data information to a planning algorithm module as calculation input; the data information comprises a pre-programmed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose;

a planning algorithm module: realizing the calculation of planning algorithms of various motion modes; receiving data parameters output by the instruction processing module, selecting a corresponding motion mode, and outputting expected joint angle and expected angular velocity sequence data to the basic calculation module; the motion modes comprise a pre-programmed motion mode, a single joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode;

a basic computing module: the method comprises the steps of realizing a basic calculation function, realizing conversion between terminal pose/speed and joint angle/speed, and outputting detected collision-free expected joint angle and expected angular speed sequence data to a joint controller; the basic calculation comprises positive kinematics calculation, inverse kinematics calculation, speed continuous change displacement curve construction and collision detection calculation.

Further, the positive kinematics calculation is carried out, and the pose of the tail end of the mechanical arm is obtained through calculation according to the parameters and the joint angles of the DH of the space mechanical arm.

Further, the inverse kinematics calculation is used for calculating the speed of each joint of the mechanical arm according to the parameters of the DH and the speed of the tail end of the space mechanical arm.

Further, the building of the displacement curve with continuously changing speed comprises the following steps:

and planning the operation path of the mechanical arm according to a trapezoidal method, and obtaining a function expression of each section of a displacement curve with continuously changing speed by taking time as an independent variable according to the starting point time, the end point time, the acceleration time, the maximum speed and the displacement corresponding to the end point.

Further, the collision detection calculation includes: a ground algorithm and an on-orbit algorithm;

an on-orbit algorithm: dynamic collision detection of a simplified geometric model is adopted, the space station cabin body and the mechanical arm body are simplified into a plurality of externally-wrapped cylindrical geometric models, the distance between the bodies is calculated through traversal according to the actual angle of the joint to realize collision detection, and whether collision identification occurs or not, and the distance between collision parts and the distance between the collision parts are output;

and (3) ground algorithm: and (3) adopting a scattered point scanning static space interference method, establishing an outer envelope model for the space station cabin body and the mechanical arm body by a scattered point scanning method, traversing and calculating distances among all the scattered points according to the actual angles of the joints to realize collision detection, and outputting whether collision identification occurs, specific collision points and distances among all the points on the part.

Further, in the pre-programmed motion mode, the Central Processing Unit (CPU) pre-stores the expected angles and the expected angular velocities of all joints, and after receiving a pre-programmed control instruction sent by the operating platform, the CPU reads the pre-stored data of the corresponding address and outputs the pre-stored data to each joint controller according to a control cycle to control the output track of each joint;

in a preprogrammed movement mode, a pre-stored joint desired angle and angular velocity are issued once within a control period.

Further, the single joint position mode includes:

after a single joint is selected, the position planning of the single joint is carried out, the joint planning angle and the angular speed instruction are output to a basic computing module, and are issued to a joint controller after no collision is verified until the joint controller runs in place;

the position planning of the single joint adopts a trapezoidal joint speed planning of firstly accelerating, then uniformly and secondly decelerating to obtain the expected joint angular speed; and calculating to obtain a joint angle command sequence according to a displacement curve method with continuously changing construction speed in the basic algorithm module until the operation is in place, and finishing the planning.

Further, the multi-joint linkage movement mode comprises:

after target angles of all joints are selected, position planning of joint spaces is carried out, angular speed and angle command sequences are output to a basic calculation module, and the command sequences are issued to joint controllers after no collision is verified until the joint controllers run in place; planning movement of each joint simultaneously, firstly calculating a required rotation angle of each joint to obtain the maximum joint rotation deviation, calculating movement time by using the maximum joint angle deviation and the maximum set speed, and recalculating the maximum movement speed of the rest joints according to the movement time to ensure the movement synchronization of each joint.

Further, the end linear motion mode includes:

planning the end linear velocity: calculating to obtain the tail end position and speed of each moment according to the linear distance length, the maximum speed and the acceleration time between the initial tail end pose and the target tail end pose of the mechanical arm;

and (3) planning the angular speed: and calculating to obtain the expected attitude and the angular speed at each moment according to the deviation between the initial attitude and the termination attitude, the maximum angular speed and the acceleration time.

Further, the end handle movement pattern includes:

according to task requirements, the terminal moving speed in the expected direction under the current coordinate system is given through a handle; obtaining the terminal moving speed under a space station base coordinate system through coordinate system transformation, and obtaining the motion angular speed of each joint by utilizing inverse kinematics calculation; and ending the movement until the handle input speed command is zero.

Further, the visual servo motion pattern comprises:

target pose information measured by a mechanical arm wrist camera in real time is provided for a planning algorithm module, and a mechanical arm automatically moves to a target point through a visual servo algorithm to realize the tracking of a space moving target object;

the visual servoing algorithm comprises:

(a) Calculating the position of the target point coordinate system W relative to the terminal coordinate system T in the terminal coordinate system TAttitude coordinate difference D _oe And calculating the distance d between the end and the target point _v ；

(b) Calculating the next displacement S _end ＝vel*dt*D _oe /d _v Wherein vel is the set motion speed of the tail end of the mechanical arm, dt is the motion control period, and d _v Is the distance between the end and the target point;

(c) According to the amount of displacement S of the end _end Calculating two sets of joint angle values theta corresponding to the starting end and the tail end by inverse kinematics solution _now And theta _next Wherein theta _now Is the current joint angle value, theta _next The joint angle value at the beginning of the next period;

(d) Determining the distance d between the end and the target point _v Whether the terminal attitude Euler angle and the target object attitude difference Delta Eul are within a given error range or not, and if so, ending the tracking; otherwise, returning to the step (a) to continue tracking until the condition is met.

The multi-mode path planning method for the space station mechanical arm by using the multi-mode path planning system for the space station mechanical arm adaptive to the complex task comprises the following steps:

receiving a control instruction input by an operation console, and analyzing an instruction format and data information; the data information comprises a preprogrammed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose;

selecting a corresponding motion mode according to the analyzed data information, and outputting expected joint angle and expected angular velocity sequence data; the motion modes comprise a pre-programmed motion mode, a single-joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode;

and outputting the expected joint angle and the expected angular speed sequence data without collision to a joint controller to control the joint motion.

Compared with the prior art, the invention has the advantages that:

the invention adopts various mechanical arm motion path planning modes, selects a proper path planning mode according to the characteristics of each link of a task, and can solve the problems of safe and rapid path planning under the constraints of limited computing resources of the mechanical arm of a space station, severe time constraint, narrow motion space, complex task type and link and the like by combining and using various modes.

Drawings

FIG. 1 is a block diagram of a robot motion path planning system;

FIG. 2 is a 7-DOF robot arm coordinate system definition diagram;

FIG. 3 is a graph of speed variation;

FIG. 4 is a graph of linear interpolation displacement;

FIG. 5 is a diagram of a collision detection strategy in conjunction with a task;

FIG. 6 is a diagram of the trajectory of the end of the robot arm;

FIG. 7 is a graph of tip velocity with a parabolic transition;

FIG. 8 is a schematic diagram of a visual servo pattern.

Detailed Description

The invention is described with reference to the accompanying drawings.

The invention discloses a multi-mode path planning system and a multi-mode path planning method for a mechanical arm of a space station, which are suitable for complex tasks, and solve the problem of safe and rapid path planning under the condition constraints of task types, complex links and the like by combining multiple path planning modes. The on-orbit simplified collision detection algorithm and the ground fine collision detection algorithm are combined for use, and the problem of reliable path planning with limited capacity and extremely high safety requirement of a space station mechanical arm satellite-borne computer is solved.

As shown in fig. 1, a space station mechanical arm multi-mode path planning system adapted to complex tasks includes: the system comprises an instruction processing module, a planning algorithm module and a basic computing module;

the instruction processing module: receiving a control instruction input by an operation console, analyzing the instruction format and the data information, and outputting the analyzed data information to a planning algorithm module as calculation input; the data information comprises a pre-programmed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose;

a planning algorithm module: realizing the calculation of a plurality of motion mode planning algorithms; receiving data parameters output by the instruction processing module, selecting a corresponding motion mode, and outputting expected joint angles and expected angular velocity sequence data to the basic calculation module; the motion modes comprise a pre-programmed motion mode, a single-joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode;

a basic computing module: the method comprises the steps of realizing a basic calculation function, realizing conversion between a terminal pose/speed and a joint angle/speed, and outputting detected collision-free expected joint angle and expected angular speed sequence data to a joint controller; the basic calculation comprises positive kinematics calculation, inverse kinematics calculation, speed continuous change displacement curve construction and collision detection calculation.

And calculating the positive kinematics to obtain the pose of the tail end of the mechanical arm according to the DH parameters and the joint angles of the space mechanical arm.

And the inverse kinematics calculation is used for calculating the speed of each joint of the mechanical arm according to the parameters of the space mechanical arm DH and the tail end speed.

The displacement curve with continuously changing construction speed comprises the following steps:

and planning the operation path of the mechanical arm according to a trapezoidal method, and obtaining a function expression of each section of a displacement curve with continuously changing speed by taking time as an independent variable according to the starting point time, the end point time, the acceleration time, the maximum speed and the displacement corresponding to the end point.

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

an on-orbit algorithm: dynamic collision detection of a simplified geometric model is adopted, the space station cabin body and the mechanical arm body are simplified into a plurality of externally-wrapped cylindrical geometric models, the distance between the bodies is calculated through traversal according to the actual angle of the joint to realize collision detection, and whether collision identification occurs or not, and the distance between collision parts and the distance between the collision parts are output;

and (3) ground algorithm: and (3) adopting a scattered point scanning static space interference method, establishing an outer envelope model for the space station cabin body and the mechanical arm body by a scattered point scanning method, traversing and calculating distances among all the scattered points according to the actual angles of the joints to realize collision detection, and outputting whether collision identification occurs, specific collision points and distances among all the points on the part.

In the pre-programmed motion mode, the CPU of the central controller prestores the information of expected angles and expected angular velocities of all joints, and after the CPU of the central controller receives a pre-programmed control instruction sent by the operating platform, the CPU reads the prestored data of corresponding addresses and outputs the prestored data to each joint controller according to a control cycle to control the output track of each joint;

in a preprogrammed movement mode, a pre-stored joint desired angle and angular velocity are issued once within a control period.

The single joint position mode comprising:

after a single joint is selected, the position planning of the single joint is carried out, the joint planning angle and the angular speed instruction are output to a basic computing module, and are issued to a joint controller after no collision is verified until the joint controller runs in place;

the position planning of the single joint adopts a trapezoidal joint speed planning of firstly accelerating, then uniformly and secondly decelerating to obtain the expected joint angular speed; and calculating to obtain a joint angle command sequence according to a displacement curve method with continuously changing construction speed in the basic algorithm module until the operation is in place, and finishing the planning.

The articulated linkage motion pattern comprising:

after target angles of all joints are selected, position planning of joint space is carried out, angular speed and an angle command sequence are output to a basic calculation module, and are issued to a joint controller after no collision is verified until the joint controller runs in place; planning movement of each joint simultaneously, firstly calculating a required rotation angle of each joint to obtain the maximum joint rotation deviation, calculating movement time by using the maximum joint angle deviation and the maximum set speed, and recalculating the maximum movement speed of the rest joints according to the movement time to ensure the movement synchronization of each joint.

The tip linear motion mode comprising:

planning the end linear velocity: calculating to obtain the tail end position and speed of each moment according to the linear distance, the maximum speed and the acceleration time between the initial tail end pose and the target tail end pose of the mechanical arm;

and (3) planning the angular speed: and calculating to obtain the expected attitude and the angular speed at each moment according to the deviation between the initial attitude and the termination attitude, the maximum angular speed and the acceleration time.

The end handle motion profile comprising:

according to task requirements, the terminal moving speed in the expected direction under the current coordinate system is given through a handle; obtaining the moving speed of the tail end under a space station base coordinate system through the change of the coordinate system, and obtaining the motion angular speed of each joint by utilizing inverse kinematics calculation; and ending the movement until the speed command input by the handle is zero.

The visual servomotion pattern comprising:

the pose information of the target is provided to a planning algorithm module through the target pose information measured by a wrist camera of the mechanical arm in real time, and the mechanical arm automatically moves to a target point through a visual servo algorithm to realize the tracking of a space moving target object;

the visual servoing algorithm comprises:

(a) Calculating the pose coordinate difference D of the target point coordinate system W relative to the tail end coordinate system T under the tail end coordinate system T _oe And calculating the distance d between the end and the target point _v ；

(b) Calculating the next displacement S _end ＝vel*dt*D _oe /d _v Wherein vel is the set motion speed of the tail end of the mechanical arm, dt is the motion control period, and d _v Is the distance between the end and the target point;

(c) According to the amount of terminal displacement S _end Calculating two groups of joint angle values theta corresponding to the starting end and the tail end by inverse kinematics solution _now And theta _next Wherein θ _now Is the current value of the joint angle, theta _next The joint angle value at the beginning of the next period;

(d) Determining the distance d between the end and the target point _v Whether the terminal attitude Euler angle and the target object attitude difference Delta Eul are within a given error range or not, and if so, ending the tracking; otherwise, returning to the step (a) to continue tracking until the condition is met.

The multi-mode path planning method for the space station mechanical arm by using the multi-mode path planning system for the space station mechanical arm adaptive to the complex task comprises the following steps:

receiving a control instruction input by an operation console, and analyzing an instruction format and data information; the data information comprises a preprogrammed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose;

according to the analyzed data information, selecting a corresponding motion mode, and outputting expected joint angles and expected angular velocity sequence data; the motion modes comprise a pre-programmed motion mode, a single-joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode;

and outputting the expected joint angle and the expected angular speed sequence data without collision to a joint controller to control the joint motion.

Example (b):

the invention aims to realize a multi-mode path planning method for a mechanical arm of a space station, which is suitable for multiple tasks. The mechanical arm motion path planning hardware carrier is a central controller, the central controller is realized by a CPU and an FPGA (field programmable gate array), the CPU realizes the functions of instruction processing, mode switching, information management and the like, the FPGA mainly realizes path planning algorithm calculation, and the main modules of the space station mechanical arm multi-mode path planning system suitable for complex tasks are shown in figure 1 and comprise an instruction processing module, a planning algorithm module and a basic calculation module, wherein the instruction processing module receives a control instruction input by an operation console and analyzes an instruction format and data information, the data information comprises a preprogrammed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose, and the analyzed data information is output to the planning algorithm module to be used as calculation input; the planning algorithm module is used for realizing the calculation of a plurality of motion mode planning algorithms such as preprogramming, single joint position, multi-joint linkage, tail end straight line, tail end handle, visual servo and the like, and outputting expected joint angle and expected angular velocity sequence data to the basic calculation module; and the basic calculation module realizes basic calculation functions of forward kinematics calculation, inverse kinematics calculation, speed continuous change displacement curve construction, collision detection calculation and the like, and finally outputs the expected joint angle and the expected angular velocity sequence data without collision to the joint controller.

(1) Basic computing module

The basic calculation module mainly comprises functions of mechanical arm forward kinematics calculation, inverse kinematics calculation, collision detection calculation, speed continuous change displacement curve construction and the like, and conversion between the terminal pose/speed and the joint angle/speed is realized. The specific conversion method among attitude representation methods such as an attitude rotation matrix, an Euler angle, an attitude quaternion and the like is detailed in teaching materials such as robot dynamics and control and the like.

1) Positive kinematics calculation

The positive kinematics calculation of the mechanical arm is mapping from a mechanical arm joint space to a tail end operation space, and when the mechanical arm DH parameters and joint angles in the space are known, the pose of the tail end of the mechanical arm can be obtained through calculation. Defining a 7-degree-of-freedom tandem robot arm coordinate system as shown in FIG. 2, including a root mount coordinate system F ₀ And each joint coordinate system F ₁ ～F ₇ 。

Position and pose descriptors between the robot arm coordinate systems are defined as follows, where i =0,1,2, ·,7, j =0,1,2, ·,7:

C ^i，j represents a coordinate system F _j Relative to a coordinate system F _i The rotation matrix of (a);

is shown in coordinate system F ₀ Coordinate system F under reference _j Relative to a coordinate system F _i Is determined by the position vector of (a),

the end position p of the seven-degree-of-freedom mechanical arm under a given joint angle can be obtained ^0，7 And attitude rotation matrix C ^0，7 。

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

C ^0，7 ＝C ^0，1 C ^1，2 C ^2，3 C ^3，4 C ^4，5 C ^5，6 C ^6，7 。

2) Inverse kinematics calculation

The inverse kinematics solution of the mechanical arm is calculated by mapping the speed of the tail end of the mechanical arm in the working space into corresponding joint angular velocity, and then calculating the DH parameter and the tail end speed v omega of the mechanical arm in the known space _e In time, the velocity of each joint of the mechanical arm can be obtained by calculation

In the fixed base control mode, the velocity v omega of the tail end of the mechanical arm _e ＝[v _e ，ω _e ] ^T Angular velocity of joint with mechanical arm

The following relationships exist:

where J is the Jacobian matrix of the mechanical arm. v. of _e ，ω _e Respectively, end linear velocity and end angular velocity.

The inverse kinematics of the velocity stage of the robot in the fixed base mode can be obtained from the above equation:

wherein, J ⁺ A generalized inverse of the jacobian matrix.

J ⁺ ＝J ^T ·(J·J ^T ) ^-1 。

3) Constructing displacement curve with continuously changing speed

Defining the current time as t and the current displacement f of the mechanical arm _d 。

From an initial point P ₀ Coordinate P _0，ξ And a termination point P _f Coordinate P _f，ξ Calculating the length of the straight line distance between the head end and the tail end as follows:

planning its running path according to trapezoidal method (speed continuous), setting starting point and end point time as 0 and t _f Acceleration time of t _s The transition point time of the curves of the acceleration section and the deceleration section is t _s 、t _f -t _s Maximum velocity v _m The displacement corresponding to the end point is d _f 。

Because the curve has symmetry, the acceleration and deceleration time is the same. The trajectory is shown in fig. 3.

d _f ＝dist，

Let t _a ＝t _f -t _s ，

v _m ＝d _f /t _a ，

And calculating other variables and curve functions according to the known variables. FIG. 4 is a graph of displacement curves having the functional expression

4) Collision detection calculation

Considering the calculation resource limit of the spaceborne computer and the actual requirements of tasks, the space station mechanical arm collision detection strategy is shown in figure 5, the mechanical arm collision detection algorithm comprises a ground algorithm and an on-orbit algorithm, dynamic collision detection of a simplified geometric model is adopted on the orbit, a space station cabin body and a mechanical arm body are simplified into a plurality of externally-wrapped cylindrical geometric models, the distance between bodies is calculated according to the actual angle of a joint in a traversing manner to realize collision detection, and whether collision identification, collision components and the distance between the components occur or not can be output; a scattered point scanning static space interference method is adopted on the ground, an accurate outer envelope model is built for the space station cabin body and the mechanical arm body through the scattered point scanning method, the distances among all the scattered points are calculated according to the actual angle traversal of the joint to realize collision detection, and whether collision identification occurs or not, the specific collision points and the distances among the points on the part can be accurately output. The on-orbit algorithm has the advantages that the calculation result is relatively conservative, the calculation amount is small, the on-orbit monitoring purpose is taken as the main purpose, the collision risk caused by the abnormal motion of the mechanical arm is prevented, the calculation result of the ground algorithm is accurate, but the calculation amount is large, the on-orbit algorithm is mainly used for verifying the ground planned path, and the minimum distance in the motion process of the mechanical arm can be predicted. Meanwhile, in the actual motion process of the mechanical arm, collision detection can be indirectly realized through monitoring images of a camera on the space station and measurement values of a mechanical arm joint and a tail end torque sensor.

(2) Planning algorithm module

The planning algorithm module comprises a pre-programmed motion mode, a single-joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode, receives data parameters output by the instruction processing module, calls a corresponding sub-mode program according to the selection mode and outputs a planned joint expected angle and an angular speed.

1) Preprogrammed pattern of motion

The pre-programmed motion mode does not need on-track planning, the CPU pre-stores the information of the expected angles and the expected angular velocities of all joints, and after the CPU receives the pre-programmed control instruction sent by the operating platform, the CPU reads the pre-stored data of the corresponding address and outputs and sends the pre-stored data to each joint controller according to the control period to control the output track of each joint.

In the preprogrammed movement mode, the pre-stored joint desired angle and angular velocity are required to be issued once in a control period.

2) Single joint position movement pattern

In the single joint position mode, after a single joint is selected, the central controller FPGA carries out position planning on the single joint, and outputs joint planning angle and angular speed instructions to the basic computing module firstly, and the joint planning angle and angular speed instructions are issued to the joint controller after no collision is verified until the joint controller runs in place. To facilitate compliance with actual joint motion characteristics, a trapezoidal joint velocity plan of first acceleration, then constant velocity and then deceleration is used as shown in fig. 3.

Defining the current time t and the control period t ₀ Initial joint angle θ _int Planning the current joint angle theta _now Target joint angle θ _des Desired angular velocity ω _d Desired angular acceleration a, joint angular velocity command sequence ω _i . Then

Joint rotation distance:

dist＝|θ _des -θ _int |

acceleration time:

t _s ＝ω _d /a

total time for planning single joint:

t _f ＝dist/ω _d +t _s

joint rotation deviation:

θ _d ＝θ _des -θ _int

and then calling a 'displacement curve with continuously changed construction speed' module to obtain a joint angle command sequence.

3) Multi-joint linkage motion mode

And in the multi-joint linkage planning, after a target joint angle is selected, the central controller FPGA carries out position planning of a joint space, an angular speed command sequence is firstly output to the basic computing module, and is issued to the joint controller after no collision is verified until the joint controller runs in place. The method is characterized in that 7 joints plan motion simultaneously in a single joint position motion mode, the planning principle of each joint in a multi-joint linkage mode is the same as that of the single joint position, in order to guarantee synchronous motion of the 7 joints, the required rotation angles of the 7 joints are firstly calculated to obtain the maximum joint rotation deviation, the motion time is calculated by utilizing the maximum joint angle deviation, and the maximum motion speeds of other 6 joints are recalculated according to the motion time, so that motion synchronization of the 7 joints can be guaranteed.

4) End linear motion mode

The linear planning of the space manipulator is to make the manipulator move from an initial end pose to a specified end pose along a straight line. Let the initial terminal pose Pe0= [ P ] of the mechanical arm _e0 ，ψ _e0 ]And end pose Pef = [ P = _ef ，ψ _ef ]Then the end trajectory of the linear planning of the end of the mechanical arm is shown in fig. 6.

Planning the end linear velocity:

firstly, calculating the linear distance length d between the head and the tail end according to the coordinates of the initial point and the final point _f 。

Assuming that the running path is planned according to a trapezoid method with parabola transition arcs, the starting point and the end point are set as 0 and t _z When the acceleration is a, let t _s ＝t _z /(1+d _f A), the transition point of the curve of the acceleration section and the deceleration section is t _a /3、2t _a /3、t _z -2t _a [ 3 ] and t _z -t _a At the transition point time t _a /3 (or t) _z -t _a /3)、2t _a /3 (or t) _z -2t _a Velocity at/3) is v ₁ And v ₂ Maximum velocity v _m The length of the straight line distance between the head and the tail is d _f ，t _a For the acceleration time, the travel path is shown in fig. 7.

For general planning, d _f In known amounts. Can be provided with t _z 、t _a And v _m Any 2 variables in (3) are known, and other variables and curve functions are found. For convenience of calculation, let t _z And t _a Since the velocity profile is symmetric, t is known _s ＝t _z -2t _a . By definition, let the functions of curves 1-6 be: f. of ₁ (t)＝a ₁ t ² +b ₁ t，f ₂ (t)＝a ₂ t+b ₂ ，f ₃ (t)＝a ₃ t ² +b ₃ t+c ₃ ，f ₄ (t)＝a ₄ t ² +b ₄ t+c ₄ ，f ₅ (t)＝a ₅ t+b ₅ ，f ₆ (t)＝a ₆ t ² +b ₆ t+c ₆ . In order to ensure the continuity of the speed and the acceleration, the slope of the curves, i.e. the acceleration value, is equal at the transition point, in addition to the speed value being equal. At the same time at 0, t _a 、t _a +t _s And t _z At that moment, the slope, i.e. the acceleration, is zero. Under the above conditions, the curves 1-3 of the acceleration section are calculated first, and an equation set can be written. The function coefficients of curves from 1 to 3 in the acceleration section and the speed v at the transition point can be solved according to the following equation set ₁ 、v ₂ And a maximum velocity v _m And the curve coefficients of each section. a is ₁ ～a ₆ ，b ₁ ～b ₆ Are all coefficients.

And (3) planning the angular speed:

firstly, the attitude pointing deviations of the head end and the tail end are calculated and are converted into axial angle relations represented by quaternions. Then, the angular displacement of each axial direction at the tail end of the mechanical arm is planned by adopting a trapezoid method with parabola transition arcs, and further the planned angular velocity of the tail end in each control period can be obtained.

The linear velocity and the angular velocity of the planned operation of the tail end of the mechanical arm can be obtained.

5) End handle motion pattern

And the astronaut gives the terminal moving speed in the expected direction under the current coordinate system through the handle according to the task requirement. Obtaining the moving speed of the tail end under a space station base coordinate system through the change of the coordinate system, and then obtaining the motion angular speed of each joint by utilizing inverse kinematics calculation; and ending the movement until the handle input speed command is zero.

Assuming terminal linear velocity v _e The terminal angular velocity is omega _e And calculating by inverse kinematics solution to obtain the joint angular velocity:

wherein, J ⁺ Is the generalized inverse of the jacobian matrix.

6) Visual servo motion pattern

The visual servo mode is planning control of the mechanical arm to autonomously reach a target point from a starting point under the guidance of visual pose measurement. The planning mode is mainly used for capturing a moving target in a space environment, the pose information of the target is given in real time through a camera and provided for a central controller, and a central controller FPGA enables a mechanical arm to move to a target point automatically by calling a visual servo algorithm, so that the space moving target is tracked and captured. The principle of the visual servoing algorithm is shown in fig. 8.

Let the base coordinate be I ( ^I O- ^I X ^I y ^I Z), the coordinate system of the end of the mechanical arm is T (Z) ^T O- ^T X ^T Y ^T Z), the coordinate system of the target point is W ( ^W O- ^W X ^W Y ^W Z)。

(a) Calculating the pose coordinate difference D of the target point coordinate system W relative to the tail end coordinate system T under the tail end coordinate system T _oe And calculating the distance d between the end and the target point _v 。

(b) Calculating the next displacement S _end ＝vel*dt*D _oe /d _v Wherein vel is the set mechanical arm end motion speed, dt is the motion control period, d _v Is the distance between the end and the target point.

(c) According to the amount of displacement S of the end _end Calculating the beginning and end from inverse kinematicsTwo corresponding sets of joint angle values theta _now ，θ _next Wherein θ _now Is the current value of the joint angle, theta _next Is the joint angle value at the beginning of the next cycle.

(d) Determining the distance d between the end and the target point _v Whether the attitude difference between the tail end attitude Euler angle and the target object is within a given error range or not, and if so, ending the tracking; otherwise, returning to the step (a) to continue tracking until the condition is met.

The present invention has not been described in detail, partly as is known to the person skilled in the art.

Claims (12)

1. A multi-mode path planning system for a mechanical arm of a space station, which is suitable for complex tasks, is characterized by comprising: the system comprises an instruction processing module, a planning algorithm module and a basic computing module;

the instruction processing module: receiving a control instruction input by an operation console, analyzing the instruction format and the data information, and outputting the analyzed data information to a planning algorithm module as calculation input; the data information comprises a preprogrammed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose;

a planning algorithm module: realizing the calculation of a plurality of motion mode planning algorithms; receiving data parameters output by the instruction processing module, selecting a corresponding motion mode, and outputting expected joint angle and expected angular velocity sequence data to the basic calculation module; the motion modes comprise a pre-programmed motion mode, a single-joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode;

a basic computing module: the method comprises the steps of realizing a basic calculation function, realizing conversion between terminal pose/speed and joint angle/speed, and outputting detected collision-free expected joint angle and expected angular speed sequence data to a joint controller; the basic calculation comprises positive kinematics calculation, inverse kinematics calculation, speed continuous change displacement curve construction and collision detection calculation.

2. The system for multi-modal path planning for space station robot arm of claim 1, wherein the pose of the end of the robot arm is calculated according to the DH parameters and the joint angles of the space robot arm in the positive kinematics calculation.

3. The system for multi-modal path planning for space station robot arm of claim 1, wherein the inverse kinematics calculation calculates the velocity of each joint of the robot arm according to the DH parameters and the tip velocity of the space robot arm.

4. The multi-mode path planning system for the space station mechanical arm adapting to the complex task is characterized in that the construction of the displacement curve with continuously changing speed comprises the following steps:

and planning the operation path of the mechanical arm according to a trapezoidal method, and obtaining a function expression of each section of a displacement curve with continuously changing speed by taking time as an independent variable according to the starting point moment, the end point moment, the acceleration time, the maximum speed and the displacement corresponding to the end point.

5. The multi-mode path planning system for the spatial station mechanical arm adapting to the complex task is characterized in that the collision detection calculation comprises the following steps: a ground algorithm and an on-orbit algorithm;

an on-orbit algorithm: dynamic collision detection of a simplified geometric model is adopted, the space station cabin body and the mechanical arm body are simplified into a plurality of externally-wrapped cylindrical geometric models, the distance between the bodies is calculated through traversal according to the actual angle of the joint to realize collision detection, and whether collision identification occurs or not, and the distance between collision parts and the distance between the collision parts are output;

and (3) ground algorithm: a scattered point scanning static space interference method is adopted, an outer envelope model is built for a space station cabin body and a mechanical arm body through a scattered point scanning method, the distance between all scattered points is calculated through traversal according to the actual angle of a joint to achieve collision detection, and whether collision identification occurs or not, the specific collision points and the distance between the points on a component are output.

6. The multi-mode path planning system for the space station mechanical arm adapting to the complex task is characterized in that the CPU pre-stores the information of the expected angles and the expected angular velocities of all joints in the CPU in the pre-programmed motion mode, and after the CPU receives the pre-programmed control instruction sent by the operating platform, the CPU reads the pre-stored data of the corresponding address and outputs the pre-stored data to each joint controller according to the control period to control the output track of each joint;

in a preprogrammed movement mode, a pre-stored joint desired angle and angular velocity are issued once within a control period.

7. The multi-modal path planning system for a space station mechanical arm adapting to complex tasks according to claim 1, wherein the single joint position mode comprises:

after a single joint is selected, the position planning of the single joint is carried out, the joint planning angle and the angular speed instruction are output to a basic computing module, and the joint planning angle and the angular speed instruction are issued to a joint controller after no collision is verified until the joint controller runs in place;

the position planning of the single joint adopts a trapezoidal joint speed planning of firstly accelerating, then uniformly and secondly decelerating to obtain the expected joint angular speed; and calculating to obtain a joint angle command sequence according to a displacement curve method with continuously changing construction speed in the basic algorithm module until the operation is in place, and finishing the planning.

8. The multi-modal path planning system for a space station mechanical arm adapting to a complex task according to claim 1, wherein the multi-joint linkage motion mode comprises:

after target angles of all joints are selected, position planning of joint spaces is carried out, angular speed and angle command sequences are output to a basic calculation module, and the command sequences are issued to joint controllers after no collision is verified until the joint controllers run in place; the joints plan movement at the same time, the required rotation angle of each joint is calculated firstly to obtain the maximum joint rotation deviation, the movement time is calculated by utilizing the maximum joint angle deviation and the maximum set speed, the maximum movement speed of the rest joints is recalculated according to the movement time, and the movement synchronization of each joint is ensured.

9. The system for multi-mode path planning of space station mechanical arm adapting to complex tasks according to claim 1, wherein the terminal linear motion mode comprises:

planning the linear velocity of the tail end: calculating to obtain the tail end position and speed of each moment according to the linear distance length, the maximum speed and the acceleration time between the initial tail end pose and the target tail end pose of the mechanical arm;

planning the angular speed: and calculating to obtain the expected attitude and the angular speed at each moment according to the deviation between the initial attitude and the termination attitude, the maximum angular speed and the acceleration time.

10. The multi-mode path planning system for the space station mechanical arm adapting to the complex task, according to claim 1, is characterized in that the motion mode of the end handle comprises:

according to task requirements, the terminal moving speed in the expected direction under the current coordinate system is given through a handle; obtaining the terminal moving speed under a space station base coordinate system through coordinate system transformation, and obtaining the motion angular speed of each joint by utilizing inverse kinematics calculation; and ending the movement until the speed command input by the handle is zero.

11. The system for multi-modal path planning for space station robotic arm capable of accommodating complex tasks according to claim 1, wherein the visual servo motion pattern comprises:

target pose information measured by a mechanical arm wrist camera in real time is provided for a planning algorithm module, and a mechanical arm automatically moves to a target point through a visual servo algorithm to realize the tracking of a space moving target object;

the visual servoing algorithm comprises:

(a) Calculating the position and orientation coordinate difference D of the target point coordinate system W relative to the terminal coordinate system T under the terminal coordinate system T _oe And calculating the distance d between the end and the target point _v ；

(b) Calculating the next displacement S _end ＝vel*dt*D _oe /d _v Wherein vel is the set tail end movement speed of the mechanical arm, dt is the movement control period, d _v Is the distance between the end and the target point;

(c) According to the amount of terminal displacement S _end Calculating two sets of joint angle values theta corresponding to the starting end and the tail end by inverse kinematics solution _now And theta _next Wherein theta _now Is the current joint angle value, theta _next The joint angle value at the beginning of the next period;

(d) Determining the distance d between the end and the target point _v Whether the terminal attitude Euler angle and the target object attitude difference Delta Eul are within a given error range or not, and if so, ending the tracking; otherwise, returning to the step (a) to continue tracking until the condition is met.

12. The multi-mode path planning method for a spatial station mechanical arm using the multi-mode path planning system for a spatial station mechanical arm adapted to a complex task according to any one of claims 1 to 11, comprising:

receiving a control instruction input by an operation console, and analyzing an instruction format and data information; the data information comprises a pre-programmed instruction sequence number, a target point expected pose, a joint expected angle, joint speed/acceleration setting, tail end speed/acceleration setting and a visual servo target point pose;

selecting a corresponding motion mode according to the analyzed data information, and outputting expected joint angle and expected angular velocity sequence data; the motion modes comprise a pre-programmed motion mode, a single-joint position motion mode, a multi-joint linkage motion mode, a tail end linear motion mode, a tail end handle motion mode and a visual servo motion mode;

and outputting the sequence data of the expected joint angle and the expected angular velocity without collision to a joint controller to control joint motion.

Images