标签：quat 路径 prop length 源码 params 无人机 sdot arm
1 简介
SSA主要模拟了麻雀觅食的过程。麻雀觅食过程是发现者-跟随者模型的一种，同时还叠加了侦查预警机制。麻雀中容易找到食物的个体作为发现者，其他个体作为跟随者，同时种群中选取一定比例的个体进行侦查预警，如果发现危险则放弃食物，安全第一。由文献可知，SSA 是一种优于GWO、PSO、引力搜索算法（Gravity Search Algorithm，GSA）等算法的一种新的群智能优化算法。SSA算法中有发现者、追随者以及警戒者，分别按照各自规则进行位置更新，更新规则如下：
2 部分代码
function sdot = quadEOM_readonly(t, s, F, M, params) % QUADEOM_READONLY Solve quadrotor equation of motion % quadEOM_readonly calculate the derivative of the state vector % % INPUTS: % t - 1 x 1, time % s - 13 x 1, state vector = [x, y, z, xd, yd, zd, qw, qx, qy, qz, p, q, r] % F - 1 x 1, thrust output from controller (only used in simulation) % M - 3 x 1, moments output from controller (only used in simulation) % params - struct, output from nanoplus() and whatever parameters you want to pass in % % OUTPUTS: % sdot - 13 x 1, derivative of state vector s % % NOTE: You should not modify this function % See Also: quadEOM_readonly, nanoplus %************ EQUATIONS OF MOTION ************************ % Limit the force and moments due to actuator limits A = [0.25, 0, -0.5/params.arm_length; 0.25, 0.5/params.arm_length, 0; 0.25, 0, 0.5/params.arm_length; 0.25, -0.5/params.arm_length, 0]; prop_thrusts = A*[F;M(1:2)]; % Not using moment about Z-axis for limits prop_thrusts_clamped = max(min(prop_thrusts, params.maxF/4), params.minF/4); B = [ 1, 1, 1, 1; 0, params.arm_length, 0, -params.arm_length; -params.arm_length, 0, params.arm_length, 0]; F = B(1,:)*prop_thrusts_clamped; M = [B(2:3,:)*prop_thrusts_clamped; M(3)]; % Assign states x = s(1); y = s(2); z = s(3); xdot = s(4); ydot = s(5); zdot = s(6); qW = s(7); qX = s(8); qY = s(9); qZ = s(10); p = s(11); q = s(12); r = s(13); quat = [qW; qX; qY; qZ]; bRw = QuatToRot(quat); wRb = bRw'; % Acceleration accel = 1 / params.mass * (wRb * [0; 0; F] - [0; 0; params.mass * params.grav]); % Angular velocity K_quat = 2; %this enforces the magnitude 1 constraint for the quaternion quaterror = 1 - (qW^2 + qX^2 + qY^2 + qZ^2); qdot = -1/2*[0, -p, -q, -r;... p, 0, -r, q;... q, r, 0, -p;... r, -q, p, 0] * quat + K_quat*quaterror * quat; % Angular acceleration omega = [p;q;r]; pqrdot = params.invI * (M - cross(omega, params.I*omega)); % Assemble sdot sdot = zeros(13,1); sdot(1) = xdot; sdot(2) = ydot; sdot(3) = zdot; sdot(4) = accel(1); sdot(5) = accel(2); sdot(6) = accel(3); sdot(7) = qdot(1); sdot(8) = qdot(2); sdot(9) = qdot(3); sdot(10) = qdot(4); sdot(11) = pqrdot(1); sdot(12) = pqrdot(2); sdot(13) = pqrdot(3); end
3 仿真结果
4 参考文献
[1]刘贵云等. "基于混沌自适应麻雀搜索算法的无人机三维航迹规划方法.".
标签：quat,路径,prop,length,源码,params,无人机,sdot,arm 来源： https://blog.csdn.net/m0_60703264/article/details/121170612
本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享； 2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关； 3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关； 4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除； 5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。