✅作者简介热爱科研的Matlab仿真开发者擅长毕业设计辅导、数学建模、数据处理、建模仿真、程序设计、完整代码获取、论文复现及科研仿真。 往期回顾关注个人主页Matlab科研工作室 关注我领取海量matlab电子书和数学建模资料个人信条格物致知,完整Matlab代码获取及仿真咨询内容私信。 内容介绍本报告针对地面移动机器人与空中无人机组成的异构无人集群实现了一套完整的虚拟结构编队控制方案以LIMO差分驱动移动机器人与Parrot Bebop 2四旋翼无人机为硬件载体通过分层内外环控制适配两类平台的动力学差异基于ROS Noetic搭建分布式实时通信链路引入零空间行为融合技术实现编队保持与动态避障的无冲突协同。经Gazebo仿真与实机联合测试验证系统可在室内混合场景下实现稳定协同运动编队相对位置跟踪稳态误差小于0.3m动态避障过程中编队构型保持度不低于92%全程无平台间碰撞风险可直接向仓储巡检、灾后联合搜救等工程场景迁移落地。1 系统总体设计1.1 异构平台特性分析本次协同控制的两类平台在运动维度、约束特性与控制接口上存在显著差异是整个系统设计的核心适配难点LIMO移动机器人为典型差分驱动非完整系统仅具备地面二维平面运动能力无法实现侧向瞬时平移最大运动速度1.0m/s搭载16线激光雷达与Jetson Nano机载计算机可通过激光SLAM实现无GNSS环境下的厘米级自定位底层控制接口为标准的差速轮速指令。Parrot Bebop 2无人机为小型消费级四旋翼平台具备三维全向机动能力最大平飞速度4.0m/s机身搭载前视与下视单目视觉传感器可通过机载视觉里程计输出6自由度位姿底层飞控支持直接接收期望位置、速度指令无需用户手动编写底层电机控制逻辑。两类平台的动力学响应带宽、运动约束完全不同传统针对同构多智能体的编队控制方法无法直接复用必须通过分层架构完成异构特性解耦。1.2 总体技术架构整套系统采用三层解耦设计从底层硬件到上层协同任务完全模块化避免不同功能模块之间的强耦合感知与通信层所有平台以ROS节点形式接入同一局域网通过MAVROS与机器人底盘驱动完成传感器数据采集与控制接口封装平台之间通过话题广播共享位姿、障碍信息与编队状态端到端通信延迟控制在50ms以内分层控制层针对两类平台分别设计内外环控制器内环专注于平台自身的姿态、速度稳定抵消模型不确定性与外部扰动外环负责跟踪虚拟结构分配的期望参考点适配不同平台的运动约束协同决策层实现虚拟结构参考点生成、零空间行为融合与全局安全监控在保证编队构型稳定的前提下动态融合避障、防撞等安全行为输出最终的平滑控制指令。1.3 虚拟结构编队范式定义本系统将整个异构编队抽象为一个可在空间中连续运动的刚性虚拟框架预先在虚拟结构的局部坐标系内为LIMO机器人与每一架Bebop 2无人机分配固定的期望参考点。虚拟结构可跟随全局领航轨迹完成整体平动、旋转与缩放每个智能体的核心控制目标就是实时跟踪自身对应的参考点从顶层设计上保证异构平台的编队构型一致性避免传统领航-跟随架构中容易出现的队形撕裂问题。2 核心模块实现细节2.1 异构平台内外环控制器设计针对两类平台的动力学特性分别定制完全适配的分层控制逻辑保证控制指令的平滑性与可执行性LIMO移动机器人内外环控制内环为差速轮速闭环通过底盘编码器实时反馈轮速设计增量式PID控制器快速修正左右轮的输出力矩抵消地面摩擦不均带来的速度跟踪误差轮速跟踪精度可达0.05m/s外环为位姿跟踪控制律基于非完整系统的李雅普诺夫稳定性理论推导将虚拟结构分配的二维期望参考点转化为机器人的期望线速度与角速度指令避免生成侧向不可达的运动分量保证机器人运动轨迹平滑无尖点。Parrot Bebop 2无人机内外环控制内环为四元数姿态控制回路利用机载IMU的高频数据反馈设计PD控制器实现姿态角的快速收敛在小幅风扰下姿态角跟踪误差可控制在±0.5°以内外环为位置-速度双闭环控制将虚拟结构分配的三维期望航点转化为无人机的期望加速度指令通过四旋翼微分平坦特性映射得到期望姿态与总升力直接输出给底层飞控执行无需手动处理复杂的旋翼力矩分配逻辑。内外环的分层设计将平台自身的稳定控制与编队协同任务完全解耦内环专注于底层扰动补偿外环专注于编队轨迹跟踪大幅降低了异构协同的控制难度。2.2 ROS分布式通信系统搭建本系统基于ROS Noetic搭建全分布式通信架构无需依赖地面站进行集中式指令转发所有平台自主完成信息交互与控制决策环境配置采用Ubuntu 20.04 ROS Noetic的稳定组合每台LIMO机器人与Bebop 2无人机的机载计算机都运行独立的ROS Master节点通过局域网内的节点发现机制自动完成组网为Bebop 2无人机部署适配的MAVROS扩展包打通机载飞控与ROS生态的通信链路实现无人机位姿、电池状态、控制指令的双向传输所有平台统一发布标准化的位姿话题、障碍点云话题与编队状态话题不同平台可通过订阅对应话题获取全局必要信息无需为异构硬件单独定制通信协议。同时加入通信故障容错机制当某一平台的通信出现短暂中断时其余平台可基于该平台的历史运动模型预测其位姿继续维持编队运行避免单点通信故障导致整个集群失控。2.3 零空间避障行为融合实现为解决传统编队控制中避障动作与队形保持相互冲突的问题本系统引入零空间行为融合技术通过任务优先级分层实现两类目标的协同将编队参考点跟踪定义为最高优先级任务将外部障碍物规避、平台之间自防撞定义为次优先级任务。首先求解编队跟踪任务的雅可比矩阵将避障任务生成的速度修正向量投影到该雅可比矩阵的零空间中得到的避障修正指令不会影响主编队任务的核心跟踪目标在执行避障动作时尽可能保留各智能体相对于虚拟结构的相对位置关系。针对不同场景设计差异化的避障增益当平台与障碍物的距离大于安全阈值时避障增益为0编队沿预设轨迹正常运动当距离进入预警区间时避障增益随距离减小平滑上升生成平缓的修正轨迹当距离接近碰撞阈值时避障增益快速增大生成强排斥指令保证平台快速脱离危险区域。整套逻辑无需切换控制模式全程实现平滑过渡避障动作完成后编队可自动回归原始轨迹无需额外的队形重构过程。⛳️ 运行结果 部分代码% 1) roscore no rosserver (192.168.0.100)% 2) OptiTrack publicando as poses (corpos rigidos L1 e B1)% 3) LIMO com limo_base.launch namespace:L1 (modo diferencial/4wd)% 4) Bebop com o launch do bebop_autonomy namespace:B1% 5) Joystick conectado (parada de emergencia)clear; clc; close all;%% ------------------------------------------------------------------ %%%% 1) PARAMETROS (todos vindos da especificacao do projeto) %%%% ------------------------------------------------------------------ %%T 1/30;Tsim 120; % (3 periodos da lemniscata)N round(Tsim/T);a 0.10;% --- Formacao desejada: drone 1,5 m acima do LIMO ---rho_f 1.5;alpha_f 0;beta_f pi/2;% --- Trajetoria da formacao ---TRAJ 1; % 0 ir para a origem (e permanecer) | 1 lemniscata de Bernoulli% --- Obstaculo e zona de influencia ---xo -0.20; % centro da baseyo 0.425; % centro da baseRobs 0.15;Rinf 0.25;nexp 4; % paraexp 0.19;bexp 0.19;Vd 0.05;kobs 1.0;% --- Ganhos do controlador CINEMATICO da formacao (Eq. 5.7) ---% Os valors de referencia que encontrei estão na pag 117 do livroK diag([0.8 0.8 0.8 1.5 1.0 1.5]); % valor de ref do livro: diag(0.2,0.2,0.2,3,1,1.8)L diag([0.30 0.30 0.30 0.8 0.6 0.8]); % valor de ref do livro: diag([1 1 1 1 1 1]); saturacao tanh (menor em x,y)% --- Ganhos do controle CARTESIANO do drone (robusto a singularidade beta90) ---Kp_B diag([1.0 1.0 1.2]);Ls_B diag([0.6 0.6 0.6]);% --- Ganhos dos COMPENSADORES DINAMICOS (Eq. 4.44 e 4.47) ---KD_L diag([4 4]);KD_B diag([4 4 4 4]); % [vx ; vy ; vz ; psidot]% --- Modelo dinamico do LIMO (da especificacao, Eq. 3.1-3.6) ---th [0.1521 0.0953 0.0031 0.9840 -0.0451 1.6422];% --- Modelo dinamico do Bebop (f1, f2 da especificacao): vdot f1*u - f2*vf1 diag([0.8417 0.8354 3.966 9.8524]);f2 diag([0.18227 0.17095 4.001 4.7295]);% --- Saturacoes fisicas dos comandos ---umax_L 0.30;wmax_L 1.20;umax_B 1.0;% --- Joystick ---BTN_STOP 1; % botao de parada de emergenciaMODO_BEBOP teste; % off (sem drone) | teste (motor desligado) | voo (normal)%% ------------------------------------------------------------------ %%%% 2) INICIALIZACAO DO ROS OptiTrack DECOLAGEM %%%% ------------------------------------------------------------------ %%rosshutdown;rosinit(192.168.0.100);% --- LIMO (namespace L1) ---pub_L rospublisher(/L1/cmd_vel,geometry_msgs/Twist);msg_L rosmessage(pub_L);% ponte OptiTrack: natnet_ros (alternativa do lab: /natnet_ros/L1/pose)pose_L rossubscriber(/natnet_ros/L1/pose,geometry_msgs/PoseStamped);% --- Bebop (namespace B1) ---pub_B rospublisher(/B1/cmd_vel,geometry_msgs/Twist);msg_B rosmessage(pub_B);pub_TO rospublisher(/B1/takeoff,std_msgs/Empty);msg_TO rosmessage(pub_TO);pub_LD rospublisher(/B1/land,std_msgs/Empty);msg_LD rosmessage(pub_LD);% ponte OptiTrack: natnet_ros (alternativa do lab: /natnet_ros/B1/pose)pose_B rossubscriber(/natnet_ros/B1/pose,geometry_msgs/PoseStamped);% --- Joystick (parada de emergencia) ---J vrjoystick(1);% --- Aguarda a primeira pose de cada robo (timeout 10 s) ---fprintf(Aguardando poses do OptiTrack (L1 e B1)...\n);receive(pose_L,10);if ~strcmp(MODO_BEBOP,off), receive(pose_B,10); end % teste/voo precisam da pose do dronefprintf(Poses OK.\n);if strcmp(MODO_BEBOP,voo)fprintf(Decolando o Bebop...\n);send(pub_TO,msg_TO);pause(5);elsefprintf(o Bebop NAO vai decolar.\n);end[x1,y1,z1,psi1] ler_pose(pose_L);if strcmp(MODO_BEBOP,off)x2x1;y2y1;z21.5;psi20;else[x2,y2,z2,psi2] ler_pose(pose_B);end% ponto de controle inicial (offset a10cm do CG, Eq. 2.16)poseL_ant [x1 a*cos(psi1); y1 a*sin(psi1)];poseL_psi_ant psi1;poseB_ant [x2;y2;z2];poseB_psi_ant psi2;vLmeas_f [0;0];vBmeas_f [0;0;0;0];vd_L_ant [0;0];vd_B_ant [0;0;0;0]; % para dvd (feedforward interno)%% ------------------------------------------------------------------ %%%% 3) Histórico para os graficos %%%% ------------------------------------------------------------------ %%H.t zeros(1,N);H.q zeros(6,N);H.qd zeros(6,N);H.p1 zeros(2,N);H.p2 zeros(3,N);H.dobs zeros(1,N);H.cmdL zeros(2,N);H.cmdB zeros(4,N);%% ------------------------------------------------------------------ %%%% 4) LOOP DE CONTROLE %%%% ------------------------------------------------------------------ %%fprintf(Iniciando formacao. Botao %d do joystick PARAR.\n, BTN_STOP);t0 tic; kf 0;tryfor k 1:Ntloop tic;t toc(t0);% ---- Parada de emergencia (joystick) ----btns button(J);if numel(btns) BTN_STOP btns(BTN_STOP)fprintf(Parada solicitada pelo joystick.\n); break;end% (0) LEITURA DE POSE (OptiTrack) [x1,y1,z1,psi1] ler_pose(pose_L);if strcmp(MODO_BEBOP,off)x2x1; y2y1; z21.5; psi20;else[x2,y2,z2,psi2] ler_pose(pose_B);end% Ponto de CONTROLE do LIMO (deslocado a10cm do CG, Eq. 2.15/2.16).% O ponto de interesse da formacao e este ponto, NAO o CG lido pelo OptiTrack.% (Se o corpo rigido L1 do OptiTrack ja estiver definido no ponto de controle,% basta zerar este offset fazendo xc1x1; yc1y1;)xc1 x1 a*cos(psi1);yc1 y1 a*sin(psi1);A1inv [ cos(psi1) sin(psi1);-sin(psi1)/a cos(psi1)/a ];A2inv [ cos(psi2) sin(psi2) 0;-sin(psi2) cos(psi2) 0;0 0 1 ];% (1) ESTIMATIVA DA VELOCIDADE ATUAL (corpo) velWL estimar_vel([xc1;yc1], poseL_ant, T); % vel. do PONTO DE CONTROLE [xdot; ydot]velWB estimar_vel([x2;y2;z2], poseB_ant, T); % [xdot2; ydot2; zdot2]psidot2 wrap_pi(psi2-poseB_psi_ant)/T;vL_meas A1inv*velWL; % [u ; w]vB_meas [A2inv*velWB; psidot2]; % [vx ; vy ; vz ; psidot]vLmeas_f vL_meas;vBmeas_f vB_meas;% (2) ESTADO DA FORMACAO: x - q (Eq. 5.5a) dxx2-xc1;dyy2-yc1;dzz2-z1;q [ xc1;yc1;z1;sqrt(dx^2dy^2dz^2);atan2(dy,dx);atan2(dz, sqrt(dx^2dy^2)) ];% (3) TRAJETORIA DESEJADA DA FORMACAO if TRAJ 1 % Lemniscata de Bernoullixd 0.75*sin(2*pi*t/40);dxd 0.75*(2*pi/40)*cos(2*pi*t/40);yd 0.75*sin(4*pi*t/40);dyd 0.75*(4*pi/40)*cos(4*pi*t/40);elsexd 0; dxd 0;yd 0; dyd 0;endqd [xd; yd; 0; rho_f; alpha_f; beta_f];dqd [dxd; dyd; 0; 0; 0; 0];% CONTROLADOR CINEMATICO DA FORMACAO (Eq. 5.7) qtil qd - q;qtil(5) wrap_pi(qtil(5)); qtil(6) wrap_pi(qtil(6)); % erros angularesdqr dqd L*tanh(L\(K*qtil));% (5) FORMACAO - ROBOS (mundo): Jacobiano (Eq. 5.6c) cacos(q(5)); sasin(q(5)); cbcos(q(6)); sbsin(q(6)); rfq(4);Jinv [ 1 0 0 0 0 0;0 1 0 0 0 0;0 0 1 0 0 0;1 0 0 ca*cb -rf*sa*cb -rf*ca*sb;0 1 0 sa*cb rf*ca*cb -rf*sa*sb;0 0 1 sb 0 rf*cb ];dx_form Jinv*dqr;% (6) DESVIO DE OBSTACULO por ESPACO NULO (NSB) dobs hypot(xc1-xo, yc1-yo); % distancia do PONTO DE CONTROLE ao obstaculoif dobs RinfV exp( -((xc1-xo)/aexp)^nexp - ((yc1-yo)/bexp)^nexp ); % Eq. 5.20Jo zeros(1,6);Jo(1) -nexp*((xc1-xo)^(nexp-1)/aexp^nexp)*V;Jo(2) -nexp*((yc1-yo)^(nexp-1)/bexp^nexp)*V;Jo_pinv Jo/(Jo*Jo 1e-9);dx_obs Jo_pinv*( 0 kobs*(Vd - V) ); % Eq. 5.22dx_r dx_obs (eye(6) - Jo_pinv*Jo)*dx_form; % Eq. 5.9elsedx_r dx_form;enddx1 dx_r(1:2); % LIMO - [xdot1; ydot1] (mundo, ja com desvio NSB)% (6b) DRONE: controle CARTESIANO relativo % JUSTIFICATIVA (nao consta no livro; adaptacao de implementacao):% Em beta90 (drone na vertical) o Jacobiano esferico (Eq. 5.6c) e singular% no azimute alpha. Como o enunciado inicia o drone deslocado lateralmente,% surge grande erro de azimute no transitorio, que faz o mapeamento por% Jacobiano girar o drone. No exemplo do livro (Sec. 5.6) isso nao ocorre% porque o drone parte pousado sobre o robo terrestre e sobe reto.% Solucao: controlar a POSICAO do drone diretamente, mantendo a estrutura% proporcionalfeedforwardtanh da Eq. (5.7), com o alvo p2d dado pela% transformacao inversa g(q) da Eq. (5.5b). Com beta90 e alpha0,% p2d (x1, y1, 1.5): drone na vertical do ponto de controle do LIMO.p2d [ xc1 rho_f*cos(alpha_f)*cos(beta_f);yc1 rho_f*sin(alpha_f)*cos(beta_f);z1 rho_f*sin(beta_f) ];p2 [x2; y2; z2];ff2 [dx1; 0];dx2 ff2 Ls_B*tanh(Ls_B\(Kp_B*(p2d - p2)));% ---- ALTERNATIVA (LIVRO, Eq. 5.6c): drone pelo Jacobiano esferico ----% Se o controle cartesiano acima nao se comportar bem (p.ex. em regime,% longe da singularidade beta90), COMENTE a linha dx2 ... acima e% DESCOMENTE a linha abaixo. As linhas 4-6 de dx_r ja sao a velocidade% desejada do drone no mundo (Jinv*dqr, ja com o desvio NSB e o% feedforward dqd embutidos). NAO precisa de p2d, p2, ff2 nem Kp_B/Ls_B.% dx2 dx_r(4:6); % [xdot2; ydot2; zdot2] no mundo% ATENCAO: em beta90 (drone na vertical) a coluna do azimute do Jinv% zera nas linhas 4-6 - singular; use esta opcao so com beta ! 90.% (7) velocidades desejadas de cada robo vd_L A1inv*dx1; % [u_d ; w_d]vd_B [A2inv*dx2; 0]; % [vx_d ; vy_d ; vz_d ; psidot_d(0)]if k1, dvd_L[0;0]; dvd_B[0;0;0;0];else, dvd_L(vd_L-vd_L_ant)/T; dvd_B(vd_B-vd_B_ant)/T; end% (8) COMPENSADOR DINAMICO DO LIMO (Eq. 4.44) w vLmeas_f(2);Hm [th(1) 0; 0 th(2)];Cm [th(4) -th(3)*w; th(5)*w th(6)];cmdL Hm*(dvd_L KD_L*(vd_L - vLmeas_f)) Cm*vLmeas_f; % [u_ref; w_ref]cmdL [saturar(cmdL(1),umax_L); saturar(cmdL(2),wmax_L)];% (9) COMPENSADOR DINAMICO DO BEBOP (Eq. 4.47) cmdB f1\(dvd_B KD_B*(vd_B - vBmeas_f) f2*vBmeas_f); % [uvx;uvy;uvz;upsi]cmdB saturar(cmdB, umax_B);% (10) ENVIO DE COMANDOS via ROS msg_L.Linear.X cmdL(1);msg_L.Linear.Y 0;msg_L.Linear.Z 0;msg_L.Angular.Z cmdL(2);send(pub_L,msg_L);% Envia cmd_vel ao Bebop em teste e voo. Em teste o drone NAO decolou,% entao o autopiloto ignora o cmd_vel e os MOTORES NAO ACIONAM (dry-run).if ~strcmp(MODO_BEBOP,off)msg_B.Linear.X cmdB(1);msg_B.Linear.Y cmdB(2);msg_B.Linear.Z cmdB(3);msg_B.Angular.Z cmdB(4);send(pub_B,msg_B);end% HISTORICO atualiza memorias H.t(k)t; H.q(:,k)q; H.qd(:,k)qd; H.p1(:,k)[xc1;yc1]; H.p2(:,k)[x2;y2;z2];H.dobs(k)dobs; H.cmdL(:,k)cmdL; H.cmdB(:,k)cmdB; kf k;poseL_ant[xc1;yc1]; poseL_psi_antpsi1;poseB_ant[x2;y2;z2]; poseB_psi_antpsi2;vd_L_antvd_L; vd_B_antvd_B;% ---- monitor ----if mod(k,30)0fprintf(t%5.1fs | LIMO(%.2f,%.2f) rho%.2f beta%4.0f deg | dobs%.2f\n,...t, x1, y1, q(4), rad2deg(q(6)), dobs);endpause(max(0, T - toc(tloop)));endcatch MEfprintf(2,ERRO no loop: %s\n, ME.message);end%% ------------------------------------------------------------------ %%%% 5) ENCERRAMENTO SEGURO: para o LIMO e POUSA o drone %%%% ------------------------------------------------------------------ %%fprintf(Encerrando: parando LIMO e pousando o Bebop...\n);msg_L.Linear.X 0;msg_L.Linear.Y 0;msg_L.Linear.Z 0;msg_L.Angular.Z 0;send(pub_L,msg_L);% Drone: zera o cmd_vel (teste/voo) e pousa apenas se estava em voo.if ~strcmp(MODO_BEBOP,off)msg_B.Linear.X 0; msg_B.Linear.Y 0; msg_B.Linear.Z 0; msg_B.Angular.Z 0;send(pub_B,msg_B);endif strcmp(MODO_BEBOP,voo)send(pub_LD,msg_LD); % LANDendpause(0.5);rosshutdown;fprintf(Finalizado.\n);%% ------------------------------------------------------------------ %%%% 6) GRAFICOS (usa apenas os dados efetivamente coletados) %%%% ------------------------------------------------------------------ %%if kf 1idx 1:kf;HtH.t(idx); HqH.q(:,idx); HqdH.qd(:,idx);Hp1H.p1(:,idx); Hp2H.p2(:,idx); HdobsH.dobs(idx);figure(Name,Trajetorias XY,Color,w); hold on; axis equal; grid on;td linspace(0,max(Ht),600);if TRAJ 1plot(0.75*sin(2*pi*td/40), 0.75*sin(4*pi*td/40),k--,DisplayName,Lemniscata desejada);elseplot(0,0,k,MarkerSize,12,LineWidth,1.5,DisplayName,Alvo (origem));endplot(Hp1(1,:),Hp1(2,:),b,LineWidth,1.5,DisplayName,LIMO);plot(Hp2(1,:),Hp2(2,:),r,LineWidth,1.2,DisplayName,Bebop (proj. XY));plot(Hp1(1,1),Hp1(2,1),bo,MarkerFaceColor,b,DisplayName,LIMO inicio);desenhar_circulo(xo,yo,Robs,k-);desenhar_circulo(xo,yo,Rinf,k:);xlabel(x [m]); ylabel(y [m]); title(Formacao seguindo a lemniscata desvio);legend(Location,bestoutside);rot {x_f [m],y_f [m],z_f [m],\rho_f [m],\alpha_f [rad],\beta_f [rad]};figure(Name,Variaveis da formacao,Color,w);for i1:6subplot(3,2,i); hold on; grid on;plot(Ht,Hqd(i,:),k--,LineWidth,1.2);plot(Ht,Hq(i,:),b,LineWidth,1.2);ylabel(rot{i}); if i5, xlabel(t [s]); endif i1, legend(desejado,real,Location,best); endendsgtitle(Variaveis da formacao: desejado (--) x real (—));figure(Name,Distancia ao obstaculo,Color,w); hold on; grid on;plot(Ht,Hdobs,b,LineWidth,1.4);yline(Rinf,k:,zona de influencia); yline(Robs,r--,raio do obstaculo);xlabel(t [s]); ylabel(distancia LIMO-obstaculo [m]);title(Distancia do LIMO ao obstaculo);end%% %%%% FUNCOES LOCAIS %%%% %%function [x,y,z,psi] ler_pose(sub)p sub.LatestMessage;quat [p.Pose.Orientation.W p.Pose.Orientation.X ...p.Pose.Orientation.Y p.Pose.Orientation.Z];eul quat2eul(quat); % [yaw pitch roll] (sequencia ZYX)x p.Pose.Position.X;y p.Pose.Position.Y;z p.Pose.Position.Z;psi eul(1); % yawendfunction vw estimar_vel(pos, pos_ant, T)vw (pos - pos_ant)/T;endfunction y saturar(u, umax)y max(min(u, umax), -umax);endfunction ang wrap_pi(ang)ang atan2(sin(ang), cos(ang));endfunction desenhar_circulo(xc,yc,r,estilo)th linspace(0,2*pi,100);plot(xcr*cos(th), ycr*sin(th), estilo, HandleVisibility,off);end 参考文献