CN111872938A - Spatial three-dimensional large-scale kinematics simulation system and method - Google Patents

Spatial three-dimensional large-scale kinematics simulation system and method Download PDF

Info

Publication number
CN111872938A
CN111872938A CN202010753564.8A CN202010753564A CN111872938A CN 111872938 A CN111872938 A CN 111872938A CN 202010753564 A CN202010753564 A CN 202010753564A CN 111872938 A CN111872938 A CN 111872938A
Authority
CN
China
Prior art keywords
aircraft
target aircraft
relative
mechanical arm
motion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202010753564.8A
Other languages
Chinese (zh)
Other versions
CN111872938B (en
Inventor
张继文
刘琛
刘宇
冯渭春
陈恳
宋立滨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
BEIJING INSTITUTE OF TRACKING AND COMMUNICATION TECHNOLOGY
Tsinghua University
Original Assignee
BEIJING INSTITUTE OF TRACKING AND COMMUNICATION TECHNOLOGY
Tsinghua University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by BEIJING INSTITUTE OF TRACKING AND COMMUNICATION TECHNOLOGY, Tsinghua University filed Critical BEIJING INSTITUTE OF TRACKING AND COMMUNICATION TECHNOLOGY
Priority to CN202010753564.8A priority Critical patent/CN111872938B/en
Publication of CN111872938A publication Critical patent/CN111872938A/en
Application granted granted Critical
Publication of CN111872938B publication Critical patent/CN111872938B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1628—Programme controls characterised by the control loop
    • B25J9/1651—Programme controls characterised by the control loop acceleration, rate control
    • G—PHYSICS
    • G09—EDUCATION; CRYPTOGRAPHY; DISPLAY; ADVERTISING; SEALS
    • G09B—EDUCATIONAL OR DEMONSTRATION APPLIANCES; APPLIANCES FOR TEACHING, OR COMMUNICATING WITH, THE BLIND, DEAF OR MUTE; MODELS; PLANETARIA; GLOBES; MAPS; DIAGRAMS
    • G09B9/00—Simulators for teaching or training purposes
    • G09B9/02—Simulators for teaching or training purposes for teaching control of vehicles or other craft
    • G09B9/08—Simulators for teaching or training purposes for teaching control of vehicles or other craft for teaching control of aircraft, e.g. Link trainer

Abstract

The invention provides a spatial three-dimensional large-scale kinematics simulation system and method. The space three-dimensional large-scale kinematics simulation system comprises an experimental site, a target aircraft simulation mechanism and a task aircraft simulation mechanism. The target aircraft simulation mechanism comprises a target aircraft and a first industrial mechanical arm, and the tail end of the first industrial mechanical arm is connected to the target aircraft. The task aircraft simulation mechanism comprises a task aircraft, a second industrial mechanical arm and an all-directional mobile platform, wherein a base of the second industrial mechanical arm is fixed on the all-directional mobile platform, the tail end of the second industrial mechanical arm is connected to the task aircraft, and the all-directional mobile platform is arranged on an experimental site and can move on the experimental site. According to the space three-dimensional large-scale kinematics simulation method, the limitation of a ground experiment environment is broken through by combining the second mechanical arm and the omnibearing mobile platform, kinematics simulation in a larger range is realized, and the flexibility of a simulation system is increased.

Description

Spatial three-dimensional large-scale kinematics simulation system and method
Technical Field
The invention relates to the technical field of three-dimensional semi-physical simulation, in particular to a spatial three-dimensional large-scale kinematics simulation system and method.
Background
With the development of space technology, the tasks that the spacecraft needs to perform become increasingly complex, and in order to complete the increasingly complex tasks, it is necessary to research the motion relation and control strategy among various aircrafts in a space environment, but when the spacecraft is directly tested in an on-orbit operation process, the cost and the risk are high. On the other hand, with the development of the artificial intelligence technology, the control decision technology of the spacecraft also enters the intelligence era, but the research of the artificial intelligence technology at present often depends on a large number of data sets, the data sets are difficult to obtain under the environment due to the particularity of the space environment, and the verification of the artificial intelligence related algorithm is difficult to directly carry out in the space environment. Under the condition, a set of reasonable ground simulation systems for simulating the relative motion relation between the spacecraft plays an important role in researching the motion and control of the spacecraft in the space environment and researching the artificial intelligence algorithm of the spacecraft.
Currently, some ground simulation systems for simulating mutual motion of multiple spacecraft exist, for example, chinese patent CN105539890A published in 2016, 5, month and 4 proposes a device and a method for simulating a space robot to capture a ground three-dimensional space microgravity of a target spacecraft. However, due to the limitation of the fixed base, the ground simulation system can only simulate the relative motion between the mission aircraft and the target aircraft in the space within a certain range on the ground, the action space is small, the simulation can only be performed on the small-range motion, and the direct simulation of the three-dimensional large-range motion in the space is difficult.
Disclosure of Invention
In view of the problems existing in the background art, the present invention aims to provide a spatial three-dimensional large-scale kinematics simulation system and method, which can realize the simulation of the relative motion between aircrafts in a large-scale three-dimensional space in a limited ground experimental space, increase the flexibility of the simulation system, reduce the complexity of the problems, and reduce the construction cost of the whole simulation system.
In order to achieve the above object, in a first aspect, the present invention provides a spatial three-dimensional large-scale kinematics simulation system, which includes an experimental site, a target aircraft simulation mechanism, and a mission aircraft simulation mechanism; the target aircraft simulation mechanism comprises a target aircraft and a first industrial mechanical arm, wherein a base of the first industrial mechanical arm is fixed on an experimental field, and the tail end of the first industrial mechanical arm is connected to the target aircraft; the task aircraft simulation mechanism comprises a task aircraft, a second industrial mechanical arm and an all-directional mobile platform, wherein a base of the second industrial mechanical arm is fixed on the all-directional mobile platform, the tail end of the second industrial mechanical arm is connected to the task aircraft, and the all-directional mobile platform is arranged on an experimental site and can move on the experimental site.
In one embodiment, the spatial three-dimensional large-scale kinematic simulation system further comprises: and the positioning system is arranged on the omnibearing mobile platform and is used for acquiring the relative pose of the omnibearing mobile platform relative to the base of the first industrial mechanical arm in real time.
In order to achieve the above object, in a second aspect, the present invention provides a method for simulating spatial three-dimensional large-scale kinematics, which performs simulation based on the spatial three-dimensional large-scale kinematics simulation system of the first aspect, and includes: establishing a fixed coordinate system F of the target aircraftT，FTThe origin point of the target aircraft is the mass center of the target aircraft, and a target aircraft orbit coordinate system F is establishedW，FWThe origin point of the target aircraft is the mass center of the target aircraft, and a task aircraft fixed coordinate system F is establishedSEstablishing a coordinate system F of the experimental siteeA first industrial robot base coordinate system FTbAnd a second industrial robot base coordinate system FSbWherein, the translation of the target aircraft along the space orbit is not considered, and only the relative motion relation between the target aircraft and the mission aircraft in the space is considered; in a coordinate system FT、FW、FSThen, based on the initial angular velocity vector value of the given target aircraft and according to the calculation of space dynamics, the fixed point motion track T of the target aircraft in the space environment is obtained2(T), wherein T2(t) is the target aircraft's trajectory of rotation about the center of mass; in a coordinate system FT、FW、FSThen, aiming at the control strategy of the mission aircraft, the mission aircraft is controlled to move relative to the target aircraft, and the motion trail T of the mission aircraft in the space environment is directly obtained according to the flight route of the mission aircraft1(T), wherein T1(t) is the fixed coordinate system F of the mission aircraftSRelative orbit coordinate system FWRelative movement track of, T1(t) is represented by a series of dense discrete points; and (3) performing motion planning on the relative motion of the target aircraft on the space in an experimental field by the mission aircraft: dividing the relative motion between the mission aircraft and the target aircraft into a first virtual motion and a second virtual motion, wherein the first virtual motion is FTAround FWThe second virtual motion is FSRelative to FWThe second virtual motion is FSRelative to FSb、FSbRelative to Fe、FeRelative to FTb、FTbRelative to FWThe continuous motion chain is virtualized to be a super-redundant mechanical arm, and a base coordinate system of the super-redundant mechanical arm is FWThe tail end of the super-redundant mechanical arm is a fixed coordinate system F of the mission aircraftS(ii) a According to the given movement locus T of the mission aircraft1(t) performing motion planning and trajectory planning based on the virtual kinematics model of the super-redundant mechanical arm, and calculating to obtain the motion trajectory of each joint of the super-redundant mechanical arm, namely the motion trajectory of each joint of the second industrial mechanical arm
Figure BDA0002610773230000031
i∈[1,NS]Wherein N isSIs the number of joints of the second industrial robot arm; movement locus of each degree of freedom of omnibearing mobile platform
Figure BDA0002610773230000032
i∈[x,y,θ](ii) a And each joint motion track of the first industrial mechanical arm
Figure BDA0002610773230000033
i∈[1,NTT]Wherein N isTTIs the number of joints of the first industrial robot arm; according to the motion track Tra of each joint of the first industrial mechanical armTTi(t) calculating to obtain the end F of the first industrial robot armWRelative base FTbIs TraT(t)＝[xT(t) yT(t) zT(t) rxT(t) ryT(t) rzT(t)]T(ii) a Attaching a first industrial robot end FWIs TraT(T) a movement locus T around the center of mass with the target aircraft2(t) fusing to obtain the end F of the first industrial robotTAt FTbDown motion trajectory, then based on FTAt FTbThe movement track of the final joint closing movement of the first industrial mechanical arm is obtained
Figure BDA0002610773230000034
i∈[1,NT]Wherein N isTIs the number of joints of the first industrial robot arm; and simulating the relative motion of the mission aircraft relative to the target aircraft according to the track planning result of each joint.
In one embodiment, a first industrial robot end F is positionedWIs TraT(T) a movement locus T around the center of mass with the target aircraft2(t) the specific steps of performing fusion include: establishing a positive kinematic equation PTm＝X(qTm) Equation of inverse kinematics qTm＝Y(PTm) Wherein P isTmShowing the first industrial robot arm end at FTbPosition attitude vector of lower, qTmRepresenting angle vectors of respective joints of the first industrial robot arm in joint space; end F of super redundant mechanical armWRelative base FTbPlanning the obtained movement track TraT(t) is represented by a homogeneous transformation matrix as
Figure BDA0002610773230000035
Moving track T of target aircraft around centroid2(t) expansion into homogeneous transformation matrices
Figure BDA0002610773230000036
According to
Figure BDA0002610773230000037
The first industrial robot end F can be calculatedTAt FTbUnderlying homogeneous transformation matrix
Figure BDA0002610773230000038
Then based on
Figure BDA0002610773230000039
Inverse kinematics equation q by first Industrial robot armTm＝Y(PTm) Solving the position of each joint, calculating the speed and the acceleration of each joint through a difference method, and finally obtaining a first industrial mechanical arm combined motion planning result
Figure BDA00026107732300000310
In one embodiment, during the final step of executing, the method for the ground simulation system to simulate the approach of the mission aircraft to the target aircraft in the space further comprises: the omnibearing mobile platform according to the measured data track
Figure BDA0002610773230000041
i∈[x,y,θ]Establishing a PID feedback control system to follow the expected motion track of each degree of freedom of the omnibearing mobile platform
Figure BDA0002610773230000042
i∈[x,y,θ]。
The invention has the following beneficial effects: by the spatial three-dimensional large-scale kinematics simulation method, the limitation of a ground experiment environment is broken through by combining the second mechanical arm and the omnibearing mobile platform, kinematics simulation in a larger range is realized, and the flexibility of a simulation system is increased; in addition, the super-redundant mechanical arm is established, the complex collaborative planning problem of a plurality of actuating mechanisms is integrated into the planning problem of the super-redundant mechanical arm, and the complexity of the problem is reduced; moreover, by defining a track coordinate system, and abstracting a virtual super-redundant mechanical arm and a fixed point motion structure, the problem of motion planning of a plurality of systems in the prior art is converted into the problem of collaborative planning of two independent virtual motion mechanisms, so that the complexity of the problem is reduced.
Drawings
FIG. 1 shows a coordinate system F of an orbitWThe following schematic diagram of a spatial three-dimensional large-scale kinematic simulation system according to the present invention.
FIG. 2 is a schematic diagram of the chain of continuous motion between the mission aircraft and the target aircraft of FIG. 1 being virtualized as a super-redundant robotic arm, wherein a virtual robotic arm is established between the omni-directional mobile platform and the coordinate system of the test site.
Wherein the reference numerals are as follows:
1 laboratory site 32 second industrial robot arm
2-target aircraft simulation mechanism 33 omnibearing mobile platform
21 target aircraft 4 positioning system
22 first industrial robot R super redundant robot
Virtual mechanical arm m of 3-task aircraft simulation mechanism
31 mission aircraft
Detailed Description
The following describes a spatial three-dimensional large-scale kinematics simulation system and method according to the present invention in detail with reference to the accompanying drawings.
Referring to fig. 1 and 2, the spatial three-dimensional large-scale kinematics simulation system according to the present invention includes an experimental site 1, a target aircraft simulation mechanism 2, a mission aircraft simulation mechanism 3, and a positioning system 4.
The target aircraft simulation mechanism 2 comprises a target aircraft 21 and a first industrial mechanical arm 22, wherein the base of the first industrial mechanical arm 22 is fixed on the experimental site 1, and the tail end of the first industrial mechanical arm 22 is connected to the target aircraft 21. The first industrial robot arm 22 adjusts the movement of the target aircraft 21 by adjusting the tip position attitude. It should be added that the first industrial robot arm 22 is mainly composed of a joint and a connecting rod, and the joint is composed of a motor, a harmonic reducer, an absolute position sensor, a joint torque sensor, a joint controller, and the like.
In one embodiment, the target aircraft 21 is connected to the first industrial robot arm 22 by a connecting flange.
In one embodiment, the first industrial robot 22 is a six-axis robot, but not limited thereto, and the first industrial robot 22 may have more or less degrees of freedom according to actual situations to better meet the use requirements.
Task aircraft simulation mechanism 3 includes task aircraft 31, second industry arm 32 and all direction movement platform 33, and on the base of second industry arm 32 was fixed in all direction movement platform 33, the end-to-end connection of second industry arm 32 in task aircraft 31, all direction movement platform 33 set up on experimental area 1 and can move on experimental area 1.
In one embodiment, mission aircraft 31 is connected to second industrial robot arm 32 via a connecting flange.
In one embodiment, the second industrial robot 32 is a six-axis robot, and similarly, the second industrial robot 32 may have more or less degrees of freedom depending on the actual situation to better meet the use requirement. It should be added that the second industrial robot arm 32 is mainly composed of a joint and a connecting rod, and the joint is composed of a motor, a harmonic reducer, an absolute position sensor, a joint torque sensor, a joint controller and the like.
The omni-directional mobile platform 33 is an omni-directional mobile AGV, such as an omni-directional mobile cart based on multiple sets of Mecanum wheels or an omni-directional mobile cart based on multiple sets of steerable wheels. The omnibearing movable platform 33 moves in the experimental site 1, the translation of X, Y and the rotation of the Z axis can be realized, and meanwhile, the second industrial mechanical arm 32 adjusts the pose of the task aircraft 31 at the tail end and controls the motion of the task aircraft 31 together with the omnibearing movable platform 33. Compared with the prior art, the omnibearing mobile platform 33 greatly increases the motion range of the mission aircraft 31, avoids the constraint of a guide rail on a motion mechanism, and meets the requirement of space large-scale three-dimensional motion simulation; in addition, the simulation system can increase the number of simulated objects, for example, two task aircrafts can be arranged to track the target aircraft, thereby increasing the flexibility of the simulation system.
In one embodiment, as shown in FIG. 1, the positioning system 4 is disposed on the omni-directional mobile platform 33. Of course, the positioning system 4 may be disposed at other positions, and may be disposed according to specific needs. The positioning system 4 is used to monitor in real time the base F of the omni-directional mobile platform 33 relative to the first industrial robot 22TbThen calculates the real-time motion track of the omni-directional mobile platform 33 according to the relative pose
Figure BDA0002610773230000061
i∈[x,y,θ]Wherein, in the step (A),
Figure BDA0002610773230000062
for real-time angles of the i-axis direction of the omni-directional mobile platform 33 at a certain time,
Figure BDA0002610773230000063
for real-time angular velocity of the omni-directional mobile platform 33 in the i-axis direction at a certain moment,
Figure BDA0002610773230000064
a PID feedback control system is established for the real-time angular acceleration of the omni-directional mobile platform 33 in the i-axis direction at a certain moment, so that the pose of the omni-directional mobile platform 33 is continuously adjusted until the pose is adjusted to be described laterExpected motion trajectory
Figure BDA0002610773230000065
i∈[x,y,θ]Wherein q isVi(t) is a desired angle of the i-axis direction of the omni-directional mobile platform 33 at a certain time,
Figure BDA0002610773230000066
for a desired angular velocity of the omni-directional mobile platform 33 in the i-axis direction at a certain time,
Figure BDA0002610773230000067
the desired angular acceleration of the omni-directional mobile platform 33 in the i-axis direction at a certain time is obtained, thereby improving the positioning accuracy of the omni-directional mobile platform 33. It should be noted that the expected movement trace TraViEach data in (T) is based on T in the experimental field1(t) theoretical data obtained by calculation. The positioning system 4 may be a camera positioning system, a high precision lidar positioning system, or an indoor GPS or high precision motion capture system, etc. The positioning process of the positioning system is specifically explained by taking a high-precision laser radar as an example: under the condition that the first industrial mechanical arm 22 is fixedly installed on the experiment site 1, the omni-directional mobile platform 33 provided with the high-precision laser radar is placed in the experiment site 1, the movement of the omni-directional mobile platform 33 is controlled, the laser radar is used for scanning the surrounding environment to obtain a point cloud picture of the surrounding environment, and meanwhile, a slam algorithm (such as mapping, cartographer and the like) is used for drawing the surrounding environment according to the self-milemeter information of the omni-directional mobile platform 33 until a global environment map with higher precision is obtained; in the simulation process, the laser radar scans the surrounding environment of the current position to generate a local point cloud map of the current position, matches the local point cloud map with the global environment map, and uses the matched position as the real-time position of the current omni-directional mobile platform 33 (i.e., the omni-directional mobile platform 33 is relative to the base F of the first industrial robot 22 as described above)TbRelative pose). The space positioning problem of the omnibearing moving platform 33 is solved by adopting a high-precision positioning system, so that the mechanism such as a large-stroke guide rail is not adoptedUnder the condition, the spatial pose of the omnibearing mobile platform 33 is accurately measured, and the motion simulation range and the simulation precision are met.
The following describes the spatial three-dimensional large-scale kinematics simulation method of the present invention.
The simulation method of the space three-dimensional large-scale kinematics, which is based on the simulation system of the space three-dimensional large-scale kinematics, comprises the following steps: establishing a fixed coordinate system F of the target aircraft 21T，FTIs the center of mass of the target aircraft 21, and establishes an orbit coordinate system F of the target aircraft 21WTaking FWIs the center of mass, F, of the target aircraft 21WPoints to the tangent and normal directions of the space orbit to establish a fixed coordinate system F of the mission aircraft 31SEstablishing a coordinate system F of the experimental site 1eFirst industrial robot 22 base coordinate system FTb(FTbAnd FeCoincident) and second industrial robot 32 base coordinate system FSbWherein only the relative motion relationship between target vehicle 21 and mission vehicle 31 in space is considered, regardless of the translational motion of target vehicle 21 along the trajectory of space. It should be noted that F is the target vehicle 21 that does not account for the translation along the trajectory of spaceWThe origin of (a) is stationary.
In a coordinate system FT、FW、FSThen, based on the initial angular velocity vector value of the given target aircraft 21 and according to the calculation of space dynamics, the fixed point motion track T of the target aircraft 21 in the space environment is obtained2(T), wherein T2(t) is the trajectory of the target aircraft 21 around the center of mass. Specifically, the movement locus of the target aircraft 21 is divided into the translational locus T of the centroid of the target aircraft 21 according to the calculation result of the space dynamicst(T) and the locus T of rotation of the target aircraft 21 about the center of massr(T), T, since the translation of target vehicle 21 along the trajectory of space is not taken into account during the simulation, T2(t)＝Tr(t), the spatial dynamics calculation process is as follows: vector of angular velocity
Figure BDA0002610773230000071
Wherein Ω ═ C ωsAnd C is a constant related to the inertial parameter of the target aircraft. OmegasIs a self-rotation angular velocity and is a constant, omeganIs the lateral angular velocity, is a constant,
Figure BDA0002610773230000075
the initial deflection angle is the angular velocity and is constant. The initial value omega is given as requiredx0,ωy0,ωz0Can calculate ωs,ωn,
Figure BDA0002610773230000076
The three constants are:
Figure BDA0002610773230000072
ωs＝ωz0；
Figure BDA0002610773230000073
the deflection angle can be obtained by integrating the angular velocity vector:
Figure BDA0002610773230000074
wherein R is0,Y0,P0Is an initial angle set as required. The angle of rotation and the speed of the target aircraft 21 at any given time can thus be determined from the above equations. And for the obtained Euler angles R, P and Y, directly converting into a homogeneous transformation matrix according to a formula. Fixed point motion trajectory T2(t) can be described by a dense series of discrete points, where any one discrete point contains the time and location of the point.
In a coordinate system FT、FW、FSThen, aiming at the control strategy of the mission aircraft 31, the mission aircraft 31 is controlled to move relative to the target aircraft 21, and the motion track T of the mission aircraft 31 in the space environment is directly obtained according to the flight route of the mission aircraft (31)1(T), wherein T1(t) is the fixed coordinate system F of mission aircraft 31SRelative orbit coordinate system FWIs described by a known series of closely spaced discrete points,any one discrete point contains the time and location of the point. It is supplementary that the control strategy of mission aircraft 31 is the mission execution strategy to be tested, and the control strategy has various implementation forms, for example, a remote control mode through ground teleoperation; as another example, the spatial trajectory that mission aircraft 31 will produce, i.e., the rigid coordinate system F of mission aircraft 31, is based on the current control strategy based on the corresponding movements of the aircraft's own sensors via the sensing environment and planningSRelative to the orbital coordinate system FWTrack T of1(t)。
The mission aircraft 31 carries out simulation planning on the relative motion of the target aircraft 21 in space in an experimental field: the relative motion between mission aircraft 31 and target aircraft 21 is divided into a first virtual motion and a second virtual motion, the first virtual motion being FTAround FWThe second virtual motion is FSRelative to FWThe second virtual motion is FSRelative to FSb(FSRelative to FSbN by the second industrial robot arm 32SIndividual joint realization), FSbRelative to Fe(FSbRelative to FeRealized by three joints of a virtual arm m described below), FeRelative to FTb(mutual pose is fixed), FTbRelative to FW(FTbRelative to FWN by the first industrial robot 22TRealizing each joint), and virtualizing the continuous motion chain into a super-redundant mechanical arm R, wherein the base coordinate system of the super-redundant mechanical arm R is FWThe tail end of the super-redundant mechanical arm R is a fixed coordinate system F of the mission aircraft 31S. It should be noted that, in order to establish the super-redundant manipulator R, a virtual manipulator m (as shown in fig. 2) is constructed between the omni-directional mobile platform 33 and the experimental site 1, and unlike the first industrial manipulator 22 and the second industrial manipulator 32, the virtual manipulator m has only three degrees of freedom, which are respectively translation in x and y directions and rotation around z direction on the experimental site 1, and here, the three degrees of freedom of the virtual manipulator m can be abstracted into three joints.Therefore, the super-redundant mechanical arm R is formed by connecting the first industrial mechanical arm 22, the virtual mechanical arm m and the second industrial mechanical arm 32 in series, and the target aircraft 21 winds the base F of the super-redundant mechanical arm RWAnd performing fixed-point motion.
According to the given movement trajectory T of mission aircraft 311(t) performing motion planning and trajectory planning based on the kinematic model of the virtual super-redundant mechanical arm R, and calculating the motion trajectory of each joint of the super-redundant mechanical arm R, namely the motion trajectory of each joint of the second industrial mechanical arm 32
Figure BDA0002610773230000081
i∈[1,NS]Wherein N isSIs the number of joints, q, of the second industrial robot arm 32Si(t) is the angle of a certain joint of the second industrial robot arm 32 at a certain moment,
Figure BDA0002610773230000082
the angular velocity of a certain joint of the second industrial robot arm 32 at a certain moment,
Figure BDA0002610773230000083
angular acceleration of a certain joint of the second industrial robot arm 32 at a certain moment; movement locus of each degree of freedom of the omni-directional mobile platform 33
Figure BDA0002610773230000084
i∈[x,y,θ](ii) a And the respective joint movement trajectories of the first industrial robot arm 22
Figure BDA0002610773230000085
i∈[1,NTT]Wherein N isTTIs the number of joints of the first industrial robot arm 22.
In the case of a constructed super redundant robot arm R according to the first industrial robot arm 22, at FWThe movement track Tra of each joint calculated for the base coordinate systemTTi(t) calculating and deriving the end F of the first industrial robot 22WRelative base FTbIs TraT(t)＝[xT(t) yT(t) zT(t) rxT(t) ryT(t) rzT(t)]T。
End F of first industrial robot 22WIs TraT(T) a trajectory T with the target aircraft 21 about the center of mass2(t) fusion to obtain the end F of the first industrial robot 22TAt FTbDown motion trajectory, then based on FTAt FTbThe movement locus of the first industrial robot arm 22, and the joint movement locus of the final joint movement of the first industrial robot arm 22
Figure BDA0002610773230000091
i∈[1,NT]Wherein N isTIs the number of joints of the first industrial robot arm (22). It should be noted that the motion trajectory TraT(t) is the end F of the first industrial robot arm 22WRelative base FTbThe motion trajectory of (1) is a result calculated by the second virtual motion, and if the first industrial robot arm 22 is supposed to perform spatial simulation on the target aircraft 21, the first virtual motion is considered, and the first virtual motion and the Tra are combinedT(t) are synthesized so that the joint locus Tra of the combined motion of the first industrial robot arm 22 can be obtainedTi(t)。
Based on the result of the trajectory planning for each joint, the simulation of the relative motion of the mission aircraft 31 with respect to the target aircraft 21 is performed.
By the space three-dimensional large-scale kinematics simulation method, the limitation of a ground experimental environment is broken through by combining the second mechanical arm 32 and the omnibearing moving platform 33, kinematics simulation in a larger range is realized, and the flexibility of a simulation system is increased; in addition, the super-redundant mechanical arm R is established, the complex collaborative planning problem of a plurality of actuating mechanisms is integrated into the planning problem of the super-redundant mechanical arm R, and the complexity of the problem is reduced; furthermore, by defining an orbital coordinate system FWAnd the abstracted virtual super-redundant mechanical arm R and a fixed-point motion structure convert the problem of respectively carrying out motion planning on a plurality of systems in the prior art into two independent virtual motion mechanisms (the first virtual motion mechanism is described above)And a second virtual motion), reducing the complexity of the problem.
It should be added that the specific steps for establishing the super-redundant mechanical arm R include: f is to beSbRelative to FeAbstract as a virtual robot m (as shown in fig. 2), the three degrees of freedom of the virtual robot m are assumed as three joints, and the first industrial robot 22, the virtual robot m, and the second industrial robot 32 may be connected in series to form a super-redundant robot R.
Kinematics model and spatial motion trajectory T for virtual super-redundant mechanical arm R1(t), the specific process of determining the motion trajectory of each joint is as follows: according to the Jacobian matrix of the virtual super-redundant mechanical arm R, the space velocity of each joint can be obtained
Figure BDA0002610773230000092
And super redundant end of arm velocity
Figure BDA0002610773230000093
The relationship of (1):
Figure BDA0002610773230000094
wherein JAIs a Jacobian matrix. Defining the weighting matrix W ═ diag [ omega ]1,ω2,…,ωN]Wherein N is NS+NV+NTN is the number of degrees of freedom of the super-redundant mechanical arm R,
Figure BDA0002610773230000095
h (q) is an objective function to be optimized, and optimization performance indexes comprise the motion flexibility, obstacle avoidance, joint angle limit and the like of the mechanical arm. Under the action of the weighting matrix, defining a weighted Jacobian matrix
Figure BDA0002610773230000101
And weighting joint angular velocities
Figure BDA0002610773230000102
Based on the weighted Jacobian matrix and the weighted joint angular velocity, have
Figure BDA0002610773230000103
From this, a weighted joint angular velocity can be determined
Figure BDA0002610773230000104
Wherein the content of the first and second substances,
Figure BDA0002610773230000105
further get the formula (I)
Figure BDA0002610773230000106
Will be provided with
Figure BDA0002610773230000107
Into formula (I) and further to obtain
Figure BDA0002610773230000108
Track T1(t) is comprised of a series of discrete points with respect to time, the time interval between adjacent points being Δ t, the ultra-redundant robotic arm tip speed being based on
Figure BDA0002610773230000109
Is calculated, from which it can be calculated
Figure BDA00026107732300001010
Further, the joint angle at the current moment can be calculated according to the joint angular velocity at the previous moment
Figure BDA00026107732300001011
According to the calculated joint angle q (t), aiming at any single joint, the speed and acceleration values are calculated by adopting a cubic polynomial interpolation mode, and therefore, the planning result of the super-redundant mechanical arm, namely, the motion track of each joint of the second industrial mechanical arm 32 can be finally obtained
Figure BDA00026107732300001012
i∈[1,NS](ii) a Movement locus of each degree of freedom of the omni-directional mobile platform 33
Figure BDA00026107732300001013
i∈[x,y,θ](ii) a And the respective joint movement trajectories of the first industrial robot arm 22
Figure BDA00026107732300001014
i∈[1,NTT]。
End F of first industrial robot 22WIs TraT(T) a trajectory T with the target aircraft 21 about the center of mass2(t) the specific steps of performing fusion include: establishing a positive kinematic equation PTm＝X(qTm) Equation of inverse kinematics qTm＝Y(PTm) Wherein P isTmShowing the end of the first industrial robot 22 at FTbPosition attitude vector of lower, qTmAn angle vector representing each joint of the first industrial robot arm 22 in joint space; end F of super redundant mechanical armWRelative base FTbPlanning the obtained movement track TraT(t) is represented by a homogeneous transformation matrix as
Figure BDA00026107732300001015
Locus T of motion of target aircraft 21 around centroid2(t) expansion into homogeneous transformation matrices
Figure BDA00026107732300001016
According to
Figure BDA00026107732300001017
The end F of the first industrial robot 22 may be calculatedTAt FTbUnderlying homogeneous transformation matrix
Figure BDA00026107732300001018
Then based on
Figure BDA00026107732300001019
Inverse kinematics equation q through the first industrial robot arm 22Tm＝Y(PTm) The position of each joint is solved, the velocity and acceleration of each joint are calculated through a difference method, and finally the plan of the combined motion of the first industrial mechanical arm 22 is obtainedResults
Figure BDA00026107732300001020
i∈[1,NT]。
The first and second industrial robots 22, 32 are provided with joint controllers themselves so that they can accurately follow the planned motion trajectory (i.e., Tra) during the simulation processSi(t) and TraTi(t)). The omni-directional mobile platform 33 can precisely adjust the pose thereof through the high-precision positioning system 4, thereby precisely following the planned motion trajectory, specifically, the omni-directional mobile platform 33 follows the actual measurement data trajectory
Figure BDA00026107732300001021
i∈[x,y,θ]The PID feedback control system is established to follow the expected motion trajectory of the respective degrees of freedom of the omni-directional mobile platform 33
Figure BDA0002610773230000111

Claims (5)

1. A space three-dimensional large-scale kinematics simulation system is characterized by comprising an experimental site (1), a target aircraft simulation mechanism (2) and a task aircraft simulation mechanism (3);
the target aircraft simulation mechanism (2) comprises a target aircraft (21) and a first industrial mechanical arm (22), the base of the first industrial mechanical arm (22) is fixed on the experimental site (1), and the tail end of the first industrial mechanical arm (22) is connected to the target aircraft (21);
task aircraft simulation mechanism (3) are including task aircraft (31), second industry arm (32) and all direction movement platform (33), and on the base of second industry arm (32) was fixed in all direction movement platform (33), the end-to-end connection in task aircraft (31) of second industry arm (32), all direction movement platform (33) set up on experimental area (1) and can move on experimental area (1).
2. The spatial three-dimensional large-scale kinematic simulation system according to claim 1, wherein the spatial three-dimensional large-scale kinematic simulation system further comprises: and the positioning system (4) is arranged on the omnibearing movable platform (33) and is used for acquiring the relative pose of the omnibearing movable platform (33) relative to the base of the first industrial mechanical arm (22) in real time.
3. A method for simulating a spatial three-dimensional large-scale kinematics based on the spatial three-dimensional large-scale kinematics simulation system of claim 1, comprising:
establishing a fixed coordinate system F of the target aircraft (21)T，FTThe origin point of the target aircraft (21) is the mass center of the target aircraft (21), and an orbit coordinate system F of the target aircraft (21) is establishedW，FWThe origin point of the target aircraft (21) is the mass center of the target aircraft, and a fixed coordinate system F of the mission aircraft (31) is establishedSEstablishing a coordinate system F of the experimental site (1)eA first industrial robot arm (22) base coordinate system FTbAnd a second industrial robot arm (32) base coordinate system FSbWherein, the translation of the target aircraft (21) along the space orbit is not considered, and only the relative motion relation between the target aircraft (21) and the mission aircraft (31) in the space is considered;
in a coordinate system FT、FW、FSThen, a fixed point motion track T of the target aircraft (21) in the space environment is obtained based on the initial angular velocity vector value of the given target aircraft (21) and according to the calculation of space dynamics2(T), wherein T2(t) is the target aircraft's (21) trajectory of rotation about the center of mass;
in a coordinate system FT、FW、FSThen, aiming at the control strategy of the mission aircraft (31), the mission aircraft (31) is controlled to move relative to the target aircraft (21), and the movement track T of the mission aircraft (31) in the space environment is directly obtained according to the flight path of the mission aircraft (31)1(T), wherein T1(t) is a fixed coordinate system F of the mission aircraft (31)SRelative orbit coordinate system FWRelative movement track of, T1(t) is represented by a series of dense discrete points;
the relative movement of the mission aircraft (31) to the target aircraft (21) in spaceAnd (3) performing sports planning on an experimental field: dividing the relative movement between the mission aircraft (31) and the target aircraft (21) into a first virtual movement and a second virtual movement, the first virtual movement being FTAround FWThe second virtual motion is FSRelative to FWThe second virtual motion is FSRelative to FSb、FSbRelative to Fe、FeRelative to FTb、FTbRelative to FWThe continuous motion chain is virtualized to be a super-redundant mechanical arm (R), and the base coordinate system of the super-redundant mechanical arm (R) is FWThe tail end of the super-redundant mechanical arm (R) is a fixed coordinate system F of the mission aircraft (31)S；
According to the movement locus T of the given mission aircraft (31)1(t) performing motion planning and trajectory planning based on the kinematic model of the virtual super-redundant mechanical arm (R), and calculating the motion trajectory of each joint of the super-redundant mechanical arm (R), namely the motion trajectory of each joint of the second industrial mechanical arm (32)
Figure FDA0002610773220000021
Wherein N isSIs the number of joints of the second industrial robot arm (32); motion locus of each degree of freedom of the omni-directional mobile platform (33)
Figure FDA0002610773220000022
And each joint movement locus of the first industrial robot arm (22)
Figure FDA0002610773220000023
Wherein N isTTIs the number of joints of the first industrial robot arm (22);
according to the movement locus Tra of each joint of the first industrial mechanical arm (22)TTi(t) calculating the end F of the first industrial robot arm (22)WRelative base FTbIs TraT(t)＝[xT(t) yT(t) zT(t) rxT(t) ryT(t) rzT(t)]T；
Attaching a first industrial robot arm (22) end FWIs TraT(T) a trajectory T with the target aircraft (21) around the center of mass2(t) fusing to obtain a terminal F of the first industrial robot arm (22)TAt FTbDown motion trajectory, then based on FTAt FTbThe movement locus of the final joint movement of the first industrial robot arm (22) is obtained
Figure FDA0002610773220000024
Wherein N isTIs the number of joints of the first industrial robot arm (22);
and simulating the relative motion of the mission aircraft (31) relative to the target aircraft (21) according to the trajectory planning result of each joint.
4. The method of claim 3, wherein the spatial three-dimensional large-scale kinematics simulation system,
attaching a first industrial robot arm (22) end FWIs TraT(T) a trajectory T with the target aircraft (21) around the center of mass2(t) the specific steps of performing fusion include:
establishing a positive kinematic equation PTm＝X(qTm) Equation of inverse kinematics qTm＝Y(PTm) Wherein P isTmShowing the end of the first industrial robot arm (22) at FTbPosition attitude vector of lower, qTmAn angle vector representing each joint of the first industrial robot arm (22) in joint space;
the end F of the super-redundant mechanical arm (R)WRelative base FTbPlanning the obtained movement track TraT(t) is represented by a homogeneous transformation matrix as
Figure FDA0002610773220000031
A movement locus T of the target aircraft (21) around the center of mass2(t) expansion into homogeneous transformation matrices
Figure FDA0002610773220000032
According to
Figure FDA0002610773220000033
The end F of the first industrial robot arm (22) can be calculatedTAt FTbUnderlying homogeneous transformation matrix
Figure FDA0002610773220000034
Then based on
Figure FDA0002610773220000035
Inverse kinematics equation q by a first industrial robot arm (22)Tm＝Y(PTm) The position of each joint is solved, the speed and the acceleration of each joint are calculated through a difference method, and finally a first industrial mechanical arm (22) and motion planning result is obtained
Figure FDA0002610773220000036
5. The method of claim 3, wherein the spatial three-dimensional large-scale kinematics simulation system,
in the last step of execution, the method for the ground simulation system to simulate the approach of the mission aircraft to the target aircraft in the space further comprises:
the omnibearing moving platform (33) is based on the measured data track
Figure FDA0002610773220000037
Establishing a PID feedback control system to follow a desired motion trajectory of respective degrees of freedom of the omni-directional mobile platform (33)
Figure FDA0002610773220000038
CN202010753564.8A 2020-07-30 2020-07-30 Spatial three-dimensional large-scale kinematics simulation system and method Active CN111872938B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010753564.8A CN111872938B (en) 2020-07-30 2020-07-30 Spatial three-dimensional large-scale kinematics simulation system and method

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010753564.8A CN111872938B (en) 2020-07-30 2020-07-30 Spatial three-dimensional large-scale kinematics simulation system and method

Publications (2)

Publication Number Publication Date
CN111872938A true CN111872938A (en) 2020-11-03
CN111872938B CN111872938B (en) 2022-01-25

Family

ID=73204775

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010753564.8A Active CN111872938B (en) 2020-07-30 2020-07-30 Spatial three-dimensional large-scale kinematics simulation system and method

Country Status (1)

Country Link
CN (1) CN111872938B (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116587323A (en) * 2023-05-12 2023-08-15 广东德中众惠智能装备科技有限公司 Mechanical arm dynamic simulation system based on machine vision

Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3243500B2 (en) * 1990-09-28 2002-01-07 科学技術庁航空宇宙技術研究所長 Zero gravity simulator and arbitrary gravity simulator
US20050230557A1 (en) * 2003-12-30 2005-10-20 Canadian Space Agency Zero-G emulating testbed for spacecraft control system
CN105009184A (en) * 2013-01-14 2015-10-28 穆格公司 Motion simulator
CN105539890A (en) * 2015-12-30 2016-05-04 哈尔滨工业大学 Device and method for simulating space mechanical arm to capture ground three-dimensional space microgravity of target satellite
CN105904457A (en) * 2016-05-16 2016-08-31 西北工业大学 Heterogeneous redundant mechanical arm control method based on position tracker and data glove
CN106055522A (en) * 2016-06-30 2016-10-26 大连大学 Minimum base attitude disturbance track planning method for redundant space manipulator
CN106166749A (en) * 2016-06-29 2016-11-30 北京控制工程研究所 The motion track planing method of multi-arm robot is moved in a kind of space
CN107161360A (en) * 2017-06-07 2017-09-15 北京航空航天大学 Replaceable free across the yardstick checking device of pedestal motion reappearance of space tasks
CN107207088A (en) * 2014-12-04 2017-09-26 埃尔瓦有限公司 Reconfigurable unmanned vehicle system
CN107244432A (en) * 2017-06-07 2017-10-13 北京航空航天大学 Free pedestal Spatial Cooperation task motion reappearance experimental system
CN108621202A (en) * 2018-05-15 2018-10-09 清华大学深圳研究生院 Multi-arm robot for space cooperates with accurate operation experimental system on land
CN110641738A (en) * 2019-10-15 2020-01-03 大连理工大学 Trajectory tracking control method of space five-degree-of-freedom free flying mechanical arm
CN110815215A (en) * 2019-10-24 2020-02-21 上海航天控制技术研究所 Multi-mode fused rotating target approaching and stopping capture ground test system and method
US10596700B2 (en) * 2016-09-16 2020-03-24 Carbon Robotics, Inc. System and calibration, registration, and training methods
US20200150852A1 (en) * 2017-01-10 2020-05-14 Cast Group Of Companies Inc. Systems and Methods for Tracking and Interacting With Zones in 3D Space

Patent Citations (15)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP3243500B2 (en) * 1990-09-28 2002-01-07 科学技術庁航空宇宙技術研究所長 Zero gravity simulator and arbitrary gravity simulator
US20050230557A1 (en) * 2003-12-30 2005-10-20 Canadian Space Agency Zero-G emulating testbed for spacecraft control system
CN105009184A (en) * 2013-01-14 2015-10-28 穆格公司 Motion simulator
CN107207088A (en) * 2014-12-04 2017-09-26 埃尔瓦有限公司 Reconfigurable unmanned vehicle system
CN105539890A (en) * 2015-12-30 2016-05-04 哈尔滨工业大学 Device and method for simulating space mechanical arm to capture ground three-dimensional space microgravity of target satellite
CN105904457A (en) * 2016-05-16 2016-08-31 西北工业大学 Heterogeneous redundant mechanical arm control method based on position tracker and data glove
CN106166749A (en) * 2016-06-29 2016-11-30 北京控制工程研究所 The motion track planing method of multi-arm robot is moved in a kind of space
CN106055522A (en) * 2016-06-30 2016-10-26 大连大学 Minimum base attitude disturbance track planning method for redundant space manipulator
US10596700B2 (en) * 2016-09-16 2020-03-24 Carbon Robotics, Inc. System and calibration, registration, and training methods
US20200150852A1 (en) * 2017-01-10 2020-05-14 Cast Group Of Companies Inc. Systems and Methods for Tracking and Interacting With Zones in 3D Space
CN107161360A (en) * 2017-06-07 2017-09-15 北京航空航天大学 Replaceable free across the yardstick checking device of pedestal motion reappearance of space tasks
CN107244432A (en) * 2017-06-07 2017-10-13 北京航空航天大学 Free pedestal Spatial Cooperation task motion reappearance experimental system
CN108621202A (en) * 2018-05-15 2018-10-09 清华大学深圳研究生院 Multi-arm robot for space cooperates with accurate operation experimental system on land
CN110641738A (en) * 2019-10-15 2020-01-03 大连理工大学 Trajectory tracking control method of space five-degree-of-freedom free flying mechanical arm
CN110815215A (en) * 2019-10-24 2020-02-21 上海航天控制技术研究所 Multi-mode fused rotating target approaching and stopping capture ground test system and method

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
YONGNAN JIA;QING LI;SHANQIAO QIU: "Distributed Leader-Follower Flight Control for Large-Scale Clusters of Small Unmanned Aerial Vehicles", 《UNMANNED AIRCRAFT SYSTEMS》 *
卢佳园: "自由漂浮空间机器人的轨迹规划方法及地面半物理仿真实验研究", 《中国优秀硕士学位论文全文数据库信息科技辑》 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116587323A (en) * 2023-05-12 2023-08-15 广东德中众惠智能装备科技有限公司 Mechanical arm dynamic simulation system based on machine vision
CN116587323B (en) * 2023-05-12 2023-11-21 德中(深圳)激光智能科技有限公司 Mechanical arm dynamic simulation system based on machine vision

Also Published As

Publication number Publication date
CN111872938B (en) 2022-01-25

Similar Documents

Publication Publication Date Title
CN108381553B (en) Relative navigation close-range tracking method and system for space non-cooperative target capture
Zhong et al. A practical visual servo control for aerial manipulation using a spherical projection model
Lee et al. Obstacle avoidance using image-based visual servoing integrated with nonlinear model predictive control
Jithu et al. Quadrotor modelling and control
Xu et al. Autonomous path planning and experiment study of free-floating space robot for target capturing
CN110146224A (en) A method of identification assembly spacecraft mass, centroid position and inertial tensor
Bolognini et al. Lidar-based navigation of tethered drone formations in an unknown environment
Proctor et al. Vision-only approach and landing
Lai et al. Image dynamics-based visual servo control for unmanned aerial manipulatorl with a virtual camera
Zhao et al. Minimum base disturbance control of free-floating space robot during visual servoing pre-capturing process
Fagerer et al. Visual grasping with long delay time of a free floating object in orbit
CN111872938B (en) Spatial three-dimensional large-scale kinematics simulation system and method
Danko et al. Evaluation of visual servoing control of aerial manipulators using test gantry emulation
Lai et al. An onboard-eye-to-hand visual servo and task coordination control for aerial manipulator based on a spherical model
Karimoddini et al. Hierarchical control design of a UAV helicopter
Yang et al. Ground verification of space robot capturing the free-floating target based on visual servoing control with time delay
Seweryn et al. Validation of the robot rendezvous and grasping manoeuvre using microgravity simulators
CN116714780A (en) Rotor flying mechanical arm and planning and control method for rapid aerial grabbing
Bouzgou et al. PD sliding mode controller based decoupled aerial manipulation
Mutawe et al. Robust Path Following Controllers for Quadrotor and Ground Robot
Kutia et al. Modeling and characterization of a canopy sampling aerial manipulator
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following
Xu et al. Dynamics and control of a novel manipulator on vtol aircraft (mova) system-a planar case study
Morton et al. Search and retrieve with a fully autonomous aerial manipulator
Fei et al. Swan-inspired unmanned aerial vehicles with long-neck visual perception system

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
CB03 Change of inventor or designer information
CB03 Change of inventor or designer information

Inventor after: Zhang Jiwen

Inventor after: Liu Yu

Inventor after: Liu Chen

Inventor after: Feng Weichun

Inventor after: Chen Ken

Inventor after: Song Libin

Inventor before: Zhang Jiwen

Inventor before: Liu Chen

Inventor before: Liu Yu

Inventor before: Feng Weichun

Inventor before: Chen Ken

Inventor before: Song Libin

GR01 Patent grant
GR01 Patent grant