Matlab Robotics project 1 phase 1 ＜Experiment Recording＞

最新推荐文章于 2022-01-30 01:21:24 发布

爱安敝之

最新推荐文章于 2022-01-30 01:21:24 发布

阅读量674

点赞数 2

分类专栏：学习机器人 Matrix 文章标签： matlab 1024程序员节

本文链接：https://blog.csdn.net/libizhide/article/details/120839909

版权

学习同时被 3 个专栏收录

21 篇文章 2 订阅

订阅专栏

机器人

14 篇文章 4 订阅

订阅专栏

Matrix

4 篇文章 0 订阅

订阅专栏

Preface:This article is written for a nifty girl who I cherish.

在这里插入图片描述

(0)Introduction

(0.0)General Description

本实验是机器人学基于matlab 的四旋翼飞行器仿真实验，要求根据已有的飞行器模型创建控制器（输入期望控制，输出飞行器整体推力与力矩），根据创建的飞行器在空间中绕曲线飞行

(0.1)Original Materials

Free Resource

(1)On working

(1.0)Design controller

Based on quadrotor dynamics:

function [F, M] = controller(t, s, s_des)
global params

m = params.mass;
g = params.grav;
I = params.I;

% s(1:3) current position
% s(4:6) current velocity
% s(7:10) current attitude quaternion
% s(11:13) current body angular velocity

% s_des(1:3) desire position
% s_des(4:6) desire velocity
% s_des(7:9) desire acceleration
% s_des(10) desire yaw
% s_des(11) desire yaw rate
% F = 1.0; M = [0.0, 0.0, 0.0]'; % You should calculate the output F and M
%PD parameters
s=s';
s_des=s_des';
kp_pos = [7 7 50];%[7 7 50]
kd_pos = [4 4 80];%[4 4 80]
kp_rpy = [1000 1000 100];%[1000 1000 100]
kd_rpy = [98 98 49];%[98 98 49]

%caculate position pid
p_c_acc=s_des(7:9)+kp_pos.*(s_des(1:3)-s(1:3))+kd_pos.*(s_des(4:6)-s(4:6));
%caculate Thrust F
F=m*(g+p_c_acc(3));

%Trnasform quaternion to attitude angle 
R = quaternion_to_R(s(7:10));
[phi,theta,yaw] = RotToRPY_ZXY(R);
%caculate phi_c and theta_c
phi_c=1/g*(p_c_acc(1)*sin(yaw)-p_c_acc(2)*cos(yaw));
theta_c=1/g*(p_c_acc(1)*cos(yaw)+p_c_acc(2)*sin(yaw));
%caculate angular accelerate
angular_delta=[phi_c,theta_c,s_des(10)]-[phi,theta,yaw];
angular_acc_c=[0,0,s_des(11)];
angular_delta(3)=mod(angular_delta(3)+pi,2*pi)-pi;
%caculate moment
ang_acc=kp_rpy.*angular_delta+kd_rpy.*(angular_acc_c-s(11:13));
M=I*(ang_acc')+cross(s(11:13)',I*(s(11:13)'));
end

(1.1)Controller designing - Draw a heart

Only give position

function s_des = unnamed_trajectory(t, true_s)
    s_des = zeros(11,1);
    x=(16*sin(t/25*2*pi)^3)/4;
    y=(13*cos(t/25*2*pi)-5*cos(2*t/25*2*pi)-2*cos(3*t/25*2*pi)-cos(4*t/25*2*pi))/4;
    s_des(1)=x;
    s_des(2)=y;
end

可见控制具有一定的延时，是因为没有前馈的值

Give position and velosity

function s_des = unnamed_trajectory(t, true_s)
    s_des = zeros(11,1);
    T=t;
    x=(16*sin(T/25*2*pi)^3)/4;
    y=(13*cos(T/25*2*pi)-5*cos(2*T/25*2*pi)-2*cos(3*T/25*2*pi)-cos(4*T/25*2*pi))/4;
    x_d=(24*pi*cos((2*pi*T)/25)*sin((2*pi*T)/25)^2)/25;
    y_d=(pi*sin((4*pi*T)/25))/5 - (13*pi*sin((2*pi*T)/25))/50 + (3*pi*sin((6*pi*T)/25))/25 + (2*pi*sin((8*pi*T)/25))/25;
    angle_d=y_d/x_d;
    yaw_angle=atan(angle_d)+(sign(x_d)==-1)*pi;
    s_des(1)=x;
    s_des(2)=y;
    s_des(4)=x_d;
    s_des(5)=y_d;
    s_des(10)=yaw_angle;
end

在这里插入图片描述

(2)Conclusion

(2.0)Intuition of this experiment

Intuition of formula
公式整体的推导较为复杂，涉及机器人运动学与动力学，且会解欧拉牛顿方程，但对公式的直观理解可以更好理解公式；这个公式基本是外环位置，内环姿态，计算扭矩与推力，可见推力与飞行器质量与z轴加速度有关，通过计算期望角度计算扭矩

在复杂的模型中，建立模型控制以及进行高阶控制可以更好地实现控制

(0.1)Mapping angle to (-pi,pi)

Rule: first aligning, the mod mapping ,the devide
$\theta_1=[(\theta+\pi)\mod 2\pi]-\pi$

(0.2)Direction angle of parameter equation

Reason: Derivative has two directions, one must follow the movement direction
$a n g l e = a r c t a n (d y / d x) + (s i g n (d x / d t) = = - 1) * p i$

(0.3)Comprehension of PD initinal parameters

Description: PD 中的 $P$ 即一个比例，我们想系统响应要快，那么P就要尽可能大，但不能超过系统的性能极限，如下图，系统在达到 $T_p$ 前都可以近似看作是一个线性的，线性的导数则是 $P$ ，那么在调试P值时可以带入的初值即是纵坐标除以横坐标 $t$ 的参数的最大值，加入纵坐标表示位置，那么就是系统可以达到的速度的最大值；对于 $D$ 值，通常用以稳定系统超调与震荡，可以用经验公式 $D = P * T / 8$ , $T$ 是震荡的周期
在一个系统中，如果是多输入多输出，就会有多个相互关联的PD控制器，PD的调参就只有模型化后导入simulink中进行；如果通过人工调节，最好的方法就是确定一个较为稳定可靠的初值