CN106503426A - A kind of body language detection and tracking system method for designing towards medical robot - Google Patents
A kind of body language detection and tracking system method for designing towards medical robot Download PDFInfo
- Publication number
- CN106503426A CN106503426A CN201610884559.4A CN201610884559A CN106503426A CN 106503426 A CN106503426 A CN 106503426A CN 201610884559 A CN201610884559 A CN 201610884559A CN 106503426 A CN106503426 A CN 106503426A
- Authority
- CN
- China
- Prior art keywords
- data
- byte
- module
- attitude
- information
- 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
Links
Classifications
-
- G—PHYSICS
- G16—INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR SPECIFIC APPLICATION FIELDS
- G16H—HEALTHCARE INFORMATICS, i.e. INFORMATION AND COMMUNICATION TECHNOLOGY [ICT] SPECIALLY ADAPTED FOR THE HANDLING OR PROCESSING OF MEDICAL OR HEALTHCARE DATA
- G16H40/00—ICT specially adapted for the management or administration of healthcare resources or facilities; ICT specially adapted for the management or operation of medical equipment or devices
- G16H40/60—ICT specially adapted for the management or administration of healthcare resources or facilities; ICT specially adapted for the management or operation of medical equipment or devices for the operation of medical equipment or devices
- G16H40/63—ICT specially adapted for the management or administration of healthcare resources or facilities; ICT specially adapted for the management or operation of medical equipment or devices for the operation of medical equipment or devices for local operation
Landscapes
- Health & Medical Sciences (AREA)
- Engineering & Computer Science (AREA)
- Biomedical Technology (AREA)
- Business, Economics & Management (AREA)
- General Business, Economics & Management (AREA)
- Epidemiology (AREA)
- General Health & Medical Sciences (AREA)
- Medical Informatics (AREA)
- Primary Health Care (AREA)
- Public Health (AREA)
- Arrangements For Transmission Of Measured Signals (AREA)
Abstract
A kind of the present invention relates to machine vision and artificial intelligence field and limb action identification field, more particularly to body language detection and tracking system method for designing towards medical robot.Including:(1) overall structure of design system;(2) the slave computer module of design system;Type selecting is carried out to attitude transducer, main control chip, communication device, and carries out lower computer hardware design and slave computer software design respectively;(3) upper computer module of design system:Receiving the information that sends of slave computer, the information to receiving carries out attitude algorithm, shows that curve, 3D draw.The present invention has fully taken into account the impact that one hand control scalpel of operation Shi doctor, one hand control endoscope is caused to operation using the Orientation differences of the attitudes vibration control endoscope of doctor foot, the success rate of operation is at utmost improve, the operation burden of doctor is alleviated.
Description
Technical field
The invention belongs to machine vision and artificial intelligence field and limb action identification field, more particularly to one kind is towards doctor
Treat body language detection and the tracking system method for designing of robot.
Background technology
With the continuous progress of the booming and automation subject of national healthcare hygiene industry, with automation theory as core
The medical auxiliary apparatus of the heart emerge in an endless stream.In recent years, some medical apparatus, such as clinical thermometer, blood-glucose meter, sterilamp, scalpel,
High electrograph therapy instrument etc., is all constantly improving, towards intelligent, miniaturization and portability development, is being that doctor and patient provide
Great convenience.
In emerging medicine equipment, the automated machine aided in operation accounts for significant proportion.Due to majority
Surgical time is long, and intensity is big, and required precision is high, has very high requirement to the technology and muscle power of operating doctor, and sharp
The related equipment manufactured with Theory of Automatic Control, while precision is not affected, can work long hours, and can compare perfect
Similar Problems are solved, mitigates the burden of doctor, the also rehabilitation for sufferer has added a guarantee.This is a part for medical advance,
It is also the classical case of automation subject practical application simultaneously.
For the surgical operation for needing to use endoscope, traditional method usually needs to control endoscope with a hand
Situation, which limits movement range and the precision of the hand of doctor's another manipulation scalpel, increased the consumption of doctor's muscle power,
Increased the risk of related surgical to a certain extent, patient is reduced by the chance of successful treatment.The present invention is based on man-machine friendship
Mutually technology and Theory of Automatic Control, by the posture information of the foot with sensor collection doctor, and recognize some specific appearances
State change and its combination in certain sequence, complete the control to endoscope, on the premise of ensureing that operation can be smoothly completed,
Alleviate the burden of operator doctor, it is achieved that we facilitate doctor, benefit the target of patient.
Content of the invention
It is an object of the invention to provide a kind of body language detection and tracking system design side towards medical robot
Method.
The object of the present invention is achieved like this:
(1) overall structure of design system;
Total system is divided into two big functional modules, is slave computer module and upper computer module respectively;
Slave computer module is made up of main control chip, attitude transducer and communication device;After upper electricity, main control chip is to each
Individual part is initialized, and reads data with fixed frequency afterwards from attitude transducer, after simple smoothing is passed through,
By communication module, with set communication protocol, it is transmitted to host computer;
Upper computer module is created by communication module, attitude algorithm module, curve display module, 3D model creation modules, bone
Modeling block, bone conversion module, 3D show and update module composition;Top during startup using 3D model creations module to 3D models
Point, pinup picture, endoskeleton fitting information are initialized, and are imported renderer to be used, and are created the environment of drawing;Pass through afterwards
Communication module is communicated with slave computer, after the attitude information that slave computer is transmitted is received, is passed through both stationary interfaces
Give attitude algorithm module;By relative theory, the initial data solution that slave computer is transmitted is counted as required attitude algorithm module
Attitude information, and data are passed to curve display module and bone conversion module further;Curve display module is real by data
When data form show;Bone conversion module will be done to bone according to attitude information, depth-first traversal bone tree
Posture changing calculated, bone posture information is passed to 3D update modules afterwards;3D update modules are according to the bone for receiving
Posture information, most result is shown on the screen of computer in the form of 3D models at last;
(2) the slave computer module of design system;Type selecting, and difference is carried out to attitude transducer, main control chip, communication device
Carry out lower computer hardware design and slave computer software is designed;
Attitude transducer adopts six axle tracking transducer MPU-6050, main control chip to adopt STM32F103C8T6 single-chip microcomputers,
Communication module is communicated using wireless serial;
Wherein MPU-6050 is fixed on the pin of doctor, gathers attitude information, and single-chip microcomputer STM32F103C8T6 is responsible for MPU-
6050 initial work, and data are read from MPU-6050, host computer is sent to by wireless serial;Use
VCC the and GND interfaces of the SWD interface downloaders of STM32F103C8T6 complete to power, and MPU-6050 and wireless serial are connected to
On STM32F103C8T6, introduce TPS76833 and power;
The Software for Design of system slave computer includes the initialization of STM32F103C8T6 and MPU-6050, the number of MPU-6050
According to reading, host computer is sent data to by wireless serial;
The initialization of STM32F103C8T6 mainly completes four steps:
(2.1) the SysTick timers of initialization system, will trigger per the 1m seconds after being allowed to start and once interrupt;
(2.2) initialization of serial ports:Start the equipment clock of GIPOA and USART1, GPIOA9 and GPIOA10 are set to multiple
With recommending output mode pattern, floating input pattern;Its baud rate is set to 9600 by configuration USART1, and data bit is 8,1 stopping
Position, no parity, without hardware controls, using data receiver and data is activation;
(2.3) by the output redirection of the printf () in C java standard libraries to USART1;
(2.4) initialization of I2C communications:Start the clock of GPIOB and I2C1, it is defeated that GPIOB6 and GPIOB7 is set to multiplexing leakage
The mode of operation of exit pattern I2C1 is set to I2C, and when low-level data changes, the Duty of SCL lines is 0.33, unlatching acknowledgement messaging, and 7
Bit address, setting Self address are 0xB;
The initialization of MPU-6050 is that internal register is configured, and wherein register PWR_MGMT_1 is set to
0x00, register SAMPLE RATE DIVIDER are set to 0x07, and register CONFIG is 0x06, register ACCELEROMETER
CONFIGURATION is set to 0x01, and register GYROSCOPE CONFIGURATION are set to 0x18;Read MPU-6050's
WHO_AM_I registers, normally start if its value is 0x68, otherwise reinitialize;Configure MPU-6050 and examine correct
Look into errorless after, start read MPU-6050 data;
Read the method for the data using poll of MPU-6050, every 100ms reading primary acceleration meters and turn meter
Data, wherein MPU-6050 measurements obtain acceleration information and angular velocity data respectively occupies six continuous registers, temperature
The measured value of sensor occupies two registers;
It is to be realized by formulating related communication protocol by wireless serial by slave computer data is activation to host computer;
Transmission including acceleration:First character section represents the beginning of packet for 0x55;Second byte is 0x51, table
Show in this packet storage is acceleration information;3rd byte is AxL, represents the least-significant byte of accelerometer X-axis data;
4th byte is AxH, represents the most-significant byte of speedometer X-axis data;5th byte is AyL, represents accelerometer Y-axis data
Least-significant byte;6th byte is AyH, represents the most-significant byte of accelerometer Y-axis data;7th byte is AzL, represents acceleration
The least-significant byte of meter Z axis data;8th byte is AzH, represents the most-significant byte of accelerometer Z axis data;9th is TL, is temperature
The least-significant byte of information;Tenth is TH, is the most-significant byte of temperature information;11st is, check bit, and its value is above all bytes
Remainder of the sum to 0x100;The transmission of angular speed:First character section represents the beginning of packet for 0x55;Second byte be
0x52, represent in this packet storage is angular velocity data;3rd byte is WxL, represents turn meter X-axis data
Least-significant byte;4th byte is WxH, represents the most-significant byte of speedometer X-axis data;5th byte is WyL, represents turn meter Y
The least-significant byte of number of axle evidence;6th byte is WyH, represents the most-significant byte of turn meter Y-axis data;7th byte is WzL, represents
The least-significant byte of turn meter Z axis data;8th byte is WzH, represents the most-significant byte of turn meter Z axis data;9th is TL,
Least-significant byte for temperature information;Tenth is TH, is the most-significant byte of temperature information;11st is, check bit, and its value is above institute
There is remainder of the byte sum to 0x100;
(3) upper computer module of design system:Receiving the information that sends of slave computer, the information to receiving carries out attitude
Resolve, show that curve, 3D draw;
Serial communication modular by means of Qt5 SerialPort storehouses completing, be responsible for communicating with slave computer, gone here and there by monitoring
Data are parsed by the data that mouth sends according to set communication protocol, and sending the data to attitude algorithm module carries out appearance
State is resolved;Serial port module is made up of two parts:Part I SerialDialog classes, are responsible in UI to the setting of serial ports and man-machine
Interaction, interface are as shown in Figure 6;Part II is SerialThread classes, is responsible in another thread monitoring serial ports in real time,
And which is parsed according to communication protocol;The method that serial data parsing is adopted is the team set up in a similar data structure
The buffering area of row, receives the afterbody that data are just added to buffering area every time, begins look for from the head of buffering area afterwards
Whether the beginning of communication protocol, after finding beginning, judge the length of the ending for arriving buffering area from there more than or equal to communication protocols
The length of view, if meeting this condition, just from the beginning of starting in buffering area, takes out the word of communication protocol specific length from agreement
Joint number group, and buffering area beginning is deleted to the data for taking out array end position, repeat this action afterwards, until buffering area
In without communication protocols justice beginning, or length thereafter less than agreement length, jump out, wait receive data next time;
Attitude algorithm module is the soul in host computer procedure, and the data obtained from serial port module are carried out attitude algorithm,
And continue to pass data to curve display module and 3D display module;Attitude algorithm is according to 3-axis acceleration, three shaft angles speed
Spend what the theoretical calculation of quaternary number was completed, attitude matrix is represented with quaternary number:
Solve attitude and solve attitude matrix, be equivalent to the renewal of quaternary number, and the renewal of quaternary number can pass through three shaft angles
Velocity interpolation, concretely comprises the following steps:
(3.1) three axis angular rates are obtained, and represents three axles of angular speed with gx, gy, gz respectively, obtained using the method for integration
Attitude is obtained, gravity direction is estimated according to quaternary number before this;
(3.2) value of accelerometer is obtained from gyroscope, and represents 3-axis acceleration respectively with ax, ay, az.Carry out
Before attitude algorithm, which is standardized;
(3.3) using complementary filter algorithm fusion angular speed and accekeration and error is asked for, by this error through ratio
With the post-compensation of integral element in the middle of the measured value of angular speed;
(3.4) single order runge kutta method updates quaternary number;
Host computer curve display module uses the third party of Qt to increase income storehouse QCustomPlot, curvilinear function initial
Change and data renewal function is located in class MainWindow of main interface;Show that three axles add in the host computer procedure main interface of system
Rate curve, three axis angular rate curves, four curves of attitude angular curve and quaternary number curve, the drafting of curve is and main interface UI
Renewal together carry out;
3D display module is responsible for the loading of 3D modules, the calculating and application of bone conversion, the renewal that model shows;Its middle mold
Type is loaded into summit, pinup picture and the bone information for using Assimp to increase income storehouse importing 3D models, using OpenGL to importing
Information is rendered;The system constructs 33 pieces of bones in modeling using Blender modeling softwares, and its change information is deposited
Storage is in data-structure tree;The method of 3D model modifications is:First summit of 3D models different bones have all been bundled in
On, different bones are represented using weight to the influence degree on same summit, and when corresponding bone enters line translation, by its shadow
Also corresponding response can be made according to weight in loud summit.By controlling the motion of bone, the purpose for making model sport is realized.
The beneficial effects of the present invention is:
(1) operation Shi doctor one has been fully taken into account using the Orientation differences of the attitudes vibration control endoscope of doctor foot
The impact that one hand control endoscope of hand control scalpel is caused to operation, at utmost improves the success rate of operation, alleviates
The operation burden of doctor.
(2) considering, directly endoscope being controlled by foot action can increase the inconvenience of doctor, cause certain shadow to operation
Ring, design host computer 3D models directly reappear foot action and curvilinear motion reflection attitudes vibration, and then control endoscope, enter one
Step reduces the complexity of operation, improves success rate.
Description of the drawings
Fig. 1 overall system architecture block diagrams;
Fig. 2 system lower computer hardware structural design drawings;
Fig. 3 system lower computer hardware pictorial diagrams;
Fig. 4 system slave computer software flow charts;
Fig. 5 system host computer general structures;
Fig. 6 serial ports correlation windows;
Fig. 7 serial ports executes flow process;
Fig. 8 curve display interfaces;
Fig. 9 3D display interfaces;
Figure 10 bone trees.
Specific embodiment
Illustrate below in conjunction with the accompanying drawings and the present invention is described in more detail:
This method achieves human body attitude control medical machinery and is performed the operation, and solves in the surgery for needing to use endoscope
In operation, doctor needs a hand control scalpel, the problem of a hand control endoscope.The operation that doctor not only can so be mitigated is born
Load, while also improve the success rate of operation.
The design of the present invention is realized by below scheme:
(1) overall structure of design system.
(2) the slave computer module of design system.Type selecting, and difference is carried out to attitude transducer, main control chip, communication device
Carry out lower computer hardware design and slave computer software is designed.
(3) upper computer module of design system, needs the work for carrying out to have:Receive information, docking that slave computer sends
The information for receiving carries out attitude algorithm, shows that curve, 3D draw.
Operation Shi doctor has been fully taken into account on the other hand using the Orientation differences of the attitudes vibration control endoscope of doctor foot
The impact that control one hand control endoscope of scalpel is caused to operation, at utmost improves the success rate of operation, alleviates doctor
Raw operation burden.
Consideration directly controls endoscope by foot action can increase the inconvenience of doctor, and operation is affected,
Design host computer 3D models directly reappear foot action and curvilinear motion reflection attitudes vibration, and then control endoscope, further
The complexity of operation is reduced, success rate is improve.
Body language detection towards medical robot is designed with tracking system, mainly includes two big modules:Slave computer mould
Block and upper computer module.It is characterized in:For the surgical operation for needing endoscope, hand is replaced using the attitudes vibration of doctor foot
The orientation of dynamic operational control endoscope;For making system not affect the change of whole surgical procedure, foot's attitude to adopt host computer 3D
Model and curvilinear motion show.
A kind of body language detection towards medical robot proposed by the present invention specifically includes following several with tracking system
Individual step.
Step one:The overall structure of design system.
The overall structure block diagram of system is as shown in Figure 1.Total system is functionally divided into two big functional module compositions,
It is slave computer module and upper computer module respectively.
Slave computer is made up of main control chip, attitude transducer and communication device.Main working method be upper electricity after,
Main control chip is initialized to all parts, reads data with fixed frequency afterwards from attitude transducer, through letter
Single smooth after, by communication module, with set communication protocol, be transmitted to host computer.
Host computer is functionally divided into communication module, attitude algorithm module, curve display module, 3D model creation modules,
Bone creation module, bone conversion module, 3D show and the composition such as update module.Main working method is:Use during startup
3D model creations module is initialized to information such as the summit of 3D models, pinup picture, endoskeleton fittings, and imported renders
Device, and create the environment of drawing;Communicated with slave computer by communication module afterwards, whenever receiving what slave computer was transmitted
After attitude information, passed through both stationary interfaces and given attitude algorithm module;Attitude algorithm module passes through relative theory, by bottom
The initial data solution that machine is transmitted is counted as required attitude information, and data are passed to curve display module and bone conversion further
Module;The form of data real time data is shown by curve display module;Bone conversion module is according to attitude information, depth
First traversal bone tree, the posture changing to be done to bone calculate, and bone posture information is passed to 3D afterwards and updates mould
Block;According to the bone posture information for receiving, most result shows the screen in computer in the form of 3D models to 3D update modules at last
On.
Step 2:The slave computer module of design system.Type selecting is carried out to attitude transducer, main control chip, communication device, and
Carry out lower computer hardware design respectively and slave computer software is designed.
Consider experiment demand and economic problems, six axle tracking transducer MPU-s of the attitude transducer using low-power consumption
6050, main control chip adopts the STM32F103C8T6 single-chip microcomputers of feature richness and simple structure, communication module to use wireless serial
Communicated.
The lower computer hardware structural design drawing of system is as shown in Fig. 2 hardware pictorial diagram is as shown in Figure 3.Wherein MPU-6050
It is fixed on the pin of doctor, gathers attitude information, single-chip microcomputer STM32F103C8T6 is responsible for the initial work of MPU-6050, and
Data are read from MPU-6050, after simple process, host computer is sent to by wireless serial.System is respectively adopted two kinds not
Same power supply mode is applied to the debugging in development process and practical application.Due to system hardware structure during actual development
Simply, power consumption is little and electric current does not have too great fluctuation process substantially, the simple SWD interface downloaders using STM32F103C8T6
VCC and GND interfaces can complete to power, MPU-6050 and wireless serial are connected on STM32F103C8T6, it is not necessary to extra
Increase supply module.Such connected mode, simple and convenient, workload is substantially zeroed.But in actual applications, it is contemplated that the next
On the confidential pin for being fixed on doctor, together move with the pin of doctor, it is impossible to using wired power supply mode, therefore introduce
TPS76833 powers.
The Software for Design flow chart of system slave computer is as shown in figure 4, mainly include STM32F103C8T6 and MPU-6050's
Initialization, the reading of the data of MPU-6050 send data to the steps such as host computer by wireless serial.
The initialization of STM32F103C8T6 mainly completes four steps:
(1) the SysTick timers of initialization system, will trigger per the 1m seconds after being allowed to start and once interrupt.
(2) initialization of serial ports:Start GIPOA and USART1 (i.e. when GPIOA9 and GPIOA10 are multiplexed with serial port function pair
The serial ports that answers) equipment clock, by GPIOA9 and GPIOA10 be set to be multiplexed recommending output mode pattern, floating input pattern;Configuration
Its baud rate is set to 9600 by USART1, and data bit is 8,1 stop position, and no parity, without hardware controls, using data
Receive and data is activation.
(3) by the output redirection of the printf () in C java standard libraries to USART1.
(4) initialization of I2C communications:Start the clock of GPIOB and I2C1, GPIOB6 and GPIOB7 is set to multiplexing leakage output
The mode of operation of pattern I2C1 is set to I2C, and when low-level data changes, the Duty of SCL lines is 0.33, unlatching acknowledgement messaging, 7
Address, setting Self address are 0xB.
The initialization of MPU-6050 is mainly configured to internal register, and wherein register PWR_MGMT_1 is arranged
For 0x00, register SAMPLE RATE DIVIDER are set to 0x07, and register CONFIG is 0x06, register
ACCELEROMETER CONFIGURATION are set to 0x01, and register GYROSCOPE CONFIGURATION are set to 0x18.Connect
The WHO_AM_I registers for reading MPU-6050, is normally started if its value is 0x68, otherwise explanation there is a problem, need
Reinitialize.In correct configuration MPU-6050 and check errorless after, you can start the data for reading MPU-6050.
The data for reading MPU-6050 can adopt interruption method or polling method, be to realize that conveniently, system uses wheel
The method of inquiry, reads the data of primary acceleration meter and turn meter per 100ms, and wherein MPU-6050 measurements obtain accelerating the number of degrees
Six continuous registers are respectively occupied according to angular velocity data, and the measured value of temperature sensor occupies two registers.
It is to be realized by formulating related communication protocol by wireless serial by slave computer data is activation to host computer, tool
Body is described as:(1) transmission of acceleration:First character section represents the beginning of packet for 0x55;Second byte is 0x51, table
Show in this packet storage is acceleration information;3rd byte is AxL, represents the least-significant byte of accelerometer X-axis data;
4th byte is AxH, represents the most-significant byte of speedometer X-axis data;5th byte is AyL, represents accelerometer Y-axis data
Least-significant byte;6th byte is AyH, represents the most-significant byte of accelerometer Y-axis data;7th byte is AzL, represents acceleration
The least-significant byte of meter Z axis data;8th byte is AzH, represents the most-significant byte of accelerometer Z axis data;9th is TL, is temperature
The least-significant byte of information;Tenth is TH, is the most-significant byte of temperature information;11st is, check bit, and its value is above all bytes
Remainder of the sum to 0x100.(2) transmission of angular speed:First character section represents the beginning of packet for 0x55;Second byte
For 0x52, represent storage in this packet is angular velocity data;3rd byte is WxL, represents turn meter X-axis data
Least-significant byte;4th byte is WxH, represents the most-significant byte of speedometer X-axis data;5th byte is WyL, represents turn meter
The least-significant byte of Y-axis data;6th byte is WyH, represents the most-significant byte of turn meter Y-axis data;7th byte is WzL, table
Show the least-significant byte of turn meter Z axis data;8th byte is WzH, represents the most-significant byte of turn meter Z axis data;9th is
TL, is the least-significant byte of temperature information;Tenth is TH, is the most-significant byte of temperature information;11st is, check bit, and its value is front
Remainder of all byte sums in face to 0x100.
Step 3:The upper computer module of design system, needs the work for carrying out to have:Receive slave computer send information,
Attitude algorithm is carried out to the information for receiving, show that curve, 3D draw.
Master system occupies vital status in the realization of total system, essentially, whole slave computer system
System simply act as the role of a sensor for host computer.Accurately foot's attitude is detected and is tracked
Mainly by the attitude algorithm module of host computer, Drawing of Curve and 3D model realizations.Therefore, the design of host computer need to have
Module coupling degree is low, performance is good, the characteristics of facilitate easy-to-use.
The master system of system is mainly completed using C Plus Plus, uses Qt as UI solutions, and OpenGL is used as 3D
Draw and support, for convenience of programming, also use the STL java standard libraries of C++, and the open source software such as QCustomPlot and Assimp.
For improving the operational efficiency of program, host computer many places have used multithreading.
The general structure of host computer design is as shown in Figure 5.
In figure, serial communication modular is responsible for communicating with slave computer mainly by means of the SerialPort storehouses of Qt5 completing, logical
The data for monitoring that serial ports sends are crossed, data is parsed according to set communication protocol, is sent the data to attitude algorithm mould
Block carries out attitude algorithm.According to demand, serial port module is made up of two parts:Part I SerialDialog classes, are responsible in UI
Setting and man-machine interaction to serial ports, interface are as shown in Figure 6;Part II is SerialThread classes, is mainly responsible in addition
Monitor serial ports in one thread in real time, and which is parsed according to communication protocol.The method that serial data parsing is adopted is to build
The buffering area of the queue in a similar data structure is found, the afterbody that data are just added to buffering area is received every time, it
Begin look for the beginning of communication protocol afterwards from the head of buffering area, after finding beginning, judge the ending for arriving buffering area from there
Length whether more than or equal to the length of communication protocol, if meeting this condition, just from the beginning of starting from agreement in buffering area,
The byte arrays of communication protocol specific length are taken out, and buffering area beginning is deleted to the data for taking out array end position, it
After repeat this action, in buffering area without communication protocols justice beginning, or length thereafter less than agreement length, jump out,
Wait and receive data next time.Overall serial ports flow process is as shown in Figure 7.
Attitude algorithm module is the soul in host computer procedure, and the data obtained from serial port module are carried out attitude algorithm,
And continue to pass data to curve display module and 3D display module.Attitude algorithm is according to 3-axis acceleration, three shaft angles speed
Spend what the theoretical calculation of quaternary number was completed, represent that attitude matrix is as follows with quaternary number:
Solve attitude and solve attitude matrix, the renewal of quaternary number can be equivalent to again, and the renewal of quaternary number can pass through
Three axis angular rates are realized, are concretely comprised the following steps:
(1) three axis angular rates are obtained, and represents three axles of angular speed with gx, gy, gz respectively, obtained using the method for integration
Attitude, estimates to gravity direction according to quaternary number before this;
(2) value of accelerometer is obtained from gyroscope, and represents 3-axis acceleration respectively with ax, ay, az.Carrying out appearance
Before state is resolved, which is standardized;
(3) using complementary filter algorithm fusion angular speed and accekeration and ask for error, by this error through ratio and
The post-compensation of integral element is in the middle of the measured value of angular speed;
(4) single order runge kutta method updates quaternary number.Equation is as follows:
Wherein,
Host computer curve display module uses the third party of Qt to increase income storehouse QCustomPlot, curvilinear function initial
Change and data renewal function is located in class MainWindow of main interface.As shown in figure 8, in the host computer procedure main interface of system
3-axis acceleration curve, three axis angular rate curves, four curves of attitude angular curve and quaternary number curve will be shown, curve is painted
System is together carried out with the renewal of main interface UI, therefore and does not need another ledger line journey.
A part the most complicated in 3D display module master system, the loading of main responsible 3D modules, bone are converted
Calculating and application, the renewal that model shows.Wherein model be loaded into use Assimp increase income storehouse import 3D models summit,
Pinup picture and bone information, are rendered to the information for importing using OpenGL, obtain 3D models such as Fig. 9.The system is in modeling
33 pieces of bones are constructed using Blender modeling softwares, and its change information is stored in data-structure tree, such as Figure 10 institutes
Show.The main method of 3D model modifications is:First summit of 3D models is all bundled on different bones, different bones
Bone represented using weight to the influence degree on same summit, and when corresponding bone enters line translation, the summit being affected by
Corresponding response can be made according to weight.In this manner it is possible to pass through the motion for controlling bone, the mesh for making model sport is realized
's.The present invention completes the collection of doctor foot attitude information by designing slave computer, achieves foot by designing host computer
The accurate detection of attitude information and tracking, and reappear doctor's foot action in real time with 3D models, it is in subsequent mechanical precise control
Sight glass is prepared, and also opens a new page for biomedical research.
Claims (1)
1. a kind of body language towards medical robot is detected and tracking system method for designing, it is characterised in that including as follows
Step:
(1) overall structure of design system;
Total system is divided into two big functional modules, is slave computer module and upper computer module respectively;
Slave computer module is made up of main control chip, attitude transducer and communication device;After upper electricity, main control chip is to each portion
Part is initialized, and reads data with fixed frequency afterwards from attitude transducer, after simple smoothing, is passed through
Communication module, with set communication protocol, is transmitted to host computer;
Upper computer module creates mould by communication module, attitude algorithm module, curve display module, 3D model creation modules, bone
Block, bone conversion module, 3D show and update module composition;During startup using 3D model creations module to the summit of 3D models,
Pinup picture, endoskeleton fitting information are initialized, and are imported renderer to be used, and are created the environment of drawing;Afterwards by logical
Letter module is communicated with slave computer, after the attitude information that slave computer is transmitted is received, is passed through both stationary interfaces and is sent
Give attitude algorithm module;By relative theory, the initial data solution that slave computer is transmitted is counted as required appearance to attitude algorithm module
State information, and data are passed to curve display module and bone conversion module further;Curve display module is by data with real time
The form of data shows;, according to attitude information, depth-first traversal bone tree is to be cooked to bone for bone conversion module
Posture changing is calculated, and bone posture information is passed to 3D update modules afterwards;3D update modules are according to the bone position for receiving
Appearance information, most result is shown on the screen of computer in the form of 3D models at last;
(2) the slave computer module of design system;Type selecting is carried out to attitude transducer, main control chip, communication device, and is carried out respectively
Lower computer hardware design and slave computer software are designed;
Attitude transducer adopts six axle tracking transducer MPU-6050, and main control chip is using STM32F103C8T6 single-chip microcomputers, communication
Module is communicated using wireless serial;
Wherein MPU-6050 is fixed on the pin of doctor, gathers attitude information, and single-chip microcomputer STM32F103C8T6 is responsible for MPU-6050
Initial work, and from MPU-6050 read data, host computer is sent to by wireless serial;Use
VCC the and GND interfaces of the SWD interface downloaders of STM32F103C8T6 complete to power, and MPU-6050 and wireless serial are connected to
On STM32F103C8T6, introduce TPS76833 and power;
The Software for Design of system slave computer includes the initialization of STM32F103C8T6 and MPU-6050, the data of MPU-6050
Read, host computer is sent data to by wireless serial;
The initialization of STM32F103C8T6 mainly completes four steps:
(2.1) the SysTick timers of initialization system, will trigger per the 1m seconds after being allowed to start and once interrupt;
(2.2) initialization of serial ports:Start the equipment clock of GIPOA and USART1, GPIOA9 and GPIOA10 are set to multiplexing and are pushed away
Draw output mode, floating input pattern;Its baud rate is set to 9600 by configuration USART1, and data bit is 8,1 stop position, nothing
Even-odd check, without hardware controls, using data receiver and data is activation;
(2.3) by the output redirection of the printf () in C java standard libraries to USART1;
(2.4) initialization of I2C communications:Start the clock of GPIOB and I2C1, GPIOB6 and GPIOB7 is set to multiplexing leakage output mould
The mode of operation of Formulas I 2C1 is set to I2C, and when low-level data changes, the Duty of SCL lines is 0.33, opens acknowledgement messaging, 7 ground
Location, setting Self address are 0xB;
The initialization of MPU-6050 is that internal register is configured, and wherein register PWR_MGMT_1 is set to 0x00,
Register SAMPLE RATE DIVIDER are set to 0x07, and register CONFIG is 0x06, register ACCELEROMETER
CONFIGURATION is set to 0x01, and register GYROSCOPE CONFIGURATION are set to 0x18;Read MPU-6050's
WHO_AM_I registers, normally start if its value is 0x68, otherwise reinitialize;Configure MPU-6050 and examine correct
Look into errorless after, start read MPU-6050 data;
The method of the data using poll of MPU-6050 is read, and the data of primary acceleration meter and turn meter are read per 100ms,
Wherein MPU-6050 measurements obtain acceleration information and angular velocity data respectively occupies six continuous registers, temperature sensor
Measured value occupy two registers;
It is to be realized by formulating related communication protocol by wireless serial by slave computer data is activation to host computer;
Transmission including acceleration:First character section represents the beginning of packet for 0x55;Second byte is 0x51, represents this
In individual packet, storage is acceleration information;3rd byte is AxL, represents the least-significant byte of accelerometer X-axis data;4th
Individual byte is AxH, represents the most-significant byte of speedometer X-axis data;5th byte is AyL, represents the low by 8 of accelerometer Y-axis data
Position;6th byte is AyH, represents the most-significant byte of accelerometer Y-axis data;7th byte is AzL, represents accelerometer Z axis
The least-significant byte of data;8th byte is AzH, represents the most-significant byte of accelerometer Z axis data;9th is TL, is temperature information
Least-significant byte;Tenth is TH, is the most-significant byte of temperature information;11st is, check bit, and its value is above all byte sums
Remainder to 0x100;The transmission of angular speed:First character section represents the beginning of packet for 0x55;Second byte is 0x52,
Represent in this packet storage is angular velocity data;3rd byte is WxL, represents the low by 8 of turn meter X-axis data
Position;4th byte is WxH, represents the most-significant byte of speedometer X-axis data;5th byte is WyL, represents turn meter Y-axis number
According to least-significant byte;6th byte is WyH, represents the most-significant byte of turn meter Y-axis data;7th byte is WzL, represents angle speed
The least-significant byte of degree meter Z axis data;8th byte is WzH, represents the most-significant byte of turn meter Z axis data;9th is TL, is temperature
The least-significant byte of degree information;Tenth is TH, is the most-significant byte of temperature information;11st is, check bit, and its value is above all words
Section remainder of the sum to 0x100;
(3) upper computer module of design system:Receiving the information that sends of slave computer, the information to receiving carries out attitude solution
Calculate, show that curve, 3D draw;
Serial communication modular by means of Qt5 SerialPort storehouses completing, be responsible for communicate with slave computer, by monitoring serial ports
Data are parsed by the data that sends according to set communication protocol, and sending the data to attitude algorithm module carries out attitude solution
Calculate;Serial port module is made up of two parts:Part I SerialDialog classes, are responsible for setting and man-machine friendship to serial ports in UI
Mutually, interface is as shown in Figure 6;Part II is SerialThread classes, is responsible in another thread monitoring serial ports in real time, and
Which is parsed according to communication protocol;The method that serial data parsing is adopted is the queue that sets up in a similar data structure
Buffering area, receive the afterbody that data are just added to buffering area every time, begin look for leading to from the head of buffering area afterwards
Whether the beginning of letter agreement, after finding beginning, judge the length of the ending for arriving buffering area from there more than or equal to communication protocol
Length, if meeting this condition, just from buffering area from agreement beginning from the beginning of, take out communication protocol specific length byte
Array, and buffering area beginning is deleted to the data for taking out array end position, repeat this action afterwards, in buffering area
Without communication protocols justice beginning, or length thereafter is jumped out less than the length of agreement, is waited and is received data next time;
Attitude algorithm module is the soul in host computer procedure, and the data obtained from serial port module are carried out attitude algorithm, and after
Continue and pass data to curve display module and 3D display module;Attitude algorithm is arrived according to 3-axis acceleration, three axis angular rates
The theoretical calculation of quaternary number is completed, and represents attitude matrix with quaternary number:
Solve attitude and solve attitude matrix, be equivalent to the renewal of quaternary number, and the renewal of quaternary number can pass through three axis angular rates
Realize, concretely comprise the following steps:
(3.1) three axis angular rates are obtained, and represents three axles of angular speed with gx, gy, gz respectively, appearance is obtained using the method for integration
State, estimates to gravity direction according to quaternary number before this;
(3.2) value of accelerometer is obtained from gyroscope, and represents 3-axis acceleration respectively with ax, ay, az.Carrying out attitude
Before resolving, which is standardized;
(3.3) using complementary filter algorithm fusion angular speed and accekeration and error is asked for, by this error through ratio and product
The post-compensation of point link is in the middle of the measured value of angular speed;
(3.4) single order runge kutta method updates quaternary number;
Host computer curve display module uses the third party of Qt to increase income storehouse QCustomPlot, the initialization of curvilinear function and
Data renewal function is located in class MainWindow of main interface;Show 3-axis acceleration in the host computer procedure main interface of system
Curve, three axis angular rate curves, four curves of attitude angular curve and quaternary number curve, the drafting of curve is with main interface UI more
Newly together carry out;
3D display module is responsible for the loading of 3D modules, the calculating and application of bone conversion, the renewal that model shows;Wherein model is carried
Enter to use Assimp storehouses of increasing income to import the summit of 3D models, pinup picture and bone information, using OpenGL to the information that imports
Rendered;The system constructs 33 pieces of bones in modeling using Blender modeling softwares, and its change information is stored in
In data-structure tree;The method of 3D model modifications is:First summit of 3D models has all been bundled on different bones,
Different bones are represented using weight to the influence degree on same summit, and when corresponding bone enters line translation, are affected by
Also corresponding response can be made according to weight in summit.By controlling the motion of bone, the purpose for making model sport is realized.
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610884559.4A CN106503426B (en) | 2016-10-10 | 2016-10-10 | A kind of body language detection and tracking system design method towards medical robot |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201610884559.4A CN106503426B (en) | 2016-10-10 | 2016-10-10 | A kind of body language detection and tracking system design method towards medical robot |
Publications (2)
Publication Number | Publication Date |
---|---|
CN106503426A true CN106503426A (en) | 2017-03-15 |
CN106503426B CN106503426B (en) | 2019-04-19 |
Family
ID=58293823
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201610884559.4A Active CN106503426B (en) | 2016-10-10 | 2016-10-10 | A kind of body language detection and tracking system design method towards medical robot |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN106503426B (en) |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111504144A (en) * | 2020-05-27 | 2020-08-07 | 西安工业大学 | Built-in shell action detection system and action measurement analysis method thereof |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103136912A (en) * | 2013-03-05 | 2013-06-05 | 广西师范大学 | Moving posture capture system |
US20160023352A1 (en) * | 2014-07-25 | 2016-01-28 | California Institute Of Technology | SURROGATE: A Body-Dexterous Mobile Manipulation Robot with a Tracked Base |
CN105497936A (en) * | 2016-01-28 | 2016-04-20 | 佛山市爱普克斯环保科技有限公司 | Disinfecting and epidemic preventing robot |
CN105856273A (en) * | 2016-06-08 | 2016-08-17 | 中国科学院重庆绿色智能技术研究院 | Industrial robot kinematics performance testing device |
-
2016
- 2016-10-10 CN CN201610884559.4A patent/CN106503426B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN103136912A (en) * | 2013-03-05 | 2013-06-05 | 广西师范大学 | Moving posture capture system |
US20160023352A1 (en) * | 2014-07-25 | 2016-01-28 | California Institute Of Technology | SURROGATE: A Body-Dexterous Mobile Manipulation Robot with a Tracked Base |
CN105497936A (en) * | 2016-01-28 | 2016-04-20 | 佛山市爱普克斯环保科技有限公司 | Disinfecting and epidemic preventing robot |
CN105856273A (en) * | 2016-06-08 | 2016-08-17 | 中国科学院重庆绿色智能技术研究院 | Industrial robot kinematics performance testing device |
Non-Patent Citations (1)
Title |
---|
孙强等: "基于体感遥控的全向移动机器人的系统设计", 《计算机技术与应用》 * |
Cited By (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN111504144A (en) * | 2020-05-27 | 2020-08-07 | 西安工业大学 | Built-in shell action detection system and action measurement analysis method thereof |
Also Published As
Publication number | Publication date |
---|---|
CN106503426B (en) | 2019-04-19 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110303471B (en) | Power-assisted exoskeleton control system and control method | |
WO2018098961A1 (en) | Sports and health management platform and smart sports equipment | |
CN204468106U (en) | Doctor terminal, patient end remote assistant diagnostic equipment and system | |
CN105748037A (en) | Body-sensing tank top with biofeedback system for patients with scoliosis | |
CN108187333A (en) | More inertia node wireless monitoring and evaluation system and methods in rehabilitation training of upper limbs | |
CN110347388A (en) | A kind of graphical mode programming platform generating means | |
CN108056758A (en) | A kind of imitative traditional Chinese medical science takes vein system to unite automatically | |
CN106968979A (en) | A kind of fan control system | |
CN106503426B (en) | A kind of body language detection and tracking system design method towards medical robot | |
Ding et al. | Medical remote monitoring of multiple physiological parameters based on wireless embedded internet | |
CN108170268A (en) | A kind of Whole Body motion capture devices based on Inertial Measurement Unit | |
CN106389070A (en) | Intelligent rehabilitation training system and application | |
CN104667368B (en) | Control with calculating the insulin pump dynamic control implementation method separated | |
CN106112997B (en) | Ectoskeleton clothes | |
CN206463441U (en) | A kind of knee joint recovery auxiliary equipment | |
CN112631148A (en) | Exoskeleton robot platform communication protocol and online simulation control system | |
CN202891937U (en) | Hand function training device | |
CN107692986A (en) | Self-service examination all-in-one | |
CN208031213U (en) | A kind of near infrared spectrum non-invasive glucose monitoring instrument | |
Gao et al. | Design on a wearable armband device for assessing the motion function of upper limbs | |
CN205905027U (en) | Robot control system | |
CN211271430U (en) | Control system of intelligent bionic knee joint | |
Aarthy et al. | Signal monitoring in a telemedicine system for emergency medical services | |
CN104939805B (en) | A kind of hip curved-surface measurement and normal direction pressure diagnosis and therapy system | |
CN204731572U (en) | Based on the human parameters robot that body sense controls |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |