全向轮运动平台pdf
全向轮，全向移动2,3,4轮小车，变换矩阵。设李雅普诺夫函数为V1=;(x2+y2+0)求其导数如下,当渐进稳定时导数小于0Ⅵ1=xx+yy+ade =-kxre, yeke8上式系数为正时,李雅普诺夫函数的导数小于零,系统渐进稳定代入微分方程得到控制律如下:+ vr cos日a+k-xea+ v sin 8e t kyy+ke022差动轮直角坐标运动学方程差动轮与全向轮的区别是,全向轮小车速度方向与四个轮子的共同朝向相同可为仼意方向,而差动轮小车的切向速度方向与X轴重合,故方程中v=0微分方程如下v+v cos 0PRxet vr sin221差动轮直角坐标下控制律设计选择 Lyapunov函数如下:V2=(x2+y2)+(1k(-cosee对上式沿求导+-。sin6e cea-v+ vr cos ee)+yec-xew+ vr sin ge)D sin 0rev+xe vr cos 8e+yevr sin Be+rwr sin 0e -- sin 8 e11-Xev+xevr cos Be year sin 0e +Wrsin eeksinbe选择如下速度控制输入s。+kxxOrt vr(kye t kosine e)将上式代入 Lyapunov函数导数得到esin 2 0当上式系数为正时,V2≤0,故以上 Lyapunov函数选择正确。由此得到堪于运动学模型的轨迹跟踪速度控制律为2:os 8+lcV(kye t resin其中,k,kx,k为控制器参数。22.2控制器参数选取将控制律代入微分方程得下式(rt vr (lye t))xeRyexe(ar+ vr(kye t kesinee))+ vr sin Be-v (kye t kesinee)上式在零点附近线性化,忽略高次项得PR= ApA0Vrky -vr ke系数值与角速度和速度指令值共同决定系统根,当系数为正是所有根为负数。23对比仿真与结果仿真系统结果图如下ct(pea qle)p(7)elrorxPe, qe)图3轨迹跟踪结构图图中q(yo),v、o分别为移动机器人的线速度和角速度,ε1=(xy0)r,对于差动机器人运动学方程可表示为:COS日0Stn图中 J-sine0:pR=y):qa对于全向轮机器人运动学方程可表示为60sine cose ov=R(O)1 vy对角速度为0.2和线速度为5的圆形轨迹进行跟踪,仿真结果如下图:35302501510-5图4圆形轨迹跟踪仿真图图中×点线为差动轮跟踪轨迹,O点线为全向轮跟踪轨迹。、全向轮平台的设计对全向轮采用如下图所示的结构时,进行系统分析与设计图5互补型全向轮( omni wheels31运动学模型X图6全向轮式移动机器人运动学模型移动坐标X-Y固定在机器人重心上,而质心正好位于几何中心上。机器人P点在全局坐标系的位置坐标为:(x2y,0),三个全向轮以3号轮中心转动轴反方向所为机器人的ⅹ轴。假设三个全向轮完全相同,三个全向轮中心到车体中心位置的距离L。在移动坐标X-Y的速度用 1xe 1表示。由文献[3可得三个全间轮的速度与其在移动坐标和全局坐标系下的速度分量之间的关系分别为以下二式sin(60)xeV)=(-s(60os60)()=011-21-213×3ysin(60-0)Cos(60-6)sin(60+6)cos(60+6)Lysinecose32动力学模型在移动坐标X-Y中,设机器人在沿轴X2和Y方向上收到的力分别为Fx和Fyc第1、2、3号驱动轮提供给机器人的驱动力分别为f1、卫、3,机器人惯性转矩为M,根据牛顿第二定律可得到如下的动力学方程:3√3cos(30)-cos(30)01fFre=sin(30) sin (30)1ML2LTb22/2在地理坐标系X一Y下的方程如下:mxcos(30+0)-cos(30-0) sing 1fiFr= sin(30+0)sin(30-0)-cosefzL33基于动力学模型的控制器设计如上式所示,基于机器人动力学模型的控制方案,直接根据机器人的动力学模型设讣运动控制器,控制器的输出为机器人上驱动电机的驱动电压。基于动力学模型的控制方案,不需对驱动电机进行底层的速度控制,消除了底层速度控制带来的延时。由功力学方程:nmx3×3M」可知在休坐标系中各个方向上的控制输入输出是独立的并且相互之间无耦合;于是可在体坐标中对各个控制量分别进行控制。当以各个电机电压作为控制量U时,对体坐标系中各个方向上的控制量UF经过Ta3×3变换后得到各个电机的控制量UUF先对输入UF到体坐标各个方冋上速度V的系统等效参数[m′门进行辨识,得到由控制量UF到体坐标速度Ⅴ的传递函数:然后设计UF的控制器,经过变换后得到各电机的电压U;速度控制指令 1xe vye (l由第2节控制律求得。34基于编码器的位姿推算圆弧模型在文献L4中介绍机器人里程计圆弧模型是把移动机器人在运动过程中的实际轨迹通过圆弧去逼近234图7平台样品示意图YAYR11B(x12+11Un-1XAA(r()图8采样期间的圆弧运动轨迹图中A(xmy,0n)和B(xnx+1,yn+1,On+1)分别为在采样时问间隔内起始点与终点的位姿坐标,AB为采样期间的圆弧轨迹,利用图中儿何关系可以得到运动轨迹为圆弧时的推算公式如下L(△SR+△S少sin△SR-△Sn+1xn+6n+2(△sinenR△SL(ΔSR+△S△SYn+1=ynCOS+
- 2020-06-06下载
- 积分：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
|
联系客服（QQ：3324461878）
