客服电话
APP下载
迪恩网络APP
随时随地掌握行业动态
官方微信
扫描二维码
关注迪恩网络微信公众号
四轴飞行器1.1 Matlab 姿态显示开始做四轴了，一步一步来，东西实在很多，比较杂。先做matlab上位机，主要用来做数据分析，等板子到了可以写飞控的程序了，从底层一层一层开始写。。希望能好好的完成它。。。关于matlab上位机，首先做个姿态显示，然后等板子来了，把板子底层程序写好后，加上matlab的串口接收部分，基本的环境就算搭建好了。。。。 这个代码写了一天，写到最后出现戏剧性的一幕，实在是太恶心了哈。。开始自己的想法就是通过输入pitch roll yaw三个欧拉角，然后在空间中现实飞机的姿态，为了学习matlab翻了matlab的书，还看了线性代数，为了画这个姿态图，看了高中的立体解析几何，向量运算等。。。都是泪啊，说回正题，首先计算xOy平面中的转动，也就是yaw轴，这个相对比较简单，让三角形的三个点分别在图中的大圆和小圆上，如图所示：
yaw解决了之后就需要解决pitch了，就是俯仰角，约定是以坐标的（0 0 0）点进行旋转的，也是两个圆的圆心，所以算pitch只需要在xOz平面内计算，通过sin（pitch）可以算出来A B C三个点在Z轴上的坐标了，这里需要注意下，A点变换后，相对应的X轴变化是cos(pitch),y轴也是，算到这里会发现一个问题，用matlab算B C连个点的时候，只需算B或者C，解出来是有两个解的，一个B一个C，B和C必须分辨清楚，否则在计算roll的时候因为 B C没有分清楚会导致roll旋转方向不确定，后面再说B C怎么分辨。 % A % E O F % B D C 1 %% 2 %2014.7.19 由 sky.zhou 编写 3 function DrawAttitude(pitch,roll,yaw) 4 %% 5 %用于显示飞机姿态，输入为pitch，roll，yaw。 6 %自己的2B算法算的太慢了，我勒个去。。。还是用方向余弦吧 7 mode = 2 %标记用那种方法进行计算，1：表示用自己写的2B算法进行计算，2表示用方向余弦矩阵进行计算 8 9 %pitch = 60; 10 %roll = 45; 11 %yaw = 35; 12 r1 =3; %大圆半径 13 r2 = 0.618*r1; %小圆半径 14 15 if mode == 2 16 pitch = -pitch; %角度定义不一样，改一下 17 roll = -roll; %角度定义方式不一样，自己习惯改就好，看你希望是以怎样的方向转 18 end 19 dc = [cosd(yaw)*cosd(pitch)-sind(yaw)*sind(roll)*sind(pitch) sind(yaw)*cosd(pitch)+cosd(yaw)*sind(roll)*sind(pitch) cosd(roll)*(-sind(pitch)); 20 sind(yaw)*(-cosd(roll)) cosd(yaw)*cosd(roll) sind(roll) ; 21 cosd(yaw)*sind(pitch)+sind(yaw)*sind(roll)*cosd(pitch) sind(yaw)*sind(pitch)-cosd(yaw)*sind(roll)*cosd(pitch) cosd(roll)*cosd(pitch) ] 22 %三角形规约：A为定点，B C为两边的角，具体方位如下 23 % A 24 % B C 25 t_fpa = 35; %三角形定点角度设置为40度，fpa On behalf of Fixed point angle 26 t_b = (180 - t_fpa) / 2; 27 t_c = t_b; 28 29 if t_fpa > asind((r2/r1))*2 30 t_fpa = asind((r2/r1))*2 31 end 32 33 %xd,yd,zd存放真是数值，与符号xyz区分开来 34 %约定 xd yd zd 第 1 2 3 4位分别代表三角形ABC的 A、B、A、C坐标 35 if mode == 2 36 xd=[3 -1.2735;3 -1.2735]; 37 yd=[0 1.3474;0 -1.3474]; 38 zd=[0 0;0 0]; 39 %上面几个初始化的点是根据 定义的。 40 %pitch = 0; 41 %roll = 0; 42 %yaw = 0; 43 %r1 =3; %大圆半径 44 %r2 = 0.618*r1; %小圆半径 45 else 46 xd=[]; 47 yd=[]; 48 zd=[]; 49 tempA =[]; %保存中间计算角度，目前之用来保存角BOA 50 end 51 temp = []; 52 if mode == 2 53 temp = [xd(1,1) yd(1,1) zd(1,1); 54 xd(1,2) yd(1,2) zd(1,2); 55 xd(2,2) yd(2,2) zd(2,2)]; 56 temp = temp*dc; 57 xd = [temp(1:2,1)';temp(1,1),temp(3,1)] 58 yd = [temp(1:2,2)';temp(1,2),temp(3,2)] 59 zd = [temp(1:2,3)';temp(1,3),temp(3,3)] 60 %到此位置，方向余弦矩阵已经计算完毕，可以直接用后面的函数进行显示 61 end 62 63 if mode == 1 %执行自己的2B算法 64 %xs ys zs分别问记录方程的解 xs 为sysm缩写 65 syms x y z r xs ys zs; %x y z 惯性坐标系中三个正交基，r为xOy平面中的大圆和小圆半径 66 %定义各点的坐标符号参数 67 syms xa ya za xb yb zb za zb zc ; 68 69 %% 70 c1 = sym('x^2+y^2 = r^2'); %大圆方程 71 c1 = subs(c1,'r',r1) %换成实际数值 72 73 c2 = sym('x^2+y^2 = r^2'); %校园方程，可以表达为：c2 = 'x^2+y^2 = r^2'，效果是一样的 74 c2 = subs(c2,'r',r2) 75 76 l1 = sym('cosd(yaw)*y=sind(yaw)*x') 77 %l1 = sym('y=tand(yaw)*x') %不用这个公式是因为这个公式有零点，90和-90无法使用 78 %l1 = subs(l1, 'yaw', yaw) %换成实际数值,这里不要转成实际数值，为了方便subs的运算 79 %% 80 [xs ys] = solve(c1,l1,'x','y') %注意，这里算出来的xd yd是符号变量，matlab自动转换了，下面重新对其赋值，可以变回数值变量 81 82 %双百分号还可以类似于分类的作用，挺好。 83 temp = subs([xs;ys]) 84 85 %% 86 %计算A点坐标 87 if yaw > -90 && yaw < 90 %判断角度的范围，用来选择在坐标中三角形的顶点是正还是负 88 %这个可能有点难理解，角度确定了，就可以知道焦点在x轴的正负，从前两个数值中取对应的X解后，然后取对应的Y的解 89 temp = temp([temp(1:2)>0;temp(1:2)>0]) 90 elseif yaw == -90 91 temp = [ 0 ;temp(temp<0)] 92 elseif yaw == 90 93 temp = [ 0 ;temp(temp>0)] 94 else 95 temp = temp([temp(1:2)<0;temp(1:2)<0]) 96 end 97 98 %得到在XOY平面中三角形定点的第一个解 99 xd = [xd temp(1)] 100 yd = [yd temp(2)] 101 102 %% 103 %计算B点坐标 104 105 %temp计算出来表示的是 AB段的长度， 106 % A 107 % O 108 % B D C 109 %其中 sind(t_b/2)*r2 表示的是OD段的长度，cosd(t_b/2)*r2是BD段的长度， 110 %temp计算的最终结果是AB的长度 111 %利用三角形边与对面角正弦成比例进行运算 112 % AB BC A0 B0 113 % ----- = ----- ----- = --------- 114 % sin(C) sin(A) sin(角ABO) sin(角OAB)（ps:A的一半） 115 % 可以求出角ABO，然后通过内角和可以求出角AOB 116 % AB BO 117 % ----- = -------- 可以求出AB长度，简化代码如下 118 % sin(角AOB) sin(角OAB) 119 % (180 - asind((r1/r2)*sind(t_fpa/2)) - (t_fpa/2)) 为角BOA的大小 120 tempA = sym('(180 - asind((r1/r2)*sind(t_fpa/2)) - (t_fpa/2))'); 121 temp = sym('(r2/sind(t_fpa/2))*sind(tempA)'); 122 tempA = subs(tempA); 123 temp = subs(temp); 124 125 126 %temp = subs(sym('sqrt(((sind(t_b/2)*r2)+r1)^2 + (cosd(t_b/2)*r2)^2)')); 127 128 %假设 符号 xa ya 为 A点的坐标,x,y为要求的B点坐标 129 temp = subs(sym('(x-xa)^2 + (y-ya)^2 = temp^2'),'temp',temp); 130 %将xa和ya换成数值xa和ya，嵌套换的 131 temp = subs(subs(temp,'xa',xd(1)),'ya',yd(1)) 132 [xs ys] = solve(temp,c2,'x','y') 133 134 %通过下面的计算就已经可以得到 B C的坐标了 135 temp = subs([xs;ys]) 136 137 %下面需要做的是区别哪个点是A，哪个点是B。 138 %% 139 % 下面是在xOy平面内的旋转 140 % B 141 % D O A yaw=0度的时候三角型在X0Y平面的方位，其中水平位置为x轴竖直方向为Y轴 142 % C 143 % 取一个与DB方向一样的方向向量n（0，1） 144 % 用旋转矩阵让它跟三角形同步旋转 145 % 因为n与OB为锐角，与OB为钝角，so，n与OB点乘为负数，与OC点乘为正数，从而区分出B点和C点 146 %% 147 % 为了避免roll为90度的时候按照之前的定义方向向量n=（0，0），区分不出来B和C点，所以用方向余弦矩阵进行计算 148 %方向余弦矩阵定义 149 %dc = [cosd(yaw)*cosd(pitch)-sind(yaw)*sind(roll)*sind(pitch) sind(yaw)*cosd(pitch)+cosd(yaw)*sind(roll)*sind(pitch) cosd(roll)*(-sind(pitch)); 150 % sind(yaw)*(-cosd(roll)) cosd(yaw)*cosd(roll) sind(roll) ; 151 % cosd(yaw)*sind(pitch)+sind(yaw)*sind(roll)*cosd(pitch) sind(yaw)*sind(pitch)-cosd(yaw)*sind(roll)*cosd(pitch) cosd(roll)*cosd(pitch) ] 152 %%算到这里的时候我发现只要在xOy平面内将三角形的初始化坐标ABC三个点输入后，用方向余弦矩阵算就可以了，然后花了10分钟不到的时间就实现了 153 %不过这里还是决定把这个方法写完。。。都是泪。。。。。。。。。。。。。。。。。 154 %% 155 n = [0 1 0] %方向向量 156 n = n*dc %对方向向量进行旋转 157 %约定 xd yd zd 第 1 2 3 4位分别代表三角形ABC的 A、B、A、C坐标 158 n = n*[temp(1);temp(3);0] 159 if n > 0 %说明夹角是锐角，该角是B点 160 xd = [ xd temp(1) xd temp(2)] 161 yd = [ yd temp(3) yd temp(4)] 162 else 163 xd = [ xd temp(2) xd temp(1)] 164 yd = [ yd temp(4) yd temp(3)] 165 end 166 167 %处理成变成矩阵形式 168 xd = [xd(1:2);xd(3:4)] 169 yd = [yd(1:2);yd(3:4)] 170 171 %当存在pitch角度的时候，X坐标做相印调整 172 xd = xd.*cosd(pitch) 173 yd = yd.*cosd(pitch) 174 175 176 %% 177 %约定 xd yd zd 第 1 2 3 4位分别代表三角形ABC的 A、B、A、C坐标 178 %计算z中A的坐标，其中B和C是相等的 179 zd = [zd sind(pitch)*r1] 180 181 %下面OD的长度，然后可以计算出B和C在Z轴上的坐标,也就是D点的坐标 182 od = (sind(tempA - 90)*r2) 183 %zd = [zd temp;zd temp] 184 185 %计算roll状态下B和C的坐标 186 % A 187 % E O F 188 % B D C 189 % 先计算在roll下OF的长度，然后算F在Z轴的高度，然后等比后算B和C在Z轴的高度 190 %下面计算OF的长度 191 l2 = tand(t_fpa/2)*r1 192 %下面计算F在Z轴上的变化高度 193 l2 = sind(roll)*l2 194 %下面计算C点在Z轴上的变化高度,通过相似三角形计算 195 l2 = l2*(r1+od)/r1 196 197 zd = [zd -l2;zd l2] 198 %x,y轴根据picth角度缩放 199 yd(:,2) = yd(:,2).*cosd(roll) 200 xd(:,2) = xd(:,2).*cosd(roll) 201 202 %额。。这方法写的心力交瘁。。。。。。。还是方向余弦好。。。四元素再学。。。。。。 203 204 205 end 206 surf(xd,yd,zd) 207 axis([-3 3 -3 3 -3 3]) 208 xlabel('X') 209 ylabel('Y') 210 zlabel('Z') 211 text(xd(1,1),yd(1,1),zd(1,1),'A点') 212 text(xd(1,2),yd(1,2),zd(1,2),'B点') 213 text(xd(2,2),yd(2,2),zd(2,2),'C点') 214 %% 215 %测试用圆 216 hold on 217 alpha=0:pi/20:2*pi; 218 x=r1*cos(alpha); 219 y=r1*sin(alpha); 220 plot(x,y); 221 222 hold on 223 x=r2*cos(alpha); 224 y=r2*sin(alpha); 225 plot(x,y); 226 227 hold off 228 end
|
2023-10-27
2022-08-15
2022-08-17
2022-09-23
2022-08-13
六六分期app的软件客服如何联系？不知道吗？加qq群【895510560】即可！标题：六六分期
今天小编告诉大家如何处理win10系统火狐flash插件总是崩溃的问题，可能很多用户都不知
今天小编告诉大家如何对win10系统删除桌面回收站图标进行设置，可能很多用户都不知道
今天小编告诉大家如何对win10系统电脑设置节能降温的设置方法，想必大家都遇到过需要
我们在使用xp系统的过程中,经常需要对xp系统无线网络安装向导设置进行设置，可能很多
今天小编告诉大家如何处理win7系统玩cf老是与主机连接不稳定的问题，可能很多用户都不
电脑对日常生活的重要性小编就不多说了，可是一旦碰到win7系统设置cf烟雾头的问题，很
我们在日常使用电脑的时候，有的小伙伴们可能在打开应用的时候会遇见提示应用程序无法
今天小编告诉大家如何对win7系统打开vcf文件进行设置，可能很多用户都不知道怎么对win
今天小编告诉大家如何对win10系统s4开启USB调试模式进行设置，可能很多用户都不知道怎
请发表评论