# ServoArm

**Repository Path**: Lrc2021/servo-arm

## Basic Information

- **Project Name**: ServoArm
- **Description**: 机器人课设作业，舵机机械臂
- **Primary Language**: Unknown
- **License**: Not specified
- **Default Branch**: master
- **Homepage**: None
- **GVP Project**: No

## Statistics

- **Stars**: 0
- **Forks**: 0
- **Created**: 2023-10-28
- **Last Updated**: 2024-09-11

## Categories & Tags

**Categories**: Uncategorized

**Tags**: None

## README

# 机器人学课设

> 该项目目前实现功能：
> 1. 基于三陀螺仪的体感控制，对机械臂末端（夹爪）进行引导
> 2. 基于视觉识别手臂姿态，对机械臂末端（夹爪）进行引导
> 3. 语音识别交互控制
> 4. 上位机仿真控制
> 5. 使用手机蓝牙app进行总体的基本交互控制
> 具体效果视频见仓库Video文件夹


<div align = center><img src= "resource/体感控制1.jpg"width="600"></div>
<div align = center>
    <img src= "resource/体感控制2.jpg"width="300"alt="image1">
    <img src= "resource/体感控制3.jpg"width="300"alt="image2">
</div>



### 舵机臂本体
<div align = center><img src= "resource/舵机臂本体.jpg"width="200"></div>

### 建模草图
<div align = center><img src= "resource/建模草图.jpg"width="600"></div>

DH参数：

```
Servo_Arm = 
 
noname:: 5 axis, RRRRR, modDH, slowRNE                           
+---+-----------+-----------+-----------+-----------+-----------+
| j |     theta |         d |         a |     alpha |    offset |
+---+-----------+-----------+-----------+-----------+-----------+
|  1|         q1|          0|          0|          0|          0|
|  2|         q2|          0|       0.01|       PI/2|          0|
|  3|         q3|          0|      0.105|          0|          0|
|  4|         q4|          0|      0.098|          0|          0|
|  5|         q5|         15|      0.160|      -PI/2|          0|
+---+-----------+-----------+-----------+-----------+-----------+
 
```

### Matlab建模

##### PeterCorke Robotic Toolbox
代码见MatlabCode/ServoArm_P_C_video.m
``` matlab
% % DH参数
% Modified DH = [theta_i d_i a_i-1 alpha_i-1] 
L1 = Link([0 0 0 0], 'modified');
L2 = Link([0 0 1 pi/2], 'modified');
L3 = Link([0 0 10.5 0], 'modified');
L4 = Link([0 0 9.8 0], 'modified');
L5 = Link([0 0 16 -pi/2], 'modified');

% % 建立连杆模型
Servo_Arm = SerialLink([L1 L2 L3 L4 L5]);

% % 初始化关节变量
q0 = [0 pi/4 -pi/2 pi/4 0];

% % 画模型
figure
Servo_Arm.teach(q0);
Servo_Arm.display()
```matlab
<div align = center><img src= "resource/Model.P.C.png"width="600"></div>

##### Matlab Robotic System Tool
代码见MatlabCode/ServoArm_SYS_DH.m
```matlab
%% 改进型DH参数：A alpha d theta
dhparams = [0   	0   	0.08   	0;
            0.01	pi/2   0       0;
            0.105	0   	0   	0;
            0.098  	0   	0   	0;
            0.16    -pi/2       0       0];

%% 建立刚体模型
collisions = cell(5,1);
collisions{1} = collisionCylinder(0.06, 0.08);
collisions{1}.Pose = trvec2tform([0 0 -0.04]);
collisions{2} = collisionBox(0.105, 0.025, 0.055);
collisions{2}.Pose = trvec2tform([0.05 0 0]);
collisions{3} = collisionBox(0.10, 0.025, 0.055);
collisions{3}.Pose = trvec2tform([0.05 0 0]);
collisions{4} = collisionBox(0.18, 0.055, 0.025);
collisions{4}.Pose = trvec2tform([0.09 0 0])
collisions{5} = collisionBox(0, 0, 0);
collisions{5}.Pose = trvec2tform([0 0 0])

%% 建立机械臂模型
robot = rigidBodyTree;
bodies = cell(5,1);
joints = cell(5,1);
for i = 1:5
    bodies{i} = rigidBody(['body' num2str(i)]);
    joints{i} = rigidBodyJoint(['jnt' num2str(i)],"revolute");
    setFixedTransform(joints{i},dhparams(i,:),"mdh");
    bodies{i}.Joint = joints{i};
    if i == 1 % Add first body to base
        addBody(robot,bodies{i},"base");
    else % Add current body to previous body by name
        addBody(robot,bodies{i},bodies{i-1}.Name);
    end
    addCollision(robot.Bodies{i},collisions{i});
end
showdetails(robot)

%% 画机械臂
figure(Name="ServoArm Model")
config = homeConfiguration(robot);
config(1).JointPosition = 0;
config(2).JointPosition = pi/4;
config(3).JointPosition = -pi/2;
config(4).JointPosition = pi/4;
show(robot, config, Collisions="on",Visuals="off");
```

<div align = center><img src= "resource/Model.SYS.png"width="600"></div>


### 正逆运动学推算
##### 手推

<div align = center><img src= "resource/FKIK.jpg"width="600"></div>

##### matlab校对 
代码详见MatlabCode/ServoArm_FKIK_Solve.mlx

```matlab
%% 舵机臂正逆运动学解算公式推导

%% 正运动学公式推导
syms o a r d
o = sym('o',[5,1]);
% a = sym('a',[5,1]);
r = sym('r',[3,3]);
d = sym('d',[3,1]);

% 验证公式用，配合工具箱正运动学求解函数验证
% o = [0 -3*pi/4 -pi/4 pi/4 0];
a = [0.01,0.105,0.098,0.16,0];

T1 = [cos(o(1)),    -sin(o(1)), 0,          0               ;
      sin(o(1)),    cos(o(1)),  0,          0               ;
      0,            0,          1,          0               ;
      0,            0,          0,          1               ];

T2 = [cos(o(2)),    -sin(o(2)), 0,          a(1)            ;
      0,            0,          1,          0               ;
      sin(o(2)),    cos(o(2)),  0,          0               ;
      0,            0,          0,          1               ];

T3 = [cos(o(3)),    -sin(o(3)), 0,          a(2)            ;
      sin(o(3)),    cos(o(3)),  0,          0               ;
      0,            0,          1,          0               ;
      0,            0,          0,          1               ];

T4 = [cos(o(4)),    -sin(o(4)), 0,          a(3)            ;
      sin(o(4)),    cos(o(4)),  0,          0               ;
      0,            0,          1,          0               ;
      0,            0,          0,          1               ];
T5 = [1,0,0,a(4);
      0,0,1,0;
      0,1,0,0;
      0,0,0,1];
T = T1*T2*T3*T4*T5;
T_ = [r(1,1),r(1,2),r(1,3),d(1);
      r(2,1),r(2,2),r(2,3),d(2);
      r(3,1),r(3,2),r(3,3),d(3);
      0,     0,     0,     1;];
equ = (T==T_);
ans = solve(equ,[r,d])
% 验证公式用，配合工具箱正运动学求解函数验证
% ans_double = structfun(@double,ans)


%% 逆运动学公式推导
syms x y z 

% x = (a(1)+a(2)*cos(o(2))+a(3)*cos(o(2)+o(3))+a(4)*cos(o(2)+o(3)+o(4)))*cos(o(1))
% y = x*tan(o(1))
% z = -(a(2)*sin(o(2))+a(3)*sin(o(2)+o(3))+a(4)*sin(o(2)+o(3)+o(4)))

equ2 = -a(2)*sin(o(2))-a(3)*sin(o(2)+o(3)) == z
ans2 = solve(equ2,o(3))

equ3 = a(2)*cos(o(2))+a(3)*cos(o(2)+o(3)) == x-a(1)-a(4)
ans3 = solve(equ3,o(3))

equ4 = [-a(2)*sin(o(2))-a(3)*sin(o(2)+o(3)) == z,
        a(2)*cos(o(2))+a(3)*cos(o(2)+o(3)) == x-a(1)-a(4)]
ans4 = solve(equ4,[o(2),o(3)],'ReturnConditions',true)
```

##### matlab FK/IK代码
详见 MatlabCode/myfkine.m 、 fkine_ServoArm.m 、 ikine_ServoArm.m
```matlab
function T = myfkine(Q,dh)
    T = eye(4);
    t = eye(4);
    for i = 1:length(Q)
        ct = cos(Q(i));
        st = sin(Q(i));
        a = dh.a(i);
        ca = cos(dh.alpha(i));
        sa = sin(dh.alpha(i));
        d = dh.d(i);
   
        t(1,1) = ct;
		t(1,2) = -st;
		t(1,3) = 0;
		t(1,4) = a;
		t(2,1) = st*ca;
		t(2,2) = ct*ca;
		t(2,3) = -sa;
		t(2,4) = -sa*d;
		t(3,1) = st*sa;
		t(3,2) = ct*sa;
		t(3,3) = ca;
		t(3,4) = ca*d;
		t(4,1) = 0;
		t(4,2) = 0;
		t(4,3) = 0;
		t(4,4) = 1;
        % disp(t);
		T = T*t;
    end
end
```
```matlab
function T = fkine_ServoArm(Q,dh)
    a = dh.a;
    c1 = cos(Q(1));
    s1 = sin(Q(1));
    c2 = cos(Q(2));
    s2 = sin(Q(2));
    c23 = cos(Q(2)+Q(3));
    s23 = sin(Q(2)+Q(3));
    c234 = cos(Q(2)+Q(3)+Q(4));
    s234 = sin(Q(2)+Q(3)+Q(4));
    
    T = eye(4);
    T(1,1) = c1*c234;
    T(1,2) = -s1;
    T(1,3) = -c1*s234;
    T(1,4) = c1*(a(2)+a(3)*c2+a(4)*c23+a(5)*c234);
    T(2,1) = s1*c234;
    T(2,2) = c1;
    T(2,3) = -s1*s234;
    T(2,4) = s1*(a(2)+a(3)*c2+a(4)*c23+a(5)*c234);
    T(3,1) = s234;
    T(3,2) = 0;
    T(3,3) = c234;
    T(3,4) = a(3)*s2+a(4)*s23+a(5)*s234;
end
```
```matlab
function q = ikine_ServoArm(T,dh)
    r = tform2rotm(T);  p = tform2trvec(T);
    a1 = dh.a(2);  a2 = dh.a(3);  a3 = dh.a(4);  a4 = dh.a(5);

% 求q1
    q1 = acos(p(1)/sqrt(p(1)*p(1)+p(2)*p(2)));
    if(p(2)<0)
        q1 = q1+pi;
    end
    if(p(2)==0&&p(1)>0)
        q1 = 0;
    end
    if(p(2)==0&&p(1)<0)
        q1 = PI;
    end

% 求q3
    c1 = cos(q1); c234 = r(3,3); s234 = r(3,1); 
    m = (p(1)/c1) - a4*c234 - a1;
    n = p(3) - a4*s234;
    q3_1 = acos((m*m+n*n-a2*a2-a3*a3)/(2*a2*a3));
    q3_2 = -q3_1;
% 求q2
    c3 = cos(q3_1);  s3 = sin(q3_1);
    q2_1 = acos(((a2+a3*c3)*m + a3*s3*n)/((a2+a3*c3)^2+(a3*s3)^2));
    c3 = cos(q3_2);  s3 = sin(q3_2);
    q2_2 = acos(((a2+a3*c3)*m + a3*s3*n)/((a2+a3*c3)^2+(a3*s3)^2));
% 求q4
    q4_1 = asin(s234)-q2_1-q3_1;
    q4_2 = asin(s234)-q2_2-q3_2;

    q = [q1, q2_1, q3_1, q4_1, 0;
         q1, q2_2, q3_2, q4_2, 0];
end
```
##### C++FKIK代码
```C++
/**
 * @brief 递推求正运动学解
 *
 * @param q 关节矢量
 * @return matrix::SquareMatrix<float, 4> 末端位姿齐次变换矩阵
 */
matrix::SquareMatrix<float, 4> &ServoArm_c::fkine_num(const float* q) {
    SquareMatrix<float, 4> T = eye<float, 4>();
    for (uint8_t i = 0; i < n; i++)
    {
        T *= dh[i].T(q[i]);
    }
    return T;
}

/**
 * @brief 公式法直接求正运动学解
 *
 * @param q 关节矢量
 * @return matrix::SquareMatrix<float, 4> 末端位姿齐次变换矩阵
 */
matrix::SquareMatrix<float, 4> &ServoArm_c::fkine_ana(const float* q) {
    volatile float c1 = cos(q[0]), s1 = sin(q[0]);
    volatile float c2 = cos(q[1]), s2 = sin(q[1]);
    volatile float c23 = cos(q[1] + q[2]), s23 = sin(q[1] + q[2]);
    volatile float c234 = cos(q[1] + q[2] + q[3]), s234 = sin(q[1] + q[2] + q[3]);
    volatile float a1 = dh[1].a, a2 = dh[2].a, a3 = dh[3].a, a4 = dh[4].a;

    SquareMatrix<float, 4> T = eye<float, 4>();
    T(0, 0) = c1 * c234;
    T(0, 1) = -s1;
    T(0, 2) = -c1 * s234;
    T(0, 3) = c1 * (a1 + a2 * c2 + a3 * c23 + a4 * c234);
    T(1, 0) = s1 * c234;
    T(1, 1) = c1;
    T(1, 2) = -s1 * s234;
    T(1, 3) = T(0, 3) * s1 / c1;
    T(2, 0) = s234;
    T(2, 1) = 0;
    T(2, 2) = c234;
    T(2, 3) = a2 * s2 + a3 * s23 + a4 * s234;

    return T;
}

/**
 * @brief 求逆运动学解析解，公式法直接求解
 *
 * @param T 末端位姿齐次变换矩阵
 * @param q 第一个关节解
 * @param q_  第二个关节解
 */
void ServoArm_c::ikine_ana(const matrix::SquareMatrix<float, 4>& T, float* q, float* q_) {
    volatile float a1 = dh[1].a, a2 = dh[2].a, a3 = dh[3].a, a4 = dh[4].a;
    volatile float px = T(0, 3), py = T(1, 3), pz = T(2, 3);

    /*q1*/
    volatile float am = sqrt(px * px + py * py);
    if (am < 1e-3f) return;
    volatile float q1 = acos(px / sqrt(px * px + py * py));
    if (py < 0) q1 += PI;
    if (py == 0 && px > 0) q1 = 0;
    if (py == 0 && px < 0) q1 = PI;

    /*q3 两个解*/
    volatile float c1 = cos(q1), s234 = T(2, 0), c234 = T(2, 2);
    volatile float m = (px / c1) - a4 * c234 - a1;
    volatile float n = pz - a4 * s234;
    volatile float q3_1 = acos((m * m + n * n - a2 * a2 - a3 * a3) / (2 * a2 * a3));
    volatile float q3_2 = -q3_1;

    /*q2 两个解*/
    volatile float c3 = cos(q3_1), s3 = sin(q3_1);
    volatile float q2_1 = acos(((a2 + a3 * c3) * m + a3 * s3 * n) / ((a2 + a3 * c3) * (a2 + a3 * c3) + (a3 * s3) * (a3 * s3)));
    c3 = cos(q3_2), s3 = sin(q3_2);
    volatile float q2_2 = acos(((a2 + a3 * c3) * m + a3 * s3 * n) / ((a2 + a3 * c3) * (a2 + a3 * c3) + (a3 * s3) * (a3 * s3)));

    /*q4 两个解*/
    volatile float q4_1 = asin(s234) - q2_1 - q3_1;
    volatile float q4_2 = asin(s234) - q2_2 - q3_2;

    /*最终两个解*/
    q[0] = q1, q[1] = q2_1, q[2] = q3_1, q[3] = q4_1, q[4] = 0;
    q_[0] = q1, q_[1] = q2_2, q_[2] = q3_2, q_[3] = q4_2, q_[4] = 0;
}
```

### 直线轨迹生成
这里只做了定姿态下的直线轨迹生成，即保持夹爪在欧拉空间中的姿态不变，直线运动
具体思路见下图：

<div align = center><img src= "resource/LineTraj_Idea.jpg"width="600"></div>

##### matlab仿真代码
```matlab
% % 定姿态的直线轨迹规划
qs = [0, 3*pi/8, -3*pi/4, 3*pi/8, 0];
Ts = fkine_ServoArm(qs,dh); 
Te = Ts;
Te(1,4) = Te(1,4)+5;
Te(2,4) = Te(2,4)+5;
Te(3,4) = Te(3,4)+5;
Q = mytraj(qs,Te,10,0.5,pi/90,dh);

% % 生成笛卡尔空间的轨迹
step = 10;
tInterval = [0 step];
tSamples = 0:step;
[T,vel,acc] = transformtraj(Ts,Te,tInterval,tSamples);
Ptraj = squeeze(T(1:3,4,:)) % 提取齐次变换矩阵的位置矢量
% % 画轨迹
figure
hold on;
title('p0 到 pf 直线轨迹');grid on;
zlim([-30,30]);
plot3(Ptraj(1, :), Ptraj(2, :), Ptraj(3, :), '-o', 'LineWidth', 1)

% % 画运动动画
Servo_Arm.plot(Q);


% % 逆解有效性判断函数
function flag = check(x,y,z,q1,q4,a2,a3,a4)
    am = (a2+a3)^2+a4^2-2*(a2+a3)*a4*cos(pi-abs(q4));
    if x*x+y*y > am
        flag = 0;
    else (x/cos(q1))^2+z^2 > am
        flag = 0;
    end
    flag = 1;
end

% 直线轨迹生成函数，传入的参数有： 
% 初始关节位形，终点齐次变换矩阵，插值点个数，欧式空间最大位移，关节最大变化量
function Q = mytraj(qs,Te,dt,dm,dmq,dh)
    Ts = fkine_ServoArm(qs,dh);
    psx = Ts(1,4); psy = Ts(2,4); psz = Ts(3,4); 
    pex = Te(1,4); pey = Te(2,4); pez = Te(3,4);

    a2 = dh.a(3);  a3 = dh.a(4);  a4 = dh.a(5);

    % 计算应生成的欧拉空间轨迹点数量
    dpx = (pex-psx)/dt; dpy = (pey-psy)/dt; dpz = (pez-psz)/dt;
    maxd = max([dpx,dpy,dpz]);
    if(maxd>dm)
        k = dm/maxd; dpx = dpx*k; dpy = dpy*k; dpz = dpz*k;
        maxd = dm;
    end
    cnt = max([pex-psx,pey-psy,pez-psz])/maxd;

    q = qs;  T = Ts; Q = [q];
    px = psx; py = psy; pz = psz;
    for i = 1:cnt   %先欧拉空间上生成cnt个轨迹点
       % 迭代生成的下一个目标位置
       px = px + dpx;
       py = py + dpy;
       pz = pz + dpz;
       % 判断目标位置是否可达(即逆解是否存在)
       if check(px,py,pz,q(1),q(4),a2,a3,a4) == 0
           return
       end
       % 对于生成的下一个目标位姿，逆解出关节目标
       T(1,4) = px; T(2,4) = py; T(3,4) = pz;
       qq = ikine_ServoArm(T,dh);
       % 选取关节变化最小的目标
       dq_1 = qq(1,:) - q;
       dq_2 = qq(2,:) - q;
       if max(abs(dq_1))>max(abs(dq_2))
           dq = dq_2; q_=qq(2,:);
       else
           dq = dq_1; q_=qq(1,:);
       end
       % 若关节变化量超过限制，再插补生成几个轨迹点
       if max(dq)>dmq
           dq = dq*(dmq / max(dq));
       end
       % 记录轨迹点
       for j = q(1):dq(1):q_(1)
           q = q + dq;
           Q = [Q;q];
       end

    end
end
```

仿真效果图：

<div align = center><img src= "resource/LineTraj_Matlab.png"width="600"></div>


##### C++ 实现代码：
这一部分代码详见：Stm32Code, 即机械臂本体的主控代码
```C++
/**
 * @brief 检查固定末端姿态下（θ2+θ3+θ4恒定），末端是否能达到目标位置x,y,z(欧拉空间下)
 *
 * @param x,y,z 目标位置x,y,z坐标
 * @param q1 θ1，数组中应为q[0]
 * @param q4 θ4，数组中应为q[3]
 * @return true 可以到达
 * @return false 无法到达
 */
bool ServoArm_c::checkXYZaccessibility(float x, float y, float z, float q1, float q4) {
    volatile float a2 = dh[2].a, a3 = dh[3].a, a4 = dh[4].a;
    volatile float am = (a2 + a3) * (a2 + a3) + a4 * a4 - 2 * (a2 + a3) * a4 * cos(PI - abs(q4));
	volatile float xx = x*x, yy = y*y, zz = z*z;
    if (xx + yy + zz > am )return 0;
    return 1;
}

/**
 * @brief 欧拉空间长距离直线运动的轨迹生成
 *
 * @param qs 初始位形（初始关节角矢量）
 * @param Te 目标位姿齐次变换矩阵
 * @param max_dp 最大欧拉空间位移量
 * @param max_dq 最大关节变化量
 * @return std::vector<float*> 轨迹点（关节矢量集合）
 */
std::vector<float*>& ServoArm_c::lineTraj(const float* qs, const matrix::SquareMatrix<float, 4>& Te, float max_dp, float max_dq) {
    std::vector<float*> traj;
    matrix::SquareMatrix<float, 4> Ts = fkine_num(qs);

    volatile float psx = Ts(0, 3), psy = Ts(1, 3), psz = Ts(2, 3);
    volatile float pex = Te(0, 3), pey = Te(1, 3), pez = Te(2, 3);
    volatile float a2 = dh[2].a, a3 = dh[3].a, a4 = dh[4].a;

    // 计算应生成的欧拉空间轨迹点数量cnt
    volatile float dpx = pex - psx, dpy = pey - psy, dpz = pez - psz;
    volatile int cnt = std::max(abs(dpx), std::max(abs(dpy), abs(dpz))) / max_dp;   // 四舍五入取最大迭代次数
    dpx /= cnt; dpy /= cnt; dpz /= cnt;

    volatile float px = psx, py = psy, pz = psz;
    float* q = new float[n];
    float* qq = new float[n];
    float* qq_ = new float[n];
    float* dq = new float[n];
    memcpy(q, qs, n * sizeof(float));
    matrix::SquareMatrix<float, 4> T = Ts;

    for (int i = 0; i < cnt; i++)    // 先欧拉空间上生成cnt个轨迹点
    {
        // 迭代生成的下一个目标位置
        px = px + dpx;
        py = py + dpy;
        pz = pz + dpz;
        // 判断目标位置是否可达(即逆解是否存在)
        if (checkXYZaccessibility(px, py, pz, q[0], q[3]) == 0) break;

        // 对于生成的下一个目标位姿，逆解出关节目标
        T(0, 3) = px, T(1, 3) = py, T(2, 3) = pz;
        ikine_ana(T, qq, qq_);

        // 选取关节变化最小的目标
        float max_dq_1 = 0.0f, max_dq_2;
        for (int j = 0;j < n;j++)
            max_dq_1 = std::max(abs(qq[j] - q[j]), max_dq_1),
            max_dq_2 = std::max(abs(qq_[j] - q[j]), max_dq_2);
        int cntq = 1;
        if (max_dq_1 > max_dq_2)
        {
            if (max_dq_2 > max_dq)cntq = ceil(max_dq_2 / max_dq); // 若关节变化量大于约束值，则再插补cntq个轨迹点（向上取整）
            for (int j = 0;j < n;j++) dq[j] = (qq_[j] - q[j]) / cntq;
        }
        else if (max_dq_1 < max_dq_2)
        {
            if (max_dq_1 > max_dq)cntq = ceil(max_dq_1 / max_dq); // 若关节变化量大于约束值，则再插补cntq个轨迹点（向上取整）
            for (int j = 0;j < n;j++) dq[j] = (qq[j] - q[j]) / cntq;
        }
        // 记录轨迹点
        for (int j = 0;j < cntq;j++)
        {
            for (int k = 0;k < n;k++) q[k] += dq[k];
            float* q_ans = new float[n];
            memcpy(q_ans, q, n * sizeof(float));
            traj.push_back(q_ans);
        }
    }
    delete[] q;
	delete[] qq;
	delete[] qq_;
	delete[] dq; // 回收内存，注意，q_ans中new出来的大量空间要在下一次调用轨迹生成函数时释放，通过vector traj一并释放
	return traj;
}

/*
new创建出来的内存空间是不会被销毁的，因为它是在堆上创建的。而指针变量是会被销毁的，因为它是在栈上创建的。

这是一种常见的内存泄露的情况。尽量避免手动管理指针，采用c++11中的shared_ptr和unique_ptr来管理。待学

手动释放vector中保存的指针指向的空间：
for (int i = 0; i < traj.size(); ++i)
{
    delete traj[i];
    traj[i] = NULL;
}
MyTestMap.clear();
MyTestMap.shrink_to_fit();
*/
```

注：实际代码在单片机的freeRTOS的环境上跑,作了简化处理：
```C++
matrix::SquareMatrix<float, 4> T;
float q_debug[5], q_debug_[5];
float qq[5], qq_[5];
float cnt, dx, dy, dz, max_d;
float dq = 0.0f, dq_ = 0.0f;
void tskMov(void* arg)
{
	/*	pre load for task	*/
	TickType_t xLastWakeTime_t;
	xLastWakeTime_t = xTaskGetTickCount();

	T = ServoArm.fkine_ana(q);
	cx = tx = T(0, 3); cy = ty = T(1, 3), cz = tz = T(2, 3);

	for (;;)
	{
		/* wait for next circle */
		vTaskDelayUntil(&xLastWakeTime_t, 10);

		if (Ctrl_Mode == 1 || Ctrl_Mode == 2) {
			tx = std_lib::constrain(tx, 0.0f, 0.40f);
			ty = std_lib::constrain(ty, 0.0f, 0.35f);
			tz = std_lib::constrain(tz, 0.0f, 0.80f);

			dx = tx - cx, dy = ty - cy, dz = tz - cz;
			max_d = std::max(abs(dx), std::max(abs(dy), abs(dz)));
			if (max_d > 0.01f)
			{
				cnt = 0.01f / max_d;
				dx *= cnt, dy *= cnt, dz *= cnt;
			}
			cx += dx, cy += dy, cz += dz;

			if (ServoArm.checkXYZaccessibility(cx, cy, cz, q[0], q[3]) == 1)
			{
				T(0, 3) = cx, T(1, 3) = cy, T(2, 3) = cz;
				ServoArm.ikine_ana(T, qq, qq_);
				dq = 0.0f, dq_ = 0.0f;
				for (uint8_t i = 0; i < 5; i++)
					dq = std::max(dq, abs(qq[i] - q[i])),
					dq_ = std::max(dq_, abs(qq_[i] - q[i]));
				if (dq < dq_)
					for (uint8_t i = 0; i < 5; i++) q[i] = qq[i];
				else
					for (uint8_t i = 0; i < 5; i++) q[i] = qq_[i];
			}
			else
			{
				cx -= dx, cy -= dy, cz -= dz;
				tx = cx, ty = cy, tz = cz;
			}

			for (uint8_t i = 0; i < 4; i++) Joint_Target[i] = q[i];
			// 加重力补偿
			float bias = 1.0 * (sqrt(cx * cx + cy * cy) - 0.25);
			Joint_Target[1] += 0.8 * bias;
			Joint_Target[2] += 0.8 * bias;
			Joint_Target[3] += 0.6 * bias;
		}
	}
}
```



## 体感控制
基本思路：

<div align = left><img src= "resource/体感控制idea.jpg"width="600"></div>

实际用到了三块陀螺仪，如开头图所示，通过测量手臂各部位转动角度，正运动学计算手指末端位置和姿态，同时，多余的陀螺仪自由度用于其他控制，比如夹爪开合等

### 核心C++代码
```C++
x = (a1 * sin(-e1.alpha) + a2 * cos(-e2.alpha) + a3 * cos(-e3.alpha)) * cos(e2.gamma);
y = (a1 * sin(-e1.alpha) + a2 * cos(-e2.alpha) + a3 * cos(-e3.alpha)) * sin(e2.gamma);
z = a1 - a1 * cos(-e1.alpha) + a2 * sin(-e2.alpha) + a3 * sin(-e3.alpha);
yaw = e2.gamma;
pitch = -e3.alpha;
roll = -e3.beta;
gripper = Degrees(e3.gamma - e2.gamma);
```

整份工程项目见：LPS_Test， 也是运行在stm32f405环境上

## 视觉手臂姿态识别的引导控制
视觉识别手臂姿态，计算手的姿态和位置，从而引导夹爪姿态和位置

工程项目见：PythonCode

## 语音识别交互控制
语音识别PC端发送的语音信号，并将识别后对应的操作发送到STM32，从而控制机械臂。
指令有“上”、“下”、“左”、“右”、“前”、“后”、“夹取”和 “松”八种

工程项目见YuyinCode

## 上位机仿真控制


## 手机蓝牙所用APP
轮趣科技开发的app，安卓安装包也放在仓库中了，即：resource\minibalance_v5.7(使用这个).apk，下载到手机安装即可用使用

连接蓝牙：

<div align = center><img src= "resource/bluetooth_connect.jpg"width="200"></div>

控制模式切换：

<div align = center><img src= "resource/切换控制模式.jpg"width="200"></div>

按键控制：手机app交互，切换到首页页面即可控制臂前进后退等

<div align = center><img src= "resource/app.jpg"width="200"></div>

重置：臂回到初始位置，控制模式变为按键控制

体感控制：可进行外部的体感控制、视觉手势识别控制、语音交互控制、上位机仿真控制等

关节控制：可通过上边滑动条对每个关节单独控制

## 关于板子所用串口
uart6 给蓝牙模块，连接手机蓝牙app作总控制与模式切换
uart3 给体感控制器、视觉控制器、语音控制器、上位机控制器，具体通信协议以及相关使用方式见：通信协议.md