程序员经验分享-hwhale

伺服电机堵转检测

2023-11-12

一. 电流数据的分析

电机工作时的电流如下图：
在这里插入图片描述

电机正常工作时，电机电流具有两个状态：正常旋转和堵转。
正常旋转时，电流在控制算法的作用下，一开始会有很快的上升，过程中电流受到控制算法的作用，没有平稳阶段。
堵转时，电机结束了控制算法，所以堵转时电流上升然后保持一段时间的平稳状态。
用电流曲线的斜率描述这一状态：开始斜率是正的，然后斜率变为0。所以可以计算电流曲线的斜率来识别电机是否处于堵转状态。

二. 检测原理

在这里插入图片描述
上图中黄色曲线为matlab计算得出的电流曲线斜率，蓝色曲线为单片机计算得出的斜率。
实际上由于所使用的算法在长时间运行下存在过拟合，所以在实际中，在电机开始工作时对算法的参数做重新的初始化，导致单片机一开始的计算结果波动较大。
算法的基本流程：

1.检测斜率是否接近于0，如果接近于0说明电流此时处于平稳的状态。
2.检测此时斜率的均值，均值是此时刻及之前40个时刻的斜率值得均值。若此时刻均值大于0.3说明，在过去的40个时刻电流是上升状态。
3.若此刻斜率均值大于0.3，检测此时刻后300ms内斜率是否会小于-0.2。若存在这种情况说明电流又快速下降，手指并未处在堵转状态。
4.没有小于-0.2的情况，说明电机此时处于堵转状态。

三. 斜率计算实现

考虑到单片机的性能限制，使用了递推最小二乘方法来递推电流曲线的斜率，并使用队列来保存40个时刻的斜率数据用来递推均值，减少运算量。为了能尽快的使计算结果跟踪电流曲线的变化，在开始的40个计算周期内使用带遗忘因子的递推最小二乘，之后使用有限数据窗+遗忘因子的方式计算。数据窗的长度即参与计算的数据个数为40个。
算法原理可参考以下博客
https://blog.csdn.net/qq_33243369/article/details/102713303

四. 实际效果

可以在发生过流后70ms左右检测到过流。
在这里插入图片描述

图中蓝色曲线是计算得出的电流斜率。黄色曲线为电流斜率的递推均值，绿色曲线代表有无检测出过流。
在这里插入图片描述
在检测出过流后，将手指电机占空比设置为0.上图第二段曲线是手指正常打下的电流曲线，第一段曲线是手指打下后有堵转，可以看到堵转后70ms内电流回复了正常值。

五.代码实现

least_squares.h

#ifndef _LEAST_SQUARES_H
#define _LEAST_SQUARES_H



#include "queue.h"

typedef struct
{
    float   P[4];
	  float   L[2];
	  float   Q[4];
	  float   lam_p;
	  float   lamp;
      QUEUE   K0_q;
	  QUEUE   K1_q;
	  float   K[2];
      float   lam;
      QUEUE   cur;
	  float   last_mean_cur;
	 // unsigned int  i;
	  uint32_t phi;
	  float mean_k;
	  float k_sum;
	  float start_time;
	  float check_f;
	  uint8_t overcur;
	  float cur_sum;
	  float mean_cur;
} least_squares_paras_t;


void least_squares_init(void);
void least_squares_porcess(float cur);
#endif

least_squares.c

#include "least_squares.h"
#include "math.h"
least_squares_paras_t least_squares_paras;
extern robot_finger_module_t robot_finger_module;


//init paras
void least_squares_init(void)
{
	least_squares_paras.lam = 0.9;
	least_squares_paras.lam_p = 67.6550;
	least_squares_paras.lamp = 0.0148;
    least_squares_paras.phi = 1;
	least_squares_paras.k_sum=0;
	least_squares_paras.mean_k=0;
	least_squares_paras.P[0] = 1000000;
	least_squares_paras.P[1] = 0;
	least_squares_paras.P[2] = 0;
	least_squares_paras.P[3] = 1000000;
	least_squares_paras.K[0] = 0;
	least_squares_paras.K[1] = 0;
	least_squares_paras.check_f = 0;
	least_squares_paras.overcur=0;
	CreateQueue(&least_squares_paras.cur,40);
	CreateQueue(&least_squares_paras.K0_q,40);
	CreateQueue(&least_squares_paras.K1_q,40);
}



void least_squares_porcess(float cur)
{

	//time counter
  least_squares_paras.phi++;
	
	//queue is not full, use forgetting factor
	if(!FullQueue(&least_squares_paras.cur))
	{
		Enqueue(&least_squares_paras.cur,cur);
		least_squares_paras.cur_sum+=cur;
		least_squares_paras.mean_cur = least_squares_paras.cur_sum/least_squares_paras.cur.rear;
		
		//Extract the reused process value and calculate it separately
		float res1 = least_squares_paras.lam + least_squares_paras.phi*least_squares_paras.phi*least_squares_paras.P[0]
		           + least_squares_paras.phi*least_squares_paras.P[1]
	               + least_squares_paras.phi*least_squares_paras.P[2]
	               + least_squares_paras.P[3];
	
	  //calculate L
		least_squares_paras.L[0] = (least_squares_paras.P[0]*least_squares_paras.phi+least_squares_paras.P[1])/res1;
		least_squares_paras.L[1] = (least_squares_paras.P[2]*least_squares_paras.phi+least_squares_paras.P[3])/res1;

    //calculate P
		float P_t[4] = {least_squares_paras.P[0],least_squares_paras.P[1],least_squares_paras.P[2],least_squares_paras.P[3]};
		least_squares_paras.P[0] = (P_t[0]*(1-least_squares_paras.L[0]*least_squares_paras.phi)
															 - least_squares_paras.L[0]*P_t[2])/least_squares_paras.lam;

		least_squares_paras.P[1] = (P_t[1]*(1-least_squares_paras.L[0]*least_squares_paras.phi)
															 - least_squares_paras.L[0]*P_t[3])/least_squares_paras.lam;

		least_squares_paras.P[2] = (P_t[2]*(1-least_squares_paras.L[1])
															 - least_squares_paras.L[1]*P_t[0]*least_squares_paras.phi)/least_squares_paras.lam;

		least_squares_paras.P[3] = (P_t[3]*(1-least_squares_paras.L[1])
															 - least_squares_paras.L[1]*P_t[1]*least_squares_paras.phi)/least_squares_paras.lam;
		
		//calculate slope
		float K_o[2]={least_squares_paras.K[0] ,least_squares_paras.K[1]};
    least_squares_paras.K[0] = least_squares_paras.K[0] 
	                           + least_squares_paras.L[0]*(cur - least_squares_paras.phi*K_o[0]-K_o[1]);
	  least_squares_paras.K[1] = least_squares_paras.K[1] 
                            + least_squares_paras.L[1]*(cur - least_squares_paras.phi*K_o[0]-K_o[1]);
		
		Enqueue(&least_squares_paras.K0_q,least_squares_paras.K[0]);
		Enqueue(&least_squares_paras.K1_q,least_squares_paras.K[1]);
		least_squares_paras.k_sum+=least_squares_paras.K[0];
		least_squares_paras.mean_k = least_squares_paras.k_sum/least_squares_paras.K0_q.rear;
		
		
	}
	else //queue is full,use Limited data window and forgetting factor
	{
		//current before 40 cycles
		float cur_p =0;
		
		//the present time
    float t = least_squares_paras.phi;
	  
		//time before 40 cycles
		float t_p = least_squares_paras.phi-40;
		
		//Extract the reused process value and calculate it separately
		Dequeue(&least_squares_paras.cur,&cur_p);
		float p_t[10];
		uint8_t p_cnt = 0;
		for(int i=0;i<4;i++)
		{
			for(int j=i;j<4;j++)
			{
				p_t[p_cnt]=least_squares_paras.P[i]*least_squares_paras.P[j];
				p_cnt++;
			}
		}
		
		float res = least_squares_paras.lam+least_squares_paras.P[3]
								+t*(least_squares_paras.P[2]+t*least_squares_paras.P[0]+least_squares_paras.P[1]);


    //calculate Q
		least_squares_paras.Q[0] = (least_squares_paras.P[0]-(p_t[5]+t*p_t[2]+t*p_t[1]+t*t*p_t[0])/res)/least_squares_paras.lam;
		least_squares_paras.Q[1] = (least_squares_paras.P[1]-(p_t[6]+t*p_t[3]+t*p_t[4]+t*t*p_t[1])/res)/least_squares_paras.lam;
		least_squares_paras.Q[2] = (least_squares_paras.P[2]-(p_t[8]+t*p_t[7]+t*p_t[3]+t*t*p_t[2])/res)/least_squares_paras.lam;
		least_squares_paras.Q[3] = (least_squares_paras.P[3]-(p_t[9]+t*p_t[8]+t*p_t[6]+t*t*p_t[5])/res)/least_squares_paras.lam;

		
		//Extract the reused process value and calculate it separately
		float q_t[10];
		uint8_t q_cnt = 0;
		for(int i=0;i<4;i++)
		{
			for(int j=i;j<4;j++)
			{
				q_t[q_cnt]=least_squares_paras.Q[i]*least_squares_paras.Q[j];
				q_cnt++;
			}
		}

		
		res = least_squares_paras.Q[3]-least_squares_paras.lam_p+t_p*(least_squares_paras.Q[2]+t_p*least_squares_paras.Q[0]+least_squares_paras.Q[1]);

		//calculate Q
		least_squares_paras.P[0] = least_squares_paras.Q[0]-(q_t[5]+t_p*q_t[2]+t_p*q_t[1]+t_p*t_p*q_t[0])/res;
		least_squares_paras.P[1] = least_squares_paras.Q[1]-(q_t[6]+t_p*q_t[3]+t_p*q_t[4]+t_p*t_p*q_t[1])/res;
		least_squares_paras.P[2] = least_squares_paras.Q[2]-(q_t[8]+t_p*q_t[7]+t_p*q_t[3]+t_p*t_p*q_t[2])/res;
		least_squares_paras.P[3] = least_squares_paras.Q[3]-(q_t[9]+t_p*q_t[8]+t_p*q_t[6]+t_p*t_p*q_t[5])/res;
		

		//calculate slope
		float K_o[2]={least_squares_paras.K[0] ,least_squares_paras.K[1]};
		float res1 = K_o[1]-cur+t*K_o[0];
		float res2 = K_o[1]-cur_p+t_p*K_o[0];
		least_squares_paras.K[0] = K_o[0]-(least_squares_paras.P[1]+t*least_squares_paras.P[0])*res1
		         +(least_squares_paras.lamp*least_squares_paras.P[1]+least_squares_paras.lamp*least_squares_paras.P[0]*t_p)*res2;
		
		least_squares_paras.K[1] = K_o[1]-(least_squares_paras.P[3]+t*least_squares_paras.P[2])*res1
		         +(least_squares_paras.lamp*least_squares_paras.P[3]+least_squares_paras.lamp*least_squares_paras.P[2]*t_p)*res2;
		
		float K_p[2];
		Dequeue(&least_squares_paras.K0_q,&K_p[0]);
		Dequeue(&least_squares_paras.K1_q,&K_p[1]);
		least_squares_paras.k_sum=least_squares_paras.k_sum-K_p[0]+least_squares_paras.K[0];
		least_squares_paras.mean_k = least_squares_paras.k_sum/least_squares_paras.K0_q.maxsize;
		Enqueue(&least_squares_paras.K0_q,least_squares_paras.K[0]);
		Enqueue(&least_squares_paras.K1_q,least_squares_paras.K[1]);
	  Enqueue(&least_squares_paras.cur,cur);
	}
	
	

	
	if(least_squares_paras.check_f)
	{
		uint32_t time = HAL_GetTick();
		
		if(time>least_squares_paras.start_time+150&&time<least_squares_paras.start_time+450)
		{
			if(least_squares_paras.K[0]>0&&least_squares_paras.K[0]<0.2&&least_squares_paras.mean_k>0.35)
			{
				least_squares_paras.overcur=1;
			}
			if(least_squares_paras.K[0]<-0.2)
			{
				least_squares_paras.overcur=0;
			}
		}
		else if(time>least_squares_paras.start_time+450&&time<least_squares_paras.start_time+800)
		{
			//least_squares_paras.check_f=0;
			ClearQuene(&least_squares_paras.cur);
			ClearQuene(&least_squares_paras.K0_q);
			ClearQuene(&least_squares_paras.K1_q);
			least_squares_paras.phi = 1;
			least_squares_paras.k_sum=0;
			least_squares_paras.mean_k=0;
			least_squares_paras.P[0] = 100;
			least_squares_paras.P[1] = 0;
			least_squares_paras.P[2] = 0;
			least_squares_paras.P[3] = 100;
			least_squares_paras.K[0] = 0;
			least_squares_paras.K[1] = 0;
			
			
			least_squares_paras.mean_k = 0;
			
			if(least_squares_paras.overcur)
			{
				//检测出过流，执行相应的保护操作				
			}
			least_squares_paras.overcur=0;
		}
		else if(time>least_squares_paras.start_time+800)
		{
			least_squares_paras.check_f=0;
		}
	}
		
}

queue.h

#ifndef __QUEUE_H_
#define __QUEUE_H_
typedef struct queue 
{
	float *pBase;
	int front;    
	int rear;   
	int maxsize; 
}QUEUE,*PQUEUE;
 
void ClearQuene(PQUEUE Q);
void CreateQueue(PQUEUE Q,int maxsize);
void TraverseQueue(PQUEUE Q);
int FullQueue(PQUEUE Q);
int EmptyQueue(PQUEUE Q);
int Enqueue(PQUEUE Q, float val);
int Dequeue(PQUEUE Q, float *val);
#endif

queue.c

#include<stdlib.h>
#include"queue.h"
/***********************************************
Function: Create a empty stack;
************************************************/
void CreateQueue(PQUEUE Q,int maxsize)
{
	Q->pBase=(float *)malloc(sizeof(float)*maxsize);
	if(NULL==Q->pBase)
	{
		return;
	}
	Q->front=0;         
	Q->rear=0;
	Q->maxsize=maxsize;
}

void ClearQuene(PQUEUE Q)
{
	Q->front=0;         
	Q->rear=0;
}
/***********************************************
Function: Print the stack element;
************************************************/
void TraverseQueue(PQUEUE Q)
{
	int i=Q->front;

	while(i%Q->maxsize!=Q->rear)
	{
		//printf("%d ",Q->pBase[i]);
		i++;
	}
	//printf("\n");
}
int FullQueue(PQUEUE Q)
{
	if(Q->front==(Q->rear+1)%Q->maxsize)    
		return 1;
	else
		return 0;
}
int EmptyQueue(PQUEUE Q)
{
	if(Q->front==Q->rear)    
		return 1;
	else
		return 0;
}
int Enqueue(PQUEUE Q, float val)
{
	if(FullQueue(Q))
		return 0;
	else
	{
		Q->pBase[Q->rear]=val;
		Q->rear=(Q->rear+1)%Q->maxsize;
		return 1;
	}
}


int Dequeue(PQUEUE Q, float *val)
{
	if(EmptyQueue(Q))
	{
		return 0;
	}
	else
	{
		*val=Q->pBase[Q->front];
		Q->front=(Q->front+1)%Q->maxsize;
		return 1;
	}
}
本文内容由网友自发贡献，版权归原作者所有，本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容，请联系:hwhale#tublm.com(使用前将#替换为@)

控制算法

最小二乘

单片机

驱动开发

伺服电机堵转检测 的相关文章

  • vue 数字变星号 过滤器_Vue自定义过滤器格式化数字三位加一逗号实现代码

    前端处理一些金额 数字类的数据要求按照固定的格式显示 比如9 527 025 或者带有小数 如1 587 23 仍要三位一断 有些话也不必多说 既然要求如此 实现呗 作为前端主流框架之一的Vue 类似的功能肯定都有人写的很完善了 我呢 最讨

  • React Hooks 学习笔记

    大家好 小编最近在梳理 React Hook 的相关内容 由于看视频 看书 自己做项目总觉得缺点什么 总觉得看过了 内容太简单 有啥好写的 但是过了一段时间 感觉有些东西又模糊了 同时又觉得怕简单 写了也不一定有人看 但是想想 还是整理成文

随机推荐

  • iOS开发之第三方支付支付宝支付教程，史上最新最全第三方支付宝支付方式实现、支付宝集成教程，支付宝实现流程

    本章项目demo https github com zhonggaorong alipayDemo 支付宝支付大致流程为 1 公司与支付宝进行签约 获得商户ID partner 和账号ID seller 和私钥 privateKey 开发中

  • 【自然语言处理】主题建模：Top2Vec（理论篇）

    主题建模 Top2Vec 理论篇 Top2Vec 是一种用于 主题建模 和 语义搜索 的算法 它自动检测文本中出现的主题 并生成联合嵌入的主题 文档和词向量 算法基于的假设 许多语义相似的文档都可以由一个潜在的主题表示 首先 创建文档和词向

  • 没有头的猫在笛卡尔坐标系上随机漫步（未完成）

    想用python模拟随机运动 布朗运动 BROWNIAN MOTION 英国科学家布朗在两百年前第一次观察到水分子推动花粉颗粒在水面做不规则运动 现在我们也想用电脑模拟 在无外力作用下 花粉颗粒在水面的随机运动 如何实现呢 详细解释直接写在

  • LSTM 和 Bi-LSTM

    承上启下 承接上文介绍过的 SimpleRNN 这里介绍它的改进升级版本 LSTM RNN 和 LSTM 比较 RNN 的记忆很短 容易产生梯度消失的长依赖问题 而 LSTM 可以解决这个问题 它有更长的记忆 RNN 模型比较简单 只有一个

  • c++处理数据

    处理数据 简单变量 const限定符 类型转换和auto声明 整型 char类型 字符和小整数 成员函数cout put 通用字符名 signed char和unsigned char wchar t 浮点数 bool类型 c 算术运算符

  • Python入门笔记(1)

    纯小写的就是python的BIF 内置函数 dir builtins ArithmeticError AssertionError AttributeError BaseException BlockingIOError BrokenPip

  • 避免Unity错误剥离代码

    开启ProjectSettings里的Strip Engine Code项目后 Unity会尝试剥离未使用的引擎代码 以降低包体大小和运行时内存占用 但是某些运行时需要的组件会被错误剥离

  • Hibernate之关于多对多单向关联映射

    Hibernate 之关于多对多单向关联映射 老师和学生 最典型的多对多关联 Teacher和Student 所谓单向意思就是说 老师知道自己的教的是哪些学生而学生不知道是哪些老师教 也可以这么说 在查询的时候 通过老师可以级联查询出学生

  • 有些话，只说给懂的人听

    生活中最 孤独的时候 往往不是因为孤身一人 而是即使身边有很多人 有些心事依旧无人能懂 以前的时候 我们常常会把所有的喜怒悲欢 都说与别人听 后来渐渐变得沉默了 不是因为学会了独自消化 只是明白了 有些话只能说给懂的人听 人与人之间 只有彼

  • mybatis+mysql insert时返回自增主键

    mybatis mysql insert时返回自增主键 mysqlmybatisinsert返回自增主键 使用mybatis执行insert操作时 需要返回自增主键 网上清一色的答案 useGeneratedKeys设置为true keyP

  • qt开发使用camera类获取摄像头信息并拍照保存

    首先是UI布局 在pro文件中添加两个类 QT multimedia QT multimediawidgets 然后我们需要包含几个摄像头使用的头文件并创建摄像头的对象 include

  • win10远程桌面的坑

    win10的远程桌面的确是清晰度非常好 操作非常流程的 但是还是有坑的 举两个踩坑例子 1 录屏软件在远程桌面退出后无效了 无法录制屏幕了 2 监控客户端在退出远程桌面后 再进去远程桌面 打圈圈卡死 因此一些应用不适合在win10远程桌面办

  • GNS3 配置GRE

    1 简述 GRE Generic Routing Encapsulation GRE是一种最传统的隧道协议 其根本功能就是要实现隧道功能 以实现异地网络之间可以通过内部私网相互访问 以上图为例 假设IP地址为10 1 1 1的XP1想访问I

  • 基于Zigbee的SHT10温湿度数据采集系统(已实现控制12个终端节点)——Zigbee协调器主要代码解析

    之前实现了基于Zigbee的SHT10温湿度数据采集系统 这里来重新复盘一些主要的知识和代码 写在前面 1 功能介绍 使用Zigbee终端节点采集环境的温度和湿度数据 然后将数据无线发送的Zigbee协调器 最后在电脑端显示获得到的数据 2

  • Ubuntu初学思维导图（后继续补充）

    关于虚拟机 Ubuntu的命令内容简要 1 创建用户 sudo adduser user01 创建用户时 同步创建对应组 同步创建家目录 sudo useradd user02 仅创建用户 单独设置完密码后 才能登陆 2 修改用户密码 su

  • http请求头部（header）详解

    当我们在浏览器中访问一个网站时 我们的浏览器实际上会向该网站发送一个 HTTP 请求 而 HTTP 请求头部 Header 则是一组包含请求信息的键值对 用来描述 HTTP 请求的各种属性和特征 以下是 HTTP 请求头部的一些常见属性和含

  • linux重启服务的脚本命令

    最近做网站测试 每次测试完成都要重启服务 为此写了一个简单的shell脚本 linux服务重启shell脚本示例 2014年12月18日 linux服务重启脚本 如何实现linux服务的定时重启 可以借助shell脚本来完成 ps命令捕获进

  • 方差分析在特征筛选中的应用

    方差分析在特征筛选中的应用 方差分析 Analysis of Variance 简称ANOVA 是一种常见的统计分析方法 它可以用于比较两个或多个组之间的均值差异 在机器学习中 我们可以应用方差分析来进行特征筛选 从而得到对模型有显著影响的

  • 高光谱图像端元提取——vertex component analysis（VCA/python）

    在高光谱图像中 VCA是一种常用的端元提取方法 算法来源 Vertex Component Analysis A Fast Algorithm to Unmix Hyperspectral Data submited to IEEE Tra

  • 伺服电机堵转检测

    一 电流数据的分析 电机工作时的电流如下图 电机正常工作时 电机电流具有两个状态 正常旋转和堵转 正常旋转时 电流在控制算法的作用下 一开始会有很快的上升 过程中电流受到控制算法的作用 没有平稳阶段 堵转时 电机结束了控制算法 所以堵转时电

热门标签