The damped least squares method is also called the Levenberg-Marquardt method. Levenberg-Marquardt算法是最优化算法中的一种。它是使用最广泛的非线性最小二乘算法,具有梯度法和牛顿法的优点。当λ很小时,步长等于牛顿法步长,当λ很大时,步长约等于梯度下降法的步长。

  The damped least squares method can be theoretically justified as follows.Rather than just finding the minimum vector ∆θ that gives a best solution to equation (pseudo inverse method就是求的极小范数解), we find the value of ∆θ that minimizes the quantity:

where λ ∈ R is a non-zero damping constant. This is equivalent to minimizing the quantity:

The corresponding normal equation is(根据矩阵论简明教程P83 最小二乘问题:设ARm×nbRm. 若x0RnAx=b的最小二乘解,则x0是方程组ATAx=ATb的解,称该式为Ax=b的法方程组.)

This can be equivalently rewritten as:

It can be shown that JTJ + λ2I is non-singular when λ is appropriate(选取适当的参数λ可以保证矩阵JTJ + λ2I非奇异). Thus, the damped least squares solution is equal to:

Now JT is an n × n matrix, where n is the number of degrees of freedom. It is easy to find that (JTJ + λ2I)−1JT= JT (JJT + λ2I)−1(等式两边同乘(JTJ + λ2I)进行恒等变形). Thus:

The advantage of the equation is that the matrix being inverted is only m×m where m = 3k is the dimension of the space of target positions, and m is often much less than n. Additionally, the equation can be computed without needing to carry out the matrix inversion, instead row operations can find f such that (JJT + λ2If e and then JTf is the solution. The damping constant depends on the details of the multibody and the target positions and must be chosen carefully to make equation numerically stable. The damping constant should large enough so that the solutions for ∆θ are well-behaved near singularities, but if it is chosen too large, then the convergence rate is too slow.

  以平面二连杆机构为例,使用同样的V-rep模型,将目标点放置在接近机构奇异位置处,使用DLS方法求逆解。在下面的Python程序中关节角初始值就给在奇异点上,可以看出最终DLS算法还是能收敛,而pseudo inverse方法在奇异点处就无法收敛。The damped least squares method avoids many of the pseudo inverse method’s problems with singularities and can give a numerically stable method of selecting ∆θ

import vrep             #V-rep library
import sys
import time
import math
import numpy as np # Starts a communication thread with the server (i.e. V-REP).
clientID=vrep.simxStart('127.0.0.1', 20001, True, True, 5000, 5) # clientID: the client ID, or -1 if the connection to the server was not possible
if clientID!=-1: #check if client connection successful
print 'Connected to remote API server'
else:
print 'Connection not successful'
sys.exit('Could not connect') # Exit from Python # Retrieves an object handle based on its name.
errorCode,J1_handle = vrep.simxGetObjectHandle(clientID,'j1',vrep.simx_opmode_oneshot_wait)
errorCode,J2_handle = vrep.simxGetObjectHandle(clientID,'j2',vrep.simx_opmode_oneshot_wait)
errorCode,target_handle = vrep.simxGetObjectHandle(clientID,'target',vrep.simx_opmode_oneshot_wait)
errorCode,consoleHandle = vrep.simxAuxiliaryConsoleOpen(clientID,'info',5,1+4,None,None,None,None,vrep.simx_opmode_oneshot_wait) uiHandle = -1
errorCode,uiHandle = vrep.simxGetUIHandle(clientID,"UI", vrep.simx_opmode_oneshot_wait)
buttonEventID = -1
err,buttonEventID,aux = vrep.simxGetUIEventButton(clientID,uiHandle,vrep.simx_opmode_streaming) L1 = 0.5 # link length
L2 = 0.5
lamda = 0.2 # damping constant
stol = 1e-2 # tolerance
nm = 100 # initial error
count = 0 # iteration count
ilimit = 1000 # maximum iteration # initial joint value
# note that workspace-boundary singularities occur when q2 approach 0 or 180 degree
q = np.array([0,0]) while True:
retcode, target_pos = vrep.simxGetObjectPosition(clientID, target_handle, -1, vrep.simx_opmode_streaming) if(nm > stol):
vrep.simxAuxiliaryConsolePrint(clientID, consoleHandle, None, vrep.simx_opmode_oneshot_wait) # "None" to clear the console window x = np.array([L1*math.cos(q[0])+L2*math.cos(q[0]+q[1]), L1*math.sin(q[0])+L2*math.sin(q[0]+q[1])])
error = np.array([target_pos[0],target_pos[1]]) - x J = np.array([[-L1*math.sin(q[0])-L2*math.sin(q[0]+q[1]), -L2*math.sin(q[0]+q[1])],\
[L1*math.cos(q[0])+L2*math.cos(q[0]+q[1]), L2*math.cos(q[0]+q[1])]]) f = np.linalg.solve(J.dot(J.transpose())+lamda**2*np.identity(2), error) dq = np.dot(J.transpose(), f)
q = q + dq nm = np.linalg.norm(error) count = count + 1
if count > ilimit:
vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,"Solution wouldn't converge\r\n",vrep.simx_opmode_oneshot_wait)
vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,'q1:'+str(q[0]*180/math.pi)+' q2:'+str(q[1]*180/math.pi)+'\r\n',vrep.simx_opmode_oneshot_wait)
vrep.simxAuxiliaryConsolePrint(clientID,consoleHandle,str(count)+' iterations'+' err:'+str(nm)+'\r\n',vrep.simx_opmode_oneshot_wait) err, buttonEventID, aux = vrep.simxGetUIEventButton(clientID,uiHandle,vrep.simx_opmode_buffer)
if ((err==vrep.simx_return_ok) and (buttonEventID == 1)):
'''A button was pressed/edited/changed. React to it here!'''
vrep.simxSetJointPosition(clientID,J1_handle, q[0]+math.pi/2, vrep.simx_opmode_oneshot )
vrep.simxSetJointPosition(clientID,J2_handle, q[1], vrep.simx_opmode_oneshot ) '''Enable streaming again (was automatically disabled with the positive event):'''
err,buttonEventID,aux=vrep.simxGetUIEventButton(clientID,uiHandle,vrep.simx_opmode_streaming) time.sleep(0.01)

参考:

“逆运动学”——从操作空间到关节空间(上篇)

V-rep学习笔记:机器人逆运动学数值解法(Damped Least Squares / Levenberg-Marquardt Method)的更多相关文章

  1. V-rep学习笔记:机器人逆运动学数值解法(The Jacobian Transpose Method)

    机器人运动学逆解的问题经常出现在动画仿真和工业机器人的轨迹规划中:We want to know how the upper joints of the hierarchy would rotate ...

  2. V-rep学习笔记:机器人逆运动学数值解法(The Pseudo Inverse Method)

    There are two ways of using the Jacobian matrix to solve kinematics. One is to use the transpose of ...

  3. V-rep学习笔记:机器人逆运动学数值解法(Cyclic Coordinate Descent Method)

    When performing inverse kinematics (IK) on a complicated bone chain, it can become too complex for a ...

  4. V-rep学习笔记:机器人逆运动学解算

    IK groups and IK elements VREP中使用IK groups和IK elements来进行正/逆运动学计算,一个IK group可以包含一个或者多个IK elements: I ...

  5. matlab学习笔记10_6 字符串与数值间的转换以及进制之间的转换

    一起来学matlab-matlab学习笔记10 10_6 字符串与数值间的转换以及进制之间的转换 觉得有用的话,欢迎一起讨论相互学习~Follow Me 参考书籍 <matlab 程序设计与综合 ...

  6. ES6学习笔记(四)-数值扩展

    PS: 前段时间转入有道云笔记,体验非常友好,所以笔记一般记录于云笔记中,每隔一段时间,会整理一下, 发在博客上与大家一起分享,交流和学习. 以下:

  7. python学习笔记(五)数值类型和类型转换

    Python中的数值类型有: 整型,如2,520 浮点型,如3.14159,1.5e10 布尔类型 True和False e记法: e记法即对应数学中的科学记数法 >>> 1.5e1 ...

  8. ES6学习笔记(四)数值的扩展

    1.二进制和八进制表示法 ES6 提供了二进制和八进制数值的新的写法,分别用前缀0b(或0B)和0o(或0O)表示. 0b111110111 === 503 // true 0o767 === 503 ...

  9. Python学习笔记(2)数值类型

    进制转换 int函数任意进制转换为10进制 第一个参数传入一个字符串,任意进制的,第二个参数传入对这个字符串的解释,解释他为几进制 hex oct bin转换进制为16 8 或者2进制 例题中石油87 ...

随机推荐

  1. html5,表格

    <table border="1"><caption>表格的实例</caption><tr><td>单元格</td ...

  2. 160927、用jquery 重置表单的方法

    清空 我们项目小小部分的搜索条件: 客户要做的是,只要一键 "清空搜索条件" 即可清空维护地点.订单ID等条件. js函数 //重置表单 function resetform(){ ...

  3. PHP开发框架Laravel优点,Laravel5.3中文文档

    PHP开发框架Laravel优点 Laravel的设计思想是很先进的,非常适合应用各种开发模式TDD, DDD和BDD,作为一个框架,它为你准备好了一切,composer是个php的未来,没有comp ...

  4. zabbix如何实现微信报警 转载

    现实生产环境中,我们通常使用邮件和短信接受zabbix报警信息,但是邮件经常被工作人员搁置在角落中甚至被设置为垃圾邮件被过滤掉.公司的短信接口又太贵,复杂环境中使用短息报警会使运维成本增加很多.微信提 ...

  5. [转]学习 WCF (6)--学习调用WCF服务的各种方法

    转自:http://www.cnblogs.com/gaoweipeng/archive/2009/07/26/1528263.html 作者这篇博文写得很全面. 根据不同的情况,我们可以用不同的方法 ...

  6. Serialize Documents with the C# Driver

    1.介绍 该文档是1.8版本的C#驱动. 本节教程讨论C#类和BSON文档之间的序列化和反序列化.序列化是将对象映射成一个可以存储在MongoDB中的BSON文档的过程,反序列化是从一个BSON文档重 ...

  7. HDU 1024:Max Sum Plus Plus(DP)

    http://acm.hdu.edu.cn/showproblem.php?pid=1024 Max Sum Plus Plus Problem Description Now I think you ...

  8. 20145227 《Java程序设计》第4周学习总结

    20145227 <Java程序设计>第4周学习总结 教材学习内容总结 第六章 继承与多态 6.1 何谓继承 1.继承共同行为 多个类中存在相同属性和行为时,将这些内容抽取到单独一个类中, ...

  9. ACM题目————STL练习之Ananagrams

    Description Most crossword puzzle fans are used to anagrams--groups of words with the same letters i ...

  10. 复利计算--4.0 单元测试之JAVA版-软件工程

    复利计算--4.0 单元测试-软件工程 前言:由于本人之前做的是C语言版的复利计算,所以为了更好地学习单元测试,于是将C语言版的复利计算修改为JAVA版的. 一.主要的功能需求细分: 1.本金为100 ...