Based on the multisensor correction communication method of CAN
Technical field
The present invention relates to the bearing calibration of sensing data, specifically one is communicated by CAN, relates to the bearing calibration of acceleration and gyroscope two kinds of sensing datas.
Background technology
Along with the development of science and technology, robot appears at the visual field of people more and more continually, in the life of people, as automation pilot, air-robot.And in the control of robot, the precision of attitude is the most key, calculate attitude transducer in acceleration and gyroscope the most common.Generally our degree of will speed up and gyroscope two kinds of transducers are collectively referred to as MEMS (micro-electro-mechanical system), referred to as measuring unit node in this patent.Owing to there is temperature variant null offset (bias) and temperature variant scale factor (scale) distortion, and the linear processes error such as nonopiate coupling error (M) between three axles that bring of the three-dimension sensor that brings of production technology or welding procedure.
Need in the project to correct before measuring unit node uses, in order to the convenience produced in batches, need one simple and effective bearing calibration, need correct multiple measuring unit node simultaneously and the low cost of calibration equipment will be controlled.Now need the data volume of measurement comparatively large, frequency is higher to improve certainty of measurement, and carries out multisensor correction to save time, and due to amount of calculation larger, measuring unit node cannot correct alone, needs computer to carry out computing, therefore the communication protocol requirements needed is as follows:
1) without congested, between measuring unit node, there is no prioritization, and traffic model be a main frame to multiple measuring unit node, when measuring unit node increases, communication can not occur that data block.And when multiple measuring unit node sends data and do not control simultaneously, have very large probability to cause data congestion.
2) without packet loss, owing to needing the stable time interval in most of sensor calibration algorithm, after packet loss, noise can be caused to algorithm, reduce accuracy; And due to the data volume of single measurement data comparatively large, need subpackage, after packet loss, easily occur data dislocation or data exception, data congestion.
3) quick, due in sensing algorithm, the measuring intervals of TIME of data is shorter, and the result of correction is more accurate, so data acquisition is very fast, if can take double data bandwidth by polling mode.
CAN is the abbreviation (hereinafter referred to as CAN) of Controller Area Network, is the serial communication protocol of ISO International standardization, and is widely used in the aspects such as industrial automation, boats and ships, Medical Devices, industrial equipment.Fieldbus is one of focus of nowadays automatic art, is described as the LAN (Local Area Network) of automatic field.It appear as that dcs to realize between each node in real time, data communication reliably provides strong technical support.
Summary of the invention
For the problems referred to above, the object of the present invention is to provide a kind of bearing calibration of MEMS measuring unit node, a kind of simple and effectively, batch and low cost, bearing calibration based on CAN and main-machine communication.
For achieving the above object, the technical scheme that the present invention takes is:
Based on the multisensor correction communication method of CAN, it comprises the following steps:
Step 1, measuring unit node send application numbers frame to main frame after powering on, the data segment in application numbers frame is the Unique Chip numbering of measuring unit node, and described measuring unit node comprises accelerometer and gyroscope;
After step 2, main frame receive described application numbers frame, search Hash table and record described Unique Chip numbering, meanwhile, sending numbering distribute frame to measuring unit node, described numbering is distributed frame and is comprised element number for measuring unit peer distribution and group number;
The numbering that step 3, measuring unit node receive main frame transmission is distributed frame and judges that described numbering is distributed frame and whether conformed to, if conformed to, then this measuring unit node serial number is assigned, wait for the control command of main frame, measuring unit node after numbering distribution no longer judges with its Unique Chip numbering, and judges with the element number of host assignment and group number;
Step 4, main frame carries out measurement procedure control by graphic interface, and enable corresponding measuring unit node enters correcting process, to obtain the correction matrix of each measuring unit node, described main frame is corrected measuring unit node by calibration equipment, described calibration equipment comprises horizontal revolving stage and with multiple measuring unit node slot and can the square measuring unit node installation frame put of six direction level, described horizontal revolving stage drives lower rotation by main frame by the motor that CAN controls, described square measuring unit node installation frame is fixedly installed in the upper center position of horizontal revolving stage, described measuring unit node installation is in square measuring unit node installation frame, described correction matrix comprises null offset matrix, nonopiate coupling error matrix between scale factor matrix and three axles,
Step 5, corrected after, the correction data frame with correction matrix is sent to corresponding measuring unit node by CAN by main frame, and measuring unit node records after receiving correction data frame and replaces legacy data calculating parameter.
In described step 2, the method for host assignment group number according to measuring unit node electric sequence or user selects or element number carries out, each measuring unit node can only a corresponding group number.
In described step 3, judge that the method whether described numbering distribution frame conforms to is: first judge whether measuring unit node is in and wait for numbering allocated phase, if so, then the Unique Chip again numbering being distributed chip number in frame and measuring unit node is numbered to compare and is verified.
In described step 4, described correcting process comprises the following steps:
Step 41, main frame enter the sensor data acquisition stage by the enable measuring unit node of graphic interface, now, main frame sends start frame or continues frame, and the measuring unit node that the measuring unit node of enable target group number or start element number end unit numbering starts sensor data acquisition; During data acquisition, the rotary state that main frame controls motor by CAN obtains each measuring unit node correction data, described correction data comprise accelerometer x, y, z three axle under static state the output data of six direction, gyroscope x, y, z three axle under static state six direction output data and rotate along gyroscope x, y, z three axle with fixed angles speed time real time sensor data, the output data under inactive state and real time sensor data are many groups;
Step 42, main frame are calculated by correcting algorithm and correction data and obtain correction matrix.
Described step 42 comprises the following steps:
Step 421, set up error model:
When not considering that temperature affects, correction matrix is respectively:
Wherein, in formula (1)-(3), bias, scale and M represent the nonopiate coupling error matrix between null offset matrix, scale factor matrix and three axles respectively;
Error model is expressed as:
D_M=scale*M*D_T+bias (4)
In formula (4), D_M is the data that the actual measurement of measuring unit node obtains, and D_T is the theoretic output data of measuring unit node;
Error model can be rewritten into:
D_T=N*(D_M-bias) (5)
In formula (5):
N=inv(scale*M) (6)
Step 422, obtain accelerometer and gyrostatic null offset matrix respectively:
For the null offset matrix bias of accelerometer
1: by inactive state brief acceleration meter every axially under put time, the reading of its upper and lower both direction and 1/2nd, then average and obtain;
For gyrostatic null offset matrix bias
2: by gyroscope during inactive state every axially under put time, the reading of its upper and lower both direction and 1/2nd, then quadrature and obtain;
Step 423, embody the hybrid matrix of the nonopiate coupling error matrix between scale factor matrix and three axles due to the N in formula (6), therefore, only need obtain accelerometer and gyrostatic hybrid matrix:
For the hybrid matrix N of accelerometer
1:
The calculation matrix Ba of 33 is obtained after reading by accelerometer three axles:
In formula (7), P
xyfor when accelerometer x-axis is put downwards, repeatedly reading corresponding to y-axis all deducts ybias
1after the average of trying to achieve; Ybias
1for null offset matrix bias
1zero drift value in corresponding y-axis;
And accelerometer corresponding 33 theoretical calculation matrix Aa be:
In formula (8), G is acceleration of gravity;
By theoretical calculation matrix Aa and calculation matrix Ba, obtain hybrid matrix N
1=Aa/Ba;
For gyrostatic hybrid matrix N
2:
The calculation matrix Bg of acquisition 33 is obtained after reading when being rotated by gyroscope three axles:
In formula (9), Q
xyduring for rotating along gyroscope x-axis, repeatedly reading corresponding to y-axis all deducts ybias
2after the integrated value of trying to achieve; Ybias
2for null offset matrix bias
2zero drift value in corresponding y-axis;
And gyroscope corresponding 33 theoretical calculation matrix Ag be:
In formula (10), n is for rotating the number of turns;
By theoretical calculation matrix Ag and calculation matrix Bg, obtain hybrid matrix N
2=Ag/Bg.
In described step 41, target measurement cell node receives start frame or judges after continuing frame whether start frame or the group number continued in frame are zero, if zero illustrates not to be inquiry with group number, judge whether that belonging to start element numbers end unit Serial Number Range again, if be in scope, start to measure transmission data.
In described step 41, main frame sends start frame or after continuing frame to measuring unit node, measuring unit node sends data to main frame and expands frame.
The identification fields that described data expand frame is divided into function number, data number, element number and frame ID, described function number is 0x08, described data number is the count number of current measurement data, and described element number is the numbering of current measuring unit node, and frame ID is subpackage sequence number.
Further comprising the steps of after described step 5:
After step 6, measuring unit nodes records correction matrix, again send correction data to main frame, compare with set point after main frame receives described correction data, with the difference by comparison whether in allowed band, thus judge whether successfully trimming process.
Main frame carries out measurement procedure by graphic interface and controls at least to comprise one of following content:
Carry out grouping planning to measuring unit node, now main frame sends group # distribution frame, start element is numbered the measuring unit peer distribution group # of end unit numbering; Measuring unit node receives group # and distributes frame laggard line range coupling, checks and whether belongs to range of distribution, if then record oneself ownership group number;
Be numbered replacement to measuring unit node, now main frame sends and resets frame, and the measuring unit node of the measuring unit node of target group number or start element being numbered end unit numbering is numbered replacement; Whether target measurement cell node receives the group number reset after frame in judgment frame is zero, if zero illustrates not to be inquiry with group number, judge whether that belonging to start element numbers end unit Serial Number Range again, if be in scope, reset group number and element number, after postponing at one section, again send request frame to main frame;
Stop measuring unit node measurement transmission data, now main frame sends Abort Frame or end, the measuring unit node of termination target group number or start element number the measuring unit node measurement transmission data of end unit numbering, whether the group number after target measurement cell node receives Abort Frame in judgment frame is zero, if zero illustrates not to be inquiry with group number, judge whether that belonging to start element numbers end unit Serial Number Range again, if be in scope, stop to measure transmission data, send data to main frame and complete acknowledgement frame.
Compared with prior art, beneficial effect of the present invention is:
1, bearing calibration comprise simply, the calibration equipment of low cost, the simply correcting algorithm of successful and match with it the CAN application layer protocol and sensor calibration software systems that can correct in batches, realize the correction of measure batch cell node, efficiency is high, cost is low.
2, arranged by the segmentation priority of identification fields, ensure effectively fast and safely transmitting of the many packetized data of many measuring units node.
Accompanying drawing explanation
Fig. 1 is the flow chart of the multisensor correction communication method that the present invention is based on CAN;
Fig. 2 is the structural representation of horizontal revolving stage;
Fig. 3 is the flow chart that correction matrix obtains;
Fig. 4 is CAN inclusion composition of the present invention;
Fig. 5 is CAN standard frame and expands frame format figure.
Embodiment
Below in conjunction with the drawings and specific embodiments, content of the present invention is described in further details.
Embodiment
Please refer to shown in Fig. 1, based on the multisensor correction communication method of CAN, it comprises the following steps:
Step 1, measuring unit node send application numbers frame to main frame after powering on, the data segment in application numbers frame is the Unique Chip numbering of measuring unit node, and described measuring unit node comprises accelerometer and gyroscope.
After step 2, main frame receive described application numbers frame, search Hash table and record described Unique Chip numbering, simultaneously, send numbering to measuring unit node and distribute frame, described numbering is distributed frame and is comprised element number for measuring unit peer distribution and group number, the method of host assignment group number according to measuring unit node electric sequence or user selects or element number carries out, each measuring unit node can only a corresponding group number.
The numbering that step 3, measuring unit node receive main frame transmission is distributed frame and judges that described numbering is distributed frame and whether conformed to, judge that the method whether described numbering distribution frame conforms to is: first judge whether measuring unit node is in and wait for numbering allocated phase, if so, then the Unique Chip again numbering being distributed chip number in frame and measuring unit node is numbered to compare and is verified.If conformed to, then this measuring unit node serial number is assigned, and waits for the control command of main frame, and the measuring unit node after numbering distribution no longer judges with its Unique Chip numbering, and judges with the element number of host assignment and group number.
Step 4, main frame carries out measurement procedure control by graphic interface, and enable corresponding measuring unit node enters correcting process, to obtain the correction matrix of each measuring unit node, described main frame is corrected measuring unit node by calibration equipment, please refer to shown in Fig. 2, described calibration equipment comprises horizontal revolving stage and with multiple measuring unit node slot and can the square measuring unit node installation frame put of six direction level, described horizontal revolving stage drives lower rotation by main frame by the motor that CAN controls, described square measuring unit node installation frame is fixedly installed in the upper center position of horizontal revolving stage, described measuring unit node installation is in square measuring unit node installation frame, described correction matrix comprises null offset matrix, nonopiate coupling error matrix between scale factor matrix and three axles.
Please refer to shown in Fig. 4, main frame carries out measurement procedure by graphic interface and controls at least to comprise one of following content:
(1) carry out grouping planning to measuring unit node, now main frame sends group # distribution frame, start element is numbered the measuring unit peer distribution group # of end unit numbering; Measuring unit node receives group # and distributes frame laggard line range coupling, checks and whether belongs to range of distribution, if then record oneself ownership group number;
(2) be numbered replacement to measuring unit node, now main frame sends and resets frame, and the measuring unit node of the measuring unit node of target group number or start element being numbered end unit numbering is numbered replacement; Whether target measurement cell node receives the group number reset after frame in judgment frame is zero, if zero illustrates not to be inquiry with group number, judge whether that belonging to start element numbers end unit Serial Number Range again, if be in scope, reset group number and element number, after postponing at one section, again send request frame to main frame;
(3) enable measuring unit node starts to measure transmission data, and now main frame sends start frame or continues frame, and the measuring unit node that the measuring unit node of enable target group number or start element number end unit numbering carries out measurement transmission data.Whether the group number after target measurement cell node receives start frame in judgment frame is zero, if zero illustrates not to be inquiry with group number, then judges whether that belonging to start element numbers end unit Serial Number Range, if be in scope, starts to measure transmission data.
(4) measuring unit node measurement transmission data are stopped, now main frame sends Abort Frame or end, the measuring unit node of termination target group number or start element number the measuring unit node measurement transmission data of end unit numbering, whether the group number after target measurement cell node receives Abort Frame in judgment frame is zero, if zero illustrates not to be inquiry with group number, judge whether that belonging to start element numbers end unit Serial Number Range again, if be in scope, stop to measure transmission data, send data to main frame and complete acknowledgement frame.
Please refer to shown in Fig. 3, in described step 4, described correcting process comprises the following steps:
Step 41, main frame enter the sensor data acquisition stage by the enable measuring unit node of graphic interface, now, main frame sends start frame or continues frame, and the measuring unit node that the measuring unit node of enable target group number or start element number end unit numbering starts sensor data acquisition.
Main frame sends start frame or after continuing frame to measuring unit node, measuring unit node sends data to main frame and expands frame.Please refer to shown in Fig. 5: the identification fields that described data expand frame is divided into function number, data number, element number and frame ID, described function number is 0x08, described data number is the count number of current measurement data, and described element number is the numbering of current measuring unit node, and frame ID is subpackage sequence number.The priority of carrying out hardware by identification fields due to CAN is resolved, and the less then priority of numerical value of identification fields is higher.After numbering is arranged above, first carry out priority judgement with data number, guarantee that the data of each measuring unit node can not delay transmission; Carry out priority judgement by measuring unit node serial number again, guarantee that each group data is all send continuously, and by the order that frame ID maintains subpackage, there will not be entanglement.
During data acquisition, main frame obtains the correction data of each measuring unit node by the rotary state of CAN control motor, correction data comprise accelerometer x, y, z three axle under static state the output data of six direction, gyroscope x, y, z three axle under static state six direction output data and rotate along gyroscope x, y, z three axle with fixed angles speed time real time sensor data, the output data under inactive state and real time sensor data are many groups;
Step 42, main frame are calculated by correcting algorithm and correction data and obtain correction matrix.
Described step 42 comprises the following steps:
Step 421, set up error model:
When not considering that temperature affects, correction matrix is respectively:
Wherein, in formula (11)-(13), bias, scale and M represent the nonopiate coupling error matrix between null offset matrix, scale factor matrix and three axles respectively;
Error model is expressed as:
D_M=scale*M*D_T+bias (14)
In formula (14), D_M is the data that the actual measurement of measuring unit node obtains, and D_T is the theoretic output data of measuring unit node;
Error model can be rewritten into:
D_T=N*(D_M-bias) (15)
In formula (15):
N=inv(scale*M) (16)
Step 422, obtain accelerometer and gyrostatic null offset matrix respectively:
For the null offset matrix bias of accelerometer
1: by inactive state brief acceleration meter every axially under put time, the reading of its upper and lower both direction and 1/2nd, then average and obtain;
For gyrostatic null offset matrix bias
2: by gyroscope during inactive state every axially under put time, the reading of its upper and lower both direction and 1/2nd, then quadrature and obtain;
Step 423, embody the hybrid matrix of the nonopiate coupling error matrix between scale factor matrix and three axles due to the N in formula (16), therefore, only need obtain accelerometer and gyrostatic hybrid matrix:
For the hybrid matrix N of accelerometer
1:
The calculation matrix Ba of 3 × 3 is obtained after reading by accelerometer three axles:
In formula (17), P
xyfor when accelerometer x-axis is put downwards, repeatedly reading corresponding to y-axis all deducts ybias
1after the average of trying to achieve, the acquisition of other elements and P
xysimilar (such as, P
xxfor when accelerometer x-axis is put downwards, repeatedly reading corresponding to x-axis all deducts xbias
1after the average of trying to achieve); Xbias
1, ybias
1be respectively null offset matrix bias
1zero drift value in corresponding x, y-axis;
And accelerometer corresponding 33 theoretical calculation matrix Aa be:
In formula (18), G is acceleration of gravity;
By theoretical calculation matrix Aa and calculation matrix Ba, obtain hybrid matrix N
1=Aa/Ba;
For gyrostatic hybrid matrix N
2:
The calculation matrix Bg of acquisition 33 is obtained after reading when being rotated by gyroscope three axles:
In formula (19), Q
xyduring for rotating along gyroscope x-axis, repeatedly reading corresponding to y-axis all deducts ybias
2after the integrated value of trying to achieve, the acquisition of all the other elements and Q
xysimilar (such as Q
xxduring for rotating along gyroscope x-axis, repeatedly reading corresponding to x-axis all deducts xbias
2after the integrated value of trying to achieve); Xbias
2, ybias
2for null offset matrix bias
2zero drift value in corresponding x, y-axis.
And gyroscope corresponding 33 theoretical calculation matrix Ag be:
N, for rotating the number of turns, by theoretical calculation matrix Ag and calculation matrix Bg, obtains hybrid matrix N
2=Ag/Bg.
Step 5, corrected after, the correction data frame with correction matrix is sent to corresponding measuring unit node by CAN by main frame, and measuring unit node records after receiving correction data frame and replaces legacy data calculating parameter.
After step 6, measuring unit nodes records correction matrix, again send correction data to main frame, compare with set point after main frame receives described correction data, with the difference by comparison whether in allowed band, thus judge whether successfully trimming process.
Above-listed detailed description is illustrating for possible embodiments of the present invention, and this embodiment is also not used to limit the scope of the claims of the present invention, and the equivalence that all the present invention of disengaging do is implemented or changed, and all should be contained in the scope of the claims of this case.