登录
首页 » Others » 基于STM32的两轮平衡小车，代码详尽，效果理想

基于STM32的两轮平衡小车，代码详尽，效果理想

于 2021-05-06 发布
0 256
下载积分: 1 下载次数: 1
我要下载

代码说明：

基于STM32的两轮平衡小车，代码详尽，效果理想

下载说明：请别用迅雷下载，失败请重下，重下不扣分！

发表评论

0 个回复

  • Python-深度强化学习PyTorch实现集锦
    This repository contains most of classic deep reinforcement learning algorithms, including - DQN, DDPG, A3C, PPO, TRPO. (More algorithms are still in progress)
    2020-02-04下载
    积分：1
  • jackson-core-asl-1.9.7.jar等4个jar包
    spring3 mvc利用@RequestBody、@ResponseBody传输json格式数据需要的jar包
    2020-12-07下载
    积分：1
  • modbus协议+modbus软件开发实战指南PDF打包下载.rar
    【实例简介】学习modbus开发好资料。 【modbus协议+modbus软件开发实战指南PDF打包下载】
    2021-11-25 00:32:18下载
    积分：1
  • stc8a8k64s4a12最小系统
    【实例简介】stc8a8k64s4a12最小系统板原理图加PCB，同时带有自建元件集成库的源文件，最小系统板全引脚引出，4串口引出，4个LED，4个独立按键，一个RC522SPI接口
    2021-10-29 00:32:24下载
    积分：1
  • 内存乒乓缓存机制和消息分发机制的C代码实现
    用C代码实现乒乓内存缓冲机制，具体实用价值，帮助您提高内存响应速度与及时数据的处理。unsigned long writeunsigned long greadunsigned long overflowST TWTMSG QUEUE:/ Helper macros for accessing Msg queues. *#define tWt QUEUE EMPty(a)(((g->write==(q)->gread)? 1: 0)#define twt_ QUeUe full(a)(((((q)->qwrite +1% TIMEWEIGHT TASKQUEUE SIZED)==(q)->gread)?1: 0)米 generate a Msg entity*正常返回消息体的指针,异常返回NULLT TWTMSG* generateMsg(tT-TWTMSG* pmSg=nULL,if(NULL =-(ptMsg malloc(sizeof(T TWTMSG)))return NULL;memset(ptMsg, 0, sizeof(T TWTMSG)return pmSg;destroy a Msgvoid destroy Msg(t TWTMSG ptMsgif(NULL ptMsg->pfDestroyMsg)pt Msg->pfDestroy Msg(ptMsg->pvMsg)if (NULL != pt Msg)free(pmSgfree a Msg Queuevoid freeTWTMsg Que(T TWTMSG QUEUE* ptMsgQif(NULL =ptMsg Afree(ptMsg Q);Init a Msg QT TWTMSG QUEUE* initTWTMsg QueoT TWTMSG QUEUE pmSg Q= NULlif (NULL ==(ptMsgQ malloc(sizeof(T_ TWTMSG QUeue)goto ErrRetmemset(ptMsgQ, 0, sizeof(T TWTMSG QUEUE))return pmSg Q;Errretprintf( initTWTMsg Que Fail! ")freeTWTMsgQue(ptMsg Q)return nullPop a pvMsg packet from a msg packet queues param g is the packet queue from which to pop the pbuf@return pointer to pvMsg packet if available, NULl otherwiseT TWTMSG* TWTMsg Get(T_ TWTMSG QUEUE aT TWTMSG*//*加锁if(TWT_ QUEUE_ EMPTY(a))iReturn a NUll pointer if the queue is emptypmSg=NULL;else is The queue is not empty so return the next frame from itand adjust the read pointer accordinglypmSg=g->pvMsglq >greadg->gread =((q->gread +1)% TIMEWEIGHT_TASKQUEUE_ SIZE)/*解锁return(pmSg);Push a pmSg packet onto a pvMsg packet queue@param p is the pmsg to push onto the packet queue@param g is the packet queue.W @return 0 if successful, -1 if q is fullnt TWTMsg Send(T tWTMSg*p, T TWTMSG QUEUE *qint ret/if(!TWT_ QUEUE FULL(al)iThe queue isn t full so we add the new frame at the currentw write position and move the write pointer.g->pvMsgla->write]=pg- >write =((q->qwrite+1)% TIMEWEIGHT TASKQUEUE SIZE;ret =oThe stack is full so we are throwing away this value. Keep trackof the number of times this happensg->overflow++ret =-1://*解锁return(ret)**米**米来米***来米*半米*米*半米*米求***半*米米求半**米求半**半求半和*米*//消息分发机制//*算法是//*正常返回0,出错返回-1水米米******水*米*水**米*半*水米米冰半**水水*水米米半米冰水*米水水*水*米水水extern int RecToFile MsgProc(T_ MSG_ REC2 FILE* ptMsg);int DispatchMsg(T TWTMSG *ptMsgitif(NULl = ptMsg goto ErrRet/*dispatch msg*/switch(ptMsg->en Msg Type)case TWT PINgPoNgBuff recRecTo File Msg Procl(TMSG_REC2FE) (pmSg->pMsg);/*处理消息*/destroy Msg( pmSg;/*消毁消息breakdefault.printf("Dispatch Msg Msgtype Error! n")break.return 0ErrRetprintf("Dispatch Msg Fail! ";return-1./*buff size*/#defineP| NGPONG_ BUFF BSIZE0X20000//10*1024*1024/*10M*/*ping pong buff*///chargacPINGBUFF[PINGPONG BUFF BSIZE]; /* Ping Buff*///chargacPONGBUFF[PINGPONG_ BUFF BSIZE] *Pong Buff*/水米米*********米*水**米*半*水米米水**冰水*水米米半半水半米冰水*米水水*水*米米//*释放 ping pong buff/必然成功//*无返回木***木*水****本**水*水水*水****本水**水水****水水***本***米*水void Destroy Ping Pong Butt(T PINGPONGBUFF USE DES ptPing Pong Butt)nLoopif (NULL = pt Ping pong buffreturnfor (nLoop=0; nLooptIng BuffUse[nLoop]. pcHeadAddr)free(ptPing Pong ->t Ping BuffUse[nLoop]. pcHeadAddr)free(pt Ping pong Buff)/初始化 ping pong buff返回 pign pong buff的描述指针//*正常返回0,出错返回-1水水水水水水水水水木水木水水水水水木水木水水水水水水水水本水水水水水水水水水本水水水水水水水水水水水水水T PINGPONGBUFF USE DES InitPing Pong Buff(unsigned int n BuffSizeT PINGPONGBUFF USE DES* ptBuffDes=NULLintnLoop/*获取buf描述*if NULL==(ptBuffDes=malloc(sizeof(T PINGPONGBUFF USE DES))))goto ErrRetmemset(pt BuffDes, 0, sizeof(T_PINGPONGBUFF USE_ DES));/*分别初始化ping和pong*/for(nLoop=0; nLooptIng BuffUsenLoop pcHeadAddr =malloc n BuffSize))goto Err Ret;ptBuffDes->tIng BuffUselnLoop] nBuffSize nBuffsizeptBuffDes->tPing BuffUse[nLoop]. oFfsetptBuffDes->tPing BuffUsenLoop) eUseStatus-=BUFF WRITEABLE;pt BuffDes->eCurUseIDBUFF PINGreturn pt BuffDesErrretprintf("lnitPing Pong Buff Fail!");DestroyIng Pong Buff(pt BuffDes)turn null平**米**米*米***来米米*米*米*半米*米米米来*半米平**米米求*来*半求半来*米求*和*米*/*Reset ping pong buff//*正常返回0,出错返回-1米米米米水冰米*米米水**米米冰*米水米米米米水米水*水米米来米米x米来米米水冰来来宋来水米来米来冰#define ResetBuffUse(ptBuffuse)pt BuffUse->oFfset0pt BuffUse->eUseStatus BUFF WRITEABLEgenerate a file rec msg*正常返回消息体的指针,异常返回NULLT_MSG_REC2lGK SenFRMSB(T_BUFF_USE_DES *ptBuffUse, REC_FILE_DESLIST *ptFileListRT MSG REC2FILE KE* ptRFMsg= NULL;if(NULL ==(ptRFMsg malloc(sizeof(T_ MSG REC2 FILE)return nUllptRFMsg- >pt BuffUse pt BuffUseptRFMsg->pt Filelist ptFilelist;return ptRFMsg
    2020-12-03下载
    积分：1
  • matlab 神经网络的文字识别 有详细注释-补充样本
    之前上传过 《matlab 神经网络的文字识别 有详细注释》这个贴，有人评论说 不会运行。 是样本文件需要自己添加，特此 上传样本文件，方便大家。 所有文件放进一个工程，运行主程序即可!
    2020-12-04下载
    积分：1
  • 复合材料子程序fortran
    用于abaqus采用实体单元分析复合材料冲击损伤
    2020-11-28下载
    积分：1
  • Protege新手入门(入门篇+进阶篇+推理篇+使用笔记)
    Protege新手入门(入门篇+进阶篇+推理篇+使用笔记)
    2021-05-06下载
    积分：1
  • VINS论文推倒及代码解析
    VINS 的功能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线程,分别是:前端图像跟踪、后端非线性优化(其中初始化和 IMU 预积分在这个线程中)、闭环检测、闭环优化。、总体框架Measurement PreprocessingInitializationCamera(30hz)Feature Detectionnd rackerVisual-lnertialInitializedis- onlySfMAlignmentIMU (100hMU Pre-integrationLocal Visual-Inertial: OldestSliting WindowNewestNonli+、 Keyframe?OptimizationBundle Adjustment II Loop detectionwith RelocalizationStates from Loop ClosureFealure retrievel oop Deleted二二1---11------22===Global Pose Graph4-DoF Pose Graph OptimizationKeyframe DatabaseOptimization图1VINS框架ⅵINS的玏能模块可包括五个部分:数据预处理、初始化、后端非线性优化、闭环检测及闭环优化。代码中主要开启了四个线稈,分别是:前端图像跟踪、后端非线性伉化(其中初始化和IMU预积分在这个线程中)、闭环检测、闭环优化各个功能模块的作用上要有:1.I图像和MU预处理●图像:提取图像 Harris角点,利用金字塔光流跟踪相邻帧,通过 RANSAC去除异常点,最后将跟踪到的特征点push到图像队列中,并通知后端进行处理●IU:将IMU数据进行积分,得到当前时刻的位置、速度和旋转(PVQ),同时计算在后端优化中将用到的相邻帧的预积分增量,及预积分误差的 Jacobian矩阵和协方差项。1.2初始化首先,利用SFM进行纯视觉佔计滑窗內所有帧的位姿及3D点逆深度,最后与IMU预积分进行对齐求解初始化参数1.3后端滑窗优化将视觉约束、IMU约束和闭环约束放在·个大的目标函数中进行非线性优化,求解滑窗内所有帧的PVQ、bias等。L M States in the sliding windowIMU:k States from loop clos1Camera:冷 MU measurements>visual measurements★ Catur图2滑窗优化示意图14闭环检测和优化利用D)BoW进行闭环检测,当检测成功后进行重定位,最后对整个相机轨迹进行闭环优化。U预积分VisionIMUVision图3MU预积分示意图21当前时刻pVQ的连续形式将第k唢和第kl帧之间的所有IMU进行积分,可得第kHI帧的位置、速度和旋转(PVQ),作为视觉估计的初始值,这里的旋转采用的四元数。v△t+k+1∈[k,k+1]rW(at-ba ) -owletbk JtE[k, k+1]n(,-bdt∈[k,k+1]其中,a2和O为ⅠMU测量的加速度和角速度,是在Body自身坐标系, world坐标系是IMU所在的惯导系,上式的旋转公式推导可参考附录10.1。22当前时刻PVQ的中值法离散形式公式(1)给出的是连续吋刻的相机当前PVR的达代公式,为了跟代码致,下面给出基于中值法的公式,这与 Estimator:; processIMg(O函数中的Ps]、Rs]和Vs是一致的,IMU积分出来的第j时刻的物理量可以作为第j帧图像的初始值。tr t+a26t(2)ka,St其中q(a1-ba)-g"+q:+1(a+1-ba)(a;+o;+1)2.3两帧之间PVQ增量的连续形式通过观察公式(1)可知,IvU的预积分需要依赖与第k帧的ν和R,当我们在后端进行非线性优化时,需要迭代更新第κ唢的ν和R,这将导致我们需要根据每次迭代后值重新进行积分,这将非常耗吋。因此,我们考虑将优化变量从第k帧到第κ+1帧的IU预积分项中分离开来,通过对公式(1)左右两侧各乘Rb,可化简为:R(+p2k-=2△)+ak+1b其中DtElk, k+1t∈[k,k+1R k(at-bar)ldt)Wendtt∈[kk+1这样我们就得到了连续时刻的MU预积分公式,可以发现,上式得到的MU预积分的值只与不同时刻的a2和o相关。这里我们需要重新讨论下公式(5)预积分公式,以ab,为例,我们发现它是与MU的bias相关的,而bias也是我们需要优化的变量,这将导致的问题是,当每次迭代时,我们得到一个新的bias,又得根据公式(巧5)重新对第k帧和第k+1帧之间的IMU预积分,非常耗时。这里假设预积分的变化量与bias是线性关系,可以写成:ab,+/6n6ba+/16b+8 8ba +p(6)k+1sb24两帧之间PVQ增量的欧拉法离散形式面给出离散时刻的IMU预积分公式,首先按照论文中采用的欧拉法,给出第i个MU时刻与第i1个IMU时刻的变量关系为b+1k+的6t+元R(P)(1+R(P")(a2-bbn)δt25两帧之间PⅤQ增量的中值法离散形式卜面给出代码中采用的基」中值法的IMU预积分公式,这与 Estimator: processIMUO函数中的 Integration Base: push backo上是一致的。注意这里跟公式(2)是不一样的,这里积分出来的是前后两顿之间的IU增量信息,而公式(2)给出的当前帧时刻的物理量信息+1+B k St +=a, &tbb+1Bi + au1其中a,=slqilai-bai)+qiDi t aitl2.6连续形式下PVQ增量的误差、协方差及 JacobianIMU在每个吋刻积分出来的值是有误差的,下面我们对误差进行分析。首先我们直接给出在t时刻误差项的导数为:sa00016a000000-82(066hkR;0006|=00-(a-bh)0-1192k|+|000|mLL000016ba00101n000018b000F+ozk+ Gt其中:F25×15,G215×2,62x1,n12×,上式推导可参考附录102。下面我们讨论它的作用,将其可以简写为:6之k=F62z+Gtnt根据导数定义可知:62b=1m24-6262+8=62+628t=(+F6t)6z+(Gt6t)nt(11)这里我们对公式(1)的IMU误差运动方程再说明,将上式和EKF对比可知,上式恰好给出了如EKF一般对非线性系统线性化的过程,这里的意义是表示下一个时刻的IMU测量误差与上一个时刻的成线性关系,这样我们根据当前时刻的值,可以预测出下一个时刻的均值和协方差,而公式(1)给出的是均值预测,协方差预测公式如下Pb+6=(1+Ft)P(+Fl6t)7+(G,t)Q(G18t)ot(12)上式给出了协方差的选代公式,初始值Pk=0。其中,Q为表示噪声项的对角协方差矩阵000003000另外根据(11)式可获得诀差项的 Jacobian的迭代公式:(I+F26t)(14)其中 Jacobian的初始值为bk=12.7离散形式的PVQ增量误差分析我们首先直接给出PVQ增量误差在离散形式下的矩阵形式,为了与代码一致,我们修改下变量顺序,这和代码中 midPointIntegration(函数是一致的。(但不知为何计算的V中与前四个噪声项相关的差个负号?)1t fo660f106t‖loeBk+1=0f211f20016bδb0[6b102001rnot000kRkotk+1(15006t0n0000δt其中,推导可参考附录10.3:stE(ak-ba)02-4B+1(kk+121k+1b.)6t|6t2(Rr+ rk+18t2Stn=71=Rk+1(a+1-b)6tWr+ wf1=Ik+11+Gb。)δt-Rn+1(ak+121=-2配+1Stl st21(RK+Ruts)4rula1RrotstStR+1(a1R,+114/+11t28离散形式的PVQ增量误差的 Jacobian和协方差将公式(15)简写为:k+1F15×158215×1+V15×13Q则 Jacobian的迭代公式为k+15×15=F/k(16)其中, Jacobian的初始值为/k=l。这里计算出来的k+1只是为了给后面提供对bias的acoblar。协方差的迭代公式为P+15×15=FPFr+vQv(17)其中,初始值P=0。Q为表示噪声项的对角协方差矩阵:00000000aa000Q18×180a00(18)000000三、后端非线性优化31状态向量状态向量共包括滑动窗口内的n+l1个所有相机的状态(包括位置、朝向、速度、加速度计bias和陀螺仪bias)、 Camera到IMU的外参、m+1个3D点的逆深度X=[xr=pw,vb bpc,q3.2目标函数吗+(喻,2)+2(19)其中三个残差项即误差项分别为边缘化的先验信息、IMU测量残差、视觉的重投影残差。三种残差都是用马氏距离表示。根据《十四讲》中高斯牛顿法,若要计算目标函数的最小值,可以理解为,当优化变量有一个增量后,目标函数值最小,以IU残差为例,可写成如下所示:nin lre2bk, X+8Xrk x)+HSⅩDk+1oXk+1k+1其中HB,为B关于 XIK Jacobian,将上式展开并令关于6X的导数为0,可得增量δx的计算公式:H k 8X=k+1TB那么,公式(28)可写成+∑+∑Tk∑1rc上式中,B为MU预积分噪声项的协方差,P为vual观测的噪声协方差。当MU的噪声协方差P越大时,其信息矩阵Pk,将越小,意味着该MU观测越不可信,换句话说,因MU噪声较大,越不可信IMU预积分数据,而更加相信 visual观测。注意,这里的IMU和vsua协方差的绝对值没有意义,因为考虑得是两者的相对性可将上式继续简化为:(Ap+AB +Acox=bp +bB +bc其中,Ap,AB和Ac为 Hessian矩阵,上述方程称之为增量方程。33MU约束1)残差:两帧之间的PVQ和bias的变化量的差△tx+k+1bk qbk+1bR+1 xyz+g"△t)-Bk(20)sbbbb其中各增量关于bias的 Jacobian可从公式(16)的大 Jacobian中的相应位置获得。上面与代码中 Integration base: evaluateD对应,2)优化变量pb, 0W, Svb ,8ba:,bor Opb,, 80W ,Swb,, bakr, Sba3)Jacobian:计算 Jacobian时,残差对应求偏导对象分别为p6e,6vB,6h,ba],6b,6b
    2020-12-07下载
    积分：1
  • 波束形成代码和原理解析
    包含波束形成代码和波束形成的原理解析，主要针对水声以及信号处理相关的设计人员有很大的帮助。
    2020-12-12下载
    积分：1
  • 696518资源总数
  • 106148会员总数
  • 10今日下载

热门标签

Visual C++ matlab C/C++ Java Others Unix_Linux EasyLanguage易语言 Visual Basic C# PDF C++ Delphi VHDL ASP WINDOWS
热门下载

Copyright© 2019-2025 | Powered by Imdn | 浙ICP备16007336号 | IMDN交流 | 联系客服（QQ：3324461878）

本网站资源收集自网络及网站用户上传，若有侵权请邮件至（3324461878@qq.com），核实后我们会在24小时内删除，谢谢！