光流模块在无人机中的应用—光流模块倾角补偿

作者：凌启科技

• 光流模块为什么需要倾角补偿

当无人机有倾角时，光流模块也会测到一个位移输出，这是我们不需要的，可以用姿态角进行补偿，具体的倾角补偿原理可参考csdn文章《光流模块在无人机中的应用》https://blog.csdn.net/LLZ15827447646/article/details/137211143?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522172179271316800211521147%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=172179271316800211521147&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~sobaiduend~default-1-137211143-null-null.142^v100^pc_search_result_base7&utm_term=%E5%85%89%E6%B5%81%E6%A8%A1%E5%9D%97%E5%9C%A8%E6%97%A0%E4%BA%BA%E6%9C%BA%E4%B8%AD%E7%9A%84%E5%BA%94%E7%94%A8&spm=1018.2226.3001.4187

• 光流模块倾角补偿matlab仿真

具体的仿真代码如下：

% 作者：凌启科技
% 加速度计和双目测距3阶互补滤波
clear;


y = [98.90 -53.62 -10.10 -8.42 98.20 -52.96 -10.30 -8.60 97.40 -52.28 -10.50 -8.73 96.70 -52.05 -10.50 -8.91 96.50 -52.08 -11.20 -9.23 96.50 -51.98 -11.20 -9.44 96.30 -51.51 -11.90 -9.56 95.60 -51.23 -12.10 -9.51 95.60 -51.28 -12.10 -9.35 95.60 -51.58 -12.10 -9.30 95.60 -51.56 -12.10 -9.20 95.60 -51.26 -12.10 -9.04 95.60 -50.95 -12.10 -8.83 94.90 -50.66 -11.70 -8.71 94.90 -50.29 -11.70 -8.63 94.90 -49.99 -11.70 -8.49 93.90 -49.39 -11.50 -7.95 92.90 -48.93 -9.90 -7.29 92.80 -48.28 -9.10 -6.42 92.30 -47.92 -8.40 -6.01 92.30 -47.47 -8.40 -5.58 91.90 -46.96 -7.60 -5.43 90.40 -46.24 -7.40 -5.61 90.40 -45.78 -7.40 -6.00 89.30 -45.38 -8.40 -6.56 89.20 -45.54 -9.10 -6.65 89.20 -45.80 -9.10 -6.47 89.20 -45.76 -9.10 -6.48 89.20 -45.75 -9.10 -6.88 89.40 -45.68 -9.70 -7.39 89.40 -45.22 -10.60 -8.10 87.80 -44.72 -11.70 -8.39 87.80 -44.62 -11.70 -8.51 87.80 -45.15 -11.70 -8.54 88.50 -45.62 -12.10 -8.62 88.50 -45.43 -12.10 -8.69 88.50 -45.18 -12.10 -8.62 87.70 -45.44 -12.50 -8.35 88.60 -45.98 -11.70 -7.74 88.90 -45.95 -11.10 -7.32 88.90 -45.65 -11.10 -7.08 88.00 -45.53 -10.60 -7.12 88.00 -45.55 -10.60 -7.21 88.00 -45.67 -10.60 -7.22 88.40 -45.66 -11.20 -7.18 88.40 -45.39 -11.20 -7.22 88.40 -45.25 -11.20 -7.10 88.40 -45.41 -11.20 -6.73 88.30 -45.25 -10.20 -6.37 88.30 -45.03 -10.20 -6.59 87.60 -45.19 -10.20 -7.45 88.30 -45.53 -12.30 -8.06 88.30 -45.85 -12.30 -8.23 88.30 -45.89 -12.30 -7.93 88.30 -45.87 -12.30 -7.63 88.40 -45.97 -12.30 -7.99 88.60 -46.14 -13.40 -8.95 88.70 -46.08 -14.40 -9.21 88.70 -45.75 -14.40 -8.64 87.90 -45.60 -13.00 -8.02 87.80 -45.69 -12.10 -7.96 88.20 -45.34 -13.00 -8.49 86.80 -44.16 -13.90 -9.06 85.80 -43.93 -14.30 -8.86 85.90 -44.46 -13.50 -7.72 87.40 -44.68 -11.10 -7.12 87.40 -44.31 -11.10 -7.49 86.30 -44.05 -12.50 -8.29 86.10 -44.29 -13.60 -9.00 86.10 -44.64 -13.60 -9.04 87.00 -44.74 -14.00 -8.87 87.00 -44.67 -14.00 -8.79 87.00 -44.51 -14.00 -8.80 87.00 -44.56 -14.00 -8.93 87.00 -44.85 -14.00 -9.53 87.30 -44.97 -15.70 -10.30 87.10 -44.91 -16.70 -10.91 87.10 -45.02 -16.70 -10.58 87.10 -45.92 -15.80 -9.39 88.10 -46.84 -14.50 -8.93 90.40 -47.41 -14.80 -9.48 90.20 -46.94 -16.10 -10.04 89.20 -46.56 -16.60 -9.95 89.00 -47.13 -15.90 -8.85 90.60 -48.18 -14.10 -6.81 92.20 -48.28 -10.10 -5.80 92.20 -48.12 -10.10 -5.87 91.90 -48.01 -11.00 -6.59 91.90 -47.87 -12.00 -7.15 91.90 -48.20 -12.00 -7.05 92.70 -48.57 -11.70 -6.55 92.70 -48.47 -11.70 -6.29 92.70 -48.40 -11.70 -6.26 92.70 -48.68 -11.70 -6.34 92.70 -49.23 -11.70 -6.46 93.30 -49.51 -11.70 -6.74 94.00 -49.39 -12.80 -7.32 93.70 -48.86 -13.50 -7.57 92.70 -48.28 -13.70 -7.12 92.30 -48.20 -12.00 -6.39 92.30 -47.59 -11.10 -5.96 91.40 -46.63 -11.20 -6.42 90.00 -45.22 -12.30 -7.62 88.90 -44.01 -13.50 -8.41 85.90 -41.47 -14.60 -9.29 82.80 -38.48 -15.20 -9.91 78.50 -32.95 -15.80 -11.07 73.10 -27.77 -16.90 -12.58 66.80 -19.02 -18.70 -15.20 49.10 -11.24 -23.00 -16.70 39.10 -0.24 -23.70 -17.78 29.40 7.57 -23.70 -17.69 20.80 17.18 -22.40 -16.45 4.80 24.04 -18.80 -15.36 -2.20 31.95 -17.40 -14.14 -8.00 36.42 -16.30 -13.61 -12.10 40.77 -16.10 -13.80 -14.90 43.57 -16.50 -14.84 -17.80 48.08 -17.80 -17.04 -26.80 52.19 -21.70 -18.52 -32.30 57.97 -22.70 -20.23 -37.50 61.84 -23.80 -21.48 -41.80 66.58 -25.10 -23.40 -50.40 70.82 -29.10 -25.39 -55.90 76.29 -31.70 -28.03 -60.10 79.14 -33.60 -29.77 -62.10 80.17 -35.20 -31.44 -62.10 79.35 -36.50 -31.96 -60.60 77.65 -37.10 -31.99 -57.10 76.21 -37.40 -32.28 -55.00 73.01 -38.10 -33.35 -51.40 69.15 -39.60 -34.18 -46.10 62.28 -40.40 -34.04 -33.10 56.45 -39.30 -32.77 -25.70 48.41 -37.40 -30.48 -18.50 41.63 -35.50 -28.61 -9.70 30.18 -33.10 -26.11 0.80 19.71 -31.00 -24.37 27.10 4.33 -27.60 -22.39 40.60 -6.96 -26.70 -21.76 52.70 -20.25 -27.10 -22.04 63.20 -28.95 -28.10 -22.42 72.60 -39.75 -28.90 -22.50 91.30 -47.30 -29.20 -21.96 100.00 -55.78 -28.30 -20.73 106.40 -60.40 -27.50 -19.98 110.90 -65.51 -27.00 -19.42 119.20 -68.76 -26.70 -19.17 122.00 -71.78 -26.70 -18.91 124.00 -73.32 -26.40 -18.75 125.30 -74.38 -26.40 -18.85 126.10 -74.18 -26.50 -19.26 126.10 -72.70 -26.50 -20.00 122.30 -71.07 -28.10 -20.57 119.90 -66.85 -28.60 -21.77 115.00 -61.10 -29.60 -23.18 107.10 -50.79 -31.00 -25.70 86.10 -41.46 -35.10 -27.98 73.50 -27.45 -37.80 -31.50 60.10 -17.06 -40.60 -33.66 48.30 -5.78 -42.40 -35.60 38.80 0.38 -43.40 -36.58 26.80 8.02 -45.30 -37.93 19.00 14.64 -46.80 -38.70 10.70 24.40 -47.10 -38.66 1.40 31.96 -46.40 -38.21 -7.50 41.49 -45.50 -37.50 -23.40 47.79 -43.90 -36.61 -30.40 55.52 -42.70 -35.08 -36.90 60.72 -40.70 -33.88 -43.20 66.80 -39.10 -33.46 -51.50 69.72 -39.30 -33.76 -52.80 71.30 -39.70 -34.15 -52.80 71.29 -39.70 -33.90 -51.90 70.73 -39.00 -33.38 -51.30 69.59 -38.60 -33.18 -49.70 65.83 -38.50 -32.82 -40.20 60.23 -37.80 -32.40 -31.10 48.40 -37.50 -31.60 -17.80 36.97 -36.70 -30.51 -3.00 20.45 -35.30 -28.23 26.80 8.04 -30.90 -26.10 41.20 -9.06 -28.20 -22.81 56.50 -23.04 -25.60 -20.50 73.30 -41.51 -23.60 -18.13 89.90 -53.02 -22.30 -17.15 103.10 -63.61 -21.60 -17.10 117.20 -68.16 -22.90 -17.61 120.40 -71.08 -23.90 -18.79 121.70 -71.75 -25.00 -19.50 122.00 -72.13 -25.60 -20.70 123.00 -72.66 -28.20 -21.80 123.80 -72.68 -29.30 -23.12 122.90 -70.71 -30.00 -23.96 119.80 -65.25 -30.50 -25.57 114.40 -58.52 -31.50 -27.60 93.60 -45.37 -36.90 -32.18 79.30 -32.64 -42.20 -36.97 61.90 -11.17 -47.80 -43.37 39.90 7.17 -52.60 -47.24 16.40 30.67 -56.30 -51.88 -22.20 45.56 -64.20 -55.88 -36.20 60.10 -69.30 -60.73 -46.10 67.44 -73.60 -63.02 -53.00 73.03 -76.10 -63.69 -57.80 74.46 -75.60 -62.86 -56.90 74.54 -73.90 -61.00 -56.90 74.90 -72.90 -59.91 -57.70 75.08 -72.30 -59.47 -57.70 74.18 -72.30 -60.16 -56.30 72.20 -73.30 -61.60 -52.30 70.82 -75.60 -62.05 -51.20 69.44 -75.30 -61.72 -49.80 68.18 -74.40 -61.42 -48.10 66.52 -74.00 -61.40 -45.20 65.74 -73.70 -61.06 -45.00 65.72 -72.80 -60.29 -45.00 65.77 -71.50 -59.72 -44.90 65.75 -70.60 -59.67 -44.90 66.07 -70.60 -60.35 -45.40 66.48 -72.50 -61.39 -45.40 66.39 -72.50 -61.93 -44.70 66.27 -73.20 -62.50 -44.60 66.45 -73.80 -62.93 -44.60 66.49 -73.80 -63.03 -44.60 66.36 -74.50 -62.67 -44.30 66.49 -73.50 -62.07 -44.40 66.75 -72.90 -61.71 -44.40 67.00 -72.90 -61.48 -44.60 67.16 -72.20 -61.59 -45.10 67.41 -73.00 -62.48 -44.90 67.38 -73.80 -63.14 -45.00 67.92 -74.60 -63.52 -45.50 68.21 -74.60 -63.05 -45.80 68.28 -73.80 -62.18 -45.60 68.49 -72.70 -62.16 -45.60 68.63 -72.70 -62.69 -45.80 68.14 -73.40 -63.03 -44.90 64.14 -73.30 -62.19 -33.60 57.63 -70.60 -60.53 -22.60 44.90 -67.80 -57.29 -9.40 33.64 -64.80 -54.62 4.20 18.28 -62.10 -50.65 17.90 7.04 -58.60 -46.77 31.10 -6.59 -53.90 -40.84 54.80 -14.85 -43.10 -36.53 62.10 -22.22 -39.20 -32.47 67.20 -25.14 -36.40 -30.63 69.40 -27.33 -34.80 -29.87 72.20 -27.86 -35.00 -30.48 71.00 -25.17 -36.80 -32.65 66.20 -20.08 -39.10 -34.83 58.40 -9.80 -41.50 -38.38 48.20 0.49 -44.20 -41.76 20.80 17.00 -51.90 -47.11 5.10 30.90 -56.60 -51.46 -12.00 50.66 -61.40 -56.80 -30.80 66.06 -66.20 -59.46 -50.00 86.20 -69.10 -60.95 -84.80 98.53 -69.80 -61.16 -96.30 110.03 -70.10 -61.99 -103.40 114.78 -71.10 -62.52 -106.70 117.23 -71.70 -62.37 -107.70 117.83 -71.40 -62.58 -106.60 116.41 -72.20 -63.07 -104.00 113.02 -73.10 -63.59 -98.10 103.23 -73.80 -63.26 -87.80 91.69 -73.80 -61.46 -72.90 70.54 -71.70 -56.85 -30.00 51.84 -62.80 -52.29 -5.80 25.79 -56.80 -45.43 17.30 7.73 -51.30 -41.25 37.90 -12.40 -47.70 -37.66 69.20 -23.81 -44.70 -36.35 78.70 -33.33 -45.30 -36.44 83.50 -36.15 -46.30 -37.17 84.40 -36.84 -47.70 -39.27 83.90 -36.91 -50.10 -41.17 83.80 -37.99 -52.90 -43.44 86.40 -39.11 -56.80 -44.42 87.80 -39.92 -57.60 -44.92 87.80 -40.02 -57.60 -45.15 87.80 -40.40 -57.60 -46.04 88.90 -40.77 -60.50 -47.36 89.30 -40.79 -62.80 -49.45 88.60 -40.60 -64.90 -50.43 88.20 -40.41 -65.70 -50.14 88.20 -40.15 -65.70 -48.82 87.10 -39.49 -61.30 -46.90 86.00 -37.70 -59.90 -46.03 82.40 -31.34 -59.20 -45.62 75.40 -22.82 -59.40 -45.53 63.90 -8.04 -59.30 -46.96 36.00 4.67 -61.90 -49.80 20.20 22.89 -65.80 -55.04 3.10 37.05 -70.80 -59.22 -14.10 55.00 -75.20 -63.65 -44.10 65.97 -80.90 -65.99 -53.60 75.09 -82.30 -68.16 -57.70 77.32 -83.60 -68.78 -57.70 74.40 -83.60 -67.94 -53.50 67.13 -82.50 -65.73 -44.10 50.54 -80.00 -61.25 -10.30 34.27 -71.70 -57.62 10.70 10.95 -67.90 -52.50 31.50 -6.16 -63.60 -48.06 51.70 -28.18 -58.10 -41.59 89.10 -43.92 -47.80 -37.61 105.90 -62.16 -45.00 -34.19 120.20 -71.94 -43.70 -32.93 129.90 -79.43 -43.50 -32.67 134.50 -82.94 -43.80 -32.91 137.50 -87.05 -44.50 -33.29 144.50 -89.55 -45.60 -33.64 146.80 -91.69 -46.30 -34.32 148.10 -92.60 -47.00 -34.98 148.90 -93.36 -47.50 -36.15 149.50 -93.94 -49.60 -37.11 149.80 -94.47 -50.80 -38.36 150.10 -94.68 -52.10 -39.15 150.10 -94.12 -52.90 -40.04 149.50 -92.60 -53.40 -40.53 144.20 -88.98 -54.20 -41.37 140.00 -84.73 -55.50 -42.38 134.20 -74.58 -57.40 -45.27 123.40 -60.73 -60.30 -49.26 106.20 -33.45 -64.40 -55.89 49.20 -8.71 -74.40 -60.29 17.60 23.87 -77.80 -65.57 -9.10 44.12 -81.70 -69.82 -30.00 63.11 -86.00 -75.28 -55.20 72.87 -95.50 -77.83 -65.10 83.93 -97.40 -78.74 -73.70 89.70 -96.70 -77.13 -78.60 92.36 -93.20 -73.11 -79.30 91.71 -89.40 -69.68 -77.70 88.76 -85.30 -65.37 -70.90 85.23 -78.60 -62.95 -65.80 77.44 -76.50 -60.41 -57.80 68.16 -74.60 -58.72 -46.50 50.23 -72.80 -56.60 -9.70 32.96 -68.70 -54.09 13.30 8.10 -64.80 -49.96 35.10 -10.15 -61.50 -47.20 55.30 -32.04 -58.70 -43.76 74.50 -44.67 -56.00 -41.74 89.60 -54.34 -53.70 -40.19 102.90 -57.75 -51.60 -40.23 104.50 -60.85 -52.70 -40.64 107.00 -63.66 -53.30 -40.82 111.00 -66.92 -53.50 -41.43 116.60 -68.26 -55.10 -42.33 117.40 -69.21 -56.40 -43.37 117.90 -69.79 -57.20 -43.56 118.60 -70.70 -57.20 -43.29 119.70 -71.63 -56.60 -43.61 121.60 -72.49 -58.00 -45.12 122.20 -72.82 -60.40 -46.39 122.30 -72.40 -62.10 -47.13 121.70 -71.52 -62.50 -47.15 120.20 -69.07 -62.20 -47.07 115.00 -65.16 -61.80 -47.33 108.00 -54.27 -62.30 -48.76 95.00 -41.38 -63.20 -51.30 78.10 -19.14 -65.90 -56.14 34.50 -0.75 -73.70 -59.69 12.90 21.85 -76.80 -63.72 -4.50 34.84 -79.70 -66.19 -17.00 47.13 -82.00 -69.31 -25.80 55.08 -84.40 -71.12 -34.40 66.53 -86.40 -72.54 -55.40 74.15 -88.40 -73.12 -62.40 79.39 -88.20 -73.17 -64.70 80.60 -88.00 -73.07 -64.70 80.83 -88.00 -72.32 -64.10 80.20 -85.70 -71.72 -61.80 76.23 -84.90 -70.73 -57.20 70.35 -84.00 -70.04 -48.50 58.17 -82.90 -68.18 -36.90 46.65 -81.20 -65.70 -22.70 30.50 -78.20 -61.71 5.70 19.45 -71.30 -59.29 17.30 7.94 -69.40 -58.02 26.00 2.41 -69.40 -58.65 31.60 -1.38 -70.60 -60.53 35.80 -3.02 -74.70 -61.61 37.60 -5.08 -75.70 -62.47 39.50 -6.61 -76.00 -62.75 41.40 -8.10 -76.30 -63.61 42.70 -8.80 -77.20 -64.51 43.90 -9.93 -79.40 -65.48 45.30 -11.05 -79.90 -65.68 46.90 -12.17 -80.00 -65.56 47.90 -12.67 -79.90 -65.71 47.90 -13.25 -79.90 -66.45 49.30 -13.63 -82.20 -67.04 49.70 -14.00 -82.90 -67.17 49.70 -14.16 -82.90 -66.59 50.10 -14.49 -81.80 -65.33 50.60 -14.46 -79.60 -64.71 50.60 -13.65 -79.60 -64.65 48.70 -13.09 -79.50 -65.13 48.10 -12.80 -80.30 -66.01 48.00 -12.34 -81.10 -66.40 48.00 -11.09 -81.10 -66.44 45.20 -10.24 -81.20 -66.25 44.50 -9.67 -80.70 -65.77 44.50 -9.55 -80.70 -65.64 44.10 -9.22 -80.10 -66.24 43.30 -8.87 -81.50 -67.03 43.00 -8.95 -82.40 -67.52 43.00 -9.39 -82.40 -67.58 44.00 -9.97 -82.50 -67.43 44.00 -9.92 -82.50 -67.37 44.80 -9.04 -82.30 -67.27 43.00 -8.47 -82.20 -67.23 42.30 -8.55 -82.20 -67.36 42.30 -8.77 -82.20 -67.53 43.20 -8.62 -82.30 -67.64 43.20 -8.72 -82.30 -67.36 43.20 -9.57 -81.70 -66.69 44.40 -10.11 -81.00 -66.27 45.10 -10.12 -80.60 -66.25 45.10 -9.93 -80.60 -66.48 44.50 -9.71 -81.00 -66.67 44.50 -9.60 -81.00 -66.63 44.50 -9.82 -81.00 -66.49 44.50 -10.39 -81.00 -66.32 45.20 -11.09 -80.20 -66.86 46.40 -11.54 -81.50 -69.74 46.70 -13.08 -87.10 -78.80 48.50 -14.58 -97.10 -87.60 50.20 -15.07 -107.80 -99.93 48.30 -14.33 -129.30 -108.91 46.20 -13.79 -138.60 -119.86 45.20 -14.75 -147.90 -127.35 46.30 -17.39 -156.20 -134.85 48.80 -19.06 -162.20 -137.38 50.90 -19.96 -165.10 -137.66 51.10 -19.89 -163.30 -136.98 51.10 -19.68 -163.30 -137.08 50.70 -19.73 -165.10 -137.99 50.70 -19.72 -166.40 -138.86 50.70 -19.29 -166.40 -138.28 49.40 -17.47 -163.80 -132.83 45.80 -15.41 -145.20 -124.44 45.50 -13.71 -133.70 -111.91 46.40 -13.42 -122.90 -103.20 47.00 -12.42 -113.50 -92.11 45.00 -10.83 -104.40 -84.23 42.70 -8.97 -95.00 -73.76 40.30 -8.39 -75.80 -65.51 40.10 -7.89 -66.80 -55.27 39.40 -6.96 -58.10 -48.02 37.80 -5.27 -49.60 -39.30 34.80 -4.23 -34.60 -32.99 34.10 -3.48 -26.50 -24.40 34.10 -3.44 -18.40 -17.79 34.60 -3.31 -10.30 -9.62 34.20 -2.95 -3.70 -4.80 33.80 -2.72 1.10 -0.51 33.90 -2.60 6.30 1.14 33.90 -2.58 6.30 1.62 33.90 -2.77 6.30 1.45 33.90 -3.07 6.30 0.76 34.60 -3.36 3.60 -0.75 35.20 -4.14 -0.20 -5.04 36.10 -5.35 -5.70 -10.26 37.70 -7.77 -13.10 -20.51 40.10 -9.50 -24.20 -30.25 41.90 -10.71 -37.10 -43.90 42.10 -10.93 -62.10 -54.29 41.60 -11.33 -73.90 -68.18 41.50 -12.62 -86.70 -79.70 43.00 -15.41 -101.70 -95.50 47.60 -17.37 -128.40 -106.20 48.60 -19.29 -138.40 -118.31 49.20 -20.72 -147.80 -126.33 50.70 -23.47 -156.00 -134.41 53.40 -26.43 -162.60 -138.77 57.30 -30.66 -167.10 -142.76 64.70 -32.98 -172.40 -144.95 66.10 -34.09 -174.60 -147.18 65.90 -34.08 -176.20 -148.16 65.30 -34.51 -177.00 -148.04 66.40 -34.81 -175.10 -146.84 65.90 -33.80 -172.10 -143.09 64.00 -32.46 -167.10 -138.89 62.50 -31.28 -161.30 -131.42 62.50 -30.85 -153.90 -124.05 62.80 -29.01 -144.80 -112.27 58.90 -25.89 -122.40 -102.99 54.50 -20.49 -111.50 -91.62 49.80 -16.91 -102.10 -84.01 46.10 -14.37 -94.40 -73.49 44.80 -13.71 -86.20 -63.68 44.20 -12.08 -61.00 -47.69 42.10 -10.17 -45.00 -34.88 39.70 -8.36 -29.70 -19.42 38.70 -8.41 -16.00 -9.72 39.50 -9.70 -5.00 0.73 42.60 -10.48 10.60 5.98 43.10 -10.63 14.50 10.31 43.10 -10.49 17.00 12.36 43.00 -10.88 18.90 14.64 44.30 -11.56 23.00 16.66 45.20 -12.10 26.00 19.72 45.30 -12.09 28.70 21.38 45.30 -12.10 30.10 22.32 45.30 -12.13 30.10 22.63 45.30 -11.89 30.10 22.80 45.40 -11.96 30.80 22.75 45.40 -12.55 30.80 22.74 46.60 -12.78 30.60 22.82 46.60 -12.76 30.60 22.56 46.60 -13.13 29.50 21.14 47.60 -15.13 26.50 15.63 50.00 -17.41 19.90 6.91 52.40 -20.64 13.30 -12.58 54.40 -20.34 -1.80 -27.64 54.40 -20.62 -1.80 -45.53 55.20 -20.38 -36.00 -56.23 53.90 -20.62 -48.40 -69.71 52.40 -21.19 -57.90 -78.72 51.80 -23.01 -65.50 -88.55 54.90 -25.93 -77.00 -96.73 57.50 -28.53 -79.90 -101.80 59.30 -29.59 -85.40 -106.57 58.80 -29.86 -89.80 -112.66 58.50 -29.93 -93.70 -116.66 57.60 -29.43 -95.20 -118.24 56.30 -28.51 -93.30 -116.28 52.00 -26.31 -78.00 -107.95 49.60 -24.08 -63.40 -97.28 48.20 -22.19 -47.20 -79.99 48.60 -21.99 -30.30 -66.21 49.20 -21.76 -13.70 -47.37 48.20 -21.26 19.50 -33.66 47.60 -20.72 34.30 -15.92 48.10 -20.68 50.20 -3.23 48.70 -21.18 64.20 11.32 49.30 -21.72 75.00 19.36 50.30 -22.36 82.80 25.35 50.80 -22.31 86.60 26.28 50.00 -21.65 84.70 24.61 49.30 -21.41 81.90 23.02 49.30 -21.02 80.30 21.38 48.00 -20.41 77.70 20.06 47.50 -20.06 75.60 17.88 47.80 -20.22 73.20 15.94 48.00 -20.30 70.30 12.56 48.00 -20.53 66.80 9.70 49.30 -21.66 59.10 5.40 50.50 -22.43 55.30 2.50 50.30 -21.89 52.30 -0.55 48.70 -20.75 49.80 -2.54 46.90 -20.11 47.40 -4.41 46.60 -20.21 45.70 -4.79 46.60 -19.98 45.70 -4.10 46.60 -19.41 46.90 -3.26 45.70 -18.63 47.70 -2.85 44.30 -18.09 46.90 -3.12 44.10 -17.72 46.10 -3.68 44.10 -17.69 46.10 -3.77 44.10 -17.81 46.10 -3.30 44.10 -18.09 46.10 -2.93 44.60 -18.32 46.90 -2.64 44.60 -18.35 46.90 -2.45 44.60 -18.37 46.90 -2.53 44.60 -18.42 46.90 -2.91 44.80 -18.41 45.90 -3.58 44.60 -18.40 45.20 -3.87 44.60 -18.69 45.20 -3.52 45.20 -19.01 45.90 -3.03 45.20 -19.66 45.90 -3.36 46.50 -20.11 45.30 -4.71 47.00 -20.32 40.30 -7.30 47.00 -20.21 37.90 -8.95 46.60 -19.73 37.00 -9.34 46.00 -19.58 38.10 -7.81 46.20 -19.66 41.20 -3.99 46.60 -19.57 50.20 0.24 46.40 -19.24 57.50 8.72 46.40 -19.04 68.70 17.37 46.50 -19.02 80.70 30.79 47.00 -19.15 105.00 40.71 47.60 -19.89 115.10 52.58 49.00 -21.00 123.90 60.18 51.00 -22.71 132.10 69.40 52.40 -23.14 139.00 75.21 52.40 -23.27 145.40 81.37 52.80 -23.34 154.10 85.22 52.50 -22.71 157.20 88.44 51.80 -22.02 159.00 89.68 51.30 -21.03 159.80 89.76 50.00 -20.31 158.70 89.69 50.00 -19.46 158.70 89.19 49.20 -18.99 158.00 88.77 49.20 -18.33 158.00 88.10 48.00 -17.51 156.80 86.95 46.90 -16.39 154.70 83.75 45.70 -16.01 146.70 79.31 45.60 -16.01 138.60 69.55 45.60 -16.67 127.70 59.62 46.30 -19.11 114.50 44.34 51.00 -21.27 85.10 32.51 52.40 -23.13 72.10 16.22 52.40 -23.91 56.10 1.63 53.00 -25.83 37.00 -19.97 55.30 -27.95 16.70 -35.75 57.80 -30.40 -1.80 -55.29 60.20 -31.26 -34.10 -67.97 59.70 -32.08 -46.30 -81.42 60.50 -33.17 -57.00 -89.14 61.70 -34.94 -65.90 -96.66 63.40 -35.73 -71.60 -99.29 63.90 -35.81 -74.30 -100.26 63.90 -35.84 -75.00 -100.82 63.90 -36.64 -76.00 -101.85 65.10 -36.78 -77.30 -102.08 65.10 -34.80 -77.30 -100.55 59.20 -32.05 -72.10 -95.69 55.40 -28.39 -61.40 -81.69 53.10 -26.86 -45.00 -66.00 53.10 -26.21 -23.80 -41.62 54.10 -26.26 -0.50 -25.10 55.10 -26.50 20.60 -10.47 55.90 -26.52 42.10 -5.71 55.70 -25.95 43.40 -6.43 55.20 -25.39 39.90 -10.20 54.30 -24.71 34.10 -15.67 52.90 -24.38 24.50 -18.43 52.50 -24.00 23.10 -19.56 52.50 -23.81 23.10 -19.25 52.00 -23.94 24.60 -18.53 52.10 -24.57 25.50 -18.44 52.70 -26.03 25.60 -19.24 55.50 -26.96 24.00 -20.88 56.30 -27.28 21.40 -24.14 56.40 -27.36 18.00 -26.86 56.20 -27.94 14.30 -29.91 57.30 -29.01 9.50 -31.52 59.10 -31.19 8.20 -32.69 61.30 -33.44 7.80 -33.05 64.20 -38.04 7.60 -34.11 68.40 -42.79 6.90 -35.84 73.90 -49.56 4.90 -38.90 85.30 -54.44 -0.90 -42.11 90.10 -59.59 -4.70 -46.68 93.90 -62.89 -9.30 -49.85 97.70 -65.98 -13.40 -53.54 101.50 -66.21 -19.90 -55.83 99.50 -63.96 -22.40 -58.04 96.40 -61.29 -24.00 -58.92 92.70 -55.77 -24.40 -58.21 88.10 -48.87 -23.40 -55.76 71.20 -35.76 -16.10 -50.48 57.60 -23.95 -9.90 -45.36 43.20 -8.47 -2.90 -37.80 29.40 0.68 4.50 -32.09 18.40 8.57 12.10 -25.57 7.60 13.00 23.70 -20.97 2.60 21.83 28.80 -14.76 -6.20 31.23 34.50 -9.76 -18.80 43.03 40.70 -2.59 -37.80 49.01 54.00 3.18 -42.60 55.99 61.00 11.80 -48.50 62.55 68.80 18.63 -56.40 71.68 76.90 25.36 -65.30 76.74 82.90 27.48 -72.10 77.93 84.90 27.43 -71.40 76.27 83.20 26.45 -64.70 72.33 79.00 23.74 -59.00 67.54 74.40 20.10 -50.70 58.10 67.70 12.16 -29.70 49.21 47.50 3.91 -17.50 35.67 35.80 -8.81 -5.60 25.46 24.40 -18.28 4.70 13.43 15.10 -28.00 13.50 6.32 10.00 -31.62 19.10 -0.05 8.90 -33.01 26.20 -3.19 9.30 -33.13 29.20 -6.57 9.40 -32.95 32.20 -8.94 9.20 -32.87 35.30 -12.12 9.00 -33.11 38.20 -14.10 8.30 -33.56 41.00 -15.39 6.20 -34.81 41.00 -15.76 4.60 -36.05 41.70 -16.58 3.30 -37.15 42.70 -17.28 3.30 -37.03 43.50 -18.09 4.30 -35.50 46.10 -19.41 8.80 -33.34 49.30 -22.47 11.30 -30.18 52.30 -24.56 14.20 -27.26 54.40 -26.97 19.00 -21.26 59.00 -28.75 33.00 -15.47 60.50 -30.52 39.80 -7.58 61.30 -31.24 46.30 -1.97 62.20 -32.42 52.60 4.90 63.90 -34.10 58.10 9.30 66.60 -36.25 62.80 13.18 68.60 -36.12 65.60 13.89 64.90 -33.50 63.20 11.30 60.80 -29.64 57.60 6.44 54.30 -21.82 49.50 -3.12 34.90 -14.15 28.30 -12.02 24.60 -3.13 17.20 -24.78 16.10 4.39 6.00 -34.99 8.40 12.92 -6.30 -49.07 2.20 18.09 -18.20 -59.43 -3.40 22.81 -29.60 -70.75 -8.40 24.78 -44.70 -76.84 -9.70 26.55 -49.70 -82.78 -10.30 26.97 -53.00 -85.33 -9.50 26.00 -54.40 -85.21 -6.00 24.82 -48.00 -82.56 -4.50 23.05 -43.10 -77.20 -2.80 21.50 -36.90 -71.90 -0.40 18.24 -28.60 -62.48 3.20 14.11 -19.40 -54.68 9.10 7.91 -10.30 -44.92 19.20 4.84 6.00 -37.78 21.50 1.68 14.20 -29.01 24.60 -1.00 20.60 -23.71 28.10 -3.97 25.80 -17.60 30.60 -4.59 36.20 -12.95 27.90 -1.52 41.30 -6.89 21.30 4.14 46.50 -2.33 12.00 14.51 52.70 4.38 2.10 23.40 59.30 9.94 -19.80 35.96 74.50 18.06 -31.00 45.38 81.90 24.02 -41.20 57.07 88.00 31.26 -50.40 64.58 94.30 36.13 -58.10 71.74 99.30 40.34 -63.70 73.28 102.20 41.79 -61.10 71.16 101.70 41.69 -57.20 67.97 99.70 39.90 -51.60 60.26 95.20 33.96 -31.90 51.09 77.70 26.32 -18.10 35.89 65.50 13.59 -4.70 24.59 53.50 3.91 7.10 11.01 42.70 -7.42 17.80 1.65 33.90 -13.87 28.50 -10.03 27.40 -18.61 47.40 -17.59 23.40 -19.48 55.30 -26.28 24.40 -18.44 62.50 -32.19 26.30 -16.73 69.10 -39.21 28.20 -13.35 80.00 -43.58 35.40 -9.66 85.40 -49.91 40.00 -4.17 92.00 -55.13 44.10 -0.25 97.90 -59.27 48.20 3.77 99.30 -59.17 50.40 5.19 97.20 -56.66 51.00 4.89 92.30 -54.71 48.10 3.05 90.10 -50.92 45.00 -0.72 84.50 -45.38 40.70 -5.31 75.40 -34.34 34.10 -15.20 53.70 -25.62 12.40 -24.94 45.50 -16.03 0.50 -38.88 39.50 -11.10 -11.60 -49.21 35.50 -7.25 -23.60 -60.98 33.20 -5.86 -32.20 -65.77 32.30 -6.22 -35.40 -65.23 35.30 -7.94 -27.40 -60.96 38.20 -10.18 -19.40 -52.78 39.90 -11.33 -12.20 -47.98 41.00 -12.60 -8.50 -43.89 42.80 -13.26 -3.60 -41.87 42.80 -13.23 -1.80 -39.93 41.80 -12.61 -0.70 -38.93 40.90 -12.72 0.30 -37.65 41.60 -13.48 1.70 -36.49 43.30 -13.88 3.90 -35.81 43.30 -13.78 3.90 -36.13 42.70 -14.19 2.50 -36.94 43.60 -14.82 1.70 -37.40 43.60 -15.16 1.70 -37.31 44.60 -15.19 3.00 -36.44 45.00 -15.44 4.80 -34.82 45.20 -15.40 6.30 -33.95 44.90 -14.86 6.90 -33.76 43.10 -14.13 6.30 -33.96 42.30 -13.36 6.40 -33.64 41.90 -13.07 7.70 -32.77 41.60 -12.86 8.90 -31.84 41.60 -12.87 8.90 -32.08 41.60 -13.11 8.90 -33.47 42.00 -13.63 5.20 -34.19 42.70 -14.32 5.40 -33.80 43.00 -14.53 6.70 -33.13 43.10 -15.22 7.40 -32.89 44.70 -15.84 6.90 -33.15 44.70 -15.69 6.90 -33.55 43.60 -15.40 6.40 -33.45 43.60 -15.53 6.40 -33.12 44.00 -15.72 7.30 -33.27 44.00 -15.74 7.30 -33.89 44.10 -15.83 6.40 -34.05 44.10 -16.03 6.40 -33.07 44.60 -15.90 8.50 -31.94 44.20 -15.44 9.90 -31.37 43.40 -15.22 9.60 -31.74 43.30 -15.04 8.70 -32.45 43.30 -14.81 8.70 -32.63 42.60 -14.83 8.30 -32.54 42.60 -15.13 8.30 -32.53 43.20 -15.01 8.50 -32.90 42.20 -14.41 7.50 -33.25 41.40 -14.08 7.40 -33.27 41.40 -14.23 7.40 -33.01 41.90 -14.41 8.00 -32.69 41.90 -14.68 8.00 -32.63 42.80 -15.11 8.40 -32.62 42.80 -15.07 8.40 -32.56 42.80 -14.97 8.40 -32.59 42.80 -14.99 8.40 -32.65 42.80 -14.87 8.40 -32.55 42.80 -14.97 8.40 -32.44 42.90 -15.34 9.10 -32.38 42.90 -15.45 9.10 -32.36 42.90 -15.44 9.10 -32.55 ];


compen_rate = 1.2; % 光流位移和倾角位移有一些误差，这里将倾角位移放大1.2倍比较合适
flow_data_pos_x = y(1:4:length(y)); % 光流位移x，单位mm
roll_compensate = y(2:4:length(y)) * compen_rate; % 倾角roll位移，roll角乘以高度数据，单位mm
flow_data_pos_y = y(3:4:length(y)); % 光流位移y，单位mm
pitch_compensate = y(4:4:length(y)) * compen_rate; % 倾角pitch补偿位移，pitch角乘以高度数据，单位mm

flow_x_pos_lpf_i(1) = 0;
flow_y_pos_lpf_i(1) = 0;
lpf = 0.4;
for i = 2:1:length(flow_data_pos_x)
    flow_x_pos_lpf_i(i) = flow_x_pos_lpf_i(i - 1) * (1 - lpf) + flow_data_pos_x(i) * lpf; % 光流位移滤波
    flow_y_pos_lpf_i(i) = flow_y_pos_lpf_i(i - 1) * (1 - lpf) + flow_data_pos_y(i) * lpf;
end

flow_x_att(1) = 0;
flow_y_att(1) = 0;
for i = 2:1:length(flow_data_pos_x)
    flow_x_att(i) = flow_x_att(i - 1) * (1 - lpf) + roll_compensate(i) * lpf; % 姿态角位移滤波
    flow_y_att(i) = flow_y_att(i - 1) * (1 - lpf) + pitch_compensate(i) * lpf;
end

lpf = 0.1;
flow_x_lpf_att_i(1) = 0; % x补偿后位移
flow_y_lpf_att_i(1) = 0; % y补偿后位移
for i = 2:1:length(flow_data_pos_x)
    flow_x_lpf_att_i(i) = flow_x_lpf_att_i(i - 1) * (1 - lpf) + (flow_x_pos_lpf_i(i) + flow_x_att(i)) * lpf; % x补偿后位移滤波
    flow_y_lpf_att_i(i) = flow_y_lpf_att_i(i - 1) * (1 - lpf) + (flow_y_pos_lpf_i(i) - flow_y_att(i)) * lpf; % x补偿后位移滤波
end

Hz = 50;
flow_vel_x_i = diff(flow_x_lpf_att_i) * Hz;
flow_vel_y_i = diff(flow_y_lpf_att_i) * Hz;
flow_x_vel_lpf_i(1) = 0;
flow_y_vel_lpf_i(1) = 0;

lpf = 0.1;
for i = 2:1:length(flow_vel_x_i)
    flow_x_vel_lpf_i(i) = flow_x_vel_lpf_i(i - 1) * (1 - lpf) + flow_vel_x_i(i) * lpf; % 光流速度滤波
    flow_y_vel_lpf_i(i) = flow_y_vel_lpf_i(i - 1) * (1 - lpf) + flow_vel_y_i(i) * lpf;
end

figure();
% plot(flow_data_pos_x, 'r');
% hold on;
plot(flow_x_pos_lpf_i, 'r');
% hold on;
% plot(-roll_compensate, 'b');
hold on;
plot(-flow_x_att, 'g');
hold on;
plot(flow_x_lpf_att_i, 'b');
hold on;
plot(flow_x_vel_lpf_i, 'm');
% ylim([-200,200]);
grid on;

% figure();
% % plot(flow_data_pos_y, 'r');
% % hold on;
% plot(flow_y_pos_lpf_i, 'r');
% % hold on;
% % plot(-pitch_compensate, 'b');
% hold on;
% plot(flow_y_att, 'g');
% hold on;
% plot(flow_y_lpf_att_i, 'b');
% hold on;
% plot(flow_y_vel_lpf_i, 'm');
% % ylim([-500,500]);
% grid on;

仿真结果如图1

图1

在通过无人机采集光流位移数据和倾角时需要注意，通常无人机的姿态角解算周期远大于光流数据的周期，例如，我们使用的无人机姿态角解算周期为6ms，而使用的光流模块是凌启科技自研的双目测距及光流一体模块，其周期是20ms，也就是50Hz，如果每次接收到光流数据后就实时的获取当前倾角补偿，那最终效果就是倾角位移始终领先光流位移，如图2所示，补偿后结果偏大，

图2

因为接收到的光流数据，其实是20ms前系统的位移，而倾角是6ms前的，所以会出现这种情况，我们具体的做法是在接收到光流数据时就获取倾角并保存，在下一次接收到光流数据时采用该倾角补偿。

另外，在仿真中我们发现，光流位移和倾角位移存在一定的比例误差，我们的做法是将倾角位移扩大或缩小一定倍数，如代码中的compen_rate变量为1.2倍。