A mirror control system based on upper limb robotic arm

Abstract

The present application discloses a mirror control system based on an upper limb mechanical arm, including a posture data acquisition module and a posture data sending module, wherein the posture data acquisition module respectively acquires posture information of selected parts of the healthy upper limb of the patient on the X-axis, Y-axis and Z-axis; the posture data sending module is configured to send each posture information to a motion intention recognition module; the motion intention recognition module is configured to recognize the patient's motion intention according to each posture information, and to generate driving information of the motor at the corresponding axial rotation angle according to the posture information; when the patient's motion intention is judged to be a motion state, the driving information is sent to the joint motor control module; the joint motor control module is configured to control the motor according to the driving information, and the motor is used to control the mechanical structure movement of the corresponding part of the upper limb mechanical arm. Compared with the fixed grip type strain gauge force sensor, the present application is more friendly in the entire acquisition process, does not need to keep the hand on the control rod, can provide highly sensitive motion acquisition for the affected limb, and perform continuous mirror rehabilitation exercises.

Description

Technical Field

[0001] The present application relates to the field of robot control technology, and in particular to a mirror control system based on an upper limb robotic arm. Background Art

[0002] In some application scenarios where the robot needs to be manually remotely controlled, the controller needs to exercise high-sensitivity control over the robot, such as quad-rotor drones, surgical robotic arms, and game objects in some VR\AR scenarios. The control of these scenarios is based on the real-time motion acquisition of human limb movements. The entire execution process needs to be as short as possible, that is, the entire process of acquisition, sending, receiving, processing, and execution should be as fast as possible.

[0003] Traditional robot control systems usually use input devices such as remote controls or keyboards for control, but this approach has certain limitations. In some application scenarios that require highly precise control, such as hovering of quad-rotor drones, fine manipulation of surgical robotic arms, and control of game objects in VR / AR scenarios, input devices such as remote controls or keyboards are inconvenient to operate, and the high sensitivity requirements for the controller's hand movements make traditional control methods unable to meet them.

[0004] Therefore, it is necessary to provide a robot control system to solve the technical problems that the prior art is inconvenient to operate and cannot provide high-sensitivity motion acquisition for the affected limb. Summary of the invention

[0005] The technical purpose of the present application is to provide a mirror control system based on an upper limb robotic arm to address the technical problems that existing robot control systems are inconvenient to operate and cannot provide high-sensitivity motion acquisition for the affected limb.

[0006] In order to achieve the above technical objectives, this application adopts the following technical solutions.

[0007] In a first aspect, the present application provides a mirror control system based on an upper limb robotic arm, comprising:

[0008] A posture data acquisition module, wherein the posture data acquisition module is configured to acquire posture information of a selected part of the patient's healthy upper limb on an X-axis, a Y-axis, and a Z-axis;

[0009] A posture data sending module, wherein the posture data sending module is configured to send the posture information to a motion intention recognition module;

[0010] A movement intention recognition module is configured to recognize the patient's movement intention according to the posture information, and generate driving information of the motor at the corresponding axial rotation angle according to the posture information; when it is determined that the patient's movement intention is a movement state, the driving information is sent to the joint motor control module;

[0011] The joint motor control module is configured to control the motor according to the driving information, and the motor is used to control the mechanical structure movement of the upper limb robotic arm and the joint corresponding to the part.

[0012] Furthermore, the posture data acquisition module includes a wearing body and a main body, the wearing body is used to be worn at a selected position of the healthy upper limb of the patient; the main body is a cubic structure, and is provided with a first IMU module for collecting X-axis posture information, a second IMU module for collecting Y-axis posture information, and a third IMU module for collecting Z-axis posture information; the first IMU module, the second IMU module and the third IMU module each include two IMU sensors arranged on opposite sides of the cubic structure; the result of adding the measurement data of the two IMU sensors arranged on the opposite sides is the posture information of the corresponding axis.

[0013] Further, the posture information includes rotation angular velocity;

[0014] The patient's movement intention is identified according to the posture information, including:

[0015] Calculate the mean and variance of the angular velocity of each axis in the time sliding window respectively;

[0016] If the mean and the variance are both greater than respective preset thresholds, the patient's movement intention is determined to be a movement state;

[0017] If the mean and the variance are both less than or equal to the respective thresholds, it is determined that the patient's movement intention is in a stop state.

[0018] Furthermore, the posture data sending module adopts a wifi communication protocol.

[0019] Further, the mirror control system includes a plurality of posture information acquisition modules, a plurality of the robot modules, a soft bus scheduling module and a scheduling server, each of the robot modules is provided with an upper limb mechanical arm, each of the robot modules includes the movement intention recognition module and the joint motor control module, and the posture information acquisition module includes the posture data acquisition module and the posture data sending module;

[0020] Each of the robot modules and the soft bus scheduling module has a preset network address;

[0021] The posture information acquisition module is connected to the soft bus scheduling module in a WebSocket manner; the posture information acquisition module sends the posture information to the motion intention recognition module in the robot module in the form of a WebSocket message through the soft bus scheduling module;

[0022] The scheduling server is configured to point the network address of the pre-selected robot module to the network address of the soft bus scheduling module to achieve control of the upper limb robotic arm in the pre-selected robot module.

[0023] Still further, the posture data sending module is configured to include a first sending program and a second sending program;

[0024] When the posture data sending module is successfully connected to the soft bus scheduling module, the posture data sending module monitors the content space occupied by the first sending program and the second sending program. If the memory space occupied by one of them reaches a preset recycling condition, its memory space is released and the other is enabled to send the currently obtained posture information; the first sending program and the second sending program alternately send the currently obtained posture information.

[0025] Furthermore, the posture information collection module is configured to further include:

[0026] The posture information acquisition module sends a verification message to the pre-selected robot module through the soft bus scheduling module; after receiving the confirmation frame returned by the robot module in response to the verification message, the posture information acquisition module sends the current posture information;

[0027] If the posture information acquisition module has not received the confirmation frame within the preset response time, the current posture information will be discarded.

[0028] Furthermore, the mirror control system uses an X86 industrial computer to build the ROS platform and process messages;

[0029] The modular robot module, the soft bus scheduling module and the scheduling server are all independent ROS nodes of the ROS platform;

[0030] The soft bus scheduling module is configured to set up multiple coroutines, each coroutine is responsible for the WebSocket message of each posture information acquisition module respectively, and the soft bus scheduling module receives the WebSocket message sent by each posture information acquisition module through a unified receiving port and then uses each coroutine to poll.

[0031] Furthermore, the soft bus scheduling module receives the WebSocket message, and sends the WebSocket message to the motion intention recognition module through inter-process socket communication. The motion intention recognition module uses the designated port of the local loopback port to send the drive information to the joint motor control module.

[0032] Further, generating driving information of the motor at a corresponding axial rotation angle according to the posture information includes:

[0033] Determine the total control amount Path of the motor's X-axis rotation angle within the preset time sliding window, and the expression is as follows:

[0034]

[0035] u(k)=(A+B)*wx k-1 +B*wx k ,

[0036] Where u(k) is the control value of the motor's rotation angle on the X-axis at time k, A is the mean of the X-axis angular velocity in the preset time sliding window, B is the variance of the X-axis velocity in the preset time sliding window, and wx k-1 is the angular velocity at time k-1 in the X-axis direction, wx k is the angular velocity at time k in the X-axis direction, and N is the length of the preset time sliding window;

[0037] The method for determining the total control amount Path of the motor's rotation angle on the Y-axis and the Z-axis within the preset time sliding window is the same as the method for determining the total control amount Path of the motor's rotation angle on the X-axis.

[0038] Compared with the prior art, the mirror control system based on the upper limb mechanical arm provided by the present application has the following advantages: the embodiment of the present application instantly captures the posture information of the selected part of the patient's healthy upper limb through the posture data acquisition module, controls the mechanical arm of the affected upper limb based on the posture information of the healthy upper limb, and realizes the mirror movement of the healthy upper limb and the affected upper limb; intelligently identifies the patient's movement intention according to the collected posture information, and generates corresponding motor drive information to realize automatic control of the mechanical arm. The system is simple to operate. The patient (or rehabilitation therapist) only needs to operate the healthy upper limb to automatically complete the control of the affected upper limb mechanical arm. The system can accurately capture the posture information of the patient's healthy upper limb in the X-axis, Y-axis and Z-axis. This highly sensitive motion acquisition helps to more accurately identify the patient's movement intention, thereby realizing more sophisticated mechanical arm control. BRIEF DESCRIPTION OF THE DRAWINGS

[0039] The drawings described herein are for explanation purposes only and are not intended to limit the scope of the present application in any way. In addition, the shapes and proportional dimensions of the components in the drawings are only for illustration purposes and are used to help understand the present application. They do not specifically limit the shapes and proportional dimensions of the components of the present application. Under the guidance of the present application, those skilled in the art can select various possible shapes and proportional dimensions to implement the present application according to specific circumstances. In the drawings:

[0040] Figure 1A schematic diagram of the structure of a mirror control system based on an upper limb robotic arm provided in an embodiment;

[0041] Figure 2 A schematic diagram of an application scenario of a mirror control system based on an upper limb robotic arm provided in an embodiment;

[0042] Figure 3 This is a schematic diagram of the wearing method of the posture data set module in the embodiment;

[0043] Figure 4 A schematic diagram of the structure of a posture data acquisition module in a mirror control system based on an upper limb robotic arm provided in an embodiment;

[0044] Figure 5 A schematic diagram of the decomposed structure of a posture data acquisition module in a mirror control system based on an upper limb robotic arm provided in an embodiment;

[0045] Figure 6 A schematic diagram of the IMU sensor principle of the posture data acquisition module in the mirror control system based on the upper limb mechanical arm provided in the embodiment;

[0046] Figure 7 Schematic diagram of the zero-point drift of the IMU sensor data on the positive side in the embodiment;

[0047] Figure 8 The zero point drift of the contralateral IMU sensor data in the embodiment;

[0048] Fig. 9 The zero drift of the IMU sensor data after the IMU module corrects it in the embodiment;

[0049] Fig.10 A schematic diagram of a flow chart of a motion intention recognition module determining a motion state in an embodiment;

[0050] Fig.11 A schematic structural diagram of a mirror control system based on an upper limb robotic arm provided in another embodiment;

[0051] Fig.12 A schematic diagram of a process for establishing a connection between a posture information acquisition module and a soft bus scheduling module in an embodiment;

[0052] Fig.13 It is a schematic diagram of the double-end retransmitter flow of the posture information acquisition module in the embodiment;

[0053] Fig.14 A schematic diagram of the ROS system architecture and message sending and receiving process of the mirror control system based on the upper limb robotic arm provided in the embodiment;

[0054] Figure numerals: 1-patient, 2-robot module, 3-robotic arm, 4-posture information acquisition module, 41-wearing body, 42-main body, 431-first IMU sensor, 432-second IMU sensor, 433-third IMU sensor, 434-fourth IMU sensor, 435-fifth IMU sensor. DETAILED DESCRIPTION

[0055] In order to enable those skilled in the art to better understand the technical solutions in the present application, the technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work should fall within the scope of protection of this application.

[0056] In the description of this application, the terms "first" and "second" are used for descriptive purposes only and should not be understood as indicating or implying relative importance or implicitly indicating the number of technical features indicated. Therefore, the features defined as "first" and "second" may explicitly or implicitly include one or more features.

[0057] Some robot control system solutions try to use sensor equipment solutions to collect the controller's limb movement in real time to realize active driving of the robot arm. The sensor equipment solutions mainly include potentiometer solutions, mechanical sensor solutions and motor torque control solutions. Among them, the main product of the potentiometer solution is the joystick. According to the principle, there are Hall potentiometers, carbon film potentiometers, etc. Carbon film potentiometers are used more frequently, and many teaching pendants are equipped with potentiometers. Mechanical sensor solutions such as piezoelectric sensors are often deployed on the equipment with fixed grips. The motor torque control solution is common in collaborative robot arm drag teaching, using the motor's own current loop for torque estimation.

[0058] The potentiometer solution can be made into an operating lever and deployed at the end of the rehabilitation robot arm. The disadvantage is that the operator's hand must hold the controller to ensure real-time control of the equipment. The carbon film potentiometer will wear out after long-term use, and zero bias correction is required according to the requirements of control accuracy and control method.

[0059] Mechanical sensors have high precision and can measure force accurately. However, their disadvantage is that they are usually expensive and the operator must hold the handle at all times to control them. They are commonly found in some high-precision instruments and some product models that are not sensitive to cost.

[0060] The motor torque control solution does not require the installation of additional sensors. The load current changes generated when the motor is pushed by human hands are used to control the robot arm. Many collaborative robot arms use this solution to directly drag and teach at the end of the robot arm. The disadvantage is that in some cases, the motion trajectory and load current noise are too large to facilitate the dexterous control of the robot arm, and the accuracy is not high.

[0061] The embodiments of the present application are intended to provide a mirror control system based on an upper limb robotic arm to address the technical problems that existing robot control systems are inconvenient to operate and cannot provide high-sensitivity motion acquisition for the affected limb.

[0062] The present application is further described below in conjunction with the accompanying drawings and specific embodiments.

[0063] See also Figure 1 and Figure 2 This embodiment provides a mirror control system based on an upper limb mechanical arm, including a posture data acquisition module, a posture data transmission module, a motion intention recognition module and a joint motor control module. The posture data acquisition module is configured to collect posture information of a selected part of the healthy upper limb of the patient 1 on the X-axis, Y-axis and Z-axis; the posture data transmission module is configured to send the posture information to the motion intention recognition module.

[0064] The movement intention recognition module is configured to recognize the movement intention of the patient 1 according to the posture information, and to generate the driving information of the motor at the corresponding axial rotation angle according to the posture information; when it is determined that the movement intention of the patient 1 is a movement state, the driving information is sent to the joint motor control module. The joint motor control module is configured to control the motor according to the driving information, and the motor is used to control the mechanical structure movement of the upper limb mechanical arm 3 and the corresponding joint of the selected part of the healthy upper limb.

[0065] In some embodiments, the application scenarios of the mirror control system are as follows: Figure 2 and Figure 3 As shown. The posture data acquisition module is worn on the healthy upper arm and forearm of the patient 1 to collect the posture information of the upper arm and forearm on the X-axis, Y-axis and Z-axis, and the posture information is sent to the movement intention recognition module, so that the movement intention recognition module determines that the movement intention of the patient 1 is a movement state, and sends the driving information to the joint motor control module to drive the motor, which is used to control the mechanical structure movement of the upper limb mechanical arm 3 and the corresponding joint of the position where the posture data acquisition module is worn. The upper limb mechanical arm 3 is tied to the affected upper limb of the patient 1, thereby realizing rehabilitation training for the affected limb of the patient 1.

[0066] The mirror control system provided in the embodiment of the present application uses an inertial sensor (IMU sensor) worn on the upper limb of patient 1 to estimate the movement angle of the healthy upper limb of patient 1. Compared with a fixed grip-type strain gauge force sensor, the entire acquisition process is more friendly. After the posture data acquisition module is tied, it can be driven by the natural movement of the healthy side of patient 1. There is no need to keep the hand on the control stick all the time. The mechanical information input is more consistent. There is no oscillation caused by the reverse input of the grip by the fixed sensor in the sudden start and stop state, or the movement trajectory of the motor torque control scheme in some cases. The load current may have too much noise. It can provide highly sensitive motion acquisition for the affected limb and perform continuous mirror rehabilitation exercises.

[0067] In some embodiments, Figure 4 and Figure 5 As shown, the posture data acquisition module includes a wearing body 41 and a main body 42, the wearing body 41 is used to be worn at a selected position of the healthy upper limb of the patient 1; the main body 42 is a cubic structure, and is provided with a first IMU module for collecting X-axis posture information, a second IMU module for collecting Y-axis posture information, and a third IMU module for collecting Z-axis posture information; the first IMU module, the second IMU module, and the third IMU module each include two IMU sensors arranged on opposite sides of the cubic structure; the result of adding the measurement data of the two IMU sensors arranged on the opposite sides is the posture information of the corresponding axis.

[0068] In the embodiment of the present application, the posture data acquisition module adopts a method of deploying IMU sensors in multiple positions, so that a group of IMU sensors can obtain the minimum drift error when the patient 1 is in a static state, thereby improving the sampling accuracy.

[0069] In some embodiments, the posture data transmission module adopts the wifi communication protocol. The posture data transmission module can be arranged in the main body 42 (not shown in the figure). Optionally, the posture data acquisition module and the posture data transmission module are deployed on a set of minimum system modules supporting wifi.

[0070] In some embodiments, the posture data acquisition module can transmit data to the posture data sending module through a wired communication interface (such as RS-232, RS-485, Ethernet, etc.).

[0071] IMU sensor is a type of inertial measurement sensor, the principle diagram is as follows Figure 6 As shown, the acceleration and rotational angular velocity in the XYZ directions can be measured. After the IMU device is fixed in position, a set of angular velocity data sequences with zero drift (in the stopped state) can be measured in the stopped state.

[0072] In some embodiments, the wearable body 41 in the attitude data acquisition module is a wearable ring, and an IMU sensor can be installed on each opposite side of the cube, so that each opposite side can constitute a pair of sensor groups, such as Figure 5 As shown in the figure, the first IMU sensor 431 and the fourth IMU sensor 434, as well as the third IMU sensor 433 and the fifth sensor 435, are provided opposite to the second IMU sensor 432. An IMU sensor (not shown in the figure) is also provided. Since the two opposite IMU sensors have the same magnitude and opposite directions of rotation angular velocity, of course, the zero point offsets of the two are also the same magnitude and opposite directions, as shown in the figure. Figure 7 The zero drift data on the positive side (+9 up and down) is displayed. Figure 8 The zero drift data of the opposite side (-9 above and below) is shown, such as Fig. 9 As shown, the measurement data of the two oppositely arranged IMU sensors are added to obtain a set of zero drift data (0 up and down) in the stopped state. The zero drift problem can be corrected by installing an IMU sensor on the opposite side of the cube structure.

[0073] In some embodiments, the attitude data acquisition module continuously collects attitude information and continuously integrates the X, Y and Z three-axis acceleration and three-axis angular velocity. According to the collected attitude information, the driving information of the motor at the corresponding axial rotation angle can be generated. Optionally, the time sliding window length can be dynamically adjusted, and the integration result can be processed by windowing observation. The correspondence between the attitude information collected by the IMU sensor and the transmission message is as follows:

[0074]

[0075] Where Acc represents acceleration, Gry represents angular velocity, Qua represents fused Euler angles, and the x, y, and z subscripts represent their respective axes: 0 arrive Respectively represent the packaged data in a data packet.

[0076] The single-joint posture matrix needs to be converted into a matrix data format. The assignment process of the posture matrix is ​​as follows:

[0077]

[0078] Pose 0 To Pose 8 It converts the packaged data format in the data packet into a single-joint posture matrix.

[0079] After obtaining the states of each single joint one by one, they are combined into a multi-joint state group matrix, which contains the angular velocity, gravity acceleration and Euler angle information of multiple joints. As shown in formula (3), there are 8 joint groups.

[0080]

[0081] The mirror control system based on the upper limb robotic arm provided in this embodiment uses a posture data acquisition module to collect the human posture information of the healthy upper limb of patient 1. Compared with the fixed grip type strain gauge force sensor, the entire acquisition process is more friendly. The patient 1 can drive the robotic arm 3 with natural movement, and there is no need to keep the hand on the control rod all the time. The mechanical information input is more coherent, and high-sensitivity motion acquisition can be provided for the affected limb, allowing continuous mirror rehabilitation exercises.

[0082] In some embodiments, typical user control states include a motion state and a stop state. The specific judgment method is to judge whether the user is performing a sudden motion. For example, when waving an arm, a large angular velocity change will occur in the motion direction, while when it is in a stop (or static) state, such a huge change will not occur. Therefore, the sudden change in angular velocity is the basis for distinguishing between motion and stop.

[0083] In the motion state, the motion intention recognition module sends the motor drive information to the joint motor control module, thereby allowing the posture information data to directly drive the mechanical arm 3 to move. In the stop state, the motor drive information is not sent to the joint motor control module, thereby shielding the driving of the mechanical arm 3 by the posture information data. This can avoid the shaking of the mechanical arm 3 in the static state. If the healthy arm is always placed and motionless, the motion state cannot be determined, and the motor is in the off state, and it is impossible to distinguish the movement of the affected upper limb, so that "raise the hand and move, stop the hand and do not move".

[0084] In some embodiments, it is impossible to process all data in actual processing, but a certain length of data is selected for time sliding window analysis. The way to identify the change of angular velocity is to construct a data analyzer, and perform variance and mean calculations on all data entering the time sliding window length in real time. If the variance and mean are too large, it can be judged as a moving state, otherwise it is a static state. The length of the time sliding window can be customized, and the entry rule is first-in-first-out. Each time the window is full, it will be popped up.

[0085] In some embodiments, the movement intention recognition module recognizes the movement intention of the patient 1 according to the posture information, such as Fig.10 As shown, it includes: respectively calculating the mean and variance of the angular velocity of each axis rotation in the time sliding window; if the mean and variance are both greater than the preset respective thresholds, then judging that the movement intention of patient 1 is a moving state; if the mean and variance are both less than or equal to the respective thresholds, then judging that the movement intention of patient 1 is a stopping state.

[0086] See formula (4) and formula (5) for details.

[0087]

[0088]

[0089] Among them, wx i represents the angular velocity of the X axis at time i (i.e., the rotational angular velocity), N is the length of the time sliding window, E(wx) represents the mean of all angular velocity values ​​of the X axis in the time sliding window, var(wx) represents the variance of all angular velocity values ​​in the time sliding window, wx k represents the angular velocity along the X axis at time k.

[0090] In some embodiments, a typical user control state also includes a recovery state. The motion intention recognition module is configured to recognize the motion intention of patient 1 based on the posture information, and to generate drive information of the motor at the corresponding axial rotation angle based on the posture information; when it is determined that the motion intention of patient 1 is a recovery state, the drive information is sent to the joint motor control module to drive the robotic arm 3 to return to the zero position state. The recovery state requires patient 1 to place the healthy arm vertically on one side, which will generate a gravity acceleration close to the full scale on the IMU sensor, and this judgment basis can very clearly drive the robotic arm 3 to recognize the recovery state, and drive the motor to automatically return the robotic arm 3 to the zero position.

[0091] Based on the above embodiments, some embodiments provide a mirror control system based on an upper limb robotic arm, such as Fig.11 As shown, it includes multiple posture information acquisition modules 4, multiple robot modules 2, soft bus scheduling modules (soft bus servers) and scheduling servers, each robot module 2 is provided with an upper limb mechanical arm 3, and each robot module 2 includes a motion intention recognition module and a joint motor control module. Each robot module 2 and the soft bus scheduling module have a preset network address. The posture information acquisition module 4 includes a posture data acquisition module and a posture data sending module, and the posture data acquisition module and the posture data sending module are deployed on a set of minimum system modules (posture information acquisition module 4) supporting wifi, so that the posture information acquisition module 4 is more compact and lightweight, reduces the wiring between components, and reduces the complexity and failure rate.

[0092] like Fig.11As shown, an editable network and its soft bus are shown. The top scheduling server is connected to the robot module 2 group that needs to be controlled, and the bottom group of multiple posture information acquisition modules 4 (subnet mask 255.255.255.0, the current number of robots and the number of sensors does not exceed 254, the specific number can be edited). The posture information acquisition module 4 is connected to the soft bus scheduling module in a WebSocket manner: the posture information acquisition module 4 sends the posture information to the motion intention recognition module in the robot module 2 in the form of a WebSocket message through the soft bus scheduling module; the scheduling server is configured to point the network address of the pre-selected robot module 2 to the network address of the soft bus scheduling module, so as to realize the control of the upper limb manipulator 3 in the pre-selected robot module 2.

[0093] like Fig.11 As shown, all posture information acquisition modules 4 are connected to the soft bus scheduling module with the address of 192.168.6.27 in the form of WebSocket, so that all IMU sensor data are published on this soft bus for the robot module 2 to call. The scheduling server only needs to point the network address of the robot module 2 that needs to be controlled at this time to the address of the soft bus scheduling module, and the robot module 2 can obtain all the sensor information.

[0094] The purpose of using this topology in the mirror control system based on the upper limb robot arm 3 is that not only can a group of sensors be edited by software to control a robot module 2 in a multi-point to single-point manner, but the information flow of the sensor can be easily changed as needed, and the scheduling server only needs to be responsible for modifying the information direction of the sensor and does not need to undertake data forwarding. The overall network is a star structure, but most of the time it works in a point-to-point manner. This saves a lot of bandwidth compared to maintaining a large star network for real-time transmission and reception.

[0095] In some embodiments, the attitude data sending module in the attitude information acquisition module 4 adopts a wifi-based communication solution. The platform is a high-speed communication terminal based on esp32 (microcontroller), equipped with esp-now instant connection technology, which can achieve multi-node second-level connection, and has the characteristics of automatically discovering servers within 100 meters, and can continuously and real-timely high-speed delivery of collected IMU messages. A good message delivery and distribution mechanism requires reliable and fast connection time, sufficient bandwidth, message verification and efficient server receiving interface, which are completed together with the same set of network protocol interfaces. In actual development, multiple optimizations can be made on the public version of the WebSocket process and server platform provided by the esp platform to meet the message frequency requirements in the specific implementation method.

[0096] The embodiment of the present application ensures that the posture information acquisition module 4 and the robot module 2 have sufficient bandwidth during the communication process, and can complete data upload at the fastest possible speed while completing the acquisition. This requires that the sensor and the host computer adopt the same bandwidth communication protocol, and the client connection status attributes can be edited. When the posture information acquisition module 4 and the host computer soft bus scheduling module communicate in the same network in the form of C / S mode using the same technology using WebSocket communication, it is convenient to synchronize and optimize at both ends.

[0097] In actual transmission, the technical combination of wireless sensors and robots, with a frequency of 5KB / s and 70HZ measured on the robot side, synchronously refreshes the displayed posture data on the robot side. This posture data refresh rate helps to ensure that the motion intention recognition module and the joint motor control module can obtain a sufficient amount of effective information in a short time interval and complete some high-real-time control. The posture information acquisition module 4 and the robot module 2 communicate in a standardized C / S mode (Client / Server), which makes all connection properties (bandwidth, delay, frequency, and message flow) schedulable and editable, and has a certain degree of scalability, that is, a rehabilitation therapist can simultaneously command multiple patient 1 robotic arms 3 for rehabilitation in a mirrored manner in the classroom.

[0098] like Fig.12 As shown, in some embodiments, the posture information acquisition module 4 is configured to also include the posture information acquisition module 4 establishing a connection with the soft bus scheduling module before sending each posture information. After the posture information acquisition module 4 is turned on, it will read the LAN IP and port of the corresponding soft bus scheduling module stored in the memory, and try to establish a connection. If the request to try to establish a connection fails, the on-chip program will be restarted to re-establish the connection. If the preset number of attempts (such as ten times) fails, it will enter the standby sleep mode. If the connection is passed, the corresponding interface will be read to try to establish a connection with the host computer server (also known as the soft bus server). If the connection is successfully established, it will be ready to enter the sending mode, waiting for the posture information attempted to be sent to the receiving port corresponding to the soft bus scheduling module.

[0099] Once the posture information acquisition module 4 is turned on, it will continue to update and collect the position posture information. During this process, once the connection with the soft bus scheduling module is successfully established, the posture information acquisition module 4 will transmit the corresponding posture information to the sending object entity.

[0100] In some embodiments, when the posture information acquisition module 4 sends posture information, there may be a connection failure caused by insufficient signal strength and other reasons. In some embodiments, a reachability confirmation is performed for each transmission, that is, the posture information acquisition module 4 is configured to include before sending each posture information: the posture information acquisition module 4 sends a verification message to the pre-selected robot module 2 through the soft bus scheduling module; the posture information acquisition module 4 receives the confirmation frame returned by the robot module 2 in response to the verification message, and then sends the current posture information; if the posture information acquisition module 4 exceeds the preset response time and still does not receive the confirmation frame, the robot module 2 will trigger a timeout verification, and the posture information data packet that has not been responded to for more than the preset number of times is discarded. The same timeout verification mechanism is also used in the process of requesting to establish a connection and requesting the soft bus scheduling module (soft bus server) to connect.

[0101] In some embodiments, one of the main optimizations for the sending performance of the posture information acquisition module 4 is that at a higher sending speed and frequency (5Kb / s, 70Hz) of custom data messages, the applicant found that the official websocket interface has certain efficiency problems, which is manifested in that after a fixed number of sent messages, the posture data sending module will always pause for a certain period of time before starting the next sending process. This may not be a problem under low-speed and low-frequency conditions, but for the posture data sending module that requires a certain real-time performance, it means that part of the real-time data is lost, which is obviously not allowed.

[0102] In response to this problem, the applicant analyzed and believed that after a large number of data messages are sent, the data cache may need to release some continuous memory space to receive and store the next collection data. However, after a large amount of data is collected and sent, this part of the heap space may not be released synchronously with the collection cycle. This will cause the collection program to wait for the sending program to complete the recycling and release. The specific solution is, Fig.13 As shown, two sending programs are instantiated on the single chip in the posture information acquisition module 4. When the No. 1 sending program has completed a large number of sending tasks and needs to recycle the released heap space, the No. 2 sending program is notified to send the current data to be sent. When the No. 2 sending program completes a large number of sending tasks and needs to recycle the released heap space, the No. 1 sending program has already completed the release and recycling. The No. 2 sending program transfers the information to be sent to the No. 1 sending program, and the No. 2 sends the information. Such alternating sending can ensure that in the face of a continuous data stream, there is always a sending program that can quickly send the information.

[0103] In some embodiments, the image control system uses an X86 industrial computer to build the ROS platform and process messages, such as Fig.14As shown; wherein the module robot module 2, the soft bus scheduling module and the scheduling server are all independent ROS nodes of the ROS platform; the soft bus scheduling module is configured to set multiple coroutines, each coroutine is responsible for the WebSocket message of each posture information acquisition module 4, and the soft bus scheduling module receives the WebSocket message sent by each posture information acquisition module 4 through a unified receiving port, and then uses each coroutine to poll. This system uses an asynchronous communication host server based on the master node of ROS, and the host server is deployed with a soft bus scheduling module. The host server platform is a PC with an X86 architecture, which uses utf-8 for encoding and decoding and package unpacking, and uses JSON strings for message parsing. The server can process 700,000 unpacking rates per second, and the inter-process message transmission rate is 5.5KB / S, and it continues to communicate and control at a frequency of 70hz. Considering the possible situation that multiple posture information acquisition modules 4 (i.e., acquisition terminals) are online at the same time, and facing multiple message nodes online at the same time, if a large number of independent ROS nodes are written, a large amount of TCP / IP bandwidth inside the system will be occupied, and unnecessary scheduling delays will also be introduced. In the embodiment, a concurrent method is used to receive messages within a single node, which can save the scheduling overhead generated by parallel processes. However, the use of ROS threads here will cause a related problem. If the message receiving component to be expanded in the future needs to design an I / O read / write module, then the blocking I / O thread switching problem it generates will slow down the overall receiving speed of the soft bus scheduling module (the soft bus scheduling module is the processing center of the robot arm 3), and the logic of thread switching needs to be written separately. If the thread can partially complete the task during the receiving process and then wait for the next user's scheduling execution, it will actively hand over the scheduling right, so directly implementing a coroutine interface can meet the actual application needs.

[0104] In some embodiments, the soft bus scheduling module receives the WebSocket message, sends the WebSocket message to the motion intention recognition module through the socket communication between processes, and the motion intention recognition module uses the designated port of the local loopback interface to send the drive information to the joint motor control module. The loopback interface (Loopback Interface) is a virtual network interface, also known as the local loopback address (Loopback Address) or the local loopback interface. This interface does not exist physically, but is a logical interface inside a network device (such as a router, computer, etc.), which is mainly used for self-testing or internal communication of the device.

[0105] In some embodiments, driving information of the motor at a corresponding axial rotation angle is generated based on the posture information, including: determining the mean A of the X-axis angular velocity obtained by the posture data acquisition module within a preset time sliding window and the variance B of the X-axis line velocity obtained by the posture data acquisition module within the preset time sliding window, as expressed in formula (6).

[0106]

[0107] Item A is used to obtain the angular velocity change of a certain axis of the IMU sensor, wx i is the angular velocity at time i in the x-axis direction, N is the length of the time sliding window, and item B is used to obtain the linear velocity change of a certain axis of the IMU sensor; vx k is the linear velocity at time k in the sliding window, E(vx) is the mean of all linear velocity values ​​in the sliding window, and the specific calculation method is similar to A.

[0108] Determine the control value u(k) of the motor's rotation angle on the X axis at time k. The expression is as follows:

[0109] u(k)=(A+B)*wx k-1 +B*wx k (7)

[0110] Formula (7) is an input parameter equation, which is used to update the motor control quantity of a certain axis at each moment, and represents the motor control quantity of the axis at moment k. In this step, the fusion of angular velocity and linear velocity is realized.

[0111] Determine the total control measurement Path of the motor's X-axis rotation angle within the preset time sliding window. The rotation angle Path is the accumulation of the motor's X-axis control amount within the time sliding window. The expression is as follows:

[0112]

[0113] wx k-1 wx is the angular velocity at time k-1 in the X-axis direction obtained by the attitude data acquisition module. k is the angular velocity at time k in the X-axis direction obtained by the attitude data acquisition module, and N is the length of the preset time sliding window; the method for determining the total control amount of the motor's rotation angle on the Y-axis and Z-axis within the preset time sliding window is the same as the method for determining the total control amount Path of the motor's rotation angle on the X-axis.

[0114] The embodiment of the present application uses the variance and mean observations in the time sliding window observer to measure the linear acceleration and angular acceleration, and uses the mean of the angular velocity and the variance of the linear velocity as weight parameters to update the input parameter equation in real time. For the input of a single motor, the control quantity input based on the current angular velocity and corrected by the linear velocity and angular velocity weight parameters at the previous moment is used as the input variable. After integration, the control quantity of the specific motor rotation angle per unit time can be obtained.

[0115] The above is a detailed introduction to the mirror control system based on the upper limb robotic arm provided by the present application. Specific examples are used in this article to illustrate the principles and implementation methods of the present application. The description of the above embodiments is only used to help understand the concept of the present application and should not be understood as limiting the scope of protection of the present application.

Claims

1. A mirror control system based on an upper limb robotic arm, characterized in that: include: A posture data acquisition module, wherein the posture data acquisition module is configured to acquire posture information of a selected part of the patient's healthy upper limb on an X-axis, a Y-axis, and a Z-axis; A posture data sending module, wherein the posture data sending module is configured to send the posture information to a motion intention recognition module; A movement intention recognition module is configured to recognize the patient's movement intention according to the posture information, and generate driving information of the motor at the corresponding axial rotation angle according to the posture information; when it is determined that the patient's movement intention is a movement state, the driving information is sent to the joint motor control module; A joint motor control module, configured to control a motor according to the drive information, wherein the motor is used to control the movement of the mechanical structure of the upper limb mechanical arm and the joint corresponding to the part; The mirror control system includes a plurality of posture information acquisition modules, a plurality of robot modules, a soft bus scheduling module and a scheduling server, each of the robot modules is provided with an upper limb mechanical arm, each of the robot modules includes the movement intention recognition module and the joint motor control module, and the posture information acquisition module includes the posture data acquisition module and the posture data sending module; Each of the robot modules and the soft bus scheduling module has a preset network address; The posture information acquisition module is connected to the soft bus scheduling module in a WebSocket manner; the posture information acquisition module sends the posture information to the motion intention recognition module in the robot module in the form of a WebSocket message through the soft bus scheduling module; The scheduling server is configured to point the network address of the pre-selected robot module to the network address of the soft bus scheduling module to achieve control of the upper limb robotic arm in the pre-selected robot module.

2. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: The posture data acquisition module includes a wearing body and a main body, the wearing body is used to be worn at a selected position of the healthy upper limb of the patient; the main body is a cubic structure, and is provided with a first IMU module for collecting X-axis posture information, a second IMU module for collecting Y-axis posture information, and a third IMU module for collecting Z-axis posture information; the first IMU module, the second IMU module, and the third IMU module each include two IMU sensors arranged on opposite sides of the cubic structure; the result of adding the measurement data of the two IMU sensors arranged on the opposite sides is the posture information of the corresponding axis.

3. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: The posture information includes rotation angular velocity; Performing patient movement intention recognition according to the posture information includes: Calculate the mean and variance of the angular velocity of each axis in the time sliding window respectively; If the mean and the variance are both greater than respective preset thresholds, the patient's movement intention is determined to be a movement state; If the mean and the variance are both less than or equal to the respective thresholds, it is determined that the patient's movement intention is in a stop state.

4. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: The posture data sending module adopts the wifi communication protocol.

5. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: The posture data sending module is configured to include a first sending program and a second sending program; When the posture data sending module is successfully connected to the soft bus scheduling module, the posture data sending module monitors the content space occupied by the first sending program and the second sending program. If the memory space occupied by one of them reaches a preset recycling condition, its memory space is released and the other is enabled to send the currently obtained posture information; the first sending program and the second sending program alternately send the currently obtained posture information.

6. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: The posture information collection module is configured to further include, before sending each of the posture information: The posture information acquisition module sends a verification message to the pre-selected robot module through the soft bus scheduling module; after receiving the confirmation frame returned by the robot module in response to the verification message, the posture information acquisition module sends the current posture information; If the posture information acquisition module has not received the confirmation frame within the preset response time, the current posture information is discarded.

7. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: The mirror control system uses an X86 industrial computer to build the ROS platform and process messages; The robot module, the soft bus scheduling module and the scheduling server are all independent ROS nodes of the ROS platform; The soft bus scheduling module is configured to set up multiple coroutines, each coroutine is responsible for the WebSocket message of each posture information acquisition module respectively, and the soft bus scheduling module receives the WebSocket message sent by each posture information acquisition module through a unified receiving port and then uses each coroutine to poll.

8. The mirror control system based on the upper limb robotic arm according to claim 7, characterized in that: The soft bus scheduling module receives the WebSocket message, and sends the WebSocket message to the motion intention recognition module through inter-process socket communication. The motion intention recognition module uses the designated port of the local loopback port to send the drive information to the joint motor control module.

9. The mirror control system based on the upper limb robotic arm according to claim 1, characterized in that: Generating driving information of the motor at a corresponding axial rotation angle according to the posture information, including: Determine the total control amount Path of the motor's X-axis rotation angle within the preset time sliding window, and the expression is as follows: u(k)=(A+B)*wx k-1 +B*wx k , Where u(k) is the control value of the motor's rotation angle on the X-axis at time k, A is the mean of the X-axis angular velocity in the preset time sliding window, B is the variance of the X-axis velocity in the preset time sliding window, and wx k-1 is the angular velocity at time k-1 in the x-axis direction, wx k is the angular velocity at time k in the x-axis direction, and N is the length of the preset time sliding window; The method for determining the total control amount Path of the motor's rotation angle on the Y-axis and the Z-axis within the preset time sliding window is the same as the method for determining the total control amount Path of the motor's rotation angle on the X-axis.

Images