CN106503426B - A kind of body language detection and tracking system design method towards medical robot - Google Patents
A kind of body language detection and tracking system design method towards medical robot Download PDFInfo
- Publication number
- CN106503426B CN106503426B CN201610884559.4A CN201610884559A CN106503426B CN 106503426 B CN106503426 B CN 106503426B CN 201610884559 A CN201610884559 A CN 201610884559A CN 106503426 B CN106503426 B CN 106503426B
- Authority
- CN
- China
- Prior art keywords
- data
- module
- attitude
- byte
- angular velocity
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 33
- 238000013461 design Methods 0.000 title claims abstract description 12
- 238000001514 detection method Methods 0.000 title claims abstract description 9
- 238000004891 communication Methods 0.000 claims abstract description 53
- 230000009471 action Effects 0.000 claims abstract description 12
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 4
- 238000004364 calculation method Methods 0.000 claims description 33
- 230000001133 acceleration Effects 0.000 claims description 21
- 210000000988 bone and bone Anatomy 0.000 claims description 16
- 230000009466 transformation Effects 0.000 claims description 15
- 230000008859 change Effects 0.000 claims description 14
- 230000006870 function Effects 0.000 claims description 8
- 230000005540 biological transmission Effects 0.000 claims description 7
- 238000004590 computer program Methods 0.000 claims description 6
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000003993 interaction Effects 0.000 claims description 4
- 238000005259 measurement Methods 0.000 claims description 4
- 230000004044 response Effects 0.000 claims description 4
- 102100025825 Methylated-DNA-protein-cysteine methyltransferase Human genes 0.000 claims description 3
- 230000000295 complement effect Effects 0.000 claims description 3
- 238000001914 filtration Methods 0.000 claims description 3
- 238000007667 floating Methods 0.000 claims description 3
- 230000005484 gravity Effects 0.000 claims description 3
- 238000009499 grossing Methods 0.000 claims description 3
- 108040008770 methylated-DNA-[protein]-cysteine S-methyltransferase activity proteins Proteins 0.000 claims description 3
- 238000012544 monitoring process Methods 0.000 claims description 3
- 238000007405 data analysis Methods 0.000 claims description 2
- 238000013473 artificial intelligence Methods 0.000 abstract description 2
- 238000010586 diagram Methods 0.000 description 8
- 230000008569 process Effects 0.000 description 4
- 238000011161 development Methods 0.000 description 3
- 238000005516 engineering process Methods 0.000 description 2
- WQZGKKKJIJFFOK-GASJEMHNSA-N Glucose Natural products OC[C@H]1OC(O)[C@H](O)[C@@H](O)[C@@H]1O WQZGKKKJIJFFOK-GASJEMHNSA-N 0.000 description 1
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 239000008280 blood Substances 0.000 description 1
- 210000004369 blood Anatomy 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 239000008103 glucose Substances 0.000 description 1
- 230000036541 health Effects 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001954 sterilising effect Effects 0.000 description 1
- 238000001356 surgical procedure Methods 0.000 description 1
- 230000001225 therapeutic effect Effects 0.000 description 1
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
The present invention relates to machine vision and artificial intelligence field and limb action identification field more particularly to a kind of body language detections and tracking system design method towards medical robot.It include: the overall structure of (1) designing system；(2) the slave computer module of designing 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) information, the information progress attitude algorithm to receiving, display curve, 3D drawing that slave computer is sent the upper computer module of designing system: are received.The present invention has fully taken into account operation Shi doctor using the Orientation differences of the attitudes vibration control endoscope of doctor foot and has controlled scalpel proficiency control endoscope on the other hand to influence caused by operation, the success rate for utmostly improving operation alleviates the operation burden of doctor.
Description
Technical Field
The invention belongs to the field of machine vision and artificial intelligence and the field of limb action recognition, and particularly relates to a design method of a medical robot-oriented limb language detection and tracking system.
Background
With the vigorous development of the national medical health industry and the continuous progress of the automation discipline, the medical auxiliary instruments taking the automation theory as the core are in the endlessly. In recent years, medical instruments such as thermometers, blood glucose meters, sterilizing lamps, scalpels, high-potential therapeutic apparatuses and the like are continuously advanced, develop towards intellectualization, miniaturization and portability, and provide great convenience for doctors and patients.
Among the emerging medical devices, automated machines used to assist in surgery comprise a large percentage. Because most surgical operations are long in time, high in strength and high in precision requirement, the technical and physical requirements of a doctor are very high, and related instruments manufactured by utilizing an automatic control theory can work for a long time without influencing the precision, can perfectly solve similar problems, relieves the burden of the doctor and also provides a guarantee for the rehabilitation of patients. This is part of the medical advance and is also the classic case of practical applications in the automated chemistry industry.
For the surgical operation needing an endoscope, the traditional method often needs to use one hand to control the endoscope, so that the action amplitude and the action precision of the other hand for operating the scalpel are limited, the physical consumption of a doctor is increased, the risk of related operations is increased to a certain extent, and the probability of successful treatment of a patient is reduced. The invention is based on man-machine interaction technology and automatic control theory, and finishes the control of the endoscope by using the sensor to collect the position and posture information of the feet of the doctor and identifying certain specific posture changes and the combination of the posture changes according to a certain sequence, thereby lightening the burden of a doctor on the premise of ensuring that the operation can be finished smoothly, and realizing the purposes of bringing convenience to the doctor and benefiting the patient.
Disclosure of Invention
The invention aims to provide a medical robot-oriented limb language detection and tracking system design method.
The purpose of the invention is realized as follows:
(1) designing the integral structure of the system;
the whole system is divided into two large functional modules which are a lower computer module and an upper computer module respectively;
the lower computer module consists of a main control chip, an attitude sensor and a communication element; after power is on, the main control chip initializes each part, reads data from the attitude sensor at a fixed frequency, and sends the data to the upper computer through the communication module and a set communication protocol after simple smoothing;
the upper computer module consists of a communication module, an attitude calculation module, a curve display module, a 3D model creation module, a skeleton transformation module and a 3D display and update module; initializing vertex, chartlet and endoskeleton fitting information of the 3D model by using a 3D model creating module during starting, importing the information into a needed renderer, and creating a drawing environment; then, the lower computer is communicated through a communication module, and the attitude information transmitted by the lower computer is sent to an attitude calculation module through a set interface every time the attitude information is received; the attitude calculation module calculates the original data transmitted by the lower computer into required attitude information through a correlation principle, and further transmits the data to the curve display module and the bone transformation module; the curve display module displays the data in a real-time data form; the skeleton transformation module traverses the skeleton tree according to the posture information in a depth-first mode, calculates the posture transformation to be performed on the skeleton, and then transmits the skeleton pose information to the 3D updating module; the 3D updating module finally displays the result on a screen of a computer in a 3D model mode according to the received skeleton pose information;
(2) designing a lower computer module of the system; selecting types of the attitude sensor, the main control chip and the communication device, and respectively designing lower computer hardware and lower computer software;
the attitude sensor adopts a six-axis tracking sensor MPU-6050, the main control chip adopts an STM32F103C8T6 singlechip, and the communication module uses a wireless serial port for communication;
the single chip microcomputer STM32F103C8T6 is responsible for initialization work of the MPU-6050, reads data from the MPU-6050 and sends the data to an upper computer through a wireless serial port; the power supply is completed by using VCC and GND interfaces of an SWD interface downloader of the STM32F103C8T6, an MPU-6050 and a wireless serial port are connected to the STM32F103C8T6, and TPS76833 is introduced for power supply;
the software design of the system lower computer comprises the initialization of STM32F103C8T6 and MPU-6050, the reading of data of the MPU-6050 and the sending of the data to the upper computer through a wireless serial port;
the initialization of STM32F103C8T6 mainly completes four steps:
(2.1) initializing a SysTick timer of the system, and triggering an interrupt every 1m seconds after the SysTick timer is started;
(2.2) initializing the serial port: starting device clocks of GIPOA and USART1, and setting GPIOA9 and GPIOA10 to multiplex push-pull output mode and floating input mode; configuring USART1 to set the Baud rate to 9600, the data bit to 8 bit, 1 bit stop bit, no parity check, no hardware control, using data receiving and data sending;
(2.3) redirecting the output of printf () in the C-standard library to USART 1;
(2.4) initialization of I2C communication: starting clocks of GPIOB and I2C1, setting the working mode of GPIOB6 and GPIOB7 as a multiplexing output leakage mode I2C1 as I2C, setting the Duty of an SCL line to be 0.33 when low-level data changes, starting message response, setting a 7-bit address to be 0 xB;
initialization of the MPU-6050 is to set an internal register, in which the register PWR _ MGMT _1 is set to 0x00, the register SAMPLE RATE DIVIDER is set to 0x07, the register CONFIG is 0x06, the register accelerome CONFIGURATION is set to 0x01, and the register GYROSCOPE CONFIGURATION is set to 0x 18; reading a WHO _ AM _ I register of the MPU-6050, if the value of the register is 0x68, normally starting, and otherwise, reinitializing; after correctly configuring MPU-6050 and checking for no error, starting to read the data of MPU-6050;
reading data of the MPU-6050 by adopting a polling method, and reading data of the accelerometer and the angular velocity meter once every 100ms, wherein acceleration data and angular velocity data obtained by measurement of the MPU-6050 occupy six continuous registers respectively, and measurement values of the temperature sensor occupy two registers;
the data of the lower computer is sent to the upper computer through the wireless serial port by formulating a related communication protocol;
including the transmission of acceleration: the first byte, 0x55, indicates the beginning of the packet; the second byte is 0x51, which indicates that acceleration data is stored in the data packet; the third byte is AxL, representing the lower 8 bits of accelerometer X-axis data; the fourth byte is AxH, representing the upper 8 bits of the speedometer X-axis data; the fifth byte is AyL, representing the lower 8 bits of accelerometer Y-axis data; the sixth byte is AyH, representing the upper 8 bits of accelerometer Y-axis data; the seventh byte is AzL, representing the lower 8 bits of accelerometer Z-axis data; the eighth byte is AzH, representing the upper 8 bits of accelerometer Z-axis data; the ninth bit is TL which is the lower 8 bits of the temperature information; the tenth position is TH, which is the high 8 position of the temperature information; the eleventh bit is a check bit, whose value is the remainder of the sum of all the preceding bytes to 0x 100; transmission of angular velocity: the first byte, 0x55, indicates the beginning of the packet; the second byte is 0x52, which indicates that the angular velocity data is stored in the data packet; the third byte is WxL, representing the lower 8 bits of the angular velocity meter X-axis data; the fourth byte is WxH, which represents the upper 8 bits of speedometer X-axis data; the fifth byte is WyL, representing the lower 8 bits of the angular velocity meter Y-axis data; the sixth byte is WyH, representing the upper 8 bits of the angular velocity meter Y-axis data; the seventh byte is WzL, representing the lower 8 bits of the angular velocity meter Z-axis data; the eighth byte is WzH, representing the upper 8 bits of the angular velocity meter Z-axis data; the ninth bit is TL which is the lower 8 bits of the temperature information; the tenth position is TH, which is the high 8 position of the temperature information; the eleventh bit is a check bit, whose value is the remainder of the sum of all the preceding bytes to 0x 100;
(3) designing an upper computer module of the system: receiving information sent by a lower computer, and performing attitude calculation, curve display and 3D (three-dimensional) drawing on the received information;
the serial port communication module is completed by means of a SerialPort library of Qt5 and is responsible for communicating with a lower computer, data sent by a serial port is monitored, the data are analyzed according to a set communication protocol, and the data are sent to the attitude calculation module for attitude calculation; the serial port module consists of two parts: the first part of SerialDialog classes is responsible for setting serial ports in the UI and human-computer interaction, and the interface is shown in FIG. 6; the second part is SerialThread class, which is responsible for monitoring the serial port in real time in another thread and analyzing the serial port according to a communication protocol; the method adopted by the serial port data analysis is that a buffer area of a queue in a similar data structure is established, each time data is received, the data is added to the tail of the buffer area, then the beginning of a communication protocol is searched from the head of the buffer area, after the beginning is found, whether the length from the beginning to the end of the buffer area is larger than or equal to the length of the communication protocol is judged, if the condition is met, a byte array with the specified length of the communication protocol is taken out from the beginning of the protocol in the buffer area, the data from the beginning of the buffer area to the end position of the taken-out array is deleted, then the action is repeated until no communication protocol beginning exists in the buffer area, or the length of the buffer area is smaller than the length of the protocol, the data is jumped out, and the next time of receiving;
the attitude resolving module is a soul in an upper computer program, performs attitude resolving on data obtained from the serial port module, and continuously transmits the data to the curve display module and the 3D display module; the attitude solution is completed according to the theoretical calculation from three-axis acceleration, three-axis angular velocity to quaternion, and the quaternion is used for expressing an attitude matrix:
solving the attitude, namely solving an attitude matrix, which is equivalent to updating the quaternion, wherein the updating of the quaternion can be realized by three-axis angular velocity, and the specific steps are as follows:
(3.1) acquiring three-axis angular velocity, respectively representing three axes of the angular velocity by gx, gy and gz, acquiring the attitude by adopting an integral method, and estimating the gravity direction according to the quaternion before;
and (3.2) acquiring the value of the accelerometer by using a gyroscope, and representing the triaxial acceleration by ax, ay and az respectively. Before the attitude calculation is carried out, the attitude calculation is normalized;
(3.3) fusing the angular velocity and the acceleration value by using a complementary filtering algorithm, solving an error, and compensating the error into the measured value of the angular velocity after the error passes through a proportion link and an integral link;
(3.4) updating quaternion by a first-order Runge Kutta method;
the upper computer curve display module uses a third-party open source library QCustomPlut of Qt, and the initialization and data updating functions of the curve function are positioned in a main interface class MainWindow; displaying four curves of a triaxial acceleration curve, a triaxial angular velocity curve, an attitude angle curve and a quaternion curve in a main interface of an upper computer program of the system, wherein the curves are drawn together with the updating of a UI of the main interface;
the 3D display module is responsible for loading of the 3D module, calculation and application of skeleton transformation and updating of model display; the model loading uses vertex, chartlet and skeleton information which are imported into the 3D model by the Assimp open source library, and the imported information is rendered by using OpenGL; the system uses Blender modeling software to construct 33 skeletons during modeling, and change information of the skeletons is stored in a data structure tree; the method for updating the 3D model comprises the following steps: the first vertex of the 3D model is bound on different bones, the influence degree of the different bones on the same vertex is represented by using weights, and when the corresponding bones are transformed, the influenced vertices respond correspondingly according to the weights. The purpose of moving the model is achieved by controlling the movement of the skeleton.
The invention has the beneficial effects that:
(1) the posture change of the feet of the doctor is adopted to control the direction change of the endoscope, so that the influence of the doctor controlling the scalpel with one hand and the endoscope with the other hand on the operation is fully considered, the success rate of the operation is improved to the maximum extent, and the operation burden of the doctor is reduced.
(2) The inconvenience of doctors can be increased by directly controlling the endoscope through the foot action and certain influence is caused on the operation, the 3D model of the upper computer is designed to directly reproduce the foot action and curve change to reflect the posture change, the endoscope is further controlled, the complexity of the operation is further reduced, and the success rate is improved.
Drawings
FIG. 1 is a block diagram of the overall system architecture;
FIG. 2 is a diagram of the hardware architecture of the lower computer of the system;
FIG. 3 is a hardware object diagram of a lower computer of the system;
FIG. 4 is a software flow diagram of the lower computer of the system;
FIG. 5 shows the overall structure of the upper computer of the system;
FIG. 6 is a diagram of a serial port correlation window;
FIG. 7 is a serial port execution flow;
FIG. 8 is a graphical illustration of an interface;
FIG. 93D shows an interface;
fig. 10 a skeletal tree.
Detailed Description
The invention will now be described in more detail by way of example with reference to the accompanying drawings in which:
the method realizes the operation of the medical machine controlled by the human body posture, and solves the problems that in the surgical operation needing the endoscope, a doctor needs to control the scalpel by one hand and control the endoscope by the other hand. Therefore, the operation burden of a doctor can be reduced, and the success rate of the operation is improved.
The design of the invention is realized by the following scheme:
(1) and designing the integral structure of the system.
(2) And designing a lower computer module of the system. And (3) selecting types of the attitude sensor, the main control chip and the communication device, and respectively designing lower computer hardware and lower computer software.
(3) Designing the upper computer module of the system, the work that needs to be carried out has: receiving information sent by a lower computer, and performing attitude calculation, curve display and 3D drawing on the received information.
The posture change of the feet of the doctor is adopted to control the direction change of the endoscope, so that the influence of the doctor controlling the scalpel with one hand and the endoscope with the other hand on the operation is fully considered, the success rate of the operation is improved to the maximum extent, and the operation burden of the doctor is reduced.
The inconvenience of doctors can be increased by directly controlling the endoscope through the foot action and certain influence is caused on the operation, the 3D model of the upper computer is designed to directly reproduce the foot action and curve change to reflect the posture change, the endoscope is further controlled, the complexity of the operation is further reduced, and the success rate is improved.
The design of the body language detecting and tracking system for the medical robot mainly comprises two modules: the device comprises a lower computer module and an upper computer module. The method is characterized in that: aiming at the surgical operation needing an endoscope, the posture change of the feet of a doctor is adopted to replace manual operation to control the orientation of the endoscope; in order to ensure that the system does not influence the whole operation process, the change of the foot posture is displayed by adopting a 3D model and curve change of the upper computer.
The invention provides a body language detection and tracking system for a medical robot.
The method comprises the following steps: and designing the integral structure of the system.
The overall structure block diagram of the system is shown in fig. 1. The whole system is functionally divided into two large functional modules, namely a lower computer module and an upper computer module.
The lower computer is composed of a main control chip, an attitude sensor and a communication element. The main working mode is that after power is on, the main control chip initializes each component, then reads data from the attitude sensor at a fixed frequency, and after simple smoothing, the data is sent to the upper computer through the communication module according to a set communication protocol.
The upper computer is functionally divided into a communication module, an attitude calculation module, a curve display module, a 3D model creation module, a skeleton transformation module, a 3D display and update module and the like. The main working mode is as follows: initializing information such as a vertex, a chartlet, endoskeleton fitting and the like of the 3D model by using a 3D model creating module during starting, importing the information into a needed renderer, and creating a drawing environment; then, the lower computer is communicated through a communication module, and the attitude information transmitted by the lower computer is sent to an attitude calculation module through a set interface every time the attitude information is received; the attitude calculation module calculates the original data transmitted by the lower computer into required attitude information through a correlation principle, and further transmits the data to the curve display module and the bone transformation module; the curve display module displays the data in a real-time data form; the skeleton transformation module traverses the skeleton tree according to the posture information in a depth-first mode, calculates the posture transformation to be performed on the skeleton, and then transmits the skeleton pose information to the 3D updating module; and the 3D updating module finally displays the result on a screen of a computer in a 3D model mode according to the received bone pose information.
Step two: and designing a lower computer module of the system. And (3) selecting types of the attitude sensor, the main control chip and the communication device, and respectively designing lower computer hardware and lower computer software.
In consideration of experimental requirements and economic problems, the attitude sensor adopts a low-power-consumption six-axis tracking sensor MPU-6050, the main control chip adopts an STM32F103C8T6 single chip microcomputer which is rich in functions and simple in structure, and the communication module uses a wireless serial port for communication.
The hardware structure design diagram of the lower computer of the system is shown in fig. 2, and the hardware object diagram is shown in fig. 3. MPU-6050 is fixed on the foot of a doctor to collect posture information, and the single chip microcomputer STM32F103C8T6 is responsible for initialization work of the MPU-6050, reads data from the MPU-6050, and sends the data to an upper computer through a wireless serial port after simple processing. The system adopts two different power supply modes to be applied to debugging and practical application in the development process respectively. In the actual development process, because the hardware structure of the system is simple, the consumed power is small and the current basically has no large fluctuation, the power supply can be completed by simply using VCC and GND interfaces of an SWD interface downloader of the STM32F103C8T6, and an MPU-6050 and a wireless serial port are connected to the STM32F103C8T6 without additionally adding a power supply module. The connection mode is simple and convenient, and the workload is basically zero. However, in practical applications, the TPS76833 is introduced to supply power, considering that the lower unit is fixed to the foot of the doctor and moves along with the foot of the doctor, and thus a wired power supply method cannot be used.
The software design flow chart of the system lower computer is shown in fig. 4, and mainly comprises the steps of initializing an STM32F103C8T6 and an MPU-6050, reading data of the MPU-6050, sending the data to the upper computer through a wireless serial port and the like.
The initialization of STM32F103C8T6 mainly completes four steps:
(1) the SysTick timer of the system is initialized so that it will trigger an interrupt every 1 msec after starting.
(2) Initialization of a serial port: starting device clocks of GIPOA and USART1 (namely, a serial port corresponding to GPIOA9 and GPIOA10 when multiplexing as a serial port function), and setting GPIOA9 and GPIOA10 to be a multiplexing push-pull output mode and a floating input mode; the USART1 was configured to have its baud rate set to 9600, data bits 8 bits, 1 bit stop bit, no parity, no hardware control, using data reception and data transmission.
(3) The output of printf () in the C-standard library is redirected to USART 1.
(4) Initialization of I2C communication: the clock of GPIOB and I2C1 is started, the operation mode of GPIOB6 and GPIOB7 is set as multiplexing output mode I2C1 is set as I2C, the Duty of SCL line is 0.33 when the low level data changes, the message response is started, the 7-bit address is set, and the self address is set as 0 xB.
The initialization of the MPU-6050 is mainly to set internal registers, in which the register PWR _ MGMT _1 is set to 0x00, the register SAMPLE RATE DIVIDER is set to 0x07, the register CONFIG is 0x06, the register accelerome CONFIGURATION is set to 0x01, and the register GYROSCOPE CONFIGURATION is set to 0x 18. Then, the WHO _ AM _ I register of the MPU-6050 is read, and if the value is 0x68, the normal start is performed, otherwise, the problem is indicated, and the reinitialization is required. After the MPU-6050 is correctly configured and checked, the data of the MPU-6050 can be read.
The MPU-6050 can read the data by an interrupt method or a polling method, and the system adopts a polling method for reading the data of the accelerometer and the angular velocity meter every 100ms, wherein the data of the acceleration and the angular velocity measured by the MPU-6050 respectively occupy six continuous registers, and the measured value of the temperature sensor occupies two registers.
The data of the lower computer is sent to the upper computer through the wireless serial port by establishing a related communication protocol, which is specifically described as follows: (1) transmission of acceleration: the first byte, 0x55, indicates the beginning of the packet; the second byte is 0x51, which indicates that acceleration data is stored in the data packet; the third byte is AxL, representing the lower 8 bits of accelerometer X-axis data; the fourth byte is AxH, representing the upper 8 bits of the speedometer X-axis data; the fifth byte is AyL, representing the lower 8 bits of accelerometer Y-axis data; the sixth byte is AyH, representing the upper 8 bits of accelerometer Y-axis data; the seventh byte is AzL, representing the lower 8 bits of accelerometer Z-axis data; the eighth byte is AzH, representing the upper 8 bits of accelerometer Z-axis data; the ninth bit is TL which is the lower 8 bits of the temperature information; the tenth position is TH, which is the high 8 position of the temperature information; the eleventh bit is a check bit whose value is the remainder of the sum of all preceding bytes to 0x 100. (2) Transmission of angular velocity: the first byte, 0x55, indicates the beginning of the packet; the second byte is 0x52, which indicates that the angular velocity data is stored in the data packet; the third byte is WxL, representing the lower 8 bits of the angular velocity meter X-axis data; the fourth byte is WxH, which represents the upper 8 bits of speedometer X-axis data; the fifth byte is WyL, representing the lower 8 bits of the angular velocity meter Y-axis data; the sixth byte is WyH, representing the upper 8 bits of the angular velocity meter Y-axis data; the seventh byte is WzL, representing the lower 8 bits of the angular velocity meter Z-axis data; the eighth byte is WzH, representing the upper 8 bits of the angular velocity meter Z-axis data; the ninth bit is TL which is the lower 8 bits of the temperature information; the tenth position is TH, which is the high 8 position of the temperature information; the eleventh bit is a check bit whose value is the remainder of the sum of all preceding bytes to 0x 100.
Step three: designing the upper computer module of the system, the work that needs to be carried out has: receiving information sent by a lower computer, and performing attitude calculation, curve display and 3D drawing on the received information.
The upper computer system plays a crucial role in the realization of the whole system, and essentially, the whole lower computer system only plays a role of a sensor relative to the upper computer. The accurate detection and tracking of the foot posture is mainly realized through a posture resolving module of an upper computer, curve drawing and a 3D model. Therefore, the design of the upper computer needs to have the characteristics of low module coupling degree, good performance, convenience and easiness in use.
The upper computer system of the system is mainly completed by using C + + language, Qt is used as a UI solution, OpenGL is used as 3D drawing support, and for convenience of programming, a C + + STL standard library, QCustomplot, Assimp and other open source software are also used. In order to improve the running efficiency of the program, a multithreading technology is used at multiple positions of the upper computer.
The general structure of the upper computer design is shown in fig. 5.
The serial port communication module in the figure is mainly completed by means of a SerialPort library of Qt5 and is responsible for communicating with a lower computer, data sent by a serial port is monitored, the data are analyzed according to a set communication protocol, and the data are sent to the attitude calculation module for attitude calculation. According to the requirement, the serial port module consists of two parts: the first part of SerialDialog classes is responsible for setting serial ports in the UI and human-computer interaction, and the interface is shown in FIG. 6; the second part is SerialThread class, which is mainly responsible for monitoring the serial port in real time in another thread and analyzing the serial port according to the communication protocol. The method for analyzing serial port data is that a buffer area of a queue in a similar data structure is established, each time data is received, the data is added to the tail of the buffer area, then the head of a communication protocol is searched from the head of the buffer area, after the head is found, whether the length from the head to the tail of the buffer area is larger than or equal to the length of the communication protocol is judged, if the condition is met, a byte array with the specified length of the communication protocol is taken out from the head of the protocol in the buffer area, the data from the head of the buffer area to the tail position of the taken out array is deleted, then the action is repeated until no communication protocol head exists in the buffer area or the length from the head of the buffer area to the tail position of the taken out array, and the data is jumped out to wait for next data reception. The whole serial port process is shown in fig. 7.
The attitude calculation module is a soul in an upper computer program, performs attitude calculation on data obtained from the serial port module, and continuously transmits the data to the curve display module and the 3D display module. The attitude solution is completed according to the theoretical calculation from the three-axis acceleration, the three-axis angular velocity to the quaternion, and the quaternion is used for expressing an attitude matrix as follows:
solving the attitude, namely solving an attitude matrix, can be equivalently used as updating of quaternion, and updating of quaternion can be realized through three-axis angular velocity, and the method specifically comprises the following steps:
(1) acquiring three-axis angular velocity, respectively representing three axes of the angular velocity by gx, gy and gz, acquiring a posture by adopting an integral method, and estimating a gravity direction according to a quaternion before;
(2) and acquiring the value of the accelerometer by using a gyroscope, and representing the triaxial acceleration by using ax, ay and az respectively. Before the attitude calculation is carried out, the attitude calculation is normalized;
(3) fusing the angular velocity and the acceleration value by utilizing a complementary filtering algorithm, solving an error, and compensating the error into a measured value of the angular velocity after the error passes through a proportion link and an integral link;
(4) and updating the quaternion by a first-order Runge Kutta method. The equation is as follows:
wherein,
the upper computer curve display module uses a third party open source library QCustomPlut of Qt, and the initialization and data updating functions of the curve function are located in the class MainWindow of the main interface. As shown in fig. 8, the main interface of the upper computer program of the system displays four curves, namely a three-axis acceleration curve, a three-axis angular velocity curve, an attitude angle curve and a quaternion curve, and the curves are drawn together with the updating of the UI of the main interface, so that no additional thread is required.
The most complex part of the 3D display module upper computer system is mainly responsible for loading of the 3D module, calculation and application of skeleton transformation and updating of model display. The vertex, the mapping and the bone information of the 3D model are imported by the Assimp open source library, and the imported information is rendered by OpenGL to obtain the 3D model as shown in FIG. 9. The system constructs 33 bones using Blender modeling software in modeling, and stores the change information in the data structure tree, as shown in fig. 10. The main method for updating the 3D model comprises the following steps: the first vertex of the 3D model is bound on different bones, the influence degree of the different bones on the same vertex is represented by using weights, and when the corresponding bones are transformed, the influenced vertices respond correspondingly according to the weights. Thus, the purpose of moving the model can be achieved by controlling the movement of the skeleton. The invention completes the acquisition of the foot posture information of the doctor by designing the lower computer, realizes the accurate detection and tracking of the foot posture information by designing the upper computer, and reproduces the foot actions of the doctor in real time by using the 3D model, thereby preparing for the subsequent mechanical precise control of the endoscope and opening a new chapter for the research of biomedicine.
Claims (1)
1. A design method of a medical robot-oriented limb language detection and tracking system is characterized by comprising the following steps:
(1) designing the integral structure of the system;
the whole system is divided into two large functional modules which are a lower computer module and an upper computer module respectively;
the lower computer module consists of a main control chip, an attitude sensor and a communication element; after power is on, the main control chip initializes each part, reads data from the attitude sensor at a fixed frequency, and sends the data to the upper computer through the communication module and a set communication protocol after simple smoothing;
the upper computer module consists of a communication module, an attitude calculation module, a curve display module, a 3D model creation module, a skeleton transformation module and a 3D display and update module; initializing vertex, chartlet and endoskeleton fitting information of the 3D model by using a 3D model creating module during starting, importing the information into a needed renderer, and creating a drawing environment; then, the lower computer is communicated through a communication module, and the attitude information transmitted by the lower computer is sent to an attitude calculation module through a set interface every time the attitude information is received; the attitude calculation module calculates the original data transmitted by the lower computer into required attitude information through a correlation principle, and further transmits the data to the curve display module and the bone transformation module; the curve display module displays the data in a real-time data form; the skeleton transformation module traverses the skeleton tree according to the posture information in a depth-first mode, calculates the posture transformation to be performed on the skeleton, and then transmits the skeleton pose information to the 3D updating module; the 3D updating module finally displays the result on a screen of a computer in a 3D model mode according to the received skeleton pose information;
(2) designing a lower computer module of the system; selecting types of the attitude sensor, the main control chip and the communication device, and respectively designing lower computer hardware and lower computer software;
the attitude sensor adopts a six-axis tracking sensor MPU-6050, the main control chip adopts an STM32F103C8T6 singlechip, and the communication module uses a wireless serial port for communication;
the single chip microcomputer STM32F103C8T6 is responsible for initialization work of the MPU-6050, reads data from the MPU-6050 and sends the data to an upper computer through a wireless serial port; the power supply is completed by using VCC and GND interfaces of an SWD interface downloader of the STM32F103C8T6, an MPU-6050 and a wireless serial port are connected to the STM32F103C8T6, and TPS76833 is introduced for power supply;
the software design of the system lower computer comprises the initialization of STM32F103C8T6 and MPU-6050, the reading of data of the MPU-6050 and the sending of the data to the upper computer through a wireless serial port;
the initialization of STM32F103C8T6 mainly completes four steps:
(2.1) initializing a SysTick timer of the system, and triggering an interrupt every 1m seconds after the SysTick timer is started;
(2.2) initializing the serial port: starting device clocks of GIPOA and USART1, and setting GPIOA9 and GPIOA10 to multiplex push-pull output mode and floating input mode; configuring USART1 to set the Baud rate to 9600, the data bit to 8 bit, 1 bit stop bit, no parity check, no hardware control, using data receiving and data sending;
(2.3) redirecting the output of printf () in the C-standard library to USART 1;
(2.4) initialization of I2C communication: starting clocks of GPIOB and I2C1, setting the working mode of GPIOB6 and GPIOB7 as a multiplexing output leakage mode I2C1 as I2C, setting the Duty of an SCL line to be 0.33 when low-level data changes, starting message response, setting a 7-bit address to be 0 xB;
initialization of the MPU-6050 is to set an internal register, in which the register PWR _ MGMT _1 is set to 0x00, the register SAMPLE RATE DIVIDER is set to 0x07, the register CONFIG is 0x06, the register accelerome CONFIGURATION is set to 0x01, and the register GYROSCOPE CONFIGURATION is set to 0x 18; reading a WHO _ AM _ I register of the MPU-6050, if the value of the register is 0x68, normally starting, and otherwise, reinitializing; after correctly configuring MPU-6050 and checking for no error, starting to read the data of MPU-6050;
reading data of the MPU-6050 by adopting a polling method, and reading data of the accelerometer and the angular velocity meter once every 100ms, wherein acceleration data and angular velocity data obtained by measurement of the MPU-6050 occupy six continuous registers respectively, and measurement values of the temperature sensor occupy two registers;
the data of the lower computer is sent to the upper computer through the wireless serial port by formulating a related communication protocol;
including the transmission of acceleration: the first byte, 0x55, indicates the beginning of the packet; the second byte is 0x51, which indicates that acceleration data is stored in the data packet; the third byte is AxL, representing the lower 8 bits of accelerometer X-axis data; the fourth byte is AxH, representing the upper 8 bits of the speedometer X-axis data; the fifth byte is AyL, representing the lower 8 bits of accelerometer Y-axis data; the sixth byte is AyH, representing the upper 8 bits of accelerometer Y-axis data; the seventh byte is AzL, representing the lower 8 bits of accelerometer Z-axis data; the eighth byte is AzH, representing the upper 8 bits of accelerometer Z-axis data; the ninth bit is TL which is the lower 8 bits of the temperature information; the tenth position is TH, which is the high 8 position of the temperature information; the eleventh bit is a check bit, whose value is the remainder of the sum of all the preceding bytes to 0x 100; transmission of angular velocity: the first byte, 0x55, indicates the beginning of the packet; the second byte is 0x52, which indicates that the angular velocity data is stored in the data packet; the third byte is WxL, representing the lower 8 bits of the angular velocity meter X-axis data; the fourth byte is WxH, which represents the upper 8 bits of speedometer X-axis data; the fifth byte is WyL, representing the lower 8 bits of the angular velocity meter Y-axis data; the sixth byte is WyH, representing the upper 8 bits of the angular velocity meter Y-axis data; the seventh byte is WzL, representing the lower 8 bits of the angular velocity meter Z-axis data; the eighth byte is WzH, representing the upper 8 bits of the angular velocity meter Z-axis data; the ninth bit is TL which is the lower 8 bits of the temperature information; the tenth position is TH, which is the high 8 position of the temperature information; the eleventh bit is a check bit, whose value is the remainder of the sum of all the preceding bytes to 0x 100;
(3) designing an upper computer module of the system: receiving information sent by a lower computer, and performing attitude calculation, curve display and 3D (three-dimensional) drawing on the received information;
the serial port communication module is completed by means of a SerialPort library of Qt5 and is responsible for communicating with a lower computer, data sent by a serial port is monitored, the data are analyzed according to a set communication protocol, and the data are sent to the attitude calculation module for attitude calculation; the serial port module consists of two parts: the first part is SerialDialog class which is responsible for setting the serial port and man-machine interaction in the UI, and the second part is SerialThread class which is responsible for monitoring the serial port in another thread in real time and analyzing the serial port according to a communication protocol; the method adopted by the serial port data analysis is that a buffer area of a queue in a similar data structure is established, each time data is received, the data is added to the tail of the buffer area, then the beginning of a communication protocol is searched from the head of the buffer area, after the beginning is found, whether the length from the beginning to the end of the buffer area is larger than or equal to the length of the communication protocol is judged, if the condition is met, a byte array with the specified length of the communication protocol is taken out from the beginning of the protocol in the buffer area, the data from the beginning of the buffer area to the end position of the taken-out array is deleted, then the action is repeated until no communication protocol beginning exists in the buffer area, or the length from the beginning of the buffer area to the end position of the taken-out array is smaller than the length of the protocol;
the attitude resolving module is a soul in an upper computer program, performs attitude resolving on data obtained from the serial port module, and continuously transmits the data to the curve display module and the 3D display module; the attitude solution is completed according to the theoretical calculation from three-axis acceleration, three-axis angular velocity to quaternion, and the quaternion is used for expressing an attitude matrix:
solving the attitude, namely solving an attitude matrix, which is equivalent to updating the quaternion, wherein the updating of the quaternion can be realized by three-axis angular velocity, and the specific steps are as follows:
(3.1) acquiring three-axis angular velocity, respectively representing three axes of the angular velocity by gx, gy and gz, acquiring the attitude by adopting an integral method, and estimating the gravity direction according to the quaternion before;
(3.2) selecting a gyroscope to obtain the value of the accelerometer, and representing the triaxial acceleration by ax, ay and az respectively; before the attitude calculation is carried out, the attitude calculation is normalized;
(3.3) fusing the angular velocity and the acceleration value by using a complementary filtering algorithm, solving an error, and compensating the error into the measured value of the angular velocity after the error passes through a proportion link and an integral link;
(3.4) updating quaternion by a first-order Runge Kutta method;
the upper computer curve display module uses a third-party open source library QCustomPlut, and the initialization and data updating functions of the curve function are positioned in a main interface class MainWindow; displaying four curves of a triaxial acceleration curve, a triaxial angular velocity curve, an attitude angle curve and a quaternion curve in a main interface of an upper computer program of the system, wherein the curves are drawn together with the updating of a UI of the main interface;
the 3D display module is responsible for loading of the 3D module, calculation and application of skeleton transformation and updating of model display; the model loading uses vertex, chartlet and skeleton information which are imported into the 3D model by the Assimp open source library, and the imported information is rendered by using OpenGL; the system uses Blender modeling software to construct 33 skeletons during modeling, and change information of the skeletons is stored in a data structure tree; the method for updating the 3D model comprises the following steps: the first vertex of the 3D model is bound on different bones, the influence degree of the different bones on the same vertex is represented by using the weight, when the corresponding bones are transformed, the influenced vertices can make corresponding response according to the weight, and the purpose of moving the model is realized by controlling the movement of the bones.
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 CN106503426A (en) | 2017-03-15 |
| CN106503426B true 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) |
Families Citing this family (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 (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103136912A (en) * | 2013-03-05 | 2013-06-05 | 广西师范大学 | Moving posture capture system |
| 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 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9862090B2 (en) * | 2014-07-25 | 2018-01-09 | California Institute Of Technology | Surrogate: a body-dexterous mobile manipulation robot with a tracked base |
-
2016
- 2016-10-10 CN CN201610884559.4A patent/CN106503426B/en active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103136912A (en) * | 2013-03-05 | 2013-06-05 | 广西师范大学 | Moving posture capture system |
| 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 |
|---|
| 基于体感遥控的全向移动机器人的***设计;孙强等;《计算机技术与应用》;20150630;第41卷(第6期);第157-160页 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN106503426A (en) | 2017-03-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| US20220335851A1 (en) | Identification and analysis of movement using sensor devices | |
| Wang et al. | Design of a low-cost miniature robot to assist the COVID-19 nasopharyngeal swab sampling | |
| Kugler et al. | Shimmer, Cooja and Contiki: A new toolset for the simulation of on-node signal processing algorithms | |
| CN110327048B (en) | Human upper limb posture reconstruction system based on wearable inertial sensor | |
| US20220000448A1 (en) | Instrumented Ultrasound Probes For Machine-Learning Generated Real-Time Sonographer Feedback | |
| CN106503426B (en) | A kind of body language detection and tracking system design method towards medical robot | |
| Lin et al. | Development of the miniaturized wireless Inertial Measurement Unit WB-4: Pilot test for mastication analysis | |
| CN104382584B (en) | A kind of wearable electrocardiosignal monitoring and evaluating system and implementation method | |
| Lung et al. | Wireless data acquisition system for IoT applications | |
| CN205612475U (en) | Wearing formula pulse measurement analogue means | |
| Abed et al. | Design and Implementation of Real Time Health Care Monitoring System Based on IoT | |
| US20210259563A1 (en) | System and method for heterogenous data collection and analysis in a deterministic system | |
| Chen et al. | An inertial-based human motion tracking system with twists and exponential maps | |
| Kalyan et al. | IOT based heart function monitoring and heart disease prediction system | |
| Carneiro et al. | Study of an assistive robotics 5-dof system prototype to be taught in undergraduate engineering classes | |
| Chenghao et al. | Research on human posture recognition system based on inertial sensor | |
| TWI616193B (en) | Oral-occlusal path measurement device, method of sensing an oral occlusion motion of an oral cavity, and device for sensing movement of an oral cavity | |
| Ribeiro et al. | A novel smart sensing platform for vital signs and motor activity monitoring | |
| Valchinov et al. | Wearable system for early diagnosis and follow up of spine curvature disorders | |
| Mitterer | Virtual prosthesis control and visualization of active muscles based on spatial location of arm segments of intact or prosthesis arm | |
| Zestas et al. | A smart-glove approach in upper-limb rehabilitation assessment | |
| CN110767021A (en) | Robot joint replacement test system based on virtual reality technology | |
| CN108720845A (en) | A kind of noninvasive dynamics monitoring Intelligent bracelet | |
| Guidolin | Misurazione e Modellazione Multisensore del Movimento Umano | |
| CN109589088A (en) | Body gait acquires equipment and acquisition method |
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 |