利用加速度传感器测量物体的倾斜角度

CAST Reference Design Specification 利用加速度传感器测量利用加速度传感器测量利用加速度传感器测量利用加速度传感器测量 物体物体物体物体的倾斜角度的倾斜角度的倾斜角度的倾斜角度 Author Jia Ning Version V1.0 -------------------------------------------------------------------------------------------------------------- Analog Devices Inc. has the full intelligent property (IP) of this document and the contents described in this document. Analog Devices Inc. has the right to change any of the descriptions in the document without notifying the readers. If readers need any technical help, please contact China Applications Support Team via china.support@analog.com or the toll-free No. 8008101742. -------------------------------------------------------------------------------------------------------------- Revision History Date Revision History Reviser 2008.9.18 V1.0 Ning Jia Abbreviation Table of Contents 1 说明........................................................................................................................................... 2 2 基本原理 ................................................................................................................................... 2 3 硬件实现 ................................................................................................................................... 3 4 软件算法 ................................................................................................................................... 3 4.1 反三角函数算法 ....................................................................................................... 4 4.2 查表算法 ................................................................................................................... 4 5 倾斜角度的精度与加速度值的分辨率之间的关系 ............................................................... 5 6 改进的查表算法 ....................................................................................................................... 8 附录一 整数开平方算法 ........................................................................................................... 9 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 2 1 说明说明说明说明 测量物体的倾斜角度是加速器传感器的一种常见的应用。虽然其基本原理十分简单，但是在 具体实现中仍然会遇到很多困难，比如倾斜角度的精度问题，数学计算过于复杂等等。本文 将对精度问题进行详细讨论，并给出一种简化的计算方法。 2 基本原理基本原理基本原理基本原理 由于加速度传感器在静止放置时受到重力作用，因此会有 1g 的重力加速度。利用这个性质， 通过测量重力加速度在加速度传感器的 X 轴和 Y 轴上的分量，可以计算出其在垂直平面上 的倾斜角度。 αcosgAy =αsingAx = g α gAy = 0=Ax 如上图所示，有 αsingAx = ， αcosgAy = 。则 αtan= Ay Ax ，即 )arctan( Ay Ax =α 。 这样，根据以上原理一个 2 轴加速度传感器可以测量在 X-Y 平面上的倾斜角度。 需要注意的是需要注意的是需要注意的是需要注意的是，，，，2 轴加速度传感器只能测量轴加速度传感器只能测量轴加速度传感器只能测量轴加速度传感器只能测量 X 轴和轴和轴和轴和 Y 轴上的重力分量轴上的重力分量轴上的重力分量轴上的重力分量，，，，因而只能测量因而只能测量因而只能测量因而只能测量 X-Y 平面上的倾斜角度平面上的倾斜角度平面上的倾斜角度平面上的倾斜角度。。。。可是由于物体在空间倾斜的时候，很难保证倾斜完全在 X-Y 平面上， 这样只使用 2 轴加速度传感器进行测量会存在局限性，因此，我们考虑使用 3 轴加速度传感 器。如下图所示，3 轴加速度传感器可以测量 X 轴、Y 轴和 Z 轴的重力分量，计算空间倾 斜角度的公式可以推广为 )arctan( 22 Ay AzAx + =α 。 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 3 Ax Ay Az 这个公式就是本文中用来测量物体倾斜角度的基本原理。需要说明的是，这里利用的是物体 在静止时受到重力的性质，如果物体同时也有运动加速度的话，那么这个公式将不再准确。 所以必须为公式增加一个限制条件，即      =++ + = gAyAzAx Ay AzAx 1 )arctan( 222 22 α 3 硬件实现硬件实现硬件实现硬件实现 目前，在消费类产品中使用的加速度传感器分为数字输出（例如 ADXL345）和模拟输出（例 如 ADXL335）两种。数字输出的加速度传感器可以直接通过 I2C 或 SPI 总线与 MCU 进行 连接；模拟输出的加速度传感器则需要使用 ADC 进行采样。现在，普遍使用的 MCU 中基 本都有内置的 ADC 通道，所以无论是数字输出还是模拟输出的加速度传感器都可以非常容 易地和 MCU 进行连接，进而实现测量功能。 4 软件软件软件软件算法算法算法算法 本文在第一部分讨论了测量的基本原理，即      =++ + = gAyAzAx Ay AzAx 1 )arctan( 222 22 α China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 4 4.1 反三角函数算法反三角函数算法反三角函数算法反三角函数算法 直接利用以上公式进行计算是最直观的方法。具体步骤为： 1） 测量 X、Y、Z 轴的加速度。 Ax ， Ay ， Az 。（对于数字输出的加速度传感器，直接通 过 I2C 或 SPI 总线读取；对于模拟输出的加速度传感器，需要通过 ADC 进行采样。在 算法说明部分将不对此加以区分。） 2） 计算 222 AyAzAx ++ ，如果这个平方和接近 g1 的平方，那么说明这组采样值是有效的， 可以用来计算；否则将该采样值丢弃，重复第一步。 3） 利用有效的采样值，通过开平方和反正切函数等数学计算，求出倾斜角度 )arctan( 22 Ay AzAx + =α 。 4） 重复第一步。 这个算法的缺点在于需要用到复杂的数学计算。虽然开平方，反正切等计算在 C 语言中都 有可以直接使用的库函数，但是它们都是浮点计算，因此需要占用很大的代码空间和计算时 间，对 MCU 的要求相对较高。由于在消费类产品中，制造成本是至关重要的，因此使用这 种直接的方法并不是很好的选择。 4.2 查表算法查表算法查表算法查表算法 可以发现，第一种算法的主要问题在于开平方和反正切函数的计算。如果可以避开这些计算， 则可以大幅度的降低算法的复杂度。 下面的算法中，我们分两步来降低算法的复杂度。 1． 使用整数开方算法代替 C 语言中的开平方函数，本文将在附录一中详细介绍整数开平方 算法。 2． 利用查表方法来代替反正切计算。即，提前计算出每个角度α 的正切值 αtan ，并保存 在 EEPROM 中，形成一个表。在计算工程中，只要计算 Ay AzAx 22 + ，并在表中查找 与其最接近的一个正切值 0tanα ，则 0α 就是所要求的倾斜角度。 需要注意，我们要测量的倾斜角度α 的范围为 ]180,0[ oo∈α ，如果考虑对倾角计算的精 度要求为 o1.0 ，那么表中需要保存 1800 个正切值。利用三角函数公式，有 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 5         −=− −=+ =− αα α α α α α tan)180tan( tan 1)90tan( tan 1)90tan( tan o o o 这样，只需要保存 ]45,0[ oo∈α 的正切值表。但是即使这样， 由于每个正切值都是一个浮点数，通常为 4Byte，所以仍然需要很大的 EEPROM，对 MCU 的要求同样很高。因此这种查表的方法仍然不是最佳算法。 5 倾斜角度的精度与加速度值的分辨率之间的关系倾斜角度的精度与加速度值的分辨率之间的关系倾斜角度的精度与加速度值的分辨率之间的关系倾斜角度的精度与加速度值的分辨率之间的关系 在给出最佳的改进算法之前，我们先对倾斜角度的精度与加速度值的分辨率之间的关系进行 分析。这不仅将告诉我们，要得到一定精度的倾斜角度时需要使用多高分辨率的加速度值， 这将决定 ADC 位数的选择。而且，分析过程将有助于我们找到一种更加简化的查表算法。 分析同样从公式      =++ + = gAyAzAx Ay AzAx 1 )arctan( 222 22 α 开始。如果假设 gAyAzAx 1222 =++ 的条件 已经满足，那么计算公式理论上可以简化为 ))1(arctan( 22 Ay Ayg − =α ，此时公式中只有 Ay 一个变量，分析起来会比较简单。 我们以 ADXL335 加一个 8bit 分辨率 ADC 的系统为例。ADXL335 的测量范围是 g3± ，假设其为 3V 供电， 其 g1 的灵敏度为 0.3V（1/10 供电电压）。如果认为 ADC 的输入范围是 0V 至 3V，那么采样后得到的 g1 的灵 敏度只有 LSB252 10 1 8 =× 。也就是说，我们采样后得到的 Ay 只有 252,1,0 K 。 Ay F = sqrt(1g*1g – Ay*Ay) / Ay 1/F A = arctanF Δ(F) * 10000 Δ(A) Δ(1/F) * 10000 0 IN 0 90 IN 2.29244128 0 1 24.97999199 0.040032038 87.70755872 125200.562 2.296122999 400.3203845 2 12.45993579 0.080257235 85.41143572 41868.2003 2.303536883 402.2519694 3 8.273115764 0.120873445 83.10789884 21036.34424 2.314793681 406.1620921 4 6.16948134 0.16208818 80.79310516 12705.01854 2.330062851 412.1473509 5 4.898979486 0.204124145 78.46304231 8540.924524 2.34958137 420.3596554 6 4.044887033 0.247225693 76.11346094 6163.156046 2.373664386 431.015478 7 3.428571429 0.291666667 73.73979655 4678.91608 2.402720218 444.4097364 8 2.960679821 0.337760265 71.33707633 3691.456451 2.437271181 460.9359807 9 2.591534175 0.385871817 68.89980515 3002.46328 2.477982496 481.1155183 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 6 10 2.291287847 0.43643578 66.42182265 2503.838183 2.525702702 505.639639 11 2.040904029 0.489978944 63.89611995 2132.613459 2.581520921 535.4316303 12 1.827642683 0.547152903 61.31459903 1850.152599 2.646849529 571.739596 13 1.642627423 0.608780778 58.6677495 1631.763393 2.723546291 616.2787442 14 1.479451084 0.675926369 55.94420321 1461.177508 2.814099951 671.4559189 15 1.333333333 0.75 53.13010326 1327.475388 2.921921904 740.7363058 16 1.200585795 0.83292673 50.20818136 1223.326898 3.051823596 829.2673007 17 1.078253105 0.927426034 47.15635776 1144.002395 3.210837449 944.9930343 18 0.963852865 1.037502752 43.94552031 1086.909278 3.409717509 1100.767184 19 0.855161937 1.169369164 40.5358028 1051.619373 3.665904528 1318.664124 20 0.75 1.333333333 36.86989827 1040.638111 4.010017335 1639.64169 21 0.645936189 1.54814054 32.85988094 1061.933667 4.502243879 2148.072063 22 0.539742822 1.852734226 28.35763706 1137.44606 5.283718601 3045.936867 23 0.425998216 2.34742767 23.07391846 1343.315495 6.813713474 4946.934438 24 0.291666667 3.428571429 16.26020499 2916.666667 16.26020499 10811.43758 25 0 IN 0 0 0 IN 上表中，第一列和第二列给出了 Ay 为 252,1,0 K 时，对应的 Ay Ayg F 22)1( − = 以及 22)1( 1 Ayg Ay F − = 。其对应的角度 Farctan=α 在第三列给出。由 通过第五列中给出的 α∆ ，可以看出倾斜角度的精度在 0=Ay 出最高，为 o3.2 左右，随着 Ay 的增大，精度会越来越差。红颜色的一行对应的倾角为 o45 ，此时精度为 o4.3 。可以看 出 ]90,45[ oo 范围内的分辨率要高于 ]45,0[ oo 。前面的查表算法中已经说明过，只需要保存 任意一个 o45 范围内（ ]45,0[ oo ， ]90,45[ oo ， ]135,90[ oo ， ]180,135[ oo ）的正切值就可以 得到全部 ]180,0[ oo 的角度。这样看来，显然使用 ]90,45[ oo 的查表可以得到更高的精度。 另外，可以看出， Ay Ayg F 22)1( − = 为单调递减，而 22)1( 1 Ayg Ay F − = 为单调递增。表 中的第四列和第六列给出了 F 和 F 1 的递减和递增的步进（这里已经通过乘以 10 的 n 次方 而把小数变成合适的整数）。这样我们在实际保存查找表的时候，不需要直接保存正切数（浮 点数，通常要 4Byte），而只需要保存这个处理过的整数步进（只需要 1Byte）。 当 ADC 的分辨率更高的时候，比如 10bit 或 12bit 等等，将会有很多点的步进值是相等的。 下表是 10bit 的 ADC 的情况，可以发现 F 1 的步进在前 8 个点都是 98（取整数部分）后面四 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 7 个点位 99，之后 2 个点位 100……。因此，我们可以不用每个点的步进值是多少，而改 为记录每个步进值有多少个点，这样可以进一步减少查找表中保存的数值，也就是减小了对 EEPROM 空间的要求。 Ay F = sqrt(1g*1g – Ay*Ay) / Ay 1/F A = arctanF Δ(F) * 10000 Δ(A) Δ(1/F) * 10000 0 IN 0 90 IN 0.561730802 0 1 101.9950979 0.009804393 89.4382692 510049.0279 0.56178634 98.04392764 2 50.99019514 0.019611614 88.87648286 170049.042 0.561894392 98.0722075 3 33.98529094 0.029424494 88.31458847 85049.06323 0.562056588 98.12880803 4 25.48038461 0.039245875 87.75253188 51049.09158 0.562273067 98.21381097 5 20.37547545 0.049078609 87.19025881 34049.12706 0.562544018 98.32733928 6 16.97056275 0.058925565 86.62771479 24334.884 0.562869677 98.46955757 7 14.53707435 0.068789632 86.06484512 18263.5053 0.563250328 98.6406726 8 12.71072382 0.078673726 85.50159479 14215.94338 0.563686305 98.84093406 9 11.28912948 0.088580789 84.93790848 11382.67449 0.564177991 99.07063539 10 10.15086203 0.098513801 84.37373049 9322.140255 0.564725822 99.33011476 11 9.218648007 0.108475776 83.80900467 7776.764988 0.565330282 99.61975631 12 8.440971508 0.118469776 83.24367439 6588.040608 0.565991912 99.93999141 13 7.782167447 0.128498906 82.67768248 5654.0691 0.566711306 100.2913003 14 7.216760537 0.138566327 82.11097117 4906.91849 0.567489115 100.6742135 15 6.726068688 0.148675258 81.54348206 4299.885586 0.568326045 101.0893143 16 6.29608013 0.158828982 80.97515601 3800.003466 0.569222866 101.5372402 按照以上的分析方法，我们可以得出结论如下。 8bit 的 ADC 可以得到 o4.3 以上的精度。 10bit 的 ADC 可以得到 o8.0 以上的精度。 12bit 的 ADC 可以得到 o2.0 以上的精度。 14bit 的 ADC 可以得到 o05.0 以上的精度。 16bit 的 ADC 可以得到 o0124.0 以上的精度。 需要注意的是，这里的结论是在使用 ADXL335 的情况下得出的。此时，ADC 的测量范围 为 ADXL335 的整个输出范围（ g3± ）。而实际上，我们只关心其中 g10 − 的部分，还以 8bit 的 ADC 为例，其分辨率为 25628 = ，但是在我们的应用中，只使用了 LSB252 10 1 8 =× ， 因此 ADC 的测量范围有很大一部分时浪费的，也就是说 ADC 的分辨率在这种应用中损失 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 8 了 3bit 以上（ 3.310log2 = ）。准确地说，我们关心的是对加速度传感器 g10 − 输出的分辨 率，所以如果可以把加速度传感器的 g10 − 输出经过滤波、放大到 ADC 输入的满幅范围， 这样则可以充分利用 ADC。因此，我们将前面得到的结论修改为。 8bit 的 ADC，有效分辨率为 252 10 1 8 =× ，可以得到 o4.3 以上的精度。 10bit 的 ADC，有效分辨率为 1022 10 1 10 =× ，可以得到 o8.0 以上的精度。 12bit 的 ADC，有效分辨率为 4092 10 1 12 =× ，可以得到 o2.0 以上的精度。 14bit 的 ADC，有效分辨率为 16382 10 1 14 =× ，可以得到 o05.0 以上的精度。 16bit 的 ADC，有效分辨率为 65532 10 1 16 =× ，可以得到 o0124.0 以上的精度。 还有一点需要主要的是，我们在分析过程中，使用的公式是 ))1(arctan( 22 Ay Ayg − =α 。 虽然在理论上，它与 )arctan( 22 Ay AzAx + =α 是一样的。但是实际中，由于 Ax ，Ay ，Az 的不同取值，我们使用 )arctan( 22 Ay AzAx + =α 必然会得到更多的数值，也就是对角度的 分辨率会比使用 ))1(arctan( 22 Ay Ayg − =α 计算略高一些。 6 改进的查表算法改进的查表算法改进的查表算法改进的查表算法 从以上的分析中，我们可以出改进的查表算法为。 先计算 ]90,45[ oo 范围内，各个角度的正切值，通过乘以 10 的 n 次方（比如 10000）取出合 适大小的整数部分。保存这些值的步进为一个查找表。由于取整数后，会有很多点的步进相 等，为了进一步减小查找表，可以保存每个步进值对应得点数。 1） 测量 X、Y、Z 轴的加速度。 Ax ， Ay ， Az 。 2） 进行数字滤波以得到稳定的数字输出。兼顾输出稳定性和响应速度，通常使用 4 阶的平 均进行数字滤波。 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 9 3） 利用整数开方算法，计算 22 10000 AzAx Ay + 4） 将查找表中保存的步进累加，直到累加值大于计算出的 22 10000 AzAx Ay + ，此时步进值对应 的角度就是要求的倾斜角度α 。 5） 重复第一步。 这种算法即克服了直接计算三角函数时计算量大的缺点。同时也不像一般查表算法那样需要 很大的 EEPROM。 附录一附录一附录一附录一 快速快速快速快速整数开平方算法整数开平方算法整数开平方算法整数开平方算法 一个整数可以表示为 2 进制数的形式，即 ∑ = •= N i i ida 0 2 ，其中 id 为 0 或者 1， N 决定了整 数 a 的范围。比如，通常的 MCU 中，无符号长整型数为 4Byte 即 32 位的，那么 a 的范围 为 ]12,0[ 32 − 。 当已知一个整数 a的平方 A，要反求a 的时候，只需要确定每一个 id ， Ni L,1,0= 是 0 还 是 1。 先考虑最高的一位 Nd ，如果 1=Nd ，那么有 NNNda 22 =•≥ ，则需要 12 2)2( +=≥ NNA 。 也就是说，如果 12 2)2( +=≥ NNA ，则 1=Nd ；否则 0=Nd 。 通过这种方法，从 0,1,1, L−= NNi ，直到找到一个 M ，使得 12 2)2( +=≥ MMA ，这时 1=Md 是整数 ∑ = •= N i i ida 0 2 中第一个不为 0 的 id 。 这时 MMMd 22 =• 是我们已经开方出来的部分，记为 χ ，此时整数 a 可以写为 ∑ − = •+= 1 0 2 M i i ida χ 。下面的任务要判断 1−Md 是 0 还是 1 。如果 11 =−Md ，则有 11 1 22 −− − +=•+≥ MMMda χχ ，则需要 21)2( −+≥ MA χ ，即 1212 22)2( −− +≥− MMA χχ ， 这里我们把 2χ−A 看作时已经开方后残余的部分，记为δ 。 China Application Support Team Toll-free number：800-810-1742 Toll-free fax：800-810-1747 Email：china.support@analog.com 10 所以，当得到了第一个不为 0 的 1=Md 后，之后每一个 1=id 的条件是      •= +=+≥− ∑ += + N ip p p iiii d A 1 12 2 2222)2( χ χχχ 由此递推可以求出所有的 id ，也就是求出了正数a 的数值。 可见整个算法里只需要计算加减法和 2 的乘方，2 的乘方在程序中可以通过移位计算。所以 整个算法的计算量很小。 基于以上原理，简化后的开平方算法代码为：（此代码来自互联网，作者 Paul Bourke） long int Sqrt(long int r) { long t,b,c=0; for (b=0x40000000;b!=0;b>>=2) { t = c + b; c >>= 1; if (t <= r) { r -= t; c += b; } } return(c); }