CN104440926A - Mechanical arm somatic sense remote controlling method and mechanical arm somatic sense remote controlling system based on Kinect - Google Patents

Abstract

The invention relates to a mechanical arm somatic sense remote controlling method and a mechanical arm somatic sense remote controlling system based on Kinect, and belongs to the technical field of visual control of computers. The method comprises the following steps of (1) detecting change of positions of bones of an arm of a human body in real time by using the Kinect which is provided by Microsoft and serves as a sensor on a client side; (2) analyzing acquired data by using a PC (personal computer) of the client side, packaging an analysis result which serves as a control command into a data format of control words, and transmitting the control words to a server side in a Socket communication mode; (3) analyzing data of the received control words by using a server and forwarding an analysis result to a singlechip by using a serial port; and (4) generating PWM (pulse-width modulation) pulse width after the singlechip receives the data, outputting the PWM pulse width to a corresponding steering engine and controlling a mechanical arm in a somatic sense remote mode. In the method, a non-contact type control mode is used, the labor cost is reduced by an Internet facility, and people can remotely control the mechanical arm in any places at which the Internet is available and do not need to arrive at a scene.

Description

A kind of mechanical arm body sense long-range control method based on Kinect and system

Technical field

The invention belongs to computer vision control technology field, relate to a kind of mechanical arm body sense long-range control method based on Kinect and system.

Background technology

Along with the quickening of present society aging process and cause the increase of the number of limb injury due to reasons such as various disease, industrial injury, traffic accidents, and limited medical care human resources can not meet the needs of the full-time nursing of physical disabilities in a sense, individuals with disabilities also has the requirement and serious hope of living on one's own life with strength of controlling oneself, for elderly and the disabled scholar provides the application tool of superior performance to become one of entire society's issues that need special attention already simultaneously.Service robot arises at the historic moment as a kind of mobile robot, and in good time is in the sight of people.Its several functions comprised covers independent navigation, keeps away barrier and these aspects of human-computer dialogue, in addition, and the row that extraordinary service is also providing.Thus elderly and the disabled scholar has had and has greatly improved in daily life and work quality, this also enable them to regain self care ability and be socially reintegrated become a kind of may.At present, the researcher of countries in the world is extensively carrying out probing into of service robot correlation technique.When family's service field has welcome intelligent robot technology, the appearance of service robot has just become possibility, and its technology related to mainly contains four aspects, is respectively Machine Design, control method, sensor and artificial intelligence.From this angle of the field that it is contained, mainly containing five aspects, is machine vision, Navigation and localization, Multi-sensor Fusion, pattern-recognition and System Discrimination and Man Machine Interface particularly.

Due to the difference of each population profile of individuals with disabilities, thus they are naturally not identical to the requirement of service robot function yet.Develop the man-machine interface of many services robot for these Study on Problems persons, be intended to solve different customer demands.Control stick controls, can be used in those disabled degree by modes such as key control, touch-screen controls comparatively light, and limbs dynamic role is higher and realize good crowd.And such as Voice command, head controls, gesture controls, tongue action controls, even electromyographic signal control and the way such as brain idea signal control then can be used in disabled degree higher, the crowd that limbs dynamic role is lower.Designer can install various man-machine interface according to the difference of user's disability degree usually, finally realizes the mutual of number of ways, and provides more reliable, the movement control mode of safety.It should be noted that, the user that those disabled degree are relatively heavier, the service robot having installed light-duty mechanical arm meets their demand more, and this kind of robot can help them to complete complementary actions such as picking up article, enabling, pouring.But in the control of mechanical arm, it is open poor to exist, poor fault tolerance, and autgmentability is poor, the problem of the poor and poor real of interactivity.The existence of these problems makes mechanical arm have a lot of weak point helping the elderly in the work of helping the disabled, and limits mechanical arm and plays its due function.

Carry out controller mechanical arm according to Kinect, beyond thought effect can be reached.If using Kinect as input equipment, so Kinect is as input equipment, when user's arm makes a variety of changes action flexibly, the effect of tracking can be played, the consciousness of such user oneself will naturally be sent in computer, then realizes real-time control to mechanical arm by computer to mechanical arm sending controling instruction.Operator directly can carry out controller mechanical arm by Kinect, simple to operate, directly perceived, and can complete complicated operation.But, can only adopt in the traditional control method indirectly controlled, user needs input control order, and interactivity nature difficulty obtains the approval of user, and both are once comparing, just be very easy to find, control method interactivity in conjunction with Kinect has great raising, and this control method seems direct, and the operating habit of Nature and Man is more identical, its operation is not only simple, and seems very efficient.

Summary of the invention

In view of this, the object of the present invention is to provide a kind of mechanical arm body sense long-range control method based on Kinect and system, the Kinect that the method and system provide using Microsoft is as sensor, and the change of real-time tracking human arm bone, realizes human arm Long-distance Control mechanical arm.

For achieving the above object, the invention provides following technical scheme:

Based on a mechanical arm body sense long-range control method of Kinect, comprise the following steps:

Step one: use the Kinect that provides of Microsoft as sensor in client, the change of real-time human body arm bone site;

Step 2: the data analysis process that client PC will collect, is packaged into the data format of control word, and by Socket communication mode, control word is sent to server end as control command using the result of process;

Step 3: server carries out Data Analysis to the control word received, and analysis result is forwarded to single-chip microcomputer by serial ports;

Step 4: after single-chip microcomputer receives data, produces PWM pulsewidth, exports and controls corresponding steering wheel, and final realization carrys out controller mechanical arm in the mode that body sense is long-range.

Further, in step one, need to follow the trail of human arm bone, set up the 3D coordinate in each joint of human body by process depth of field data, specifically comprise the following steps:

1) first Kinect launches infrared ray, and detects infrared light reflection;

2) calculate the depth value of each pixel in field range, namely obtain depth data;

3) from depth data, object body and shape is extracted;

4) utilize these shape informations to mate the various piece of human body, finally calculate and match the position of each joint in human body.

Present invention also offers a kind of mechanical arm body sense tele-control system based on Kinect, comprise Kinect sensor, client PC, Socket communication module, server and mechanical arm body;

Described Kinect sensor constitutes the acquisition module of body sense tele-control system, for whole control system provides the source of signal; Client PC as client's control end of whole control system, for carrying out analyzing and processing to the signal collected; Socket communication module is used for the communication between client PC and service device; The control word that server sends over for receiving client PC, and control word is carried out Data Analysis, subsequently analysis result is transmitted to serial ports; Mechanical arm body is the controlled device of control system, is used for verifying the realization of body sense Long-distance Control.

Beneficial effect of the present invention is: method and system provided by the present invention does not need special controller, and human body is controller inherently, is a kind of contactless control mode; Meanwhile, utilize the network facilities of Internet to reduce human cost, people can realize the Long-distance Control to mechanical arm in any place that can be linked to Internet, and do not need through on-the-spot.

Accompanying drawing explanation

In order to make object of the present invention, technical scheme and beneficial effect clearly, the invention provides following accompanying drawing and being described:

Fig. 1 is the schematic flow sheet of the method for the invention;

Fig. 2 is the simplification line chart of four-degree-of-freedom manipulator in embodiment;

Fig. 3 is waist joint rotational angle schematic diagram;

Fig. 4 is shoulder joint rotational angle schematic diagram;

Fig. 5 is elbow joint rotational angle schematic diagram;

Fig. 6 is wrist joint rotational angle schematic diagram;

Fig. 7 is the general structure schematic diagram of system of the present invention.

Detailed description of the invention

Below in conjunction with accompanying drawing, the preferred embodiments of the present invention are described in detail.

Fig. 1 is the schematic flow sheet of the method for the invention, and as shown in the figure, mechanical arm body sense long-range control method of the present invention, comprises the following steps:

Step one: use the Kinect that provides of Microsoft as sensor in client, the change of real-time human body arm bone site;

Step 2: the data analysis process that client PC will collect, is packaged into the data format of control word, and by Socket communication mode, control word is sent to server end as control command using the result of process;

Step 3: server carries out Data Analysis to the control word received, and analysis result is forwarded to single-chip microcomputer by serial ports;

Step 4: after single-chip microcomputer receives data, produces PWM pulsewidth, exports and controls corresponding steering wheel, and final realization carrys out controller mechanical arm in the mode that body sense is long-range.

Below by specific embodiment, this method is described in detail:

Mechanical arm kinematics analysis:

Robot kinematics's the name of the game is the problem of the relation of research each joint variable of robot and end effector pose.Robot kinematics's problem can be divided into two class problems: a class is positive kinematics problem, and namely the displacement in known each execution joint, asks the pose of corresponding end effector; Another kind of is Inverse Kinematics Problem, i.e. the pose of known end effector, asks the displacement in corresponding each execution joint.

Any two adjacent links attached body transformation of coordinates battle arrays:

$\begin{array}{c}T_{n+1}^n=A_{n+1}=\mathrm{Rot}(z,{\mathrm{\θ}}_{n+1})\×\mathrm{Trans}(\mathrm{0,0},d_{n+1})\×\mathrm{Trans}(a_{n+1},\mathrm{0,0})\×\mathrm{Rot}(x,{\mathrm{\α}}_{n+1})=\\ \left[\begin{array}{cccc}C{\mathrm{\θ}}_{n+1}& -S{\mathrm{\θ}}_{n+1}& 0& 0\\ S{\mathrm{\θ}}_{n+1}& C{\mathrm{\θ}}_{n+1}& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_{n+1}\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{cccc}1& 0& 0& a_{n+1}\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]*\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& C{a}_{n+1}& -S{a}_{n+1}& 0\\ 0& S{a}_{n+1}& C{a}_{n+1}& 0\\ 0& 0& 0& 1\end{array}\right]=\\ \left[\begin{array}{cccc}C{\mathrm{\θ}}_{n+1}& -S{\mathrm{\θ}}_{n+1}C{\mathrm{\α}}_{n+1}& S{\mathrm{\θ}}_{n+1}S{\mathrm{\α}}_{n+1}& a_{n+1}C{\mathrm{\θ}}_{n+1}\\ S{\mathrm{\θ}}_{n+1}& C{\mathrm{\θ}}_{n+1}C{\mathrm{\α}}_{n+1}& -C{\mathrm{\θ}}_{n+1}S{\mathrm{\α}}_{n+1}& a_{n+1}S{\mathrm{\θ}}_{n+1}\\ 0& S{\mathrm{\α}}_{n+1}& C{\mathrm{\α}}_{n+1}& d_{n+1}\\ 0& 0& 0& 1\end{array}\right]\end{array}$

Such as, the conversion between the joint 2 of general robot and joint 3 can be reduced to:

$\left[\begin{array}{cccc}C{\mathrm{\θ}}_3& -S{\mathrm{\θ}}_{3}C{a}_3& S{\mathrm{\θ}}_{3}S{a}_3& a_{3}C{\mathrm{\θ}}_3\\ S{\mathrm{\θ}}_3& C{\mathrm{\θ}}_{C}C{a}_3& -C{\mathrm{\θ}}_{3}S{a}_3& a_{3}S{\mathrm{\θ}}_3\\ 0& S{a}_3& C{a}_3& d_3\\ 0& 0& 0& 1\end{array}\right]$

Kinematics model is set up:

Manipulator selects joint and 4 connecting rods to form by 4, according to D-H representation, sets up link rod coordinate system to four-degree-of-freedom Serial manipulator, as shown in Figure 2, and fills in the D-H parameter list in each joint of robot, as table 1.

Table 1 D-H parameter list

Connecting rod i	θ	d	a	α
					1	θ 1	0	0	0
2	θ 2	d 1	0	90
					3	θ 3	0	a 2	0

4

θ 4

0

a 3

90

Illustrate any two reference frames above to describe with certain homogeneous transform matrix, comprising two spin matrixs, two translation matrix.According to D-H coordinate system transformation method, by parameter in table 1 respectively in people's formula, each connecting rod transformation matrix can be tried to achieve.Each connecting rod transformation matrix is multiplied, total transformation matrix can be obtained, i.e. Forward Kinematics Analysis.

Just can obtain total transformation matrix by four matrixes that the right side takes advantage of expression four to move successively, finally write out total transformation matrix of robot according to following formula.

$A_1^{0}A_2^{1}A_3^{2}A_4^3=T_4=\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]$

Wherein, robot location and attitude equation are, in formula: s ₁, s ₂, s ₃, s ₄; c ₁, c ₂, c ₃, c ₄represent sin θ respectively ₁, sin θ ₂, sin θ ₃, sin θ ₄; Cos θ ₁, cos θ ₂, cos θ ₃, cos θ ₄.

n _x＝-s ₂₃c ₁c ₄+s ₁s ₄，n _y＝-s ₂₃s ₁c ₄-c ₁s ₄，n _z＝c ₂₃c ₄，o _x＝s ₂₃c ₁s ₄+s ₁c ₄，o _y＝s ₂₃s ₁s ₄-c ₁c ₄，o _z＝-c ₂₃s ₄，a _x＝c ₂₃c ₁，a _z＝s ₂₃，p _x＝(c ₂₃a ₃+c ₂a ₂)c ₁，p _y＝(c ₂₃a ₃+c ₂a ₂)s ₁，p _z＝s ₂₃a ₃+s ₂a ₂+d ₁。

Skeletal joint information analysis process:

Obtain the detailed process of skeleton information.The depth of field data limited use that Kinect produces, utilize Kinect to create truly mutual, also need, on the basis of depth of field data, to extract skeleton information, and this process is exactly that bone is followed the trail of.Bone tracer technique sets up the 3D coordinate in each joint of human body by process depth of field data.Detailed process is: first Kinect launches infrared ray, and detects infrared light reflection, thus can calculate the depth value of each pixel in field range, and namely obtain depth data, what extract at first from depth data is object body and shape.Then mate the various piece of human body by these shape informations, finally calculate the position of each joint in human body of mating out.

Kinect can follow the trail of at most human body 20 skeleton points, according to the needs of controlled device, only adopts the information of these 5 bone nodes of ShoulderLeft, ShoulderRight, ElbowRight, WristRight, HandRight wherein.

(1) waist joint rotational angle

Obtain the rotational angle in human lumbar joint, the three-dimensional coordinate of these two artis of ShoulderLeft and ShoulderRight (x, y, z) can be utilized to calculate.If the three-dimensional coordinate of ShoulderLeft is (x ₁, y ₁, z ₁), the three-dimensional coordinate of ShoulderRight is (x ₂, y ₂, z ₂), these two bone nodes form straight line in solid space, are set to l ₁.Because waist joint y coordinate when rotating is constant, therefore only consider xoz plane, this linear equation is as shown in formula (1).

z＝k ₁x+b ₁(1)

Wherein b ₁due to waist joint rotational angle calculate in less than, so locate non-formulas for calculating.L ₁θ is set to the angle of x-axis ₁, as shown in Figure 3, θ ₁be the rotational angle in human lumbar joint, computing formula is shown below:

${\mathrm{\θ}}_1=\arctan k_1=\arctan \left(\frac{z_2-z_1}{x_2-x_1}\right),x_2\≠x_1---\left(2\right)$

(2) shoulder joint rotational angle

Obtain the rotational angle of human body shoulder joint, the three-dimensional coordinate of these two artis of ShoulderRight and ElbowRight (x, y, z) can be utilized to calculate.If the three-dimensional coordinate of ElbowRight is (x ₃, y ₃, z ₃), these two bone nodes form straight line in solid space, are set to l ₂.Because shoulder joint z coordinate when rotating is constant, therefore only consider xoy plane, this linear equation as shown in formula (3),

y＝k ₂x+b ₂(3)

Wherein b ₂due to shoulder joint rotational angle calculate in less than, so locate non-formulas for calculating.L ₂θ is set to the angle of y-axis ₂, as shown in Figure 4, θ ₂be the rotational angle of human body shoulder joint, computing formula is shown below:

$\left\{\begin{array}{c}{\mathrm{\θ}}_2=\arctan k_2=\arctan \left(\frac{y_3-y_2}{x_3-x_2}\right),x_3\≠x_2\\ {\mathrm{\θ}}_2=0,x_3=x_2\end{array}\right.---\left(4\right)$

(3) elbow joint rotational angle

Obtain the rotational angle of human elbow, the three-dimensional coordinate of these three artis of ShoulderRight, ElbowRight and WristRight (x, y, z) can be utilized to calculate.If the three-dimensional coordinate of WristRight is (x ₄, y ₄, z ₄), these three bone nodes form a triangle in solid space, as shown in Figure 5, required by this leg-of-mutton supplementary angle is.Because these three joints z coordinate when rotating is constant, therefore only consider xoy plane, the wherein straight line l of ShoulderRight and ElbowRight formation ₂introduced at last joint, the straight line that ElbowRight and WristRight is formed is set to l ₃, equation as shown in formula (5),

y＝k ₃x+b ₃(5)

Wherein b ₃due to elbow joint rotational angle calculate in less than, so locate non-formulas for calculating.L ₃with l ₄angle be set to θ ₃, as shown in Figure 5, θ ₃be the rotational angle of human elbow, computing formula is shown below:

${\mathrm{\θ}}_3=\arctan \left|\frac{k_2-k_3}{1+k_2{k}_3}\right|,k_2{k}_3\≠-1---\left(6\right)$

(4) wrist joint rotational angle

Obtain the carpal rotational angle of human body, the three-dimensional coordinate of these three artis of ElbowRight, WristRight and HandRight (x, y, z) can be utilized to calculate.If the three-dimensional coordinate of HandRight is (x ₅, y ₅, z ₅), these three bone nodes form a triangle in solid space, required by this leg-of-mutton supplementary angle is.Because these three joints z coordinate when rotating is constant, therefore only consider xoy plane, the wherein straight line l of ElbowRight and WristRight formation ₃introduced above, the straight line that WristRight and HandRight is formed is set to l ₄, equation as shown in formula (7),

y＝k ₄x+b ₄(7)

Wherein b ₄due in calculating at carpal rotational angle with less than, so locate non-formulas for calculating.L ₃with l ₄angle be set to θ ₄, as shown in Figure 6, θ ₄be the carpal rotational angle of human body, computing formula is shown below:

${\mathrm{\θ}}_4=\arctan \left|\frac{k_3-k_4}{1+k_3{k}_4}\right|,k_3{k}_4\≠-1---\left(8\right)$

In addition, in the present invention, in order to how monitor mechanical arm under emergency case, native system adds Long-distance Control, solves this problem well.The elementary object of Long-distance Control is: user by server access Long-distance Control object (i.e. mechanical arm), sends server to the control command of mechanical arm to by Socket communication at remote terminal (i.e. client).Having the following advantages relative to traditional remote measuring and controlling system: do not need special controller, human body is controller inherently, is a kind of contactless control mode.Utilize the network facilities of Internet to reduce human cost, people can realize the Long-distance Control to mechanical arm in any place that can be linked to Internet, and do not need through on-the-spot, and Fig. 7 is the general structure schematic diagram of system of the present invention.

Native system adopts AVR single chip process to receive the control word data of host computer, and control word agreement is as shown in the table.

Table 2 servos control protocol format

1	2	3	4	5	6
						Prefix	Address	State/frame length	Command word	Servos control	School Affairs

0x550xaa

0x71

0x18

0x01

Steering wheel angle, speed

Last position

What finally illustrate is, above preferred embodiment is only in order to illustrate technical scheme of the present invention and unrestricted, although by above preferred embodiment to invention has been detailed description, but those skilled in the art are to be understood that, various change can be made to it in the form and details, and not depart from claims of the present invention limited range.

Claims (3)

1., based on a mechanical arm body sense long-range control method of Kinect, it is characterized in that: comprise the following steps:

Step one: use the Kinect that provides of Microsoft as sensor in client, the change of real-time human body arm bone site;

Step 2: the data analysis process that client PC will collect, is packaged into the data format of control word, and by Socket communication mode, control word is sent to server end as control command using the result of process;

Step 3: server carries out Data Analysis to the control word received, and analysis result is forwarded to single-chip microcomputer by serial ports;

Step 4: after single-chip microcomputer receives data, produces PWM pulsewidth, exports and controls corresponding steering wheel, and final realization carrys out controller mechanical arm in the mode that body sense is long-range.

2. a kind of mechanical arm body sense long-range control method based on Kinect according to claim 1, it is characterized in that: in step one, need to follow the trail of human arm bone, set up the 3D coordinate in each joint of human body by process depth of field data, specifically comprise the following steps:

1) first Kinect launches infrared ray, and detects infrared light reflection;

2) calculate the depth value of each pixel in field range, namely obtain depth data;

3) from depth data, object body and shape is extracted;

4) utilize these shape informations to mate the various piece of human body, finally calculate and match the position of each joint in human body.

3., based on a mechanical arm body sense tele-control system of Kinect, it is characterized in that: comprise Kinect sensor, client PC, Socket communication module, server and mechanical arm body;

Described Kinect sensor constitutes the acquisition module of body sense tele-control system, for whole control system provides the source of signal; Client PC as client's control end of whole control system, for carrying out analyzing and processing to the signal collected; Socket communication module is used for the communication between client PC and service device; The control word that server sends over for receiving client PC, and control word is carried out Data Analysis, subsequently analysis result is transmitted to serial ports; Mechanical arm body is the controlled device of control system, is used for verifying the realization of body sense Long-distance Control.