CN103770115A - Silkworm spinning trajectory and speed control method - Google Patents

Abstract

The invention discloses a method for controlling silkworm spinning track and speed. This method is realized by a control system composed of a mechanical arm hardware platform, robot control technology, and artificial intelligence. Its purpose is to simulate the silk-spinning behavior of silkworms by developing a robot system that simulates silk-spinning trajectories, and finally let silkworms spin silk according to the artificially designed trajectory and speed, so as to obtain silk material groups that are convenient for classification, comparison and analysis.

Description

The control method of a kind of family silkworms spin silk track and speed

Technical field

The invention belongs to Robotics and artificial intelligence field, its essence is in utilize the correlation technique in this field to be applied to house silkworms spin silk behavior analysis research, further simulate and control house silkworms spin silk track and speed, to obtain silk material group.

Background technology

Silkworm spinning rate is different for different silkworm individualities, and along with environmental condition and the weaving silk the stage of silkworm and change.Equally, family silkworms spin silk that track is also considered to silk physicochemical property to have a direct impact.(for example consider family's silkworms spin silk behavior, track and speed etc.) and silk physicochemical property between relevance, to realize the lifting of silk physicochemical property or specially to turn to target, add various chemistry or processing method physically, can develop various new functional materials, widen the new purposes of silk.But how achievement in the past is only conceived to house silkworms spin silk behavior or other research consider silkworms spin silk the system of behavior in silkworms spin silk behavioral study basis the gets on simulation of known family and emulation man, finally allows silkworm weave silk according to the track of artificial design and speed.

Summary of the invention

Technical problem to be solved by this invention is a kind of method that silkworm can weave silk according to the track of artificial design and speed of controlling, it is that artificial intelligence theory and Robotics are combined, on the basis that the behavioral data of being in that silkworms spin silk is resolved, design and invent a kind of method and control family's silkworms spin silk track and speed, silk is weaved silk according to the track of artificial design and speed, be convenient to obtain the family of various combination silkworms spin silk the silk material group of track and speed, thereby facilitate researcher to study silk physicochemical property.

The technology of the present invention solution is as follows:

The present invention is on the basis of the characteristic such as behavior and pose of weaving silk that parses silkworm, design and invent a kind of mechanical arm control system and control family's silkworms spin silk track and speed, its system architecture comprises: mechanical arm, mechanical arm driving governor, family silkworms spin silk platform.Wherein mechanical arm can be fixing flexibly with silkworm, and type of drive can be taked the forms such as motor-driven, and silkworms spin silk that platform is two dimensional surface or 3 D stereo platform in family.

The control method of a kind of family silkworms spin silk track and speed, its implementation is under space coordinates, initial position, movement locus and the speed of given silkworm, calculate the control set point of family's silkworms spin silk trajectory path, thereby motor is carried out to SERVO CONTROL, realize effective control and the planning of the space tracking point to mechanical arm distal point place.Thereby realize the control to family's silkworms spin silk behavior (track and speed etc.), and silkworm is weaved silk according to the track of artificial design and speed.

The concrete steps that described control method realizes are as follows:

(1) no matter build overall rectangular coordinate system, be all this overall situation rectangular coordinate systems relatively of control set point in each path in the initial position of family's silkworms spin silk track or silkworm movement locus; Meanwhile, also set up joint space coordinate system, this coordinate system acts on the operation of controlling mechanical arm;

(2) according to given family silkworms spin silk track, utilize the matlab track of weaving silk to sample into discrete point, this discrete point is the motion control set point that silkworm weaves silk in overall rectangular coordinate system;

(3) joint driver of supposition mechanical arm is given in joint space coordinate system, the mechanical arm tail end control set point obtaining retrains in global coordinate system, therefore also need anti-Inverse Kinematic Problem solution to obtain that (the method is known, manipulator motion trajectory planning well-known process), namely the control set point in rectangular coordinate system is mapped in set up joint space, obtain the each joint variable in mechanical arm tail end control silkworm silking process, thereby can better operate mechanical arm;

The explanation of step 2 and step 3: owing to mechanical arm being controlled to some discrete control points that first will draw in joint space coordinate system, therefore in order to obtain the discrete point very approaching with given curve track, need to adopt the method for certain function approximation the track discretization in overall rectangular coordinate system, the overall rectangular coordinate system at the relative curvilinear path of the TRAJECTORY CONTROL point place of discretization draws, in joint of mechanical arm space coordinates to the operation of mechanical arm, so need to the TRAJECTORY CONTROL of the discretization in rectangular coordinate system point be mapped to the computation of inverse-kinematics to the control point, parametrization path that meets joint path constraint.

(4) the control set point in a series of joint spaces that step (3) obtained is converted to the set-point of motor, and motor is carried out to SERVO CONTROL, namely mechanical arm is carried out to control operation, thereby realizes silkworm movement locus and the speed controlled.

Control family's silkworms spin silk track and speed by inventive method, can make silk weave silk according to the track of artificial design and speed, be convenient to obtain the family of various combination silkworms spin silk the silk material group of track and speed, thereby facilitate researcher to study silk physicochemical property.

Accompanying drawing explanation

Fig. 1 is the structural representation that adopts multi-joint manipulator arm control man silkworms spin silk track and speed;

Fig. 2 is the structural representation that adopts rectangular coordinate system mechanical arm control man silkworms spin silk track and speed;

In Fig. 1: 1,2, the turning joint point of 3-multi-joint manipulator arm, count and be not limited to 3 in actual joint, and quantity can >=1,4-mechanical arm and silkworm fixed terminal, 5-family silkworms spin silk platform, global coordinate system of 6-control system.

In Fig. 2: 1,2, the 3-X of corresponding rectangular coordinate system mechanical arm respectively, Y, Z rotating shaft, and can be respectively at corresponding X, Y, Z direction parallel.4-mechanical arm and silkworm fixed terminal, 5-family silkworms spin silk platform, global coordinate system of 6-control system.

The specific embodiment

The system architecture that the present invention adopts comprises: mechanical arm, mechanical arm drive and control, family's silkworms spin silk platform.Wherein mechanical arm can be fixing flexibly with silkworm, and type of drive can be taked the forms such as motor-driven, and silkworms spin silk that platform is two dimensional surface or 3 D stereo platform in family.

The specific embodiment is as follows:

(1) build rectangular coordinate system and joint space coordinate system, the initial position of silkworm and the movement locus of silkworm are for rectangular coordinate system, and the joint variable of mechanical arm is given in joint space coordinate system.

(2) the known rectangular coordinate system player whose turn comes next movement locus that silkworms spin silk, utilizes uniformly-spaced curvilinear function discretization method (the method has uniformly-spaced method, chord length method, equal error method etc. conventionally) that this movement locus is separated into series of points, if be (p _xi, p _yi, p _zi), i=1,2,3....Wherein, p _xi, p _yi, p _zirepresent respectively the coordinate figure of series of points x, y, z axle under rectangular coordinate system; I represents the number of series of points, and since 1 counting, increment is 1.

(3) in the situation that of known family silkworms spin silk discrete location coordinate points, use robot inverse kinematics to solve each joint variable of mechanical arm, first explanation is below by the each symbolic variable using: a _n: the length of mechanical arm connecting rod; α _n: connecting rod torsion angle; d _n: the distance of adjacent two connecting rods; θ _n: the angle of adjacent two connecting rods; N: the number of degrees of freedom, of mechanical arm; Wherein, mechanical arm type difference, joint variable difference, rotary joint θ _nfor joint variable, and arthrodia d _nfor joint variable, remaining is all the link parameters of mechanical arm.The Robot kinematics equations with n the free degree is generally:

⁰T _n＝ ⁰T ₁ ¹T ₂ ²T ₃... ^n-1T _n＝A ₁A ₂A ₃...A _n （1）

Wherein T _nrepresent the pose of mechanical arm tail end connecting rod with respect to joint space coordinate system, it can use formula (2) to represent; It is by three translational component p _x, p _y, p _z(determining locus) and three rotating vector n (approaching vector), o (orientation vector), a (normal vector), these three vectors are determined attitude.A _n(n=1,2, ...) represent the transformation matrix between adjacent two joint coordinate systems, can use formula (3) to represent, wherein the coordinate system in each joint of mechanical arm is all to set up according to Denavit and Harenberg (being D-H) method (standard method of robot motion's modeling)

$T_n=\left[\begin{array}{cccc}n& o& a& p\end{array}\right]=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]---\left(2\right)$

$A_n=\left[\begin{array}{cccc}c{\mathrm{\θ}}_n& -s{\mathrm{\θ}}_{n}c{\mathrm{\α}}_n& s{\mathrm{\θ}}_{n}s{\mathrm{\α}}_n& a_{n}c{\mathrm{\θ}}_n\\ s{\mathrm{\θ}}_n& c{\mathrm{\θ}}_{n}c{\mathrm{\α}}_n& -c{\mathrm{\θ}}_{n}s{\mathrm{\α}}_n& a_{n}s{\mathrm{\θ}}_n\\ 0& s{\mathrm{\α}}_n& c{\mathrm{\α}}_n& d_n\\ 0& 0& 0& 1\end{array}\right]---\left(3\right)$

Wherein, c represents cos θ, and s represents sin θ.

(4) supposing that joint of mechanical arm number is 3, counts and is not limited to 3 in actual joint, and quantity can >=1, and is rotary joint, can be obtained by formula (1) (2):

$\mathrm{<figure-callout\; id="3"\; label="T"\; filenames="140227153248.png"\; state="\{\{state\}\}">T</figure-callout>}_3^0=\left[\begin{array}{cccc}{n}_x& o_x& a_x& p_x\\ n_y& o_y& a_y& p_y\\ n_z& o_z& a_z& p_z\\ 0& 0& 0& 1\end{array}\right]=A_1{A}_2{A}_3$

(5) simultaneous formula (3) (4), establishes an equation and obtains the joint variable θ of mechanical arm in its motion process ₁, θ ₂, θ ₃, wherein a _i, α _iand d _ibe fixed value, its type according to mechanical arm is different and different; In addition, (p _xi, p _yi, p _zi) be known family silkworms spin silk position coordinates point, wherein i=1,2 ... n.

(6) joint variable in a series of joint spaces that obtain is converted to the set-point of motor.

(7) thus motor is carried out SERVO CONTROL and is realized the control method of silkworm movement locus and speed.

Take several frequently seen mechanical arm as example:

Referring to Fig. 1, to multi-joint manipulator arm, the artis being connected with pedestal can be rotated around pedestal, and all the other artis can realize horizontal and vertical direction and rotate, thereby makes any space coordinates point movement that arm distal point can be in can coverage area.

Referring to Fig. 2, to right angle coordinate manipulator arm, can be by controlling respectively X, the Y of mechanical arm, the free degree of tri-directions of Z is moved in each coordinate length range.

Wherein the mechanical arm in Fig. 1 and Fig. 2 is common two types, is not limited to this two kinds of structures when actual use.

To circular cylindrical coordinate machinery arm, perform an action to do bearing rotary around polar region main body, mechanical arm can be rotatablely moved with angle of pitch vertical direction in the horizontal direction.

Polar coordinates mechanical arm is supported by two rotating shafts, and one is rotated around pedestal, can stretch out with linear movement income, and another makes the rotation of vertical linearity around pedestal.

To ball-type coordinate mechanical arm, there are two and rotatablely move and a rectilinear motion joint, and press the motion of spherical coordinates form.

Claims (5)

1. A method for controlling the silkworm spinning track and speed, characterized in that: the method is to utilize the end of the mechanical arm to fix the silkworm, after the initial position of the silkworm and the silkworm spinning track, speed and target position are given, the silkworm is calculated. The control set point of the spinning trajectory path, through the servo control of the motor, realizes that the mechanical arm drives the silkworm to spin silk according to the preset trajectory and speed; The concrete steps that described control method realizes are as follows: (1) Establish a global Cartesian coordinate system. Both the initial position of the silkworm spinning trajectory and the control setting points of each path in the silkworm trajectory are relative to the global Cartesian coordinate system. At the same time, a joint space coordinate system is also established. The coordinate system is used to control the operation of the robotic arm; (2) According to the given spinning trajectory of the silkworm, use matlab to sample the spinning trajectory curve into discrete points, which are the motion control set points of silkworm spinning in the global Cartesian coordinate system; (3) Assuming that the joint driver of the manipulator is given in the joint space coordinate system, the obtained control set point at the end of the manipulator is constrained in the global coordinate system, so it is necessary to inversely solve the kinematics inverse problem , that is, to map the control set point in the Cartesian coordinate system to the established joint space, that is, to obtain the joint variables in the process of controlling silkworm silk spinning at the end of the robotic arm, so that the robotic arm can be better operated; (4) Convert a series of control set points in the joint space obtained in step (3) into the given value of the motor, and perform servo control on the motor, that is, control the mechanical arm, so as to realize the control of the silkworm's trajectory and speed. 2. the method for controlling silkworm spinning track and speed according to claim 1, characterized in that: the structural form of the mechanical arm can be a multi-joint mechanical arm, a rectangular coordinate system mechanical arm, a spherical coordinate mechanical arm, a polar coordinate robotic arm or cylindrical coordinate robotic arm; Using a multi-joint robotic arm, the joints connected to the base can rotate around the base, and the rest of the joints can rotate horizontally and vertically, so that the end of the robotic arm can move at any spatial coordinate point within the reach; The Cartesian coordinate robot arm is used to move within the range of each coordinate length by separately controlling the degrees of freedom in the X, Y, and Z directions of the robot arm; Cylindrical coordinate manipulators are used to rotate the bearings around the polar body to perform actions, so that the manipulators can rotate in the horizontal direction and the vertical direction of the pitch angle; The polar coordinate robot arm is supported by two rotating shafts, one rotates around the base and can extend in and out with a linear motion, and the other makes a vertical linear rotation around the base; The mechanical arm adopts spherical coordinates, has two rotary motions and one linear motion joint, and moves in the form of spherical coordinates. 3. The method for controlling silkworm spinning trajectory and speed according to claim 2, characterized in that: the number of degrees of freedom of the mechanical arm is >=1, and the mechanical arm can reach any point within the required range of motion space. 4. A method for controlling silkworm spinning trajectory and speed according to claim 2, characterized in that: the motion speed of the mechanical arm is regulated by the control system, and can realize uniform or variable speed motion. the 5. The method for controlling silkworm spinning trajectory and speed according to any one of claims 1-4, characterized in that: the trajectory can be any trajectory of a straight line or a curve in a two-dimensional plane or a three-dimensional space.

Images