演示视频已上传至Bilibili视频平台：https://www.bilibili.com/video/BV1wUiRYxE8z

一、系统功能与整体架构设计

系统实现功能

（1）单片机在按键控制下，产生1kHz的正弦波或方波；

（2）单片机能够采集波形，并且显示；

（3）单片机能够分析采集波形的频谱，并且显示频谱与基波频率。

整体架构设计图

系统主页与按键对应功能简介

每次启动系统都会默认直接进入该主页面：

（1）蓝色部分的文字为系统名称与作者姓名，这会在后续的每个功能页面中都有显示；

（2）黑色部分的文字为各按键对应的功能介绍。

正如主页的功能介绍栏所示：

（1）按下KEY0：PA4引脚开始持续输出1kHz的正弦波信号，并在屏幕上实时显示从PA5引脚采集到的输入信号波形；

（2）按下KEY1：PA4引脚开始持续输出1kHz的方波信号，并在屏幕上实时显示从PA5引脚采集到的输入信号波形；

（3）按下KEY2：在屏幕上实时显示从PA5引脚采集到的输入信号的频谱分析结果（幅值谱，频率范围为0~1000Hz）；

（3）按下KEY3（KEY_UP）：在屏幕上实时显示从PA5引脚采集到的输入信号的频谱分析结果（幅值谱，频率范围为0~8000Hz）。

二、各部分功能实现

1、1kHz正弦波与方波的产生

模块功能架构设计

在实际单片机编程实现时，导入并调用DSP库加速信号数组（正弦波）的计算，并通过时钟TIM6（分频）控制DMA的数据搬运过程，并设置DAC数模转换将搬运后的信号数字数据在PA4引脚以模拟信号形式输出。

模块功能实现依据

为使用单片机产生指定频率的波形，需要根据上述架构设置对应的参数，基本的设置逻辑如下：

（1）首先，这里使用定时器TIM6来控制DMA搬移数据的过程，在CubeMX中已预先设置其时钟频率为84MHz；

（2）在生成信号数组时，C语言程序中设定数组长度为1024（与后续采集一致，为4的整数次幂以便于进行快速傅里叶变换FFT）；

（3）事实上，对于信号数组长度N、定时器频率fT与信号基波频率f而言存在如下关系式：f = fT / N，这意味着以输出基波频率f = 1kH的信号为例，经过时钟分频后的定时器频率fT是可以直接确定的，进而可以确定分频倍数（时钟频率/分频后定时器频率）。

经过计算，当分频倍数设置为82时（实际单片机控制程序中为两次分频，取第一次分频倍数为41、第二次分频倍数为2即二分频），输出的信号基波频率f约为1000（由于数组长度为1024，在分频倍数必须取整的情况下，基波频率无法精准等于1000Hz，实际约为1000.38Hz）。

在MATLAB中，可以编写简单的测试程序模拟这一过程：

1
2
3
4
5
6
7
8

TIM6_Frequency = 84000000; %DAC_DMA时钟TIM6频率
DAC_DMA_Divide1 = 41; %DAC_DMA时钟一次分频
DAC_DMA_Divide2 = 2; %DAC_DMA时钟二次分频
DAC_DMA_Frequency = TIM6_Frequency / (DAC_DMA_Divide1 * DAC_DMA_Divide2); %分频后时钟频率

N = 1024; %数组长度与采样点数

f = DAC_DMA_Frequency / N; %产生信号频率（期望值1000）

模块功能实现效果

启动系统后按压按键KEY_0启动正弦波生成，将示波器的通道正极与信号输出引脚PA4连接，示波器的通道负极与单片机的地GND连接，可在示波器上显示出如下波形：

可以看到输出的波形形状为标准的正弦波，输出电平范围为03.3V（对应生成的正弦信号数组振幅为2048、偏置为2047即数据点范围位于04095），均值为1.6V，且周期约为1kHz（示波器显示1.00045kHz；一个周期大致占据五格、每格代表200us即一个周期为1ms）。

按压按键KEY_1切换为生成方波，可在示波器上显示出如下波形：

可以看到输出的波形形状为标准的方波（占空比50%），输出低电平为0V、高电平为3.3V（对应生成的方波信号数组前一半值为0、后一半值为4095），均值为1.6V，且周期约为1kHz（示波器显示1.00043kHz；一个周期大致占据五格、每格代表200us即一个周期为1ms）。

2、波形信号的采集与显示

模块功能架构设计

在实际单片机编程实现时，通过定时器控制从PA5引脚读入模拟信号，通过ADC模数转换为数字数组并通过DMA搬运将其存入长度为1024（为4的整数次幂以便于进行快速傅里叶变换FFT）的数组中，存满一次数组即中断一次DMA搬运并将该数组数据（即采集波形）显示在显示屏上，短暂延迟（控制屏幕刷新速度合适）后进行新一轮的信号采集、搬运与波形显示。

模块功能实现依据

为使用单片机采集信号数据并以合适的形式将波形显示在显示屏上，需要根据上述架构设置对应的参数，基本的设置逻辑如下：

（1）首先，控制ADC1的定时器在CubeMX中已预先设置其时钟频率为84MHz，但根据相关手册与文档，硬件上对于分频后的ADC实际频率有限制，不能高于30MHz，在这样的条件下一般取四分频（仅分频一次，以对应结构体参数hadc1.Init.ClockPrescaler = ADC_CLOCK_SYNC_PCLK_DIV4实现），即分频后定时器频率为21MHz；

（2）其次，根据相关手册与文档，完成一次采样至少会花费12个时钟周期，为调控实际采样频率通常还可以设置额外的时钟周期（库函数限制只能为特定的几个值），即实际的采样频率Fs应为：分频后定时器频率（21MHz）/一次采样花费的时钟周期数（12+额外设置的时钟周期）；

（3）事实上，要想控制屏幕上显示的波形不过于松散/密集，需要控制一次采样（填满数组，DMA中断）内包含的信号周期数量，这可以通过将信号产生的定时器频率fT除以采样频率Fs得到；

（4）另一方面还需要注意为使得采集到的波形没有失真（频域混叠）现象，要求采样频率Fs与待采集波形频率f满足：Fs≥2f。

经过计算与测试，当额外设置的时钟周期设置为112时（sConfig.SamplingTime = ADC_SAMPLETIME_112CYCLES），一次采样中包含（屏幕上显示）的信号周期约为6，这样的显示效果较为合理；同时此时的采样频率Fs约为42683Hz，远大于待采集波形频率f = 1000Hz的两倍，不会发生频谱混叠。

在MATLAB中，可以编写简单的测试程序模拟这一过程（以正弦信号为例）：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24

ADC_Timer_Frequency = 84000000; %ADC时钟频率
%硬件限制：要求ADC实际时钟频率不能超过30MHz
ADC_Divide = 4; %取四分频，分完后达到21MHz满足要求
ADC_Frequency = ADC_Timer_Frequency / ADC_Divide; %分频后ADC时钟频率

%完成一次采样需要多个时钟周期
Collect1 = 12; %固定消耗12次循环，无法更改
Collect2 = 112; %可设置额外消耗循环数以调整采样频率
Fs = ADC_Frequency / (Collect1 + Collect2); %ADC采样频率

Cycle = DAC_DMA_Frequency / Fs; %一次采样采出多少个周期

A = 2047; %幅值
B = 2048; %直流偏置分量
t = 0 : 1 / Fs : (N - 1) / Fs;
x = A * sin(2 * pi * f * t) + B;

% 绘制原始信号
figure;
subplot(2,1,1);
plot(t, x);
title('正弦波信号');
xlabel('时间 (秒)');
ylabel('幅值');

运行该MATLAB程序，绘制出一次采样采集到的波形如下图所示：

模块功能实现效果

启动系统后按压按键KEY_0启动正弦波信号的生成与采集，将信号输出引脚PA4与信号输入引脚PA5连接，屏幕上显示采集波形效果如下：

按压按键KEY_1切换为生成方波信号并采集，屏幕上显示采集波形效果如下：

图上的横坐标单位为ms；可以看到屏幕上显示的即为6个周期的信号波形，这与MATLAB的模拟计算结果是完全一致的，且波形无失真。

3、采集波形信号的频谱分析

模块功能实现依据

在频谱分析与频谱图显示方面，有如下要点需要注意：

（1）首先，频谱分析依赖于对于信号的傅里叶变换，在数字信号层面对于离散的数据点则需要采用离散傅里叶变换，但这样的变换计算速度往往很感人，因此需要利用其快速算法，即快速傅里叶变换FFT，MATLAB可直接调用fft函数实现，单片机编程中在DSP库中也有相应的函数可以实现完全相同的过程，但要求信号数组的长度应为4的整数次幂，故先前均选取1024作为发生与采集信号的数组长度；

（2）其次，经过FFT变换后会得到一个长度相同（1024）的新数组，其中每一个数字的下标index对应的实际频率应为index*Fs/1024，这意味着如果直接将整个FFT变换结果数组作为频谱图显示到屏幕上，横坐标的跨度实际上为Fs≈42683Hz，为使得频谱图更加直观，需要限制绘制频谱图的频率范围，并对应控制绘制数组中的部分数据；

（3）事实上，FFT变换结果的数组中各数值并不是期望的对应频率的幅值，还需要除以数组长度1024（单片机程序中对于起始点只需要除以一半的数组长度即512）才可得到正确的幅值。

由于涉及到信号的基波频率检测以及方波的频谱分析，在KEY_2和KEY_UP按键分别设置了频谱频率范围为01000Hz与08000Hz两种模式。在MATLAB中，可以编写简单的测试程序模拟频谱分析过程并在0~8000Hz的频段上展示频谱：

1
2
3
4
5
6
7
8
9

f_range = linspace(0, Fs, N);%频域横坐标，注意奈奎斯特采样定理，最大原信号最大频率不超过采样频率的一半
xk = fft(x) / N; %用fft得出离散傅里叶变换

% 计算并绘制频谱
subplot(2,1,2);
plot(f_range(1:50),abs(xk(1:50)));%画双侧频谱幅度图
title('正弦波频谱');
xlabel('频率 (Hz)');
ylabel('幅度');

运行该MATLAB程序，绘制出一次采样采集到的波形如下图所示（以正弦信号为例）：

可以看到该信号具有直流分量（频率为0）以及1000Hz除的正弦分量，两者幅值均为2048（与产生波形时的一致）。

除此之外，在单片机编程中，为寻找并在屏幕上打印出信号的基波频率，还需要在显示频率波形的同时完成对于除直流分量外最高幅值对应频率的计算（数组返回最大值对应下表，算法较简单在此省略实现过程）。

模块功能实现效果

启动系统后按压按键KEY_0启动1000Hz正弦波信号的生成与采集，将信号输出引脚PA4与信号输入引脚PA5连接，并按压按键KEY_2可启动短频段0~1000Hz的频谱显示如下：

按压按键KEY_UP可启动长频段0~8000Hz的频谱显示如下：

可以看到此时只有直流分量和1000Hz的正弦分量两个尖峰，与MATLAB模拟计算结果一致。

按压按键KEY_1，切换为1000Hz方波波信号的生成与采集，将信号输出引脚PA4与信号输入引脚PA5连接，并按压按键KEY_2可启动短频段0~1000Hz的频谱显示如下：

按压按键KEY_UP可启动长频段0~8000Hz的频谱显示如下：

可以看到此时在01000Hz频段只有直流分量和1000Hz的正弦分量两个尖峰，但在08000Hz频段，由于方波实质上是不同频率的正弦信号的叠加，所以频谱会在基波的奇数倍（1、3、5……）处也有尖峰，但尖峰的幅值会远小于基波1000Hz处，且倍数越大幅值越小，这使得按照先前的算法也能识别出基波频率约为1000Hz。

4、补充测试

由于还需要对于基波频率在0~1000Hz范围内的任意输入信号进行频谱分析，经过调试后，当输入信号频率为200Hz时，为使得显示波形合理，将ADC环节设置的额外时钟周期由112调整至480，结果如下所示：

200Hz正弦波：

时域：

频域：

短频段（0~1000Hz）：

长频段（0~8000Hz）：

200Hz方波：

时域：

频域：

短频段（0~1000Hz）：

长频段（0~8000Hz）：

在ADC环节额外时钟周期设置为480的情况下，可以计算得出，对于频率为1000Hz的信号，一次采样（即屏幕内显示）包含21个周期（正好为整数），结果如下所示：

1000Hz正弦波：

时域：

频域：

长频段（0~8000Hz）：

1000Hz方波：

时域：

频域：

长频段（0~8000Hz）：

可以看到此时虽然时域上波形显示更加狭窄密集，但是频域上尖峰的变化过程也有了迅速的提升，且测得的基波频率也更加精准。

三、总结

通过本次项目实践，不仅在实验中进一步加深了对于数字信号的产生、采集与频谱分析处理过程的理解，特别是通过期望发生信号频率去计算定时器分频系数、采样频率的计算过程以及FFT计算与频谱图像绘制的过程；而且也增加了对于STM32F407单片机开发的实战经验，在巩固了引脚GPIO与时钟配置相关内容的同时，又对于DMA内存搬运及其中断以及DAC数模转换输出与ADC模数转换输入等功能模块有了更深刻的认识，包括定时器对于这些过程的调控也涉及到相关频率的计算，所有模块的配置之间都有着密切的联系。

摘要

本项目围绕机器学习分类算法展开， 以支持向量机（support vector machine，SVM）为主要研究对象，探究其分类性能、模型改进与优化算法。SVM和Logistic回归模型都可以用于解决二分类问题，但模型设计的思路不同，因此希望能够比较两者在分类性能上的差异。项目过程中代码完全基于R语言实现，主要以二维样本数据为研究对象，生成模拟数据并基于经典硬间隔SVM模型和Logistic回归模型分别进行建模与训练，在测试集上对模型进行验证，最终比较两者的分类效果差异。实验结果表明，当样本数据本身区分度不明显时，两种分类模型效果均较差，但Logistic模型明显优于经典硬间隔SVM；当样本数据本身具有明显的差异性时，两种分类模型效果均较好，SVM略优于Logistic。此外，还对改进后的SVM模型（核函数由线性函数更换为高斯核函数）进行性能测试，发现其在区分度不明显的数据集上显著优于经典硬间隔SVM，说明其显著提升了其在非线性可分数据上的分类效果；但在区分度较明显的数据集上分类效果反而略逊于经典SVM模型。最后，对两篇有关SVM改进模型的文献进行了阅读与调研，总结了软间隔SVM模型在正则项和损失项拓展方面的研究进展，并介绍了柔性套索惩罚和快速截断Huber损失等改进方法。

关键词: 机器学习分类算法、二分类、支持向量机（SVM）、Logistic回归、硬间隔、软间隔、高斯核函数、改进的SVM模型、柔性套索惩罚、快速截断Huber损失、R语言

项目概述

问题背景

人工智能的概念是在1956年首次被提出，其目标旨在希望通过计算机模拟人的思维能力或智能行为，从而让计算机能够像人类一样进行思考。目前，人工智能已经在机器翻译、智能控制、图像识别、语音识别、游戏博弈等领域得到广泛的应用。

机器学习作为人工智能的一个发展方向，起源于20世纪50年代的感知机数学模型，其目标是使得机器能够像人类一样具有学习能力。机器学习的基本过程主要是基于样本数据（客体）去训练/学习某个模型或决策函数（主体）。一般而言，正则化框架下的机器学习过程主要由学习机、损失项和正则项（惩罚项）三个部分构成，最终通过学习得到模型。

支持向量机(support vector machine，SVM)最早由Cortes和Vapnik二人于1995年为解决二分类问题而提出[^1]。作为经典的机器学习模型之一，SVM有坚实的统计理论基础，算法实现容易，且决策函数具有很强的几何含义。由于其在模式识别等数据分析问题中的优越表现，SVM如今已成为最经典的判别分析方法之一。与SVM相类似，广义线性回归统计模型中的Logistic回归模型同样也可用于解决二分类问题。本质上来说，两种方法都注重研究一组协变量X_1, …, X_p是如何影响二元的响应变量Y的，在用途上具有极大的相似性，因而希望研究并比较两者分类效果的差异性。

除此之外，SVM作为一种经典且基础的机器学习算法，在漫长的发展历程中也经历了多次迭代，有多种多样的改进版本。最基本的版本为硬间隔SVM，但由于实际的样本数据很可能不满足线性可分的理想情况，又发展出了采用不同求解算法的软间隔SVM模型以及基于核函数升维思想实现的非线性SVM，基于软间隔SVM模型又集中在模型损失项与正则项两个方面进行了理论上的拓展。这样的发展是永无止境的，在此希望对过去的部分研究改进成果进行理论总结与代码实现，以更好地了解SVM模型的发展现状。

项目任务

在本次项目中，需要随机生成模拟数据，并在该样本数据基础上分别利用经典SVM模型与Logistic模型进行统计建模，同时对比两者的分类效果；此外还需要总结并实现部分改进版本的SVM算法，分析其预测效果。具体而言可细分为如下任务：

• 任务1:随机生成200条模拟数据并将其分为训练数据集与测试数据集，利用训练数据集分别基于经典硬间隔SVM模型与Logistic广义线性回归模型建立统计模型，实现样本数据的分类且在测试数据集上进行验证，比较两者的分类效果差异。

• 任务2:代码实现某一种改进版本的SVM模型，简单测试其性能并将其分类结果与经典版本进行对比。

• 任务3:查阅SVM模型改进相关的文献，基于正则化框架对于文献中涉及的模型（学习机、损失项、正则项）、创新点与求解算法进行重述与总结。

项目过程

本项目代码部分完全基于R语言实现，主要涉及样本模拟数据的生成，以及SVM（经典与改进版本）与Logistic回归模型的建立、训练与测试。

模拟数据生成

本项目中涉及到的样本数据完全由模拟方法生成。具体而言，不论是SVM还是Logistic回归模型，其目的都是为了研究一系列协变量对于一个二元的响应变量的影响。为方便起见，选择采用协变量的维度为二维，即二元响应变量Y只由两个变量X_1, X_2决定。在生成数据时，为了较好地区分出两类数据，分别在正态总体下以均值为0和均值为1生成两组模拟数据（同一条数据中的两个变量X_1, X_2来自同一均值的总体），并分别打上分类标签（即对应Y的取值为）0或1：

1
2
3
4
5
6
7
8
9
10
11
12

set.seed(123) #设置随机种子，固定每次运行程序生成的随机数，使结果可重复
n <- 200  # 每个类别的数据点数

# 生成类别0的数据
x1 <- matrix(rnorm(n * 2, mean = 0), ncol = 2)
y1 <- rep(0, n)
# 生成类别1的数据
x2 <- matrix(rnorm(n * 2, mean = 1), ncol = 2)
y2 <- rep(1, n)
# 合并数据
x <- rbind(x1, x2)
y <- c(y1, y2)

绘制样本点对应的散点图，初步观察其分类情况：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19

# 加载 ggplot2 包
library(ggplot2)

# 创建数据框
data <- data.frame(x1 = x[, 1], x2 = x[, 2], y = factor(y))

# 设置点的大小和透明度
p <- ggplot(data, aes(x = x1, y = x2, color = y)) +
geom_point(size = 3, alpha = 0.7) +  # 调整点的大小和透明度
scale_color_manual(values = c("blue", "red")) +
labs(x = "x1", y = "x2", color = "Class") +
theme_minimal()

# 调整背景和边界线
p + theme(panel.background = element_rect(fill = "white", color = "black"),
panel.border = element_rect(color = "black", fill = NA),
axis.line = element_line(color = "black"))

ggsave("2.png", plot = p, width = 6, height = 6, units = "in", dpi = 300)

观察上左图可知，由于基于正态总体生成模拟数据时仅制定了均值而未指定方差（默认为1），导致令均值为0和1的情况下两类数据没有办法明显的区分开来，这样的分类效果显然是不好的。经过调试，当设置两类数据均值分别为0和5时，数据点呈现良好的区分性（如上右图所示）。

为便于后续的模型训练，还需要将样本模拟数据分成训练集与测试集两部分，根据经验，较为合适的数据集数量比例为7：3，即样本数据中的70%为训练集，另外30%为测试集用于验证。

1
2
3
4
5
6
7
8
9
10

# 将数据分为训练集和测试集
train_index <- sample(1:(2 * n), 0.7 * 2 * n)
x_train <- x[train_index, ]
y_train <- y[train_index]
x_test <- x[-train_index, ]
y_test <- y[-train_index]

# 合并 
train_data <- cbind(x_train, y_train)
test_data <- cbind(x_test, y_test)

在此对部分训练集数据进行罗列：

1
2

#观察数据集
head(train_data,10)

基于经典硬间隔SVM模型的建模

硬间隔支持向量机是一种基于线性可分数据集的分类模型。线性可分，意味着可用一条直线将两类数据分开。显然这样的直线有无穷多条，但对应直线的上下移动又因分类要求的限制而存在极限位置。因此，硬间隔支持向量机所要解决的关键问题就是，如何从无穷多条直线（对应无穷多个分类器）中选择最优？

实际上，具有”最大间隔”的分类器就是SVM要寻找的最优解，而最优解对应的两侧虚线（上下极限状态）所穿过的样本点，就是SVM中的支持样本点，称为"支持向量"。SVM中寻找最优分类器的问题，本质上是一个优化问题。对于一般的优化问题，往往有3个基本要素需要重点关注：

• 目标函数:希望优化的目标指标；

• 优化对象:期望通过改变哪些因素（协变量）使目标函数达到最优；

• 约束条件:优化对象一般需要满足一些特定的约束条件。

假定
$$
{ {({x_i}^T,y_i)} }_{i=1}^n
$$
表示一个二分类数据集，其中第i个样本x_i ∈ R^p，样本对应的标签y_i ∈ {-1,+1}, i=1, …, n。对于优化对象x_i而言，可以根据解析几何的基本知识构造其分类器（超平面）的一般表达式：

$$
w_1x_1+\cdots+w_px_p+b=w^Tx+b=0
$$
其中
$$
w={(w_1,\cdots,w_p)}^T
$$
为该超平面的法向量，b为超平面的截距。

显然，SVM中的优化对象就是上述分类器，或者说超平面中的参数w, b。在本项目的模拟数据中，令样本协变量维度p=2，此时分类器为R^3上的平面。

在线性SVM算法中，目标函数显然就是”分类间隔”，即目标是最大化“分类间隔”W=2d (如下图所示)。其中d表示“支持向量”到最优分类器的距离，最大化W等价于最大化d。

最后是约束条件的确定。首先要考虑的就是，如何判断超平面是否将样本点正确分类？此外，目标函数本质上是求距离d的最大值，要确定约束条件，还必须要找到哪些是”支持向量”。总而言之，对于目标函数d的取值范围受到的限制和约束条件的确定，关键问题是如何将其数学化。

以上述平面上的分类问题为例：对任意一点x_i，其到最优分类直线的距离为
$$
d_i=\frac{|w^Tx_i+b|}{||w||}
$$
一方面，如果此时最优分类直线确实实现了分类目标，则所有样本点(y_i =1 or -1)必定都要满足约束(d 为最优距离)：
$$
d_i=\frac{|w^Tx_i+b|}{||w||}\geq d \Leftrightarrow
\begin{cases}
\frac{w^Tx_i+b}{||w||}\geq d,\ y_i=1\ \
\frac{w^Tx_i+b}{||w||}\le-d,\ y_i=-1
\end{cases}
\Leftrightarrow \ y_i\bullet\frac{w^Tx_i+b}{||w||}\geq d\
\Leftrightarrow \frac{1}{||w||d}\bullet y_i(w^Tx_i+b)\geq1
$$
注意到:
$$
w^Tx+b=0与\frac{1}{||w||d}\bullet(w^Tx+b)=0
$$
本质上代表同一个超平面，因此上述约束条件可以等价改写为：
$$
y_i(w^Tx_i+b)\geq1
$$
另一方面，注意到”支持向量”都是位于最优分类超平面上，即若点(x_i, y_i)为”支持向量”，则必有：
$$
w^Tx_i+b=1
$$
于是最大化目标函数(“支持向量”到最优分类超平面的间隔)，等价于最大化：
$$
\frac{1}{||w||}
$$
也等价于最小化：
$$
\frac{1}{2}{||w||}^2
$$
综上所述，硬间隔SVM的基本数学模型可以描述为如下不等式约束的二次型函数的约束优化问题：
$$
\begin{cases}
\mathop{min}\limits_{w,b}{ {\frac{1}{2}||w||}^{2} }\
st:\ y_i(w^Tx_i+b)\geq1,\ i=1,\cdots,n \
\end{cases}
$$
该优化问题由于受不等式约束，因此求解过程中还需要考察拉格朗日对偶问题和KKT条件。基于不等式约束的凸优化问题，可以基于拉格朗日对偶问题和KKT条件，然后利用SMO算法求解,得到最优w*, b*，从而可构造最优分类超平面：
$$
{(w^\ast)}^Tx+b^\ast=0
$$
对待分类的样本点x，根据以下决策函数来进行分类判别
$$
f(x)=sgn({(w^\ast)}^Tx+b^\ast)
$$
即当
$$
{(w^\ast)}^Tx+b^\ast>0
$$
时返回0，否则返回1。

基于以上理论， 可实现经典硬间隔SVM模型建构。在R语言中，e1071程序包内的svm()函数是对于经典硬间隔SVM模型的封装实现，其基本用法如下：

1
2

# 训练 SVM 模型
model <- svm(formula,data,labels,scale=TRUE/FALSE,kernel,gamma=g,degree=d,cost=C,epsilon=0.1,na.action=na.omit/na.fail)

其中：

• formula:拟合公式，以R公式的形式指定输出变量和输入变量，其格式一般为：输出变量名 输入变量名；

• data:训练数据集（各协变量），通常是一个数据框或矩阵；

• labels:标签数据（二元响应变量），通常是一个因子向量，用于指定那些将被用来训练模型的采样数据;

• scale:逻辑向量，指定特征数据是否需要标准化(默认标准化为均值0，方差1)，默认为True;

• kernel:核函数类型， 常用的有"linear"、"polynomial"、"radial" (RBF) 和 "sigmoid";

• gamma:用于指定多项式核以及径向基核中的参数，默认gamma是线性核中的常数项，等于1/p(p为特征空间中的维度);

• degree:用于指定多项式核中的阶数d;

• cost:惩罚参数，用于控制误分类的惩罚程度;

• epsilon:用于指定支持向量回归中的带，默认值为0.1;

• na.action:用于指定当样本数据中存在无效的空数据时系统应该进行的处理，默认值na.omit表明程序会忽略那些数据缺失的样本；另外一个可选的赋值是na.fail，它指示系统在遇到空数据时给出一条错误信息。

硬间隔支持向量机（Hard-Margin SVM）是支持向量机的一种特殊情况，适用于数据完全线性可分的情况。它通过最大化间隔来找到一个分离超平面，使得所有数据点都在间隔之外且没有误分类点。为实现经典的硬间隔SVM模型，可以将svm()函数的参数如下设置：

1
2
3
4

# 训练硬间隔SVM模型
library(e1071) # 需要先导入e1071库
cost_value <- 1e5 # 设置 cost 为一个非常大的值，从而确保没有误分类
svm_model <- svm(x_train,y_train, type = "C-classification", kernel = "linear", cost = cost_value, scale = FALSE)

可以通过summary()方法查看训练出的模型的信息：

1
2
3
4
5
6
7
8
9
10
11
12
13
14

summary(svm_model)
	
Call:
svm.default(x = x_train, y = y_train, scale = FALSE, type = "C-classification",
kernel = "linear", cost = cost_value)

Parameters:
SVM-Type:  C-classification   #模型类别
SVM-Kernel:  linear   #模型使用的核函数
cost:  1e+05   #模型确定的约束违反成本
Number of Support Vectors:  2	( 1 1 )  
#模型找到的支持向量数量，两类各有一个
Number of Classes:  2
Levels:	0 1   #模型中分类的目标类别

至此已经基于训练集数据完成了经典硬间隔SVM模型的建立与训练过程。

基于Logistic回归模型的建模

与SVM模型类似，Logistic回归模型也旨在研究协变量X_1, …, X_p是如何影响响应变量y的。但不同的是，Logistic作为广义线性回归模型的一种，本质上还是基于”回归”的基本思想，在多元线性回归的基础上进行调整从而能够处理离散的二元数据。

假定对二元响应变量y有n次观测：
$$
\ y_i \sim B(1,\mu_i),\ i=1,\cdots,n
$$
显然其均值方差分别为：
$$
{E(y}i)=\mu_i;\ Var(y_i)=\mu_i(1-\mu_i)
$$
同时，对协变量X_1, …, X_p也有n次观测，且记其线性组合分别为：
$$
\eta_i=\beta_0+x{i1}\beta_1+\cdots+x_{ip}\beta_p=x_i^T\beta,\ i=1,\cdots,n
$$
其中：
$$
x_i={(1,x_{i1},\cdots,x_{ip})}^T, \beta={(\beta_0,\beta_1,\cdots,\beta_p)}^T
$$
在多元线性回归的过程中，由于响应变量y连续，因此可以直接令其均值E(y)等于协变量与未知参数的线性组合η；但对于二元（取值仅存在0或1两种情况）的响应变量而言，这样简单粗暴的处理方式显然是不合适的，需要在原来的方法上做推广，即”广义”的线性回归模型：既然无法直接令两者相等，不妨寻找一个链接函数，将多元线性回归中值域为R的协变量线性组合
$$
\eta_i=x_i^T\beta
$$
映射到μ_i ∈ (0*,* 1)区间上，从而便于分类过程的实现。

具体而言，在建立广义线性回归模型的过程中，需要在多元线性回归的基础上选取合适的链接函数g(•)把y_i的期望µ_i = E(y)和协变量的线性组合η _i = x_i^T * β联系起来，使得g(µ _i) = η _i。这样的链接函数应该具有良好的性质（如光滑等），才便于后续计算。

针对二元响应变量的分类过程，常用的连接函数为Logistic链接函数：
$$
g(x)=ln{\frac{x}{1-x}},\ x\in(0,1)
$$
基于Logistic链接函数构造的广义线性回归模型即为Logistic回归模型，其基本内容如下：
$$
\begin{cases}
y_i \sim B(1,\mu_i), i=1,\cdots,n \
g(\mu_i)=\mu_i=x_i^T\beta
\end{cases}
$$
事实上，基于给定的样本数据（训练集数据），训练模型的过程即为对未知参数向量β进行极大似然估计的过程，但与线性模型不同的是，该模型中极大似然估计并没有显式解，因此需要基于特定的算法进行数值求解，一般采用Newton-Raphson迭代算法，这里不详细展开求解过程。

在R语言中，Logistic模型的训练由基本包中自带的glm()函数实现，其基本用法如下：

1
2

# 训练广义线性模型
model <- glm(formula, data = mydata, family = family(link = "link_function"))

其中：

• formula:表示响应变量与解释变量的关系公式，如y ∼ x1 + x2；

• data:所用的数据集（需要为数据框格式来识别特征和标签）；

• family:表示所拟合的GLM模型类型，包括（但不限于）高斯、二项分布、泊松分布等;

• link:表示链接函数，常见的有"identity"、”logit”、"log"等。

为实现广义回归模型中的Logistic回归模型，可按照如下方式进行调用：

1
2
3
4
5

# 将训练数据转换为数据框
train_data <- data.frame(x = x_train, y = as.factor(y_train))

# 训练Logistic回归模型
logistic_model <- glm(y ~ ., data = train_data, family = binomial)

同样采用summary()方法查看训练模型的信息：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

summary(logistic_model)

Call:   # 调用信息, 说明模型使用了二元逻辑回归和所有特征。
glm(formula = y ~ ., family = binomial, data = train_data)

Coefficients:   # 估计值、标准误差、z值和p值的表格。
(Intercept)  -169.33   32205.14  -0.005    0.996
# 截距项的估计值、标准误差、z值和p值。
x.1            15.32    5873.78   0.003    0.998
# 第一个特征x.1的估计值、标准误差、z值和p值。
x.2            63.10   13139.18   0.005    0.996
# 第二个特征x.2的估计值、标准误差、z值和p值。
(Dispersion parameter for binomial family taken to be 1)
# 二项分布的离散参数被设定为1。
Null deviance: 3.8815e+02  on 279  degrees of freedom
# 零偏差：模型中只有截距时的偏差, 279个自由度。
Residual deviance: 6.1211e-08  on 277  degrees of freedom
# 残差偏差：模型拟合后的偏差, 277个自由度。
AIC: 6	# AIC 值（信息准则）, 用于模型选择。
Number of Fisher Scoring iterations: 25   
# Fisher评分算法的迭代次数。

至此已经基于训练集数据完成了Logistic回归模型的建立与训练过程。

二者分类效果的比较

前文在生成模拟数据的过程中提到，若生成时设置的正态总体均值差异不同，其聚集效果也会不同，在默认方差为1的情况下，设置均值为0和1的数据点区分不明显，而设置均值为0和5的数据点区分明显。在模型效果测试中，为保证模型聚合等多方面因素，分别选用均值为0和1（数据点区分不明显），以及均值为0和2.8的数据（数据点区分明显）进行模型训练与性能测试。

首先是选用均值为0和1的数据，即标签为0的数据应聚集在点(0, 0)附近，标签为1的数据应聚集在点(1, 1)附近。将训练好的模型应用于测试集上并计算其分类准确率，同时绘制其分类结果与分类器效果图像：

1
2
3
4
5
6
7
8
9
10

# 使用SVM模型进行预测
svm_predictions <- predict(svm_model, x_test)
# 计算SVM模型的分类准确率
svm_accuracy <- sum(svm_predictions == y_test) / length(y_test)

# 使用Logistic回归模型进行预测
logistic_probabilities <- predict(logistic_model, newdata = data.frame(x = x_test), type = "response")
logistic_predictions <- ifelse(logistic_probabilities > 0.5, 1, 0)
# 计算Logistic回归模型的分类准确率
logistic_accuracy <- sum(logistic_predictions == y_test) / length(y_test)

运行程序，得到该情况下SVM模型的分类准确率为0.525，Logistic回归模型的分类准确率为0.767。结合观察图像可知，此时对于区分不明显的数据，两个模型的分类效果都较差（90%以下），但Logistic回归模型在数据区分不明显的情况下分类性能要明显优于经典硬间隔SVM模型。

然后选用均值为0和2.8的数据，即标签为0的数据应聚集在点(0, 0)附近，标签为1的数据应聚集在点(2*.8, 2.*8)附近。将训练好的模型应用于测试集上并计算其分类准确率，同时绘制其分类结果与分类器效果图像：

运行程序，得到该情况下SVM模型的分类准确率为0.967，Logistic回归模型的分类准确率为0.950。结合观察图像可知，此时对于区分较明显的数据，两个模型的分类效果都较好（90%以上），但经典硬间隔SVM模型在数据区分较明显的情况下分类性能会略优于Logistic回归模型，但总体而言差异并不是很大。

改进后SVM模型的性能测试

上述内容中用到的经典硬间隔SVM模型实际上对于数据有着较高的要求，其实现依赖于样本数据为线性可分这一假设条件，但现实中的实际数据往往很难满足这一点，这也是其缺陷所在。针对不是线性可分的数据，Cortes和Vapnik采用了一种非常巧妙的方式进行改进，即借助函数映射，将低维数据映射到高维空间中，使得在低维空间原来不能线性可分的数据，在高维空间变得线性可分，且可以证明这样的映射始终存在。

具体而言，构造升维函数：
$$
K(x,z)=\varphi(x)\bullet\varphi(z)
$$
并构造相应的非线性SVM：
$$
\begin{cases}
\mathop{min}\limits_{w,\ b,\ \delta_i} { {\frac{1}{2}||w||}^2} \
st:\ y_i(w^TK(x_i,z)+b)\geq1,\ \ i=1,\cdots,n
\end{cases}
$$
相应的最优分类超平面为：
$$
{(w^\ast)}^TK(x_i,z)+b^\ast=0
$$
这里选择高斯函数(径向基函数RBF)作为核函数，将每一个样本点映射到一个无穷维的特征空间从而实现降维，构造改进的SVM模型：
$$
K(x, x’) = \exp\left(-\gamma | x - x’ |^2\right)
$$
高斯核函数实现的功能是：先将原始的数据点(x, y)映射为新的样本(x′, y′)，再将新的特征向量点乘(x′, y′)，返回其点乘结果。其主要目的是找到更有利分类任务的新的空间，本质是在衡量样本和样本之间的”相似度”，在一个刻画”相似度”的空间中，让同类样本更好的聚在一起，进而线性可分。

在R语言中，要实现高斯核函数，仅需将调用的svm()函数中的参数进行改变：

1
2

# 训练改进后的非线性SVM模型
svm_model_modify <- svm(x_train,y_train, type = "C-classification", kernel = "radial", gamma = 2, cost = 1, scale = FALSE)

其中核函数类型的参数kernel由原先的线性函数”linear”更换为高斯核函数”radial”，并对核函数中的参数gamma以及控制误分类的惩罚参数cost进行了相应调整。

在样本数据区分不明显（即总体均值设置为0和1）的情况下，运行程序，得到改进后的非线性SVM模型的分类准确率为0.767，与Logistic回归模型在该样本下的分类准确率一致，较原先经典硬间隔SVM的预测准确率0.525有显著提升。改进后模型的分类效果如下图所示。

在样本数据区分较明显（即总体均值设置为0和2.8）的情况下，运行程序，得到改进后的非线性SVM模型的分类准确率为0.958，虽然仍优于Logistic回归模型在该样本下的分类效果，但较原先经典硬间隔SVM的预测准确率0.967有略微下降。尽管如此，改进后的模型分类准确率仍然在0.95以上，具有良好的分类效果。改进后模型的分类效果如下图所示。

文献调研

硬间距SVM要求构建的分类超平面完全正确的分类所有训练数据，但由于实际样本被假设成线性可分的条件太强，为避免过拟合，更多时候需要考虑软间距（soft-margin）SVM模型。

所谓软间距，就是允许一些点不满足线性可分的约束条件，即：
$$
y_i\left(w^Tx_i+b\right)<1
$$
当然，为保证拟合模型仍然为一种SVM方法的”本性”，我们希望这样的点不能太多，因此要对违反约束的样本点(x_i, y_i)施加一定的非负惩罚（松弛变量）δ_i：
$$
\delta_i=max{0, 1-y_i\left(w^Tx_i+b\right)}=
\begin{cases}
0, y_i\left(w^Tx_i+b\right)\geq1\ \
1-y_i\left(w^Tx_i+b\right),\ y_i\left(w^Tx_i+b\right)<1
\end{cases}
$$
这样就可以得到软间隔SVM的基本数学模型：
$$
\begin{cases}
\mathop{min}\limits_{w,\ b,\ \delta_i} { {\frac{1}{2}||w||}^2+c\sum_{i=1}^{n}\delta_i} \
st:\ y_i(w^Tx_i+b)+\delta_i\geq1,\ \delta_i\geq0,\ i=1,\cdots,n
\end{cases}
$$
值得注意的是，每一个样本都有一个对应的松弛变量，表征该样本不满足约束的程度；c>0为惩罚参数，其值越大，对分类的惩罚越大。跟硬间隔SVM一样，先用拉格朗日乘子法得到拉格朗日函数，再求其对偶问题。

在正则化的框架下，软间隔SVM的基本数学模型结构还可改写为：
$$
\mathop{min}\limits_{w,\ b,\ \delta_i} { {\frac{1}{2}||w||}^2+c\sum_{i=1}^{n}\delta_i}
$$
其中学习机、损失项、正则化项分别定义为：

• 学习机：
  $$
  f\left(w\right)=w^Tx+b
  $$

• 损失项：
  $$
  c\sum_{i=1}^{n}\delta_i
  $$

• 正则项：
  $$
  \frac{1}{2}w^Tw={\frac{1}{2}||w||}^2
  $$

基于软间隔SVM模型，理论上的拓展主要集中在模型损失项与正则项两个方面，针对这两个方面，接下来将分别选取一篇文献，对其改进模型、求解算法与创新点进行简单的书面总结。

正则项拓展：柔性套索惩罚[^2]

模型重述

柔性套索（Pliable lasso）是一种新的互动模型，由Robert Tibshirani和Jerome Friedman于2018年提出。该模型是原始套索问题的扩展，允许套索回归模型接受修改变量、预测因子和结果。原始的套索（Lasso）问题本质上是将1-范数作为正则项，而柔性套索是在此基础上的改进。1-范数支持向量机的缺点之一是，在一些输入高度相关的情况下，并且所有输入都与输出相关，1-范数惩罚最终会挑选出少数输入，并将其余的减少到零，这意味着”群体选择”将是对1范数支持向量机的挑战。

柔性套索惩罚允许估计协变量X的主要影响以及协变量与一组修饰符Z之间的相互作用影响，以处理相互作用效应。为了处理变量选择并帮助选择相关变量组，引入修正变量Z，并将柔性套索作为正则项（惩罚项），得到具有柔性套索惩罚的SVM目标函数：
$$
\min_{\beta, h} \left( \frac{1}{n} \sum_{i=1}^n (1 - y_i (\beta_0 + x_i^T \beta))+^2 + \lambda_1 \left( \sum{j=1}^p |\beta_j|1 + \sum{j=1}^p |h_j|_1 \right) + \frac{\lambda_2}{2} \left( |\beta|2^2 + \sum{j=1}^p |h_j|_2^2 \right) \right)
$$
其中，学习机、损失项和正则化项（惩罚项）定义如下：

• 学习机：
  $$
  y = \beta_0 + Z h_0 + \sum_{j=1}^p X_j (\beta_j + Z h_j) + \epsilon_i
  $$

• 损失项：损失平方和（hinge函数）
  $$
  \frac{1}{n} \sum_{i=1}^n (1 - y_i (\beta_0 + x_i^T \beta))^2
  $$

• 正则项：柔性套索惩罚
  $$
  \lambda_1 \left( \sum_{j=1}^p |\beta_j|1 + \sum{j=1}^p |h_j|_1 \right) + \frac{\lambda_2}{2} \left( |\beta|2^2 + \sum{j=1}^p |h_j|_2^2 \right)
  $$

求解算法与创新点

在目标函数优化的求解过程中， 主要使用块方向坐标下降过程（the block-wise coordinate descent procedure）优化目标函数：

本篇文章在过去已有模型的基础上再次进行了改进，创造性地把柔性套索（优化后的1-范数）作为惩罚项（正则项）引入软间隔SVM模型中，创新点有如下几条：

• 可调套索惩罚的应用：将可调套索惩罚应用于支持向量机，能够有效估计协变量与修饰变量之间的交互作用。

• 交互项排除机制：通过可调套索，能够在相应主效应为零时自动排除交互项，提高模型的解释性和简洁性。

• 平方铰链损失结合块坐标下降算法:使用该组合优化目标函数，在模拟和实际数据（结肠癌和前列腺癌数据集）上验证了方法的有效性。

损失项拓展：快速截断Huber损失[^3]

模型重述

支持向量机作为一种有用的分类工具，在许多领域得到了广泛的应用。然而，在非常大的样本数据集上，它可能会导致计算上的不可行性。为了解决这一问题，该文章提出了一种新颖的带有截断Huber损失的稀疏鲁棒支持向量机模型，即L_th-SVM，其基本数学模型如下：
$$
\min_{w \in \mathbb{R}^n, b \in \mathbb{R}} \frac{1}{2} |w|^2 + \gamma \sum_{i=1}^m \ell_{th}(1 - y_i(\langle w, x_i \rangle + b))
$$
也可写作：
$$
\min_{w \in \mathbb{R}^n, b \in \mathbb{R}} \frac{1}{2} |w|^2 + \gamma \sum_{i=1}^m \ell_{th}(h)
$$
其中，学习机、损失项和正则化项（惩罚项）定义如下：

• 学习机：
  $$
  h:= \mathbf{1} - \mathcal{G}\mathbf{w} - b\mathbf{y} \in \mathbb{R}^{m},其中\begin{aligned}
  \mathcal{G} &:= \left[ \begin{array}{cccc}
  y_{1}x_{1} & y_{2}x_{2} & \cdots & y_{m}x_{m}
  \end{array} \right]^{\top} \in \mathbb{R}^{m \times n}
  \end{aligned}
  $$

• 损失项：快速截断Huber损失（Truncated Huber Loss）：
  $$
  \ell_{\text{th}}(t) = \begin{cases}
  1, & \text{if } t > 1 + \frac{\delta}{2}, \
  t - \frac{\delta}{2}, & \text{if } t \in \left[\delta, 1 + \frac{\delta}{2}\right], \
  \frac{t^2}{2\delta}, & \text{if } t \in [0, \delta), \
  0, & \text{if } t < 0.
  \end{cases}
  $$

• 正则项：L2正则化
  $$
  \frac{1}{2}w^Tw={\frac{1}{2}||w||}^2
  $$

求解算法与创新点

针对L_th-SVM问题，文章提出了一种新颖高效的乘子与工作集交替方向的方法L_th-ADMM)，证明了L_th-ADMM产生的序列是L_th-SVM的局部最小值，并且在工作集很小的情况下具有较低的计算复杂度。大量的数值实验表明，L_th-ADMM能够提供更高的预测精度，提供更少的支持向量，并且运行速度快，特别是在大规模数据集设置中。其求解算法大致如下：

本文的创新点主要包括以下几个方面：

• 截断Huber损失函数的提出：提出了一种新的截断Huber损失函数，该函数能够在保持稀疏性的同时，增加对异常值的鲁棒性。与现有的hinge损失、huberized pinball损失和Huber损失相比，截断Huber损失函数在稀疏性和鲁棒性方面表现出更优的性能。

• 稀疏鲁棒SVM模型的构建：基于截断Huber损失函数，构建了新的稀疏鲁棒支持向量机模型（Lth-SVM）。该模型不仅继承了SVM分类能力强、理论框架完善等优点，还通过引入截断Huber损失函数提高了模型的稀疏性和鲁棒性，使其在处理大规模分类问题时更加有效。

• 一阶最优性条件的建立：为L_th-SVM模型建立了基于新引入的P-稳定点的一阶必要和充分条件，定义了L_th支持向量和工作集。这些条件为算法设计提供了理论基础，同时也证明了所有L_th-SVM支持向量仅占整个训练集的一小部分，这为后续开发高效算法提供了可能。

• L_th-ADMM求解算法的设计：提出了一种具有工作集的新型交替方向乘子法（L_th-ADMM）来求解L_th-SVM。该算法通过引入工作集策略，显著降低了每次迭代中的计算复杂度，使得算法在处理大规模数据集时能够快速收敛。同时，理论证明表明，该算法生成的序列能够收敛到L_th-SVM的一个局部极小值。

参考文献

[^1]: Cortes C, Vapnik V. Support-vector networks[J]. Machine learning, 1995, 20(3): 273-297.
[^2]: Asenso, T. Q., Wang, P., & Zhang, H. (2022). Pliable lasso for the support vec- tor machine. Communications in Statistics - Simulation and Computation, 53(2), 786–798.
[^3]: Huajun Wang, Yuanhai Shao, Fast truncated Huber loss SVM for large scale clas- sification, Knowledge-Based Systems, Volume 260, 2023.

摘要

本项目的核心任务是通过统计过程控制（SPC）方法，对某工厂生产的滚珠直径数据进行产品质量管理，评估产品工艺水平及其生产过程是否受控。统计过程控制作为一种基于概率统计的过程控制方法，自1924年由Shewhart博士提出控制图以来，已广泛应用于现代制造过程的质量控制中。本项目基于Matlab软件开发，对于收集到的数据，通过描述性统计分析、正态性检验、总体均值检验、工序能力指数计算与绘制均值控制图和方差控制图等多种方式，对数据进行全面分析，最终评估工艺水平及生产过程的受控状态。具体而言，首先，在数据收集过程中，结合实际数据与模拟数据，得到25组产品质量样本数据（每组至少5个样本）；随后，通过描述性统计分析方法对数据进行宏观的认知，根据均值、方差、极差、直方图等指标对数据进行初步分析；接着又利用假设检验的方法，分别通过Pearson卡方检验与t检验等方法验证数据是否符合正态分布，并对其总体均值进行检验。基于对于正态总体及其均值、方差的检验合理性，最终计算并评估了数据的工序能力指数，同时绘制了均值控制图和方差控制图，以直观反映生产过程中的数据波动情况。通过多种方法的综合分析，最终得出了工厂生产的滚珠直径的工艺水平评价和生产过程的受控状态判断。

关键词: 产品质量管理、统计过程控制（SPC）、假设检验、Shewhart控制图、Matlab

项目概述

问题背景

现代产品质量管理主要有两个核心目标：第一，评价产品的工艺水平如何，即产品质量如何；第二，评价产品的生产过程是否受控，即产品可靠性如何。从现代质量控制的角度看，一方面要求产品质量好，另一方面还要求可靠性高。

统计过程控制(Statistical Process Control, SPC)是一种基于概率统计的过程控制方法。1924年美国贝尔实验室的Shewhart博士提出控制图，标志着产品统计质量控制的正式起点。作为现代质量管理的重要方法，SPC广泛应用于现代制造过程的质量控制。20实际80年代，国际上半导体制造过程已经普遍采用SPC技术提升产品合格率和可靠性。比如Motorola公司提出并在美国通用电器公司等广泛应用的6σ管理，其主要技术基础就是SPC理论。

生产过程是否处于统计受控状态，取决于生产过程是否存在异常因素导致产品质量的起伏变化。产品加工结果是否满足加工规范要求，反应的是工艺水平的高低；而工艺是否受控，反应的是生产过程是否存在异常因素。

项目任务

在本次项目中，需要收集批量产品质量数据，利用SPC方法对其进行管理，分析整体工艺水平并判断生产过程是否处于统计受控状态，具体而言可细分为如下任务：

• 任务 1:收集或生成25组以上的数据，每组至少5个样本。

• 任务2:根据均值，方差，极差，直方图等指标，对数据进行描述性统计分析。

• 任务 3:对数据进行正态性检验与总体的均值检验。

• 任务 4:计算数据工序能力指数并对其进行评估。

• 任务 5:描绘均值控制图和方差控制图。

• 任务 6:得出结论：工艺水平如何；生产过程是否处于统计受控状态。

项目过程

本项目完全基于Matlab代码实现SPC方法进行产品质量管理。

数据收集

本项目中的数据收集采用实际数据与模拟数据结合的方式，其中实际数据以书本7.4节的例题7.4.4[^1]为主要来源，在此基础上利用生成的模拟数据对其进行扩充，最终得到25组产品质量样本数据，每组中含有5个数据。

具体而言，本项目的问题情境为对工厂生产的滚珠直径（单位：$mm$）进行产品质量管理。书本上的例题提供了50个直径数据，将其作为扩充生成模拟数据的源数据source_data，即：

1
2
3
4
5
6
7
8
9
10

source_data=[15.0,15.8,15.2,15.1,15.9,
             14.7,14.8,15.5,15.6,15.3,
             15.0,15.6,15.7,15.8,14.5,
             15.1,15.3,14.9,14.9,15.2,
             15.9,15.0,15.3,15.6,15.1,
             14.9,14.2,14.6,15.8,15.2,
             15.2,15.0,14.9,14.8,15.1,
             15.5,15.5,15.1,15.1,15.0,
             15.3,14.7,14.5,15.5,15.0,
             14.7,14.6,14.2,14.2,14.5]; 

设置扩充后的数据组数num_groups以及各组样本数据个数samples_per_groups：

1
2

num_groups = 25;
samples_per_group = 5;

两者相乘得到样本数据总数num_data_points：

1

num_data_points = num_groups * samples_per_group;

要对原始数据进行扩充，首先需要复制原始数据并添加随机扰动：

1
2

replicated_data = repmat(source_data, 1, ceil(num_data_points / length(source_data)));
perturbed_data = replicated_data(1:num_data_points) + 0.05 * randn(1, num_data_points);

接着使用插值生成更多数据点，并保留一位小数：

1
2
3
4

x = 1:length(perturbed_data);
xi = linspace(1, length(perturbed_data), num_data_points);
interpolated_data = interp1(x, perturbed_data, xi, 'spline');
interpolated_data = round(interpolated_data, 1);

最后将生成的模拟数据点重新组织为25组，每组5个数据，得到完整样本数据：

1
2

expanded_data = reshape(interpolated_data, samples_per_group, num_groups)';
data = expanded_data;

最终的样本数据如下表所示：

其中每行表示一组数据，共25组，每组中有5个数据。

描述性统计分析

描述性统计量主要有以下几种：

• 均值 (Mean): 计算每组数据的平均值，反映该组数据的中心位置。

• 方差 (Variance): 度量数据分散程度，数值越大表示数据的波动越大。

• 标准差 (Standard Deviation): 方差的平方根，同样反映数据的分散程度，但单位与数据相同。

• 极差 (Range): 最大值与最小值的差，表示数据的跨度。

利用Matlab软件中自带的库函数，可以分别实现对于各组数据均值、方差、标准差与极差等统计量的计算：

1
2
3
4

means = mean(data, 2); 
variances = var(data, 0, 2); 
ranges = range(data, 2); 
stds = std(data, 0, 2);

以及计算总体数据的均值、方差、标准差与极差：

1
2
3
4

overall_mean = mean(data(:)); 
overall_variance = var(data(:)); 
overall_range = range(data(:)); 
overall_std = std(data(:));

值得注意的是，Matlab中计算方差的函数 var(A) 默认会按照 N-1（N是观测值数量，即上文中的num_data_points）实现归一化，从而可以作为对于总体分布的方差的无偏估计。上述得到的均为修正后的样本方差。

通过对于上述四个描述性统计量的样本观测结果进行初步分析，可以得到如下结论：

• 滚珠直径的总体平均值约为15.1mm，可作为后续总体均值检验的检验目标。

• 各组数据的方差均在0.5以内，总体方差与标准差也较低，说明生产过程中数据波动较小，产品质量稳定性较高。

• 数据极差范围适中，总体极差不超过2，处于合理区间。

除此之外，还可以通过绘制频数分布直方图的方式对于样本数据进行描述性统计分析：

1

h = histogram(data);

观察图表可知，数据的频数分布大致呈”两边少，中间多”的趋势，推测其总体大致服从正态分布，需要进行进一步检验。

数据正态性检验

要验证总体是否服从正态分布，需要采用Pearson卡方检验方法，对总体的分布类型进行检验。

Matlab软件中，有相应的函数chi2gof可通过Pearson卡方检验来验证总体分布类型是否为正态分布：

1

[result, p_chi2] = chi2gof(data(:));

chi2gof函数是使用Pearson卡方检验返回原假设的检验决策，其原假设假定样本数据来自正态分布的总体，备择假设是样本数据不来自正态分布的总体。默认情况（仅将样本数据data输入chi2gof函数）下，检验的显著性水平为δ=0.05，且默认检验总体是否服从正态分布；通过增加函数的输入参数可以对显著性水平和检验的分布类型进行更改，在此不再赘述。

返回的参数result为正态性检验的结果，若result=0，说明无法拒绝原假设，即可认为样本数据来自正态分布的总体；若result=1，则拒绝原假设，认为数据不来自正态分布的总体。另一返回参数p_chi2则为该检验的p值，当其大于显著性水平δ时不拒绝原假设。

通过对样本数据进行正态性检验，可认为数据来自正态分布的总体，正态性检验的p值为0.37005。

从原理出发，也可根据假设检验的基本方法流程对总体的分布类型进行检验：

首先，提出原假设与备择假设：

• H_0：总体服从正态分布，即
  $$
  F(x) = \phi(\frac{x-\mu}{\sigma})
  $$

• H_1：总体不服从正态分布，即
  $$
  F(x) \neq \phi(\frac{x-\mu}{\sigma})
  $$

其中F(x)为总体的分布，μ和σ为正态分布的均值与方差（均为未知参数）。

接着，根据假设的提出给出对应的拒绝域形式：
$$
R_0 = \left{ T>c \right}
$$
其中T为构造的统计量：

$$
T=\sum_{i=1}^{r}\frac{ {(n_i-n{p}_i)}^2}{np_i}
$$
若将对总体X所有可能的取值范围分割成$r个两两不交的部分，则式中：

• n_i：统计样本中实际落入第i个部分的个数（实际频数）；
• n：样本数据总数，即上文代码中的num_data_points；
• p_i：任一样本落入第$i$个部分的概率。

显然，当原假设
$$
H_0: F(x) = \phi(\frac{x-\mu}{\sigma})
$$
成立时，理论频数n*p_i应与实际频数n_i较为接近，这也是拒绝域形式的确定依据。

可以注意到，Pearson卡方统计量
$$
T=\sum_{i=1}^{r}\frac{ { {(n_i-np}_i)}^2}{np_i}
$$
服从如下的大样本分布：

$$
\hat{T}=\sum_{i=1}^{r}\frac { { {(n_i-n\hat{p}}_i)}^2}{n{\hat{p} }_i}\rightarrow X^2(r-1-s)
$$
其中：

s表示待估未知参数的个数，显然本项目中s=2；

hat(p)_i为任一样本落入第i部分的概率p_i的估计值，这是由于假定的分布类型中含有未知参数，因此需要先根据样本数据对未知参数进行极大似然估计，再代入得到概率的估计值。

然后，需要计算犯第一类错误的概率
$$
\alpha=P \left{H_1|H_0\right} = \left{T>c|\F(x)=F_0(x)\right}
$$
并根据α = δ = 0.05求得拒绝域中未知参数c的临界值criticalValue = 11.0705：

1
2
3
4
5
6

deta=0.05;
mu_MLE = overall_mean;
sigma2_MLE = overall_variance*(num_data_points-1)/num_data_points; 
sigma_MLE = sqrt(sigma2_MLE);
dimension=numBins-1-2;
criticalValue = chi2inv(1 - deta, dimension);

事实上，上文中提到Matlab软件中的var(A)函数默认按照N-1实现归一化，而正态分布方差的极大似然估计应为未修正的样本方差，故特此进行更正；同时，在2.2节的描述性统计分析过程中，涉及到频数直方图的绘制时已经对数据完成了分组，可直接通过读取上述的图表属性获取分组数numBins（对应上文中r）、各组频数binCounts（对应上文中实际频数n_i与各分组边界binEdges（可用于计算p_i））：

1
2
3

numBins = h.NumBins;
binCounts = h.Values;
binEdges = h.BinEdges;

最后，通过样本数据计算出对应卡方统计量T的观测kafang = 2.6897：

1
2
3
4
5
6
7
8
9

kafang=0;
p_hat(1)= normcdf(binEdges(2), mu_MLE, sigma_MLE);
for i=2:numBins-1
    p_hat(i)= normcdf(binEdges(i+1), mu_MLE, sigma_MLE)-normcdf(binEdges(i), mu_MLE, sigma_MLE);
end
p_hat(numBins)= 1-normcdf(binEdges(numBins), mu_MLE, sigma_MLE);
for i=1:numBins
    kafang=kafang+(binCounts(i)-num_data_points*p_hat(i))^2 / (num_data_points*p_hat(i));
end

由于kafang<criticalValue，即不位于拒绝域内，因此无法拒绝原假设H_0，即可以认为数据确实来自正态分布的总体，与直接调用Matlab内置库函数的结果一致。

总体均值检验

通过Pearson卡方检验，已经可以认为在显著性水平δ=0.05的情况下，总体近似服从正态分布。进一步地，还希望对该正态总体的均值参数μ进行假设检验。在2.2节的描述性统计分析过程中，已经计算得到样本的总体均值（实际上也是正态分布参数μ的无偏估计与最大似然估计）μ_0 约为15.1(mm)，因此接下来的假设检验过程就将其作为检验对象：

1

miu0=15.1;

在正态总体的均值检验过程中，主要采取t检验的方法，即认为正态总体的另一参数σ未知。这样的检验方法的合理性是显然的，此时需要构造服从t分布的统计量以进行t检验。

在Matlab软件中，对于t检验仍然有相对应的库函数ttest()可以调用：

1

[result1, ttest_p_value] = ttest(data(:),miu0);

函数ttest(data(:),miu0)使用单样本t检验返回原假设的检验决策，其原假设假定数据data来自均值等于输入参数miu0且方差未知的正态分布，而备择假设是总体分布的均值不等于miu0。如果该检验在默认为5%的显著性水平上拒绝原假设，则函数的返回值result1为1，即认为正态总体的均值不为miu0，否则为0；而另一返回值ttest_p_value则为该检验的p值，当其大于显著性水平时不拒绝原假设，即可认为正态总体的均值等于miu0。

运行代码可得：正态总体均值检验p值ttest_p_value=0.8687，可认为正态总体的均值约为μ_0=15.1。

同样的，这样的假设检验过程也可以基于一般的统计方法实现：

首先，提出原假设与备择假设：

• H_0：正态总体的均值为15.1，即μ=μ_0=15.1；
• H_1：正态总体的均值不为15.1，即μ ≠ μ_0。

接着，根据假设的提出给出对应的拒绝域形式：
$$
R_0 = \left{ (x_1, x_2,…,x_n)^T :|\bar{x}-\mu_0| >c \right}
$$
该拒绝域形式的构造是具有合理性的，因为显然可证明样本均值bar{x}是对于正态总体参数μ的无偏估计，若原假设H_0成立，则说明bar{x}应与设定检验值μ_0较为接近，反之可得其拒绝域应描述为bar{x}与μ _0相距较远的情形，基于这样的原理也可构造其他形式的拒绝域，这里只是给出一种可能性。

然后，需要计算犯第一类错误的概率
$$
\alpha=P \left{H_1|H_0\right} = \left{\bar{x}-\mu_0| >c|\mu=\mu_0\right}
$$
并根据α = δ = 0.05求得拒绝域中未知参数c的临界值tcriticalValue = 0.0765：

1
2
3
4

deta=0.05;
dimension=num_data_points-1;
s=sqrt(var(data(:)));
tCriticalValue = tinv(1 - deta/2, dimension)*s/sqrt(num_data_points);

上述计算操作的依据是，在对第一类错误发生概率α的计算过程中，可以根据抽样分布定理构造出对应的t统计量
$$
\frac{\bar{x}-\mu_0}{s/\sqrt{n}} \sim t(n-1)
$$
从而查表进行计算求解得到临界值
$$
c=\frac{t_{1-\frac{\delta}{2}}(n-1)*s}{\sqrt{n}}
$$
其中s为样本数据的标准差（修正后），n为样本容量num_data_points。

最后，前文已经计算出样本均值为bar{x}，若将其与待检验值μ_0之间的距离|bar{x}-μ _0|定义为参数d，则在判定是否处于拒绝域中时只需要比较d与临界值tcriticalValue即可：

1

d=abs(overall_mean-miu0);

计算得到d=0.0064，小于临界值tcriticalValue = 0.0765，即不位于拒绝域内，因此无法拒绝原假设，可认为正态总体的均值μ约为15.1。这同样与直接调用Matlab内置函数的结果相一致。事实上，只要μ_0在样本均值bar{x}=15.1064上下tcriticalValue = 0.0765的区间范围内，都可在显著性水平δ=0.05的情况下认为是正态总体的均值μ。

数据工序能力指数的计算与评估

在2.3节和2.4节中，通过假设检验的方法已经分别证明在显著性水平平δ=0.05的情况下，可以认为总体近似服从正态分布，且正态总体的均值μ约为15.1mm。基于类似的方法，不难得到正态总体的标准差σ约等于样本数据的标准差（修正）s=0.4319。基于这样的假设并通过假设检验验证其合理性之后，可以对于产品的生产质量利用数据工序能力指数等指标进行评估。

在工业生产中，为了综合表示工艺水平满足工艺参数规范要求的程度，通常借助于工序能力指数 (Process Capability index, Cp or Cpk) 来描述生产线是否具有较高的工艺水平能否生产出质量好的产品。潜在工序能力指数Cp的定义为：

$$
Cp=\frac{T_U-T_L}{6\sigma}
$$
其中T_U，T_L分别表示工艺参数规范的上限和下限。在本项目的问题背景之下，一般认为滚珠直径在14mm-16mm都可视为合理；但值得注意的是，在此定义中，隐含要求工艺参数分布中心μ与工艺规范要求的中心值T_0=(T_U+T_L)/2相重合。由2.4节的均值检验过程可知，μ约为15.1且上下限14和16的均值15不在可以通过假设检验的区间范围内，因此在此先进行近似处理，假定T_U=16.1，T_L=14.1，则可以计算出此时的潜在工序能力指数Cp=0.7717：

1
2
3
4
5

Tu=16.1;
Tl=14.1;
spec_limits = [Tl, Tu]; 
sigma = std(data(:)); 
cp = (spec_limits(2) - spec_limits(1)) / (6 * sigma);

而事实上，在原设定T_U=16，T_L=14的条件下，由于规范中心T_0=(T_U+T_L)/2与参数分布中心μ不重合，上述的近似处理并不能很准确在的反映其工序能力，此时则需要采用实际工序能力指数Cpk来度量工艺水平的高低：

$$
Cpk=\frac{T_U-T_L}{6\sigma}(1-K)
$$
其中K用于度量工艺参数分布均值μ对规范中心T_0的相对偏离度，即：

$$
K=\frac { {|\mu-T}_0|} { {(T}_U-T_L)/2}
$$

1
2
3
4
5
6

Tu=16;
Tl=14;
spec_limits = [Tl, Tu]; 
T0=(spec_limits(2) + spec_limits(1)) / 2;
k=abs(overall_mean-T0)*2/(spec_limits(2) - spec_limits(1));
cpk=(spec_limits(2) - spec_limits(1)) *(1-k)/ (6 * sigma);

计算得到：相对偏离度K=0.1064，实际工序能力指数Cpk=0.6896。其中相对偏离度K<1，说明均值未偏离到规范范围外，表明工艺加工结果合适，且实际工序能力指数Cpk>0，表明该工序具有一定的生产能力，但并未达到国际上的普遍要求Cpk>1.5，说明生产水平还存在相当大的提升空间。

除此之外，还可以计算单侧规范下的实际工序能力指数C_PU和C_PL:

若工艺参数只有上规范T_U的要求，则实际工序能力指数C_PU定义为：
$$
C_{PU}=\frac{T_U-\mu}{3\sigma}
$$
若工艺参数只有下规范T_L的要求，则实际工序能力指数C_PL定义为：
$$
C_{PL}=\frac { {\mu-T}_L}{3\sigma}
$$

1
2

cpu = (spec_limits(2) - mean(data(:))) / (3 * sigma);
cpl = (mean(data(:)) - spec_limits(1)) / (3 * sigma);

计算得到：实际工序能力指数C_PU=0.6896、C_PL=0.8538，也并未达到国际一般标准。

均值控制图与方差控制图

Shewhart控制图是实施SPC过程中判断生产过程是否处于统计受控状态的基本工具，其中使用最为广泛的为均值控制图和方差（标准差）控制图。通过连续采集工艺参数数据，可以利用Shewhart控制图来定量分析和度量生产线的运行过程处于统计受控状态。

均值控制图

在均值控制图中，包含有三条水平线，分别是：

• **UCL(Upper Control Limit)**：表示上控制限；

• **LCL(Lower Control Limit)**：表示下控制限；

• **CL(Center Line)/Avg(Average)**：表示中心线。

对于各批次的样本数据，用折线连接每批次的均值，从而得到反映不同批次数据均值波动情况的折线图。

对于服从正态分布总体N(μ, σ^2)的工艺参数而言，其取值落在(μ ± 3σ)区间范围内的概率为99.73%，因此可以采用如下的3σ准则来确定均值控制图的中心线以及上下控制限：

$$
UCL=\mu+3\sigma
$$

$$
CL=\mu
$$

$$
LCL=\mu-3\sigma
$$

* 我国标准”GB/T 4091常规控制图(2001版)”规定采用上述法则来计算控制限。

基于以上法则，可绘制均值控制图如下：

1
2
3
4
5
6
7
8
9
10

miu_hat=overall_mean;
sigma_hat=overall_variance;
figure;
plot(means, 'o-');
CL= miu_hat;
UCL=miu_hat+3*sigma_hat;
LCL=miu_hat-3*sigma_hat
yline(CL, 'r-', 'CL');
yline(UCL, 'r--', 'UCL');
yline(LCL, 'r--', 'LCL');

但在实际生产实践中，并不是所有的产品工艺指标都会近似服从正态分布。因此，在实际操作中，均值控制限常用如下公式估算：

$$
UCL=\hat{\mu}+A_s\bullet \hat{s}
$$

$$
CL=\hat{\mu}
$$

$$
LCL=\hat{\mu}-A_s\bullet \hat{s}
$$

其中，若采集工艺参数样本数据组数k=25，各批次数据数量n=5，令x_ij表示第i批次的第j个数据，bar{x_i}表示组内均值；则可用μ表示个各批次所有数据的样本均值，hat{s}表示各个批次数据标准偏差的算数平均值，即：

$$
{\bar{x}}i=\frac{1}{n}\sum{j=1}^{n}x_{ij}
$$

$$
\hat{s}=\frac{1}{k}\sum_{i=1}^{k}{\hat{s}}_i
$$

$$
\hat{\mu}=\frac{1}{k}\sum_{i=1}^{k}{\bar{x}}i=\frac{1}{kn}\sum{i=1}^{k}\sum_{j=1}^{n}x_{ij}
$$

$$
{\hat{s}}i=\sqrt{\frac{1}{n-1}\sum{j=1}^{n}{ {(x}_{ij}-{\bar{x} }_i)}^2}
$$

又查表[^2]可得：当每批次样本数据数n=5时，有A_s=1.427。

于是绘制出实际均值控制图：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18

figure;
plot(means, 'o-');
xi=mean(data, 2);
for i=1:num_groups
    sum=0;
    for j=1:samples_per_group
        sum=sum+((data(i,j)-xi(i))^2);   
    end
    si_hat(i)=sqrt(sum/(num_groups-1));
end
s_hat=mean(si_hat);
As=1.427;
CL= miu_hat;
UCL=miu_hat+s_hat*As;
LCL=miu_hat-s_hat*As;
yline(CL, 'r-', 'CL');
yline(UCL, 'r--', 'UCL');
yline(LCL, 'r--', 'LCL');

方差控制图

方差控制图的结构与均值控制图非常类似，也是由中心线以及上下控制限构成。只是图中的数据点描述的每批次数据的标准偏差，因此可用于定量判断不同批次数据的标准偏差(数据的波动性)在变化中是否存在”异常因素”。

类似于均值控制图，在实际操作中，常用如下公式估算其中心线以及上下控制限：

$$
UCL=B_U\bullet\ \hat{s}
$$

$$
CL=\hat{s}
$$

$$
LCL=B_L\bullet\hat{s}
$$

其中B_U、B_L为依赖于每批次样本数据数n的常数参数，查表[^2]可得：当每批次样本数据数n=5时，有B_U=2.089、B_L=0。

据此可绘制出对应的方差控制图：

1
2
3
4
5
6
7
8
9
10
11

Bu=2.089;
Bl=0;
figure;
plot(variances, 'o-');
hold on;
UCL=(Bu*s_hat);
CL=(s_hat);
LCL=(Bl*s_hat);
yline(CL, 'r-', 'CL');
yline(UCL, 'r--', 'UCL');
yline(LCL, 'r--', 'LCL');

产品质量分析结论

为根据Shewhart控制图判断产品生产过程是否受控，可对于均值控制图与方差控制图分别采用如下八条常见规则（规则均不能触碰，否则视为不受控），以识别产品生产过程是否仅受到随机因素影响：

• 规则 1:控制图上有一个点(对应某个批次数据)位于控制限以外；

• 规则 2:连续9个点落在中心线同一侧；

• 规则 3:连续6个点递增或者递减；

• 规则 4:连续14个点交替上下；

• 规则 5:连续3个点中有2个点落在中心线同侧的B区以外；

• 规则 6:连续5个点中有4个点落在中心线同侧的C区以外；

• 规则 7:连续15个点落在中心线两侧的C区；

• 规则 8:连续8个点落在中心线两侧且无一点在C区以内。

其中，A、B、C分区如下图^^[@2]^^所示，C区为以CL控制限为对称轴的中心区域，B、A两区向两侧延伸至上下控制限UCL与LCL。

实际上，Matlab软件中对于Shewhart控制图以及SPC统计过程控制方法有一套完整封装的库函数controlchart()，调用结果如下所示：

1
2

load parts
[st,plotdata] = controlchart(data,'charttype',{'xbar' 's'});

其中第一张图为均值控制图，第二张图为标准差控制图。

通过观察图像，基于八大规则进行判断并通过查看返回对象plotdata的ooc属性（Logical that is true for points that are out of control）可知，该产品生产过程确实处于统计受控状态。除此之外，样本均值也在15.1mm左右，符合对于滚珠直径的一般行业要求，工艺水平良好；但实际工序能力指数Cpk仅有0.6896，说明工艺水平还有上升空间。

参考文献

[^1]: 茆诗松,程依明,濮晓龙编著. 概率论与数理统计教程[M]. 高等教育出版社, 2019.
[^2]: 贾新章 .. [等] 编著. 统计过程控制理论与实践[M]. 电子工业出版社, 2017.

一、串联式机械臂概括

1.应用领域总结分析

近年来，随着工业自动化水平的飞速发展，工业机器人在现代机械制造中发挥着越来越重要的作用，而串联机械臂作为工业制造领域的关键载体对航天、汽车等制造领域正起着越来越重要的作用，尤其在复杂和环境恶劣的作业条件下，更扮演着不可替代的角色。串联机械臂作为一种多学科高度综合的产品，集成了机械、计算机电子、自动控制理论等于一体，其技术水准直接反映了国家工业制造水平，目前广泛应用于汽车及汽车零部件制造业、机械加工行业、木材与家居制造业等领域，提高了各加工制造业的作业效率。比如焊接机器人在机械加工领域中代替传统的人工焊接，不仅提高了焊接效率和焊接精度，而且解决了工人在焊接恶劣条件的安全问题。

由于具有较高的灵活性和多自由度，串联机械臂在许多领域都有广泛应用：

（1）自动化生产线：串联机器人在汽车制造、电子产品组装、食品加工等自动化生产线上发挥重要作用。它们可以完成精细的装配、焊接、喷涂等工作，提高生产效率和产品质量。

（2）医疗和健康：串联机器人在手术、康复辅助和医疗器械制造等领域得到应用。它们可以进行精确的手术操作、康复训练和辅助活动，提高手术精度和患者疗效。

（3）仓储和物流：串联机器人在仓储和物流领域可以完成货物的搬运、分拣和装载等任务。

（4）精密加工：串联机器人在精密加工领域扮演着重要角色，如机械零件加工、精密雕刻和模具制造等。其高精度和灵活性使其能够进行微小尺寸和复杂形状的加工操作。

（5）实验室研究：串联机器人在科学研究和实验室应用中具有广泛的应用。例如，在化学实验、材料测试和生物医学研究中，串联机器人可以进行精确的液体分注、样品处理和实验操作。

（6）危险环境：串联机器人可以在危险环境中代替人工进行操作，如核能厂、爆炸物处理和深海勘探等。它们能够承担高温、高压、有毒或放射性环境下的任务，保护人类的安全。

（7）娱乐和艺术：串联机器人还在娱乐和艺术领域展现出其创造力和表演能力。例如，在舞台表演、电影特效和艺术创作中，串联机器人可以呈现出流畅的舞蹈动作、惊人的表演和艺术装置。

2.机械臂技术研发需求

随着中国工业机器人市场需求的不断增加，开发完全自主的工业机器人控制系统具有不可替代的意义。目前衡量工业机器人性能的重要标准是运行高速以及加工高精度，而这些也是市场非常看重的。其中工业机器人的运动精度和平稳性的基础和保障是平滑的位姿运动，同时速度规划算法的选择决定了机器人运动精度和平滑性，因此研究机器人的前瞻算法和轨迹规划控制算法对于提高机器人的控制精度和运行效率具有重要意义。

（1）机器人轨迹光顺技术

轨迹光顺是机器人轨迹规划中一个不可或缺的部分，通过各种曲线对运行路径进行近似，在满足轨迹弓高误差的情况下，生成新的运动轨迹，消除曲率和切向的不连续性，满足轨迹连续性要求，目前在三轴和五轴机床上应用非常广泛。

（2）机器人奇异位形规避技术

奇异位形是指机构在运动过程中机构的运动学、动力学性能发生瞬间突变，机构处于死点或者自由度减少，使得机构运动能力失常。串联机器人的运动奇异性由其串联机构所决定的，无法消除，产生的不良影响主要表现在两个方面：①自由度减少；②某些关节角速度趋向无穷大，引起机器人失控。

（3）机器人同步前瞻技术

速度前瞻是数控加工技术中的重要一环，基本思想是通过预读一段待运行路径，判断该路径上的高曲率约束点和危险点，提前进行速度规划，保证机床末端运行至危险点之前能够降速至合适速度，平稳过渡危险点后在加速至正常速度。

（4）机器人速度规划和插补技术

速度规划和插补是机器人运动控制中不可缺少的一部分，已知当前段始末点速度和位移，通过速度规划计算出当前段的运动时间，然后插补出每一循环周期的位移。常见的速度规划算法有直线加减速，也被称为梯形加减速，Ｓ曲线加减速，修正梯形加减速，指数型加减速，三角函数加减速等。

二、串联机械臂机构分析

1.机械臂结构分析

通过搭建组装机械臂平台，分析其基本结构与传动方式，并对部件具体尺寸进行测量，可以得到如下的机构简图，并建立对应的工具坐标：

其中，经过多次测量取平均值，测得如下物理结构参数（图中已标注)：

表1：机械臂物理参数

2.机械臂正运动学模型

基于上述坐标系，进一步分析其传动关系与结构，得出如下D-H参数表：

表2：D-H参数表

通过如下变化通式，可以在各连杆的关节坐标系之间进行变换：

代入D-H表中对应的参数值即可计算得出对应的位姿矩阵，表中的θ_i（i=1,2,3,4,5）即表示舵机转角（最后一个舵机仅控制夹子夹紧，不影响总体位姿，默认为夹紧状态），在机械臂自身物理参数已经确定的情况下，一旦给定各舵机转角，机械臂末端在全局坐标系中的坐标也可立即得到。在本项目的六轴机械臂模型中，共可建立六个坐标系，得到六个相邻坐标系之间的位姿矩阵T；将六个位姿矩阵按顺序依次右乘，即可得到机械臂末端相对于0坐标系（即全局坐标系）的位姿矩阵:

该矩阵中前三行的最后一个元素分别代表了机械臂末端在全局坐标系中的x,y和z坐标。依据这样的原理，通过Matlab编程，可以实现正运动学的计算（附件1），例如设定五个舵机转角分别为0°、-45°、45°、45°、0°，可以计算得出如下的位姿矩阵：
进一步可以绘制出此时各机械臂关节的位姿图，与实际控制结果进行比照：

3.机械臂逆运动学求解

通过观察与分析机械臂结构可以发现，由于θ5只影响末端夹子的转动，θ6只影响末端夹子夹紧的程度，即在固定θ5=0°、θ6=90°（夹子夹紧）时，机械臂的位姿与末端坐标只受θ1、θ2、θ3、θ4的控制，故只需求解该四者即可。

（1）求解θ1

观察连杆简图（上左图），可以发现: θ1为机械臂在x-y平面中的投影与x轴的夹角，即只与机械臂末端的x与y坐标有关：θ1=arctan(yP*/x*P)。

（2）求解θ2、θ3、θ4

进一步观察机械臂所在平面（上右图）：将原坐标系中的z轴作为纵坐标（y’ = z），将原x轴与y轴映射到机械臂所在平面内作为横坐标（x’ = sqrt(x^2+y^2))；利用几何法，建立方程组对θ2、θ3、θ4分别进行求解。

在求解公式推导的过程中，默认末端机械臂与水平面的夹角α已知，在给定机械臂末端坐标（x,y,z）的情况下，可解得如下结果（必须求出解析解，若将方程组输入Matlab调用求解器求解，可能会出现计算无解但实际有解的情况）：

其中：

事实上，这样求解得到的结果往往不唯一，为使得机械臂在各位姿转换的过程中更加直接迅速且满足机械结构约束，还需满足如下约束条件：

【1】求解位置不能低于底座：

【2】三角形ABC存在的几何约束

【3】三角形ACC’存在的几何约束

【4】运动连续性约束：

【5】角度范围约束：

实际编写Matlab程序进行求解时，末端机械臂与水平面的夹角α在不同位姿下会发生改变，因此需要在编程时对其在一定范围（-45°到90°之间）内进行遍历，当有合适的满足约束条件的解时输出对应的解并停止循环。

在Matlab程序（附件2）中，将目标机械臂末端坐标[x,y,z]设置为以IN_theta=[0,-45,45,45,0]作为输入的正解结果[300.9396, 0, 97.5528]，得到的逆解结果与IN_theta完全一致，正确性得到验证。

三、串联机械臂控制技术分析

1.电机特性分析

从本质上来说，电机属于一种能量转换装置，其目的是希望将电能转换为动能（机械能），但在实际的工作过程中这样的转换效率永远不会是100%，而是由于电机线圈的欧姆加热，会有相当部分的能量以热量形式散失；但总而言之，其工作过程本身仍然满足能量守恒定律，即其将输入的电能P_elec转换为输出的机械能P_mech与散失的热能P_heat：

图6：电机理论模型

将其改写成电气和机械量：

其中v_m为电机端子两端的电压，i是流过电机的电流，τ是电机产生的扭矩，ω是其角速度。

事实上，除此之外，电机内的线圈除具有电阻R之外，还具有电感L。加入电感项后，再将两边同时对电流i求导，可以得到电机的定义方程：

其中v_emf为反电动势，与电机转速n成正比，即（其中常数k_v取决于电机设计）:

电机产生的转矩与通过线圈的电流成正比，即：

但实际的输出扭矩还需要在此基础上减去负载扭矩与电机轴摩擦扭矩（与电机轴转速成正比，比例系数为电机自身特性)。

基于上述这些公式即可对于一个电机的特性与性能进行相对完整描述，通过对于上述公式进行变形和转换还可以得到电机的速度-扭矩曲线，从而得到失速扭矩与空载速度，为我们更好地把握电机的大致性能提供参考。

为了后续更精准地对电机进行控制（如调节PID参数等），可以在Simulink中对电机的基本物理特性进行仿真建模：

该模型的输入值为电机电压和负载扭矩，输出值为电机转角、电流与转速，能够很好地描述一个电机的物理特性，可以将其封装成单独的电机模块，并利用PID等控制模块实现对于该电机模块的控制。

不同的电机由于其结构、材料等的不同，性能也会有所差异，具体体现在不同的结构和材料对于上述公式中所涉及到的比例系数都有不同程度的影响，因而能够影响电机的速度-扭矩特性曲线。在对电机进行选型时，厂家往往不会直接给出这些系数，而是给出空载速度等参数以供参考，因此更需要清楚了解这些特性参数所代表的物理意义，从而更好地针对需求进行合适的选型。

2.电机控制策略与PID特性分析

PID控制器是工业过程控制中广泛采用的一种控制算法，其特点是结构简单灵活、技术成熟、适应性强。在有一定精度和灵敏度要求的电机控制过程中，往往也偏向于采用PID控制器实现电机控制。

P、I、D分别为比例（Proportion）、积分（Integral）、微分（Differential)的简写；将偏差的比例、积分和微分通过线性组合构成控制量，用该控制量对受控对象进行控制，称为PID算法，本质上是对于偏差的控制算法。PID通过对输入的偏差进行比例积分微分运算，将其叠加结果用于控制执行机构。在工程实践中，一般P是必须的，衍生出有PI、PD、PID等多种PID控制器。

连续状态下，PID控制器的计算公式如下：

（1）比例部分

作用是对系统瞬间产生的偏差进行快速修正，只有P时会有静差。偏差一旦产生，控制器立即产生控制作用，使控制量向减少偏差的方向变化。比例系数K_p越大，控制作用越强，则过渡过程越快，控制过程的静态偏差也就越小；但是越大，也越容易产生震荡，破坏系统的稳定性。因此，比例系数必须选择恰当，才能达到过渡时间少，静差小且稳定的效果。

（2）积分部分

作用是消除系统偏差，补偿系统的静态误差，只要存在偏差，则其控制作用就不断增加；只有在无偏差时，其积分值才为常数，控制作用才是一个不会增加的常数。积分环节的调节作用虽然会消除静态误差，但也会降低系统的响应速度，增加系统的超调量。积分常数越大，积分的积累作用越弱，这时系统在过渡时才不会产生震荡；但增大积分常数会减慢静态误差的消除过程，消除偏差所需的时间较长，但可以减少超调量，提高系统的稳定性。

（3）微分部分

作用是加快调节过程，阻止偏差的变化。偏差变化的越快，微分控制器的输出就越大，并能在偏差值变大之前进行修正。微分作用的引入，将有助于减少超调量，克服震荡，使系统趋于稳定，特别对高阶系统非常有利，它加快了系统的跟踪速度。但微分的作用对输入信号的噪声很敏感，对那些噪声较大的系统一般不用微分，或在微分起作用之前先对输入信号进行滤波。

总体而言，在PID控制器参数选择的过程中，一般遵循如下步骤顺序：

【1】确定比例系数Kp

【2】确定积分时间常数Ti

【3】确定微分时间常数Td

【4】系统空载、带载联调

3.嵌入式控制系统总结分析

嵌入式系统是以应用为中心，以现代计算机技术为基础，能够根据用户需求(功能、可靠性、成本、体积、功耗、环境等)灵活裁剪软硬件模块的专用计算机系统。嵌入式系统的硬件和软件必须根据具体的应用任务，以功耗、成本、体积、可靠性、处理能力等为指标来进行选择。嵌入式系统的核心是系统软件和应用软件，由于存储空间有限，因而要求软件代码紧凑、可靠，且对实时性有严格要求。目前常见的嵌入式控制开发板有C51，STM32，Arduino，树莓派等。

本项目中使用的嵌入式控制系统基于Arduino Uno开发板搭建。作为一款便捷灵活、方便上手的开源硬件产品，其具有丰富的接口，有数字I/O口，模拟I/O口，同时支持SPI,IIC,UART串口通信，能通过各种各样的传感器来感知环境，通过控制灯光、马达和其他装置来反馈、影响环境。它没有复杂的单片机底层代码，没有难懂的汇编，只是简单而实用的函数。而且具有简便的编程环境IDE，极大的自由度，可拓展性能非常高，同时其标准化的接口模式也为它的可持续发展奠定了坚实的基础，但其低廉的价格也意味着性能并不尽如人意。

4.传感系统总结分析

传感器是能感受到被测量的信息，并能将感受到的信息，按一定规律变换成为数字/模拟信号或其他所需形式的信息输出，以满足信息的传输、处理、存储、显示、记录和控制等要求的检测装置。传感器具有微型化、数字化、智能化、多功能化、系统化、网络化等特点，它是实现自动检测和自动控制的首要环节。

传感器一般由敏感元件、转换元件、变换电路和辅助电源四部分组成：敏感元件直接感受被测量，并输出与被测量有确定关系的物理量信号；转换元件将敏感元件输出的物理量信号转换为电信号；变换电路负责对转换元件输出的电信号进行放大调制；转换元件和变换电路一般还需要辅助电源供电。

按照其测量的物理量，传感器可大致分为如下种类：

【1】力学量传感器：包括压力传感器、力传感器、位移传感器、加速度传感器、速度传感器、转速传感器等，用于测量力、压力、运动等力学参数。

【2】热学量传感器：热敏电阻/热电偶/红外温度传感器等，用于测量温度。

【3】光学量传感器：如光电二极管、CCD/CMOS图像传感器等，用于检测光强度、颜色、图像信息。

【4】声学量传感器：麦克风，用于声音、噪声的检测。

【5】磁学量传感器：如霍尔效应传感器、磁敏电阻，用于测量磁场强度、磁通量。

【6】化学量传感器：如电化学传感器、气体传感器，用于检测特定气体浓度、pH值、离子浓度等。

本项目中用到的传感器主要有三类：

（1）红外光学传感器：不断发出红外线并通过检测返回的红外线强弱返回不同的电信号，可用于区分亮处/暗处，通过与对应的控制算法相配合可以使小车实现沿黑线完成循迹任务；

（2）超声波测距传感器：不断向前方发出超声波并接收反射回的超声波，通过检测发送与接收同一超声波信号的时间间隔，计算得出前方物体与自身的距离，进行距离的判断，可配合相应控制算法实现对应功能；

（3）视觉传感器：采用Openmv视觉摄像头模块，对摄像头采集的图像进行实时的处理，需要配合编写对应的识别算法并烧录进去，并将识别结果以数字信号的形式反馈到指定的引脚上，与主控制板实现通信，从而根据不同的识别结果执行对应命令。

四、综合实践环节报告

1. 任务目标与任务分解

任务目标：

任务分解：

总体而言，可以将整个项目分解成如下模块：

（1）利用红外传感器模块，实现对于黑线道路的循迹；

（2）利用超声波测距模块与视觉模块，在对应的任务点停下，并根据视觉识别结果执行对应的任务；

（3）在A点处，根据识别到的物块颜色判断应进行左转还是右转；在B点出，根据识别到的物块大小，控制机械臂沿不同的指定轨迹完成运动（写字）。

简而言之，任务主要分为循迹、视觉识别与机械臂控制运动三个模块。

2. 本人负责部分的实现过程

（1）任务描述

我主要负责的任务板块是最终B点处的写字任务，即需要在接收到Openmv模块识别反馈的物块大/小对应的电平信号后，执行对应的机械臂运动，最终达成在墙上写数字”1”或”2”的效果；由于该实现过程中大量涉及到对于Arduino编写C++程序并烧录以实现舵机控制的内容，同时还需要与Openmv视觉模块进行交互，因此我也一并负责了整个测试小车行进业务逻辑的编写与整合工作，将各个模块整合在一起。

（2）技术路径与策略

在之前的机械臂控制（正/逆解）过程中，由于不涉及到写字任务这一具体情境，只是对于实验平台基本的物理参数进行了测量；但在写字任务中，机械臂末端的夹子需要夹取白板笔，用白板笔的尖端在墙壁上完成写字任务，因此需要对于机械臂的L4参数进行重新测量，端点由夹子末端更改至白板笔的末端，测得新的L4为250mm。

针对写”1”和”2”两个不同的字符，有不同的定位方式：如写”1”只需定位上端点，中间点与下端点即可，而写”2”则根据字形需要定位6个点才能完成任务（为简化控制流程，将各控制点之间的距离设置的较小从而不需要在控制点之间再进行直线插补，目标是仅需要完成字形而并不追求大小）。附件3中的综合控制代码中给出了这些控制点的坐标，在此不详细赘述。事实上，这些点坐标的确定还与小车在停下识别时与墙壁间的距离（即超声测距决定停下识别时的临界距离）有关，实际在反复多次的测试中磨合得出。

为使得机械臂控制流程更加流畅和自然，在基于Matlab实现机械臂正逆解算的代码后，将该算法移植至C++中，并以类的形式封装（名为Arm）,一个Arm对象即对应一个位姿，其构造时需要输入机械臂的结构参数（L1,L2,L3,L4）与希望机械臂末端到达的点坐标，当再次调用其setup()方法时，会自动根据该目标坐标进行逆解算，并控制舵机按照解算出的角度转动，实现控制效果。在舵机控制时，综合了测量出的转动误差，对于各舵机的转动角度都有对应的不同的比例系数实现误差补偿。

（3）核心程序逻辑

最终完整项目的代码详见附件3，在此仅简要阐述其逻辑。

小车的默认状态为以循迹方式前进，即执行follow()函数；在行进过程中，始终构造一个Arm对象（在armset()函数中），控制机械臂保持一个相对合适的姿态（坐标280，0，320）；当超声波测距返回值小于19时，说明到达了A点，需要进行第一次识别，此时会反馈给Openmv一个高电平，告诉视觉模块需要开始识别颜色；视觉模块在识别完成后通过另一个引脚将识别结果以高/低电平形式反馈给主控Arduino开发板，主板在接收到对应信号后会对应做出向左/向右转的行动（执行turningleft()或turningright()）函数，并在随后的过程中继续完成循迹；在T字路口处，由于我们组并未采取视觉辅助循迹的解决方案，因此只能采取”背板”的方式，在A点处转弯后开始计时，当继续循迹达到30s左右时，根据前面判断执行的左转/右转指令对应再次执行左转/右转；但实际操作过程中”背板”往往无法在路口精准转弯，因此又补充了新的逻辑，在时间差不多的时候提前大角度拐弯并一直沿直线行驶，直到再次回到黑线上就继续循迹，这样就可以绕过T字路口；理想状态下，通过T字路口后再行驶短暂的一段距离后，应该就到达了需要写字的B点，此时与A点处处理类似，当超声测距返回值小于17时停下，通过Openmv模块识别红色物块的大小，并执行对应的机械臂运动操作（写”1”或”2”）。

（4）实现的实际效果

上述操作逻辑仅仅在理论上可行，而实际测试时，由于没有采用视觉循迹方案，在T字路口屡败屡战，屡战屡败，最终在多次尝试后仍然没有进入T字弯后的B点写字区域；在第二次尝试过程中，由于个人的失误，在补充了绕过T字弯的逻辑之后忘记补充循迹逻辑，导致在到达A点后失去循迹功能，最终宣告失败。但总体而言，除了T字弯之外，其他所有功能的实现都非常良好，不论是前段的循迹，把速度稍微压下来一点但是比较稳不会跑出去，还是A点的颜色判断也非常准确而迅速。在私下练习测试时，物块大小识别与写字功能也非常流畅，正确率极高，只是可惜最后没有办法展示出来了。

4. 心得体会

其实整个课程与项目过程中的心路历程和工作内容在上面已经基本讲完了，因为我没有打过机器人相关的竞赛，相比于其他同学而言少了很多这方面的经验，不论是这次课程中涉及到的机械臂正逆解算与控制也好，还是视觉模块的引入也好，甚至是超声波测距模块其实对我来说都是初次接触，都是一个迅速的从陌生到熟悉的过程，这个成长过程很突然但也很迅速，效果是拔群的。在后半学期的实践阶段，我一直在努力去主导我们整个组进度的推进，之前课上一些小的checkpoint也都顺利通过了；最终测试中也基本是我在主导整个组进度的推进，以及各模块任务的分配与整体业务逻辑的串联，而且由于我一直在跟随堂测试的机械臂控制部分同时也想充分利用我Matlab代码编写与数据处理计算的优势，我在最终测试中还主要负责了机械臂的控制部分（写字），巧妙利用C++面向对象的编程思想，大大简化了控制流程。但可惜由于我们组经验和技术水平比较有限，没有及时认识到视觉循迹的必要性，导致最终在T字路口功亏一篑，甚至使得最早调完的写字部分前功尽弃，最后一次测试本来调整了逻辑企图通过”歪门邪道”绕过T字弯（纯背板真的太难卡准了），但是由于个人失误导致在A点就提前结束了战斗，也是留下了一些遗憾吧。但总体来说，整个过程给我带来的成长和提升是显而易见的，我开始对于机器人、对于硬件慢慢地从心理上去接受这个事情并且甚至一度乐在其中，也学到了机械臂控制、电机控制等方面的一些知识，课上第一个把电机Simulink模型搭对确实给我增长了很多信心。我们组在编程的过程中除了直接移植部分模块外几乎没有用过Mixly图形化编程，而是直接使用Arduino IDE（虽然这好像也是最后逻辑不清晰留下遗憾的原因之一），这有效地提高了我们的开发效率。这一次从装车开始，到一步步熟悉起小车和开发板的每一个部分，看着程序从一行两行到最后近千行，整体功能不断扩充从一开始只有机械臂位姿控制，到加上循迹，加上与Openmv模块的通信逻辑，以及最后面对T字路口的垂死挣扎，其实在准备过程中也尝试去做视觉辅助循迹，但由于场地各方面条件限制（场地太拥挤，调试的人太多导致环境光源不稳定；摄像头安装位置不合适等），最终还是放弃了视觉循迹方案，选择背板赌一把，很可惜没有成功。

希望在以后的学习实践中，能够充分吸取本次项目的经验，利用好这次学到的理论知识，同时也在做事的过程中汲取这次的经验教训，我相信努力的过程与收获比结果更重要，当然如果能有好的结果也再好不过。感谢这门课一路上三位任课老师与助教学长的辛勤付出，是你们的倾情指导加速了我们成长的速度。

千言万语诉不尽收获与感激。最后，感谢一直努力的自己，以及一直陪伴的老师和朋友。

附件1：Matlab正运动学解算代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58

clear;clc;

%赋予输入值
L1=152;
L2=105;
L3=98;
L4=182;
IN_theta=[0,-45,45,45,0];

%建立D-H参数表
C_a=[0,0,L2,L3,0,0];
C_d=[0,L1,0,0,0,0,L4];
C_alpha=[0,-90,0,0,-90,0];
C_theta=[0,IN_theta(1),IN_theta(2),IN_theta(3),IN_theta(4)-90,IN_theta(5),0];

i=1;%i-1=0
T0_1=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);
    cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));
    sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));
    0,0,0,1];
i=2;
T1_2=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=3;
T2_3=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=4;
T3_4=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=5;
T4_5=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];
i=6;
T5_6=[cosd(C_theta(i+1)),-sind(C_theta(i+1)),0,C_a(i);cosd(C_alpha(i)).*sind(C_theta(i+1)),cosd(C_alpha(i)).*cosd(C_theta(i+1)),-sind(C_alpha(i)),-C_d(i+1).*sind(C_alpha(i));sind(C_alpha(i)).*sind(C_theta(i+1)),sind(C_alpha(i)).*cosd(C_theta(i+1)),cosd(C_alpha(i)),C_d(i+1).*cosd(C_alpha(i));0,0,0,1];

x0=[0;0;0];
x1=T0_1(1:3,4);
T0_2=T0_1*T1_2;
x2=T0_2(1:3,4);
T0_3=T0_1*T1_2*T2_3;
x3=T0_3(1:3,4);
T0_4=T0_1*T1_2*T2_3*T3_4;
x4=T0_4(1:3,4);
T0_5=T0_1*T1_2*T2_3*T3_4*T4_5;
x5=T0_5(1:3,4);
T0_6=T0_1*T1_2*T2_3*T3_4*T4_5*T5_6;
x6=T0_6(1:3,4);
% 将点的坐标分别存储在 x, y, z 向量中
x = [x0(1), x1(1), x2(1), x3(1), x4(1), x5(1), x6(1)];
y = [x0(2), x1(2), x2(2), x3(2), x4(2), x5(2), x6(2)];
z = [x0(3),x1(3), x2(3), x3(3), x4(3), x5(3), x6(3)];

% 绘制三维线
figure; % 创建一个新的图窗
plot3(x, y, z, '-o'); % 使用 '-o' 绘制带有圆圈标记的线
grid on; % 打开网格
xlabel('X-axis');
ylabel('Y-axis');
zlabel('Z-axis');
title('3D Line Plot Connecting Points');

T0_6

附件2：Matlab逆运动学解算代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40

clear;clc;
% 初始化标志变量
solution_found = false;
x = 300.9396;  
y = 0;
z = 97.5528;
theta1=rad2deg(atan(y/x));
disp(theta1);
L1 = 152;  L2 = 105;  L3 = 98;   L4 = 182;  
% 遍历 alpha 的值，从 -45 到 90
for alpha = -45:90
    t = sqrt(x^2 + y^2);
    m=z-L1-L4*sind(alpha);
    n=t-L4*cosd(alpha);
    theta3=acosd((m^2+n^2-L2^2-L3^2)/(2*L2*L3));
    A=L3*sind(theta3);
    B=L2+L3*cosd(theta3);
    theta2=asind(n/sqrt(A^2+B^2))-acosd(A/sqrt(A^2+B^2));
    if theta2<0
        theta2=180-asind(n/sqrt(A^2+B^2))-acosd(A/sqrt(A^2+B^2));
    end
    theta4=theta2-theta3-alpha;
    % 检查是否找到解
    if L4*sind(alpha)<z && L3*cosd(theta2-theta3)>0 && theta2>0 && theta2<90 && theta3>0 && theta3<90 && theta4>0 && theta4<90 
        % 记录找到解的标志
        solution_found = true;
        break;
    end
end
if solution_found
    % 显示结果
    disp('Solution found:');
    disp(['Alpha: ', num2str(alpha)]);
    disp('Theta1:');    disp(theta1);
    disp('Theta2:');    disp(-theta2);
    disp('Theta3:');    disp(theta3);
    disp('Theta4:');    disp(theta4);
else
    disp('No solution found in the given alpha range.');
end

附件3：项目测试完整Arduino控制代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553

#include <math.h>
#include <stdio.h>
#include <Servo.h>

Servo servo_7;
Servo servo_3;
Servo servo_5;
Servo servo_6;
Servo servo_9;

const double L1 = 152;
const double L2 = 105;
const double L3 = 98;
const double L4 = 250;

volatile int item;

int flag_cp1=0;
int flag_cp2=0;

unsigned long currentTime;
unsigned long elapsedTime;
unsigned long startTime;

char cmd_return_tmp[64];

int color;
int size;

struct Angles 
{
    double theta1;
    double theta2;
    double theta3;
    double theta4;
};

struct Points 
{
    double x1;
    double y1;
    double z1;
};

class Arm 
{
  public:
      Arm(double L1, double L2, double L3, double L4, Points terminal) 
      {
          this->L1 = L1;
          this->L2 = L2;
          this->L3 = L3;
          this->L4 = L4;
          this->x0 = terminal.x1;
          this->y0 = terminal.y1;
          this->z0 = terminal.z1;
          Angles angles=inverseKinematics(this->x0,this->y0,this->z0);
          this->theta1=angles.theta1;
          this->theta2=angles.theta2;
          this->theta3=angles.theta3;
          this->theta4=angles.theta4;
          Points point=forwardKinematics(this->theta1,this->theta2,this->theta3,this->theta4);
          this->x1=point.x1;
          this->y1=point.y1;
          this->z1=point.z1;
      }

      void run() 
      {
          double oc1=(140.25+1.12*this->theta1)*2/3;
          double oc2=(180+1.13*this->theta2)*2/3;
          double oc3=(142.5+1.07*this->theta3)*2/3;
          double oc4=(132+1.12*this->theta4)*2/3;
          servo_7.write(oc1);
          servo_3.write(oc2);
          servo_5.write(oc3);
          servo_6.write(oc4);
          servo_9.write(96);
          //servo_8.write(270);
          Serial.print("x1: "); Serial.println(this->x1);
          Serial.print("y1: "); Serial.println(this->y1);
          Serial.print("z1: "); Serial.println(this->z1);
          Serial.print("terminalx1: "); Serial.println(this->x0);
          Serial.print("terminaly1: "); Serial.println(this->y0);
          Serial.print("terminalz1: "); Serial.println(this->z0);
      }

  private:
      double L1;
      double L2;
      double L3;
      double L4;
      double x0;
      double y0;
      double z0;
      double theta1;
      double theta2;
      double theta3;
      double theta4;
      double x1;
      double y1;
      double z1;
      bool solution_found = false;
      Angles inverseKinematics(double x0, double y0, double z0) 
      {
          Angles angles;
          double theta1 = atan2(y0,x0) * 180.0 / PI;
          angles.theta1=theta1;

          for (int alpha = -45; alpha <= 90; alpha++) 
          {
              double t0 = sqrt(x0 * x0 + y0 * y0);
              double m = z0 - this->L1 - this->L4 * sin(alpha * PI / 180.0);
              double n = t0 - this->L4 * cos(alpha * PI / 180.0);

              double cos_theta3 = (m * m + n * n - this->L2 * this->L2 - this->L3 * this->L3) / (2 * this->L2 * this->L3);
              if (cos_theta3 < -1.0 || cos_theta3 > 1.0) 
                  continue;
              double theta3 = acos(cos_theta3) * 180.0 / PI;
              double A = L3 * sin(theta3 * PI / 180.0);
              double B = L2 + L3 * cos(theta3 * PI / 180.0);
              double sin_theta2_part = n / sqrt(A * A + B * B);
              double cos_theta2_part = A / sqrt(A * A + B * B);

              if (sin_theta2_part < -1.0 || sin_theta2_part > 1.0 || cos_theta2_part < -1.0 || cos_theta2_part > 1.0) 
                  continue;

              double theta2 = asin(sin_theta2_part) * 180.0 / PI - acos(cos_theta2_part) * 180.0 / PI;
              if (theta2 < 0) 
                  theta2 = 180.0 - asin(sin_theta2_part) * 180.0 / PI - acos(cos_theta2_part) * 180.0 / PI;
              double theta4 = theta2 - theta3 - alpha;

              if (this->L4 * sin(alpha * PI / 180.0) < z0 && this->L3 * cos((theta2 - theta3) * PI / 180.0) > 0 && theta2 >= 0 && theta2 <= 90 && theta3 >= 0 && theta3 <= 90 && theta4 >= 0 && theta4 <= 90) 
              {
                  solution_found = true;
                  angles.theta2=-theta2;
                  angles.theta3=theta3;
                  angles.theta4=theta4;
                  break;
              }
          }
          if (solution_found)
            return angles;
      }

      Points forwardKinematics(double theta1, double theta2, double theta3, double theta4) 
      {
          Points point;
          double IN_theta[] = {theta1, theta2, theta3, theta4, 0};

          // 定义DH参数
          double C_a[] = {0, 0, this->L2, this->L3, 0, 0};
          double C_d[] = {0, this->L1, 0, 0, 0, 0, this->L4};
          double C_alpha[] = {0, -90, 0, 0, -90, 0};
          double C_theta[] = {0, IN_theta[0], IN_theta[1], IN_theta[2], IN_theta[3] - 90, IN_theta[4], 0};

          // 计算变换矩阵
          double T0_1[4][4], T1_2[4][4], T2_3[4][4], T3_4[4][4], T4_5[4][4], T5_6[4][4];
          double T0_2[4][4], T0_3[4][4], T0_4[4][4], T0_5[4][4], T0_6[4][4];

          dhMatrix(T0_1, C_theta[1], C_d[1], C_a[0], C_alpha[0]);
          dhMatrix(T1_2, C_theta[2], C_d[2], C_a[1], C_alpha[1]);
          dhMatrix(T2_3, C_theta[3], C_d[3], C_a[2], C_alpha[2]);
          dhMatrix(T3_4, C_theta[4], C_d[4], C_a[3], C_alpha[3]);
          dhMatrix(T4_5, C_theta[5], C_d[5], C_a[4], C_alpha[4]);
          dhMatrix(T5_6, C_theta[6], C_d[6], C_a[5], C_alpha[5]);

          // Initialize T0_6 as identity matrix
          for (int i = 0; i < 4; i++) 
              for (int j = 0; j < 4; j++) 
                  T0_6[i][j] = (i == j) ? 1 : 0;

          // 计算最终的变换矩阵
          multiplyMatrix(T0_1, T1_2, T0_2);
          multiplyMatrix(T0_2, T2_3, T0_3);
          multiplyMatrix(T0_3, T3_4, T0_4);
          multiplyMatrix(T0_4, T4_5, T0_5);
          multiplyMatrix(T0_5, T5_6, T0_6);

          point.x1=T0_6[0][3];
          point.y1=T0_6[1][3];
          point.z1=T0_6[2][3];

          return point;
      }

      void dhMatrix(double T[4][4], double theta, double d, double a, double alpha) 
      {
          double radTheta = theta * PI / 180.0;
          double radAlpha = alpha * PI / 180.0;
          T[0][0] = cos(radTheta); T[0][1] = -sin(radTheta); T[0][2] = 0; T[0][3] = a;
          T[1][0] = cos(radAlpha) * sin(radTheta); T[1][1] = cos(radTheta) * cos(radAlpha); T[1][2] = -sin(radAlpha); T[1][3] = -d * sin(radAlpha);
          T[2][0] = sin(radAlpha) * sin(radTheta); T[2][1] = sin(radAlpha) * cos(radTheta); T[2][2] = cos(radAlpha); T[2][3] = d * cos(radAlpha);
          T[3][0] = 0; T[3][1] = 0; T[3][2] = 0; T[3][3] = 1;
      }

      void multiplyMatrix(double A[4][4], double B[4][4], double C[4][4]) 
      {
          for (int i = 0; i < 4; i++) 
              for (int j = 0; j < 4; j++) 
              {
                  C[i][j] = 0;
                  for (int k = 0; k < 4; k++)
                      C[i][j] += A[i][k] * B[k][j];
              }
      }
};

void turnL() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+400,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+320,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+320,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+320,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(10);
}

void turnR() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+(-400),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(10);
}

void forward() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-320),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(40);
}

void stop() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+0,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
}

void turningleft() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500-400,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500-600,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500-400,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500-600,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(700);
}

void turningright() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+(+600),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(+400),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+(+600),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(+400),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(800);
}

void leftturningleft() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500-800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500-800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(750);
}

void leftforward() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(40);
}

void rightturningright() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+800,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500-000,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(750);
}

void rightforward() 
{
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",6,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",7,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",8,1500+360,0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  sprintf(cmd_return_tmp, "#%03dP%04dT%04d!",9,1500+(-360),0); //组合指令
  Serial.println(cmd_return_tmp); //解析ZMotor指令-左电机正向
  delay(40);
}

float checkdistance_A1_A2() 
{
  digitalWrite(A1, LOW);
  delayMicroseconds(2);
  digitalWrite(A1, HIGH);
  delayMicroseconds(10);
  digitalWrite(A1, LOW);
  float distance = pulseIn(A2, HIGH) / 58.00;
  //delay(10);
  return distance;
}

void follow()
{
  if (digitalRead(A4) == 0 && digitalRead(A5) == 1) 
  {
    item = 0;
    forward();
  } 
  else if (digitalRead(A4) == 1 && digitalRead(A5) == 0) 
  {
    item = 1;
    forward();
  } 
  else if (digitalRead(A4) == 0 && digitalRead(A5) == 0) 
    forward();
  else if (item == 0 && (digitalRead(A4) == 1 && digitalRead(A5) == 1)) 
    turnL();
  else if (item == 1 && (digitalRead(A4) == 1 && digitalRead(A5) == 1))
    turnR();
}

void writeone()
{
  Points p1;
  Points p2;
  Points p3;
  p1.x1=345;
  p1.y1=0;
  p1.z1=400;
  p2.x1=345;
  p2.y1=0;
  p2.z1=360;
  p3.x1=345;
  p3.y1=0;
  p3.z1=320;
  Arm arm1(L1,L2,L3,L4,p1);
  arm1.run();
  delay(1000);
  Arm arm2(L1,L2,L3,L4,p2);
  arm2.run();
  delay(1000);
  Arm arm3(L1,L2,L3,L4,p3);
  arm3.run();
  delay(1000);
  flag_cp2 = 3;
}

void writetwo()
{
  Points p1;
  Points p2;
  Points p3;
  Points p4;
  Points p5;
  Points p6;
  p1.x1=345;
  p1.y1=40;
  p1.z1=400;
  p2.x1=345;
  p2.y1=-40;
  p2.z1=400;
  p3.x1=345;
  p3.y1=-40;
  p3.z1=360;
  p4.x1=345;
  p4.y1=40;
  p4.z1=360;
  p5.x1=345;
  p5.y1=40;
  p5.z1=320;
  p6.x1=345;
  p6.y1=-40;
  p6.z1=320;
  Arm arm1(L1,L2,L3,L4,p1);
  arm1.run();
  delay(1000);
  Arm arm2(L1,L2,L3,L4,p2);
  arm2.run();
  delay(1000);
  Arm arm3(L1,L2,L3,L4,p3);
  arm3.run();
  delay(1000);
  Arm arm4(L1,L2,L3,L4,p4);
  arm4.run();
  delay(1000);
  Arm arm5(L1,L2,L3,L4,p5);
  arm5.run();
  delay(1000);
  Arm arm6(L1,L2,L3,L4,p6);
  arm6.run();
  delay(1000);
  flag_cp2 = 3;
}

void armset()
{
  Points p0;
  p0.x1=280;
  p0.y1=0;
  p0.z1=320;
  Arm arm0(L1,L2,L3,L4,p0);
  arm0.run();
}

void setup()
{
  servo_7.attach(7);
  servo_3.attach(3);
  servo_5.attach(5);
  servo_6.attach(6);
  servo_9.attach(9);
  //servo_8.attach(8);
  Serial.begin(115200);
  delay(400);
  item = 2;
  pinMode(A1, OUTPUT);
  pinMode(A2, INPUT_PULLUP);
  pinMode(A5, INPUT_PULLUP);
  pinMode(A4, INPUT_PULLUP);
  pinMode(A0, OUTPUT);
  pinMode(A3, INPUT_PULLUP);
}

int leftflag=0;
int rightflag=0;

void loop()
{
  if (flag_cp2 == 0)
    armset();
  else if (flag_cp2 == 1)
    writeone();
  else if (flag_cp2 == 2)
    writetwo();
  float distance = checkdistance_A1_A2();
  if (distance > 19 && flag_cp1==0)
    follow();
  else if (distance <=19 && flag_cp1==0)
  {
    stop();
    digitalWrite(A0, HIGH);
    delay(3000);
    color = digitalRead(A3);//0-green,1-blue
    if(color == 1)
      turningleft();
    else if (color == 0)
      turningright();
    digitalWrite(A0, LOW);
    flag_cp1=1;
    startTime = millis();
  }
  else if (distance >17 && flag_cp1==1)
  {
    currentTime = millis();
    elapsedTime = currentTime - startTime;
    if(color == 1)
    {
      if (elapsedTime<=33000)
        follow();
      else if (elapsedTime>33000) 
      {
        if(leftflag == 0)
        {
          leftturningleft(); 
          leftforward();
          leftflag=1;
        }
        if (digitalRead(A4) == 0 && digitalRead(A5) == 0)
          leftforward();
        else
          follow();
      }
    }
    else if (color == 0)
    {
      if (elapsedTime<=34000)
        follow();
      else if (elapsedTime>34000) 
      {
        if(rightflag == 0)
        {
          rightturningright(); 
          rightforward();
          rightflag=1;
        }
        if (digitalRead(A4) == 0 && digitalRead(A5) == 0)
          rightforward();
        else
          follow();
      }
    }
  }
  else if (distance <=17 && flag_cp1==1)
  {
    stop();
    digitalWrite(A0, HIGH);
    delay(5000);
    size=digitalRead(A3);//0-small,1-big
    digitalWrite(A0, LOW);
    if (size==0 && flag_cp2!=3)
      flag_cp2=1;
    else if (size==1 && flag_cp2!=3)
      flag_cp2=2;
  }
}